Simultaneous localization and mapping method based on heterogeneous multimodal data fusion

Abstract

The present invention belongs to the field of unmanned driving technology, and specifically discloses a synchronous positioning and mapping method based on heterogeneous multimodal data fusion. The present invention solves the problems existing in traditional positioning and mapping methods when applied to complex environments such as tunnels by means of data fusion of millimeter-wave radar and lidar, filtering of dynamic objects, enhancement of loop detection, and global consistency optimization, and provides a more accurate, stable, and adaptable positioning and mapping method. The method of the present invention can be applied to the positioning and mapping of autonomous mobile devices such as drones and robots in dynamic environments such as tunnels, and can provide stable positioning and map building capabilities in the presence of dynamic obstacles (such as construction equipment, vehicles, etc.) and low light conditions, significantly improving the positioning accuracy, map building capabilities, and system stability of drones or robots in complex dynamic environments.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving technology, and in particular relates to a synchronous positioning and mapping method based on heterogeneous multimodal data fusion, which is particularly suitable for navigation and positioning application scenarios in complex environments such as tunnels and underground mines. Background Art

[0002] With the continuous development of automation and autonomous mobile device technology, the use of autonomous mobile devices such as drones and robots in various application scenarios has gradually become popular, especially in complex environments such as tunnels and underground mines, making positioning and mapping technology also widely used in the above environments. In the field of simultaneous localization and mapping (SLAM), the term "heterogeneous" refers to the integration of different types of sensor data, which differ in data type, measurement principle, accuracy, coverage, etc. The goal of heterogeneous sensor data fusion in SLAM is to take advantage of the advantages of these different sensors and overcome their respective limitations to achieve more accurate and robust positioning and mapping.

[0003] Traditional positioning and mapping technologies include positioning and mapping based on sensor multimodal data, simultaneous positioning and mapping of lidar and IMU, data fusion methods of lidar and millimeter-wave radar, dynamic object detection and filtering technology, etc.

[0004] In terms of positioning and mapping based on multimodal sensor data, many methods based on multi-sensor data fusion have been applied to the positioning and mapping systems of drones and autonomous vehicles, especially through the fusion of sensors such as lidar, IMU, millimeter-wave radar, and vision systems. However, there are still many challenges in the application of these methods in complex environments such as tunnels, such as interference from dynamic objects, differences in sensor accuracy, and the complexity of data fusion.

[0005] In terms of laser radar and IMU synchronous positioning and mapping, laser radar and IMU synchronous positioning and mapping is to simultaneously estimate the drone's posture and build an environmental map by optimizing the factor graph. This method performs well in static environments, but due to the lack of effective filtering and processing of dynamic objects, its stability and accuracy are poor in complex dynamic environments.

[0006] In terms of data fusion between LiDAR and millimeter-wave radar, some studies have proposed fusing the data of LiDAR and millimeter-wave radar to make up for their respective shortcomings. For example, some solutions match the speed information of millimeter-wave radar with the point cloud data of LiDAR to enhance the recognition ability of dynamic objects. However, these methods fail to make full use of the speed information of millimeter-wave radar to optimize positioning accuracy, and lack an effective loop detection mechanism, resulting in the overall positioning accuracy still not being high enough.

[0007] In terms of dynamic object detection and filtering technology, existing dynamic object detection methods usually rely on traditional speed threshold filtering or motion analysis-based algorithms (such as Kalman filtering, particle filtering, etc.). These dynamic object detection methods can perform preliminary filtering of dynamic objects, but there are still great challenges for high-precision positioning and map construction, especially in specific environments such as tunnels. The accuracy of current dynamic object detection methods is often difficult to meet the requirements.

[0008] In summary, in complex environments such as tunnels and underground mines, due to the presence of dynamic obstacles, poor lighting conditions, and complex terrain structures, traditional positioning and mapping technologies are difficult to meet the requirements of high precision and high stability. In summary, when traditional positioning and mapping methods are applied in the above complex environments, they often have the following problems:

[0009] 1. Lack of effective fusion of sensor data: Although the traditional LiDAR and IMU data fusion method can provide a certain positioning accuracy, it often ignores the impact of dynamic objects in the external environment. Millimeter-wave radar, as a supplementary sensor, can provide object velocity information and play a key role in dynamic object recognition and removal of static point clouds. However, existing technologies have not fully utilized the advantages of millimeter-wave radar and LiDAR for efficient fusion.

[0010] 2. Interference from dynamic objects: In complex environments such as tunnels, dynamic objects (such as construction equipment and moving vehicles) often appear within the sensing range of LiDAR and IMU, resulting in reduced accuracy in pose estimation and map construction. Most existing methods rely on simple static and dynamic point cloud classification methods, which cannot effectively filter out dynamic obstacles, resulting in poor system stability.

[0011] 3. Challenges of loop closure detection: Loop closure detection is particularly important in closed environments such as tunnels. Traditional loop closure detection methods mostly rely on lidar or visual sensors, which usually have problems such as over-reliance on features or poor adaptability to dynamic environments, resulting in failure or inaccuracy of loop closure detection.

[0012] 4. Differences in sensor accuracy: LiDAR and millimeter-wave radar differ in resolution, measurement method, and data update frequency. How to effectively fuse the data from these two types of sensors is the main challenge facing current technology. Summary of the invention

[0013] The purpose of the present invention is to propose a synchronous positioning and mapping method based on heterogeneous multimodal data fusion. This method provides a more accurate, stable and adaptable positioning and mapping method by combining the data fusion of millimeter-wave radar and lidar, filtering of dynamic objects, enhancement of loop detection and global consistency optimization, which significantly improves the positioning accuracy, map construction capability and system stability of autonomous mobile devices such as drones or robots in complex dynamic environments.

[0014] In order to achieve the above object, the present invention adopts the following technical scheme:

[0015] The synchronous positioning and mapping method based on heterogeneous multimodal data fusion includes the following steps:

[0016] Step 1. Align the point clouds of the millimeter-wave radar and the lidar through time synchronization and external parameter calibration, and calibrate the relative poses of the IMU, the millimeter-wave radar, and the lidar; the millimeter-wave radar uses a 4D millimeter-wave radar;

[0017] Step 2. Use laser radar and millimeter wave radar to perceive the environment in three dimensions and obtain a perceived 3D point cloud;

[0018] Step 3. Preprocess the millimeter wave radar data and filter dynamic objects;

[0019] Step 4. Use the distance-based interpolation method to fuse and match the high-resolution point cloud acquired by the lidar and the low-resolution static point cloud acquired by the millimeter-wave radar and preprocessed.

[0020] Calculate the difference between the laser radar point and the millimeter wave radar point, and introduce it into the factor graph of LIO-SAM as an error term. This error term is the fusion error of the millimeter wave radar.

