A multi-robot lightweight distributed cooperative SLAM method based on fire rescue scene command battle net

Abstract

The present application relates to the field of fire emergency rescue technology, in particular to a multi-robot lightweight distributed collaborative SLAM method based on a fire rescue scene command battle net, comprising S1, constructing the inertial measurement unit data and the laser radar data into a tightly coupled residual function; S2, constructing a subgraph according to local state estimation, compressing point cloud data using a multi-resolution voxel grid, and constructing an information matrix; S3, exchanging subgraph information when robots meet, and using a consensus optimization algorithm to jointly optimize their respective point cloud graphs; S4, probabilistically fusing and updating the global point cloud graph from the subgraph, pose and point cloud graph states; S5, adjusting the subgraph data volume in real time when robots meet, and distributing the robot computing load. For the complex building fire rescue environment with limited communication, the multi-robot efficient collaborative mapping is realized, which can significantly reduce the communication bandwidth demand and simultaneously improve the consistency and positioning accuracy of the point cloud graph in the dynamic fire environment.

Description

Technical Field

[0001] This invention relates to the field of fire emergency rescue technology, and in particular to a multi-robot lightweight distributed collaborative SLAM method based on a fire rescue on-site command and control network. Background Technology

[0002] With the continuous expansion of urbanization, my country now boasts the world's largest scale of high-rise and super high-rise buildings, underground spaces, and urban complexes. These complex urban building spaces present highly complex, dynamic, and hazardous characteristics for fire fighting and rescue operations. Large complex buildings, underground spaces, and chemical industrial parks, among other complex building scenarios, often face extreme conditions such as extremely low visibility, high temperatures, structural instability, and limited communication, posing significant challenges to fire and rescue teams. Firefighting and rescue operations in such locations place multiple stringent requirements on perception and navigation systems: First, accurate positioning is required in indoor and underground environments without relying on GNSS signals; second, robustness in spatial environment modeling is essential under harsh perception conditions such as dense smoke, high temperatures, and darkness; third, the ability to maintain collaborative operations among multiple units must be maintained even if indoor communication infrastructure is damaged; and finally, the system must possess rapid deployment and response capabilities to meet the urgent requirement of the "golden rescue time" in fire fighting and rescue operations.

[0003] Simultaneous Localization and Mapping (SLAM) is a core technology for autonomous robot navigation. SLAM solutions based on laser, vision, and multi-sensor fusion have been widely and successfully applied in industrial inspection robots, service robots, and other fields. In urban complex building fire rescue operations, some fire rescue teams have also applied it for early environmental reconnaissance and simultaneous indoor space digital modeling after a fire. However, traditional single-robot SLAM solutions have revealed many fundamental shortcomings in fire emergency rescue scenarios. First, the perception range of a single SLAM robot is limited, and completing a comprehensive exploration in large and complex disaster scenarios takes too long, failing to meet the time requirements of fire emergency response. Second, in complex building environments, a single robot may fail due to structural collapse, equipment failure, or communication interruption, leading to complete mission failure. This vulnerability sharply contradicts the high reliability requirements of fire emergency rescue missions. Moreover, in structurally complex fire scenarios, features such as corridors, partitions, and multi-layered structures easily lead to perception degradation. The error rate of loop closure detection by a single sensor in similar scenarios can reach 25%-40%, seriously affecting the point cloud. Figure 1Simultaneously, high-precision laser SLAM requires a large amount of computing resources, while firefighting robots typically need to carry payloads for tasks such as fire extinguishing and demolition. Due to the limitations of the robot's own load capacity and power supply, it is difficult to configure high-performance computing units, making it difficult to reconcile the contradiction between real-time performance and accuracy.

[0004] To overcome the technical limitations of single-robot SLAM, research institutions both domestically and internationally have proposed the concept of multi-robot collaborative SLAM, or C-SLAM. However, system SLAM solutions also have significant shortcomings in complex urban building fire fighting and rescue scenarios. First, collaborative solutions based on a central server, such as Cloud-based SLAM, require stable communication links and powerful central processing nodes. In fire emergency rescue scenarios, communication facilities are temporarily deployed based on emergency mobile networking, i.e., building a field command and control network based on mesh self-organizing network technology. The stability and reliability of the field command and control network are relatively poor compared to fixed networks, so the central node is prone to becoming a source of system failure. Second, existing SLAM solutions are mostly designed for static or slowly changing environments, while fire emergency rescue scenarios involve multiple dynamic factors such as flame spread, structural collapse, and personnel movement. The localization drift error of traditional static SLAM algorithms under dynamic disturbances can be 3-5 times that of static environments. Third, fixed-role collaborative solutions lack dynamic task allocation mechanisms, which may lead to some robots being overloaded while others are idle, resulting in resource waste in time-sensitive emergency tasks. Due to the aforementioned shortcomings, existing multi-robot collaborative SLAM methods are insufficient to meet the needs of fire emergency rescue scenarios. Summary of the Invention

