Underground environment laser radar and millimeter wave radar cooperative SLAM method

Abstract

The invention discloses an underground environment laser radar and millimeter wave radar cooperative SLAM method, and relates to unmanned driving positioning. The method comprises the following steps: collecting point cloud data of an underground environment by using a laser radar, and carrying out filtering processing; collecting target data by using a millimeter wave radar and carrying out filtering processing; curvature-based feature extraction is carried out on the preprocessed point cloud data to obtain a point cloud feature set, spatial correlation is carried out on a target point set with a speed label and the point cloud feature set, point cloud features corresponding to a dynamic target are identified and eliminated according to the speed label, and a static environment feature set is obtained; inter-frame matching and pose estimation are carried out based on the static environment feature set, a static target in a target point set with a speed label is used as an additional constraint, and an optimized pose sequence is obtained through a graph optimization method; and projecting the static environment feature set to a global coordinate system according to the optimized pose sequence, and constructing a static environment map. According to the invention, the interference of the dynamic target on the SLAM is avoided.

Description

Technical Field

[0001] The present application relates to the field of unmanned driving positioning, and in particular to a collaborative SLAM method of underground environment laser radar and millimeter wave radar. Background Art

[0002] With the rapid advancement of intelligent mining, underground autonomous driving technology has garnered widespread attention as a key technology for improving mine production efficiency and safety. In underground mining environments, positioning technology based on prior point cloud maps is a crucial component of autonomous driving technology and a prerequisite for path planning and intelligent control. Simultaneous Localization and Mapping (SLAM) technology enables intelligent equipment such as underground transport vehicles and tunnel engineering vehicles to achieve autonomous positioning and environmental awareness in unknown environments, playing a crucial role in ensuring underground operational safety and improving production efficiency.

[0003] However, the underground environment is characterized by high dust concentration, pervasive water mist, poor lighting conditions, long and narrow tunnels, and a large number of dynamic targets, posing severe challenges to traditional SLAM technology. Existing solutions typically use LiDAR-based SLAM methods to build environmental maps. Although LiDAR can provide highly accurate three-dimensional point cloud data, it faces the following problems in the complex underground environment: First, suspended particles such as dust and water mist in the mine cause severe attenuation and scattering of laser signals, significantly shortening the LiDAR detection range and severely degrading the point cloud data quality. Second, highly reflective objects such as metal equipment and pipelines in the tunnels are prone to multipath effects, causing point cloud data distortion. Third, the underground environment contains a large number of dynamic targets such as transport vehicles and workers. These dynamic targets can be mistakenly included in the static map, leading to map construction errors and seriously affecting subsequent positioning accuracy.

[0004] Millimeter-wave radar, due to its long wavelength (approximately 4mm in the 77GHz band), has strong penetration capabilities in harsh environments such as dust and smoke, and can directly measure the radial velocity of targets, giving it a natural advantage in dynamic target detection. However, millimeter-wave radar also has drawbacks such as relatively low distance and angle measurement accuracy, sparse point clouds, and difficulty accurately describing environmental geometric features. Using it alone, it cannot meet the requirements of high-precision SLAM.

[0005] More critically, existing technologies lack an effective mechanism for separating dynamic and static targets. In underground environments, the presence of dynamic targets not only contaminates static maps but also introduces erroneous constraints during inter-frame matching, leading to cumulative pose estimation errors. Traditional dynamic target detection methods based on geometric features suffer from a significant decrease in reliability in dusty environments, while methods based on motion consistency require the accumulation of multiple frames of data and suffer from poor real-time performance.

[0006] Therefore, it is difficult for a single sensor to achieve stable and reliable SLAM in the complex environment of the mine. There is an urgent need for a collaborative SLAM method that can integrate the high-precision geometric information of the lidar and the reliable speed information of the millimeter-wave radar, overcome their respective limitations through complementary advantages, and establish an effective dynamic and static target separation mechanism to achieve high-precision and strong robustness of synchronous positioning and map construction in the complex environment of the mine. Summary of the Invention

[0007] In view of the large number of dynamic targets (transport vehicles, personnel) in the mine, traditional SLAM assumes that the environment is static. This application provides a collaborative SLAM method of underground environment lidar and millimeter-wave radar, which fully utilizes the high-precision ranging capability of lidar and the anti-interference capability of millimeter-wave radar, effectively separates dynamic and static targets, and avoids the interference of dynamic targets on SLAM.

[0008] The present application provides a collaborative SLAM method for an underground environment using a laser radar and a millimeter-wave radar, including: S1, using a laser radar to collect point cloud data of an underground environment and performing filtering processing to obtain preprocessed point cloud data; S2, using a millimeter-wave radar to collect target data and performing filtering processing to obtain a target point set with a speed label; wherein the target includes a static target and a dynamic target; S3, performing curvature-based feature extraction on the preprocessed point cloud data to obtain a point cloud feature set, and spatially associating the target point set with the speed label with the point cloud feature set, identifying and eliminating the point cloud features corresponding to the dynamic target based on the speed label, and obtaining a static environment feature set; S4, performing inter-frame matching and pose estimation based on the static environment feature set, and at the same time using the static target in the target point set with the speed label as an additional constraint, and obtaining an optimized pose sequence through a graph optimization method; S5, projecting the static environment feature set into a global coordinate system according to the optimized pose sequence, constructing a static environment map, and marking the motion trajectory of the dynamic target in the target point set with the speed label in the static environment map, to obtain an underground environment map containing dynamic and static information.

