A heterogeneous sensor cooperative positioning and mapping method, device and medium

Abstract

This disclosure presents a method, device, and medium for heterogeneous sensor collaborative localization and map building. The method includes: generating a weighted fusion global descriptor for each platform; filtering cross-platform loop closure candidates through similarity calculation and maximizing algebraic connectivity; performing local point cloud registration on selected loops and employing a minimum vertex coverage algorithm to reduce communication overhead; dynamically electing and optimizing nodes; optimizing the decentralized pose graph using an asymptotic nonconvexity algorithm and fusing prior pose prediction with GNSS adaptive constraints; and unifying the global reference system through dynamic anchor point selection and iterative alignment. This disclosure achieves complementary advantages between backpack and UAV systems, significantly reducing communication and computational burdens while ensuring compliant operation in no-fly zones, and improving the accuracy and global consistency of collaborative localization and mapping in complex environments.

Description

Technical Field

[0001] Applications generally relate to the field of robotics technology, and in particular to a method, device and medium for heterogeneous sensor collaborative localization and mapping. Background Technology

[0002] Collaborative Simultaneous Localization and Mapping (C-SLAM) methods generally suffer from low computational efficiency, high communication load, and insufficient localization accuracy in large-scale complex environments. Traditional methods typically rely on a single platform's global descriptor for scene matching, which is prone to mismatches in environments with high structural similarity, and the processing efficiency of multi-robot reference systems is also low. Centralized C-SLAM architectures facilitate global management but are susceptible to single points of failure, and communication bottlenecks limit their scalability in large-scale robot swarms. While decentralized architectures do not require a central server, they still face challenges in data collaboration and efficiency optimization under resource constraints.

[0003] In applications such as mine surveying and modeling, these problems are even more pronounced. Mining areas typically exhibit the following characteristics: complex tunnel structures, complete lack of GNSS signals, and high visual and structural similarity between tunnels. Appearance-based loop closure detection methods are prone to mismatches, leading to positioning drift and map reconstruction distortion. Furthermore, the surface environment of mining areas may include open areas, buildings, and undulating terrain. Some areas may have no-fly zones due to proximity to airports or other facilities, making it difficult for a single data acquisition platform to cover the entire area.

[0004] For these complex scenarios, existing technologies have significant shortcomings. First, existing C-SLAM systems lack collaborative perception mechanisms for heterogeneous acquisition platforms. Backpack LiDAR systems and UAV LiDAR systems differ significantly in motion modes, scanning perspectives, point cloud density, and coverage, making it difficult to directly integrate the generated environment descriptors and leading to frequent failures in cross-platform data association. Second, when constructing cross-platform loopback connections, existing methods often rely on local geometric consistency, failing to consider the compatibility of topologies under different motion modes. Adding new connections may disrupt the stability of overall map optimization. Third, in mixed environments where GNSS is completely absent or intermittently available, existing solutions lack effective global consistency maintenance mechanisms, making it impossible to achieve seamless fusion and unified mapping of data acquired by backpacks and UAVs.

[0005] Therefore, it is necessary to develop a collaborative real-time localization and mapping method that is compatible with heterogeneous acquisition platforms, ensures structural stability, and adapts to positioning constraints in mixed environments, so as to support high-precision and high-efficiency collaborative operations of multi-robot systems in complex mining environments that include no-fly zones, tunnels, and open terrain. Summary of the Invention

[0006] This disclosure addresses some of the shortcomings mentioned in the background art by providing a method, device, and medium for heterogeneous sensor collaborative localization and map building.

[0007] In a first aspect, embodiments of this disclosure provide a method for heterogeneous sensor collaborative localization and map building, comprising the following steps: S1. The first robot system and the second robot system generate global descriptors for their respective keyframes. When the two systems are within communication range, the similarity between the global descriptors is calculated, and the loop to be verified is selected from the candidate loops whose similarity meets the threshold based on the principle of maximizing algebraic connectivity. S2. For the selected loops to be verified, the local point cloud data to be transmitted is determined by the minimum vertex coverage algorithm, and the local point cloud data is used to perform point cloud registration to calculate the relative pose between the first robot system and the second robot system. S3. Dynamically elect a robot system as an optimization node. The optimization node performs robust optimization of the pose graph based on the calculated relative pose and transmits the optimization results to the other robot systems through occasional communication. S4. When the robots first meet, the initial pose of the robot with the smallest identifier is selected as the anchor point of the global reference system. In subsequent encounters, the local reference systems of each robot are gradually unified to the global reference system through multiple iterative estimations.

[0008] In one embodiment of the first aspect, step S1, in which the first robot system and the second robot system respectively generate global descriptors for their respective keyframes, further includes, Keyframes are extracted from the point cloud data acquired by the first robot system and the second robot system respectively, and at least two types of descriptors are generated respectively. The at least two types of descriptors are weighted and fused to generate a fused global descriptor, wherein the weights for descriptor weighting and fusion are preset according to the sensor type or dynamically updated according to the matching success rate.