[0021] Step 5. Extract the velocity information of the millimeter-wave radar and calculate the velocity of the IMU; by constructing an error function, calculate the difference between the millimeter-wave radar velocity and the IMU velocity, and provide additional velocity constraints for the LIO-SAM factor graph;

[0022] Step 6. Use the millimeter wave radar point cloud to provide static point cloud constraints for the LIO-SAM factor graph;

[0023] Step 7. Use the millimeter-wave radar to supplement the loop closure constraint, and use the weighted voting mechanism to fuse the loop closure detection results of the millimeter-wave radar with the loop closure detection results of the lidar to determine the final loop closure detection result;

[0024] Step 8. Based on the final result of loop closure detection, construct the joint loop closure constraint of LiDAR and millimeter wave radar;

[0025] Step 9. Take the fusion error of step 4, the speed constraint of step 5, the static point cloud constraint of step 6, and the joint loop constraint of step 8 as constraint factors and add them to the factor graph of LIO-SAM to construct the factor graph; optimize the factor graph to obtain the system's pose estimation and map feature point distribution, and realize the simultaneous positioning and mapping of SLAM.

[0026] The present invention has the following advantages:

[0027] As described above, the present invention relates to a synchronous positioning and mapping method based on heterogeneous multimodal data fusion. The method solves the key problems existing in the prior art by combining the data fusion of millimeter-wave radar and laser radar, filtering of dynamic objects, enhancement of loop detection, and global consistency optimization, etc., and provides a more accurate, stable, and adaptable positioning and mapping method, which significantly improves the positioning accuracy, map building capability, and system stability of drones or robots in complex dynamic environments, and is particularly suitable for drone and robot navigation in closed environments such as tunnels. Specifically, in terms of millimeter-wave radar and laser radar data fusion, the present invention solves the difference in millimeter-wave radar and laser radar coordinate systems through precise external parameter calibration, realizes the alignment and fusion of the point cloud data of the two, and enables the data of the two sensors to complement each other and give full play to their respective advantages. Due to the difference in resolution between laser radar and millimeter-wave radar, the present invention adopts a distance-based interpolation method to match the high-resolution point cloud of the laser radar with the low-resolution point cloud of the millimeter-wave radar, solving the problem of inconsistent data of the two sensors. In terms of dynamic object detection and static point cloud separation, the present invention extracts the speed information of dynamic objects through the Doppler effect of millimeter wave radar, and distinguishes static point clouds from dynamic point clouds in combination with the set speed threshold, thereby effectively filtering dynamic objects and avoiding their interference with subsequent positioning and mapping processes. Through the identification and filtering of dynamic objects, static point clouds containing only environmental structural features are extracted to provide more accurate data input for subsequent LIO-SAM processing. In terms of optimized speed information processing, the present invention calculates the difference between the speed information provided by the millimeter wave radar and the speed estimated by the IMU, constructs an error function and introduces it into the factor graph optimization, provides external speed constraints for the IMU, and reduces the estimation error when there is no GPS positioning for a long time. In terms of global consistency optimization and accuracy improvement, the present invention uses the speed information of the millimeter wave radar to provide additional motion constraints, helping the IMU system to more accurately estimate the motion trajectory of the object, avoiding integral drift in the absence of external positioning signals such as GPS, thereby ensuring positioning accuracy. In terms of static point cloud constraints, the present invention introduces fixed obstacles or features (such as buildings, walls, etc.) in the static environment, and adds these point cloud data as constraints to the factor graph, which can provide a long-term "anchoring" effect for posture optimization, help reduce cumulative errors, and improve the accuracy of robot posture estimation, the quality of environmental maps, and robustness in dynamic scenes. In terms of loop detection and constraint optimization, the present invention performs loop detection through the target features of the millimeter-wave radar (such as reflection intensity, position, speed), and combines it with the loop detection results of the lidar, and adopts a weighted voting method to ensure the accuracy of loop matching. Especially in low-feature or dynamic environments, the present invention introduces the loop constraints of the millimeter-wave radar together with the constraints of the lidar and IMU into the factor graph optimization of LIO-SAM, optimizes the global consistency of the system, and avoids errors caused by positioning drift in the system.The present invention uses a nonlinear least squares method (such as the Levenberg-Marquardt algorithm) to perform global consistency optimization on the data of all sensors (lidar, millimeter-wave radar, IMU), minimizes the errors of all constraints, and improves the overall accuracy of positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a flow chart of the synchronous positioning and mapping method based on heterogeneous multimodal data fusion in the present invention. DETAILED DESCRIPTION

[0029] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0030] This embodiment describes a synchronous positioning and mapping method based on heterogeneous multimodal data fusion, which aims to solve the problems of dynamic object interference, sensor data accuracy difference, loop detection failure, etc. in the prior art, especially in application scenarios in complex environments such as tunnels. Specifically, the present invention achieves the goal in the following ways: In terms of effectively filtering dynamic object interference, the present invention accurately distinguishes static and dynamic objects through the speed information of the millimeter-wave radar, and removes the interference of dynamic objects in the point cloud data, thereby improving the stability and positioning accuracy of the system. In terms of optimizing the fusion of laser radar and millimeter-wave radar data, the present invention efficiently fuses the high-resolution point cloud of the laser radar with the low-resolution point cloud of the millimeter-wave radar through precise external parameter calibration and distance-based interpolation methods, making up for the shortcomings of each sensor and overcoming the problem of resolution difference. In terms of enhancing loop detection capabilities, the present invention combines the loop detection results of the laser radar and the millimeter-wave radar, and effectively improves the reliability of loop detection through a weighted voting mechanism, especially in dynamic and feature-sparse environments, to ensure the efficiency and accuracy of loop detection. In terms of improving global consistency and positioning accuracy, the present invention introduces the speed constraint of millimeter-wave radar into the optimization of the LIO-SAM factor graph for synchronous positioning and mapping of the laser radar and IMU, effectively avoiding the integral drift of the IMU when there is no external positioning signal for a long time, and ensuring the high-precision positioning and mapping of the system in closed environments such as tunnels. Through the above technical solution, the present invention can significantly improve the positioning accuracy and system stability of drones or robots in complex dynamic environments, and is particularly suitable for environments with complex structures and many dynamic obstacles such as tunnels and underground mines.

[0031] like Figure 1 As shown in FIG, the synchronous positioning and mapping method based on heterogeneous multimodal data fusion includes the following steps:

[0032] Step 1. Align the point clouds of the millimeter-wave radar and lidar through time synchronization (such as GPS timestamp) and external parameter calibration (relative pose), and calibrate the relative pose of the IMU, millimeter-wave radar, and lidar.

[0033] In this embodiment, the millimeter wave radar adopts a 4D millimeter wave radar.

