4D millimeter wave SLAM method and system

Abstract

The invention belongs to but not limited to the technical field of millimeter wave navigation, and particularly relates to a 4D millimeter wave SLAM method and system, and the method comprises the steps: 1, building a data collection platform, and collecting data; step 2, data processing, including sensor calibration and data format conversion; step 3, 4D millimeter wave radar data enhancement; step 4, performing point cloud registration on the point cloud sequence to obtain rough front-end odometer information; 5, performing closed-loop detection on each frame, and judging whether the positioning track forms a closed loop or not, thereby facilitating subsequent image optimization; and step 6, performing back-end image optimization by using an open source library g2o to obtain an accurate positioning track. According to the method, the 4D millimeter wave radar is used for SLAM, and the method has important research significance and application value. The environmental adaptability and robustness of the SLAM system are expected to be further improved, and the development and application of the automatic driving technology are promoted. Meanwhile, a new thought and possibility are provided for application of the millimeter wave radar in other fields such as robots and unmanned aerial vehicles.

Description

Technical Field

[0001] The present invention belongs to, but is not limited to, the technical field of millimeter-wave navigation, and particularly relates to a 4D millimeter-wave SLAM method and system. Background Art

[0002] The continuous development of autonomous driving technology has put forward higher requirements for Simultaneous Localization and Mapping (SLAM). The SLAM system undertakes the core tasks of real-time positioning, environmental perception, and mapping in autonomous driving, and the choice of sensors determines the accuracy, stability, and applicability of the SLAM system. Traditional SLAM methods mainly rely on visual sensors (monocular / binocular cameras) and lidar (LiDAR), but both of these sensors have limitations in practical applications. For example, visual sensors are sensitive to lighting conditions and are prone to failure in complex environments such as at night, strong light, and haze; while lidar can provide high-precision point cloud information, but it has a high cost, is difficult to obtain target speed information, and may be interfered in specific environments. Therefore, researchers have begun to pay attention to the application of 4D millimeter-wave radar in SLAM, hoping to improve the stability and robustness of the SLAM system by virtue of its strong environmental adaptability and ability to provide speed information.

[0003] Compared with traditional visual and lidar SLAM, 4D millimeter-wave radar has many advantages. First, it does not depend on lighting and can still work stably in harsh environments such as low light, fog, rain, and high temperature, making it suitable for all-weather autonomous driving. Second, the 4D millimeter-wave radar can directly measure the distance, speed, azimuth angle, and elevation angle of the target, which enables more accurate self-motion estimation in the SLAM system, thereby improving the positioning accuracy. In addition, compared with lidar, the detection ability of millimeter-wave radar is stronger, and it can penetrate rain, fog, and dust, making it suitable for autonomous driving scenarios in extreme weather. In addition, the scanning range of the 4D millimeter-wave radar is wide, and it can provide stable perception data in a large-scale scene, supporting the mapping of large-scale environments.

[0004] Existing research has shown that the application of 4D millimeter-wave radar in SLAM is gradually developing. For example, the 4DRadarSLAM system developed by the Jun Zhang team uses Pose Graph Optimization to improve the SLAM accuracy; the Li team improves the positioning accuracy of radar SLAM through IMU fusion and self-velocity pre-integration factors; while the 4D IROM system of Zhuang et al. combines IMU and GNSS data and uses an Iterative Extended Kalman Filter (IEKF) to effectively enhance the environmental adaptability of the SLAM system. These studies indicate that 4D millimeter-wave radar has great application potential in SLAM tasks, but existing methods mainly focus on using millimeter-wave radar independently for SLAM and do not fully combine the high-precision point cloud of lidar, failing to achieve the complementary advantages of the two.

[0005] Although 4D millimeter-wave radar has significant advantages in the field of SLAM, it still faces challenges. First, compared with lidar, the point cloud density of 4D millimeter-wave radar is lower, making it difficult to provide sufficient detailed information in tasks that require high-resolution mapping. Second, millimeter-wave radar is prone to clutter interference in complex environments, especially in dynamic environments such as indoors and urban traffic, where clutter may affect the accuracy of target detection and position estimation. In addition, existing 4D millimeter-wave radar SLAM systems mainly rely on millimeter-wave radar data and lack the high-precision information of vision or lidar, resulting in lower accuracy in static environments than lidar SLAM. Therefore, how to increase the density of the millimeter-wave radar point cloud, reduce the impact of clutter, and combine other sensors for data fusion is a key issue to be solved in the future.

[0006] To address the limitations of 4D millimeter-wave radar SLAM, a feasible solution is to fuse lidar and millimeter-wave radar data to achieve complementary advantages. Lidar can provide high-precision and high-density point cloud information, which is helpful for detail feature extraction, while millimeter-wave radar can provide the velocity information of targets and enhance the motion estimation ability. Through multi-sensor fusion algorithms (such as Kalman filtering, graph optimization, deep learning, etc.), the problem of insufficient accuracy of millimeter-wave radar can be effectively compensated, and at the same time, the robustness of the SLAM system in harsh environments can be improved. In addition, future research can explore the fusion of millimeter-wave radar and vision sensors, and use deep learning technology to improve the point cloud resolution, thereby enhancing the ability of the SLAM system in detail recognition and environmental mapping.

[0007] With the increasing demand for SLAM systems in fields such as autonomous driving, robot navigation, and intelligent transportation, the research and application of 4D millimeter-wave radar will usher in a broader space for development. Future research can focus on the deep integration of millimeter-wave radar with multiple sensors such as lidar, IMU, and vision, explore point cloud enhancement technology based on deep learning, and improve the resolution and accuracy of millimeter-wave radar point clouds. In addition, research in data filtering and denoising, low-power hardware optimization, and high-precision map construction will also promote the further development of 4D millimeter-wave radar SLAM. Through these technological breakthroughs, 4D millimeter-wave radar is expected to play an important role in many fields such as unmanned driving, intelligent robots, indoor navigation, and disaster relief, further improving the practicality and reliability of SLAM systems. Summary of the invention