[0005] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides a lightweight distributed collaborative SLAM method for multiple robots based on the fire rescue on-site command and combat network. For complex building fire fighting and rescue environments with limited communication, it realizes efficient collaborative mapping of multiple robots, and can simultaneously improve the consistency and positioning accuracy of point cloud maps in dynamic fire scene environments while significantly reducing communication bandwidth requirements.

[0006] A lightweight, distributed, collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network includes the following steps: S1. Lightweight front-end odometry: Each robot collects inertial measurement unit data and lidar data, pre-integrates the inertial measurement unit data, registers the lidar data point cloud, and constructs the two into a tightly coupled residual function to obtain the local state estimate and key frame data at the current moment. The key frame data includes pose and point cloud data. By tightly coupling inertial measurement unit data and lidar data, stable and efficient state estimation is achieved on the embedded platform, meeting the computing power limitations of the robot carrying firefighting mission payloads.

[0007] S2. Adaptive Subgraph Management: Each robot constructs a subgraph based on local state estimation, inputs point cloud data into the subgraph, compresses the point cloud data using multi-resolution voxel mesh, and constructs the subgraph's information matrix together with the pose. The information matrix is ​​used to represent the constraint strength of the subgraph. By adaptively partitioning the graph based on local state estimation, the large-scale graph construction problem can be decomposed into multiple local, independent, and easily manageable subtasks.

[0008] Through a lightweight design that incorporates multi-resolution voxel compression and information matrix extraction, the amount of data requiring online transmission and processing is significantly reduced at the source, directly solving the communication bottleneck in bandwidth-constrained environments.

[0009] S3. Opportunistic Collaborative Optimization: When robots meet, they exchange subgraph information and use a consensus optimization algorithm to jointly optimize their respective point cloud graphs to obtain optimized poses and globally consistent point cloud graph states. By employing opportunistic collaborative design, the system can exchange critical information through brief encounters without relying on stable communication, significantly improving its robustness in unreliable networks.

[0010] Through a distributed consensus optimization algorithm, the risk of single point of failure of the central node is eliminated, and it has natural resilience and scalability. The reliability and exploration capability of the system are enhanced with each additional robot.

[0011] S4. Incremental point cloud fusion: The sub-map from step S2 is probabilistically fused with the optimized pose and globally consistent point cloud state from step S4 to update the global point cloud map.

[0012] By adopting a probabilistic fusion framework, the local point cloud maps of each robot are fused as progressive evidence. The new evidence continuously updates the confidence distribution of the global point cloud map in an overlayable and non-overlapping manner, effectively handling multi-source information conflicts, perceptual noise and dynamic interference, and achieving smooth and robust convergence of the point cloud map.

[0013] S5. Resource scheduling based on communication awareness: When robots meet, the network quality and computing power of both robots are obtained in real time. The amount of data exchanged between the two robots in step S3 is adjusted according to the network quality. The computing load of the two robots is allocated according to their computing power, and the processing task assignment is adjusted. The processing task includes point cloud data compression in step S2 and point cloud map fusion in step S4.

[0014] By introducing cross-layer optimization concepts to design communication-aware resource scheduling, the collaborative strategy can dynamically perceive and adapt to changes in the underlying network state based on real-time network quality. This adaptive scheduling mechanism achieves the optimal match between communication resources and collaborative needs, ensuring that critical collaborative functions can be maintained even when network conditions deteriorate, thereby maintaining optimal overall task performance in a variable environment.

[0015] Furthermore, the Huber kernel function is introduced into the residual function of step S1.

[0016] By introducing the Huber kernel function, the system becomes robust to dynamic disturbances such as smoke and water droplets in the fire scene, enabling accurate positioning under harsh sensing conditions.

[0017] Furthermore, in step S2, the condition for robot i to construct the subgraph at time k is: or Time generation ; in: The m-th subgraph created by robot i; : The position vector of robot i at time k; : The rotation quaternion of robot i at time k; Subgraph The initial position of robot i; Subgraph The initial pose of robot i; Maximum translation threshold; Maximum rotation threshold; : and The angle between them.

[0018] The construction of the subgraph depends on whether the cumulative change in the robot's translational distance or rotational angle exceeds a preset threshold. This design differs from subgraph division based on fixed time or fixed distance. The preset threshold is based on the cumulative change in motion and can adapt to scenarios where the robot's movement speed is uneven. In complex buildings, robots may frequently turn and travel long distances in straight lines; this design can generate subgraphs of appropriate size and rich information.

[0019] Furthermore, in step S2, the point cloud data is compressed using a multi-resolution voxel grid as shown below: ; in: : Voxel indexing function, take a 3D point coordinate x as input, and output the index of the voxel to which the point belongs at the l-th resolution; : Coordinates of the origin of the subgraph; : The voxel side length of the l-layer voxel point cloud map; when l is 0, it is the base resolution. : Resolution level index.

[0020] By using multi-resolution voxel meshes to compress point cloud data, the amount of data that needs to be transmitted is significantly reduced while ensuring key structural information.

