Multi-robot cooperative SLAM method for snowfield environment

Abstract

A multi-robot cooperative SLAM method for a snowfield environment comprises the following steps: 1) deploying a multi-robot system, and loading a snowfield environment knowledge graph; 2) acquiring environmental data through a multi-source sensor system, and preprocessing the environmental data; 3) performing semantic segmentation, object identification, snowfield attribute prediction and visibility estimation on the environmental data through the multi-modal large model, and outputting an environmental semantic data packet; 4) dynamically updating the snowfield environment knowledge graph; 5) estimating the pose of each robot in the Riemannian space; 6) performing trajectory prediction on a dynamic target in the environment by using the trajectory prediction model; 7) constructing a local semantic dynamic map comprising a static environment map, a dynamic object map, a semantic map and a risk map; and 8) each robot shares the local semantic dynamic map and the pose information with other robots through a wireless network, and iteratively optimizes the pose and the map to obtain a global consistent semantic dynamic map.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a multi-robot cooperative SLAM method for snowy environments. Background Technology

[0002] In recent years, the polar and high-altitude snow environments have been significantly altered by the combined effects of global climate change and intensified human activities, making scientific research and environmental monitoring in these areas increasingly urgent. Against this backdrop, multi-robot systems, with their outstanding advantages such as collaborative operation, information sharing, and strong fault tolerance, offer a novel approach to detection and monitoring in snow environments. However, applying SLAM (Simultaneous Localization and Mapping) methods based on multi-robot systems to snow scenarios is not easy. Polar and high-altitude snow environments differ from traditional scenarios such as autonomous driving and indoor navigation, presenting several challenges:

[0003] 1. High reflectivity and low texture environment: Snow has an extremely high reflectivity to light, which can easily lead to camera overexposure or loss of features. At the same time, the monotonous snow scene lacks obvious texture information. This makes it difficult for traditional vision-based SLAM methods to effectively extract environmental features, and frequently results in problems such as localization drift and inaccurate map construction.

[0004] 2. Frequent dynamic changes in the environment: Snowy environments are highly susceptible to weather conditions. Natural processes such as snowfall, snowmelt, and wind erosion can rapidly alter the appearance of the environment, causing maps to become invalid in a short period of time. This places far greater demands on the dynamic map updating capabilities and robustness of SLAM methods than on general static environments, and traditional SLAM methods are simply unable to adapt to such high-frequency changes.

[0005] 3. Dramatic changes in visibility: blizzards can cause a sharp drop in visibility, severely compressing the sensor's sensing range and reducing its accuracy; while strong sunlight in clear weather can cause shadow interference and glare problems. These all pose challenges to the stability and reliability of SLAM methods.

[0006] 4. Lack of prior information support: Unlike structured urban environments, snowy wilderness scenes often lack readily available structured maps and semantic prior knowledge, making it impossible to use existing map data to assist robots in localization and navigation. This requires multiple robots to have stronger autonomous exploration and independent map building capabilities.

[0007] Traditional cooperative SLAM methods are mostly based on static environment assumptions or only process dynamic objects through simple filtering. When facing snowy environments, traditional cooperative SLAM methods have the following problems:

[0008] 1. To address the limitations of single sensors in snowy environments, multi-robot systems need to integrate data from multiple sensors, such as LiDAR, millimeter-wave radar, thermal imaging cameras, and visible light cameras, for environmental perception and information complementarity. However, different sensors have different data types, timestamps, coordinate systems, etc. Ensuring effective fusion of multi-source information and maintaining data consistency and accuracy has become a primary challenge.

[0009] 2. Insufficient anti-interference ability: Strong light reflection can easily cause image saturation, and snowflake interference can lead to missing point cloud data. Traditional algorithms have weak anti-interference ability.

[0010] 3. Feature recognition: Specialized algorithms need to be designed to extract and identify various snow terrain features from a single snow environment, such as distinguishing between flat snow, snow slopes, ice surfaces, crevasses, etc., as well as identifying potential danger zones.

[0011] 4. Complex terrain: Snowy terrain is rugged and varied. Traditional SLAM pose estimation algorithms based on planar assumptions have difficulty accurately capturing the robot's position and attitude information when faced with rugged terrain and frequent robot turns, which further affects the collaborative operation effect.

[0012] To meet the needs of scientific research and environmental monitoring, multi-robot collaborative SLAM methods must be able to achieve the following:

[0013] 1. Environmental risk assessment: Snowy environments pose safety risks such as avalanches and ice cracks, requiring real-time assessment of environmental risks to provide a basis for safe path planning for robots.

[0014] 2. Constructing high-precision maps: Polar scientific expeditions, environmental monitoring and other tasks require the construction of high-precision, high-resolution three-dimensional snow maps.

[0015] 3. Robust collaboration mechanism: Communication conditions are harsh in polar environments, so a robust collaboration mechanism needs to be designed to ensure that multiple robots can still collaborate effectively when communication is limited. Summary of the Invention

[0016] To address the aforementioned problems, this invention provides a multi-robot collaborative SLAM method for snowy environments. By employing multimodal large models, knowledge graphs, and Riemann space optimization, the method enables multi-robot systems to collaboratively perceive and understand complex snowy environments, and to construct accurate, semantically rich, and real-time updated semantic dynamic maps, ultimately achieving safe and efficient exploration and environmental monitoring in snowy environments.

[0017] The technical solution of this invention is: a multi-robot cooperative SLAM method for snowy environments, comprising the following steps:

[0018] 1) Deploy a multi-robot system consisting of multiple robots, each robot loading a pre-built snow environment knowledge graph;

[0019] 2) Each robot collects environmental data in real time through a multi-source sensor system and preprocesses the collected environmental data.

[0020] 3) Perform semantic segmentation, object recognition, snow attribute prediction, and visibility estimation on the preprocessed environmental data using a multimodal large model, and output environmental semantic data packets;

