Underground environment laser radar and millimeter wave radar cooperative slam method

Abstract

The application discloses a kind of downhole environment laser radar and millimeter wave radar collaborative SLAM method, it is related to unmanned positioning: laser radar is used to collect the point cloud data of downhole environment and carries out filtering processing;Millimeter wave radar is used to collect target data and carries out filtering processing;The point cloud feature set is obtained by extracting the feature based on curvature to the point cloud data after pre-processing, and the point cloud feature set is associated with the target point set with speed label in space, according to speed label identification and eliminate the point cloud feature corresponding to dynamic target, obtain static environment feature set;Based on static environment feature set, interframe matching and pose estimation are carried out, and the static target in the target point set with speed label is used as additional constraint, and the optimized pose sequence is obtained by graph optimization method;According to the optimized pose sequence, the static environment feature set is projected to global coordinate system, and the static environment map is constructed.The application avoids the interference of dynamic target to SLAM.

Description

TECHNICAL FIELD

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

[0002] With the rapid advancement of intelligent construction of mine, underground automatic driving technology as the key technology to improve the production efficiency and safety of mine has been widely concerned. In the mine underground working environment, the positioning technology based on prior point cloud map is an important part of automatic driving technology, and is the premise of realizing path planning and intelligent control. Simultaneous localization and mapping (SLAM) technology can enable intelligent equipment such as underground transport vehicles and tunnel engineering vehicles to realize autonomous positioning and environment perception in unknown environment, which is of great significance to ensure the safety of underground operation and improve production efficiency.

[0003] However, the underground environment has the characteristics of high dust concentration, water mist, poor lighting conditions, long and narrow roadway and a large number of dynamic targets, which brings serious challenges to traditional SLAM technology. The existing solution usually adopts a SLAM method based on laser radar to construct an environment map. Although laser radar can provide high-precision three-dimensional point cloud data, it still faces the following problems in the complex underground environment: First, the suspended particles such as dust and water mist in the underground environment cause serious attenuation and scattering of laser signals, resulting in a significant reduction in the detection distance of laser radar and a serious degradation of point cloud data quality. Second, the strong reflective objects such as metal equipment and pipelines in the roadway are prone to multi-path effects, causing distortion of point cloud data. Third, there are a large number of dynamic targets such as transport vehicles and workers in the underground environment, which are mistakenly included in the static map, resulting in incorrect map construction and seriously affecting the subsequent positioning accuracy.

[0004] Millimeter wave radar has natural advantages in dynamic target detection due to its long wavelength (77GHz frequency band wavelength about 4mm) and strong penetration ability in dust, smoke and other harsh environments, and can directly measure the radial velocity of the target. However, millimeter wave radar also has the disadvantages of relatively low distance and angle measurement accuracy, sparse point cloud and difficulty in accurately describing the geometric features of the environment, and cannot meet the needs of high-precision SLAM alone.

[0005] More importantly, the existing technology lacks an effective dynamic and static target separation mechanism. In the underground environment, the existence of dynamic targets not only pollutes the static map, but also introduces error constraints in inter-frame matching, leading to accumulation of pose estimation error. Traditional dynamic target detection methods based on geometric features have a significant decrease in reliability in dust environment, while methods based on motion consistency require multiple frames of data accumulation and have poor real-time performance.

[0006] Therefore, a single sensor is difficult to achieve stable and reliable SLAM in a complex downhole environment, and a collaborative SLAM method capable of fusing high-precision geometric information of a laser radar and reliable speed information of a millimeter wave radar is urgently needed to 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 simultaneous localization and mapping in a complex downhole environment. SUMMARY

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

[0008] The application provides a downhole environment laser radar and millimeter wave radar collaborative SLAM method, comprising: S1, collecting point cloud data of a downhole environment by using a laser radar and performing filtering processing to obtain preprocessed point cloud data; S2, collecting target data by using a millimeter wave radar and performing filtering processing to obtain a target point set with a speed label; wherein the target includes 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, identifying and removing the point cloud features corresponding to the dynamic targets according to the speed label to obtain a static environment feature set; S4, performing inter-frame matching and pose estimation based on the static environment feature set, simultaneously using the static targets in the target point set with the speed label as additional constraints, and obtaining an optimized pose sequence by a graph optimization method; S5, projecting the static environment feature set to a global coordinate system according to the optimized pose sequence, constructing a static environment map, and marking the motion trajectories of the dynamic targets in the target point set with the speed label in the static environment map to obtain a downhole environment map containing dynamic and static information.