[0008] In response to the problems existing in the prior art, the present invention provides a 4D millimeter wave SLAM method and system, which is a multi-sensor SLAM solution based on 4D millimeter wave radar as the main and laser radar as the auxiliary. The sensor data collected by the data acquisition platform can simultaneously locate the platform and update the environmental map to improve the accuracy of navigation.

[0009] The present invention is implemented as follows: a 4D millimeter wave SLAM method, comprising:

[0010] Step 1: Build a data collection platform and collect data;

[0011] Step 2: data processing, including sensor calibration, data format conversion, etc.

[0012] Step 3, 4D millimeter wave radar data enhancement;

[0013] Step 4: perform point cloud registration on the point cloud sequence to obtain rough front-end odometer information;

[0014] Step 5: Perform closed-loop detection on each frame to determine whether the positioning trajectory forms a closed loop, which is convenient for subsequent image optimization;

[0015] Step 6: Use the open source library g2o to optimize the backend graph and obtain accurate positioning trajectory.

[0016] Furthermore, for the construction of the data acquisition platform and data collection described in step 1, in terms of sensor configuration, a multi-sensor fusion solution is adopted, including cameras, millimeter-wave radars, lidars, infrared cameras, GPS, and inertial measurement units (IMUs), etc. By synchronizing and calibrating the data of these sensors in space and time, comprehensive and accurate environmental perception information can be obtained. Cameras provide rich visual information for object detection, tracking, and semantic segmentation; millimeter-wave radars and lidars provide accurate distance and speed measurements and have better adaptability to bad weather and lighting conditions; infrared cameras can work in low-light or even completely dark environments and provide thermal imaging information for detecting heat source targets such as pedestrians and animals; IMUs provide high-frequency motion state estimation for compensating the delays and errors of other sensors.

[0017] Furthermore, for the data processing described in step 2, the steps are as follows:

[0018] Step 2.1: Align the time of the sensor data. The goal is to align the timestamps of each sensor to ensure that the data they collect corresponds to the same event or object at the same time point. The present invention uses the time synchronization library message_filters provided by ROS and adopts the Approximate Time Synchronizer synchronization mechanism to synchronize the sensor data such as lidar, camera, and IMU to a time reference, ensuring that the collected data corresponds to the same object at the same time point.

[0019] Step 2.2: Calibrate the sensors spatially to determine the spatial positions and poses of each sensor so that their observation results can be uniformly mapped to the same coordinate system. The present invention uses the calibration board method. A calibration board with known features is used, and different sensors are allowed to observe it, so that the laser and 4D millimeter-wave radar estimate their external parameters by detecting the geometric features of the calibration board.

[0020] Step 2.3: Convert the 4D millimeter-wave radar point cloud data format. The present invention uses wireshark to capture 4D millimeter-wave radar data during data collection to obtain a network packet capture file in pcap format. To be compatible with the SLAM system, the present invention reads the json file parsed by wireshark, extracts the original data of the radar (such as distance, speed, angle, etc.), packs these data into ROS messages, publishes them, and saves them in a bag format file. In this way, the present invention stores the data of the 4D millimeter-wave radar and the data of other sensors in a unified bag file, which is convenient for seamless integration into an autonomous driving project developed based on ROS.

[0021] Further, for the 4D millimeter-wave radar point cloud data enhancement in step 3, the lidar point cloud is used to enhance the data of the 4D millimeter-wave radar. Using the result of the extrinsic calibration, the lidar point cloud P is transformed into the millimeter-wave radar coordinate system. For each point p in the millimeter-wave radar, the points within a certain neighborhood around it in the point cloud P are searched and interpolated around p, and the Doppler velocity and signal intensity of p are used as the Doppler velocity and signal intensity of the new interpolated points. The "voxel-based upsampling" interpolation method is used to enhance the data. The 4D millimeter-wave radar point cloud data is voxelized, and the three-dimensional space is divided into a series of small cube units. Then, the voxel grid is upsampled using the nearest neighbor interpolation or trilinear interpolation method to obtain a new voxel grid. After interpolation, Gaussian filtering is applied to the new voxel grid to eliminate the jagged and blocky effects.

[0022] Further, for the point cloud registration of the point cloud sequence described in step 4 to obtain the rough front-end odometry information, for adjacent point cloud frames P = {p1, p2,......, p n} and Q = {q1, q2,..., q m}, the goal is to register them through a rigid body transformation (R, t) such that the points in P coincide with the points in Q as much as possible after the rigid body transformation. R ∈ SO(3) is the rotation matrix. is the translation vector. The specific process is as follows:

[0023] Step 4.1, find the nearest point q i ∈ Q for each point p j , which is achieved by minimizing the following function:

[0024] d(p i - q j ) = ||p i - q j ||2

[0025] For each point p i , find the corresponding nearest point q j as

[0026]

[0027] Use the KD-tree of the nearest neighbor search algorithm to accelerate the above process.

[0028] Step 4.2, estimate the rigid body transformation for aligning the point cloud P to Q, and minimize the following objective function:

[0029]

[0030] Adopt the centroid alignment method to estimate the rotation R and translation t. First, calculate the centroids of P and Q:

[0031]

[0032] Translate the point clouds P and Q to the centroid

[0033]

[0034] Calculate the covariance matrix H:

[0035]

[0036] Perform singular value decomposition on the matrix:

[0037] H = UΣV T

[0038] The optimal rotation matrix R is given by

[0039] R = VU T

[0040] The optimal translation vector t is

[0041]

[0042] In step 4.3, update the point cloud P with the transformation (R, t) obtained through the above steps

[0043] p i ← Rp i + t

[0044] And perform the closest point matching again, repeating steps 4.1 and 4.2 until the objective function E(R, t) is less than the threshold ε or the maximum number of iterations is reached:

[0045] ||E k+1 (R, t) - E k (R, t)|| < ε