[0021] Furthermore, the information matrix in step S2 is as follows: ; in: Subgraph The information matrix, which is the inverse of the covariance matrix, is used to represent the constraint strength of the subgraph; Λp: Pose information matrix, The uncertainty corresponds to the six degrees of freedom of the initial pose of the subgraph, including three-way translation and three-way rotation; Λf: Feature point information matrix, K is the number of feature points extracted from the subgraph, and the feature point information matrix is ​​used to represent the strength of the geometric constraints between feature points.

[0022] Data lightweighting was achieved by designing an information matrix. The information matrix does not contain the original point cloud data, but records the structural strength and confidence of the subgraph. In subsequent collaborative optimization, only the information matrix is ​​exchanged, and the compressed point cloud is not exchanged, which can greatly save communication bandwidth.

[0023] Furthermore, in step S3, the robot continuously monitors the communication status with other robots, calculates the encounter probability based on the communication status, and determines an encounter when the encounter probability exceeds the confidence threshold, triggering subgraph information exchange.

[0024] By continuously monitoring the waiting time for encounters, it achieves independence from fixed topology or central scheduling, adapting to the normal situation of unstable communication and unpredictable robot movement paths in fire scenes. It utilizes brief encounter opportunities to exchange key information, significantly improving robustness in unreliable networks.

[0025] Furthermore, in step S3, a consensus optimization algorithm is adopted. Specifically, after each optimization iteration, the robot exchanges state information with neighboring robots within the communication range. The next state of each robot is affected by the consistency constraint of the neighboring robot's state, and the point cloud state of all robots gradually converges to global consistency.

[0026] By adopting a consensus optimization algorithm, the risk of single point of failure of the central node is eliminated, and the system has natural resilience and scalability. The reliability and exploration capability of the system will be enhanced with each additional robot.

[0027] Furthermore, the probabilistic fusion in step S4 includes evidence theory fusion, multi-resolution weight allocation, and dynamic environment update.

[0028] Evidence theory fusion enables reasonable processing of information from different robots, even conflicting information, avoiding simple "overlay" or "average" operations, and ensuring smooth and robust convergence of point cloud maps.

[0029] Multi-resolution weight allocation reduces computational and storage overhead while maintaining accuracy.

[0030] Dynamic environment updates can adapt to highly dynamic environments such as fire scenes, distinguishing dynamic obstacles such as fire sources and moving personnel from building structures, thus improving the practicality and safety of point cloud maps.

[0031] Furthermore, in step S5, when the network quality is lower than the communication quality threshold, the amount of data exchanged in the subgraph is reduced, and at the lowest level, only the information matrix of the subgraph is exchanged, without exchanging the compressed point cloud data.

[0032] By assessing network status in real time and dynamically adjusting data exchange strategies, the system can maintain basic collaborative functions and ensure availability even in the worst network environments, by discarding large amounts of point cloud data and exchanging only information matrices.

[0033] The beneficial effects of this invention are: This invention proposes a lightweight distributed collaborative SLAM method for multiple robots based on a fire and rescue on-site command and control network. It employs a multi-resolution voxel grid to compress point cloud data, significantly reducing the amount of data to be transmitted while preserving key structural information. By dynamically adjusting the grid resolution according to environmental characteristics, intelligent optimization of communication load is achieved, solving the bottleneck of real-time sharing of massive point cloud data in bandwidth-constrained environments and laying an efficient data foundation for subsequent collaborative processes.

[0034] This invention establishes an opportunistic collaborative optimization mechanism, abandoning the dependence on stable communication links, and designs an on-demand collaborative strategy based on robot encounter events. Robots only automatically initiate point cloud map information exchange and joint optimization when they detect each other entering the communication range or relevant area. This design allows the system to treat communication interruptions as normal, perfectly adapting to extreme environments with missing or highly unstable communication infrastructure in complex building fire fighting and rescue scenarios, significantly improving the system's environmental adaptability and overall robustness.

[0035] This invention achieves global point cloud without a central node by employing a fully distributed consensus algorithm. Figure 1 Consistent maintenance. Each robot can iteratively converge its point cloud estimation to a globally consistent state through local communication and computation with neighboring nodes. This not only eliminates the risk of single point failure at the central node but also enhances the system's scalability and resilience, meeting the core requirements for system reliability and autonomy in emergency scenarios at fire and rescue sites.

[0036] This invention introduces cross-layer optimization concepts and designs communication-aware resource scheduling, enabling collaborative strategies to dynamically perceive and adapt to changes in the underlying network state based on real-time network quality. It can dynamically adjust data compression rate, transmission content priority, and collaborative frequency according to real-time communication quality, achieving optimal matching between communication resources and collaborative needs. This ensures that critical collaborative functions are maintained even when network conditions deteriorate, thereby maintaining optimal overall task performance in variable environments.