[0009] In particular, there are a large number of dynamic targets such as transport vehicles and operating personnel in the downhole environment, which seriously destroy the static environment assumption of the traditional SLAM method. In a dusty environment, although the laser radar can provide dense point cloud data, it cannot directly distinguish the dynamic and static properties of the point cloud, resulting in dynamic targets being incorrectly included in the static map. The 77GHz millimeter wave radar can penetrate dust and directly measure the radial velocity of the target, which provides key information for separating dynamic and static targets.

[0010] The scheme ingeniously utilizes the complementary characteristics of the two sensors: in step S2, the millimeter wave radar works stably in the dust environment, obtains the speed information of each target and generates a target point set with a speed label, which is the data basis for realizing dynamic and static separation; in step S3, by spatially correlating the target points of the millimeter wave radar with the point cloud features of the laser radar, the speed information is transmitted to the point cloud data - for dynamic targets with radial speed greater than the threshold, all the point cloud features associated with them are removed from the feature set, so as to obtain a pure static environment feature set.

[0011] The application is based on the dynamic and static separation mechanism of the speed label, which does not depend on multi-frame accumulation or complex motion estimation, but uses single-frame speed measurement of the millimeter wave radar to realize real-time dynamic and static discrimination; secondly, in the dust environment, when the performance of the laser radar degrades, the speed information of the millimeter wave radar is still reliable, ensuring the accuracy of dynamic and static separation.

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

[0013] Further, S1, collecting point cloud data by using a laser radar and performing filtering processing to obtain preprocessed point cloud data, comprising: collecting original point cloud data of an underground environment by using a 16-line or more three-dimensional laser radar; performing filtering processing on the original point cloud data by using a K nearest neighbor statistical filtering algorithm; dividing the filtered point cloud data into voxel grids, replacing all points in the corresponding voxel grid with the gravity center point in each voxel, performing point cloud down-sampling to obtain preprocessed point cloud data;

[0014] In particular, in the environment of dust diffusion in the underground, the laser radar faces serious performance degradation problem - the scattering and absorption of dust particles to laser light cause the effective detection distance to be shortened, the point cloud to be sparse and the noise to be increased. The application first uses K nearest neighbor statistical filtering to remove outlier noise points caused by dust scattering; then, through voxel grid down-sampling, not only the data amount is reduced, but more importantly, through the aggregation of the gravity center of the point cloud in the voxel, the influence of random noise is further suppressed. This processing strategy ensures that even in the case of laser radar performance degradation, relatively reliable environmental geometric features can still be extracted from the original sparse and noise-polluted data.

[0015] Further, S2, target data is collected by the millimeter wave radar and filtered to obtain a target point set with a speed label; wherein the target includes static targets and dynamic targets, including: collecting target data in a dust environment by using a 77GHz millimeter wave radar, the target data including: distance, radial velocity, azimuth angle and pitch angle, wherein the 77GHz millimeter wave radar can penetrate dust and water mist in the downhole 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, marking the target with a radial velocity absolute value greater than a preset speed threshold as a dynamic target, and vice versa; converting the marked target 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, the 77GHz millimeter wave radar has excellent dust penetration ability due to its wavelength of about 4mm, and can still work stably in an environment where the laser radar fails. The millimeter wave radar can not only detect targets in harsh environments, but more importantly, it can directly measure the radial velocity of the target. By setting a speed threshold to distinguish between static and dynamic, the preliminary classification of the target is completed in the data collection stage, and each target carries an explicit speed label. This dynamic and static classification method based on physical measurement has higher reliability and real-time performance in a dust environment than the traditional method based on multiple frames of geometric features.

[0017] In addition, although the performance of the laser radar decreases in a dust environment, it can still provide geometric profile information of the environment after filtering; although the point cloud of the millimeter wave radar is sparse, it can reliably provide position and speed information of the target in a harsh environment. In particular, the speed label of the millimeter wave radar provides a key criterion for accurately removing the dynamic part of the laser point cloud, which cannot be achieved by a single sensor.

[0018] Further, S3, curvature-based feature extraction is performed on the pre-processed point cloud data to obtain a point cloud feature set, and the target point set with a speed label is spatially associated with the point cloud feature set, the point cloud features corresponding to the dynamic target are identified and removed according to the speed label, and a static environment feature set is obtained, including: according to the pre-processed point cloud data, the local curvature value of each point cloud is calculated, the points with a curvature value greater than a first threshold value are marked as edge feature points, and the points with a curvature value less than a second threshold value are marked as plane feature points, to obtain a point cloud feature set containing edge feature points and plane feature points; wherein the first threshold value is greater than the second threshold value; for each target point with a speed label, all feature points within a preset radius range of the target point are searched in the point cloud feature set, and an association relationship between the target point and the feature points is established; for the target point marked as a dynamic target, all point cloud features associated therewith are marked as dynamic features and removed from the point cloud feature set; wherein for the target marked as dynamic by the millimeter wave radar (such as a transport vehicle with a radial speed greater than a threshold value), all laser point cloud features within the spatial range thereof are directly removed. This method is fast and effective, and fully utilizes the speed detection advantage of the millimeter wave radar.