[0046] Furthermore, the loop closure detection for each frame described in step 5 requires comparing the current key frame with the database key frames to determine whether the platform has repeatedly reached the same position. First, perform loop closure pre-filtering according to certain rules to pre-filter out potential candidate frames in advance. Second, construct the context for the current key frame. Due to the limitations of the sensor and multiple reflected echoes, the radar altitude information has noise, so the intensity scan context is used instead of the scan context. If the similarity between the context of the current key frame and the context of the database key frame reaches a certain threshold, it is considered that a loop closure is formed.

[0047] The backend graph optimization described in step 6, after successfully identifying two closed loops, calls g2o, uses the pose of each frame as a node, the pose constraint between each pair of frames as an edge, adds the constraints generated by loop closure detection to the graph, adjusts all nodes through a global optimizer to minimize the error, and updates the optimized pose to the system to correct the error in the map.

[0048] Another object of the present invention is to provide a 4D millimeter-wave SLAM system for implementing the 4D millimeter-wave SLAM method, including:

[0049] A data collection module that builds a data acquisition platform to collect data;

[0050] A data processing module for data processing, including sensor calibration, data format conversion, etc.;

[0051] A data enhancement module for 4D millimeter-wave radar data enhancement;

[0052] A point cloud registration module that performs point cloud registration on the point cloud sequence to obtain rough front-end odometry information;

[0053] A loop closure detection module that performs loop closure detection on each frame to determine whether the positioning trajectory forms a closed loop, facilitating subsequent graph optimization;

[0054] A backend graph optimization module that uses the open-source library g2o for backend graph optimization to obtain an accurate positioning trajectory.

[0055] Another object of the present invention is to provide a computer device. The computer device includes a memory and a processor. When the computer program stored in the memory is executed by the processor, the processor executes the steps of the 4D millimeter-wave SLAM method.

[0056] Another object of the present invention is to provide a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the 4D millimeter-wave SLAM method.

[0057] Another object of the present invention is to provide an information data processing terminal, and the information data processing terminal includes the 4D millimeter-wave SLAM system described above.

[0058] Combined with the above technical solutions and the technical problems solved, the advantages and positive effects of the technical solutions to be protected by the present invention are:

[0059] The present invention proposes a SLAM method that integrates 4D millimeter-wave radar and laser radar, aiming to overcome the limitations of a single sensor solution and improve the adaptability and robustness of the autonomous driving system in complex environments. The 4D millimeter-wave radar has the ability to work all day and night, and can provide stable distance and speed information in bad weather such as low light, rain, snow, fog and haze, while the laser radar can provide high-precision, high-density point cloud data, and has a strong ability to capture detailed features. However, existing SLAM systems often rely only on a single sensor and cannot take into account both robustness and accuracy. The present invention makes up for the shortcomings of sparse millimeter-wave radar point clouds and low precision through the fusion of the two, while overcoming the applicability of laser radar in extreme weather, providing a more reliable SLAM solution for intelligent driving systems.

[0060] At present, SLAM systems around the world mainly rely on single sensors, such as solutions based on LiDAR or millimeter-wave radar, which makes the system vulnerable to restrictions in specific environments. LiDAR relies on optical scanning. Although it has high accuracy, it is easily disturbed in weather conditions such as fog, strong light, rain and snow, which affects the stability of the SLAM system. At the same time, LiDAR is expensive and not easy to popularize in large-scale autonomous driving or robotic applications. In contrast, 4D millimeter-wave radar performs well in low-visibility environments and can provide information on the distance, speed, azimuth and pitch angle of the target, making it more capable of estimating self-motion in dynamic environments. However, its point cloud density is low, making it difficult to accurately capture complex details in the environment. How to combine the accuracy advantages of LiDAR and the environmental adaptability of millimeter-wave radar has become a key challenge to improving the performance of SLAM systems.

[0061] The 4D millimeter-wave radar and laser radar collaborative SLAM method proposed in the present invention is mainly based on millimeter-wave radar and supplemented by laser radar, and improves the overall performance of the SLAM system through multi-sensor fusion technology. Specifically, the method uses 4D millimeter-wave radar to provide global positioning information and target motion information to ensure that the system can still operate stably in low light or bad weather. At the same time, laser radar is used to supplement high-precision point cloud data to enhance the system's local environment mapping capabilities to ensure that the SLAM system still has high accuracy in scenes with high detail requirements such as urban roads and indoor environments. By fusing the data of the two, the present invention can adaptively adjust the SLAM strategy under different working conditions and improve the stability and generalization ability of the system.

[0062] The present invention adopts a multi-sensor data fusion framework and uses methods such as Kalman filtering, graph optimization, and point cloud interpolation to achieve efficient fusion SLAM of 4D millimeter-wave radar and lidar. First, in the data acquisition stage, a high- and low-speed data acquisition platform is built, and millimeter-wave radar point cloud and lidar point cloud data are collected in bag format. Then, in the data preprocessing stage, the lidar data is used to enhance the millimeter-wave radar through point cloud interpolation, improving the resolution of the millimeter-wave point cloud. Finally, in the SLAM mapping and positioning stage, the bag format data is read relying on the ROS platform, and point cloud registration algorithms (such as ICP, NDT, etc.) are used to register the enhanced point cloud data to generate the front-end odometer. Subsequently, the graph optimization method is used to optimize the pose, thereby achieving high-precision and low-error SLAM positioning.

[0063] By effectively fusing the data of 4D millimeter-wave radar and lidar, the present invention significantly improves the performance of the SLAM system in dynamic environments and extreme weather conditions. During the testing process, the system can maintain stable positioning in complex scenarios such as rainy days, haze, and at night. Compared with the single-sensor solution, its error is reduced by about 30%-50%, and the robustness is significantly improved. The obtained positioning trajectory has a smaller error compared with the ground truth positioning trajectory, indicating that the positioning accuracy of the system in complex environments is effectively guaranteed. In addition, in environments such as urban driving, highways, and underground garages, this method can effectively improve the accuracy and integrity of map construction, enabling autonomous driving vehicles to still be able to perform high-precision autonomous navigation in changing environments.