[0037] This invention employs a probabilistic fusion framework to fuse the local point cloud maps of each robot as progressive evidence. New evidence can be superimposed and updated continuously in a non-overlapping manner to continuously update the confidence distribution of the global point cloud map. This effectively handles multi-source information conflicts, perceptual noise, and dynamic interference, achieving smooth and robust convergence of the point cloud map. Attached Figure Description

[0038] Figure 1 This is a flowchart of a multi-robot lightweight distributed collaborative SLAM method based on a fire rescue on-site command and control network according to the present invention; Figure 2 This is a system scheduling diagram of a multi-robot lightweight distributed collaborative SLAM method based on a fire rescue on-site command and control network according to the present invention. Figure 3 This is a system architecture diagram of a multi-robot lightweight distributed collaborative SLAM method based on a fire rescue on-site command and control network according to the present invention. Detailed Implementation

[0039] To better explain and facilitate understanding of the present invention, it will be described in detail below with reference to the accompanying drawings and specific embodiments. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a clearer and more thorough understanding of the invention and to fully convey the scope of the invention to those skilled in the art.

[0040] like Figure 1-3As shown, a lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network includes the following steps: S1. Lightweight front-end odometry: Each robot collects inertial measurement unit (IMU) data and lidar data, pre-integrates the IMU data, registers the lidar data point cloud, and constructs the two into a tightly coupled residual function to obtain the local state estimate and key frame data at the current moment. The key frame data includes pose and point cloud data.

[0041] Step S1 specifically includes the following steps.

[0042] S11. Input Data Acquisition and Preprocessing: Real-time acquisition of inertial measurement unit (IMU) data for each robot, including angular velocity and acceleration, as well as lidar data, including point cloud data. Noise reduction and zero-bias initialization are performed on the IMU data, and motion distortion correction is applied to the point cloud data from the lidar data.

[0043] S12. Multi-sensor tightly coupled state estimation: Construct and optimize a tightly coupled residual function, and jointly optimize the IMU data pre-integration model and the lidar point cloud registration model; S121, IMU pre-integration: During robot motion, IMU data is pre-integrated across consecutive key frames, such as from time i to j, to calculate the relative rotation ΔR. ij Relative velocity Δv ij and relative displacement Δp ij Construct IMU pre-integration between adjacent keyframes; ; ; ; in: ΔR ij The relative rotation change obtained by integrating IMU data between keyframe i and keyframe j; Δv ij The relative velocity change between keyframe i and keyframe j, obtained by integrating IMU data in the coordinate system of keyframe i; Δp ij The relative position change between keyframe i and keyframe j, obtained by integrating IMU data in the coordinate system of keyframe i; : The three-dimensional angular velocity and three-dimensional linear acceleration measured at the raw sampling time k of the IMU data; Δt: The sampling time interval of IMU data; exp(·): The exponential mapping function maps Lie algebras to rotation matrices.

[0044] S122. LiDAR point cloud registration; using point cloud registration algorithms, such as ICP and NDT algorithms, calculate the relative pose change T of the LiDAR from keyframe i to keyframe j. ij ; S123. Joint optimization: Construct the following residual function to minimize it, so as to simultaneously optimize the robot's pose, velocity and IMU bias to suppress the impact of mismatches, such as interference caused by dynamic objects, on the results.

[0045] The residual function is constructed as follows: ; in: The total residual constrained by the lidar is used as part of the optimization objective. : Contains all keyframe pairs (i, j) with lidar matching constraints; The square of the Mahalanobis distance measures the magnitude of the residuals in the metric space defined by its covariance matrix Σ. The relative pose transformation from keyframe i to keyframe j is calculated by matching the point cloud with LiDAR. The relative pose transformation from keyframe i to keyframe j is obtained through IMU pre-integration. The Huber kernel function is used to reduce the impact of outliers, i.e., mismatches, on the optimization results, making the system more robust to noise. The covariance matrix constrained by the lidar-IMU reflects the uncertainty in relative pose estimation. By introducing the Huber kernel function, the system becomes robust to dynamic disturbances such as smoke and water droplets in the fire scene, enabling accurate positioning under harsh sensing conditions.

[0046] S13, Output local state estimation and keyframes; Local state estimation: high-precision pose, velocity, and IMU bias of robot i at time k. This result is passed to S2 as the basis for subgraph partitioning decisions.

[0047] Keyframe pose and point cloud: The pose and corresponding compressed point cloud selected as keyframes are passed to S2 for constructing and maintaining subgraphs.

[0048] The robot's state vector is constructed as follows: ; in: : The complete state vector of robot i at time k; : The position vector of robot i at time k , Represents a three-dimensional vector; The rotation quaternion of robot i at time k, representing the rotational posture. , Representing a unit quaternion space vector, using rotation quaternions to characterize the robot's rotational posture is more compact than rotation matrices and does not produce singularities; The three-dimensional velocity of robot i at time k. , Represents a three-dimensional vector; The gyroscope and accelerometer of robot i have zero bias. , This represents a three-dimensional vector, which represents the non-zero output error of the gyroscope when stationary and the output error of the accelerometer when in a zero-acceleration state.