[0009] In particular, underground environments contain numerous dynamic targets, such as transport vehicles and workers. These dynamic targets severely undermine the static environment assumption of traditional SLAM methods. In dusty environments, while lidar can provide dense point cloud data, it cannot directly distinguish between the dynamic and static properties of the point cloud, resulting in dynamic targets being mistakenly included in the static map. However, 77GHz millimeter-wave radar is able to penetrate dust and directly measure the radial velocity of targets, providing critical information for separating dynamic and static targets.

[0010] This solution cleverly leverages the complementary characteristics of the two sensors: in step S2, the millimeter-wave radar operates stably in a dusty environment, acquiring velocity information for each target and generating a target point set with velocity labels, which serves as the data basis for achieving static and dynamic separation. In step S3, the velocity information is transferred to the point cloud data by spatially correlating the target points of the millimeter-wave radar with the point cloud features of the lidar. For dynamic targets with radial velocities greater than a threshold, all associated point cloud features are removed from the feature set, resulting in a pure static environment feature set.

[0011] This application uses a motion-static separation mechanism based on speed tags. It does not rely on multi-frame accumulation or complex motion estimation, but instead uses single-frame speed measurement of millimeter-wave radar to achieve real-time motion-static discrimination. Secondly, in a dusty environment, when the performance of the lidar degrades, the speed information of the millimeter-wave radar remains reliable, ensuring the accuracy of motion-static separation.

[0012] Finally, in the graph optimization framework, the static environment feature set after removing dynamic features is used for inter-frame matching, avoiding erroneous constraints caused by dynamic targets. At the same time, millimeter-wave radar targets marked as static are added to the optimization as additional constraints, providing supplementary information when the laser point cloud is sparse, thereby improving the accuracy and robustness of pose estimation.

[0013] Furthermore, S1, using a laser radar to collect point cloud data and performing filtering processing to obtain pre-processed point cloud data, including: using a 16-line or higher three-dimensional laser radar to collect original point cloud data of the underground environment; using a K-nearest neighbor statistical filtering algorithm to filter the original point cloud data; dividing the filtered point cloud data into voxel grids, replacing all points in the corresponding voxel grid with the centroid point in each voxel, performing point cloud downsampling, and obtaining pre-processed point cloud data;

[0014] In particular, in dusty underground environments, lidar faces serious performance degradation issues—the scattering and absorption of laser light by dust particles shortens the effective detection range, sparse point clouds, and increases noise. This application first uses K-nearest-neighbor statistical filtering to remove outlier noise points generated by dust scattering; then, through voxel grid downsampling, not only does it reduce the amount of data, but more importantly, by aggregating the centroids of point clouds within voxels, it further suppresses the impact of random noise. This processing strategy ensures that even when lidar performance degrades, relatively reliable environmental geometric features can still be extracted from sparse, noise-contaminated raw data.

[0015] Furthermore, S2 uses a millimeter-wave radar to collect target data and performs filtering processing to obtain a target point set with a speed label; wherein the target includes static targets and dynamic targets, including: using a 77GHz millimeter-wave radar to collect target data in a dusty environment, the target data includes: distance, radial velocity, azimuth and pitch angle, wherein the 77GHz millimeter-wave radar can penetrate the dust and water mist in the underground environment; performing Kalman filtering on the collected target data to eliminate measurement noise to obtain filtered target data; according to the radial velocity in the filtered target data, the target whose absolute value of the radial velocity is greater than the preset velocity threshold is marked as a dynamic target, otherwise it is marked as a static target; the marked target is converted from the millimeter-wave radar polar coordinate system to the Cartesian coordinate system to obtain a target point set with a speed label.

[0016] In particular, 77GHz millimeter-wave radar, with its approximately 4mm wavelength, has excellent dust penetration capabilities and can continue to operate stably in environments where lidar is ineffective. Millimeter-wave radar can not only detect targets in harsh environments but, more importantly, directly measure the target's radial velocity. By setting a velocity threshold for motion and static discrimination, preliminary target classification is completed during the data collection phase, with each target carrying a clear velocity label. This physical measurement-based motion and static classification method offers greater reliability and real-time performance in dusty environments than traditional methods based on multi-frame geometric features.

[0017] Furthermore, while LiDAR performance degrades in dusty environments, it can still provide information about the environment's geometric outlines after filtering. While millimeter-wave radar produces sparse point clouds, it can reliably provide target position and velocity information in harsh environments. In particular, the millimeter-wave radar's velocity signature provides a key criterion for accurately removing dynamic elements from the laser point cloud, something a single sensor alone cannot achieve.