[0021] 4) Dynamically update the snow environment knowledge graph based on the environmental semantic data package;

[0022] 5) Estimate the pose of each robot in Riemann space based on the slip model, environmental data, and snow environment knowledge graph;

[0023] 6) By integrating the historical movement trajectory of dynamic targets, the environmental semantic data package output by the multimodal large model, and the snow environment knowledge graph information, a trajectory prediction model is used to predict the trajectory of dynamic targets in the environment.

[0024] 7) Each robot constructs a local semantic dynamic map, including a static environment map, a dynamic object map, a semantic map, and a risk map, based on its pose estimation results, dynamic target trajectory prediction results, environmental semantic data packets output by the multimodal large model, and the snow environment knowledge graph.

[0025] 8) Each robot shares local semantic dynamic map and pose information with other robots through a wireless network. A distributed optimization algorithm based on average consensus is adopted, combined with loop closure detection of environmental semantic data packets and snow environment knowledge graph constraints, to iteratively optimize pose and map, and obtain a globally consistent semantic dynamic map.

[0026] Preferably, the construction of the snow environment knowledge graph includes:

[0027] 1.1) Construct a snow environment knowledge map based on existing knowledge bases, literature, and expert experience related to the snow environment. in, Represents a set of entities. Represents a set of relations. Represents a set of facts;

[0028] 1.2) The fact set of the snow environment knowledge graph is represented in a structured manner using RDF or attribute graphs;

[0029] 1.3) Establish static inference rules.

[0030] Preferably, the multi-source sensor system includes lidar, millimeter-wave radar, thermal imaging camera, visible light camera, and IMU.

[0031] Preferably, the preprocessing includes time synchronization, spatial calibration, data alignment, image fusion, and noise filtering.

[0032] Preferably, the data alignment includes the following steps:

[0033] ① Project the lidar point cloud onto the visible light image plane and the thermal imaging image plane to generate visible light images and thermal imaging images, and generate corresponding depth images based on the visible light images and thermal imaging images;

[0034] ② The visible light image, thermal imaging image and their corresponding depth image are input into a deformable convolutional network with shared weights. The visible light image feature map, thermal imaging image feature map and depth image feature map are output by the shape and position offset of the convolutional kernel.

[0035] ③ The visible light image feature map, thermal imaging image feature map, and feature maps of each depth image are respectively input into the attention mechanism module for multimodal feature weighted fusion to obtain the final multimodal feature vector and achieve data alignment.

[0036] Preferably, the image fusion includes the following steps:

[0037] a. Train a generative adversarial network model using existing snow scene image sets;

[0038] b. Input the environmental data collected by the multi-source sensor system into the generative adversarial network model to generate enhanced images;

[0039] c. A weighted average method is used to fuse the enhanced image with the environmental data to obtain the final fused image.

[0040] Preferably, the noise filtering includes the following steps:

[0041] A. Employ a multimodal large model to output the current environmental noise level based on environmental data;

[0042] B. Adjust the filter parameters according to the current ambient noise level;

[0043] C. Filter the environmental data using the adjusted filter to remove noise and abnormal data.

[0044] Preferably, the snow environment knowledge graph update includes the following steps:

[0045] 4.1) Modeling a knowledge graph of the snow environment using a graph convolutional network;

[0046] 4.2) Input the environmental semantic data package output by the multimodal large model into the graph convolutional network to update the snow environment knowledge graph.

[0047] Preferably, the estimation of robot poses in Riemann space includes the following steps:

[0048] 5.1) Incorporate the slip factor into the robot's kinematic model. The robot's slip kinematic model is as follows:

[0049] T k =T{k-1}⊕ΔT{k-1,k}⊕T slip

[0050] Among them, T k Let ΔT{k-1,k} represent the robot's pose at time k, ΔT{k-1,k} represent the pose change from time k-1 to time k calculated from odometry data, and ⊕ represent the pose composition operation. slip This indicates the pose deviation caused by slippage;

[0051] 5.2) Based on the robot's sliding kinematics model, the robot's sliding state is estimated using an extended Kalman filter;

[0052] 5.3) Based on the robot's sliding kinematics model and the robot's sliding state estimated by the extended Kalman filter, the robot's pose state is estimated by the left invariant extended Kalman filter, and a knowledge graph is introduced for constraint to optimize the pose estimation results.

[0053] Preferably, the trajectory prediction model is used to predict the trajectory of dynamic targets in the environment, including the following steps:

[0054] 6.1) A trajectory prediction multimodal model is used to fuse the historical movement trajectory of dynamic targets, the environmental semantic data package output by the multimodal large model, and the snow environment knowledge graph information to output fused features;

[0055] 6.2) Input the fused features into the graph neural network to predict the trajectory of dynamic targets in the environment.

[0056] Compared with existing technologies, this invention proposes a practical solution for dynamic cooperative SLAM of multiple robots in snowy environments, and its effectiveness has been verified through experiments, achieving the following outstanding beneficial effects:

[0057] 1. Higher accuracy and robustness of environmental perception: To a certain extent, it overcomes the problems of high reflectivity, low texture and noise interference in snowy environments, improves the accuracy and robustness of environmental perception, and provides more reliable information support for subsequent positioning, mapping and decision-making.

[0058] 2. Enhanced environmental understanding and reasoning capabilities: This enables the system to better understand semantic information about the environment and to make inferences and predictions, thereby improving the robot's autonomous decision-making ability.

[0059] 3. Higher positioning accuracy and robustness: It effectively solves the positioning drift problem caused by complex terrain and slippage in snowy environments, improves positioning accuracy and robustness, and ensures safe navigation of the robot in extreme environments.