[0034] The laser radar and millimeter wave radar are in the laser radar coordinate system and millimeter wave radar coordinate system respectively. The relative position between the two coordinate systems is obtained by external parameter calibration, that is, the rotation matrix and translation vectors .

[0035] Among them, the rotation matrix Describes the rotation of the laser radar coordinate system relative to the millimeter wave radar coordinate system; the translation vector Describes the translation of the origin of the lidar coordinate system relative to the origin of the millimeter wave radar coordinate system.

[0036] Through external parameter calibration, the problem of coordinate system differences between millimeter-wave radar and lidar is well solved. The calibrated data can be seamlessly integrated to make up for the limitations of a single sensor and improve the reliability and accuracy of the data.

[0037] The present invention realizes the spatial alignment of millimeter wave radar and lidar data through time synchronization and external parameter calibration, ensuring that the data of the two can be effectively integrated, thereby laying the foundation for subsequent positioning and mapping.

[0038] Step 2. Use laser radar and millimeter wave radar to perceive the environment in three dimensions and obtain the perceived 3D point cloud.

[0039] Specifically, laser radars and millimeter-wave radars carried by autonomous motion devices such as drones and robots are used to perform three-dimensional perception of the environment, generate point cloud data, and provide raw data for subsequent map construction and positioning.

[0040] The present invention utilizes laser radar and millimeter wave radar to perform three-dimensional perception of the environment, which has the following two advantages.

[0041] In terms of efficient environmental perception, millimeter-wave radar can reduce the impact of moving objects (such as pedestrians, other vehicles, etc.) in dynamic environments, provide more stable perception capabilities for static obstacles, and complement the high-resolution advantages of laser radar, thus improving the comprehensiveness of environmental perception. In terms of adapting to complex environments, especially in low-light environments such as tunnels and underground mines, millimeter-wave radar can provide stable detection capabilities, making up for the shortcomings of laser radar in these environments.

[0042] Step 3. Preprocess the millimeter-wave radar data and filter dynamic objects.

[0043] Step 3.1. Extract speed information. First, perform target extraction and extract points with high reflection intensity in the millimeter-wave radar point cloud, considering that these points may correspond to meaningful objects (such as metal facilities, vehicles, etc.).

[0044] Then, the speed information is extracted. Specifically, this embodiment uses the Doppler effect to extract the speed information of the target and distinguish between static targets and dynamic targets. The static target feature can be used as a supplementary benchmark for loop detection.

[0045] Millimeter wave radar measures the frequency shift of the reflected wave through the Doppler effect to obtain the speed information of the object. In a static environment, the speed of all points should be close to zero, while the speed of a dynamic object will deviate significantly from zero.

[0046] For each point cloud data in the millimeter wave radar data , Represents the point cloud coordinates, and the millimeter wave radar will provide a speed information associated with the point cloud , which indicates the moving speed of the point cloud.

[0047] Step 3.2. Set the speed threshold to identify dynamic targets , then the point cloud in the millimeter-wave radar data whose moving speed is less than the speed threshold is regarded as a static point cloud, and the point cloud whose moving speed exceeds the speed threshold is regarded as a dynamic point cloud.

[0048] Static point cloud, , dynamic point cloud, . According to the environmental characteristics in the tunnel, it can be adjusted Typically, the speed of dynamic obstacles such as vehicles or construction equipment will significantly exceed the speed of the static environment.

[0049] Step 3.3. According to the speed threshold preset in step 3.2 , separating the dynamic point cloud and the static point cloud in the millimeter-wave radar data; wherein, the dynamic point cloud in the millimeter-wave radar data is removed and only the static point cloud is retained.

[0050] Static point clouds are points with low speed, which reflect the structural features of the tunnel (such as walls, ground, ceiling, etc.). Dynamic point clouds are points with high speed, which represent moving objects and can be excluded from subsequent map construction.

[0051] The static point cloud will be used for subsequent LIO-SAM processing, and the dynamic point cloud will be removed to avoid affecting the system stability and accuracy.

[0052] The method of the present invention can achieve the purpose of accurately filtering dynamic objects, reducing error accumulation and ensuring the accuracy of point cloud data by preprocessing millimeter wave radar data.

[0053] In terms of accurate dynamic object filtering, the present invention utilizes the speed information of the millimeter-wave radar to identify dynamic objects, which significantly improves the stability and positioning accuracy of the system in a dynamic environment.

[0054] In terms of reducing error accumulation, the present invention avoids the negative impact of dynamic objects on map construction and pose estimation by removing these objects, thereby ensuring the accuracy of subsequent data processing.

[0055] In terms of ensuring the accuracy of point cloud data, static and dynamic objects are distinguished by extracting target reflection intensity and speed information from millimeter-wave radar data, and the interference of dynamic objects is removed to ensure that the point cloud data in the SLAM process is more accurate.

[0056] Step 4. Use the distance-based interpolation method to fuse and match the high-resolution point cloud obtained by the lidar and the low-resolution static point cloud obtained and preprocessed by the millimeter-wave radar to achieve the fusion and matching of the lidar and millimeter-wave radar data.

[0057] The difference between the laser radar point and the millimeter-wave radar point is calculated, and the difference between the laser radar point and the millimeter-wave radar point is introduced into the factor graph of LIO-SAM as an error term. The error term is the fusion error of the millimeter-wave radar.

[0058] The fused point cloud combines the high-resolution characteristics of the lidar and the dynamic object filtering capabilities of the millimeter-wave radar, providing high-quality input for subsequent mapping. The fused data will be used in all steps after step 5.

[0059] The method of the present invention is based on a distance interpolation method and can effectively deal with the problem caused by the difference in resolution between the two sensors. It is particularly suitable for the fusion and matching of lidar and millimeter-wave radar data in environments such as tunnels.

[0060] Step 4.1. Convert the lidar point cloud to the millimeter-wave radar coordinate system using the matrix obtained through extrinsic calibration.

[0061] LiDAR and millimeter-wave radar are usually in different coordinate systems. Assume that the relative position (position and direction) between the two coordinate systems is obtained through external calibration (external calibration matrix), that is, the rotation matrix and translation vectors .

[0062] Assume that the lidar point and millimeter wave radar points is the coordinate in each coordinate system, and the rotation matrix obtained by external parameter calibration and translation vectors To perform coordinate transformation:

[0063] ,in, are the coordinates of the converted lidar point in the millimeter-wave radar coordinate system.

[0064] For the converted feature points .

[0065] Calculate the difference between the lidar point and the mmWave radar point , and introduce it into the factor graph of LIO-SAM as an error term. The error term is the fusion error of the millimeter-wave radar, and its expression formula is as follows:

[0066] .

[0067] This error ensures the spatial consistency of the millimeter-wave radar and lidar fusion data.

[0068] Through precise external parameter calibration, lidar and millimeter-wave radar data can be seamlessly connected, improving environmental perception capabilities.