[0064] The fusion SLAM method of the present invention is not only applicable to autonomous driving but also can be extended to multiple fields such as robots, drones, indoor positioning, and intelligent transportation. For example, in drone navigation, this method can make up for the limitations of lidar affected by light, improving the autonomous flight ability of drones in forests, at night, and in bad weather; in intelligent transportation, it can be used for intelligent signal control and traffic monitoring under bad weather conditions. In addition, the fusion SLAM technology can also be used in high-risk environments such as security patrols, mine exploration, and disaster rescue, providing stronger environmental adaptability and higher positioning accuracy for various intelligent navigation systems. Brief Description of the Drawings

[0065] Figure 1 It is a flowchart of the implementation of the 4D millimeter-wave radar SLAM system provided by the embodiment of the present invention.

[0066] Figure 2 It is a schematic diagram of the platform of the low-speed data acquisition platform remote control vehicle equipped with each sensor provided by the embodiment of the present invention.

[0067] Figure 3 It is a schematic diagram of a specially modified vehicle used as a high-speed data acquisition platform provided by the embodiment of the present invention.

[0068] Figure 4 It is a schematic diagram of the process of a specially modified vehicle data acquisition used as a high-speed data acquisition platform provided by an embodiment of the present invention.

[0069] Figure 5 It is a box plot of the Relative Translation Error (RTE) of the 4D millimeter-wave SLAM method provided by an embodiment of the present invention in a low-speed sunny environment.

[0070] Figure 6 It is a schematic diagram of the rotation error of the trajectory of the 4D millimeter-wave SLAM method provided by an embodiment of the present invention after SIM(3) alignment in a low-speed sunny environment.

[0071] Figure 7 It is a comparison chart between the estimated value and the ground truth of the 4D millimeter-wave SLAM method provided by an embodiment of the present invention in a low-speed sunny environment.

[0072] Figure 8 It is a side view of the trajectory of the comparison result between the estimated trajectory and the ground truth trajectory of the 4D millimeter-wave SLAM method provided by an embodiment of the present invention in a high-speed snowy environment.

[0073] Figure 9 It is a top view of the trajectory of the comparison result between the estimated trajectory and the ground truth trajectory of the 4D millimeter-wave SLAM method provided by an embodiment of the present invention in a high-speed snowy environment.

[0074] Figure 10 It is a structural diagram of the 4D millimeter-wave SLAM system provided by an embodiment of the present invention. Detailed implementation manners

[0075] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below in conjunction with embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0076] A 4D millimeter-wave SLAM method includes:

[0077] Step 1, build a data acquisition platform and collect data;

[0078] Step 2, data processing, including sensor calibration, data format conversion, etc.;

[0079] Step 3, 4D millimeter-wave radar data enhancement;

[0080] Step 4, perform point cloud registration on the point cloud sequence to obtain rough front-end odometry information;

[0081] Step 5, perform closed-loop detection on each frame to determine whether the positioning trajectory forms a closed loop, facilitating subsequent map optimization;

[0082] Step 6, use the open-source library g2o for backend map optimization to obtain an accurate positioning trajectory.

[0083] For the construction of the data acquisition platform and data collection described in Step 1, in terms of sensor configuration, a multi-sensor fusion scheme is adopted, including cameras, millimeter-wave radars, lidars, infrared cameras, GPS, and inertial measurement units (IMUs), etc. By synchronizing and calibrating the data of these sensors in space and time, comprehensive and accurate environmental perception information can be obtained. Cameras provide rich visual information for object detection, tracking, and semantic segmentation; millimeter-wave radars and lidars provide accurate distance and speed measurements and have better adaptability to bad weather and lighting conditions; infrared cameras can work in low-light or even completely dark environments and provide thermal imaging information for detecting heat source targets such as pedestrians and animals; IMUs provide high-frequency motion state estimation for compensating the delays and errors of other sensors.

[0084] The dataset covers a variety of typical driving scenarios and environmental conditions, including indoor parking lots, outdoor sunny days, rainy days, snowy days, nights, and days. At the same time, the present invention also collects data on bumpy and non-bumpy road sections to verify the system's resistance to road vibrations and bumps and ensure its stability under complex road conditions. By collecting data under different weather, lighting, and road conditions, the robustness and adaptability of the system in various harsh environments can be tested.

[0085] To obtain a comprehensive and high-quality dataset, the present invention adopts two different data acquisition platforms: cars and remote control cars. This method can effectively cover driving scenarios at different speed ranges and provide more diverse data.

[0086] For high-speed scenarios, the present invention mounts the sensor system on a specially modified car. By driving in a real traffic environment, the present invention has collected a large amount of high-speed driving data. These data reflect the perception, decision-making, and control challenges of the vehicle at high speeds, such as rapidly changing traffic flows, complex road structures, detection and tracking of high-speed moving targets, etc. The dataset obtained through the car platform can be used to develop and verify the performance and safety of the autonomous driving system in high-speed scenarios.

[0087] For low-speed scenarios, the present invention uses a specially designed remote control vehicle platform. This vehicle is equipped with a sensor configuration similar to that of a car, but is more suitable for operating in low-speed and narrow spaces. We remotely control the vehicle in various indoor and outdoor environments, such as parking lots, campuses, etc., to collect data on low-speed driving. These scenarios usually involve more static and dynamic obstacles, such as pedestrians, bicycles, other vehicles, etc., posing different challenges to the perception and decision-making of the autonomous driving system.

[0088] The data processing steps of Step 2 are as follows:

[0089] Step 2.1: Align the sensor data in terms of time. The goal is to align the timestamps of each sensor to ensure that the data they collect corresponds to the same event or object at the same time point. The present invention utilizes the time synchronization library message_filters provided by ROS and adopts the Approximate Time Synchronizer synchronization mechanism to synchronize the sensor data of lidar, camera, IMU, etc. to a single time reference, ensuring that the collected data corresponds to the same object at the same time point.

