A laser SLAM mapping method in dynamic environment of underground mines

Abstract

The present application discloses a laser SLAM mapping method in a dynamic environment of an underground mining industry, which involves data processing, including: mapping using a Cartographer SLAM algorithm; segmenting a PCD point cloud map using an octree-based segmentation method according to a preset movement or deformation frequency threshold of objects in each area of ​​the underground mining environment; adjusting the map resolution of each area in the PCD point cloud map according to the segmentation result; performing change detection on areas in the segmented PCD point cloud map where the movement or deformation frequency of objects exceeds the threshold using a change detection algorithm, and updating the new point cloud data of the changed area to the original PCD point cloud map in an incremental manner; registering and fusing the locally updated PCD point cloud map with the original PCD point cloud map; and improving the global consistency in view of the poor global consistency of underground mining laser SLAM mapping in the prior art.

Description

Technical Field

[0001] The present application relates to the field of data processing, and in particular to a laser SLAM mapping method in a dynamic environment of an underground mine. Background Art

[0002] As mining continues to deepen, the underground environment becomes increasingly complex, and traditional manual operations can no longer meet the needs of safe and efficient production. Applying unmanned driving technology to underground mines can greatly reduce the labor intensity of personnel, improve the efficiency and safety of mining operations, and is of great significance to promoting intelligent and unmanned mining.

[0003] In unmanned driving systems, positioning technology is a key link. The point cloud map constructed by laser SLAM technology can provide global environmental perception information for unmanned vehicles, which is an important prerequisite for subsequent vehicle positioning, path planning and motion control. However, there are still many challenges when applying laser SLAM technology to underground environments.

[0004] First, the underground tunnels are long and narrow with complex structures, and there is an obvious long corridor effect. The measurement data of the lidar has almost no constraints in the tunnel direction, which easily leads to the accumulation of matching and optimization errors. Secondly, there are a large number of dynamic obstacles in the underground environment, such as transport vehicles and staff. These dynamic objects will put higher requirements on the stability and robustness of the laser SLAM system. The mismatch caused by dynamic objects will seriously interfere with the construction of the point cloud map, and then affect the positioning and control accuracy of the unmanned vehicle.

[0005] Some methods have been explored for SLAM problems in dynamic environments. The keyframe-based SLAM method responds to dynamic changes through keyframe screening and matching, and has achieved good results in slowly changing scenes. However, this type of method has high requirements for keyframe management and selection strategies, and needs to balance the real-time nature of map updates and map construction accuracy, and the algorithm complexity is relatively high. Another type is a dynamic SLAM method based on local bundle adjustment, which optimizes local areas through sliding windows to adapt to dynamic changes. However, this type of method has a large amount of calculation and a limited update frequency, making it difficult to meet the real-time requirements of the dynamic environment underground.

[0006] Therefore, how to achieve robust, real-time, and high-precision laser SLAM mapping in a complex and changeable underground environment to support the continuous positioning and autonomous navigation of unmanned vehicles is a key issue that needs to be solved urgently. In response to the above problems, the present invention proposes a laser SLAM mapping method for the dynamic environment of underground mining. The method comprehensively utilizes multi-source sensor information such as inertial measurement unit IMU, laser radar, and camera, and through strategies such as adaptive point cloud segmentation, dynamic point elimination, and incremental local map update, while improving dynamic adaptability, the global consistency of the map is ensured. The laser SLAM mapping method of the present invention can provide accurate and real-time navigation maps for unmanned vehicles, which is of great significance for improving the environmental adaptability and working stability of underground unmanned driving systems. Summary of the invention

[0007] In view of the problem of poor global consistency in laser SLAM mapping in the dynamic environment of underground mining in the prior art, the present application provides a laser SLAM mapping method in the dynamic environment of underground mining, which uses laser radar data for SLAM mapping to obtain a global point cloud map, and performs incremental updates based on the global point cloud map, thereby improving global consistency.

[0008] The purpose of this application is achieved through the following technical solutions.

[0009] The present application provides a laser SLAM mapping method in a dynamic environment of an underground mining enterprise, including: respectively collecting IMU data, laser radar point cloud data and camera data of an unmanned vehicle in an underground mining environment; and performing spatial calibration and time synchronization processing on the collected data; preprocessing the collected laser radar point cloud data; using the preprocessed laser radar point cloud data and the time-synchronized IMU data as input, and using Cartographer A SLAM algorithm is used to build a map to obtain a PCD point cloud map of the underground mining environment; an octree-based segmentation method is used to segment the PCD point cloud map according to a preset threshold value of the movement or deformation frequency of objects in each area of ​​the underground mining environment; the map resolution of each area in the PCD point cloud map is adjusted according to the comparison result between the movement or deformation frequency of objects in the segmented area and the threshold; a change detection algorithm is used to perform change detection on the area where the movement or deformation frequency of objects in the segmented PCD point cloud map exceeds the threshold, and the new point cloud data of the detected changed area is incrementally updated to the corresponding area of ​​the original PCD point cloud map to obtain a partially updated PCD point cloud map; the locally updated PCD point cloud map is registered and fused with the original PCD point cloud map, and the original PCD point cloud map is updated to obtain an updated PCD point cloud map of the underground mining environment.

[0010] Furthermore, the collected data is spatially calibrated and time-synchronized, including: calibrating the spatial conversion relationship between the lidar coordinate system and the body coordinate system of the unmanned vehicle; calibrating the spatial conversion relationship between the lidar coordinate system and the IMU coordinate system; calibrating the spatial conversion relationship between the body coordinate system of the unmanned vehicle and the camera coordinate system; obtaining the spatial coordinate transformation matrix between the lidar coordinate system, the IMU coordinate system, the body coordinate system and the camera coordinate system; using the PTP time synchronization strategy to time synchronize the collected IMU data, lidar point cloud data and camera data; in the ROS environment, using the ROS time synchronization mechanism to align the timestamps of the time-synchronized IMU data, lidar point cloud data and camera data; using the ROS coordinate transformation mechanism, according to the obtained spatial coordinate transformation matrix, the IMU data, lidar point cloud data and camera data with aligned timestamps are coordinate transformed to obtain the time-synchronized and spatially aligned data.