[0069] Step 4.2. For each lidar point, calculate the relative position of the lidar point in the millimeter-wave radar coordinate system and interpolate according to the resolution of the millimeter-wave radar.

[0070] Assume that the data of millimeter wave radar is the distance from the radar to the point and angle To represent, for the converted lidar point , calculate the corresponding coordinates of the point in the millimeter wave radar grid, the formula is as follows:

[0071] , , .

[0072] in, It indicates the horizontal angle of the target point relative to the millimeter-wave radar, that is, the azimuth angle of the target point detected by the millimeter-wave radar. Indicates the elevation angle of the target point relative to the millimeter-wave radar, applicable to the polar coordinate system in three-dimensional space.

[0073] Step 4.3. For each lidar point, find the nearest point or the closest area found by the kd-tree in the mmWave lidar point cloud and fill in the missing points using linear interpolation.

[0074] For each lidar point , the coordinates of the converted laser radar point in the millimeter wave radar coordinate system are , use KD tree to find the nearest neighbor point in the millimeter wave radar point cloud , calculate their spatial distance:

[0075] ,in, The coordinates of the converted laser radar point in the millimeter wave radar coordinate system are and the nearest neighbor in the millimeter wave radar point cloud The distance between.

[0076] Step 4.4. Use linear interpolation to calculate the value in the millimeter-wave radar data corresponding to the lidar point cloud; according to the spatial position of the lidar point, find the corresponding value in the output data grid of the millimeter-wave radar to fill.

[0077] Assume that in the point cloud of the millimeter wave radar, the lidar point The corresponding value is , and the nearest neighbor point in the millimeter wave radar point cloud The corresponding value is ,in, and are the measured values ​​of the laser radar point and the nearest millimeter wave radar point (such as reflection intensity, speed information, etc.). The linear interpolation is calculated according to the following formula:

[0078] .

[0079] in, Indicates the point cloud coordinates where the laser radar point cloud is interpolated in the millimeter wave radar coordinate system. Indicates the maximum range within which the interpolation between the current lidar point and adjacent points in the millimeter-wave radar point cloud is valid.

[0080] It is usually determined by the resolution of the mmWave radar grid or the sparsity of the point cloud.

[0081] According to the discreteness of the millimeter wave radar point cloud grid, It can be defined as the maximum point spacing of the millimeter wave radar point cloud or a preset interpolation range threshold. It is used to standardize the interpolation results.

[0082] Assume that the horizontal angle resolution of the millimeter wave radar is , the pitch angle resolution is , the distance resolution is ,but:

[0083] .

[0084] in, Indicates the distance of the target point currently scanned by the millimeter-wave radar. Determines the angular spacing of the scan points in space.

[0085] The distance-based interpolation method of the present invention effectively compensates for the resolution difference between the two sensors (laser and millimeter-wave radar), realizes efficient data fusion, provides higher-precision fused data, and provides support for subsequent positioning and mapping.

[0086] In addition, through precise external parameter calibration and distance-based interpolation methods, the high-resolution point cloud of the lidar and the low-resolution point cloud of the millimeter-wave radar are matched, solving the problem of resolution difference between the two.

[0087] Step 5. Use the velocity information of the millimeter-wave radar to provide additional motion constraints for the IMU, and introduce the obtained velocity constraint as a factor into the factor graph of the simultaneous positioning and mapping of the lidar and IMU LIO-SAM.

[0088] IMU obtains velocity by integrating acceleration, but is easily affected by noise accumulation and drift. Therefore, the present invention can limit the drift of IMU by taking the velocity information of millimeter-wave radar as a reference.

[0089] Step 5.1. Extract the velocity information of the millimeter wave radar.

[0090] Assume that the speed information obtained by the millimeter wave radar is , which represents the relative speed from the radar to the target object; the relative speed information of the millimeter-wave radar can supplement the deficiency of IMU speed estimation and be used to correct the integral drift error of IMU.

[0091] Step 5.2. Estimate the velocity of the IMU.

[0092] Acceleration provided by IMU and angular velocity It is used to integrate and calculate displacement and attitude, and the velocity can be obtained through acceleration data. , thereby estimating the object's motion trajectory.

[0093] The formula for IMU velocity estimation is as follows: .

[0094] in, is the IMU at time The acceleration function.

[0095] Step 5.3. Construct velocity constraints.

[0096] An error function is constructed based on the difference between the IMU estimated speed and the millimeter-wave radar measured speed and introduced into the factor graph optimization. This can effectively reduce the speed estimation error and improve the accuracy of the overall motion trajectory.

[0097] Specifically, the present invention is based on the speed of millimeter wave radar and the speed of the IMU An error function is constructed, the error between the two is calculated through the error function, and the error is introduced into the factor graph optimization as a speed constraint.

[0098] The error function formula that measures the difference between the velocity estimated by the IMU and the velocity of the millimeter wave radar is as follows:

[0099] .

[0100] in, is the error between the IMU and millimeter-wave radar velocity estimates, is the velocity obtained by IMU through acceleration integration, It is the relative speed of an object measured by millimeter-wave radar.

[0101] The method of the present invention provides external constraints for the IMU by utilizing the speed information of the millimeter-wave radar, thereby reducing the drift error of the IMU and optimizing the overall accuracy of positioning and motion trajectory estimation.

[0102] Specifically, IMU is prone to integration drift when there is no external positioning signal for a long time. The present invention uses the speed information provided by the millimeter wave radar to effectively constrain the drift and maintain the positioning accuracy of the system.

[0103] By minimizing the velocity error, the long-term stability and accuracy of the system in a GPS-free environment are significantly improved.

[0104] The velocity constraint is added as a factor to the factor graph of LIO-SAM. The factor graph of LIO-SAM contains constraints on pose and map feature points. By additionally introducing velocity constraints, the motion estimation of the IMU is made more stable.

[0105] In LIO-SAM, pose nodes (e.g., robot pose) and velocity nodes (e.g., velocity estimated by IMU) are connected by constraint factors, and the state of the system is optimized by minimizing the errors (including velocity errors) in the factor graph.

[0106] Since the velocity information of the millimeter-wave radar provides an external constraint for the IMU, the SLAM system can avoid the integration drift of the IMU when there is no external positioning (such as GPS) for a long time, ensuring the accuracy of the pose and velocity estimation.

[0107] In this embodiment, the millimeter wave radar not only provides speed constraints, but also supports the optimization of the lidar map through its point cloud data. In the LIO-SAM factor graph, by adding the speed constraints of the millimeter wave radar and the static point cloud constraints provided by the millimeter wave radar point cloud, the distribution of poses and feature points is jointly optimized to ensure the consistency of local motion estimation and global consistency.