[0090] Step 2.2: Calibrate the sensors spatially to determine the spatial position and pose of each sensor, so that their observation results can be uniformly mapped to the same coordinate system. The present invention uses the calibration board method. Using a calibration board with known features, different sensors are made to observe it, enabling the lidar and 4D millimeter-wave radar to estimate their extrinsic parameters by detecting the geometric features of the calibration board.

[0091] Step 2.3: Convert the 4D millimeter-wave radar point cloud data format. The present invention uses wireshark to capture 4D millimeter-wave radar data during data collection, obtaining a network packet capture file in pcap format. To be compatible with the SLAM system, the present invention reads the json file parsed by wireshark, extracts the original data of the radar (such as distance, speed, angle, etc.), packs this data into ROS messages, publishes and saves it in a bag format file. In this way, the present invention stores the data of the 4D millimeter-wave radar and the data of other sensors in a unified bag file, facilitating seamless integration into an autonomous driving project developed based on ROS.

[0092] In the 4D millimeter-wave radar point cloud data enhancement described in step 3, lidar point cloud is used to enhance the data of the 4D millimeter-wave radar. Using the result of extrinsic calibration, the lidar point cloud P is transformed into the millimeter-wave radar coordinate system. For each point p in the millimeter-wave radar, points within a certain neighborhood around it in the point cloud P are searched and interpolated around p, and the Doppler velocity and signal intensity of p are used as the Doppler velocity and signal intensity of the new interpolated points. The "voxel-based upsampling" interpolation method is used to enhance the data. The 4D millimeter-wave radar point cloud data is voxelized, and the three-dimensional space is divided into a series of small cube cells. Then, the voxel grid is upsampled using the nearest neighbor interpolation or trilinear interpolation method to obtain a new voxel grid. After interpolation, Gaussian filtering is applied to the new voxel grid to eliminate aliasing and block effects.

[0093] Specifically, for the point cloud registration performed in step 4 to obtain rough front-end odometry information, for adjacent point cloud frames P = {p1, p2,......, p n} and Q = {q1, q2,..., q m}, the goal is to register them through a rigid body transformation (R, t) such that the points in P coincide with the points in Q as much as possible after the rigid body transformation. R ∈ SO(3) is the rotation matrix. is the translation vector. The specific process is as follows:

[0094] Step 4.1, find the nearest point q i ∈ Q for each point p j , which is achieved by minimizing the following function:

[0095] d(p i - q j ) = ||p i - q j ||2

[0096] For each point p i , find the corresponding nearest point q j as

[0097]

[0098] Use the KD-tree nearest neighbor search algorithm to accelerate the above process.

[0099] Step 4.2, estimate the rigid body transformation for aligning point cloud P to Q by minimizing the following objective function:

[0100]

[0101] Adopt the centroid alignment method to estimate the rotation R and translation t. First, calculate the centroids of P and Q:

[0102]

[0103] Translate the point clouds P and Q to the centroid

[0104]

[0105] Calculate the covariance matrix H:

[0106]

[0107] Perform singular value decomposition on the matrix:

[0108] H = U∑V T

[0109] The optimal rotation matrix R is given by

[0110] R = VU T

[0111] The optimal translation vector t is

[0112]

[0113] In step 4.3, update the point cloud P with the transformation (R, t) obtained through the above steps

[0114] p i ← Rp i + t

[0115] And perform the nearest point matching again, repeating steps 4.1 and 4.2 until the objective function E(R, t) is less than the threshold ε or the maximum number of iterations is reached:

[0116] ||E k+1 (R, t) - E k (R, t)|| < ε

[0117] Specifically, for the loop closure detection of each frame described in step 5, it is necessary to compare the current key frame with the database key frames to determine whether the platform has repeatedly reached the same position. First, perform loop closure pre-filtering according to certain rules to filter out potential candidate frames in advance. Secondly, construct the context for the current key frame. Due to the limitations of the sensor and multiple reflected echoes, the radar altitude information has noise, so the intensity scan context is used instead of the scan context. If the similarity between the context of the current key frame and the context of the database key frame reaches a certain threshold, it is considered that a loop closure is formed.

[0118] Specifically, for the backend graph optimization described in step 6, after successfully identifying two closed loops, g2o is called. The pose of each frame is used as a node, the pose constraint between each pair of frames is used as an edge, and the constraints generated by loop closure detection are added to the graph. All nodes are adjusted by the global optimizer to minimize the error, and the optimized poses are updated to the system to correct the error in the map.

[0119] The following are specific embodiments based on the 4D millimeter-wave radar SLAM system:

[0120] Embodiment 1

[0121] As Figure 1 shown, the present invention provides a 4D millimeter-wave SLAM method.

[0122] In this embodiment, we consider the 4D millimeter-wave SLAM method in sunny low-speed environments and snowy high-speed environments, including the following steps:

[0123] Step 1, build a data acquisition platform to collect data.

[0124] Specifically, in terms of sensor configuration, the present invention adopts a multi-sensor fusion scheme, integrating sensors such as cameras, millimeter-wave radars, lidars, infrared cameras, and inertial measurement units (IMUs). The present invention uses Figure 2 the modified remote control car shown in Figure 3 to collect data in a low-speed sunny environment. On the other hand, as

[0125] shown in Figure 4 the present invention installs the remote control car on the top of the car for data collection in a high-speed snowy environment.

[0126] Step 2, data processing, including sensor calibration, data format conversion, etc.;

[0127] Specifically, for the sensor data in terms of time, the goal is to align the timestamps of each sensor to ensure that the data they collect corresponds to the same event or object at the same time point. The present invention utilizes the time synchronization library message_filters provided by ROS and adopts the Approximate Time Synchronizer synchronization mechanism to synchronize the sensor data such as lidar, camera, and IMU to a single time reference, ensuring that the collected data corresponds to the same object at the same time point.

[0128] Furthermore, perform spatial calibration on the sensors to determine the spatial positions and poses of each sensor, enabling their observation results to be uniformly mapped to the same coordinate system. The present invention uses the calibration board method. A calibration board with known features is used, and different sensors are made to observe it, enabling the lidar and 4D millimeter-wave radar to estimate their extrinsic parameters by detecting the geometric features of the calibration board.