[0060] 4. Superior adaptability to dynamic environments: It can predict the future trajectory of dynamic targets, improve the system's adaptability to dynamic environments, and enhance the robot's environmental perception and decision-making capabilities.

[0061] 5. More efficient and accurate collaborative SLAM: Improved efficiency and accuracy of multi-robot collaborative SLAM, providing stronger technical support for complex tasks in snowy environments. Attached Figure Description

[0062] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0063] See Figure 1 A multi-robot cooperative SLAM method for snowy environments includes the following steps:

[0064] 1) Deploy a multi-robot system consisting of multiple robots, each robot loading a pre-built snow environment knowledge graph; the construction of the snow environment knowledge graph includes:

[0065] 1.1) Construct a snow environment knowledge map based on existing knowledge bases, literature, and expert experience related to the snow environment. in, Represents a set of entities. Represents a set of relations. This represents a set of facts. Facts include two types: relational facts and attribute facts. Each fact exists in the form of a triple. Relational facts refer to the relationship between two entities, and their structure is (entity A, relation, entity B), such as (snow slope A, located in, Himalayas), (robot A, cooperating with, robot B). Attribute facts describe the intrinsic characteristics or parameters of an entity, and their structure is (entity, attribute, value). Here, "value" is usually a specific numerical value, string, date, etc., rather than another complex entity in the knowledge graph, such as (snow slope A, slope, "35 degrees"), (snow slope A, snow thickness, "2 meters").

[0066] 1.2) Use RDF or attribute graphs to structurally represent the entities, relations, and attributes of the fact set in the snow environment knowledge graph;

[0067] 1.3) Establish static reasoning rules. Based on the established static reasoning rules, use a knowledge graph reasoning engine (such as GraphDB) to reason about the knowledge graph, and infer avalanche risk levels based on terrain features and meteorological information.

[0068] 2) Each robot collects environmental data in real time through a multi-source sensor system, including lidar, millimeter-wave radar, thermal imaging camera, visible light camera, and IMU (inertial measurement unit), and preprocesses the collected environmental data. In this embodiment:

[0069] LiDAR: Used to acquire 3D point cloud data of the environment, using RoboSense RS-LiDAR-M1 or LeiShen Intelligent LS series LiDAR.

[0070] Millimeter-wave radar: used to detect obstacles and moving targets, employing Huayu Automotive millimeter-wave radar or Senstech STR series radar.

[0071] Thermal imaging camera: used to sense temperature differences, using Guide Infrared IR series thermal imagers or Dali Technology TWS series thermal imagers.

[0072] Visible light camera: used to acquire environmental texture information, using Hikvision MV series industrial cameras or Dahua Technology DH series industrial cameras.

[0073] IMU: Used to measure the linear acceleration and angular velocity of robots, using StarNet DMU series inertial measurement units or Long March Rocket Technology Co., Ltd. LN series inertial navigation systems.

[0074] The preprocessing includes time synchronization, spatial calibration, data alignment, image fusion, and noise filtering. Since different sensors have different data types, resolutions, and sampling frequencies, data alignment is required. This invention proposes a multimodal data alignment method based on deformable convolutional networks. This method can learn the spatial correspondence between different modalities and align them to a unified spatiotemporal coordinate system, improving the accuracy of subsequent feature fusion.

[0075] The data alignment includes the following steps:

[0076] ① Project the lidar point cloud onto the visible light image plane and the thermal imaging image plane to generate visible light images and thermal imaging images, and generate corresponding depth images based on the visible light images and thermal imaging images; specifically: project the lidar point cloud data P = {p i |i=1……N}, projected onto the visible light image plane and the thermal imaging image plane, generating a visible light image I. RGB and thermal imaging image Ith And based on visible light image I RGB and thermal imaging image I th Generate the depth image D of the corresponding visible light image RGB Depth image D of thermal imaging images th Wherein, the lidar point cloud data P is a set containing N three-dimensional points, each point p i =(x,y,z) T It represents a point in space.

[0077] The depth image D of the visible light image RGB The formula for generating it is:

[0078]

[0079] Where: (u,v) represents the depth image D of the visible light image. RGB pixel coordinates on p i .z is a three-dimensional point p i The z-axis component in the camera coordinate system, i.e., the depth value, π(p i ) is to convert the three-dimensional point p i The projection function, which maps to the image plane, yields pixel coordinates. This function can be calculated using the known camera intrinsic matrix K. The condition for a valid projection is that point p... i Located within the camera's field of view and its depth value p i .z is positive, and otherwise indicates that there is no valid 3D point projection at pixel (u,v), and the depth value of that point is set to 0.

[0080] The depth image D of the thermal imaging image th The generation formula and the depth image D of the visible light image RGB The generation formula is the same as that used for:

[0081]

[0082] Where: (u,v) represents the depth image D of the visible light image. RGB pixel coordinates on p i .z is a three-dimensional point p i The z-axis component in the camera coordinate system, i.e., the depth value, π(p i ) is to convert the three-dimensional point p i The projection function, which maps to the image plane, yields pixel coordinates. This function can be calculated using the known camera intrinsic matrix K. The condition for a valid projection is that point p... i Located within the camera's field of view and its depth value p i.z is positive, and otherwise indicates that there is no valid 3D point projection at pixel (u,v), and the depth value of that point is set to 0.

[0083] ② The visible light image, thermal imaging image, and their corresponding depth images are input into a deformable convolutional network with shared weights. The network outputs feature maps for the visible light image, thermal imaging image, visible light depth image, and thermal imaging depth image through kernel shape and position shifts. Specifically: the visible light image I... RGB Thermal imaging image I th Depth image D of visible light image RGB Depth image D of thermal imaging th Input into a deformable convolutional network F with shared weights DCN In this invention, Deformable ConvNets v2 is used as the F... DCN The basic framework of this network consists of 10 convolutional layers, including 3 deformable convolutional layers. Each deformable convolutional layer contains an offset prediction branch and a modulation scalar prediction branch. The offset prediction branch learns a two-dimensional offset Δp for each sampling point, while the modulation scalar prediction branch learns a modulation scalar Δm (ranging from 0 to 1) to adjust the weights of different sampling points. The final convolution operation is based on the original convolutional kernel w, with adaptive sampling and weighting based on the learned Δp and Δm. The specific calculation formula is as follows:

[0084]

[0085] In the formula, p0 represents the coordinates of a target location on the output feature map, and R is the receptive field of the standard convolutional kernel. For example, for a 3x3 convolutional kernel, R = {(-1, -1), (-1, 0) ... (1, 1)}, p n It is the enumerated offset of all positions in R, x(·) represents sampling from the input feature map, w(p n ) represents the convolution kernel weights at the corresponding positions, Δp n and Δm n These are the values ​​predicted by the network, corresponding to sampling points p0+p. n The position offset and modulation scalar, y(p0) is the final calculation result of the output feature map at position p0.

[0086] ③ The visible light image feature map, thermal imaging image feature map, visible light depth image feature map, and thermal imaging depth image feature map are respectively input into the attention mechanism module for multimodal feature weighted fusion to obtain the final multimodal feature vector f. multi This achieves data alignment.

[0087] This invention employs CBAM (Convolutional Block Attention Module) as the attention mechanism module. This module includes both channel attention and spatial attention mechanisms, which can learn the importance of features from different channels and spatial locations respectively, and then perform weighted fusion to improve the feature representation capability. This invention uses the KITTI dataset (a publicly available autonomous driving dataset jointly released by the Karlsruhe Institute of Technology in Germany and Toyota Research Institute of America) as the training dataset and employs the cross-entropy loss function to optimize the network parameters.

[0088] The image fusion includes the following steps:

[0089] a. Train a generative adversarial network (GAN) model using existing snow scene image sets; specifically: train a GAN model (generative adversarial network model) using existing snow scene images, G = (G gen G dis ), where generator G gen The discriminator G is responsible for generating realistic snow scene images. dis Responsible for distinguishing between real images acquired by the sensor and those generated by the G generator. gen Generated snow scene image. This invention uses CycleGAN as the framework of the GAN model, which can learn the mapping relationship between images of different domains and generate style-transferred images. The loss function of CycleGAN includes three parts: adversarial loss, cycle consistency loss, and identity loss, which can effectively constrain the generation effect of the generator, making it generate more realistic and natural images.

[0090] b. Input the environmental data collected by the multi-source sensor system into the generative adversarial network model to generate enhanced images; specifically: input the snow scene image I raw (i.e., visible light image) is input into the generator G of the trained GAN model. gen In the process, an enhanced image I is generated. enh :

[0091] I enh =G gen (I raw )

[0092] c. A weighted average method is used to fuse the enhanced image with the environmental data to obtain the final fused image. The formula is as follows:

[0093] I final =αI enh +(1-α)I raw

[0094] In the formula, I finalFor image fusion, α is the fusion weight, which is adaptively adjusted according to image quality and noise level. enh To enhance the image, I raw Images of a snowy scene captured by a visible light camera.

[0095] This invention enhances perception capabilities in high-reflectivity, low-texture environments like snowfields through image fusion, strengthens the characteristics of the snowfield environment, learns the texture and structural features of the snowfield environment, and generates clearer, more detailed enhanced images, providing more reliable input data for subsequent semantic understanding and prediction.

[0096] The noise filtering includes the following steps:

[0097] A. A multimodal large model is used to output the current environmental noise level based on environmental data. The multimodal large model uses image texture information (e.g., low-texture areas may correspond to visual noise) and point cloud density information (e.g., sparse point clouds may represent sensor noise or severe weather) as input to predict the noise level of the current environment. n Among them, image texture information can be obtained by analyzing environmental images acquired by a visible light camera, and point cloud density information can be calculated from point cloud data acquired by a lidar.

[0098] B. Based on the current environmental noise level n Adjusting filter parameters; for example, adjusting the standard deviation σ for a Gaussian filter, the window size k×k for a median filter, etc. Filter parameters can be adjusted using pre-defined rules or learning-based methods. For instance, machine learning algorithms such as Support Vector Machines (SVMs) or decision trees can be used to learn the optimal filter parameters based on noise levels and image features.

[0099] C. Filter the environmental data using the adjusted filter to remove noise and abnormal data.

[0100] 3) Using a multimodal large model, semantic segmentation, object recognition, snow attribute prediction, and visibility estimation are performed on the preprocessed environmental data to output an environmental semantic data package. The environmental semantic data package includes: ① a pixel-level semantic segmentation map aligned with the input image, where each pixel is assigned a clear category label (e.g., snow slope, ice surface, obstacle, etc.); ② a list containing all identified objects, where each object is accompanied by its category, 3D bounding box, position in the robot coordinate system, and confidence score; ③ classification labels for the snow attributes of the current scene (e.g., dry snow, wet snow); ④ a quantized visibility estimate in meters. This precise data will serve as direct input for subsequent knowledge graph updates, map building, and path planning modules.

[0101] This invention employs a multimodal large model, Mask2Former, to fuse the image I. final It performs semantic understanding and prediction. Mask2Former is a multi-task learning model based on the Transformer architecture, capable of simultaneously performing tasks such as semantic segmentation, object detection, and instance segmentation. The network structure of the multimodal large-scale model Mask2Former mainly includes the following parts:

[0102] Image encoder: The Swin Transformer is used as the backbone network to extract multi-scale image features.

[0103] Multi-scale feature fusion: The FPN (Feature Pyramid Network) structure is used to fuse feature maps of different scales in order to capture target objects of different sizes.

[0104] Mask predictor: Employs a DETR (Detection Transformer) structure to predict the class and mask of each target object.