[0108] Step 6. Use the millimeter wave radar point cloud to provide static point cloud constraints and introduce them into the factor graph optimization of LIO-SAM.

[0109] Using the millimeter-wave radar static point cloud preprocessed in step 3, construct the following error function:

[0110] .

[0111] in, The error introduced by the millimeter wave radar static point cloud, is the position of the point or target feature observed by the millimeter-wave radar, The position of the mmWave radar target feature calculated based on the current estimation state.

[0112] The optimization goal is to minimize the static point cloud constraints of the millimeter wave radar points .

[0113] The present invention optimizes the SLAM system state and reduces error propagation through the combined effect of millimeter-wave radar point cloud and velocity information. The millimeter-wave radar point cloud data is used as part of factor graph optimization, static point cloud constraints and velocity constraints are introduced, and the pose and feature point positions are jointly optimized to ensure local motion estimation and global consistency, thereby enhancing the stability of the SLAM system.

[0114] Step 7. Use the millimeter-wave radar to supplement the loop constraints, and use the weighted voting mechanism to fuse the loop detection results of the millimeter-wave radar with the loop detection results of the lidar to determine the final loop detection result.

[0115] Step 7.1. Target feature matching.

[0116] Extract target features (such as reflection intensity, position, and speed) from the millimeter-wave radar point cloud, and establish feature descriptors (such as reflection intensity + position + speed) for the target features extracted from the millimeter-wave radar point cloud.

[0117] Using the constructed feature descriptor, search for the best matching target feature in the constructed SLAM historical point cloud map.

[0118] Step 7.2. Use the millimeter-wave radar to supplement the loop closure constraints, and use the weighted voting mechanism to fuse the loop closure detection results of the millimeter-wave radar with the loop closure detection results of the lidar to determine the final loop closure detection result.

[0119] Loop detection is an important step in SLAM systems to eliminate cumulative errors. It adds global constraints to the factor graph by identifying the matching relationship between the current position and the previous position of the system (loop points).

[0120] In the SLAM simultaneous positioning and mapping system, a loop point refers to a point where the current posture detected by the system is similar to a previously visited location. This similarity indicates that there is an overlapping area between the current frame and the historical frame. Therefore, this information can be used to optimize the global posture and reduce the cumulative error.

[0121] The performance of LiDAR and millimeter-wave radar in dynamic environments complement each other. LiDAR performs better in feature-rich environments, while millimeter-wave radar has more advantages in dynamic or low-feature environments. The present invention can ensure the accuracy of loop detection results by enhancing the weight of LiDAR in feature-rich environments and increasing the weight of millimeter-wave radar in dynamic environments.

[0122] The specific process of using weighted voting to determine the final result of loop detection is given below.

[0123] Step 7.2.1. Set the weights of LiDAR and MMW radar according to the environment and sensor characteristics and In feature-rich environments, the weight of LiDAR is relatively high, i.e. ,In low feature or dynamic environments, the weight of millimeter-wave radar is relatively high, i.e. .

[0124] Step 7.2.2. Calculate the composite score.

[0125] Based on the matching relationship between the current frame and the historical map frame, all possible historical frames or map points that form a loop with the current frame are selected as candidate matching points. ; Calculate candidate matching points The comprehensive similarity score of , the formula is as follows:

[0126] .

[0127] in, It is the index of the candidate matching point, indicating the historical frame number that is currently detected as a possible loop point.

[0128] Should That is, the weighted sum of the similarity scores of lidar and millimeter-wave radar.

[0129] For the laser radar for candidate matching points The similarity score of The higher the score, the more likely the laser radar is to The greater the possibility of loop matching of the current frame.

[0130] For the candidate matching points of the millimeter wave radar The similarity score of The higher the score, the more the millimeter-wave radar believes that the historical frame The greater the possibility of loop matching of the current frame.

[0131] Step 7.2.3 Select the candidate frame with the highest comprehensive score as the final loop match.

[0132] According to the comprehensive similarity score obtained in step 7.2.2, the most matching historical frame is selected as the loop point to establish the basis for the subsequent loop constraint. The calculation formula of the final selected loop matching point index is as follows:

[0133] .

[0134] in is the index of the final selected loop matching point, indicating the comprehensive similarity score The highest candidate matching point, Indicates that from all candidate matching points In the above example, select the comprehensive similarity score The index of the largest point.

[0135] The most matching historical frame is the most similar frame between the current frame and the historical key frames selected through similarity measurement.

[0136] The decision-making process is to find the most likely loop point , which is the candidate matching point with the highest comprehensive score.

[0137] The present invention uses millimeter wave radar to supplement loop constraints, enhance the robustness of loop detection, and adapt to different environments.

[0138] Specifically, in terms of enhancing the robustness of loop detection, by fusing the loop detection results of millimeter-wave radar and lidar, the accuracy of loop detection is greatly improved, especially in feature sparse or dynamic environments.

[0139] In terms of adapting to different environments, the reliability and accuracy of loop detection are further improved by adjusting the weights of different environments (feature-rich or feature-less environments). The target features (such as reflection intensity, position, and speed) provided by the millimeter-wave radar are combined with the lidar loop detection results to perform loop detection, and the accuracy of matching is ensured through a weighted voting mechanism.

[0140] Step 8. Based on the final detection result of loop closure detection, the loop closure constraint of the millimeter-wave radar is added to the factor graph optimization, that is, the loop closure constraints of the lidar and millimeter-wave radar are combined to establish a joint loop closure constraint.

[0141] Based on the loop detection results of step 7, the joint constraints of the lidar and millimeter-wave radar are established.

[0142] Define the relative transformation constraints between two frames and introduce weight parameters; add loop constraint factors to the factor graph to work together with IMU, lidar and millimeter-wave radar to improve global consistency and optimization effects.

[0143] For the detected loop closure, loop closure constraints are established. The process of establishing the joint loop closure constraints is given below.

[0144] The loop closure constraints of the lidar are as follows: .

[0145] in represents the loop closure constraint of the lidar, is the position of the lidar in the current frame, is the pose of the lidar loop frame, is the relative transformation between two LiDAR frames.

[0146] The constraints of millimeter wave radar are as follows: .

[0147] in represents the constraints of millimeter wave radar, Indicates the position and posture of the millimeter-wave radar in the current frame. Represented as the pose of the loop frame of the millimeter wave radar; is the relative transformation between two frames of millimeter wave radar.

[0148] The joint loop constraints of the LiDAR and the millimeter-wave radar are constructed as follows:

[0149] .

[0150] in is the joint loop constraint, is the lidar weight, is the weight of the millimeter-wave radar. It can be dynamically adjusted according to the reliability of the laser radar and millimeter-wave radar. and The size of the value.