[0018] Furthermore, in S3, curvature-based feature extraction is performed on the preprocessed point cloud data to obtain a point cloud feature set, and the target point set with speed labels is spatially associated with the point cloud feature set. The point cloud features corresponding to dynamic targets are identified and eliminated based on the speed labels to obtain a static environment feature set. This includes: calculating the local curvature value of each point cloud based on the preprocessed point cloud data, marking points with curvature values ​​greater than a first threshold as edge feature points, and marking points with curvature values ​​less than a second threshold as plane feature points, to obtain a point cloud feature set containing edge feature points and plane feature points; wherein the first threshold is greater than the second threshold; for each target point with a speed label, searching the point cloud features for all feature points within a preset radius of the target point, and establishing an association between the target point and the feature points; for target points marked as dynamic targets, marking all associated point cloud features as dynamic features and eliminating them from the point cloud feature set; and for targets explicitly marked as dynamic by the millimeter-wave radar (e.g., a transport vehicle with a radial velocity greater than a threshold), directly eliminating all laser point cloud features within their spatial range. This method is fast and effective, fully leveraging the speed detection advantages of millimeter-wave radar.

[0019] For target points marked as static, the associated point cloud features are retained. Specifically, for targets marked as static by millimeter-wave radar, the associated point cloud features are retained. This is particularly important when LiDAR data is missing due to dust. The static targets in millimeter-wave radar provide reliable anchor points for the laser point cloud.

[0020] For point cloud features not associated with any target point, a multi-frame consistency check is used to determine their static or dynamic properties: if the point cloud feature remains unchanged in multiple consecutive frames, it is marked as a static feature; otherwise, it is marked as a potential dynamic feature and removed. In particular, for areas not covered by the millimeter-wave radar (due to its limited field of view), a multi-frame consistency check is performed to supplement the identification. This ensures the integrity of the static and dynamic separation.

[0021] All retained static feature points are combined into a static environment feature set.

[0022] In particular, the intermingling of static and dynamic targets in underground environments presents a core challenge for SLAM. Including dynamic objects such as transport vehicles and workers in the map can lead to significant positioning errors. Traditional methods rely on the geometric consistency of multiple frames of laser point clouds to discern static and dynamic attributes, but this approach fails due to the degraded quality of lidar data in dusty environments. While millimeter-wave radar can penetrate dust and directly measure velocity, its data is sparse, making it impossible to independently construct detailed maps.

[0023] This application first extracts curvature features from the laser point cloud to identify geometric structures such as edges and planes in the environment. Then, using a spatial correlation algorithm, the millimeter-wave radar velocity labels are mapped to corresponding laser point cloud features. The key to this cross-modal correlation lies in the design of a multi-dimensional correlation cost function, which not only considers spatial distance but also incorporates velocity consistency and size similarity to ensure correlation accuracy.

[0024] Furthermore, an association relationship between the target point and the feature point is established, including: for each target point with a speed label, searching the point cloud feature set for a set of candidate feature points within a preset radius of the target point;

[0025] Calculate the association cost between each target point and the corresponding candidate feature point : ,in, is the spatial distance cost, is the speed difference cost, For the cost of similar size, is the weight coefficient; Represents the three-dimensional position coordinates of the i-th target point in the Cartesian coordinate system, Represents the three-dimensional position coordinates of the j-th candidate feature point in the Cartesian coordinate system, Indicates the radial velocity value of the i-th target point, Represents the velocity value of the j-th candidate feature point based on the multi-frame position change estimation, represents the radar cross section of the i-th target point, Represents the point cloud density value in the local neighborhood of the jth candidate feature point; constructs the association cost matrix between the target point and the candidate feature points, uses the Hungarian algorithm to solve the association cost matrix, and obtains the optimal association scheme with the minimum association cost; according to the optimal association scheme, determines the set of associated feature points for each target point, and establishes the association relationship between the target point and the feature point.

[0026] In particular, in the dusty environment underground, achieving accurate association between laser point clouds and millimeter-wave radar targets faces huge challenges: first, dust causes the quality of laser radar data to deteriorate, and the point cloud becomes sparse and unevenly distributed; second, the data characteristics of the two sensors differ greatly - the laser radar provides a dense point cloud without velocity information, while the millimeter-wave radar provides sparse target points with velocity labels; most importantly, simple spatial distance matching is prone to misassociation in complex environments, resulting in incorrect judgment of dynamic and static attributes.

[0027] This application first sets the spatial distance cost This is the most basic basis for association, ensuring that only spatially adjacent points belong to the same target. However, in narrow underground tunnels, it is easy to mistakenly associate different adjacent targets based solely on distance.

[0028] Then, set the speed difference cost , radial velocity directly measured by millimeter-wave radar With high reliability, the speed of laser feature point This is achieved by estimating position changes over multiple frames. By comparing the consistency between the two, it is possible to effectively avoid incorrectly associating a static wall point cloud with a moving vehicle or confusing objects of different speeds. This is impossible with traditional methods.