[0129] Step 3, 4D millimeter-wave radar data enhancement;

[0130] Specifically, using the result of extrinsic parameter calibration, convert the lidar point cloud P to the millimeter-wave radar coordinate system. For each point p in the millimeter-wave radar, search for points within a certain neighborhood around it in the point cloud P and interpolate them around p, and use the Doppler velocity and signal intensity of p as the Doppler velocity and signal intensity of the new interpolated points.

[0131] Furthermore, for data collection in a low-speed sunny environment, since the lidar point cloud is less affected by weather, the present invention sets the fusion threshold to 5 meters to obtain as much lidar point data as possible. In a high-speed snowy environment, the lidar is greatly affected by extreme weather. Therefore, the present invention selects a smaller fusion threshold, set to 1 meter, to ensure the effectiveness and accuracy of the data.

[0132] Step 4, perform point cloud registration on the point cloud sequence to obtain rough front-end odometry information;

[0133] Step 4.1, find each point p i ∈P's nearest point q j in Q, which is achieved by minimizing the following function:

[0134] d(p i - q j ) = ||p i - q j ||2

[0135] For each point p i , find the corresponding nearest point q j as

[0136]

[0137] Accelerate the above process using the KD-tree of the nearest neighbor search algorithm.

[0138] Step 4.2, estimate the rigid body transformation for aligning the point cloud P to Q, and minimize the following objective function:

[0139]

[0140] Estimate the rotation R and translation t using the centroid alignment method. First, calculate the centroids of P and Q:

[0141]

[0142] Translate the point clouds P and Q to the centroid

[0143]

[0144] Calculate the covariance matrix H:

[0145]

[0146] Perform singular value decomposition on the matrix:

[0147] H = U∑V T

[0148] The optimal rotation matrix R is given by

[0149] R = VU T

[0150] The optimal translation vector t is

[0151]

[0152] Step 4.3, update the point cloud P using the transformation (R, t) obtained through the above steps

[0153] p i ← Rp i + t

[0154] And perform the nearest point matching again, repeat steps 4.1 and 4.2 until the objective function E(R, t) is less than the threshold ε or the maximum number of iterations is reached:

[0155] ||E k+1 (R, t) - E k (R, t)|| < ε

[0156] Step 5, perform loop closure detection for each frame to determine whether the positioning trajectory forms a loop, which is convenient for subsequent map optimization;

[0157] Specifically, the current key frame is compared with the key frames in the database to determine whether the platform has repeatedly reached the same position. First, closed-loop pre-filtering is performed according to certain rules to pre-filter potential candidate frames in advance. Second, the context of the current key frame is constructed. Due to sensor limitations and multiple reflected echoes, the radar altitude information is noisy, so the intensity scan context is used instead of the scan context. If the similarity between the context of the current key frame and the context of the database key frame reaches a certain threshold, it is considered that a closed loop is formed.

[0158] Step 6, use the open-source library g2o for backend graph optimization to obtain an accurate positioning trajectory.

[0159] Specifically, after successfully identifying two closed-loop frames, call g2o, use the pose of each frame as a node, the pose constraint between each pair of frames as an edge, and add the constraints generated by the closed-loop detection to the graph. Adjust all nodes through the global optimizer to minimize the error, and update the optimized pose to the system to correct the error in the map.

[0160] Next, a quantitative evaluation experiment is conducted on the 4D millimeter-wave SLAM method provided by the present invention. The present invention uses the SLAM evaluation tool RPG to evaluate the results.

[0161] Figure 5 Shows the relative translation error (RTE) of the 4D millimeter-wave SLAM method of the present invention in a low-speed sunny environment, which represents the translation error within adjacent time points in the trajectory. The result graph is represented by a box plot. The x-axis represents the cumulative distance traveled by the data acquisition platform along the trajectory, and the y-axis represents the relative translation error in meters. It compares the trajectory estimated by the SLAM algorithm with the ground truth at regular intervals. As can be seen from the figure, the translation error of the 4D millimeter-wave SLAM system of the present invention fluctuates slightly with the travel distance. For short distances (0m to 39m), the median translation error is stable. As the distance increases, the error distribution shows a slight upward trend, and the interquartile range and whiskers both expand slightly. Generally speaking, the present invention has relatively few outliers in terms of translation and has relatively stable performance.

[0162] Figure 6 Shows the rotation error of the trajectory of the 4D millimeter-wave SLAM method of the present invention in a low-speed sunny environment after SIM(3) alignment. Specifically, Figure 6It shows the trend of the position drift along different axes (x, y, z) changing with the increase of distance. The position drift on the x-axis shows relatively violent fluctuations as a whole, and the drift amount fluctuates in the range of -10,000 mm to 0 mm. This may indicate that the drift error in the direction of this axis is the largest and there is a large cumulative error. In contrast, the drift of the y-axis is relatively stable, with a relatively small fluctuation range, between -5,000 mm and 0 mm. This shows that the drift error in the y-axis direction is relatively small, but there is still a negative drift. The drift of the z-axis is relatively small and changes relatively stably. The drift amount fluctuates less, which may indicate that the pose estimation error in the z-axis direction is relatively small. Generally speaking, although the drifts on different axes are different, the error does not show an exponential growth trend with the increase of distance, which indicates that the positioning error of the system is generally controllable and there is no serious divergence phenomenon. This shows that the positioning of the system is relatively stable in most cases.

[0163] Figure 7 It is the comparison result of the estimated trajectory and the ground truth trajectory in a low-speed sunny environment, clearly showing the trends of the two trajectories. It can be seen from the figure that the blue "Estimate" (estimated value) and the gray "Groundtruth" (true value) trajectories are relatively consistent in the overall direction and shape, indicating that the positioning result of the algorithm is generally close to the real situation. Although there are some deviations, the overall estimated trajectory has a high degree of coincidence with the real trajectory, indicating that the performance of the algorithm in global positioning is reliable, especially for large-range displacement estimation. This deviation can be reduced by further optimization or higher-precision sensor data. The current algorithm can already achieve a relatively satisfactory effect without additional optimization, which provides a good foundation for future improvement.