[0151] Loop closure is a key concept in the SLAM system, which refers to the robot or vehicle returning to a position that has been visited during operation and detecting the overlapping relationship between this position and the current observation. The loop constraint is a mathematical constraint established by the loop point on the spatial relationship between the current posture and the historical posture after the loop is detected, which is used to optimize the consistency of the global trajectory and map. The present invention optimizes the posture constraint and improves the global consistency and accuracy by combining the loop detection of LiDAR and millimeter-wave radar. Through the loop detection results, combined with the loop constraints of LiDAR and millimeter-wave radar, a joint loop constraint is established to further optimize the global consistency. In addition, the introduction of the joint loop constraint can effectively reduce the error of the positioning system, especially during long-term operation, reduce the impact of drift, and improve positioning accuracy.

[0152] Step 9. Take the fusion error of step 4, the velocity constraint of step 5, the static point cloud constraint of step 6, and the joint loop constraint of step 8 as constraint factors and add them to the factor graph of LIO-SAM to construct the factor graph.

[0153] The factor graph is optimized to obtain the system's pose estimation and map feature point distribution, realizing SLAM positioning and mapping.

[0154] In the factor graph of LIO-SAM, the millimeter-wave radar constraint factor is added to optimize the global consistency together with the lidar constraint, IMU constraint, and the joint loop constraint obtained in step 8. The optimization goal is to minimize the joint error:

[0155] + + + .

[0156] Among them, the parameters Represents the state variable of the SLAM system. The parameter X is the unknown quantity that needs to be estimated, including the position of autonomous mobile devices such as robots and drones in the time series and the position of feature points in the environment. X contains the position and map features, where the position can be used for positioning and the map features can be used for mapping.

[0157] Represents the sum of squares of the lidar constraints. represents the millimeter-wave radar constraint sum of squares. IMU constraint represents the sum of squared constraints of the IMU. is the sum of the squares of the joint loop constraints of the millimeter-wave radar and the lidar. is the sum of squares of the fusion error between the lidar and the millimeter-wave radar. is the error function between the IMU and mmWave radar velocity estimates.

[0158] Lidar error term is the error between the lidar observation and the current estimated state.

[0159] .in, is the location of the feature point or map point observed by the lidar, The position of the feature point or map point calculated based on the current estimation state.

[0160] Millimeter wave radar error term is the error between the millimeter-wave radar observation result and the current estimated state.

[0161] is the IMU error term, that is, the error between the IMU data and the current state.

[0162] .in, is the acceleration measured by the IMU, is the angular velocity measured by the IMU, and are the acceleration and angular velocity calculated according to the current estimated state, respectively.

[0163] The factor graph is optimized by nonlinear least squares method (such as Levenberg-Marquardt algorithm) to minimize the errors of all constraints (including velocity constraints) to obtain accurate pose and velocity estimates.

[0164] Specifically, the SLAM system state variables are solved by minimizing the joint error function and using nonlinear least squares , thereby obtaining the system's pose estimation and map feature point distribution, and then realizing SLAM's simultaneous positioning and mapping.

[0165] The present invention supplements static point cloud constraints, velocity constraints and loop constraints through millimeter-wave radar, improves the positioning accuracy and map quality of the SLAM algorithm, and improves the robustness and accuracy of the algorithm. Factor graph optimization integrates the observations of lidar, millimeter-wave radar and IMU, optimizes the posture and map features of the entire system, and ensures global consistency.

[0166] Through joint optimization, the observation information of lidar, millimeter-wave radar, IMU and loop closure detection can be integrated simultaneously to achieve high-precision pose estimation and consistent map construction.

[0167] Through the above steps, the fusion of 4D millimeter-wave radar and lidar is realized, which significantly enhances the robustness of the positioning and mapping SLAM system in dynamic environments, especially under low light, bad weather and other conditions.

[0168] The present invention reduces the error accumulation of the system during long-term operation through global optimization of multi-sensor data, ensuring the accuracy and stability of the system; accurate pose and map feature estimation is obtained by minimizing errors, so that the system can adapt to more complex dynamic environments and improve estimation accuracy; the factor graph is optimized through nonlinear least squares method to minimize the errors of all constraints (lidar, millimeter-wave radar, IMU, loop closure, etc.), thereby obtaining high-precision pose and feature point estimation, thereby realizing high-precision SLAM positioning and mapping.

[0169] Compared with traditional positioning and mapping methods, the present invention has the following advantages when applied to the above complex environment:

[0170] 1. In terms of effective filtering and processing of dynamic objects, the present invention can accurately distinguish dynamic objects from static environments through the speed information of millimeter-wave radar, thereby effectively filtering dynamic obstacles (such as construction equipment, moving vehicles, etc.).

[0171] Compared with the simple static / dynamic point cloud classification method in traditional technology, the accuracy and stability of the method of the present invention are significantly improved in complex dynamic environments, avoiding the negative impact of dynamic objects on map construction and positioning accuracy.

[0172] 2. In terms of efficient data fusion and resolution difference processing, the present invention adopts precise external parameter calibration and distance-based interpolation method to effectively fuse the high-resolution point cloud of lidar with the low-resolution point cloud of millimeter-wave radar.

[0173] This method can make up for the differences in resolution of each sensor, overcome the mismatch between lidar and millimeter-wave radar in data accuracy and resolution in traditional technologies, and provide more comprehensive and accurate environmental perception.

[0174] 3. In terms of enhanced loop detection and constraint optimization, the method of the present invention combines the loop detection results of lidar and millimeter-wave radar and adopts a weighted voting method, which greatly improves the reliability of loop detection.

[0175] Especially in feature sparse or dynamic environments, traditional loop closure detection is often unstable or fails. The present invention greatly improves the accuracy and stability of loop closure detection and enhances the global consistency of the system by fusing the data of two sensors.

[0176] 4. In terms of the introduction of millimeter-wave radar speed information, the present invention provides additional motion constraints by introducing the speed information provided by the millimeter-wave radar into the LIO-SAM factor graph optimization, thereby reducing the integral drift problem of the IMU when there is no external positioning signal (such as GPS). Compared with the prior art, the method of the present invention can significantly improve the positioning accuracy during long-term operation in complex environments (such as tunnels), and avoid the long-term drift problem that may occur in the IMU system.

[0177] 5. In terms of global consistency optimization and accuracy improvement, the present invention uses nonlinear least squares method to optimize the factor graph of the observation data of all sensors, minimizes the error, and improves the overall positioning accuracy of the system.

[0178] Compared with the use of a single sensor in traditional technology, the method of the present invention can provide more accurate and stable positioning and map construction results through the fusion of multi-sensor data such as millimeter-wave radar, lidar, and IMU.

[0179] 6. In terms of the ability to adapt to complex dynamic environments, the present invention can overcome the problems in the prior art caused by interference from dynamic objects and differences in sensor accuracy in complex environments such as tunnels and underground mines. Its stability and robustness in low-light, feature-sparse or dynamically changing environments are far superior to traditional positioning and mapping systems that rely solely on a single sensor.