[0019] For the target point marked as a static target, the point cloud features associated therewith are retained; wherein for the target marked as static by the millimeter wave radar, the point cloud features associated therewith are retained. This is particularly important when the laser radar has data missing due to dust influence - the static target of the millimeter wave radar provides a reliable anchor point for the laser point cloud.

[0020] For the point cloud features not associated with any target point, the dynamic and static properties thereof are determined through multi-frame consistency inspection: if the position of the point cloud feature remains unchanged in continuous multiple frames, the point cloud feature is marked as a static feature, otherwise it is marked as a potential dynamic feature and removed; wherein for the area not covered by the millimeter wave radar (the field of view of the millimeter wave radar is limited), the dynamic and static properties of the area are determined through multi-frame consistency inspection. This ensures the integrity of the dynamic and static separation.

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

[0022] In particular, the coexistence of dynamic and static targets in the underground environment is a core challenge for SLAM - dynamic targets such as transport vehicles and operating personnel will cause serious positioning errors if included in the map. Traditional methods rely on the multi-frame geometric consistency of laser point clouds to determine the dynamic and static properties, but in a dusty environment, the quality of laser radar data decreases, and this method fails. While the millimeter wave radar can penetrate dust and directly measure speed, its data is sparse and cannot independently construct a detailed map.

[0023] The application firstly extracts the curvature features of the laser point cloud, and identifies the geometric structures such as edges and planes in the environment; then through a spatial correlation algorithm, the speed label of the millimeter wave radar is mapped to the corresponding laser point cloud features. The key of this cross-modal correlation is to design a multi-dimensional correlation cost function - not only considering the spatial distance, but also introducing the speed consistency and size similarity to ensure the accuracy of the correlation.

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

[0025] The correlation cost between each target point and the corresponding candidate feature point is calculated , wherein is a spatial distance cost, is a speed difference cost, is a size similarity cost, and is a 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, represents the radial velocity value of the i-th target point, represents the speed value estimated based on the position changes of the j-th candidate feature point in multiple frames, represents the radar scattering cross-sectional area of the i-th target point, represents the point cloud density value in the local neighborhood of the j-th candidate feature point; a correlation cost matrix between the target points and the candidate feature points is constructed, the Hungarian algorithm is used to solve the correlation cost matrix, and the optimal correlation scheme with the minimum correlation cost is obtained; according to the optimal correlation scheme, a set of associated feature points of each target point is determined, and the correlation relationship between the target points and the feature points is established.

[0026] In particular, in the underground dust environment, it is a great challenge to realize accurate correlation between the laser point cloud and the millimeter wave radar target: first, the dust causes the quality of the laser radar data to decrease, and the point cloud is sparse and unevenly distributed; second, the data characteristics of the two sensors are very different - the laser radar provides dense but speedless point cloud, and the millimeter wave radar provides sparse but speed-labeled target points; most importantly, simple spatial distance matching is prone to false correlation in a complex environment, resulting in incorrect dynamic and static attribute determination.

[0027] The application firstly sets a spatial distance cost , which is the most basic correlation basis, ensuring that only spatially adjacent points can belong to the same target. However, in the narrow underground tunnel, only distance can easily cause different adjacent targets to be incorrectly associated. ​

[0028] Then, the speed difference cost is set , the radial velocity directly measured by the millimeter wave radar has high reliability, while the speed of the laser feature points is estimated by the change of multiple frames. By comparing the consistency of the two, it can effectively avoid the wrong association of static wall point cloud to moving vehicles, or the confusion of targets with different speeds. This cannot be achieved in traditional methods.

[0029] Finally, the size similarity cost is set , which cleverly takes advantage of the complementary characteristics of the two sensors - the scattering cross section of the millimeter wave radar reflects the size of the target, and the local density of the laser point cloud also reflects the size of the target. For example, large transport vehicles have a larger radar scattering cross section, and the corresponding point cloud area density is also higher; while a single worker has smaller values for both indicators. This size consistency further improves the association accuracy.

[0030] Further, S4, based on the static environment feature set, inter-frame matching and pose estimation are performed, and the static targets in the target point set with a speed label are used as additional constraints, and an optimized pose sequence is obtained through a graph optimization method, including: based on the static environment feature set 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 the matching feature point pair between adjacent frames; wherein, based on the obtained static environment feature set, inter-frame matching is performed, which eliminates the interference of dynamic targets from the source. This ensures that even in a target-dense underground environment, only stable and reliable static features (walls, pillars, etc.) are used for pose estimation.