[0011] Furthermore, the collected lidar point cloud data is preprocessed, including: filtering the lidar point cloud data obtained by continuous scanning to obtain filtered lidar point cloud data; based on the filtered lidar point cloud data, statistically analyzing the position of each point in multiple observations, and calculating the standard deviation of the corresponding point appearance position; specifically, the appearance position here refers to the three-dimensional coordinates of the same point at different scanning times. By calculating the standard deviation of the coordinates of a point in multiple appearances, it can be determined whether the position of the point is stable.

[0012] According to the position change of each point, the consistency score of each point in the preset time window is calculated; the position of each point in multiple observations is statistically analyzed, and the standard deviation of the corresponding point position is calculated; according to the consistency score and standard deviation of each point, the dynamic nature of each point is judged, where the points with a consistency score lower than the threshold or a standard deviation higher than the threshold are marked as dynamic points. Dynamic points represent points with obvious or unstable position changes in continuous scanning or multiple observations, usually corresponding to dynamic obstacles or noise. The DBSCAN (Density-Based Spatial Clustering of Applications with Noise) clustering algorithm is used to group the filtered lidar point cloud data to obtain multiple point cloud clusters. The clustering results obtain multiple point cloud clusters, each of which corresponds to an object or area in the point cloud. The centroid tracking algorithm is used to track the point cloud clusters in continuous scanning, and the shape change of each point cloud cluster is calculated to obtain the centroid change and shape change of each point cloud cluster. The position change reflects the degree of movement of the point in the continuous scanning; the point cloud clusters with a centroid change greater than the preset threshold or a shape change greater than the preset threshold are marked as dynamic clusters; dynamic clusters represent objects or areas whose positions or shapes change significantly in continuous scanning, usually corresponding to moving or deformed obstacles. For point cloud points marked as dynamic points, they are removed from the point cloud data. For point cloud clusters marked as dynamic clusters, all points in the cluster are removed from the point cloud data. After removing dynamic points and dynamic clusters, the preprocessed lidar point cloud data is obtained, which mainly contains point cloud information of static environments and stable objects.

[0013] Specifically, the determination of dynamic points is based on the consistency score and position standard deviation of a single point, reflecting the dynamics at the point level. The determination of dynamic clusters is based on the centroid change and shape change of the point cloud cluster as a whole, reflecting the dynamics at the object level. The process of obtaining dynamic points and dynamic clusters is independent of each other. Finally, the points and clusters marked as dynamic are removed respectively to obtain the point cloud map in a static environment.

[0014] Specifically, the centroid tracking algorithm achieves tracking by continuously estimating the centroid position of the target. The basic steps are as follows: Cluster the point cloud of the current frame to obtain the centroid position of each cluster. Match the centroid of the cluster of the current frame with the centroid of the cluster of the previous frame to establish a correspondence between clusters. Common matching strategies include nearest neighbor matching and feature matching. According to the matching results, the trajectory of the target is updated. If a cluster successfully matches a cluster in the previous frame, it is considered to be the same target and its trajectory is updated; if a cluster in the current frame does not match any cluster in the previous frame, it is used as the new target trajectory; for the unmatched cluster in the previous frame, it means that the target may disappear and its trajectory will not be updated for the time being. Through centroid tracking, the motion information (center of mass change) and shape change information of the point cloud clusters between consecutive frames can be obtained, so as to judge the dynamics of the clusters.

[0015] Furthermore, the position change of each point in the filtered lidar point cloud data during continuous scanning is calculated to obtain the position change of each point , through the following formula: , where the positions in the t-th scan and the t+1-th scan are and .

[0016] Furthermore, according to the position change of each point, the consistency score of each point within the preset time window is calculated. , through the following formula: , where N is the number of scans in the time window; n is the number of times point p appears.

[0017] Furthermore, the position of each point in multiple observations is statistically analyzed to calculate the standard deviation of the corresponding point position. , calculated by the following formula: ,in, Yes The average position among n occurrences; Represents the position of the i-th point.

[0018] Furthermore, the centroid tracking algorithm is used to track the point cloud clusters in the continuous scanning, and the shape change of each point cloud cluster is calculated to obtain the centroid change of each point cloud cluster. and shape change , through the following formula: ,in, and are the cluster centroids at time t and t+1 respectively; ;in, and represents the difference of the shape descriptors of the clusters in consecutive time frames; when When , the corresponding point is judged to be a dynamic point and marked as 1, otherwise marked as 0; represents the position change threshold, represents the shape change threshold.

[0019] Furthermore, the Cartographer SLAM algorithm is used to construct a map to obtain a PCD point cloud map of the underground mining environment, including: inputting the pre-processed laser radar point cloud data and the IMU data after time synchronization processing into the Cartographer SLAM algorithm; using the Cartographer SLAM algorithm, estimating the motion trajectory of the laser radar according to the IMU data, and performing motion compensation on the laser radar point cloud data obtained by continuous scanning according to the estimated motion trajectory to obtain compensated laser radar point cloud data; using the Cartographer SLAM algorithm, through the scan matching method, determining whether there is a loop between the laser radar point cloud data obtained by the current scan and the historical laser radar point cloud data, if so, calculating the pose transformation relationship of the loop; using the Cartographer SLAM algorithm, constructing a pose graph according to the compensated laser radar point cloud data and the pose transformation relationship obtained by loop detection, and optimizing the pose graph using a graph optimization algorithm to obtain the optimized laser radar motion trajectory; splicing the obtained compensated laser radar point cloud data according to the optimized motion trajectory to obtain a PCD point cloud map of the underground mining environment.

[0020] Furthermore, an octree-based segmentation method is used to segment the PCD point cloud map, including: extraction of image change areas, and motion estimation of two consecutive frames of image data collected using an optical flow method based on visual difference. The optical flow method obtains the motion vector of each pixel by solving the optical flow constraint equation, which reflects the displacement and motion direction of the pixel between consecutive frames. According to the pixel's motion vector, its motion amplitude (such as the vector modulus) and motion direction are calculated. Pixels with a motion amplitude greater than a preset threshold are marked as changed pixels, and the changed area in the image is extracted as prior information on the dynamics of the environment.