[0180] The method of the present invention provides an efficient sensor data fusion method for complex environments such as tunnels through the synchronous positioning and mapping framework of LiDAR and IMU, combined with millimeter-wave radar. The method of the present invention can be applied to the positioning and mapping of autonomous mobile devices such as drones and robots in dynamic environments such as tunnels, and is suitable for scenes such as autonomous driving, unmanned vehicles, automated traffic monitoring, tunnel detection and inspection, and can provide stable positioning and mapping capabilities in the presence of dynamic obstacles (such as construction equipment, vehicles, etc.) and low light conditions.

[0181] Of course, the above description is only a preferred embodiment of the present invention, and the present invention is not limited to the above embodiments. It should be noted that all equivalent substitutions and obvious deformation forms made by any technician familiar with the field under the guidance of this specification fall within the essential scope of this specification and should be protected by the present invention.

Claims

1. A simultaneous positioning and mapping method based on heterogeneous multimodal data fusion, characterized in that: The steps include: Step 1. Align the point cloud of the millimeter-wave radar and the lidar through time synchronization and external parameter calibration, and calibrate the relative posture of the IMU, millimeter-wave radar, and lidar; the millimeter-wave radar uses a 4D millimeter-wave radar; Step 2. Use laser radar and millimeter wave radar to perform three-dimensional perception of the environment and obtain a perceived 3D point cloud; Step 3. Preprocess the millimeter wave radar data and filter dynamic objects; Step 4. Use the distance-based interpolation method to fuse and match the high-resolution point cloud acquired by the laser radar and the low-resolution static point cloud acquired by the millimeter-wave radar and preprocessed. Calculate the difference between the laser radar point and the millimeter wave radar point, and introduce it into the factor graph of LIO-SAM as an error term. This error term is the fusion error of the millimeter wave radar. Step 5. Extract the velocity information of the millimeter-wave radar and calculate the velocity of the IMU; by constructing an error function, calculate the difference between the millimeter-wave radar velocity and the IMU velocity, and provide additional velocity constraints for the LIO-SAM factor graph; Step 6. Use the millimeter wave radar point cloud to provide static point cloud constraints for the LIO-SAM factor graph; Step 7. Use the millimeter-wave radar to supplement the loop closure constraint, and use the weighted voting mechanism to fuse the loop closure detection results of the millimeter-wave radar with the loop closure detection results of the lidar to determine the final loop closure detection result; Step 8. Based on the final result of loop closure detection, construct the joint loop closure constraint of LiDAR and millimeter wave radar; Step 9. Take the fusion error of step 4, the speed constraint of step 5, the static point cloud constraint of step 6, and the joint loop constraint of step 8 as constraint factors and add them to the factor graph of LIO-SAM to construct the factor graph; optimize the factor graph to obtain the system's pose estimation and map feature point distribution, and realize the simultaneous positioning and mapping of SLAM.

2. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 1 is characterized in that: In step 1, the laser radar and the millimeter wave radar are in different coordinate systems; Assume that the relative position between the two coordinate systems is obtained through external parameter calibration, that is, the rotation matrix R and the translation vector t; Among them, the rotation matrix R describes the rotation of the laser radar coordinate system relative to the millimeter wave radar coordinate system; the translation vector t describes the translation of the origin of the laser radar coordinate system relative to the origin of the millimeter wave radar coordinate system.

3. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 3.

1. First, target extraction is performed to extract points with high reflection intensity in the millimeter-wave radar point cloud; then the Doppler effect is used to extract the target's velocity information; For each point cloud data p in the millimeter wave radar data i =(x i ,y i ,z i ), (x i ,y i ,z i ) represents the point cloud coordinates, and the millimeter wave radar provides a velocity information v associated with the point cloud i , represents the moving speed of the point cloud; Step 3.

2. Set a speed threshold v thresh ,The point cloud in the millimeter wave radar data with a moving speed less than the speed threshold is regarded as a static point cloud, and the point cloud with a moving speed exceeding the speed threshold is regarded as a dynamic point cloud; Step 3.

3. According to the preset speed threshold v thresh , separate the dynamic point cloud from the static point cloud in the millimeter-wave radar data; remove the dynamic point cloud in the millimeter-wave radar data and only retain the static point cloud.

4. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 2 is characterized in that: The step 4 is specifically as follows: Step 4.

1. Convert the laser radar point cloud to the millimeter wave radar coordinate system through the matrix obtained by external parameter calibration; Assume that the lidar point p L =[x L ,y L ,z L ] T and millimeter wave radar point p R =[x R ,y R ,z R ] T The coordinates in their respective coordinate systems are converted by the rotation matrix R and the translation vector t obtained by external parameter calibration: p L-R =Rp L +t; Among them, p L-R is the coordinate of the converted lidar point in the millimeter-wave radar coordinate system; For the transformed feature point p L-R =[x L-R ,y L-R ,z L-R ]; Calculate the difference between the laser radar point and the mmWave radar point fusion-lidar , and introduce it into the factor graph of LIO-SAM as an error term. The error term is the fusion error of the millimeter-wave radar, and its expression formula is as follows: e fusion-lidar =||p L-R -p R ||; Step 4.

2. For each lidar point, calculate the relative position of the lidar point in the millimeter-wave radar coordinate system and interpolate according to the resolution of the millimeter-wave radar; Assuming that the data of the millimeter-wave radar is represented by the distance r and angle (θ, φ) from the radar to the point, for the converted laser radar point p R , calculate the point p R The corresponding coordinates in the millimeter wave radar grid are given by the following formula: Among them, θ represents the horizontal angle of the target point relative to the millimeter-wave radar, that is, the azimuth angle of the target point detected by the millimeter-wave radar, and φ represents the pitch angle of the target point relative to the millimeter-wave radar, which is applicable to the polar coordinate system in three-dimensional space; Step 4.

3. For each lidar point, find the nearest point or the closest area found by the kd tree in the mmWave radar point cloud, and fill in the missing points using linear interpolation; For each lidar point p L The coordinate point of the converted laser radar point in the millimeter wave radar coordinate system is p R , use KD tree to find the nearest neighbor point in the millimeter wave radar point cloud Calculate their spatial distance: Where d is the coordinate point of the converted lidar point in the millimeter wave radar coordinate system, that is, p R and the nearest neighbor point in the millimeter wave radar point cloud The distance between Step 4.