[0031] The relative pose transformation matrix is used as the initial pose estimation, and a graph optimization model is constructed; the graph optimization model includes: the pose of the mobile platform at each time is taken as the pose node to be optimized; the laser radar pose error is constructed based on the relative pose transformation matrix; the laser radar observation error is constructed based on the matching feature point pair; the millimeter wave radar observation error is constructed based on the static targets in the target point set with a speed label; according to the measurement accuracy of the laser radar, the first weight matrix of the laser radar pose error and the laser radar observation error is set; according to the measurement accuracy of the millimeter wave radar, the second weight matrix of the millimeter wave radar observation error is set; wherein, the diagonal element value of the first weight matrix is greater than the diagonal element value of the second weight matrix, and the weight matrix is the inverse matrix of the corresponding constraint error covariance matrix;

[0032] Multi-frame motion consistency test is performed on the targets in the target point set with speed labels, the motion trajectory of the target in continuous multiple frames is tracked, if the target motion mode mutates, it is re-labeled as a dynamic target, and the corresponding millimeter wave radar observation error is removed from the graph optimization model; based on the constructed graph optimization model, the overall optimization objective function is defined as the weighted sum of squares of all constraint errors, the overall optimization objective function is minimized by iterative solution of the nonlinear least squares method, and the estimated value of each pose node is updated until convergence, and the optimized pose sequence is obtained.

[0033] In particular, the application constructs a double-constrained graph optimization framework. When the laser radar is degraded due to dust, additional geometric constraints are provided by static targets of the millimeter wave radar to ensure the stability and accuracy of the pose estimation in the underground environment. The dust environment in the underground brings serious challenges to the traditional laser radar-based pose estimation: the dust causes the point cloud to be sparse, and the features available for matching are greatly reduced; more seriously, if dynamic targets (transport vehicles, personnel) participate in inter-frame matching, false pose constraints will be introduced, leading to rapid accumulation of positioning errors and even system collapse.

[0034] When the laser radar is in normal performance, the system mainly relies on high-precision laser features to achieve accurate positioning; when the dust is serious and the laser is degraded, the static target constraints of the millimeter wave radar can provide a basic reference to maintain the basic positioning ability of the system. More importantly, all the static elements involved in the optimization are strictly selected, which fundamentally avoids the pollution of dynamic targets to the positioning system.

[0035] Further, the laser radar pose error based on the relative pose transformation matrix includes: taking the relative pose transformation matrix as the constraint between adjacent pose nodes; the laser radar pose error is defined as the Lie algebra distance between the actual pose transformation between adjacent pose nodes to be optimized and the relative pose transformation matrix.

[0036] In particular, the dust environment in the underground has multiple effects on the laser radar: not only does it cause the point cloud to be sparse, but more seriously, the number of matchable features is greatly reduced. In this case, if only a single constraint type is relied on, it is easy to cause optimization degradation or fall into local optimum due to insufficient information.

[0037] The relative pose transformation matrix obtained based on the ICP or NDT algorithm reflects the overall geometric relationship between adjacent frames. Lie algebra distance is used instead of simple Euclidean distance because pose transformation belongs to manifold, and Lie algebra distance can correctly measure the combined error of rotation and translation. The advantage of this constraint is that even when the features are sparse, as long as there are enough static structures (such as the profile of the tunnel), basic constraint relationships between frames can still be provided.

[0038] Further, the laser radar observation error is constructed based on the matched feature point pairs, and includes: for each matched feature point pair, calculating a predicted position of a feature point in a previous frame projected to a current frame according to a current pose node estimation value; and taking an Euclidean distance between the predicted position and an actual matched feature point position as the laser radar observation error.

[0039] In particular, for each successfully matched feature point pair, an observation constraint is constructed by calculating a projection error. This point-to-point constraint can make full use of high-quality local features (such as obvious edge points and corner points) to provide higher positioning accuracy in local areas. When dust causes some areas to be featureless, the inter-frame pose constraint can still provide a basic constraint through the overall structure; and in local areas where features are clear, the feature point constraint can provide more accurate positioning information.

[0040] Further, the millimeter wave radar observation error is constructed based on static targets in the target point set with a speed label, and includes: obtaining target points in the target point set with a speed label that are marked as static targets; calculating predicted observation values of the static targets under a current pose node estimation value;

[0041] taking a polar coordinate error between the predicted observation values and actual observation values as the millimeter wave radar observation error

[0042]