[0049] Step S1 achieves stable and efficient state estimation on the embedded platform by tightly coupling the data from the inertial measurement unit and the lidar data, thus meeting the computing power limitations of the robot carrying the fire-fighting mission payload.

[0050] S2. Adaptive Subgraph Management: Each robot constructs a subgraph based on local state estimation, inputs point cloud data into the subgraph, compresses the point cloud data using a multi-resolution voxel mesh, and constructs the subgraph's information matrix together with the pose. The information matrix is ​​used to represent the constraint strength of the subgraph.

[0051] Step S2 specifically includes the following steps.

[0052] S21, Subgraph construction condition judgment; Input the robot's current pose from S1 and the initial pose of the previous subgraph. The system determines whether the threshold condition for constructing a new subgraph is met. Specifically, when the cumulative change in the robot's translation distance or rotation angle exceeds a preset threshold, the creation of a new subgraph is triggered.

[0053] The condition for robot i to construct a subgraph at time k is: or Time generation ; in: The m-th subgraph created by robot i; : The position vector of robot i at time k; : The rotation quaternion of robot i at time k; Subgraph The initial position of robot i; Subgraph The initial pose of robot i; Maximum translation threshold; Maximum rotation threshold; : and The angle between them.

[0054] The construction of the subgraph depends on whether the cumulative change in the robot's translational distance or rotational angle exceeds a preset threshold. This design differs from subgraph division based on fixed time or fixed distance. The preset threshold is based on the cumulative change in motion and can adapt to scenarios where the robot's movement speed is uneven. In complex buildings, robots may frequently turn and travel long distances in straight lines; this design can generate subgraphs of appropriate size and rich information.

[0055] S22. Subgraph compression and representation; The input is a set of keyframe point clouds that need to be incorporated into the new subgraph. The point cloud data within the subgraph is compressed using a multi-resolution voxel mesh. l resolution levels are constructed, with l=1 being the finest and l=2, l=3 gradually becoming coarser. Within each voxel, only the statistical information of that voxel is retained, such as the centroid position, normal vector, and occupancy probability.

[0056] The point cloud data compressed using multi-resolution voxel meshes is shown below: ; in: : Voxel indexing function, take a 3D point coordinate x as input, and output the index of the voxel to which the point belongs at the l-th resolution; : Coordinates of the origin of the subgraph; : The voxel side length of the l-layer voxel point cloud map; when l is 0, it is the base resolution. : Resolution level index.

[0057] By using multi-resolution voxel meshes to compress point cloud data, the amount of data that needs to be transmitted is significantly reduced while ensuring key structural information.

[0058] S23, Subgraph Information Matrix Calculation; Input the compressed point cloud data and the initial pose of the sub-graph, calculate and maintain the information matrix of the sub-graph, as shown below; ; in: Subgraph The information matrix, which is the inverse of the covariance matrix, is used to represent the constraint strength of the subgraph;

[0059] P: Uncertainty in the initial pose of the subgraph;

[0060] f: The strength of geometric constraints between key feature points extracted from the subgraph, such as planes and edge points;

[0061] Λp: Pose information matrix, The uncertainty corresponds to the six degrees of freedom of the initial pose of the subgraph, including three-way translation and three-way rotation;

[0062] Λf: Feature point information matrix, K is the number of feature points extracted from the subgraph, and the feature point information matrix is ​​used to represent the strength of the geometric constraints between feature points.

[0063] Data lightweighting was achieved by designing an information matrix. The information matrix does not contain the original point cloud data, but records the structural strength and confidence of the subgraph. In subsequent collaborative optimization, only the information matrix is ​​exchanged, and the compressed point cloud is not exchanged, which can greatly save communication bandwidth.

[0064] S24. Output the compressed point cloud data and information matrix of the subgraph, which serve as the basic data unit for opportunistic collaborative optimization in S3.

[0065] Step S2 adaptively partitions the graph based on local state estimation, thus decomposing the large-scale graph construction problem into multiple local, independent, and easily manageable subtasks. Through lightweight designs involving multi-resolution voxel compression and information matrix extraction, the amount of data requiring online transmission and processing is significantly reduced at the source, directly solving the communication bottleneck in bandwidth-constrained environments.

[0066] S3. Opportunistic Collaborative Optimization: When robots meet, they exchange subgraph information and use a consensus optimization algorithm to jointly optimize their respective point cloud graphs to obtain optimized poses and globally consistent point cloud graph states. Step S3 specifically includes the following steps.

[0067] S31. Robot encounter detection; The robot continuously monitors the communication status with other robots. The communication status estimation is based on the nearest distance estimated by communication delay, signal strength, etc. The encounter probability is calculated according to the communication status. When the encounter probability exceeds the confidence threshold, it is determined to be an encounter and triggers the subgraph information exchange.