[0029] Finally, by the size similarity cost This cleverly exploits the complementary properties of the two sensors: the millimeter-wave radar's cross-sectional area reflects target size, while the local density of the laser point cloud also reflects target size. For example, a large transport truck has a larger radar cross-sectional area, and its corresponding point cloud area density is also higher; whereas a single operator has both smaller indicators. This size consistency further improves correlation accuracy.

[0030] Furthermore, S4 performs inter-frame matching and pose estimation based on a static environment feature set. Static targets in the velocity-labeled target point set are used as additional constraints to obtain an optimized pose sequence through a graph optimization method. This includes: Based on the static environment feature sets of the current and previous frames, an iterative closest point algorithm or a normal distribution transformation algorithm is used for feature matching to obtain the relative pose transformation matrix and matching feature point pairs between adjacent frames. Inter-frame matching based on the obtained static environment feature set eliminates interference from dynamic targets at the source. This ensures that even in dense underground environments, only stable and reliable static features (such as walls and pillars) are used for pose estimation.

[0031] A graph optimization model is constructed using the relative pose transformation matrix as the initial pose estimate; the graph optimization model includes: taking the pose of the mobile platform at each moment as the pose node to be optimized; the lidar pose error constructed based on the relative pose transformation matrix; the lidar observation error constructed based on the matching feature point pairs; the millimeter-wave radar observation error constructed based on the static targets in the target point set with velocity labels; according to the lidar measurement accuracy, a first weight matrix of the lidar pose error and the lidar observation error is set; according to the millimeter-wave radar measurement accuracy, a second weight matrix of the millimeter-wave radar observation error is set; wherein the diagonal element values ​​of the first weight matrix are greater than the diagonal element values ​​of the second weight matrix, and the weight matrix is ​​the inverse matrix of the corresponding constraint error covariance matrix;

[0032] A multi-frame motion consistency check is performed on the targets in the target point set with velocity labels, and the motion trajectory of the target in multiple consecutive frames is tracked. If the target motion pattern changes suddenly, it is re-labeled as a dynamic target, and the corresponding millimeter-wave radar observation error is eliminated from the graph optimization model; based on the constructed graph optimization model, the overall optimization objective function is defined as the weighted square sum of all constraint errors, and the overall optimization objective function is minimized through iterative solution using the nonlinear least squares method. The estimated value of each pose node is updated until convergence to obtain the optimized pose sequence.

[0033] Specifically, this application constructs a dual-constrained graph optimization framework. When dust degradation causes sparse LiDAR features, this framework uses millimeter-wave radar static targets to provide additional geometric constraints, ensuring the stability and accuracy of pose estimation in underground environments. The dusty underground environment poses a serious challenge to traditional LiDAR-based pose estimation: dust causes a sparse point cloud, significantly reducing the number of features available for matching. Furthermore, if dynamic targets (transport vehicles, personnel) are involved in inter-frame matching, erroneous pose constraints are introduced, leading to rapid accumulation of positioning errors and even system crashes.

[0034] When the lidar performs normally, the system relies primarily on high-precision laser signatures for precise positioning. When severe dust degrades the laser, the millimeter-wave radar's static target constraints provide a baseline reference, maintaining the system's basic positioning capabilities. More importantly, all optimization elements are strictly selected static elements, fundamentally preventing contamination of the positioning system by dynamic targets.

[0035] Furthermore, the lidar pose error is constructed based on the relative pose transformation matrix, including: using the relative pose transformation matrix as a constraint between adjacent pose nodes; the lidar pose error is defined as the Lie algebraic distance between the actual pose transformation between the adjacent pose nodes to be optimized and the relative pose transformation matrix.

[0036] In particular, the dusty environment underground impacts LiDAR in multiple ways: not only does it cause a sparse point cloud, but more seriously, it significantly reduces the number of matchable features. In this situation, relying solely on a single constraint type can easily lead to optimization degradation or trapping in a local optimum due to insufficient information.

[0037] This application uses the relative pose transformation matrix obtained by ICP or NDT algorithm to reflect the overall geometric relationship between adjacent frames. Lie algebraic distance is used instead of simple Euclidean distance because pose transformation belongs to Manifold,Lie algebra distance can correctly measure the combined error of rotation and translation.,The advantage of this constraint is that even when features are sparse,,as long as there is sufficient static structure (such as,roadway contours), it can still provide basic constraint,relations between frames.

[0038] Furthermore, the lidar observation error is constructed based on the matching feature point pairs, including: for each pair of matching feature points, calculating the predicted position of the feature point of the previous frame projected to the current frame under the estimated value of the current pose node; taking the Euclidean distance between the predicted position and the actual matching feature point position as the lidar observation error;

[0039] Specifically, for each successfully matched pair of feature points, observation constraints are constructed by calculating the projection error. This point-to-point constraint fully utilizes high-quality local features (such as distinct edges and corners) to provide higher localization accuracy in certain areas. When dust obscures features in certain areas, inter-frame pose constraints still provide a fundamental constraint based on the overall structure. In local areas with clear features, feature point constraints provide even more precise localization information.