[0043] wherein, represents a target distance measured by the millimeter wave radar, represents a position of the kth static target in a global coordinate system, represents a position of the mobile platform at the ith moment in the global coordinate system, represents a target azimuth angle measured by the millimeter wave radar;

[0044] In particular, in a downhole dust environment, when the laser radar is almost invalid, a traditional SLAM system will quickly degenerate due to insufficient constraints. The 77GHz millimeter wave radar can penetrate dust and work stably due to its 4mm wavelength.

[0045] In the present application, the distance error term : the distance directly measured by the millimeter wave radar has high reliability (not affected by dust), while the predicted distance depends on the current pose estimation and the global position of the static target. When the pose estimation 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. The angle error term : similarly, the azimuth angle measured by the millimeter wave radar is compared with a predicted angle calculated based on the pose. The angle constraint and the distance constraint complement each other to determine the accurate position of the target. ​

[0046] Compared with the prior art, the application has the advantages that:

[0047] The single sensor SLAM method in the complex downhole environment has the problems of decreased positioning accuracy caused by dust and water mist interference, serious degradation of point cloud quality of the laser radar in the low visibility environment, map construction error caused by dynamic targets, and lack of effective dynamic and static target separation mechanism.

[0048] The application collects environmental geometric feature information by laser radar, acquires target speed information by using the dust and water mist penetration characteristics of the 77GHz millimeter wave radar, and realizes accurate separation of dynamic and static targets based on the speed label. By establishing the spatial correlation relationship between the target point and the point cloud feature, the point cloud feature corresponding to the dynamic target is removed, and a pure static environment feature set is constructed. The high-precision geometric information of the laser radar and the reliable speed information of the millimeter wave radar are fused by using a graph optimization framework, the static target is taken as an additional constraint to improve the pose estimation accuracy, and finally a complete downhole environment map containing static environment structure and dynamic target motion trajectory is constructed.

[0049] The application effectively separates dynamic and static targets, avoids the interference of dynamic targets on the SLAM system, fully utilizes the complementary advantages of high-precision ranging of the laser radar and strong anti-interference ability of the millimeter wave radar, provides a complete map containing environmental structure and dynamic target information for the downhole autonomous driving vehicle, and improves the safety and efficiency of downhole operation. BRIEF DESCRIPTION OF DRAWINGS

[0050] The 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 is an exemplary flowchart of a downhole environment laser radar and millimeter wave radar collaborative SLAM method according to some embodiments of the application. DETAILED DESCRIPTION

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

[0053] As Figure 1As shown, the point cloud data of the underground environment is collected by the laser radar and filtered to obtain preprocessed point cloud data; the target data is collected by the millimeter wave radar and filtered to obtain a target point set with a speed label; wherein the target includes static targets and dynamic targets; the preprocessed point cloud data is subjected to curvature-based feature extraction to obtain a point cloud feature set, and the target point set with a speed label is spatially associated with the point cloud feature set, and the point cloud features corresponding to the dynamic targets are identified and removed according to the speed label, to obtain a static environment feature set; based on the static environment feature set, inter-frame matching and pose estimation are performed, and the static targets in the target point set with a speed label are used as additional constraints, and an optimized pose sequence is obtained by a graph optimization method; the static environment feature set is projected to a global coordinate system according to the optimized pose sequence, a static environment map is constructed, and the motion trajectories of the dynamic targets in the target point set with a speed label are marked in the static environment map, to obtain an underground environment map containing dynamic and static information.

[0054] Specifically, the laser radar and the millimeter wave radar are integrated on a mobile platform, the laser radar selects a 16-line or more three-dimensional laser radar, the millimeter wave radar selects a 77GHz frequency band, and has a multi-target tracking function. The installation positions of the two sensors satisfy the following spatial relationship: , wherein is the transformation matrix of the millimeter wave radar to the laser radar coordinate system, is a rotation matrix, is a translation matrix.

[0055] Time synchronization: a hardware clock synchronization mechanism is adopted, and the sampling time of the two sensors is aligned through a synchronization trigger signal, the time synchronization error is controlled within 1ms, and the time synchronization model is: ; wherein, is the sampling time of the laser radar, is the sampling time of the millimeter wave radar, is a fixed time offset, is a synchronization error.

[0056] The laser radar collects the point cloud data of the underground environment, and performs denoising, filtering and other preprocessing operations to remove abnormal points and invalid data.

[0057] Laser radar point cloud denoising: statistical outlier filtering is adopted: for each point, the distance to the nearest neighbor K points is calculated, and if the distance is greater than the mean value plus n times the standard deviation, it is regarded as an outlier, and the formula is: , is an outlier.