[0068] The probability of robots i and j meeting within the communication range is as follows: ; in: The probability of robots i and j meeting within the communication range; Standard deviation of communication distance; Continuous communication time; Time constant; By continuously monitoring the waiting time for encounters, it achieves independence from fixed topology or central scheduling, adapting to the normal situation of unstable communication and unpredictable robot movement paths in fire scenes. It utilizes brief encounter opportunities to exchange key information, significantly improving robustness in unreliable networks.

[0069] S32, Relative pose estimation; When robots meet, fast point cloud registration is performed between the subgraphs of robots i and j to estimate their relative pose transformation. A multi-resolution ICP algorithm is used to accelerate the matching process and estimate the relative pose. The calculation formula is as follows.

[0070] ; Where: P: the current subgraph of robot i A three-dimensional point; T: A 4×4 homogeneous transformation matrix; : in subgraph Nearest neighbor search in .

[0071] S33, Distributed pose graph optimization; Based on the keyframe poses generated in S1, the subgraph data maintained in S2, and the relative poses between robots in S32, a local pose graph containing new cooperative edges is locally constructed and optimized.

[0072] The construction of the local pose graph is as follows; Define each robot i to maintain a local pose graph. ; , Let be the set of vertices, consisting of the complete state vector of robot i at time k. constitute; , The set of edges consists of odometry edges, loopback edges, and collaborative edges, in that order. Odometry edge, connecting adjacent keyframes, constrained by S1's LiDAR-IMU fusion; : Loop edge, connecting non-adjacent keyframes. When the robot moves to a certain location again, the loop is detected by its own subgraph matching and provides constraints. Collaborative edge: A keyframe connecting robots i and j, estimated by the relative pose of S32. Provide constraints; A cooperative edge residual function is constructed for the cooperative edges to quantify the consistency between the current pose graph and the cooperative observation. The formula is as follows: ; : These represent the poses of robots i and j in the world coordinate system; Logarithmic mapping is the inverse operation of exp(·).

[0073] To achieve global consistency, a consensus optimization algorithm is adopted. After each optimization iteration, the robot exchanges state information with neighboring robots within its communication range. The next state of each robot is not only driven by the local optimization gradient, but also affected by the consistency constraints of the neighboring robot's state. This allows the point cloud map state of all robots to gradually converge to global consistency without a central server, and outputs the optimized pose and the globally consistent point cloud map state.

[0074] The consensus optimization method is calculated as follows: ; in, : The state vector of robot i at step t+1 in the optimization iteration; The learning rate or step size controls the magnitude of each state update. Local objective function of robot i In its current state vector The gradient at a given point drives the state to move toward a local optimum. The set of neighbors of robot i, that is, all other robots that can communicate and cooperate with robot i at the current moment; The consistency weight matrix determines the extent to which robot i should correlate with the state vector of its neighbor robot j. To achieve consensus on state estimation among multiple robots, they must move closer together, which is usually related to communication quality or confidence level.

[0075] By employing a distributed consensus optimization algorithm, the risk of single point of failure of the central node is eliminated, giving the system inherent resilience and scalability. The reliability and exploration capabilities of the system are enhanced with each additional robot.

[0076] Step S3, through opportunistic collaborative design, enables the exchange of critical information by taking advantage of brief encounters without relying on stable communication, thus significantly improving the system's robustness in unreliable networks.

[0077] S4. Incremental Point Cloud Fusion: The sub-image from step S2 is probabilistically fused with the optimized pose and globally consistent point cloud state from step S4 to update the global point cloud. This probabilistic fusion includes evidence-based fusion, multi-resolution weight allocation, and dynamic environment updates. The final output is an updated, robust global point cloud labeled with dynamic objects.

[0078] The probabilistic fusion in step S4 specifically includes the following steps.

[0079] S41. Point cloud image fusion based on evidence theory; The Dempster combination rule is used to fuse two sets of evidence to obtain updated evidence. This rule can effectively handle potential conflicts between evidence from different robots. Specifically, a conflict factor K is introduced. When the conflict is too large, for example, one robot considers a voxel to be in an idle state while another robot considers it to be in an occupied state, the confidence of the fused result is automatically reduced, and the degree of uncertainty is increased. The specific calculation is as follows.

[0080] Define each voxel to maintain an evidence vector. : ; in: : The basic probability assignment for belonging to category c, with a value between 0 and 1, for all categories. The sum plus uncertainty It equals 1; Uncertainty: The basic probability assigned to uncertainty or the unknown. It represents the degree to which a clear classification cannot be made due to conflict, insufficient information, or other reasons. C: Number of semantic categories, such as free, occupied, unknown, fire source, and danger.

[0081] Use Dempster's combination rule to combine two evidence vectors from different robots. and After fusion, the probability of proposition A, such as "the voxel is occupied", is assigned to the new evidence vector.

[0082] ; ; Evidence theory fusion enables reasonable processing of information from different robots, even conflicting information, avoiding simple "overlay" or "average" operations, and ensuring smooth and robust convergence of point cloud maps.