[0009] In one embodiment of the first aspect, step S1, which involves selecting candidate loops to be verified from those whose similarity satisfies a threshold based on the principle of maximizing algebraic connectivity, further includes: Construct a multi-robot pose graph that includes local platform pose graph edges and inter-platform loop closure candidate edges, wherein the weights of the inter-platform loop closure candidate edges are determined based on the similarity scores of the global descriptors; Based on the Laplacian matrix of the multi-robot pose graph, a preset number of loops to be verified are selected from the candidate closed-loop edges between the platforms with the goal of maximizing the algebraic connectivity of the pose graph.

[0010] In one embodiment of the first aspect, the method further includes introducing a structure preservation index as a joint optimization objective; the structure preservation index is used to characterize the structural consistency between the subgraphs containing the two key frame nodes to be connected; and the objective of the final edge selection is to maximize the weighted sum of algebraic connectivity and the structure preservation index.

[0011] In one embodiment of the first aspect, step S2, which involves determining the local point cloud data to be transmitted for the selected loop closures to be verified using a minimum vertex cover algorithm, further includes: When two or more loop candidate edges share the same keyframe vertex, the shared keyframe vertex is determined as the covering vertex. Only the local point cloud data corresponding to the covered vertex is transmitted, in order to calculate the relative pose measurements of all candidate edges associated with it.

[0012] In one embodiment of the first aspect, step S3, which involves the optimization node performing robust optimization of the pose graph based on the calculated relative pose, further includes... A progressive non-convex algorithm is used to robustly optimize the pose graph. The asymptotic nonconvex algorithm iteratively optimizes the algorithm by gradually increasing the nonconvexity parameters. During the iteration process, outliers are automatically identified and removed based on the residuals of the pose constraint edges.

[0013] In one embodiment of the first aspect, step S3, which involves the optimization node optimizing the pose graph based on the calculated relative pose, further includes a prior pose prediction and a global navigation satellite system adaptive fusion mechanism. The adaptive fusion mechanism performs pose prediction based on the robot system's historical trajectory and motion model, and adds the predicted pose or available global navigation satellite system observations as weighted constraint edges to the pose graph to participate in robust optimization.

[0014] In one embodiment of the first aspect, step S4 further includes, When the robot systems first meet, the initial pose of the robot system that was initialized in an environment without global navigation satellite signals is selected as the anchor point. In subsequent encounters, the pose of the robot system with the smallest cumulative error is selected as the anchor point.

[0015] In a second aspect, embodiments of this disclosure provide a heterogeneous sensor collaborative localization and mapping system for implementing the method described in the first aspect, comprising: Multiple heterogeneous robots, including at least one backpack lidar mapping system and one drone lidar mapping system; Each robot contains: The data acquisition module is used to acquire environmental point cloud data through the onboard LiDAR. The global matching module is used to generate a fused global descriptor and select cross-robot loop candidates based on an algebraic connectivity maximization strategy. The local matching module is used to perform accurate point cloud registration for loop closure candidates and to coordinate data exchange using the minimum vertex cover algorithm. The collaborative optimization module is used to participate in dynamic elections, execute or receive decentralized pose graph optimization results, and unify the global reference system.

[0016] In one embodiment of the second aspect, it further includes, When the robot system is within communication range, the collaborative optimization module dynamically elects a robot as a temporary optimization node according to preset election criteria. The election criteria include at least one or a combination of communication signal strength, communication delay, computing resource status, or robot identifier.

[0017] In a third aspect, embodiments of this disclosure provide an electronic device including a memory and a processor, wherein the memory stores a program that runs on the processor, and the processor executes the heterogeneous sensor cooperative localization and mapping method as described in the first aspect when running the program.

[0018] In a fourth aspect, embodiments of this disclosure provide a computer-readable storage medium having computer instructions stored thereon, which, when executed, perform a heterogeneous sensor collaborative localization and map building method as described in the first aspect.

[0019] This disclosure proposes a method, device, and medium for heterogeneous sensor collaborative localization and map building, which has the following beneficial effects: (1) By introducing a heterogeneous platform weight adaptive fusion mechanism, a unified environmental characterization method suitable for backpack and UAV lidar data was established, which solved the problem of inconsistency in descriptors caused by differences in platform motion mode, scanning angle and point cloud density, and realized effective association of cross-platform data.

[0020] (2) A loop candidate selection strategy based on maximizing algebraic connectivity is adopted and combined with the structure preservation index for joint optimization, which effectively suppresses topological distortion caused by environmental structure similarity and platform differences, and improves the accuracy of cross-platform loop detection and the stability of graph optimization.

[0021] (3) A decentralized pose graph optimization architecture is adopted. Optimized nodes are dynamically elected and robust optimization is performed using the progressive nonconvexity algorithm. The minimum vertex cover algorithm is combined to reduce cross-platform communication overhead. While ensuring system scalability and reliability, the communication load and computing resource consumption are significantly reduced.

[0022] (4) In response to the inconsistent GNSS signal conditions in the mining area, a priori pose prediction and GNSS adaptive fusion mechanism is introduced. The global pose consistency can be maintained in underground tunnels where GNSS is completely absent and in ground areas where GNSS is intermittently available. This achieves seamless fusion and unified mapping of data collected by backpacks and UAVs.