[0164] Figure 8 and Figure 9They are the trajectory side view and the trajectory top view of the comparison result between the estimated trajectory and the ground truth trajectory in the snow-covered highway environment. It can be seen from the side view that there is almost no significant deviation between the blue "Estimate" (estimated value) and the gray "Groundtruth" (true value) trajectories in the height direction, and the error is very small. This indicates that the positioning result of the algorithm in the vertical height is very accurate and can follow the true trajectory well. Overall, the system has high precision in height estimation, ensuring consistency in the longitudinal direction, further proving that the global positioning ability of the algorithm is close to the real situation and has strong reliability. It can be seen from the top view that in the first two-thirds of the trajectory, the error between the estimated value and the true value is small, and the trajectory estimation highly coincides with the ground truth trajectory, showing good precision and stability. This shows that in the initial and middle segments, the sensors and algorithms of the system work normally and can effectively process environmental information and provide accurate positioning. However, in the last one-third of the trajectory, due to the influence of harsh weather conditions such as snow, the performance of the sensors fluctuates greatly, resulting in the estimated trajectory deviating from the true trajectory. This may be due to the light reflection or sensor signal interference caused by snow, making the environmental information obtained by the SLAM system unclear or inaccurate. However, despite this, the overall performance of the system is still relatively good, and most of the estimated trajectories are consistent with the true trajectories, proving that the system still has strong positioning ability and reliability in complex environments.

[0165] As Figure 10 shown, the 4D millimeter-wave SLAM system provided by the embodiment of the present invention includes:

[0166] A data collection module that builds a data acquisition platform to collect data;

[0167] A data processing module for data processing, including sensor calibration, data format conversion, etc.;

[0168] A data enhancement module for 4D millimeter-wave radar data enhancement;

[0169] A point cloud registration module that performs point cloud registration on the point cloud sequence to obtain rough front-end odometry information;

[0170] A loop detection module that performs loop detection on each frame to determine whether the positioning trajectory forms a loop, facilitating subsequent map optimization;

[0171] A back-end map optimization module that uses the open-source library g2o for back-end map optimization to obtain an accurate positioning trajectory.

[0172] An application embodiment of the present invention provides a computer device, which includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the 4D millimeter-wave SLAM method.

[0173] An application embodiment of the present invention provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the 4D millimeter-wave SLAM method.

[0174] An application embodiment of the present invention provides an information data processing terminal, which includes a 4D millimeter-wave SLAM system.

[0175] In the field of autonomous driving, SLAM technology is the key to autonomous positioning and environmental perception. The 4D millimeter-wave radar and lidar fusion SLAM system proposed by the present invention can effectively improve the real-time positioning accuracy and map construction ability of autonomous driving vehicles in complex environments. Compared with traditional SLAM methods, this technology is particularly suitable for scenarios such as bad weather (rain, snow, haze), low light (night driving), high-speed driving, and urban complex roads. Through the stability of millimeter-wave radar and the high-precision point cloud of lidar, this system can provide more robust autonomous navigation capabilities, reduce the impact of environmental interference on autonomous driving, improve driving safety, and provide key support for L3 and above levels of autonomous driving.

[0176] The SLAM system of the present invention has wide applications in robotics, especially in the fields of service robots, logistics robots, industrial robots, inspection robots, etc. Robots rely on high-precision SLAM for autonomous positioning, map construction, and path planning, and the 4D millimeter-wave radar + lidar fusion solution of the present invention can ensure the stable operation of robots in complex and dynamic environments. For example, in warehousing logistics, robots can accurately navigate in challenging scenarios such as occlusion, low light, and narrow channels; in intelligent security patrols, robots can monitor all-weather and adapt to changing environments, improving the task completion rate. The SLAM solution of the present invention not only improves the adaptability of robots in different environments, but also enhances the obstacle avoidance ability and path optimization efficiency, broadening the application scope of robotics.

[0177] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated designed hardware. Those of ordinary skill in the art can understand that the above devices and methods can be implemented using computer-executable instructions and / or included in processor control code, for example, such code is provided on a carrier medium such as a disk, CD, or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. The devices and their modules of the present invention can be implemented by hardware circuits of programmable hardware devices such as very large scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, or can be implemented by a combination of the above hardware circuits and software, such as firmware.

[0178] As described above, the above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention should be covered by the protection scope of the present invention.

Claims

1. A 4D millimeter-wave SLAM method, characterized in that, Including: Step 1: Build a data acquisition platform to collect data; Step 2: Data processing, including sensor calibration and data format conversion; Step 3: 4D millimeter-wave radar data enhancement; Step 4: Perform point cloud registration on the point cloud sequence to obtain rough front-end odometry information; Step 5: Perform loop closure detection on each frame to determine whether the positioning trajectory forms a loop, facilitating subsequent map optimization; Step 6: Use the open-source library g2o for backend map optimization to obtain accurate positioning trajectories.

2. The 4D millimeter-wave SLAM method according to claim 1, characterized in that, For the construction of the data acquisition platform and data collection in Step 1, in terms of sensor configuration, a multi-sensor fusion scheme is adopted, including cameras, millimeter-wave radars, lidars, infrared cameras, GPS, and inertial measurement units; by synchronizing and calibrating the data of these sensors in time and space, comprehensive and accurate environmental perception information can be obtained; cameras provide rich visual information for object detection, tracking, and semantic segmentation; millimeter-wave radars and lidars provide accurate distance and speed measurements and have better adaptability to bad weather and lighting conditions; infrared cameras can work in low-light or even completely dark environments and provide thermal imaging information for detecting pedestrian and animal heat source targets; IMUs provide high-frequency motion state estimation for compensating the delays and errors of other sensors.