[0040] Furthermore, the millimeter-wave radar observation error constructed based on the static targets in the target point set with velocity labels includes: obtaining target points marked as static targets in the target point set with velocity labels; calculating the predicted observation value of each static target under the estimated value of the current pose node;

[0041] The polar coordinate error between the predicted observation value and the actual observation value is taken as the millimeter wave radar observation error :

[0042]

[0043] in, Indicates the target distance measured by the millimeter-wave radar, represents the position of the kth static target in the global coordinate system, represents the position of the mobile platform in the global coordinate system at the i-th moment, Indicates the target azimuth measured by millimeter-wave radar;

[0044] In particular, in dusty underground environments, where LiDAR is nearly ineffective, traditional SLAM systems quickly degrade due to insufficient constraints. However, 77GHz millimeter-wave radar, with its 4mm wavelength, can penetrate dust and operate stably.

[0045] In this application, the distance error term : Distance directly measured by millimeter-wave radar It has high reliability (not affected by dust), while the predicted distance depends on the current pose estimate and the global position of the static target. When the pose estimate is accurate, the two should be consistent; when there is a deviation, this error term will drive the pose to adjust in the correct direction. Angle error term Similarly, the azimuth angle measured by the millimeter-wave radar is compared with the predicted angle based on the pose calculation. The angle constraint complements the distance constraint to determine the target's exact position.

[0046] Compared with the existing technology, the advantages of this application are:

[0047] In the complex environment of underground mines, the single-sensor SLAM method suffers from problems such as reduced positioning accuracy due to interference from dust and water mist, severe degradation of point cloud quality of lidar in low visibility environments, map construction errors caused by dynamic targets, and lack of an effective dynamic and static target separation mechanism.

[0048] This application uses lidar to collect environmental geometric feature information, uses the ability of 77GHz millimeter-wave radar to penetrate dust and water mist to obtain target speed information, and accurately separates dynamic and static targets based on speed labels; by establishing a spatial correlation between target points and point cloud features, the point cloud features corresponding to dynamic targets are eliminated to construct a purely static environmental feature set; a graph optimization framework is used to fuse the high-precision geometric information of the lidar and the reliable speed information of the millimeter-wave radar, and static targets are used as additional constraints to improve the accuracy of pose estimation; and finally a complete underground environmental map containing static environmental structures and dynamic target motion trajectories is constructed.

[0049] This application effectively separates dynamic and static targets, avoiding interference of dynamic targets on the SLAM system; fully utilizes the complementary advantages of high-precision ranging of lidar and strong anti-interference capability of millimeter-wave radar; provides underground autonomous driving vehicles with a complete map containing environmental structure and dynamic target information, thereby improving the safety and efficiency of underground operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The present application will be further described in the form of exemplary embodiments, which will be described in detail with reference to the accompanying drawings. These embodiments are not limiting, and in these embodiments, the same numbers represent the same structures, wherein:

[0051] Figure 1 This is an exemplary flow chart of a collaborative SLAM method of a downhole environment laser radar and millimeter wave radar according to some embodiments of the present application. DETAILED DESCRIPTION

[0052] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0053] like Figure 1As shown, laser radar is used to collect point cloud data of the underground environment and filter it to obtain preprocessed point cloud data; millimeter wave radar is used to collect target data and filter it to obtain a target point set with speed labels; wherein the targets include static targets and dynamic targets; curvature-based feature extraction is performed on the preprocessed point cloud data to obtain a point cloud feature set, and the target point set with speed labels is spatially associated with the point cloud feature set, and the point cloud features corresponding to the dynamic targets are identified and eliminated according to the speed labels to obtain a static environment feature set; inter-frame matching and pose estimation are performed based on the static environment feature set, and the static targets in the target point set with speed labels are used as additional constraints to obtain an optimized pose sequence through a graph optimization method; the static environment feature set is projected into the global coordinate system according to the optimized pose sequence to construct a static environment map, and the motion trajectories of the dynamic targets in the target point set with speed labels are marked in the static environment map to obtain an underground environment map containing dynamic and static information.

[0054] Specifically, a laser radar and millimeter-wave radar are integrated on the mobile platform. The laser radar uses a 3D laser radar with 16 or more lines, and the millimeter-wave radar uses a 77GHz frequency band with multi-target tracking capabilities. The installation positions of the two sensors meet the following spatial relationship: ,in is the transformation matrix from millimeter wave radar to lidar coordinate system, is the rotation matrix, is the translation matrix.

[0055] Time synchronization: A hardware clock synchronization mechanism is used to align the sampling times of the two sensors through a synchronous trigger signal. The time synchronization error is controlled within 1ms. The time synchronization model is: ;in, is the lidar sampling time, is the millimeter wave radar sampling time, is a fixed time offset, is the synchronization error.