[0023] (5) The backpack and the drone lidar system complement each other. Under the premise of meeting the compliance requirements of no-fly zone operation, the efficient and accurate three-dimensional reconstruction and positioning of the entire mining area is completed, the global map closure error is reduced, and reliable technical support is provided for the digital and intelligent operation and maintenance of the mine. Attached Figure Description

[0024] Figure 1 A flowchart of a heterogeneous sensor collaborative localization and map building method; Figure 2 This is a schematic diagram of the weight adaptive fusion mechanism of the heterogeneous sensor collaborative localization and map building method in an embodiment of this disclosure. Detailed Implementation

[0025] The present application / disclosure will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application / disclosure and are not intended to limit the scope of the present application / disclosure. Furthermore, it should be noted that, for ease of description, only the parts relevant to the present application / disclosure are shown in the accompanying drawings, not the entire structure.

[0026] Example 1 This disclosure provides a heterogeneous sensor collaborative localization and map building method, applicable to complex environments such as mine tunnels where there are no GNSS signals or the signals are unstable. This method achieves high-precision collaborative localization and map building across platforms by fusing data from a backpack lidar system and an unmanned aerial vehicle (UAV) lidar system. Figure 1 To illustrate the flowchart of the heterogeneous sensor collaborative localization and map building method disclosed herein, the following will refer to... Figure 1 This paper provides a detailed description of the heterogeneous sensor collaborative localization and map building method disclosed herein.

[0027] In step 10, each robot generates a global descriptor containing its own motion information and environmental information. When communication is established between robots, candidate loops are identified by calculating the similarity of the global descriptors, and the target loop is selected from the candidate loops based on the algebraic connectivity maximization strategy.

[0028] In this embodiment, each robot (including the backpack system and the drone system) operates independently, using its onboard sensor data to generate a global descriptor of the environment. When the robots meet in space or enter the communication range, potential cross-platform spatial associations are identified by calculating the similarity of the descriptors. A strategy based on maximizing algebraic connectivity is used to filter out the most valuable loop candidates from numerous potential associations, thereby reducing subsequent computation and communication overhead.

[0029] In step 101, keyframes are extracted from the point cloud based on scene changes, a global descriptor is generated for each keyframe, and a unified fusion descriptor is obtained through adaptive weighted fusion.

[0030] In one embodiment, the backpack mapping system and the UAV mapping system acquire environmental point cloud data through their respective onboard LiDAR; key frames are extracted from the point cloud data at a frequency of 1 to 5 Hz; the key frames are extracted based on the degree of scene change, and new key frames are extracted when the translational change exceeds 0.5 meters or the rotational change exceeds 10 degrees.

[0031] In one embodiment, for each keyframe, a compact descriptor is extracted from the point cloud data to generate a global descriptor for both the LiDAR scan and the image data. The global descriptor for the LiDAR scan uses the ScanContext descriptor, specifically by dividing the point cloud into multiple fan-shaped regions (60-120 regions), calculating the height and density distributions of the point cloud within each fan-shaped region with a calculation accuracy of 0.1-0.5 meters, and generating a two-dimensional matrix as the scene descriptor. The global descriptor for the image data uses the CosPlace descriptor, specifically by extracting feature vectors from the image using a pre-trained convolutional neural network (ResNet or VGG network), with the feature vector dimensions set to 256-512, and generating the scene descriptor.

[0032] In one embodiment, a weighted fusion module is introduced to address the differences between the backpack system and the drone system in terms of motion patterns, scanning perspectives, and point cloud density. Assuming each robot extracts n descriptors, and n different types of descriptors D1, D2, ..., Dn can be extracted, then the fused global descriptor is defined as: Formula (1) Among them, D fused Represented as a weighted sum of the descriptors, α i This represents the adaptive weight corresponding to the i-th descriptor. Its weight can be statically set or dynamically learned based on information such as sensor type, historical matching success rate, or real-time matching residual. Static settings: set manually based on sensor type and historical accuracy characteristics; Dynamic learning: During operation, the system is dynamically updated based on feedback such as matching success rate and matching residuals.

[0033] For example, in a tunnel environment, the point cloud structure scanned by the backpack system is complete, and the weight of its descriptor can be increased accordingly; while in open ground areas, the UAV system has a wider field of view, and the weight of its descriptor can be appropriately increased. Through this adaptive fusion mechanism, the generated fused descriptor can more stably reflect the environmental characteristics of heterogeneous platforms in the same spatial area, laying the foundation for subsequent cross-platform loop closure detection.

[0034] In step 102, the cosine similarity between global descriptors of different robots is calculated, and candidate loop closures are filtered according to a preset threshold.

[0035] In this embodiment, when the backpack system and the drone system enter each other's communication range during operation, such as in areas with overlapping trajectories like tunnel exits, mine boundaries, or ventilation shafts, a cross-platform loop closure detection process is initiated. First, the robots exchange their keyframe indexes and corresponding global descriptor information via communication, and avoid duplicate transmission of known data through an accounting method. Then, the similarity between the global descriptors of different robots is calculated.

