Mine multi-time period point cloud map association system based on graph optimization

Abstract

A multi-period map correlation system based on graph optimization comprises a single SLAM mapping module, a data integration module, a global search module, a point cloud fine matching module and a global graph optimization module. The application focuses on the changing mine environment, constructs a global map for the data collected in different periods, and completes the information correlation between the global maps, provides a basis for long-period map updating, thereby converts the pose estimation problem into a joint optimization problem, realizes accurate correlation of multi-pass mapping, and significantly improves the positioning accuracy and environmental adaptability of the SLAM technology in the mine scene.

Description

Technical Field

[0001] This invention relates to a technology in the field of autonomous driving, specifically a graph-optimized multi-time point cloud map association system for mines. Background Technology

[0002] Simultaneous mapping and localization (SLAM) technology for autonomous driving utilizes onboard sensors (such as cameras, LiDAR, IMU, and wheel speedometers) to estimate pose and build maps. The complex environment of mines, with its varied terrain, limited visibility, and unstable GNSS signals, leads to a decline in positioning accuracy and an increase in map errors in traditional SLAM over long-term operation. Furthermore, the changing mine environment requires SLAM systems to adapt to these changes and effectively correlate and update maps across multiple time periods. Existing SLAM map stitching technologies, when SLAM tracking is lost, require re-initializing the mapping, which can cause time delays and data loss, impacting system real-time performance and efficiency. Moreover, mapping initialization can only serve map building tasks within the same timeframe and cannot handle the registration of maps built at multiple different times. Summary of the Invention

[0003] To address the aforementioned shortcomings of existing technologies, this invention proposes a multi-period map association system based on graph optimization. By establishing constraint relationships between keyframes in a single map building session and establishing constraints with environmentally stable frames of the prior map in subsequent multi-period map building through descriptors, point cloud registration, and other methods, this system focuses on constructing a global map based on data collected at different time periods for the changing mining environment. It also completes information association between global maps, providing a foundation for long-term map updates. This transforms the pose estimation problem into a joint optimization problem, achieving accurate association across multiple map building sessions and significantly improving the positioning accuracy and environmental adaptability of SLAM technology in mining scenarios.

[0004] This invention is achieved through the following technical solution:

[0005] This invention relates to a graph optimization-based multi-time point cloud map association system for mines, comprising: a single-pass SLAM mapping module, a data integration module, a global search module, a point cloud fine matching module, and a global graph optimization module. Specifically: the single-pass SLAM mapping module, based on the original point cloud and IMU data from a single mapping pass, obtains the pose transformation relationship between adjacent point cloud frames in the single-pass mapping using a point cloud registration algorithm, and then filters to obtain a set of nodes, edges, and point cloud data for key frames; the data integration module uses a selected mapping pass as the main graph, and other sequences as graphs to be aligned, initializing the distance between the mapping start point of the graphs to be aligned and the mapping start point of the main graph. The pose transformation relationship is analyzed. The global search module compresses each frame of point cloud data from multiple passes into a two-dimensional matrix. By calculating the matrix similarity, it finds key frame pairs that may be related across packets and generates a factor map through fine point cloud registration. The fine point cloud matching module performs a fast nearest neighbor search for the poses of key frames belonging to the main graph based on the point cloud data and factor map from multiple passes. It traverses each key frame of the graph to be aligned, further judges the similarity through fine point cloud registration, and updates the factor map. The global graph optimization module constructs a graph optimization system based on the graph node data to be aligned and optimizes and adjusts the SLAM mapping data from multiple time periods.

[0006] The original point cloud is preferably obtained by stitching together multiple solid point clouds into the same coordinate system after calibration to ensure a full scan of the mountain structure.

[0007] The point cloud registration algorithm is as follows: it provides initial registration values ​​based on global pose over multiple time periods, and performs fine point cloud registration using methods such as Normal Distribution Transform (NDT).