[0083] S42, Multi-resolution weight allocation; The sub-image information at different resolution levels generated in S2 is fused. Different weights are assigned to different resolution levels during fusion; lower resolution levels have smaller weights in the fusion process. This is because lower resolution levels have higher point cloud compression and greater spatial information loss, therefore they should be assigned lower confidence levels during global fusion. The specific calculation is as follows.

[0084] Construct fusion weights at different resolution levels: ; in: The weights used when fusing point cloud images at the l-th resolution; l: resolution level, 1 is the highest resolution, increasing l will decrease the resolution; : Attenuation coefficient, controls the rate at which the weights decay as the resolution decreases, and is an empirical value when it equals 0.5.

[0085] Multi-resolution weight allocation reduces computational and storage overhead while maintaining accuracy.

[0086] S43, Dynamic Environment Update; The determination of whether a voxel is a dynamic object is based on the observation time series of the voxel. If the observed state of a voxel changes too frequently in a short period of time, such as occupied state and idle state, its dynamic object confidence is increased. For voxels marked as dynamic, they are marked as dynamic obstacles in the final point cloud map for use by the obstacle avoidance algorithm, but they will not participate in the long-term consistency optimization of the point cloud map to avoid polluting the global point cloud map.

[0087] Handling dynamic objects in a fire: ; The probability or confidence level at which voxel v is classified as a dynamic object; The starting time of the observation; : The current moment.

[0088] : Indicator function. The function evaluates to 1 when the condition within the parentheses is true, and 0 otherwise. It is used here to count data within a time period [...]. , Within ], the number of times voxel v is observed.

[0089] Dynamic environment updates can adapt to highly dynamic environments such as fire scenes, distinguishing dynamic obstacles such as fire sources and moving personnel from building structures, thus improving the practicality and safety of point cloud maps.

[0090] Step S4 uses a probabilistic fusion framework to fuse the local point cloud maps of each robot as progressive evidence. The new evidence continuously updates the confidence distribution of the global point cloud map in an overlayable and non-overlapping manner, effectively handling multi-source information conflicts, perceptual noise and dynamic interference, and achieving smooth and robust convergence of the point cloud map.

[0091] S5. Resource scheduling based on communication awareness: When robots meet, the network quality and computing power of both robots are obtained in real time. The amount of data exchanged between the two robots in step S3 is adjusted according to the network quality. The computing load of the two robots is allocated according to their computing power, and the processing task assignment is adjusted. The processing task includes point cloud data compression in step S2 and point cloud map fusion in step S4.

[0092] Step S5 specifically includes the following steps.

[0093] S51, Network Status Assessment; Based on the link layer parameters provided by the command and combat network, a comprehensive score is given for the network quality of robot i and each neighboring robot j. The network quality score takes into account available bandwidth, packet loss rate and communication latency. The calculation formula is as follows.

[0094] ; in: The network quality score between robots i and j is a scalar value between 0 and 1, with a higher value indicating better network quality.

[0095] Available bandwidth indicates the maximum rate at which the current link can still be used to transmit data, usually measured in Mbps.

[0096] Theoretical maximum bandwidth, used for normalization.

[0097] PLR: Packet Loss Rate, a value between 0 and 1, representing the proportion of data packets lost during transmission.

[0098] Latency: Communication delay, which represents the time it takes for a data packet to be sent and received, usually in milliseconds; : Weighting coefficients, representing the importance of bandwidth, reliability (packet loss), and real-time performance (latency) in the overall score, respectively, and their sum is 1; : Delay time constant, used to map the delay value to a decay factor. When τ is 100ms, it means that when the delay exceeds 100ms, this score will be significantly reduced.

[0099] S52, Adaptive data exchange strategy; The amount of subgraph data exchanged between the two robots in step S3 is adjusted based on network quality. When the network quality score is lower than the communication quality threshold, the amount of subgraph data exchanged is reduced, and at the lowest level, only the information matrix of the subgraph is exchanged, without exchanging compressed point cloud data.

[0100] Obtain the robot's battery status. If the battery is low, reduce the amount of data exchanged and prioritize the robot's navigation function.

[0101] The formula for calculating the data volume of a swapped subgraph is as follows.

[0102] ; in, : Exchange the amount of data in the subgraph; Basic data volume, i.e., the planned data volume of the exchange subgraph under ideal network quality and robot power consumption; : Communication quality threshold, when the network quality score When the data exchange volume falls below this value, the data exchange volume begins to decrease proportionally. : Robot's current battery level; The robot is fully charged.

[0103] This design enables SLAM and communication—two traditionally separate layers—to collaborate. By assessing network conditions in real time and dynamically adjusting data exchange strategies, it facilitates extensive collaboration when network quality is good and prioritizes core functions when network conditions are poor. Even in the worst network environments, it can discard large amounts of point cloud data and exchange only information matrices, while still maintaining basic collaborative functions and ensuring system availability.