[0105] Training of multimodal large model Mask2Former:

[0106] Dataset: Mask2Former was pre-trained using the ADE20K dataset (a publicly available scene parsing dataset released by the Computer Science and Artificial Intelligence Laboratory (CSAIL) at MIT). The ADE20K dataset contains 150 semantic categories, covering a variety of indoor and outdoor scenes, and can provide rich semantic information for the model.

[0107] Loss functions: Cross-entropy loss function is used for semantic segmentation tasks, Hungarian loss function is used for object detection tasks, and Dice loss function is used for instance segmentation tasks.

[0108] Optimizer: The AdamW optimizer is used and trained using a cosine learning rate schedule.

[0109] 4) Dynamically update the snow environment knowledge graph based on the environmental semantic data package; the snow environment knowledge graph update includes the following steps:

[0110] 4.1) Modeling the snow environment knowledge graph using a graph convolutional network; converting the multimodal feature vector f multi As input to a graph neural network (GNN), it learns multimodal information and knowledge graphs. The invention establishes relationships between nodes and updates the knowledge graph using these learned relationships. It employs a Graph Convolutional Network (GCN) as the basic model for GNNs, which learns node feature representations by iteratively aggregating information from neighboring nodes. Specifically, this includes:

[0111] ① Knowledge graph Each entity e in i Initialize as a feature vector You can use pre-trained word vectors or random initialization.

[0112] ② Utilizing GCN for knowledge graphs Perform multi-layer convolution operations, with the following update rules for each layer:

[0113]

[0114] in, Represents node e i The feature vector of the l-th layer, Represents node e i The set of neighboring nodes, W (l) and b (l) Let represent the weight matrix and bias vector of the l-th layer, respectively, and σ(·) represent the activation function, such as the ReLU function.

[0115] ③ The output of the last layer of the GCN is used as the final feature representation of the entity, and these feature representations are used for knowledge updates. For example, GNNs can be used to predict missing relationships in a knowledge graph, or to update the attribute values ​​of entities based on multimodal information.

[0116] 4.2) Input the environmental semantic data package output by the multimodal large model into the graph convolutional network to update the snow environment knowledge graph.

[0117] 5) Estimate the pose of each robot in Riemann space based on the slip model, environmental data, and snow environment knowledge graph; the estimation of robot pose in Riemann space includes the following steps:

[0118] 5.1) Incorporate the slip factor into the robot's kinematic model. The robot's slip kinematic model is as follows:

[0119]

[0120] Among them, T k The pose of the robot at time k is represented by T using a Lie algebra. k =[R k ,t k ], where R k Let t represent the rotation matrix. kLet ΔT{k-1,k} represent the translation vector. ΔT{k-1,k} represents the pose change calculated from time k-1 to time k from the odometer data. T represents the composition operation of pose. slip =[R slip , t slip ] represents the pose deviation caused by slippage, R slip Let t be a rotation matrix. slip The translation vector is estimated by analyzing the robot's wheel speedometer and IMU data.

[0121] 5.2) Based on the robot's sliding kinematics model, the robot's sliding state is estimated using an extended Kalman filter (EKF), where the EKF state vector can be expressed as:

[0122] x = [t] k v k b a b g s k ] T

[0123] Among them, t k Let v be the displacement of the robot at time k. k For the robot's speed, b a and b g The zero bias of the accelerometer and gyroscope, respectively, s k The robot's slip state at time k can be represented by the slip ratio or slip angle. The observation equations of EKF can be designed based on the robot's sensor data, such as wheel speedometer data, IMU data, and GPS data.

[0124] EKF predicts the state at time k from the state at time k-1 based on the state transition equation, thereby predicting and estimating the robot's sliding state s in real time. k .

[0125] 5.3) Based on the robot's sliding kinematics model and the robot's sliding state estimated by the extended Kalman filter, the robot's pose state is estimated using a left-invariant extended Kalman filter (LAEKF), and a knowledge graph is introduced for constraints to optimize the pose estimation results. The state vector of the LAEKF can be represented as:

[0126] x = [T] k v k b a b g s k ] T

[0127] Among them, T k Let v be the robot's pose at time k.k For the robot's speed, b a and b g The zero bias of the accelerometer and gyroscope, respectively, s k The robot's slip state at time k can be represented by slip ratio or slip angle.

[0128] Regarding knowledge graph constraints:

[0129] knowledge graph The environmental constraint information provided, such as passable areas and obstacle information, is incorporated into the LAEKF algorithm as prior information or part of the optimization objective function, further improving the accuracy and robustness of pose estimation.

[0130] If the knowledge graph contains a semantic map of the environment, this semantic map can be converted into information about traversable regions and obstacles, and used as prior information for LAEKF. Specifically, the probability of traversable regions can be set to a higher value, and the probability of obstacle regions can be set to a lower value. These probability values ​​are then incorporated into the LAEKF state update equation to constrain the robot's pose estimation results.

[0131] 6) Integrating the historical movement trajectories of dynamic targets, environmental semantic data packets output by multimodal large models, and snow environment knowledge graph information, a trajectory prediction model is used to predict the trajectories of dynamic targets in the environment. The trajectory prediction model for dynamic targets in the environment includes the following steps:

[0132] 6.1) A trajectory prediction multimodal model (Trajectory Prediction Transformer, TPT) is used to fuse the historical trajectory of the dynamic target, the environmental semantic data package output by the multimodal large model, and the snow environment knowledge graph information to output fused features; specifically: first, the historical trajectory information of the dynamic target H={p{tn}……p{t-1}} and the multimodal feature vector f multi and knowledge graph information f KG These are encoded as vector representations. Recurrent Neural Networks (RNNs) can be used to encode the historical trajectory information of dynamic targets, and Multilayer Perceptrons (MLPs) can be used to encode the multimodal feature vectors f. multi and knowledge graph information f KG Encode the information. Among them, the knowledge graph information f... KG This is how it's generated: First, based on the robot's current position and orientation, in the knowledge graph... Relevant entities (such as nearby "snow slope" or "ice surface") are retrieved from the graph; then, a graph neural network (such as GCN) is used to encode and aggregate the features of these entities and their neighboring nodes, ultimately generating a feature vector f that can represent the context of the current scene knowledge graph. KG Then, the encoded vector is input into the TPT model, which employs a multi-head attention mechanism to learn the interaction relationships between different modalities. Finally, the output of the TPT model is used as the final fused feature f. fusion .

[0133] 6.2) Fusing features f fusion The input is fed into a graph neural network to predict the trajectory of dynamic targets in the environment. The graph neural network employs the Graph Networks for Trajectory Prediction (GNN-TP) model. The GNN-TP model models the interaction between environmental information and the target as a graph and uses the graph neural network for trajectory prediction, specifically including:

[0134] ① Graph Construction: Obstacles, other robots, target objects, etc. in the environment are represented as nodes in the graph, and the spatial and interaction relationships between them are represented as edges in the graph. For example, a distance function can be used to define spatial relationships, and a social force model can be used to define interaction relationships.

[0135] ② Feature encoding: Learn the feature representations of nodes and edges in the graph using Graph Convolutional Network (GCN).

[0136] ③ Trajectory generation: Based on the graph structure and node features at the current moment, a recurrent neural network (RNN) is used to predict the future trajectory of the target object.

[0137] The GNN-TP model is trained using the publicly available nuScenes dataset. The nuScenes dataset contains rich urban road scene data, including trajectory information for various objects such as vehicles, pedestrians, and bicycles, which can effectively train the trajectory prediction model. The GNN-TP model uses the Smooth L1 loss function to calculate the error between the predicted trajectory and the true trajectory. The Adam optimizer is used, and learning rate decay is employed during training.

[0138] 7) Each robot constructs a local semantic dynamic map, including a static environment map, a dynamic object map, a semantic map, and a risk map, based on its pose estimation results, dynamic target trajectory prediction results, environmental semantic data packets output by the multimodal large model, and the snow environment knowledge graph.

[0139] Static environment map: Represents the occupancy information of relatively stationary objects in the environment, such as terrain, buildings, trees, etc. It can be represented by occupancy grid map, point cloud map, triangular grid map, etc.

[0140] Dynamic object map: Represents the state information of dynamic objects, such as position, velocity, shape, predicted trajectory, etc., and can be represented using particle filters, Kalman filters, etc.

[0141] Semantic maps represent semantic information about the environment, such as the category, attributes, and functions of objects. They can be represented using semantic tags, semantic networks, and other methods.

[0142] Risk map: Represents potential risk areas in the environment, such as avalanche-prone areas, crevasse areas, etc., and can be represented using probability maps, cost maps, etc.

[0143] The semantic map is constructed based on probabilistic fusion, specifically including:

[0144] Probability representation: For each robot r i The semantic segmentation result can be represented as a probability distribution P i (c,x), where c represents the semantic category and x represents the location on the map.

[0145] Probabilistic fusion: Probabilistic fusion can be performed using Dempster-Shafer evidence theory, treating each robot's judgment that a map location x belongs to a certain semantic category c as an evidence source. The formula for probabilistic fusion using Dempster-Shafer evidence theory is as follows:

[0146]

[0147] Here, A, B, and C are subsets of propositions in the identification framework. For example, proposition A could be the hypothesis that "location x belongs to category 'snow slope'". m1(B) and m2(C) represent the basic probability assignments (BPA) for propositions B and C from two different sources of evidence (e.g., robots r1 and r2), also known as confidence levels. It represents the new level of confidence in proposition A after the fusion of two sources of evidence. ∑ B∩C＝A This represents the combination of all propositions B and C whose intersection is A. K is the conflict coefficient, representing the degree of conflict between two sources of evidence, calculated using the formula:

[0148] The dynamic object map is updated based on the dynamic target trajectory prediction results, improving the map's real-time performance and predictive capabilities, specifically including:

[0149] ① Trajectory projection: Projecting the predicted trajectory of the target object onto a dynamic object map.

[0150] ② Map Update: Update the occupancy probability or target status information of the corresponding grid in the map based on the projection results.

[0151] The risk map is updated through snow environment knowledge graph reasoning and environmental semantic data packages, providing a more reliable guarantee for the robot's safe navigation, specifically including:

[0152] Risk reasoning: Using rules in the knowledge graph to infer potential risk areas, such as inferring the avalanche risk level based on information such as the slope of the snow slope, snow thickness, and temperature.

[0153] Risk assessment: Determine whether the target object will enter the risk area based on its predicted trajectory, and update the risk level of the corresponding area accordingly.

[0154] 8) Each robot shares local semantic dynamic maps and pose information with other robots via a wireless network. A distributed optimization algorithm based on average consensus is used, combined with loop closure detection of environmental semantic data packets and constraints from a snow environment knowledge graph, to iteratively optimize poses and maps, resulting in a globally consistent semantic dynamic map. Specifically:

[0155] ① Distributed Optimization Framework: This framework allows each robot to iteratively update its pose estimation and local map by communicating only with its neighboring robots, ultimately achieving consistency in the pose and map information of all robots and constructing a globally consistent semantic dynamic map. Specifically, this invention employs an algorithm based on average consensus to implement distributed optimization. The core idea of ​​this algorithm is that each robot corrects its own state based on the state information of its neighboring robots during each iteration, eventually leading to a convergence of states among all robots.

[0156] The iterative update rules for the average consensus algorithm are as follows:

[0157]