3. The 4D millimeter-wave SLAM method according to claim 1, characterized in that, The data processing described in Step 2 is as follows: Step 2.1: Align the time of sensor data. The goal is to align the timestamps of each sensor to ensure that the data they collect corresponds to the same event or object at the same time point; in the present invention, the time synchronization library message_filters provided by ROS is used, and the Approximate Time Synchronizer synchronization mechanism is adopted to synchronize the lidar, camera, and IMU sensor data to a time reference, ensuring that the collected data corresponds to the same object at the same time point; Step 2.2: Perform spatial calibration on the sensors to determine the spatial positions and poses of each sensor, so that their observation results can be uniformly mapped to the same coordinate system; in the present invention, the calibration board method is used, and a calibration board with known features is used, and different sensors are allowed to observe it, so that the laser and 4D millimeter-wave radar estimate their external parameters by detecting the geometric features of the calibration board; Step 2.3: 4D millimeter-wave radar point cloud data format conversion; in the present invention, wireshark is used to capture 4D millimeter-wave radar data during data collection to obtain a network packet capture file in pcap format; in order to be compatible with the SLAM system, the present invention reads the json file parsed by wireshark, extracts the original data of the radar, packs these data into ROS messages, publishes and saves them in a bag format file; In this way, the present invention stores the data of the 4D millimeter-wave radar and the data of other sensors in a unified bag file, facilitating seamless integration into an autonomous driving project developed based on ROS.

4. The 4D millimeter-wave SLAM method according to claim 1, wherein In the 4D millimeter-wave radar point cloud data enhancement described in Step 3, the lidar point cloud is used to enhance the data of the 4D millimeter-wave radar. Using the result of extrinsic calibration, the lidar point cloud P is transformed into the millimeter-wave radar coordinate system. For each point p in the millimeter-wave radar, points within a certain neighborhood around it in the point cloud P are searched and interpolated around p, and the Doppler velocity and signal intensity of p are used as the Doppler velocity and signal intensity of the new interpolated points. The "voxel-based upsampling" interpolation method is used to enhance the data. The 4D millimeter-wave radar point cloud data is voxelized, dividing the three-dimensional space into a series of small cube cells. Then, the voxel grid is upsampled using the nearest neighbor interpolation or trilinear interpolation method to obtain a new voxel grid. After interpolation, Gaussian filtering is applied to the new voxel grid to eliminate aliasing and block effects.

5. The 4D millimeter-wave SLAM method according to claim 1, wherein, Perform point cloud registration on the point cloud sequence described in step 4 to obtain rough front-end odometry information. For adjacent point cloud frames P = {p1, p2,......, p n} and Q = {q1, q2,..., q m}, the goal is to register them through a rigid transformation (R, t) so that the points in P coincide with the points in Q as much as possible after the rigid transformation; R ∈ SO(3) is the rotation matrix; is the translation vector; the specific process is as follows: Step 4.1, find each point p i ∈P's nearest point q in Q j , achieved by minimizing the following function: d(p i -q j ) = ||p i -q j ||² For each point p i , find the corresponding nearest point q j For The KD-tree nearest neighbor search algorithm is used to accelerate the above process; Step 4.2, estimate the rigid body transformation that aligns the point cloud P to Q, minimizing the following objective function: Use the centroid alignment method to estimate the rotation R and translation t; first calculate the centroids of P and Q: Translate the point clouds P and Q to the centroid Calculate the covariance matrix H: Perform singular value decomposition on the matrix: H = U∑V T The optimal rotation matrix R is given by the following formula R = VU T The optimal translation vector t is Step 4.3, update the point cloud P using the transformation (R, t) obtained through the above steps p i ←Rp i +t And perform the closest point matching again, repeating Steps 4.1 and 4.2 until the objective function E(R, t) is less than the threshold ε or the maximum number of iterations is reached: ||E k+1 (R,t)-E k (R,t)||<ε。 6. The 4D millimeter-wave SLAM method according to claim 1, characterized in that, For the closed-loop detection of each frame described in Step 5, it is necessary to compare the current key frame with the database key frames to determine whether the platform has repeatedly reached the same position; first, perform closed-loop pre-filtering according to certain rules to pre-filter potential candidate frames in advance; Secondly, construct the context for the current key frame; due to sensor limitations and multiple reflected echoes, the radar altitude information has noise, so the intensity scan context is used instead of the scan context; if the similarity between the context of the current key frame and the context of the database key frame reaches a certain threshold, it is considered that a closed loop is formed; For the backend graph optimization described in Step 6, after successfully identifying the closed loop between two frames, call g2o, use the pose of each frame as a node, the pose constraint between each pair of frames as an edge, and add the constraints generated by the closed-loop detection to the graph. Adjust all nodes through the global optimizer to minimize the error and update the optimized pose to the system to correct the error in the map.

7. A 4D millimeter-wave SLAM system for implementing the 4D millimeter-wave SLAM method according to any one of claims 1 to 6, characterized in that, Include: Data collection module, build a data acquisition platform to collect data; Data processing module, data processing, including sensor calibration and data format conversion; Data enhancement module, 4D millimeter-wave radar data enhancement; Point cloud registration module, perform point cloud registration on the point cloud sequence to obtain rough front-end odometry information; Closed-loop detection module, perform closed-loop detection on each frame to determine whether the positioning trajectory forms a closed loop, facilitating subsequent graph optimization; Backend graph optimization module, use the open-source library g2o for backend graph optimization to obtain an accurate positioning trajectory.

8. A computer device, the computer device includes a memory and a processor, the memory stores a computer program, when the computer program is executed by the processor, the processor is caused to execute the steps of the 4D millimeter-wave SLAM method described in any one of claims 1 to 6.

9. A computer-readable storage medium stores a computer program, when the computer program is executed by the processor, the processor is caused to execute the steps of the 4D millimeter-wave SLAM method described in any one of claims 1 to 6.

10. An information data processing terminal, the information data processing terminal includes the 4D millimeter-wave SLAM system described in claim 7.

Images