[0036] In one embodiment, descriptor matching is performed using nearest neighbors based on cosine similarity, as shown in the formula: Formula (2) In this model, A and B are the global descriptors for the backpack system and the drone system, respectively. The cosine similarity is calculated between -1 and 1, with values ​​closer to 1 indicating greater similarity between the two descriptors. Given that surface buildings and underground tunnels in a mining environment may have accidental similarities in local geometric features, this embodiment sets a similarity threshold of 0.7 to 0.9 based on the actual characteristics of the mining scene. Only when the cosine similarity of a descriptor pair is higher than this threshold is it considered a candidate loop closure. This threshold strikes a balance between the sensitivity and accuracy of loop closure detection, avoiding numerous false matches due to accidental similarities between surface buildings and tunnel structures. This allows heterogeneous platforms to reliably identify spatially overlapping areas across platforms, such as the entrance area scanned by the backpack system within a tunnel and the same ground area photographed by the drone system from the air, thus providing crucial clues for subsequent local geometric verification and map stitching.

[0037] In this embodiment, the heterogeneous platform can effectively identify spatially related areas in the mining environment, construct a globally consistent 3D map, and improve cross-platform collaborative positioning accuracy. For example, the point cloud of the tunnel exit scanned by the backpack system and the point cloud collected by the UAV system in the corresponding ground area can be reliably detected by calculating the similarity of the fused global descriptors, thereby achieving accurate stitching of underground and surface maps and optimization of pose maps. This not only improves the efficiency and consistency of map construction in complex mining environments but also significantly enhances the collaborative operation capabilities of the heterogeneous platform team under inconsistent GNSS conditions.

[0038] In step 103, by defining a multi-robot pose graph, fixed edges are set to be unweighted and candidate edges are assigned similarity scores as weights. After ensuring graph connectivity, the greedy solution is used as the initial value. The spectrum prioritization method is used to maximize the algebraic connectivity of the augmented pose graph under budget constraints. The structure preservation index is combined for weighted optimization to select the optimal loop closure candidate.

[0039] In this embodiment, after calculating the matching and obtaining candidate loop closures, it is necessary to prioritize the loop closures so as to select the most valuable loop closures for subsequent processing under limited computing and communication resources. Therefore, this embodiment first defines the graph structure of the multi-robot pose graph: Formula (3) Where V is the set of vertices in all system pose graphs, and each vertex represents a keyframe; E local For the edges within each platform, including laser odometry measurement and platform-internal closed loop; E global These are the edges of the global pose graph corresponding to the closed loop between platforms. Further, E... global Divided into two subsets: For a fixed set of measurement values ​​that has passed local geometric verification, This is the set of candidate platform closed-loop edges to be prioritized. The candidate priority mechanism of this disclosure does not rely on precise 3D measurements or prior pose estimation; therefore, both local edges and fixed global edges are represented as undirected, unweighted edges between two vertices, while candidate edges are appended with a weight value corresponding to the similarity score calculated in step 102. With this design, a simplified cross-platform pose graph can be directly constructed based on global matching information without additional communication, making it particularly suitable for environments with limited communication bandwidth, such as mining areas.

[0040] In one embodiment, the number of loop closure candidate edges B (i.e., the budget) to be selected at each time step is set by the user based on the communication and computing capabilities of the mining robot. A basic candidate priority ranking method is a greedy strategy, which directly selects the top B candidates with the highest similarity scores. This embodiment proposes a spectral priority method, treating pose graph sparsification as a candidate priority problem. By leveraging spectral sparsification theory, candidate matching is sparsified before calculating the corresponding 3D measurements, thereby reducing the computationally expensive inter-platform point cloud registration resources used for redundant candidates and achieving higher loop closure detection accuracy in complex environments such as mining areas.

[0041] In one embodiment, the algebraic connectivity of the pose graph plays a crucial controlling role in the worst-case error of the solution to the SLAM maximum likelihood estimation problem. This algebraic connectivity corresponds to the second smallest eigenvalue of the rotation-weighted Laplacian matrix. The elements of the Laplacian matrix L are defined as follows: Formula (4) Among them, k ij Represents the weight of the edge. Let L represent the set of edges associated with vertex i. When i=j, L ij L is equal to the sum of the weights of all edges associated with vertex i; when there is an edge between i and j, L ij The edge weight is equal to or equal to a negative value; otherwise, it is 0. Unlike traditional edge weight noise models, this embodiment uses the similarity score of global matching as the confidence level. That is, for all local edges and a fixed global edge, its weight k is equal to the weight k of the edge. ij The value is 1; for a candidate edge, its weight k is 1. ij The value is the similarity score. The Laplacian matrix is ​​calculated accordingly, eliminating the need for additional information on edge noise levels to be communicated between robots, thus simplifying communication requirements.