[0056] The LiDAR collects point cloud data of the underground environment and performs pre-processing operations such as denoising and filtering to remove abnormal points and invalid data.

[0057] LiDAR point cloud denoising: Statistical outlier filtering is used: For each point, the distance to the nearest K neighboring points is calculated. If the distance is greater than the mean plus n times the standard deviation, it is considered an outlier. The formula is: , is an outlier.

[0058] Use voxel grid filtering: divide the point cloud space into a regular voxel grid, and replace the original point set with the center of gravity or centroid point within each voxel to achieve point cloud downsampling and denoising.

[0059] Millimeter-wave radar collects information such as distance and speed in the underground environment, and performs data calibration and compensation to improve data accuracy.

[0060] Millimeter-wave radar data filtering: Kalman filtering is used to smooth target distance, speed and other parameters. The Kalman filter equation is: .

[0061] LiDAR feature extraction: Using curvature-based feature extraction method to calculate the point cloud curvature: ,in, 、 for point The normal vector of is the average distance from point i to the domain points.

[0062] Setting the curvature threshold: Edge feature threshold , plane feature threshold ; According to the curvature, the points are divided into plane feature points and edge feature points.

[0063] Millimeter-wave radar target feature representation: The target detected by the millimeter-wave radar is represented as a three-dimensional point with a velocity vector , for each millimeter wave target , search for a point cloud subset within a radius of R = 1.5m in the laser point cloud;

[0064] Perform DBSCAN clustering on the point cloud subset to extract the geometric outline of the target; establish the target feature descriptor:

[0065] Feature association: For each mmWave target , search space distance threshold All laser feature points within; establish the correlation matrix between the laser radar features and the millimeter wave radar targets, and use the Hungarian algorithm to solve the optimal correlation. The correlation cost function is: ,in, is the spatial distance cost, is the speed difference cost, For the cost of similar size, is the weight coefficient. Specifically, the spatial distance cost is: , directly calculate the square of the Euclidean distance; the speed difference cost: ,in, Estimation of feature displacement of adjacent frames; size similarity cost: , and calculate the relative difference after normalization.

[0066] The system performs feature classification based on association relationships and speed labels: it traverses all association pairs and checks the speed labels of the millimeter wave targets; (0.5m / s) dynamic targets, all their associated laser features are marked as dynamic; For static targets, their associated features are marked as static. A historical position cache (5 frames) of unassociated features is maintained. Feature position changes between consecutive frames are calculated to estimate motion velocity. Objects with velocity less than a threshold and stable position are marked as static; otherwise, they are marked as potentially dynamic. The static environment feature set consists of {all feature points marked as static}; the dynamic feature set consists of {all feature points marked as dynamic or potentially dynamic}; and the association mapping table records the correspondence between each feature and the millimeter wave target for subsequent processing.

[0067] The system first performs data registration on the static environmental feature sets of the current frame and the previous frame. For data processing, either the ICP or NDT algorithm is used to process static feature point cloud data. The ICP algorithm iteratively finds the closest correspondence between point pairs and calculates the transformation matrix that minimizes registration error. The NDT algorithm divides the point cloud data into a voxel grid, describes the distribution of points within each voxel using a normal distribution, and achieves registration through probability density matching. The registration output contains two types of data: the relative pose transformation matrix T (including rotation R and translation t) and the set of successfully matched feature point pairs.

[0068] The system builds a graph data structure containing two types of nodes: posture node data: stores the position of the mobile platform at each moment The posture state , as the variable to be optimized.

[0069] Map node data: LiDAR feature data: edge feature points and plane feature points extracted through curvature analysis, each feature contains three-dimensional coordinates and local geometric descriptors; Millimeter-wave radar static target data: static targets after speed screening, with their global coordinates saved and radar measurement raw data .

[0070] The system models and quantifies errors for three types of constraint data:

[0071] LiDAR pose constraint data processing: The relative pose transformation matrix obtained by inter-frame registration is converted into Lie algebraic form, and the Lie algebraic distance between the actual adjacent pose transformation and the measured transformation is calculated as the error term. This processing method correctly handles Distance metrics on manifolds.

[0072] LiDAR observation constraint data processing: For each pair of matching feature points, the coordinates of the feature points in the previous frame are transformed by the current pose estimation value, and the Euclidean distance between the projection point and the actual feature point in the current frame is calculated to form the observation error data.

[0073] Millimeter wave radar observation constraint data processing: the polar coordinate measurement data of millimeter wave radar The predicted observations based on the current pose estimate are compared to construct a polar coordinate error model. The range error and angle error quantify the radial and tangential positioning deviations, respectively.

[0074] Millimeter-wave radar observation error :

[0075] ,in, Indicates the target distance measured by the millimeter-wave radar, represents the position of the kth static target in the global coordinate system, represents the position of the mobile platform in the global coordinate system at the i-th moment, Indicates the target azimuth measured by the millimeter-wave radar.