[0021] Using the prior information of the image change area, the point cloud map is adaptively divided into spaces, the octree is initialized, and the maximum depth L of the octree is set, which indicates the maximum number of division layers allowed by the octree. The maximum depth L determines the granularity of the point cloud map division. The larger L is, the smaller the divided area is and the higher the resolution of the map is. The change threshold θ is set to indicate the change pixel ratio threshold for judging whether a node needs to be further divided. The smaller θ is, the easier it is to trigger the node division and obtain a finer-grained segmentation result. The bounding box of the entire PCD point cloud map is used as the root node of the octree, representing the entire three-dimensional space. Recursively divide the nodes, starting from the root node, and perform the following division operations on each node until the stop condition is met: Project the point cloud data corresponding to the current node onto the image plane. In this step, the three-dimensional point cloud is converted into two-dimensional image coordinates through the camera parameters to obtain the corresponding area of ​​the node in the image. Calculate the ratio of changed pixels in the corresponding area of ​​the node. Count the total number of pixels and the number of changed pixels in the area, and calculate the ratio of the number of changed pixels to the total number. Determine whether the current node needs to be divided: If the ratio of changed pixels is greater than the preset threshold θ, and the depth of the current node has not reached the maximum depth L, the node needs to be further divided. Divide the current node into eight child nodes, each of which corresponds to eight equal divisions of the current node space. Recursively perform the above division operation on each child node. If the ratio of changed pixels is less than or equal to the threshold θ, or the depth of the current node has reached the maximum depth L, stop the division and keep the current node as a leaf node. After the construction of the octree and the node division are completed, the obtained leaf nodes correspond to different areas in the point cloud map. In areas where the image changes drastically, the octree will perform deeper divisions to obtain more and smaller leaf nodes. This is because the nodes in the changing area are more likely to meet the condition that the ratio of changed pixels is greater than the threshold θ during the division process, thereby triggering further division. In areas where the image changes slowly, the octree division will stop earlier and obtain larger leaf nodes. This is because the nodes in the stable area are more likely to meet the condition that the ratio of changed pixels is less than or equal to the threshold θ during the division process, and thus no further division is performed. Through this adaptive octree division strategy, the point cloud map is divided into areas of different granularities. Areas with dramatic changes are divided more finely, reflecting high-resolution map representation, while areas with gentle changes are divided less finely, reflecting low-resolution map representation. This adaptive division based on the dynamic characteristics of the environment can use different map resolutions in different areas, balancing the fineness of representation and computational efficiency, and achieving dynamic adaptive optimization of point cloud maps.

[0022] After completing the construction of the octree and node division, it is necessary to further calculate the attributes of the nodes, and mark and process the nodes according to the attributes, so as to finally achieve adaptive adjustment and update of the point cloud map resolution. Calculation of node attributes. For each leaf node of the octree, count the number of point clouds in the node. This step requires traversing the point cloud data corresponding to the node and accumulating the number of point clouds. Calculate the point cloud density of the node. The point cloud density represents the number of point clouds per unit volume, reflecting the compactness of the point cloud distribution in the node area. The density value can be calculated by dividing the number of point clouds in the node by the node volume. Calculation of the time rate of change of node attributes. For each node, calculate the point cloud density change rate ρ between two adjacent frames. The density change rate reflects the time-varying characteristics of the point cloud distribution in the node. The point cloud density of the nodes corresponding to the current frame and the previous frame are respectively recorded as and , then the density change rate ρ can be expressed as: ,The larger the value of ρ, the more drastic the change of point cloud density in the node, and the more obvious the dynamic characteristics of the corresponding area.

[0023] The node marking sets a preset threshold value, which indicates the frequency threshold value of the object moving or deforming. The threshold value reflects the speed characteristics of the object moving or deforming in the environment. For each node, the relationship between its density change rate ρ and the frequency threshold value is compared: if ρ is greater than the frequency threshold value, the node is marked as a changing node, indicating that there is a significant dynamic change in the area corresponding to the node. If ρ is less than or equal to the frequency threshold value, the node is marked as a stable node, indicating that the area corresponding to the node is relatively stable and the dynamic change is not obvious.

[0024] Adaptive adjustment of map resolution. For nodes marked as changing nodes and whose depth has not reached the maximum depth L, it is necessary to further improve the map resolution of the corresponding area to obtain a more detailed map representation. Continue to subdivide the child nodes of the node and recursively perform the octree division operation until the maximum depth L is reached. Update the map resolution of the area corresponding to the node to the higher resolution corresponding to the maximum depth L. For nodes marked as stable nodes, the map resolution of the corresponding area can be reduced to reduce redundant map details. Stop subdividing the child nodes of the node, merge its child nodes into the current node, and simplify the structure of the octree. Update the map resolution of the area corresponding to the node to the lower resolution corresponding to the current node depth.

[0025] The point cloud map is updated according to the segmentation results of the octree and the resolution adjustment of the nodes, and the resolution settings of each area in the PCD point cloud map are updated. For areas with frequent changes and complex dynamics, a higher resolution is used to represent them to retain more map details. For stable and static areas, a lower resolution is used to represent them to reduce redundant information and improve storage and computing efficiency. In the end, an adaptive resolution point cloud map of the underground mining environment is obtained. Different areas use different resolutions according to their dynamic characteristics, realizing adaptive optimization of map representation.

[0026] By calculating node attributes, analyzing the time change rate, and marking nodes, the dynamically changing areas and stable areas in the point cloud map can be identified. On this basis, combined with the segmentation results of the octree, an adaptive resolution adjustment strategy is adopted for different areas to dynamically update the representation of the point cloud map. This adaptive map resolution optimization method can obtain high-fidelity map details in areas with drastic changes, while reducing redundant information in stable areas, balancing the map's fineness and computational efficiency, and realizing the dynamic adaptive representation of point cloud maps in underground mining environments.