[0158] Where, x i x(t) represents the state vector of robot i at time t, which includes the robot's pose estimate (represented using Lie algebras) and local map information. i (t+1) represents the updated state vector of robot i at time t+1. ij The weighting coefficient between robots i and j is typically set to... That is, the reciprocal of the number of neighboring robots, to ensure the stability of information fusion. The set of neighboring robots of robot i can be determined by communication range or a predefined topology.

[0159] During each iteration, each robot performs the following operations:

[0160] 1) Broadcast its own state: broadcast its current state vector x i (t) Broadcast to the neighboring robot.

[0161] 2) Receive neighbor state: Receive the state vector x from the neighboring robot. j (t).

[0162] 3) State Update: Based on the received neighbor state information, update its own state vector using the iterative update rules of the average consensus algorithm described above, to obtain x. i (t+1).

[0163] By iteratively executing the above steps, the state vectors of all robots will gradually converge, ultimately achieving globally consistent pose estimation and map construction. A distributed optimization framework addresses the limitations of traditional centralized collaborative optimization methods in snowy environments, such as limited communication bandwidth and high computational resource consumption.

[0164] ② Loop Closure Detection: Utilizing the semantic segmentation results of a multimodal large model, similar locations in the environment are identified, and loop closure constraints are established to improve the accuracy and efficiency of collaborative optimization. Specifically:

[0165] 1) Feature extraction: The semantic segmentation results are converted into a bag-of-words model, and the weight of each word is calculated using the term frequency-inverse document frequency (TF-IDF) algorithm.

[0166] 2) Similarity calculation: Loop closure detection is performed using the bag-of-words model similarity, such as cosine similarity or Jaccard similarity.

[0167] ③ Snow environment knowledge graph constraints: The knowledge graph... Prior information, such as passable areas and obstacle information, is incorporated into the collaborative optimization process to improve its accuracy and efficiency. Specifically, this includes:

[0168] 1) Prior Constraints: Knowledge graph information is incorporated as prior information into the pose graph optimization objective function. For example, if two robots observe the same landmark object, a constraint relationship between the poses of the two robots can be established based on the description information of the landmark object in the knowledge graph, and this constraint relationship can be incorporated into the pose graph optimization objective function.

[0169] 2) Consistency Constraints: Consistency constraints are applied using knowledge graphs to ensure that different robots have consistent semantic labels for the same object. For example, if two robots observe the same object, they can be constrained to have the same semantic label for that object, and this constraint can be added to the pose graph optimization objective function.

[0170] Example 1

[0171] To verify the effectiveness of the multi-robot cooperative SLAM method for snowy environments proposed in this invention, a multi-robot cooperative detection experiment was conducted in a real snowy environment, and a comparative analysis was performed with existing cooperative SLAM methods.

[0172] 1) Experimental platform and environment

[0173] Experimental platform: Three domestically produced ground robot platforms equipped with the same sensors were used. Each robot was equipped with sensors such as LiDAR, millimeter-wave radar, thermal imaging camera, visible light camera and IMU, and was equipped with Ubuntu operating system and ROS (Robot Operating System) platform to run the cooperative SLAM algorithm proposed in this invention.

[0174] Experimental environment: The experiment was conducted at an ice and snow test site in northern China. This area has typical snow environment characteristics, including snow cover, ice surface, undulating terrain, etc., which can fully test the performance of the algorithm.

[0175] 2) Experimental Design and Evaluation Indicators

[0176] Experimental scheme: Three robots will work in formation to conduct collaborative exploration within the experimental area, covering an area of ​​approximately 2 square kilometers, and complete the specified task objectives, such as: reaching a designated location, drawing an environmental map, and identifying specific targets.

[0177] Evaluation metrics: The following metrics are used to evaluate the algorithm performance:

[0178] a. Positioning accuracy: Using high-precision RTK-GPS as the ground truth, the absolute trajectory error (ATE) and relative pose error (RPE) of each robot are calculated and statistically analyzed.

[0179] b. Map accuracy: Manually assess the consistency, completeness, and amount of detailed information of the map, such as checking for issues like overlap, missing parts, and distortion, as well as whether the map contains important terrain features, landmarks, etc.

[0180] c. System robustness: Count the number of times location loss and map building failure occurred during the experiment, and analyze the reasons.

[0181] d. Collaborative efficiency: Analyze indicators such as data transmission volume, computation time, and map fusion time between robots to evaluate the collaborative efficiency of the system.

[0182] 3) Experimental results and analysis: Based on the experimental platform and environment constructed above, the present invention was compared with three existing technologies, and the results are shown in the table below;

[0183]

[0184] Analyze the results in the table above:

[0185] a. Positioning accuracy: The method of this invention is significantly superior to the other three methods in terms of positioning accuracy, which is due to the following reasons:

[0186] ① It integrates information from multiple sensors, effectively overcoming the limitations of a single sensor in snowy environments.

[0187] ② The Riemann space optimization algorithm improves the accuracy and robustness of pose estimation in complex terrain.

[0188] ③ The introduction of knowledge graphs provides prior constraints for pose estimation, further improving positioning accuracy.

[0189] b. Map accuracy: The map constructed by the method of this invention has better consistency, completeness, and detailed information, which is due to:

[0190] ① Multi-layer semantic dynamic maps can represent complex environmental information more comprehensively and precisely.

[0191] ②Loop closure detection based on semantic information improves the global consistency of the map.

[0192] ③ Multi-robot collaborative exploration increases environmental observation information, making the constructed map more complete.

[0193] c. System robustness: The method of this invention only experienced one location loss during the experiment, and there were no map building failures. This indicates that the method has high robustness to snowy environments and can effectively cope with environmental changes and sensor noise interference.

[0194] d. Collaborative efficiency: The method of this invention is superior to other methods in terms of data transmission volume and map fusion time, which shows that the method has high collaborative efficiency while ensuring accuracy and robustness.