[0076] The system dynamically sets the weight matrix based on the quality of sensor data: LiDAR constraint data: Based on the point cloud density and feature matching confidence, a larger diagonal element of the information matrix (the inverse of the covariance matrix) is set. Typical values ​​are the inverse of the distance error of 0.1m and the angle error of 0.1rad.

[0077] Millimeter-wave radar constraint data: Considering its relatively low angular resolution, set smaller information matrix elements. Typical values ​​are the inverse of the distance error of 0.5m and the angle error of 0.5rad.

[0078] The system performs multi-frame consistency analysis on millimeter-wave radar target data: maintains a data association table of target ID and historical trajectory; calculates the target's motion pattern within 5 consecutive frames and detects sudden changes in speed or direction; removes targets with detected motion anomalies from the static target set and resets the corresponding observation constraint weights to zero or deletes them from the optimization graph.

[0079] Construct an overall optimization objective function, using the weighted sum of squares of all constraint errors as the optimization target. Use the Levenberg-Marquardt algorithm for nonlinear least-squares optimization: At each iteration, all error terms are updated based on the current pose estimate; the Jacobian and Hessian matrices are calculated to solve for the pose increment; and pose node data is updated until convergence (the pose increment is less than a threshold or the maximum number of iterations is reached). Output the optimized pose sequence data for subsequent map construction and dynamic target trajectory annotation.

[0080] Based on the optimized pose, the LiDAR point cloud data is transformed and projected into the global coordinate system to construct a point cloud map of the underground environment. The point cloud map can intuitively reflect the geometric shape and structural information of the underground environment.

[0081] Using target detection information from millimeter-wave radar, obstacles in the point cloud map are annotated and classified. For example, objects can be classified as pedestrians, vehicles, or equipment based on characteristics such as speed and size. The annotated and classified point cloud map contains rich semantic information, providing important support for subsequent path planning and navigation.

[0082] Point cloud maps are stored and managed using representation methods such as voxel grid maps or octree maps. Voxel grid maps divide space into small voxels, each representing the presence of an obstacle in that area. Octree maps are a hierarchical data structure that can efficiently represent large-scale 3D environments.

[0083] The invention of the present application and its implementation methods are described schematically above. This description is not restrictive. Without departing from the spirit or basic features of the present application, the present application can be implemented in other specific forms. What is shown in the accompanying drawings is only one of the implementation methods of the invention of the present application, and the actual structure is not limited to this. Therefore, if a person of ordinary skill in the art is inspired by it, without departing from the purpose of the invention, a structural method and embodiment similar to the technical solution are designed without creativity, which should all fall within the scope of protection of the present application. In addition, the word "including" does not exclude other elements or steps, and the word "one" before an element does not exclude the inclusion of "multiple" elements. Words such as first and second are used to indicate names and do not indicate any specific order.

Claims

1. A collaborative SLAM method for underground environment laser radar and millimeter wave radar, characterized in that: include: S1, using laser radar to collect point cloud data of the underground environment and perform filtering processing to obtain pre-processed point cloud data; S2, using millimeter wave radar to collect target data and perform filtering processing to obtain a target point set with speed labels; the targets include static targets and dynamic targets; S3, performing curvature-based feature extraction on the preprocessed point cloud data to obtain a point cloud feature set, and spatially correlating the target point set with the speed label with the point cloud feature set. Based on the speed label, point cloud features corresponding to dynamic targets are identified and eliminated to obtain a static environment feature set. S4, based on the static environment feature set, performs inter-frame matching and pose estimation, and uses the static targets in the target point set with velocity labels as additional constraints to obtain the optimized pose sequence through the graph optimization method; S5, projecting the static environment feature set to the global coordinate system according to the optimized pose sequence, constructing a static environment map, and marking the motion trajectory of the dynamic target in the target point set with speed labels in the static environment map to obtain an underground environment map containing dynamic and static information.

2. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1 is characterized in that: S1, using LiDAR to collect point cloud data and perform filtering processing to obtain pre-processed point cloud data, including: Use 16-line or higher 3D laser radar to collect original point cloud data of underground environment; The K-nearest neighbor statistical filtering algorithm is used to filter the original point cloud data; The filtered point cloud data is divided into voxel grids, and the centroid point in each voxel is used to replace all points in the corresponding voxel grid to perform point cloud downsampling to obtain the preprocessed point cloud data.

3. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1 is characterized in that: S2, using millimeter wave radar to collect target data and perform filtering processing to obtain a target point set with speed labels; the targets include static targets and dynamic targets, including: 77GHz millimeter-wave radar is used to collect target data in a dusty environment. The target data includes: range, radial velocity, azimuth, and pitch angle. The 77GHz millimeter-wave radar can penetrate dust and water mist in the underground environment. Perform Kalman filtering on the collected target data to eliminate measurement noise and obtain filtered target data; According to the radial velocity in the filtered target data, targets with radial velocity absolute values ​​greater than a preset velocity threshold are marked as dynamic targets, and targets with radial velocity absolute values ​​greater than a preset velocity threshold are marked as static targets; The marked target is converted from the millimeter-wave radar polar coordinate system to the Cartesian coordinate system to obtain a target point set with velocity labels.

4. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1, characterized in that: S3 performs curvature-based feature extraction on the preprocessed point cloud data to obtain a point cloud feature set. The target point set with speed labels is spatially associated with the point cloud feature set. The point cloud features corresponding to dynamic targets are identified and eliminated based on the speed labels to obtain a static environment feature set, including: Based on the preprocessed point cloud data, the local curvature value of each point cloud is calculated, points with curvature values ​​greater than a first threshold are marked as edge feature points, and points with curvature values ​​less than a second threshold are marked as plane feature points, thereby obtaining a point cloud feature set containing edge feature points and plane feature points; wherein the first threshold is greater than the second threshold; For each target point with a speed label, search for all feature points within a preset radius of the target point in the point cloud feature, and establish an association relationship between the target point and the feature point; For target points marked as dynamic targets, all point cloud features associated with them are marked as dynamic features and removed from the point cloud feature set; For target points marked as static targets, the point cloud features associated with them are retained; For point cloud features that are not associated with any target point, their dynamic and static properties are determined through multi-frame consistency testing: if the position of the point cloud feature remains unchanged in multiple consecutive frames, it is marked as a static feature, otherwise it is marked as a potential dynamic feature and removed; All retained static feature points are combined into a static environment feature set.

5. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 4 is characterized in that: Establish the relationship between target points and feature points, including: For each target point with a speed label, search the point cloud feature set for a set of candidate feature points within a preset radius of the target point; Calculate the association cost cij between each target point and the corresponding candidate feature point; Construct the association cost matrix between the target point and the candidate feature points, use the Hungarian algorithm to solve the association cost matrix, and obtain the optimal association solution with the minimum association cost; According to the optimal association scheme, the associated feature point set of each target point is determined, and the association relationship between the target point and the feature point is established.

6. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1, characterized in that: S4 performs inter-frame matching and pose estimation based on the static environment feature set. It also uses static targets in the target point set with velocity labels as additional constraints and obtains the optimized pose sequence through graph optimization methods, including: Based on the static environment feature sets of the current frame and the previous frame, the iterative closest point algorithm or the normal distribution transformation algorithm is used for feature matching to obtain the relative pose transformation matrix and matching feature point pairs between adjacent frames; A graph optimization model is constructed using the relative pose transformation matrix as the initial pose estimate. The graph optimization model includes: the pose of the mobile platform at each moment as the pose node to be optimized; the lidar pose error constructed based on the relative pose transformation matrix; the lidar observation error constructed based on matching feature point pairs; and the millimeter-wave radar observation error constructed based on static targets in a target point set with velocity labels. According to the laser radar measurement accuracy, a first weight matrix of the laser radar pose error and the laser radar observation error is set; According to the millimeter-wave radar measurement accuracy, a second weight matrix of the millimeter-wave radar observation error is set; wherein the diagonal element values ​​of the first weight matrix are greater than the diagonal element values ​​of the second weight matrix, and the weight matrix is ​​the inverse matrix of the corresponding constraint error covariance matrix; A multi-frame motion consistency check is performed on the targets in the target point set with velocity labels. The target's motion trajectory is tracked in multiple consecutive frames. If the target's motion pattern suddenly changes, it is re-labeled as a dynamic target, and the corresponding millimeter-wave radar observation error is eliminated from the graph optimization model. Based on the constructed graph optimization model, the overall optimization objective function is defined as the weighted square sum of all constraint errors. The nonlinear least squares method is used to iteratively solve the overall optimization objective function, minimize the overall optimization objective function, and update the estimated value of each pose node until convergence to obtain the optimized pose sequence.

7. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 6, characterized in that: The LiDAR pose error constructed based on the relative pose transformation matrix includes: Use the relative pose transformation matrix as a constraint between adjacent pose nodes; The LiDAR pose error is defined as the Lie algebraic distance between the actual pose transformation and the relative pose transformation matrix between adjacent pose nodes to be optimized.

8. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 6, characterized in that: The lidar observation error constructed based on matching feature point pairs includes: For each pair of matching feature points, calculate the predicted position of the feature point in the previous frame projected to the current frame under the estimated value of the current pose node; The Euclidean distance between the predicted position and the actual matching feature point position is taken as the lidar observation error.

9. The underground environment laser radar and millimeter wave radar collaborative SLAM method according to claim 6, characterized in that: The millimeter-wave radar observation error based on the static target in the target point set with velocity labels includes: Get the target points marked as static targets in the target point set with velocity labels; Calculate the predicted observation value of each static target under the estimated value of the current pose node; The polar coordinate error between the predicted observation value and the actual observation value is taken as the millimeter wave radar observation error : ;in, Indicates the target distance measured by the millimeter-wave radar, represents the position of the kth static target in the global coordinate system, represents the position of the mobile platform in the global coordinate system at the i-th moment, Indicates the target azimuth measured by the millimeter-wave radar.

Images