[0027] Furthermore, the point cloud density of the node is calculated, and the point cloud density change rate ρ of each node between two adjacent frames is calculated by the following formula: ,in, and They respectively indicate that the change rate ρ of the target voxel in the current frame and the previous frame exceeds the set threshold θ.

[0028] Compared with the prior art, the advantages of this application are:

[0029] By performing spatiotemporal synchronization and fusion of multi-source sensor data such as IMU, lidar, and camera, accurate, high-frequency odometer information and dense environmental point cloud information can be obtained, providing rich and reliable sensor data for the SLAM system and improving the accuracy of mapping and positioning.

[0030] The point cloud elimination mechanism is introduced to semantically segment dynamic points through a deep learning model, and the influence of dynamic points is eliminated during the SLAM optimization process. This strategy can effectively eliminate the interference of dynamic objects on the mapping process, improve the stability and robustness of mapping, and overcome the influence of the underground dynamic environment on the SLAM system.

[0031] Adaptive point cloud segmentation and local sub-map update strategies are adopted to adaptively divide the global map into several local sub-maps according to the characteristics of the environment and the distribution of sensor data. Independent optimization and update of local sub-maps ensures the real-time nature of map updates and reduces computational complexity. At the same time, the local sub-maps maintain global consistency and avoid the generation of cumulative errors.

[0032] While improving the map resolution, the sparsity of the map is also taken into account. The resolution of the map is dynamically adjusted through the octree structure, which reduces the amount of map data while ensuring the fineness of the map, and improves the efficiency of map storage and transmission. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] The present application will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0034] Figure 1 is an exemplary flow chart of a laser SLAM mapping method in a dynamic environment of an underground mine according to some embodiments of the present application;

[0035] Figure 2 is an exemplary flow chart of generating spatiotemporal synchronization and spatial alignment data according to some embodiments of the present application;

[0036] Figure 3 is an exemplary flow chart for obtaining pre-processed LiDAR point cloud data according to some embodiments of the present application;

[0037] Figure 4 This is an exemplary flow chart for generating a PCI point cloud map of a well operating environment according to some embodiments of the present application. DETAILED DESCRIPTION

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

[0039] like Figure 1As shown in the figure, the IMU data, LiDAR point cloud data and camera data of the unmanned vehicle in the underground mining environment are collected respectively; the collected data are spatially calibrated and time-synchronized; the collected LiDAR point cloud data are pre-processed; the pre-processed LiDAR point cloud data and the time-synchronized IMU data are used as input, and Cartographer is used to A SLAM algorithm is used to build a map to obtain a PCD point cloud map of the underground mining environment; an octree-based segmentation method is used to segment the PCD point cloud map according to a preset threshold value of the movement or deformation frequency of objects in each area of ​​the underground mining environment; the map resolution of each area in the PCD point cloud map is adjusted according to the comparison result between the movement or deformation frequency of objects in the segmented area and the threshold; a change detection algorithm is used to perform change detection on the area where the movement or deformation frequency of objects in the segmented PCD point cloud map exceeds the threshold, and the new point cloud data of the detected changed area is incrementally updated to the corresponding area of ​​the original PCD point cloud map to obtain a partially updated PCD point cloud map; the locally updated PCD point cloud map is registered and fused with the original PCD point cloud map, and the original PCD point cloud map is updated to obtain an updated PCD point cloud map of the underground mining environment.

[0040] like Figure 2 As shown, the unmanned vehicle performs spatial and temporal synchronization on each sensor, collects IMU data, lidar data, and camera data; calibrates the spatial transformation relationship between the lidar coordinate system and the body coordinate system of the unmanned vehicle. A calibration plate is set on the vehicle body, and the lidar is used to scan the calibration plate. By extracting the feature points of the calibration plate, the spatial transformation matrix from the lidar coordinate system to the body coordinate system is calculated. Calibrate the spatial transformation relationship between the LiDAR coordinate system and the IMU coordinate system. Fix the IMU on the LiDAR, rotate the LiDAR-IMU assembly, and use the accelerometer and gyroscope data to estimate the spatial transformation matrix from the LiDAR coordinate system to the IMU coordinate system. Calibrate the spatial transformation relationship between the body coordinate system and the camera coordinate system of the unmanned vehicle. Use the checkerboard calibration plate to take multiple images at different positions, extract the checkerboard corner points, and calculate the spatial transformation matrix from the camera coordinate system to the body coordinate system. Through the cascade of coordinate transformation matrices, the spatial coordinate transformation relationship between the lidar coordinate system, IMU coordinate system, vehicle coordinate system and camera coordinate system is obtained, namely: .

[0041] Time synchronization, using PTP (Precision Time Protocol) to synchronize the time of each sensor. The main controller serves as the PTP master node, and the IMU, lidar, and camera serve as PTP slave nodes to synchronize their clocks. Each sensor collects data at a fixed frequency and sends the data together with the timestamp to the main controller. Data fusion in the ROS environment, in the ROS (Robot Operating System) environment, the time-synchronized IMU data, lidar point cloud data, and camera image data are converted into ROS messages and published to the corresponding ROS topics. Using the ROS time synchronization mechanism, such as the Approximate Time synchronization strategy, the IMU messages, lidar point cloud messages, and camera image messages in different topics are aligned by timestamp, that is, a group of messages with the closest timestamps are found. Using the ROS coordinate transformation mechanism, such as the TF (Transform) tree, the coordinate transformation matrix is ​​obtained to transform the data after the timestamp alignment and unify them into the vehicle coordinate system. The lidar point cloud is transformed using the transformation matrix Convert the IMU data to the vehicle coordinate system using the transformation matrix and Convert the camera image to the vehicle coordinate system using the transformation matrix Convert to the vehicle coordinate system. So far, we have obtained the IMU data, lidar point cloud data and camera image data after time synchronization and spatial alignment. They are all in the vehicle coordinate system and can be used for subsequent laser SLAM mapping and positioning.