4. Use linear interpolation to calculate the value in the millimeter-wave radar data corresponding to the lidar point cloud; according to the spatial position of the lidar point, find the corresponding value in the output data grid of the millimeter-wave radar to fill; Assume that in the point cloud of the millimeter-wave radar, the lidar point p L The corresponding value is v L , and the nearest neighbor point in the millimeter wave radar point cloud The corresponding value is v R , then the linear interpolation is calculated according to the following formula: in, Indicates the point cloud coordinates where the laser radar point cloud is interpolated in the millimeter wave radar coordinate system, d max Indicates the maximum range within which the interpolation between the current lidar point and the nearest neighbor point in the millimeter-wave radar point cloud is valid.

5. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 1, characterized in that: The step 5 is specifically as follows: Step 5.

1. Extract the speed information of the millimeter wave radar; Assume that the speed information obtained by the millimeter wave radar is v mmWave , which represents the relative speed from the radar to the target object; Step 5.

2. Estimate the velocity of the IMU; Get the velocity v according to the acceleration data provided by IMU IMU , the formula for IMU velocity estimation is as follows: Where a(τ) is the acceleration function of the IMU at time τ; Step 5.

3. Velocity v based on millimeter wave radar mmWave and the velocity v of the IMU IMU , construct an error function, calculate the error between the two through the error function, and introduce it into the factor graph optimization of LIO-SAM as a speed constraint; The error function formula that measures the difference between the velocity estimated by the IMU and the velocity of the millimeter wave radar is as follows: e speed =v IMU (t)-v mmWave (t); Among them, e speed is the error function between the IMU and mmWave radar velocity estimates, v IMU (t) is the velocity obtained by IMU through acceleration integration, v mmWave (t) is the relative velocity of the object measured by millimeter-wave radar.

6. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 1, characterized in that: In step 6, the millimeter wave radar static point cloud preprocessed in step 3 is used to construct the following error function: Among them, e mmWave The error introduced by the millimeter wave radar static point cloud, is the position of the point or target feature observed by the millimeter-wave radar, is the position of the millimeter-wave radar target feature calculated based on the current estimation state.

7. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 1, characterized in that: The step 7 is specifically as follows: Step 7.

1. Target feature matching; Extract target features of millimeter-wave radar point cloud, establish feature descriptors for the target features extracted from millimeter-wave radar point cloud, and use the feature descriptors to search for the most matching target features in the constructed SLAM history map; Step 7.

2. Use the millimeter-wave radar to supplement the loop closure constraints, and use the weighted voting mechanism to fuse the loop closure detection results of the millimeter-wave radar with the loop closure detection results of the lidar to determine the final loop closure detection results.

8. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 7, characterized in that: In step 7.2, the specific process of using the weighted voting mechanism to determine the loop detection result is as follows: Step 7.2.

1. Set weights; According to the environment and sensor characteristics, the weights w of LiDAR and MMW radar are set. LiDAR and w mmWave ; In a feature-rich environment, the weight of the lidar is higher than that of the millimeter-wave radar, that is, w LiDAR >w mmWave , in low-feature or dynamic environments, the weight of the millimeter-wave radar is higher than that of the lidar, that is, w LiDAR <w mmWave ; Step 7.2.

2. Calculate the comprehensive score; Based on the matching relationship between the current frame and the historical map frame, all possible historical frames or map points that form a loop with the current frame are screened out as candidate matching points i; Calculate the comprehensive similarity score of candidate matching point i The formula is as follows: Where i is the index of the candidate matching point; represents the weighted sum of the similarity scores of LiDAR and MMW radar; is the similarity score of the laser radar for the candidate matching point i, The higher the score, the greater the possibility that the laser radar considers that the historical frame i is a loop match of the current frame; is the similarity score of the millimeter-wave radar for the candidate matching point i, The higher the score, the greater the possibility that the millimeter-wave radar considers that the historical frame i is a loop match of the current frame; Step 7.2.3 selects the candidate frame with the highest comprehensive score as the final loop match; According to the comprehensive similarity score obtained in step 7.2.2, the best matching historical frame is selected as the loop point to establish the basis for the subsequent loop constraint. The calculation formula of the final selected loop matching point index is as follows: where i best is the index of the final selected loop matching point, indicating the comprehensive similarity score The highest candidate matching point, Indicates the selection of comprehensive similarity scores from all candidate matching points i The index of the largest point.

9. The synchronous positioning and mapping method based on heterogeneous multimodal data fusion according to claim 8, characterized in that: The step 8 is specifically as follows: The loop closure constraints of the lidar are as follows: e loopLiDAR =||T curr ·T rel -T loopLiDAR ||; where e loopLiDAR represents the loop constraint of the lidar, T curr is the position of the lidar in the current frame, T loopLiDAR is the position of the lidar loop frame, T rel is the relative transformation between two LiDAR frames; The constraints of millimeter wave radar are as follows: where e loopmmWave represents the constraints of millimeter wave radar, Indicates the position and posture of the millimeter-wave radar in the current frame. Represented as the pose of the loop frame of the millimeter wave radar; p rel is the relative transformation between two frames of millimeter wave radar; The joint loop constraints of the LiDAR and the millimeter-wave radar are constructed as follows: have been total =w LiDAR ·have been loopLiDAR +w mmWave ·have been loopmmWave ; where e total is the joint loop constraint, w LiDAR is the laser radar weight, w mmWave is the millimeter wave radar weight.

10. The method for simultaneous positioning and mapping based on heterogeneous multimodal data fusion according to claim 1, characterized in that: The step 9 is specifically as follows: In the factor graph of LIO-SAM, the millimeter-wave radar constraint factor is added to optimize the global consistency together with the lidar constraint, IMU constraint, speed and joint loop constraint. The optimization goal is to minimize the joint error: Among them, parameter X represents the state variable of the SLAM system. The parameter X is the unknown quantity that needs to be estimated, including the pose in the time series and the position of the feature points in the environment. The pose is used for positioning, and the map features are used for mapping. represents the sum of squares of the lidar constraints; represents the sum of squares of millimeter-wave radar constraints; Constraint represents the square sum of constraints of IMU; is the sum of squares of the joint loop constraints of the millimeter-wave radar and the lidar; e speed is the error function between the IMU and mmWave radar velocity estimates; is the sum of squares of the fusion errors of the laser radar and the millimeter-wave radar; Lidar error term e LiDAR is the error between the lidar observation and the current estimated state; in, is the location of the feature point or map point observed by the lidar, is the position of the feature point or map point calculated based on the current estimation state; Millimeter wave radar error term e mmWave is the error between the millimeter-wave radar observation result and the current estimated state; e IMU is the IMU error term, which is the error between the IMU data and the current state; in, is the acceleration measured by the IMU, is the angular velocity measured by the IMU, and are respectively the acceleration and angular velocity calculated according to the current estimated state; By minimizing the joint error function and using nonlinear least squares to solve the SLAM system state variable X, the system's pose estimation and map feature point distribution are obtained, thereby achieving simultaneous positioning and mapping of SLAM.

Images