[0042] In one embodiment, leveraging the property that the Laplacian matrix can be represented as the sum of the Laplacian matrices of the subgraphs corresponding to each edge, the augmented pose graph Laplacian is defined as: Formula (5) in, It is a binary variable that determines the priority of candidate edge e. The goal of this embodiment is to select a subset under a fixed budget B. , This improves the algebraic connectivity of the augmented pose graph. maximize: Formula (6) because The integrity constraint is NP-hard, therefore this embodiment relaxes the integrity constraint and rounds the optimization result to the nearest solution in the feasible set when necessary. A gradient ascent method with low computational cost is used to solve this relaxation problem. It should be noted that this method requires the pose graph to be connected, so this embodiment first performs a greedy priority sorting until the local pose graphs of each robot are connected by at least one inter-platform closed loop, and uses the greedy solution as the initial guess for the algebraic connectivity maximization process.

[0043] In one embodiment, to further enhance the control over the impact of newly added edges on the overall graph structure and suppress topological ambiguity caused by differences in platform motion patterns and environmental perspectives, this embodiment defines a structure preservation index S. ij , representing the structural consistency between the subgraphs containing the two keyframe nodes i and j to be connected. This metric can be calculated using graph local tensors (e.g., degree distribution, clustering coefficient, adjacency entropy, etc.). Ultimately, the objective function for edge selection is extended to a joint optimization problem: Formula (7) Among them, S e The structure preservation score is represented by λ1 and λ2, which are adjustable weight coefficients. This joint optimization strategy effectively reduces the probability of selecting erroneous loop connections that may cause topological abrupt changes under sparse budget constraints, such as connecting structures that are geometrically similar from the air and ground perspectives but have different actual topologies. Thus, while maintaining communication efficiency, it preserves the consistency of the global graph structure and the stability of the optimization process.

[0044] In step 20, for the selected target loop, the three-dimensional relative pose between different robots is calculated by local feature matching, and the local point cloud data to be transmitted is determined by the minimum vertex coverage algorithm.

[0045] In this embodiment, after the candidate loop closure regions between platforms are filtered through global matching in step 10, a local matching step is then performed, which involves precise point cloud geometric verification of the candidate loop closures. In this step, a larger-scale local point cloud data acquired by the lidar is used, and a point cloud registration algorithm (such as ICP or its variant G-ICP) is employed to calculate the three-dimensional relative pose transformation matrix between the vertices of two candidate keyframes, thereby providing precise measurement constraints for subsequent pose graph optimization.

[0046] In one embodiment, to reduce cross-platform communication burden and avoid redundant calculations of the same loop, this embodiment models the problem of sharing vertex local point cloud data as a vertex coverage problem. Specifically, when two or more cross-platform loop candidate edges share the same common vertex (i.e., the same keyframe), only the complete point cloud data corresponding to the common vertex needs to be transmitted once to calculate the relative pose measurements of all associated candidate edges. By solving for the minimum vertex coverage, this embodiment implements an exchange strategy that avoids redundant data transmission in heterogeneous groups including backpack systems and UAV systems, thereby significantly saving cross-platform communication resources.

[0047] In this embodiment, both the initial spectral matching priority ranking and the vertex swapping strategy in this stage require the dynamic election of a temporary proxy among the systems within the communication range. This proxy is responsible for coordinating the computation of the matching task and sending requests to other systems to transmit specific vertex point cloud data. In a preferred embodiment of this disclosure, the proxy election mechanism comprehensively considers system ID, real-time computing load, and communication status, prioritizing platforms with sufficient computing resources and stable communication links to assume the proxy role, thereby ensuring the overall response speed and reliability of the system in complex mining area mapping tasks.

[0048] In step 30, a robot is dynamically elected as the optimization node. The optimization node uses a robust pose graph optimization algorithm to optimize the global pose graph, which includes internal constraints and inter-robot constraints, and transmits the optimization results to other robots through occasional communication.

[0049] In step 301, when the backpack system and the UAV system are within communication range, a platform is dynamically selected as an optimization node based on preset criteria to perform pose graph optimization tasks.

[0050] In one embodiment, the election criteria include: First, evaluating the communication signal strength and communication delay between platforms. A platform is included in the candidate pool only if its communication signal strength exceeds a set threshold and its communication delay is below a set threshold. The communication signal strength threshold is -70 dBm, and the communication delay threshold is 100 ms. Second, among the candidate platforms that meet the communication conditions, the platform's ID size, computing resource status, and communication stability are comprehensively compared. Platforms with smaller IDs, sufficient computing resources, and stable communication status are preferentially selected as optimized nodes. Each platform periodically broadcasts its ID, computing resources, and communication status information. The system determines the optimized node based on the comprehensive evaluation of the broadcast information and notifies all platforms of the election results via broadcast.

[0051] Step 302: After the election is completed, the selected optimization node platform executes the GNC (Graduated Non-Convexity) algorithm to optimize the pose graph.

[0052] In one embodiment, during the initial optimization phase, the non-convexity parameter μ is set to 1, and the optimization problem of the pose graph is initialized, including defining vertices representing the platform pose and edges representing pose constraints. Subsequently, the value of the non-convexity parameter μ is gradually increased, ranging from 1 to 10, and the pose graph optimization process is iteratively executed until a preset convergence condition is met. In each iteration, the error function of the current pose graph is calculated, and optimization algorithms such as gradient descent or Newton's method are used to update the vertices and edges of the pose graph based on the gradient information of the error function. Simultaneously, by gradually increasing the non-convexity parameter μ, the optimization algorithm automatically identifies and removes outliers in the pose graph. Specifically, the residual of each pose constraint edge is calculated, and edges with residuals exceeding a set threshold (ranging from 0.1 meters to 0.5 meters) are identified as outliers and removed from the pose graph to prevent erroneous constraints from affecting the optimization results.