[0058] Voxel grid filtering is adopted: the point cloud space is divided into regular voxel grids, and the original point set is replaced by the barycenter point or the centroid point in each voxel to realize point cloud downsampling and denoising.

[0059] The millimeter wave radar collects distance, speed and other information of the downhole environment, and performs data calibration and compensation to improve the accuracy of the data.

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

[0061] Laser radar feature extraction: a curvature-based feature extraction method is used to calculate the point cloud curvature: , wherein, , is the normal vector of point i, is the average distance from point i to the field point. Set the curvature threshold: edge feature threshold

[0062] , 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 R=1.5m in the laser point cloud;

[0064] DBSCAN clustering is performed on the point cloud subset to extract the geometric contour of the target; the target feature descriptor is established:

[0065] Feature association: for each millimeter wave target , search for all laser feature points within a spatial distance threshold ; establish the association matrix of the laser radar features and the millimeter wave radar targets, and use the Hungarian algorithm to solve the optimal association, and the association cost function is: , wherein, is the spatial distance cost, is the velocity difference cost, is the size similarity cost, is the weight coefficient. Specifically, the spatial distance cost: , directly calculate the squared Euclidean distance; the velocity difference cost: , wherein, is estimated by the displacement of adjacent frames; the size similarity cost: , calculate the relative difference after normalization.

[0066] The system classifies the features according to the association relationship and the speed label: traverse all the associated pairs, check the speed label of the millimeter wave target; for ​For dynamic targets (0.5 m / s), mark all laser features associated with them as dynamic; for static targets, mark their associated features as static. Maintain a history location cache (5 frames) for unassociated features; compute the location change between consecutive frames, estimate the motion velocity; mark features with velocity less than a threshold and stable location as static, otherwise mark as potentially dynamic. Static environment feature set: {all features marked as static}; dynamic feature set: {all features marked as dynamic or potentially dynamic}; association mapping table: record the correspondence between each feature and the mmWave target, for later processing.

[0067] The system first performs data registration on the static environment feature sets of the current frame and the previous frame. In data processing, the ICP algorithm or the NDT algorithm is used to process the static feature point cloud data: the ICP algorithm finds the nearest correspondence between the point pairs through iteration, and calculates the transformation matrix that minimizes the registration error; the NDT algorithm divides the point cloud data into voxel grids, and describes the distribution of points in each voxel with a normal distribution, and realizes registration through probability density matching. The registration output contains two types of data: the relative pose transformation matrix T (containing rotation R and translation t) and the set of successfully matched feature point pairs.

[0068] The system constructs a graph data structure containing two types of nodes: pose node data: stores the pose state of the mobile platform at each time , as an optimization variable.

[0069] Map node data: laser radar feature data: edge feature points and plane feature points extracted through curvature analysis, each feature containing three-dimensional coordinates and local geometric descriptors; mmWave radar static target data: static targets after velocity screening, saving their global coordinates and radar measurement raw data .

[0070] The system performs error modeling and quantization on the three types of constraint data:

[0071] Laser radar pose constraint data processing: convert the relative pose transformation matrix obtained by inter-frame registration into Lie algebra form, and calculate the Lie algebra distance between the actual adjacent pose transformation and the measured transformation as the error term. This processing correctly handles the distance measurement on the manifold.

[0072] Laser radar observation constraint data processing: for each pair of matched feature points, perform coordinate transformation on the previous frame feature point coordinates through the current pose estimate value, calculate the Euclidean distance between the projected point and the actual feature point in the current frame, and form the observation error data.

[0073] ​​Millimeter-wave radar observation constraint data processing: polar coordinate measurement data of millimeter-wave radar are converted to Cartesian coordinate data An error model in polar coordinates is constructed by comparing the predicted observation value based on the current pose estimate. The distance error and the angle error quantify the positioning bias in the radial and tangential directions, respectively.

[0074] Millimeter-wave radar observation error

[0075] wherein, represents 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 at the ith moment in the global coordinate system, represents the target azimuth angle measured by the millimeter-wave radar.

[0076] System dynamically sets weight matrix according to sensor data quality: laser radar constraint data: based on point cloud density and feature matching confidence, set larger information matrix diagonal elements (inverse of covariance matrix), typical values are inverses of distance error 0.1 m and angle error 0.1 rad;

[0077] Millimeter-wave radar constraint data: considering its relatively low angle resolution, set smaller information matrix elements, typical values are inverses of distance error 0.5 m and angle error 0.5 rad.

[0078] 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 motion pattern of the target within the last 5 frames, detects sudden changes in speed or direction; removes the target with detected motion anomaly from the static target set and sets the corresponding observation constraint weight to zero or deletes it from the optimization graph.