[0042] like Figure 3 As shown, the laser radar data is preprocessed to collect continuous scanning frames of the laser radar, and each frame contains the same number of points. Assume that the laser radar scans the environment at a fixed frequency and obtains N frames of data per second. De-noising is performed on each frame of the laser radar point cloud data, such as statistical filtering, to remove discrete points far away from the main body of the point cloud. Radius filtering is performed on the point cloud to reduce the point cloud density and improve processing efficiency. Radius filtering divides the point cloud into several grids, and only one point is retained in each grid. The filtered point cloud is voxelized to further reduce the point cloud density. Voxelization divides the point cloud space into a three-dimensional voxel grid, and the points in each voxel are approximated as the voxel center point. Point-to-point difference calculation, for each laser radar point p, find its corresponding point in two adjacent frames of point cloud data. and , calculate the Euclidean distance between them, that is, the change in position: If the distance d is greater than the preset threshold , it is considered that the point has a significant displacement between the two frames and may be a dynamic point.

[0043] To calculate the consistency score, set a time window containing N frames of continuous lidar data. For each point p, count the number of times it appears in the time window n, and whether the position change at each appearance is below the threshold. . Calculate the consistency score C of point p: ,in, If the position change of point p in all n occurrences is less than the threshold , then it is considered a static point, and the consistency score C is equal to its frequency of occurrence. Calculate the average position of point p in n occurrences , and calculate the standard deviation of its position : ; ,in, is the position of point p at the i-th occurrence. If the consistency score C of point p is less than the threshold , or the location standard deviation Greater than threshold , it is marked as a dynamic point.

[0044] Point cloud clustering and dynamic analysis, use the DBSCAN algorithm to cluster each frame of point cloud to obtain several point cloud clusters. For each point cloud cluster, track its position and shape changes in consecutive frames: calculate the centroid of the point cloud cluster at time t , and the centroid at time t+1 Compare and calculate the displacement of the center of mass . Calculate the shape descriptor of the point cloud cluster at time t (such as bounding box volume, aspect ratio, etc.), and the shape descriptor at time t+1 Compare and calculate deformation If the centroid of the point cloud cluster is displaced Greater than threshold , or shape variable Greater than threshold , then the cluster is marked as a dynamic cluster. For all points in the dynamic cluster, mark their dynamic attributes as 1; for points in the non-dynamic cluster, mark their dynamic attributes as 0. Points marked as dynamic can be removed or specially processed in the subsequent SLAM mapping process to improve the accuracy and stability of mapping.

[0045] The preprocessed LiDAR point cloud data and the time-synchronized IMU data are input into the Cartographer SLAM algorithm. The preprocessed LiDAR point cloud data and the time-synchronized IMU data are published to the corresponding ROS topics through the ROS message subscription mechanism. In the configuration file of the Cartographer SLAM algorithm, the subscribed LiDAR point cloud topic and IMU data topic are set so that the algorithm can receive sensor data. Using the Cartographer SLAM algorithm, the motion trajectory of the LiDAR is estimated based on the IMU data, and the motion compensation of the LiDAR point cloud data is performed. Based on the angular velocity and acceleration data of the IMU, the motion trajectory of the LiDAR between two consecutive frames of data is estimated using the pre-integration method to obtain the LiDAR's pose transformation matrix.

[0046] Specifically, between two consecutive frames of lidar data, the motion trajectory of the lidar is estimated using the pre-integration method based on the angular velocity and acceleration data of the IMU. Assume that the IMU measurement value at time k is the angular velocity and acceleration , the IMU measurement value at time k+1 is the angular velocity and acceleration . Calculate the rotation increment between time k and time k+1 according to the IMU measurement value and speed increment : ; in, is the sampling time interval of the IMU, Exp(*) is the exponential mapping function, which converts the rotation vector into a rotation matrix. According to the rotation increment and velocity increment, the pre-integration obtains the attitude at time k+1 ,speed and location : ; ; , where g is the gravity acceleration vector. Through continuous pre-integration, the motion trajectory of the laser radar between two consecutive frames of data is obtained, that is, the pose transformation matrix : For each point in the LiDAR point cloud , according to the estimated pose transformation matrix , transform its coordinates to the coordinate system of the previous frame of lidar point cloud: , transform the coordinates of all point cloud points to obtain the compensated lidar point cloud data.

[0047] Cartographer SLAM algorithm is used to perform loop detection by scanning matching. The laser radar point cloud data obtained by the current scan is matched with the historical point cloud data to calculate the similarity between them. Point cloud matching algorithms such as ICP (Iterative Closest Point) can be used. If the similarity between the current point cloud and the historical point cloud is higher than the preset threshold, it is considered that there is a loop, that is, the robot has returned to the previously visited position. Calculate the pose transformation relationship between the current point cloud and the historical point cloud, that is, the loop constraint. Specifically, the laser radar point cloud data obtained by the current scan is matched with the historical point cloud data to calculate the similarity between them. Use the ICP (Iterative Closest Point) algorithm for point cloud matching: For each point in the current point cloud, find the corresponding point with the closest distance in the historical point cloud. Calculate the pose transformation matrix between the current point cloud and the historical point cloud so that the distance between the corresponding points of the two point clouds is minimized. According to the pose transformation matrix, transform the current point cloud to the coordinate system of the historical point cloud. Repeat the above steps until the pose transformation matrix converges or the maximum number of iterations is reached. To calculate the similarity between the current point cloud and the historical point cloud, the following indicators can be used: The average distance between point pairs: ,in, and is the matching point pair. The standard deviation of the distance between the point pairs is: . The ratio of the number of matching point pairs to the total number of points. If the similarity index meets the preset threshold conditions (such as the average distance is less than the threshold, the standard deviation is less than the threshold, and the ratio of matching point pairs is greater than the threshold), it is considered that there is a loop between the current point cloud and the historical point cloud. The pose transformation relationship between the current point cloud and the historical point cloud is used as a loop constraint for subsequent pose graph optimization.