[0053] Step 303: After optimization, the node platform adopts an intermittent communication strategy to transmit the optimization results to other platforms within the communication range via broadcast or multicast.

[0054] In one embodiment, the optimization result includes updated pose graph vertex and edge information, which is transmitted to other platforms within the communication range via broadcast or multicast. The transmission frequency of the optimization result is dynamically adjusted according to the communication bandwidth and the number of platforms: when the communication bandwidth is sufficient and the number of platforms is small, the transmission frequency is increased to 3-5 times per minute; when the communication bandwidth is limited or the number of platforms is large, the transmission frequency is decreased to 1-2 times per minute.

[0055] In one embodiment, to further enhance the system's continuous operation capability under long-distance operation and GNSS signal variation scenarios and suppress pose drift, this embodiment introduces a priori pose prediction and GNSS adaptive fusion mechanism. This mechanism predicts and compensates for pose based on the platform's historical trajectory and motion model (including uniform velocity model or Kalman filter), combined with the GNSS availability of each platform (e.g., a UAV can use GNSS in an open area, while a backpack cannot in a tunnel). The predicted results or available GNSS observations are added as constraint edges with corresponding weights to the current pose graph, and the constraint form is expressed as follows: Formula (8) in Let Q be the predicted GNSS position, and let Q be the uncertainty covariance matrix. During the optimization process, the GNC algorithm automatically adjusts the weights of the above constraints based on their uncertainties. This provides effective multi-source prior information in a hybrid GNSS environment without compromising the robustness of graph optimization, thereby improving the system's continuous operation capability and the stability of the graph structure.

[0056] In step 40, when the robots first meet, the initial pose of the robot with a preset benchmark is selected as the global reference frame anchor point. In subsequent meetings, the anchor point is dynamically updated according to the stability of the reference frame, and through multiple iterations, the local reference frames of all robots are aligned to the global reference frame corresponding to the current anchor point.

[0057] In this embodiment, the anchor point selection and update mechanism in the Cooperative Real-Time Localization and Mapping (C-SLAM) system is a key technology for realizing a global reference system, and is especially suitable for building a globally consistent map in a mixed mining environment with inconsistent GNSS signal conditions.

[0058] In one embodiment, when the backpack and the UAV system establish communication for the first time, the system automatically identifies and selects the initial pose of the platform that begins operation in a more stable environment as the initial anchor point; specifically, the backpack system that initializes within a tunnel is preferred. This selection strategy helps to prioritize establishing a reference benchmark less susceptible to external signal interference. During subsequent interactions between platforms, the system adjusts the anchor point selection criteria upon each encounter, selecting the pose of the platform with the most stable reference frame as the current anchor point; the most stable reference frame refers to a reference frame with the smallest accumulated error. This dynamic adjustment strategy ensures that the anchor point selection is always based on the most stable and widely accepted reference frame, thereby improving the stability and consistency of the global reference frame.

[0059] In one embodiment, during each iteration, all platforms align their local reference frames to the currently selected anchor point through precise pose transformation, ensuring that pose estimation for all platforms is performed within the same global reference frame. Through multiple iterative estimations, the system gradually unifies the global reference frame. The number of iterations is dynamically adjusted based on the scale of the collaborative platforms and the complexity of the environment, typically ranging from 3 to 10 iterations: when the number of platforms is small and the environmental characteristics are distinct, fewer iterations are sufficient to achieve global reference frame unification; when the number of platforms is large or the environment is highly similar, the number of iterations is increased to ensure that the pose estimations of all platforms are accurately aligned to the global reference frame. The convergence condition for the iteration process is that the pose estimations of all platforms are completely within the same global reference frame. Once this convergence condition is met, the iteration process ends, and the pose estimations of the heterogeneous platform teams will be used for subsequent collaborative localization and mapping tasks within this unified global reference frame.

[0060] In this embodiment, the iterative anchor point update and global reference system mechanism not only improve the system's flexibility and adaptability but also ensure that the system can maintain efficient and accurate operation under dynamically changing mining environments and platform configurations. Through this innovative anchor point selection and update method, the C-SLAM system disclosed herein can achieve efficient and accurate collaborative localization and map building in heterogeneous platform teams composed of backpacks and UAVs, adapting to the needs of various complex mining tasks.

[0061] Example 2 Figure 2 This is a schematic diagram of the weight adaptive fusion mechanism in an embodiment of this disclosure, as shown below. Figure 2 As shown, taking a large mining area and its surrounding environment as an example, this area includes underground tunnels, surface facilities, and a no-fly zone near a nearby airport. To achieve full-scene, high-precision 3D digital modeling of this area, a collaborative approach using backpack LiDAR and UAV LiDAR is adopted.