[0079] Construct the overall optimization objective function, and take the weighted sum of squares of all constraint errors as the optimization objective. Use the Levenberg-Marquardt algorithm for nonlinear least squares optimization: in each iteration, update all error terms according to the current pose estimate; calculate the Jacobian matrix and Hessian matrix, solve the pose increment; update the pose node data until convergence (pose increment is less than a threshold or maximum iteration number is reached). Output the optimized pose sequence data for subsequent map construction and dynamic target trajectory labeling.

[0080] According to the optimized pose, the laser radar point cloud data is transformed in coordinates 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] ​The target detection information of the millimeter wave radar is used to label and classify the obstacles in the point cloud map. For example, the target objects can be classified as pedestrians, vehicles, equipment, etc. according to their speed, size, and other characteristics. The labeled and classified point cloud map has rich semantic information, which provides important support for subsequent path planning and navigation.

[0082] The point cloud map is stored and managed using a voxel grid map or an octree map, etc. The voxel grid map divides the space into small voxels, and each voxel represents whether there is an obstacle in this area. The octree map is a hierarchical data structure that can effectively represent large-scale three-dimensional environments.

[0083] The above describes the application creation and its implementation in a schematic manner, which is not restrictive, and the application can be implemented in other specific forms without departing from the spirit or essential characteristics of the application. The embodiment shown in the drawings is only one of the embodiments of the application, and the actual structure is not limited thereto. Therefore, if a person skilled in the art is inspired by it, without departing from the spirit of the application, similar structural forms and embodiments can be designed without creative design, which should belong to the protection scope of the application. In addition, the word "comprising" does not exclude other elements or steps, and the word "one" before the element does not exclude the inclusion of "multiple" elements. The words "first", "second", etc. are used to indicate names, not any specific order.

Claims

1. A downhole environment laser radar and millimeter wave radar collaborative SLAM method, characterized in that, Comprise: S1, collecting point cloud data of underground environment by laser radar and performing filtering processing to obtain preprocessed point cloud data; S2, collecting target data by millimeter wave radar and performing filtering processing to obtain target point set with speed label; wherein, the target includes static target and dynamic target; S3, performing curvature-based feature extraction on the preprocessed point cloud data to obtain point cloud feature set, and performing spatial correlation between the target point set with speed label and the point cloud feature set, identifying and removing the point cloud features corresponding to the dynamic target according to the speed label to obtain static environment feature set, comprising: calculating the local curvature value of each point cloud according to the preprocessed point cloud data, marking the points with curvature value greater than the first threshold value as edge feature points, and marking the points with curvature value less than the second threshold value as plane feature points to obtain the point cloud feature set containing edge feature points and plane feature points; wherein, the first threshold value is greater than the second threshold value; for each target point with speed label, searching for all feature points within the preset radius range of the target point in the point cloud feature, and establishing the correlation between the target point and the feature point; for the target point marked as dynamic target, marking all point cloud features associated therewith as dynamic features and removing them from the point cloud feature set; for the target point marked as static target, retaining the point cloud features associated therewith; for the point cloud features not associated with any target point, determining their dynamic and static properties through multi-frame consistency test: if the position of the point cloud feature remains unchanged in continuous multiple frames, it is marked as static feature, otherwise it is marked as potential dynamic feature and removed; all retained static feature points form the static environment feature set; S4, performing inter-frame matching and pose estimation based on the static environment feature set, simultaneously using the static targets in the target point set with speed label as additional constraints, and obtaining the optimized pose sequence through graph optimization method; S5, projecting the static environment feature set to the global coordinate system according to the optimized pose sequence, constructing the static environment map, and marking the motion trajectory of the dynamic target in the target point set with speed label in the static environment map to obtain the underground environment map containing dynamic and static information; Wherein, the correlation between the target point and the feature point comprises: For each target point with speed label, searching for a candidate feature point set within the preset radius range of the target point in the point cloud feature set; Calculating the correlation cost cij between each target point and the corresponding candidate feature point; Constructing the correlation cost matrix between the target point and the candidate feature point, solving the correlation cost matrix by using the Hungarian algorithm to obtain the optimal correlation scheme with the minimum correlation cost; According to the optimal correlation scheme, determining the associated feature point set of each target point, and establishing the correlation between the target point and the feature point; wherein the association cost : wherein, is a spatial distance cost, is a velocity difference cost, is a size similarity pair cost, is a weight coefficient; denotes the three-dimensional position coordinates of the ithtarget point in the Cartesian coordinate system, denotes the three-dimensional position coordinates of the jthcandidate feature point in the Cartesian coordinate system, denotes the radial velocity value of the ithtarget point, denotes the velocity value of the jthcandidate feature point estimated based on the position changes of multiple frames, denotes the radar cross section area of the ithtarget point, denotes the point cloud density value within the local neighborhood of the jthcandidate feature point.