[0048] Using the Cartographer SLAM algorithm, the pose graph is constructed and optimized based on the compensated LiDAR point cloud data and loop constraints. The compensated LiDAR point cloud data of each frame is used as a node in the pose graph, and the edges between nodes represent the pose transformation relationship between two adjacent frames. The pose transformation relationship obtained by loop detection is used as the loop edge in the pose graph to connect the corresponding nodes. Using a graph optimization algorithm, such as g2o or ceres solver, the constructed pose graph is optimized to minimize the residual error of all edges and obtain the optimized LiDAR motion trajectory. Using the Cartographer SLAM algorithm, the pose graph is constructed and optimized to obtain the LiDAR motion trajectory and the point cloud map of the environment. The nodes in the pose graph represent the pose state of the LiDAR, and the edges represent the constraint relationship between nodes, including the pose transformation between adjacent frames and the long-distance constraints obtained by loop detection. The graph optimization algorithm obtains the optimal node pose estimation by minimizing the residual error, thereby obtaining a globally consistent LiDAR motion trajectory. The optimized pose graph and point cloud data can be used to construct a three-dimensional model of the environment, providing a basis for subsequent positioning, navigation, mapping and other tasks.

[0049] Specifically, each frame of compensated lidar point cloud data is used as a node in the pose graph. Each node contains the pose information of the point cloud of the frame, namely the translation vector and the rotation quaternion (or rotation matrix). The ID of the node can be represented by a timestamp or a serial number. For two adjacent frames of lidar point cloud data, a pose graph edge is added between the corresponding nodes according to the pose transformation relationship between them. The pose graph edge represents the spatial constraint relationship between the two nodes, including the uncertainty information of the relative pose transformation and the constraint (covariance matrix). The pose transformation between two adjacent frames can be obtained by methods such as IMU pre-integration or point cloud matching. The pose transformation relationship obtained by loop detection is used as the loop edge in the pose graph to connect the corresponding nodes. The loop edge represents the spatial constraint relationship between the current node and the historical node, including the uncertainty information of the relative pose transformation and the constraint. The pose transformation of the loop edge is obtained by a point cloud matching algorithm (such as ICP).

[0050] The pose graph optimization problem is defined as minimizing the residual error of all edges, that is, finding a set of node poses so that the pose transformation error of all edges is minimized. The residual error can be measured using Euclidean distance or Mahalanobis distance. The pose of each node is used as the optimization variable, that is, the translation vector and the rotation quaternion (or rotation matrix). The initial value can use the pose estimate obtained by IMU pre-integration or point cloud matching. For each pose graph edge, a residual function is constructed to calculate the pose transformation error under the current pose estimate. The general form of the residual function is: ,in, is the pose transformation of the edge, and is the pose estimate of the node.

[0051] The output of the residual function is a 6-dimensional vector, including translation error and rotation error. Select a graph optimization algorithm, such as g2o or ceres solver. Configure the parameters of the optimization algorithm, such as the maximum number of iterations, termination conditions, etc. Input the nodes, edges and residual functions of the pose graph into the graph optimization algorithm. Execute the optimization algorithm and iteratively update the node pose until convergence or the maximum number of iterations is reached. During the optimization process, the algorithm minimizes the sum of squares of all residual functions to obtain the optimal node pose estimate. Obtain the optimized node pose from the graph optimization algorithm, that is, the motion trajectory of the laser radar. The optimized node pose is more accurate, considering the constraint relationship between all pose graph edges and loop edges. In this application, the Cartographer SLAM algorithm is used to construct and optimize the pose graph based on the compensated laser radar point cloud data and loop constraints. The pose graph optimization obtains a globally consistent laser radar motion trajectory estimate by minimizing the residual errors of all edges. This SLAM method based on graph optimization can effectively reduce cumulative errors, improve positioning accuracy, and provide reliable pose information for autonomous navigation of unmanned vehicles in underground environments.

[0052] The compensated LiDAR point cloud data is spliced ​​according to the optimized motion trajectory to obtain a point cloud map of the underground environment. For each frame of preprocessed LiDAR point cloud data, the pose information at the corresponding moment is obtained according to the optimized LiDAR motion trajectory. The pose information includes the position (x, y, z) and direction (rotation matrix or quaternion) of the LiDAR in the global coordinate system. The point coordinates in each frame of point cloud data are transformed from the LiDAR local coordinate system to the global coordinate system. The coordinate transformation process can be expressed by the following formula: ,in, Represents the coordinates of the point in the local coordinate system of the laser radar, represents the transformation matrix from the local coordinate system to the global coordinate system, Represents the coordinates of a point in the global coordinate system. Transformation matrix It can be calculated based on the position and direction of the LiDAR. Commonly used representation methods include rotation matrix, quaternion, etc. All converted LiDAR point cloud data are stitched together to form a complete environmental point cloud map.

[0053] During the stitching process, point cloud data from different frames need to be merged into the same point cloud object. You can use the point cloud stitching functions provided by the point cloud library (such as PCL), such as pcl::concatenate Point Cloud(). The stitched point cloud map contains the three-dimensional structural information of the environment and reflects the spatial layout of the underground environment. Save the stitched point cloud map as a file in PCD (Point Cloud Data) format. PCD is a commonly used point cloud data storage format that supports different point cloud types and attributes. Use the write functions provided by the point cloud library, such as pcl::io::save PCD File ASCII() or pcl::io::save PCD File Binary() to save the point cloud map to a file. The saved PCD file can be used for subsequent visualization, analysis, processing and other tasks. The pre-processed lidar point cloud data is stitched according to the optimized motion trajectory to obtain a complete point cloud map of the underground environment. The coordinate transformation converts each frame of the point cloud to the global coordinate system, and the stitching process merges the point clouds of different frames into a complete point cloud object. The spliced ​​point cloud map is saved as a PCD format file for easy storage and exchange. Post-processing of the point cloud map, such as filtering, downsampling, smoothing, etc., can optimize the quality and visualization of the point cloud. The final point cloud map reflects the three-dimensional structure of the underground environment.

[0054] like Figure 4 As shown in Figure 1, the optical flow method is used to identify dynamic objects and pixel-level changes in the environment. Assume that the pixel brightness of the image is , movement occurs within a small time step dt, according to the optical flow constraint equation: ,in, and is the spatial gradient of the image, is the time gradient of the image, and u and v are the velocity components of the pixel in the x and y directions respectively. Use optical flow algorithms such as the Lucas-Kanade method or the Horn-Schunck method to solve the optical flow equation and obtain the pixel-level motion vector. According to the size and direction of the motion vector, determine which areas are dynamically changing.