[0062] (1) The backpack mapping system and the UAV mapping system are started respectively, and each completes the initialization of the positioning and mapping system and collects point cloud data of the surrounding environment.

[0063] As an example, the backpack mapping system is equipped with a first 3D LiDAR. The operator carries the system from the entrance of the tunnel at a walking speed of 1 m / s. The first 3D LiDAR collects point cloud data of the underground tunnel and its surrounding environment at a frequency of 10 Hz. The drone mapping system is equipped with a second 3D LiDAR. It takes off from an open airspace where flight is permitted, flying along a preset route at a speed of 5 m / s and an altitude of 50 m. The second 3D LiDAR collects point cloud data of the ground and open areas at a frequency of 20 Hz. The area includes no-fly zones and nearby airports. The backpack mapping system is responsible for scanning the no-fly zones and underground tunnels, while the drone mapping system is responsible for aerial surveying of the open areas where flight is permitted.

[0064] (2) Generate a global descriptor and perform cross-platform loop closure detection.

[0065] As an example, both systems extract keyframes from their trajectories at a frequency of 1 Hz. For each keyframe: the knapsack system generates a scene descriptor D. backpack The unmanned aerial vehicle system generates a scene descriptor D. uav When the two types of systems overlap in areas such as tunnel exits and mine boundaries, a cross-platform loopback detection process is initiated.

[0066] In this embodiment, the cross-platform loop closure detection process can be specifically executed as follows: The heterogeneous platform weight adaptive fusion mechanism is invoked to assign a weight ω to the knapsack system descriptor. backpack = 0.6, assigning a weight ω to the UAV system descriptor.uav =0.4, generate fusion descriptor D fused Calculate the descriptor similarity between keyframe pairs. Based on the calculated similarity results, the system performs joint optimization of algebraic connectivity maximization and structure preservation evaluation, and selects keyframe pairs located at the alleyway exit that simultaneously meet the following conditions: descriptor similarity is higher than a set threshold, can enhance pose graph connectivity, and have consistent topology. These keyframe pairs are marked as high-priority loop closure candidates.

[0067] (3) Perform local point cloud registration on the selected high-priority loop closure candidates.

[0068] As an example, the minimum vertex cover algorithm is used to determine the point cloud data that needs to be exchanged across platforms. When multiple candidate loops share the same keyframe of the backpack system, the backpack system sends the complete point cloud of that keyframe to the UAV system only once to calculate all relevant relative pose constraints. The accurate inter-platform relative pose transformation matrix is ​​calculated using the G-ICP algorithm, and this matrix is ​​added to the global pose graph as a new constraint edge.

[0069] (4) Dynamically elect the UAV system with more computing resources as a temporary optimization node to perform decentralized pose graph optimization.

[0070] As an example, pose vertices and constraint edges are collected locally and from the backpack system. Constraint edges include odometry edges, internal loop closures, and newly generated cross-platform loop closures. The GNC algorithm is used to optimize the pose graph, automatically identifying and removing abnormal constraints. For environments with significantly different GNSS signal conditions, the system invokes a priori pose prediction compensation mechanism, incorporating pose predicted based on the motion model as a soft constraint into the optimization process. Specifically, the UAV system uses GPS observations as strong constraints in GPS-available areas, while the backpack system uses predicted poses as soft constraints in GPS-free areas.

[0071] (5) Through the distributed reference system mechanism, the initial pose of the backpack system is used as the global anchor point. After several iterations of optimization, the local reference system of the UAV system is accurately aligned to the global reference system.

[0072] As an example, after optimization, the UAV system sends the updated pose map to the backpack system; the two systems submit their respective point cloud data according to the optimized globally consistent pose, and finally merge them in the data processing center to generate a complete and seamless 3D reality model of the entire mining area.

[0073] This disclosure provides a method, device, and medium for heterogeneous sensor collaborative localization and mapping. A backpack mapping system and a drone mapping system successfully achieved collaborative localization and mapping in complex mining areas, including no-fly zones, underground tunnels, and open areas. Compared to the traditional method of manually stitching together segmented mapping data, this disclosure effectively reduces the closure error of the global map through a cross-platform loop closure detection mechanism. Simultaneously, it significantly reduces the amount of data exchanged for collaborative operations by leveraging an efficient cross-platform communication strategy. This solution not only meets the compliant operation requirements within no-fly zones but also achieves efficient and accurate mapping across the entire mining area, fully demonstrating the advantages of this disclosure for cross-platform collaborative operations in complex environments.

[0074] In a second aspect, embodiments of this disclosure provide an electronic device including a memory and a processor, wherein the memory stores a program that runs on the processor, and the processor executes the steps of the heterogeneous sensor cooperative localization and mapping method as described in the first aspect when running the program.

[0075] In a third aspect, embodiments of this disclosure provide a computer-readable storage medium having computer instructions stored thereon, which, when executed, perform the steps of the heterogeneous sensor cooperative localization and map building method as described in the first aspect.

Claims