2. The underground environment laser radar and millimeter wave radar cooperative SLAM method according to claim 1, characterized in that: S1, collecting point cloud data by laser radar and performing filtering processing to obtain preprocessed point cloud data, comprising: Collecting original point cloud data of underground environment by 16-line and above three-dimensional laser radar; Performing filtering processing on the original point cloud data by using K nearest neighbor statistical filtering algorithm; The filtered point cloud data is divided into a voxel grid, and the center of gravity in each voxel is used to replace all points in the corresponding voxel grid to perform point cloud downsampling to obtain preprocessed point cloud data.

3. The downhole environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1, characterized in that: S2, target data is collected by the millimeter wave radar and filtered to obtain a target point set with a speed label; wherein the target includes static targets and dynamic targets, including: 77GHz millimeter wave radar is used to collect target data in a dusty environment, and the target data includes distance, radial velocity, azimuth angle and elevation angle, wherein the 77GHz millimeter wave radar can penetrate dust and water mist in the downhole environment; Kalman filtering is performed on the collected target data to eliminate measurement noise to obtain filtered target data; According to the radial velocity in the filtered target data, targets with an absolute value of radial velocity greater than a preset speed threshold are marked as dynamic targets, and vice versa are marked as static targets; The marked targets are converted from the millimeter wave radar polar coordinate system to the Cartesian coordinate system to obtain a target point set with a speed label.

4. The downhole environment laser radar and millimeter wave radar collaborative SLAM method according to claim 1, characterized in that: S4, 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 a speed label are used as additional constraints to obtain an optimized pose sequence through a graph optimization method, 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 transform algorithm is used for feature matching to obtain the relative pose transformation matrix and the matching feature point pairs between adjacent frames; The relative pose transformation matrix is used as the initial pose estimation to construct a graph optimization model; the graph optimization model includes: the poses of the mobile platform at each time as the pose nodes to be optimized; laser radar pose error constructed based on the relative pose transformation matrix; laser radar observation error constructed based on the matching feature point pairs; millimeter wave radar observation error constructed based on the static targets in the target point set with a speed label; According to the laser radar measurement accuracy, the first weight matrix of the laser radar pose error and the laser radar observation error is set; According to the measurement accuracy of the millimeter wave radar, the second weight matrix of the millimeter wave radar observation error is set; wherein the diagonal element value of the first weight matrix is greater than that of the second weight matrix, and the weight matrix is the inverse matrix of the corresponding constraint error covariance matrix; The targets in the target point set with a speed label are subjected to multi-frame motion consistency test, and the motion trajectory of the target in consecutive multiple frames is tracked; if the target motion mode changes suddenly, the target is re-marked as a dynamic target, and the corresponding millimeter wave radar observation error is excluded from the graph optimization model; Based on the constructed graph optimization model, the overall optimization objective function is defined as the weighted sum of squares of all constraint errors, and the nonlinear least squares method is used for iterative solution to minimize the overall optimization objective function, and the estimated value of each pose node is updated until convergence to obtain the optimized pose sequence.

5. The downhole environment laser radar and millimeter wave radar collaborative SLAM method according to claim 4, characterized in that: the laser radar pose error constructed based on the relative pose transformation matrix comprises: the relative pose transformation matrix is taken as a constraint between adjacent pose nodes; the laser radar pose error is defined as the Lie algebra distance between the actual pose transformation between the adjacent pose nodes to be optimized and the relative pose transformation matrix.

6. The downhole environment laser radar and millimeter wave radar collaborative SLAM method according to claim 4, characterized in that: the laser radar observation error constructed based on the matched feature point pairs comprises: for each matched feature point pair, the predicted position of the previous frame feature point is calculated under the current pose node estimation value and projected to the current frame; the Euclidean distance between the predicted position and the actual matched feature point position is taken as the laser radar observation error.

7. The downhole environment laser radar and millimeter wave radar collaborative SLAM method according to claim 4, characterized in that: the millimeter wave radar observation error constructed based on the static targets in the target point set with a velocity label comprises: the target points marked as static targets in the target point set with a velocity label are obtained; the predicted observation values of the static targets under the current pose node estimation value are calculated; polar error between the predicted observation and the actual observation is taken as the millimeter wave radar observation error : ; wherein, denotes the target distance measured by the millimeter wave radar, denotes the position of the kth static target in the global coordinate system, denotes the position of the mobile platform at the ith time instant in the global coordinate system, denotes the target azimuth angle measured by the millimeter wave radar.

Images