[0055] Voxelize the point cloud and divide it into a three-dimensional voxel grid. For each voxel, calculate the point cloud density change rate ρ: ;in, and Represent the number of voxels in the current frame and the previous frame respectively. Voxels whose change rate ρ exceeds the set threshold θ are marked as change areas.

[0056] Adaptive point cloud map segmentation. According to the change detection results, the octree is used to adaptively segment the point cloud map. Set the maximum depth of the octree to L, divide the space of the current node into eight equal sub-spaces, and each sub-space forms a child node. Assume that the boundary of the current node is a cube with the center coordinate , and the side length is l. Then the center coordinate and side length of each child node are: ; Use the maximum depth L of the tree and the change threshold θ as the segmentation conditions: If the current depth d reaches the maximum depth L, stop the segmentation. If the change rate ρ of the voxel is less than the threshold θ, it is considered that the current voxel is stable and does not need to be segmented. If d < L and ρ > θ, continue to segment the current voxel. For the areas with frequent changes, continue to subdivide the child nodes to increase the resolution; for the areas with less changes, stop the subdivision.

[0057] Local map update. According to the adaptive segmentation results, perform local incremental updates on the submaps with high change frequencies. Use a point cloud registration algorithm (such as ICP) to register the updated submap with the original submap to obtain the transformation matrix. Use the transformation matrix to merge the updated submap into the global point cloud map to obtain the updated SLAM point cloud map. Specifically, according to the adaptive segmentation results, identify the submap areas with high change frequencies. For each submap with a high change frequency, perform local incremental updates. The steps of the incremental update are as follows: Obtain the latest lidar point cloud data within the submap area. Preprocess the latest point cloud data, such as removing outliers, downsampling, etc. Add the preprocessed point cloud data to the submap to update the point cloud representation of the submap. Update the octree structure of the submap and recalculate the point cloud density and change rate of each voxel. Complete the incremental updates of all submaps with high change frequencies.

[0058] Registration of the updated submap with the original submap. For each updated submap, use a point cloud registration algorithm to register it with the original submap. Commonly used point cloud registration algorithms include the ICP (Iterative Closest Point) algorithm and its variants. The basic steps of the ICP algorithm are as follows: For each point in the updated submap, find the nearest corresponding point in the original submap. Calculate the optimal transformation matrix between the updated submap and the original submap to minimize the distance between the corresponding points. Transform the updated submap according to the transformation matrix to align it with the original submap. Repeat the above steps until the transformation matrix converges or reaches the maximum number of iterations. Weights can be introduced during the registration process to assign different importance to different points, such as according to features such as the normal vector and curvature of the points. The registration result is the transformation matrix T between the updated submap and the original submap.

[0059] Merge the updated sub-image into the global point cloud map, and use the transformation matrix T to transform the updated sub-image into the coordinate system of the global point cloud map. For each point in the updated sub-image , calculate its coordinates in the global point cloud map according to the transformation matrix T : , merge the transformed sub-map point cloud data into the global point cloud map: If the global point cloud map already has point cloud data for the corresponding area, replace the original data with the updated sub-map point cloud data. If the global point cloud map does not have point cloud data for the corresponding area, directly add the updated sub-map point cloud data to the global point cloud map. Update the octree structure of the global point cloud map, and recalculate the point cloud density and change rate of each voxel. Complete the merging of all updated sub-maps to obtain the updated SLAM point cloud map. Sub-maps with high frequency of change are updated incrementally to quickly respond to environmental changes. At the same time, the updated sub-maps are registered with the original sub-maps through the point cloud registration algorithm to ensure the consistency between the local map and the global map. Finally, merge the updated sub-maps into the global point cloud map to obtain a complete, real-time updated SLAM point cloud map. This local map update strategy can improve the update efficiency and real-time performance of the SLAM system while maintaining the accuracy and consistency of the global map.

[0060] Global map update, according to the set update frequency, regularly merge the locally updated sub-maps into the global point cloud map. Use graph optimization algorithms (such as g2o) to optimize the global point cloud map to minimize the accumulated registration error. Output the optimized global point cloud map for unmanned vehicles to perform positioning, navigation, obstacle avoidance and other applications. This application can adaptively adjust the size and update frequency of the sub-map according to the frequency of environmental changes, while improving the efficiency of map updates and ensuring the accuracy and real-time performance of the map.

Claims

1. A laser SLAM mapping method in a dynamic environment of an underground mine, characterized in that: include: Collect IMU data, LiDAR point cloud data and camera data of unmanned vehicles in underground mining environments respectively; and perform spatial calibration and time synchronization processing on the collected data; Preprocess the collected LiDAR point cloud data; The pre-processed LiDAR point cloud data and the time-synchronized IMU data are used as input, and the Cartographer SLAM algorithm is used to build a map to obtain a PCD point cloud map of the underground mining environment. According to the preset movement or deformation frequency threshold of objects in each area of ​​the underground mining environment, the PCD point cloud map is segmented using an octree-based segmentation method, including: The collected camera data is subjected to image sequence extraction, and the motion of two consecutive frames of images is estimated using the optical flow method based on visual difference. The motion vector of each pixel is obtained by solving the optical flow constraint equation. According to the obtained motion vector, the motion amplitude and direction of each pixel are calculated, and the pixels with motion amplitude greater than the preset threshold are marked as changed pixels, and the changed area of ​​the image is obtained as the prior information of the dynamics of the environment; Set the maximum depth L and change threshold θ of the octree, take the bounding box of the PCD point cloud map as the root node, and recursively divide the nodes into octrees according to the prior information of the environment dynamics; According to the comparison result between the object movement or deformation frequency of the segmented area and the threshold, the map resolution of each area in the PCD point cloud map is adjusted; The change detection algorithm is used to detect changes in the areas where the object movement or deformation frequency exceeds the threshold in the segmented PCD point cloud map, and the new point cloud data of the detected change area is incrementally updated to the corresponding area of ​​the original PCD point cloud map to obtain a locally updated PCD point cloud map; The locally updated PCD point cloud map is registered and fused with the original PCD point cloud map, the original PCD point cloud map is updated, and an updated PCD point cloud map of the underground mining environment is obtained.

2. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 1, characterized in that: Perform spatial calibration and time synchronization on the collected data, including: Calibrate the spatial transformation relationship between the laser radar coordinate system and the body coordinate system of the unmanned vehicle; Calibrate the spatial transformation relationship between the lidar coordinate system and the IMU coordinate system; Calibrate the spatial transformation relationship between the body coordinate system and the camera coordinate system of the unmanned vehicle; According to the spatial transformation relationship, the spatial coordinate transformation matrix between the lidar coordinate system, IMU coordinate system, vehicle coordinate system and camera coordinate system is constructed; Use the PTP time synchronization strategy to synchronize the collected IMU data, lidar point cloud data, and camera data; In the ROS environment, the ROS time synchronization mechanism is used to align the timestamps of the time-synchronized IMU data, lidar point cloud data, and camera data; By utilizing the coordinate transformation mechanism of ROS, the IMU data, lidar point cloud data, and camera data with timestamp alignment are transformed according to the obtained spatial coordinate transformation matrix to obtain time-synchronized and spatially aligned data.

3. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 1, characterized in that: Preprocess the collected LiDAR point cloud data, including: Performing filtering processing on the laser radar point cloud data obtained by continuous scanning to obtain filtered laser radar point cloud data; Calculate the position change of each point in the filtered lidar point cloud data during continuous scanning to obtain the position change of each point; According to the position change of each point, the consistency score of each point within the preset time window is calculated; Based on the filtered LiDAR point cloud data, the position of each point in multiple observations is statistically analyzed, and the standard deviation of the corresponding point position is calculated; According to the consistency score C and standard deviation of each point , determine the dynamics of each point; among them, points with consistency scores lower than the threshold or standard deviations higher than the threshold are marked as dynamic points; The DBSCAN clustering algorithm is used to group the filtered lidar point cloud data to obtain multiple point cloud clusters; The centroid tracking algorithm is used to track the point cloud clusters in the continuous scanning, and the shape change of each point cloud cluster is calculated to obtain the centroid change and shape change of each point cloud cluster; Clustering point clouds whose centroid change is greater than a preset threshold or whose shape change is greater than a preset threshold is marked as dynamic clustering; The points marked as dynamic points and the point cloud clusters marked as dynamic clusters are eliminated to obtain the preprocessed lidar point cloud data.

4. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 3, characterized in that: Calculate the position change of each point in the filtered lidar point cloud data during continuous scanning to obtain the position change of each point , through the following formula: ； Among them, the positions in the t-th scan and the t+1-th scan are and .

5. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 3, characterized in that: Calculate the consistency score of each point within the preset time window based on the position change of each point , through the following formula: ； Where N is the number of scans in the time window; n is the number of times point p appears.

6. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 3, characterized in that: Perform statistical analysis on the position of each point in multiple observations and calculate the standard deviation of the corresponding point position , calculated by the following formula: ； in, Yes The average position among n occurrences; Represents the position of the i-th point.

7. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 3, characterized in that: The centroid tracking algorithm is used to track the point cloud clusters in continuous scanning, and the shape change of each point cloud cluster is calculated to obtain the centroid change of each point cloud cluster. and shape change , through the following formula: ； in, and are the cluster centroids at time t and t+1 respectively; ； in, and represents the differences in the shape descriptors of the clusters at consecutive time frames; when Greater than or Greater than When , the corresponding point is judged to be a dynamic point and marked as 1, otherwise marked as 0; in, represents the position change threshold, represents the shape change threshold.

8. The laser SLAM mapping method in a dynamic environment of an underground mine according to any one of claims 2 to 7, characterized in that: Cartographer SLAM algorithm is used to build the map and obtain the PCD point cloud map of the underground mining environment, including: The preprocessed LiDAR point cloud data and the time-synchronized IMU data are used as input, and the CartographerSLAM algorithm is used to estimate the motion trajectory of the LiDAR based on the IMU data. The LiDAR point cloud data obtained by continuous scanning is motion compensated according to the estimated motion trajectory to obtain the motion-compensated LiDAR point cloud data. Using the CartographerSLAM algorithm, through the scan matching method, it is determined whether there is a loop between the laser radar point cloud data obtained by the current scan and the historical laser radar point cloud data. If so, the pose transformation relationship of the loop is calculated; Using the CartographerSLAM algorithm, a pose graph is constructed based on the motion-compensated LiDAR point cloud data and the pose transformation relationship obtained by loop detection. The pose graph is optimized using a graph optimization algorithm to obtain the optimized LiDAR motion trajectory. The obtained motion-compensated LiDAR point cloud data are spliced ​​according to the optimized motion trajectory to obtain the PCD point cloud map of the well working environment.

9. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 1, characterized in that: The PCD point cloud map is segmented using an octree-based segmentation method, including: Based on the octree segmentation results, count the number of point clouds in each node, calculate the point cloud density of the node, and calculate the point cloud density change rate ρ of each node between two adjacent frames; According to the preset object movement or deformation frequency threshold, the nodes whose point cloud density change rate ρ is greater than the corresponding threshold are marked as change nodes, otherwise they are marked as stable nodes; For a changing node, if the corresponding depth does not reach the preset maximum depth L of the octree, continue to subdivide the subnodes and update the map resolution of the corresponding area to the resolution corresponding to the maximum depth L; for a stable node, stop subdividing the subnodes, merge the corresponding subnodes into the current node, and update the map resolution of the corresponding area to the resolution corresponding to the depth of the current node; According to the obtained segmentation results, the map resolution of each area in the PCD point cloud map is updated to obtain the PCD point cloud map of the underground mining environment.

10. The laser SLAM mapping method in a dynamic environment of an underground mine according to claim 9, characterized in that: Calculate the point cloud density of the node, and calculate the point cloud density change rate ρ of each node between two adjacent frames, using the following formula: ； in, and They respectively indicate that the change rate ρ of the target voxel in the current frame and the previous frame exceeds the set threshold θ.

Images