[0008] The filtering process refers to selecting keyframes based on a distance threshold (typically 5m) and a time threshold (typically 0.5s), and then selecting node V. map and edge E map Save them separately into text files to prepare for map association across multiple time periods.

[0009] The main map mentioned above is based on a relatively complete map construction, and it is generally assumed that the attitude of the starting point of the main map relative to the global coordinate system is known, which can be obtained through GNSS latitude and longitude calculation.

[0010] The image to be aligned is used to align and update the main image, so it can reflect only the local situation of the mine, and the starting coordinates can be unknown.

[0011] The aforementioned scanning context method refers to extracting the maximum height value or reflection intensity within each local sector from the vehicle's perspective as a feature quantity.

[0012] The aforementioned optimization adjustment, based on cross-packet nodes and cross-packet constraints, iterates until a maximum number of iterations (I) is reached. maxOr the optimization error is less than the error threshold Error threshold until.

[0013] The aforementioned precise point cloud registration refers to: extracting features from the source and target point clouds using a feature matching algorithm to generate a preliminary pose estimate, including the rotation matrix R and the translation vector t. After precise point cloud registration through optimizing and updating these two parameters, an optimization objective is defined to minimize the distance between the source and target point clouds, expressed by the formula J = ∑ i ||Rp i +tq i || 2 To achieve this, where: p i and q i Let R be points in the source point cloud and the target point cloud, respectively, and let the rotation matrix R satisfy the constraints of orthogonality and determinant of 1.

[0014] The iterative optimization and point cloud fine registration are performed alternately, specifically: each iterative optimization generates a pair of E origin More accurate estimates, followed by fine-tuning of point clouds to generate more potential matches, and after multiple iterations, the association of point cloud maps across multiple time periods is completed, resulting in multiple mapping runs. Figure 1 Consistency is guaranteed, and various map update algorithms can be used to detect and add / delete changed areas. Technical effect

[0015] This invention sets the initial transformation relationship of multiple mapping passes as the variable to be optimized, and performs joint optimization with the intra-packet and cross-packet constraints of each mapping pass to achieve precise alignment of the mapping to the global coordinate system. Simultaneously, for coarse matching of point clouds, it proposes using descriptors to avoid brute-force search of the point clouds themselves, thus completing the initial construction of cross-packet constraints more efficiently. By extracting the pose and point cloud information of keyframes in a single mapping pass, potential correlations between cross-time period data are found through coarse descriptor matching and fine point cloud registration in multi-time period joint optimization. Joint optimization considering cross-time period pose constraints is then performed to fully utilize prior map information to optimize the pose error of subsequent mapping. Constraints are formed based on unchanged areas in the scene to increase the consistency of multi-time period maps, facilitating map association and updates. Compared to existing technologies, this invention achieves precise alignment of the mapping to the global coordinate system, reduces pose errors, and improves map consistency through methods such as optimizing the initial transformation relationship of multiple mapping passes and coarse descriptor matching. Meanwhile, by calculating global descriptors based on mountain point cloud features and constructing a bag-of-words model, scene recall can be quickly performed, improving the detection efficiency of potential loop closures and thus reducing computation time. Considering the joint optimization of pose constraints across time periods, the system fully utilizes prior map information to further optimize the pose error of subsequent mapping and enhance map consistency. Furthermore, by using fine matching of point clouds and loop closure pose estimation, precise pose adjustments are made after loop closure detection, thereby improving the global pose optimization effect. The constraint formation for unchanged areas also enhances the system's robustness in dynamic environments, reducing mismatches caused by environmental changes. Coarse matching using descriptors avoids brute-force searches of the point cloud itself, improving the time efficiency of initial cross-bag constraint construction and making the overall algorithm more efficient. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the system of the present invention;

[0017] Figure 2 This is a schematic diagram illustrating the map association effect at different time periods in the example.

[0018] Figure 3 This is a flowchart of an example implementation. Detailed Implementation