[0195] Experimental results show that the multi-robot cooperative SLAM method for snow environments proposed in this invention, based on knowledge graphs, multimodal large models, and Riemann space optimization, can effectively solve the challenges faced by multi-robot cooperative exploration in snow environments. It outperforms existing methods in terms of positioning accuracy, map accuracy, system robustness, and cooperative efficiency. It can effectively solve the problems existing in the current technology and significantly improve the robot's perception, localization, mapping, and navigation capabilities in snow environments. It has important academic value and broad application prospects, and has extremely high application value in fields such as polar scientific research, snow disaster relief, and ice and snow sports.

[0196] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications made to the present invention by those skilled in the art without departing from the spirit of the present invention shall fall within the protection scope of the present invention.

Claims

1. A multi-robot cooperative SLAM method for snowy environments, characterized in that: Includes the following steps: 1) Deploy a multi-robot system consisting of multiple robots, each robot loading a pre-built snow environment knowledge graph; 2) Each robot collects environmental data in real time through a multi-source sensor system and preprocesses the collected environmental data. 3) Perform semantic segmentation, object recognition, snow attribute prediction, and visibility estimation on the preprocessed environmental data using a multimodal large model, and output environmental semantic data packets; 4) Dynamically update the snow environment knowledge graph based on the environmental semantic data package; 5) Estimate the pose of each robot in Riemann space based on the slip model, environmental data, and snow environment knowledge graph; 6) By integrating the historical movement trajectory of dynamic targets, the environmental semantic data package output by the multimodal large model, and the snow environment knowledge graph information, a trajectory prediction model is used to predict the trajectory of dynamic targets in the environment. 7) Each robot constructs a local semantic dynamic map, including a static environment map, a dynamic object map, a semantic map, and a risk map, based on its pose estimation results, dynamic target trajectory prediction results, environmental semantic data packets output by the multimodal large model, and the snow environment knowledge graph. 8) Each robot shares local semantic dynamic map and pose information with other robots through a wireless network. A distributed optimization algorithm based on average consensus is adopted, combined with loop closure detection of environmental semantic data packets and snow environment knowledge graph constraints, to iteratively optimize pose and map, and obtain a globally consistent semantic dynamic map.

2. The method according to claim 1, characterized in that: The construction of the snow environment knowledge graph includes: 1.1) Construct a snow environment knowledge map based on existing knowledge bases, literature, and expert experience related to the snow environment. in, Represents a set of entities. Represents a set of relations. Represents a set of facts; 1.2) The fact set of the snow environment knowledge graph is represented in a structured manner using RDF or attribute graphs; 1.3) Establish static inference rules.

3. The method according to claim 1, characterized in that: The multi-source sensor system includes lidar, millimeter-wave radar, thermal imaging camera, visible light camera, and IMU.

4. The method according to claim 1, characterized in that: The preprocessing includes time synchronization, spatial calibration, data alignment, image fusion, and noise filtering.

5. The method according to claim 3, characterized in that: The data alignment includes the following steps: ① Project the lidar point cloud onto the visible light image plane and the thermal imaging image plane to generate visible light images and thermal imaging images, and generate corresponding depth images based on the visible light images and thermal imaging images; ② The visible light image, thermal imaging image and their corresponding depth image are input into a deformable convolutional network with shared weights. The visible light image feature map, thermal imaging image feature map and depth image feature map are output by the shape and position offset of the convolutional kernel. ③ The visible light image feature map, thermal imaging image feature map, and feature maps of each depth image are respectively input into the attention mechanism module for multimodal feature weighted fusion to obtain the final multimodal feature vector and achieve data alignment.

6. The method according to claim 3, characterized in that: The image fusion includes the following steps: a. Train a generative adversarial network model using existing snow scene image sets; b. Input the environmental data collected by the multi-source sensor system into the generative adversarial network model to generate enhanced images; c. A weighted average method is used to fuse the enhanced image with the environmental data to obtain the final fused image.

7. The method according to claim 3, characterized in that: The noise filtering includes the following steps: A. Employ a multimodal large model to output the current environmental noise level based on environmental data; B. Adjust the filter parameters according to the current ambient noise level; C. Filter the environmental data using the adjusted filter to remove noise and abnormal data.

8. The method according to claim 1, characterized in that: The snow environment knowledge graph update includes the following steps: 4.1) Modeling a knowledge graph of the snow environment using a graph convolutional network; 4.2) Input the environmental semantic data package output by the multimodal large model into the graph convolutional network to update the snow environment knowledge graph.

9. The method according to claim 1, characterized in that: The estimation of robot poses in Riemann space includes the following steps: 5.1) Incorporate the slip factor into the robot's kinematic model. The robot's slip kinematic model is as follows: T k =T{k-1}⊕ΔT{k-1,k}⊕T slip Among them, T k Let ΔT{k-1,k} represent the robot's pose at time k, ΔT{k-1,k} represent the pose change from time k-1 to time k calculated from odometry data, and ⊕ represent the pose composition operation. slip This indicates the pose deviation caused by slippage; 5.2) Based on the robot's sliding kinematics model, the robot's sliding state is estimated using an extended Kalman filter; 5.3) Based on the robot's sliding kinematics model and the robot's sliding state estimated by the extended Kalman filter, the robot's pose state is estimated by the left invariant extended Kalman filter, and a knowledge graph is introduced for constraint to optimize the pose estimation results.

10. The method according to claim 1, characterized in that: The trajectory prediction model is used to predict the trajectory of dynamic targets in the environment, including the following steps: 6.1) A trajectory prediction multimodal model is used to fuse the historical movement trajectory of dynamic targets, the environmental semantic data package output by the multimodal large model, and the snow environment knowledge graph information to output fused features; 6.2) Input the fused features into the graph neural network to predict the trajectory of dynamic targets in the environment.

Images