[0104] S53, Calculate load balancing; The computational capabilities of both robots are obtained, and the computational load is allocated proportionally according to these capabilities. A dynamic decision is made regarding whether to offload some processing tasks—including point cloud data compression in step S2 and point cloud map fusion in step S4—to a neighboring robot with stronger computational capabilities. For example, if a robot equipped with demolition tools has weaker computational capabilities, the computationally intensive point cloud map fusion task will be assigned to another robot primarily responsible for reconnaissance, which has stronger computational capabilities.

[0105] The formula for robot computational load allocation is as follows.

[0106] ; in: The computational load allocated to robot i includes the number of keyframes to be processed and the number of variables to be optimized. The computing power of robot i is usually measured in floating-point operations per second. Total computational load: The total computational load that the entire robot team needs to complete; : The number of robots used for modeling.

[0107] S54. Feedback and Adjustments; The dynamically adjusted data volume of the swapped subgraphs and the processing tasks, including point cloud data compression in step S2 and point cloud map fusion in step S4, are fed back to S3, S2, and S4 respectively. The task unloading command is then sent to the corresponding robot.

[0108] Step S5 introduces cross-layer optimization to design communication-aware resource scheduling, enabling the collaborative strategy to dynamically perceive and adapt to changes in the underlying network state based on real-time network quality. This adaptive scheduling mechanism achieves optimal matching between communication resources and collaborative needs, ensuring that critical collaborative functions are maintained even when network conditions deteriorate, thereby maintaining optimal overall task performance in a variable environment.

[0109] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make modifications, alterations, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network, characterized in that, Includes the following steps: S1. Lightweight front-end odometry: Each robot collects inertial measurement unit data and lidar data, pre-integrates the inertial measurement unit data, registers the lidar data point cloud, and constructs the two into a tightly coupled residual function to obtain the local state estimate and key frame data at the current moment. The key frame data includes pose and point cloud data. S2. Adaptive Subgraph Management: Each robot constructs a subgraph based on local state estimation, inputs point cloud data into the subgraph, compresses the point cloud data using multi-resolution voxel mesh, and constructs the subgraph's information matrix together with the pose. The information matrix is ​​used to represent the constraint strength of the subgraph. S3. Opportunistic Collaborative Optimization: When robots meet, they exchange subgraph information and use a consensus optimization algorithm to jointly optimize their respective maps, resulting in optimized poses and a globally consistent map state. S4. Incremental map fusion: The sub-map from step S2 is probabilistically fused with the optimized pose and globally consistent map state from step S3, and the global map state is then updated. S5. Resource scheduling based on communication awareness: When robots meet, the network quality and computing power of both robots are obtained in real time. The amount of data exchanged between the two robots in step S3 is adjusted according to the network quality. The computing load of the two robots is allocated according to their computing power, and the processing task assignment is adjusted. The processing task includes point cloud data compression in step S2 and map fusion in step S4. In step S3, the robot continuously monitors the communication status with other robots, calculates the encounter probability based on the communication status, and determines an encounter when the encounter probability exceeds the confidence threshold, triggering subgraph information exchange. In step S5, when the network quality is lower than the communication quality threshold, the amount of data exchanged in the subgraph is reduced. At the lowest level, only the information matrix of the subgraph is exchanged, and the compressed point cloud data is not exchanged.

2. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 1, characterized in that, The Huber kernel function is introduced into the residual function of step S1.

3. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 1, characterized in that, In step S2, the condition for robot i to construct the subgraph at time k is: or Time generation ; in: The m-th subgraph created by robot i; : The position vector of robot i at time k; : The rotation quaternion of robot i at time k; Subgraph The initial position of robot i; Subgraph The initial pose of robot i; Maximum translation threshold; Maximum rotation threshold; : and The angle between them.

4. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 3, characterized in that, The point cloud data compressed using multi-resolution voxel grids in step S2 is shown below: ； in: : Voxel indexing function, take a 3D point coordinate x as input, and output the index of the voxel to which the point belongs at the l-th resolution; : Coordinates of the origin of the subgraph; : The voxel side length of the l-layer voxel map; when l is 0, it is the base resolution. : Resolution level index.

5. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 3, characterized in that, The information matrix in step S2 is shown below: ； in: Subgraph The information matrix, which is the inverse of the covariance matrix, is used to represent the constraint strength of the subgraph; Λp: Pose information matrix, The uncertainty corresponds to the six degrees of freedom of the initial pose of the subgraph, including three-way translation and three-way rotation; Λf: Feature point information matrix, K is the number of feature points extracted from the subgraph, and the feature point information matrix is ​​used to represent the strength of the geometric constraints between feature points.

6. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 1, characterized in that, In step S3, a consensus optimization algorithm is used. Specifically, after each optimization iteration, the robot exchanges state information with neighboring robots within the communication range. The next state of each robot is affected by the consistency constraint of the neighboring robot's state, and the map state of all robots gradually converges to global consistency.

7. The lightweight distributed collaborative SLAM method for multiple robots based on a fire rescue on-site command and control network as described in claim 1, characterized in that, The probabilistic fusion in step S4 includes evidence theory fusion, multi-resolution weight allocation, and dynamic environment update.

Images