[0019] like Figure 1 As shown in this embodiment, a multi-time point cloud map association system for mines based on graph optimization is included, comprising: a single-pass SLAM mapping module, a data integration module, a global search module, a point cloud fine matching module, and a global graph optimization module. The single-pass SLAM mapping module uses the original point cloud and IMU data from a single mapping pass... map The pose transformation relationship between adjacent point cloud frames in a single map construction is obtained through a point cloud registration algorithm. After filtering, a set G of key frame nodes, edges, and point cloud data is obtained.map ={V map E map P map}, where: node V map The corresponding keyframe's six-DOF pose in the current mapping coordinate system, edge E map For the relative pose transformation relationship between two keyframes, P map This refers to the scan data corresponding to the keyframes; the data integration module integrates the node, edge, and point cloud data from multiple passes (G). n ={G map1 G map2 ,...,G mapn The intermediate data from multiple time-segment mappings are read in uniformly, and a selected mapping sequence is used as the master image, while the other sequences are used as images to be aligned. The pose transformation relationship E between the mapping start point of the image to be aligned and the mapping start point of the master image is then established. origin Use a larger initial value T origin Covariance (Cov) origin Initialize; the global search module performs point cloud data P from multiple passes. n ={P map1 P map2 , ..., P mapn The process involves compressing each frame's point cloud into a two-dimensional matrix using methods such as scanning the context. By calculating the matrix similarity, potentially related cross-packet keyframe pairs are identified. Further similarity is determined through fine-tuning of the point cloud data. For similarities exceeding a threshold (Score),... descriptor The point cloud pairs are used as the pose calculated by precise registration as the edge E. descriptor Added to the factor graph; the point cloud fine matching module uses point cloud data from multiple passes P n ={P map1 P map2 ,..,P mapn} and all nodes V in the current factor graph map For the pose of keyframes belonging to the main graph, a kd-tree is built for fast nearest neighbor lookup. Each keyframe in the graph to be aligned is traversed, and its distance to the nearest keyframe in the main graph is determined to be less than a certain threshold T. threshold At that time, the similarity is further determined by fine registration of point clouds. For similarities greater than the threshold Score, radius The point cloud pairs are used as the pose calculated by precise registration as the edge E. radius Added to the factor graph; the global graph optimization module constructs a graph optimization system G based on the graph node data to be aligned. match ={V match E match The SLAM mapping data from multiple time periods was optimized and adjusted, where: V match =V map1 ∪Vmap2 ∪…∪V mapn E match =E map1 ∪E map2 ∪…∪E mapn ∪E origin ∪E descriptor ∪E radius .

[0020] The single-pass SLAM mapping module includes a transformation matrix acquisition unit, a keyframe filtering unit, and a map generation unit. Specifically: the transformation matrix acquisition unit processes the original point cloud and IMU data from the single-pass mapping using a point cloud registration algorithm to obtain the matrix relationship of pose transformations between adjacent point cloud frames; the keyframe filtering unit filters key frames based on the pose transformation relationship, constructs a node set for the key frames, and establishes a set of edges based on the matrix transformation relationship between the node key frames, thus establishing the relationship between key frames; the map generation unit integrates the point cloud data based on the scan data corresponding to the key frames to generate a complete map for the single-pass mapping.

[0021] The data integration module includes: a graph sequence determination unit, a graph to be aligned processing unit, and an integration constraint unit. Specifically: the graph sequence determination unit reads intermediate data from multiple time-segment mappings based on node, edge, and point cloud data from multiple passes, determining the main graph and the graph to be aligned; the graph to be aligned processing unit processes other sequences as graphs to be aligned based on the selected main graph (generally a relatively complete mapping) and its known starting point pose, allowing the starting point coordinates to be unknown; the integration constraint unit initializes the pose transformation relationship based on the mapping starting point of the graph to be aligned and the mapping starting point of the main graph, setting larger initial values ​​and covariance to increase the adjustment range during optimization, adapt to other constraints, and ensure the effectiveness of data integration.

[0022] The global search module includes a feature extraction unit, a preliminary matching unit, and a pose calculation unit. Specifically: the feature extraction unit, based on multiple passes of point cloud data before and after the change, compresses each frame of point cloud into a two-dimensional matrix using methods such as scanning the context, and extracts the maximum height value within a local sector as a feature quantity. When considering terrain differences, criteria such as reflection intensity can be used instead. The preliminary matching unit finds potentially related cross-packet keyframe pairs by calculating matrix similarity and performs preliminary matching. The pose calculation unit performs fine point cloud registration on point cloud pairs with similarity greater than a threshold, and adds the calculated pose as an edge to the factor graph to enhance global consistency.

[0023] The point cloud fine matching module includes: a pose estimation unit, an optimization target definition unit, an orthogonality check unit, and an iterative optimization unit. Specifically: the pose estimation unit performs preliminary pose estimation based on the feature information of the source and target point clouds, obtaining initial values ​​for the rotation matrix R and the displacement vector t; the optimization target definition unit defines the optimization target based on the distance information between the source and target point clouds, obtaining the minimized objective function J = ∑i||Rp. i +tq i || 2 The orthogonality checking unit performs orthogonality checks based on the constraints of the rotation matrix to ensure the validity of the rotation matrix R; the iterative optimization unit performs iterative nearest point (ICP) processing based on the optimization algorithm to obtain the final rotation matrix R and displacement vector t, thereby achieving accurate registration of the point cloud.

[0024] The global graph optimization module includes: a prior factor construction unit, a pose node filtering unit, an edge constraint construction unit, and a cumulative error optimization unit, wherein: the prior factor construction unit is based on the pose of the first frame and the prior covariance ∑ prior Information, using prior factors Ψ prior The initial prior constraints are obtained through the construction of the pose node selection unit; the pose node selection unit selects radar frames L based on the timestamp. i Visual frame I with its nearest neighbor i Binding is performed, keyframes are selected as pose nodes, and a set of pose nodes is obtained; the edge constraint construction unit is based on the inter-frame pose T of the front end. i，i+1 and front-end observation covariance ∑ odom Information, constructing pose edge Ψ odom The constraints are used to obtain the relationship between poses; the cumulative error optimization unit is based on the loop closure frame pair set κ and the loop closure constraint covariance ∑ output by the loop closure detection. loop Information, constructing a loop edge Ψ loop The cumulative error is optimized to obtain the final pose graph optimization result.

[0025] Based on specific practical experiments, such as those conducted under weak GNSS conditions... Figure 3 The graph-optimized multi-period point cloud map association for the mine shown is formed by integrating pose maps obtained from multiple single SLAM mappings to create a central map. This map is then obtained through point cloud matching and factor map fusion, followed by period detection. The factor maps and the map are continuously updated subsequently.

[0026] like Figure 2As shown, the average absolute pose error of multi-pass mapping in different regions is reduced by 45.37% compared to single-pass mapping of the entire region, and the accuracy can be further improved by 29.05% through the map optimization function of this system. Under GNSS-free conditions, the average recall rate of the three experimental sequences with a total of 502 loops reached 73.29%, and the accuracy reached 90.04%. The loop trajectories of different sub-maps have a high degree of overlap.

[0027] Compared with existing technologies, this invention integrates mountain continuity factors and uses a two-dimensional descriptor based on the mountain's projected contour for loop closure detection. This enables effective filtering of loop closure frames and establishment of inter-regional constraint factors in open-pit mine scenarios, ensuring mountain continuity and achieving map data fusion under GNSS-free conditions. Furthermore, in the map update section, by deleting and replacing expired point clouds in the central area of ​​the map to be updated, and determining expired points based on the projection distance along vehicle lines, this method significantly improves map timeliness. In fusion experiments on map data from different time batches in the same scene, the average chamfer distance between maps using this invention was reduced to 3.33m, indicating good adaptability to the variable mining environment.

[0028] The above-described specific implementations can be partially adjusted by those skilled in the art in different ways without departing from the principles and purpose of the present invention. The scope of protection of the present invention is defined by the claims and is not limited to the above-described specific implementations. All implementation schemes within the scope of the claims are bound by the present invention.

Claims

1. A graph optimization-based multi-time-period point cloud map association system for mines, characterized in that, include: The system comprises a single-pass SLAM mapping module, a data integration module, a global search module, a point cloud fine matching module, and a global graph optimization module. Specifically: the single-pass SLAM mapping module uses the original point cloud and IMU data from a single mapping pass to obtain the pose transformation relationship between adjacent point cloud frames in the single-pass mapping through a point cloud registration algorithm, and then filters to obtain a set of nodes, edges, and point cloud data for key frames; the data integration module uses a selected mapping pass as the main graph and other sequences as graphs to be aligned, initializing the pose transformation relationship from the mapping start point of the graph to the mapping start point of the main graph; the global search module... Each frame of point cloud data in a pass is compressed into a two-dimensional matrix. By calculating the matrix similarity, potentially related cross-packet keyframe pairs are found, and a factor map is generated through fine point cloud registration. The fine point cloud matching module performs a fast nearest neighbor search on the poses of keyframes belonging to the main graph based on the point cloud data and factor map from multiple passes. It traverses every keyframe of the graph to be aligned, further judges the similarity through fine point cloud registration, and updates the factor map. The global graph optimization module constructs a graph optimization system based on the graph node data to be aligned and optimizes and adjusts the SLAM mapping data from multiple time periods. The original point cloud is obtained by stitching together multiple solid point clouds into the same coordinate system after calibration to ensure a full scan of the mountain structure; The aforementioned scanning context method refers to extracting the maximum height value or reflection intensity within each local sector from the vehicle's perspective as a feature quantity.

2. The multi-time period point cloud map association system for mines based on graph optimization according to claim 1, characterized in that, The point cloud registration algorithm is as follows: it provides initial registration values ​​based on global pose over multiple time periods, and performs fine point cloud registration using methods such as Normal Distribution Transform (NDT).

3. The multi-time period point cloud map association system for mines based on graph optimization according to claim 1, characterized in that, The aforementioned optimization adjustment, based on cross-packet nodes and cross-packet constraints, iterates until a maximum number of iterations (I) is reached. max Or the optimization error is less than the error threshold Error threshold until.

4. The multi-time period point cloud map association system for mines based on graph optimization according to claim 1, characterized in that, The aforementioned precise point cloud registration refers to: extracting features from the source and target point clouds using a feature matching algorithm to generate a preliminary pose estimate, including a rotation matrix R and a translation vector t; performing precise point cloud registration by optimizing and updating the pose estimate results; defining an optimization objective to minimize the distance between the source and target point clouds using the formula J=∑i||Rp i +tq i || 2 To achieve this, where: p i and q i Let R be points in the source point cloud and the target point cloud, respectively, and let R be the rotation matrix satisfying the constraints of orthogonality and determinant of 1.

5. The graph optimization-based multi-time point cloud map association system for mines according to claim 1, characterized in that, The iterative optimization and point cloud fine registration are performed alternately, specifically: each iterative optimization generates a pair of E origin More accurate estimation, followed by fine registration of point clouds to generate more potential matches, and after multiple repetitions, the association of point cloud maps at multiple time periods is completed. The consistency of maps built in multiple trips is guaranteed, and the detection and addition / deletion of changed areas can be achieved through various map update algorithms.

6. The graph optimization-based multi-time point cloud map association system for mines according to any one of claims 1-5, characterized in that, The single-pass SLAM mapping module includes a transformation matrix acquisition unit, a keyframe filtering unit, and a map generation unit. Specifically: the transformation matrix acquisition unit processes the original point cloud and IMU data from the single-pass mapping using a point cloud registration algorithm to obtain the matrix relationship of pose transformations between adjacent point cloud frames; the keyframe filtering unit filters key frames based on the pose transformation relationship, constructs a node set for the key frames, and establishes a set of edges based on the matrix transformation relationship between the node key frames, thus establishing the relationship between key frames; the map generation unit integrates the point cloud data based on the scan data corresponding to the key frames to generate a complete map for the single-pass mapping.

7. The graph optimization-based multi-time point cloud map association system for mines according to any one of claims 1-5, characterized in that, The data integration module includes: a graph sequence determination unit, a graph to be aligned processing unit, and an integration constraint unit. Specifically: the graph sequence determination unit reads intermediate data from multiple time-segment mappings based on node, edge, and point cloud data from multiple passes, determining the main graph and the graph to be aligned; the graph to be aligned processing unit processes other sequences as graphs to be aligned based on the selected main graph (generally a relatively complete mapping) and its known starting point pose, allowing the starting point coordinates to be unknown; the integration constraint unit initializes the pose transformation relationship based on the mapping starting point of the graph to be aligned and the mapping starting point of the main graph, setting larger initial values ​​and covariance to increase the adjustment range during optimization, adapt to other constraints, and ensure the effectiveness of data integration.

8. The graph optimization-based multi-time point cloud map association system for mines according to any one of claims 1-5, characterized in that, The global search module includes a feature extraction unit, a preliminary matching unit, and a pose calculation unit. Specifically: the feature extraction unit, based on multiple passes of point cloud data before and after the change, compresses each frame of point cloud into a two-dimensional matrix using methods such as scanning the context, and extracts the maximum height value within a local sector as a feature quantity. When considering terrain differences, criteria such as reflection intensity can be used instead. The preliminary matching unit finds potentially related cross-packet keyframe pairs by calculating matrix similarity and performs preliminary matching. The pose calculation unit performs fine point cloud registration on point cloud pairs with similarity greater than a threshold, and adds the calculated pose as an edge to the factor graph to enhance global consistency.

9. The graph optimization-based multi-time point cloud map association system for mines according to any one of claims 1-5, characterized in that, The point cloud fine matching module includes: a pose estimation unit, an optimization target definition unit, an orthogonality check unit, and an iterative optimization unit. Specifically: the pose estimation unit performs preliminary pose estimation based on the feature information of the source and target point clouds, obtaining initial values ​​for the rotation matrix R and the displacement vector t; the optimization target definition unit defines the optimization target based on the distance information between the source and target point clouds, obtaining the minimized objective function J = ∑i||Rp. i +tq i || 2 The orthogonality checking unit performs orthogonality checks based on the constraints of the rotation matrix to ensure the validity of the rotation matrix R; the iterative optimization unit performs iterative nearest-point processing based on the optimization algorithm to obtain the final rotation matrix R and displacement vector t, thereby achieving accurate registration of the point cloud.

10. The graph optimization-based multi-time point cloud map association system for mines according to any one of claims 1-5, characterized in that, The global graph optimization module includes: a prior factor construction unit, a pose node filtering unit, an edge constraint construction unit, and a cumulative error optimization unit, wherein: the prior factor construction unit is based on the pose and prior covariance Σ of the first frame. prior Information, using prior factors Ψ prior The initial prior constraints are obtained through the construction of the pose node selection unit; the pose node selection unit selects radar frames L based on the timestamp. i Visual frame I with its nearest neighbor i Binding is performed, keyframes are selected as pose nodes, and a set of pose nodes is obtained; the edge constraint construction unit is based on the inter-frame pose T of the front end. i,i+1 and front-end observation covariance Σ odom Information, constructing pose edge Ψ odom The constraints are used to obtain the relationship between poses; the cumulative error optimization unit is based on the loop closure frame pair set κ and loop closure constraint covariance Σ output by the loop closure detection. loop Information, constructing a loop edge Ψ loop The cumulative error is optimized to obtain the final pose graph optimization result.

Images