1. A method for collaborative localization and map building using heterogeneous sensors, characterized in that, Includes the following steps: S1. The first robot system and the second robot system generate global descriptors for their respective keyframes. When the two systems are within communication range, the similarity between the global descriptors is calculated, and the loop to be verified is selected from the candidate loops whose similarity meets the threshold based on the principle of maximizing algebraic connectivity. S2. For the selected loops to be verified, the local point cloud data to be transmitted is determined by the minimum vertex coverage algorithm, and the local point cloud data is used to perform point cloud registration to calculate the relative pose between the first robot system and the second robot system. S3. Dynamically elect a robot system as an optimization node. The optimization node performs robust optimization of the pose graph based on the calculated relative pose and transmits the optimization results to the other robot systems through occasional communication. S4. When the robots first meet, the initial pose of the robot with the smallest identifier is selected as the anchor point of the global reference system. In subsequent encounters, the local reference systems of each robot are gradually unified to the global reference system through multiple iterative estimations.

2. The heterogeneous sensor collaborative localization and map building method according to claim 1, characterized in that, Step S1, where the first robot system and the second robot system respectively generate global descriptors for their respective keyframes, further includes, Keyframes are extracted from the point cloud data acquired by the first robot system and the second robot system respectively, and at least two types of descriptors are generated respectively. The at least two types of descriptors are weighted and fused to generate a fused global descriptor, wherein the weights for descriptor weighting and fusion are preset according to the sensor type or dynamically updated according to the matching success rate.

3. The heterogeneous sensor collaborative localization and map building method according to claim 1, characterized in that, Step S1, which selects the loops to be verified from candidate loops whose similarity meets a threshold based on the principle of maximizing algebraic connectivity, further includes: Construct a multi-robot pose graph that includes local platform pose graph edges and inter-platform loop closure candidate edges, wherein the weights of the inter-platform loop closure candidate edges are determined based on the similarity scores of the global descriptors; Based on the Laplacian matrix of the multi-robot pose graph, a preset number of loops to be verified are selected from the candidate closed-loop edges between the platforms with the goal of maximizing the algebraic connectivity of the pose graph.

4. The heterogeneous sensor collaborative localization and map building method according to claim 3, characterized in that, It also includes introducing a structure preservation index as a joint optimization objective; the structure preservation index is used to characterize the structural consistency between the subgraphs containing the two key frame nodes to be connected; the final edge selection objective is to maximize the weighted sum of algebraic connectivity and the structure preservation index.

5. The heterogeneous sensor collaborative localization and map building method according to claim 1, characterized in that, Step S2, which involves determining the local point cloud data to be transmitted for the selected loop closures to be verified using the minimum vertex cover algorithm, further includes: When two or more loop candidate edges share the same keyframe vertex, the shared keyframe vertex is determined as the covering vertex. Only the local point cloud data corresponding to the covered vertex is transmitted, in order to calculate the relative pose measurements of all candidate edges associated with it.

6. The heterogeneous sensor collaborative localization and map building method according to claim 1, characterized in that, Step S3, which involves the optimization node performing robust optimization of the pose graph based on the calculated relative pose, further includes... A progressive non-convex algorithm is used to robustly optimize the pose graph. The asymptotic nonconvex algorithm iteratively optimizes the algorithm by gradually increasing the nonconvexity parameters. During the iteration process, outliers are automatically identified and removed based on the residuals of the pose constraint edges.

7. The heterogeneous sensor collaborative localization and map building method according to claim 1 or 6, characterized in that, Step S3, which involves the optimization node optimizing the pose graph based on the calculated relative pose, also includes a priori pose prediction and adaptive fusion mechanism with the Global Navigation Satellite System (GNSS). The adaptive fusion mechanism predicts the pose based on the robot system's historical trajectory and motion model, and adds the predicted pose or available GNSS observations as weighted constraint edges to the pose graph to participate in robust optimization.

8. The heterogeneous sensor collaborative localization and map building method according to claim 1, characterized in that, Step S4 further includes, When the robot systems first meet, the initial pose of the robot system that was initialized in an environment without global navigation satellite signals is selected as the anchor point. In subsequent encounters, the pose of the robot system with the smallest cumulative error is selected as the anchor point.

9. A heterogeneous sensor collaborative localization and mapping system, used to implement the method according to any one of claims 1 to 8, characterized in that, include: Multiple heterogeneous robots, including at least one backpack lidar mapping system and one drone lidar mapping system; Each robot contains: The data acquisition module is used to acquire environmental point cloud data through the onboard LiDAR. The global matching module is used to generate a fused global descriptor and select cross-robot loop candidates based on an algebraic connectivity maximization strategy. The local matching module is used to perform accurate point cloud registration for loop closure candidates and to coordinate data exchange using the minimum vertex cover algorithm. The collaborative optimization module is used to participate in dynamic elections, execute or receive decentralized pose graph optimization results, and unify the global reference system.

10. The heterogeneous sensor collaborative localization and mapping system according to claim 9, characterized in that, Further including, When the robot system is within communication range, the collaborative optimization module dynamically elects a robot as a temporary optimization node according to preset election criteria. The election criteria include at least one or a combination of communication signal strength, communication delay, computing resource status, or robot identifier.

Images