An uncertainty-aware gated network enhanced cooperative localization method and system

Abstract

The application belongs to the field of multi-agent cooperative positioning, and provides a cooperative positioning method and system enhanced by uncertainty perception gating network, comprising state estimation on preprocessed multi-source sensor data to obtain state estimation results and a covariance matrix; constructing a multi-agent system dynamic graph according to the state estimation results and the covariance matrix of each agent, and extracting uncertainty perception features; performing uncertainty coding and multi-source uncertainty fusion from the dimensions of physical models, measurement quality and time sequence consistency to obtain fused uncertainty embedding; predicting gating coefficients by using the uncertainty perception features and the fused uncertainty embedding to scale the uncertainty perception features to obtain a final correction amount; and updating the state estimation results based on the final correction amount to obtain the cooperative positioning results of the agents. The application significantly improves the accuracy and environmental adaptability by learning and compensating systematic drift and unmodeled dynamics.

Description

Technical Field

[0001] This invention belongs to the field of multi-agent cooperative localization technology, specifically relating to a cooperative localization method and system enhanced by uncertainty-aware gating networks. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Multi-agent cooperative localization (MAL) technology refers to a key technique in which multiple mobile agents, in environments lacking a Global Positioning System (GPS) or with unreliable GPS signals, collaboratively estimate their respective positions and attitudes by fusing their own sensor data with relative observations between agents. This technology forms the basis of multi-agent cooperative operations and is widely used in search and rescue, environmental monitoring, cooperative transportation, and formation flying. In GPS-denied environments, the system faces numerous challenges: individual sensors such as IMUs suffer from cumulative errors; visual odometry is unstable due to factors such as lighting and texture loss; relative observations, such as ultra-wideband ranging, are susceptible to non-line-of-sight propagation interference, leading to measurement deviations; furthermore, multipath effects and magnetic field interference in complex environments are difficult to fully describe through explicit mathematical models. Although multiple agents can form a constraint network through relative observations to achieve information complementarity and error suppression, effectively fusing heterogeneous multi-source information and adaptively adjusting the fusion strategy when sensor quality dynamically changes remains a core challenge that urgently needs to be addressed.

[0004] The existing implementation schemes mainly fall into three categories: First, filtering methods based on physical models, such as extended Kalman filtering, which fuse information through a prediction-update process, but rely on fixed noise parameters and are difficult to adapt to dynamic changes; second, pure data-driven deep learning methods, which use neural networks to regress poses from raw data, but rely on a large amount of labeled data, have limited generalization ability, lack interpretability, and have high computational overhead on embedded platforms; and third, hybrid enhancement methods, which attempt to combine physical models and learning models, such as learning cooperative relationships or dynamically adjusting filter parameters through graph neural networks.

[0005] However, existing technologies still have significant limitations. Pure physics model methods, due to model simplification and fixed parameters, often exhibit systematic drift and weak environmental adaptability; pure data-driven methods require high annotation costs, suffer from insufficient generalization, and their black-box nature makes them unreliable in safety-critical scenarios; while existing hybrid methods often directly output deterministic correction quantities, lacking quantitative assessment of correction reliability and failing to adaptively determine when to trust the physical model or the learning model. In dynamic environments with time-varying sensor quality and multiple heterogeneous uncertainties, this can easily lead to insufficient correction or excessive perturbation, affecting the robustness and consistency of the overall estimation. Therefore, how to achieve dynamic sensor quality perception, quantitative correction confidence, and unified fusion of multi-source uncertainties in multi-agent cooperative localization has become a key issue in improving the system's adaptability and reliability. Summary of the Invention

[0006] To address the aforementioned issues, this invention proposes a cooperative localization method and system enhanced with an uncertainty-aware gating network. By constructing an uncertainty-aware gating network, this invention organically integrates a physical model with a deep learning model, achieving a unified representation and adaptive fusion decision for multi-source heterogeneous uncertainties. This method can dynamically sense changes in sensor quality and adaptively adjust correction strategies. While maintaining the interpretability of the physical model and computational efficiency, it significantly improves the accuracy, robustness, and environmental adaptability of multi-agent cooperative localization in GPS-denied environments.

[0007] According to some embodiments, the first aspect of the present invention provides a cooperative localization method enhanced by an uncertainty-aware gating network, employing the following technical solution:

[0008] A cooperative localization method enhanced by uncertainty-aware gating networks includes:

[0009] Acquire multi-source sensor data for each agent and preprocess it to obtain preprocessed multi-source sensor data;

[0010] The state estimation of the preprocessed multi-source sensor data is performed using a filter to obtain the state estimation results and the covariance matrix;

[0011] Based on the state estimation results and covariance matrix of each agent, a dynamic graph of the multi-agent system is constructed, and uncertainty perception features are extracted.

[0012] Uncertainty is encoded from three dimensions: physical model, measurement quality, and temporal consistency. The resulting physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding are then fused from multiple sources to obtain a fused uncertainty embedding.

[0013] Uncertainty perception features and fused uncertainty embeddings are used to predict uncertainty gating coefficients. The uncertainty perception features are scaled to obtain position correction and rotation correction.

[0014] Based on the position correction and rotation correction, the manifold is updated on the state estimation results to obtain the cooperative localization results of the agents.

[0015] Furthermore, the step of constructing a dynamic graph of the multi-agent system based on the state estimation results and covariance matrix of each agent, and extracting uncertainty-aware features, includes:

[0016] Using agents as nodes, edges are constructed between agents based on the distance measurement connections between them, forming a dynamic graph of a multi-agent system.

[0017] Based on the dynamic graph of a multi-agent system, an uncertainty perception feature is extracted using a graph neural network model.

[0018] Furthermore, the extraction of uncertainty-aware features based on the dynamic graph of the multi-agent system using a graph neural network model includes:

[0019] The original node features of the node are mapped to a 64-dimensional latent space to obtain the initial hidden state of the node.

[0020] The message of generating the edge is generated by fusing the hidden states of the nodes at both ends of each edge with the edge features. The initial hidden state is updated by aggregating the messages of all neighbors of each node in each layer on an average basis. The updated hidden state of the nodes in each layer is obtained by using residual connections.

[0021] Based on the updated hidden states of nodes in each layer, a multi-head attention mechanism is used to learn the global relationships between nodes and obtain the attention hidden states of nodes.

[0022] Based on the gated recurrent unit and the hidden state of the node in the previous time step, the attention hidden state of the node is modeled in a time sequence to obtain the hidden state of the node in the current time step.

[0023] The hidden state of the current node is mapped to obtain the intermediate position correction and the intermediate rotation correction. The hidden state of the current node, as well as the intermediate position correction and the intermediate rotation correction, are used as uncertainty perception features.

[0024] Furthermore, the uncertainty is encoded from three dimensions: physical model, measurement quality, and temporal consistency. The resulting physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding are then fused using multi-source uncertainty fusion to obtain a fused uncertainty embedding, including:

[0025] The uncertainty of the physical model is encoded based on the covariance matrix to obtain the uncertainty embedding of the physical model.

[0026] The measurement quality uncertainty is encoded based on the signal strength, ranging information, geometric precision factor, and NLOS indicator in the edge features to obtain the measurement quality uncertainty embedding;

[0027] Uncertainty encoding of temporal consistency is performed using state jump variables, innovation sequence statistics, and covariance change rate to obtain temporal uncertainty embedding;

[0028] The fused uncertainty embedding is obtained by dynamically weighting and fusing the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding through an attention mechanism.

[0029] Furthermore, the process of predicting uncertainty gating coefficients by utilizing uncertainty-aware features and fusing uncertainty embeddings, and scaling the uncertainty-aware features to obtain position correction and rotation correction amounts, includes:

[0030] The position uncertainty gating coefficient and rotation uncertainty gating coefficient are calculated based on the hidden state of the node at the current moment in the uncertainty perception feature and the fused uncertainty embedding.

[0031] The position and rotation amplitudes are obtained by adjusting the correction amplitude based on the uncertainty of the physical model filter.

[0032] The position correction is obtained by scaling the intermediate position correction based on the position uncertainty gating coefficient and the position amplitude, and the rotation correction is obtained by scaling the intermediate rotation correction based on the rotation uncertainty gating coefficient and the rotation amplitude.

[0033] Furthermore, the manifold update of the state estimation results based on position and rotation corrections to obtain the cooperative localization results of the agents includes:

[0034] Based on the position correction, the manifold is updated on the position in the state estimation result to obtain the predicted global position of the agent;

[0035] Based on the rotation correction, the manifold is updated on the rotation matrix in the state estimation result to obtain the predicted pose of the agent.

[0036] The agent's predicted global position and predicted pose are used as the agent's cooperative localization result.

[0037] According to some embodiments, a second aspect of the present invention provides a cooperative localization system enhanced with an uncertainty-aware gating network, employing the following technical solution:

[0038] A cooperative localization system enhanced with an uncertainty-aware gating network, comprising:

[0039] The data acquisition and processing module is configured to acquire multi-source sensor data from each intelligent agent and preprocess it to obtain preprocessed multi-source sensor data.

[0040] The state estimation module is configured to use filters to perform state estimation on preprocessed multi-source sensor data, and obtain the state estimation results and covariance matrix.

[0041] The uncertainty perception extraction module is configured to construct a dynamic graph of the multi-agent system based on the state estimation results and covariance matrix of each agent, and extract uncertainty perception features.

[0042] The uncertainty fusion module is configured to encode uncertainty from three dimensions: physical model, measurement quality, and temporal consistency. It then performs multi-source uncertainty fusion on the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding obtained after uncertainty encoding to obtain the fused uncertainty embedding.

[0043] The uncertainty scaling module is configured to use uncertainty-aware features and fused uncertainty embeddings to predict uncertainty gating coefficients, scale the uncertainty-aware features, and obtain position correction and rotation correction amounts.

[0044] The cooperative localization module is configured to update the manifold based on the position correction and rotation correction to obtain the cooperative localization result of the agent.

[0045] According to some embodiments, a third aspect of the present invention provides a computer-readable storage medium.

[0046] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of a cooperative localization method enhanced by an uncertainty-aware gating network as described in the first embodiment above.

[0047] According to some embodiments, a fourth aspect of the present invention provides a computer device.

[0048] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps in the cooperative localization method enhanced by an uncertainty-aware gating network as described in the first embodiment above.

[0049] According to some embodiments, a fifth aspect of the present invention provides a computer program product or computer program.

[0050] A computer program product or computer program includes computer instructions stored in a computer-readable storage medium, wherein a processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the steps in a cooperative localization method enhanced by an uncertainty-aware gating network as described in the first embodiment above.

[0051] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0052] This invention achieves a unified representation and effective integration of multi-source heterogeneous uncertainties by uniformly encoding the physical model covariance, multiple measurement quality indicators, and time series consistency indicators into a latent space, and then using an attention mechanism for dynamic weighted fusion. This method overcomes the problems of semantic and dimensional mismatch and difficulty in direct fusion of uncertainties from different sources in traditional methods, significantly improving the system's comprehensive judgment ability on information reliability in complex dynamic environments, and providing a high-quality uncertainty embedding representation for subsequent decision-making.

[0053] The adaptive gating decision-making method proposed in this invention can learn and output a gating coefficient between 0 and 1 in real time based on the fused uncertainty embedding. This coefficient acts as a soft switch, adaptively deciding when to primarily trust the predictions of the physical model and when to introduce corrections from the learned model. This enables the system to no longer blindly apply corrections but to possess context-aware decision-making capabilities, fundamentally solving the problem of missing quantification of correction confidence. It avoids unnecessary perturbations when the physical model is reliable and decisively introduces learned corrections when the model fails, enhancing the intelligence and reliability of the system.

[0054] This invention presents a covariance-based uncertainty-aware correction scaling method that dynamically adjusts the magnitude of the correction amount in the learning model's output based on the uncertainty (covariance) estimated from the current state of the physical model. When the physical model's inherent uncertainty is high, bolder corrections are allowed; when its uncertainty is low, corrections are suppressed. This method, combined with a curriculum learning strategy, constrains the correction magnitude in the early stages of training and gradually loosens it as training progresses, effectively ensuring the stability of the training process and avoiding the disruption of the physical model's baseline by random corrections in the early stages of training, enabling the hybrid system to converge smoothly and efficiently.

[0055] The gating coefficient uncertainty calibration loss function designed in this invention defines ideal gating coefficients based on estimation errors and uses binary cross-entropy loss to supervise and calibrate the actual learned gating coefficients, ensuring that the gating decision mechanism can be effectively trained and optimized. This mechanism closely links the output of the gating network with actual performance improvement, making the decision of when to trust learnable and goal-oriented, further improving the decision quality and overall performance consistency of the entire adaptive reinforcement system. Attached Figure Description

[0056] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0057] Figure 1 This is a flowchart of a collaborative localization method enhanced by an uncertainty-aware gating network according to an embodiment of the present invention;

[0058] Figure 2 This is a performance comparison chart between the method of the present invention and the pure physics model method in the embodiments of the present invention;

[0059] Figure 3 This is a performance comparison chart between the method of the present invention and the pure deep learning method in the embodiments of the present invention. Detailed Implementation

[0060] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0061] It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0062] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0063] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0064] Example 1

[0065] like Figure 1As shown, this embodiment provides a cooperative localization method enhanced by an uncertainty-aware gating network. This embodiment uses the application of this method to a server as an example for illustration. It is understood that this method can also be applied to terminals, and can also be applied to systems including terminals, servers, and other components, and is implemented through interaction between the terminal and the server. The server can be an independent physical server, a server cluster composed of multiple physical servers, or a distributed system. It can also be a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network servers, cloud communication, middleware services, domain name services, CDN security services, and big data and artificial intelligence platforms. The terminal can be a smartphone, tablet, laptop, desktop computer, smart speaker, smartwatch, etc., but is not limited to these. The terminal and server can be directly or indirectly connected via wired or wireless communication, which is not limited herein. In this embodiment, the method includes the following steps:

[0066] Step S1: Acquire multi-source sensor data for each agent and preprocess it to obtain preprocessed multi-source sensor data;

[0067] The multi-source sensor data includes:

[0068] Inertial Measurement Unit (IMU) data: including data from a three-axis accelerometer and a three-axis gyroscope, with a sampling frequency of 100Hz, used to capture the motion dynamics information of the intelligent agent;

[0069] Ultra-wideband (UWB) ranging data: relative distance measurement between agents, with a sampling frequency of 10Hz, providing cooperative constraint information;

[0070] Visual odometry (VO) data (optional): provides an estimate of relative pose change;

[0071] Magnetometer data (optional): Provides directional reference information;

[0072] Barometer data (optional): Provides altitude information.

[0073] The preprocessing process includes: data timestamp alignment, outlier removal, coordinate system transformation, and sensor calibration compensation. The preprocessed multi-source sensor data serves as input for subsequent state estimation.

[0074] Step S2: Use filters to perform state estimation on the preprocessed multi-source sensor data to obtain the state estimation results and covariance matrix;

[0075] This step uses the Error-State Kalman Filter (ESKF) as the baseline of the physical model and performs state estimation independently for each agent. Specifically:

[0076] Each agent is equipped with an independent sensor suite and runs an independent ESKF filter; using preprocessed multi-source sensor data from each agent as input, state estimation is performed using the ESKF filter to obtain the state of each agent. State estimation results and the corresponding covariance matrix The formula is as follows:

[0077] (1);

[0078] in, It is an intelligent agent Location, It is an intelligent agent speed, It is an intelligent agent Rotation matrix (attitude). It is an intelligent agent The covariance matrix quantifies the uncertainty of the current state estimate, providing a basis for subsequent uncertainty perception.

[0079] It should be noted that the traditional ESKF model is used in this embodiment. ESKF is a standard traditional filtering method that fuses multi-source sensor data through a prediction-update mechanism. The specific process of its state estimation is consistent with the process in the prior art, and will not be described in detail here.

[0080] Step S3: Based on the state estimation results and covariance matrix of each agent, construct a dynamic graph of the multi-agent system and extract uncertainty perception features;

[0081] Step S3.1: Using agents as nodes, construct edges between agents based on the distance measurement connection between them to form a dynamic graph of the multi-agent system;

[0082] Each Modeling a multi-agent system at any given time as a dynamic graph ,by The intelligent agent at each moment acts as a node. ,by The distance measurement information between agents at any given time is used as edges, resulting in the edge set. , Represents intelligent agents and intelligent agents The edge between, Indicate each Time-based intelligent agent and intelligent agents The actual distance observation values ​​between them.

[0083] Among them, node features It integrates the state estimate and uncertainty information output by the physical model, including the normalized position. ,speed Rotation (Li algebra form) Uncertainties in position, velocity, and rotation (logarithm of the diagonal elements of the covariance), as well as scalar statistics such as velocity scalar, rotation angle, and total uncertainty. All features are normalized or amplitude-limited to ensure training stability.

[0084] Edge features It encodes ranging measurements and related quality information, including relative direction unit vectors. Predicted distance Actual measured distance Normalized new information NLOS indicator (1 if non-line-of-sight propagation is detected, 0 otherwise, provided by the physical model), estimated NLOS bias, and average position uncertainty of the two end nodes.

[0085] Step S3.2: Based on the dynamic graph of the multi-agent system, extract uncertainty perception features using a graph neural network model;

[0086] The Graph Neural Network (GNN) model consists of a node encoder (Multilayer Perceptron MLP), an edge convolutional layer (Multilayer Perceptron MLP), a multi-head attention layer, a gated recurrent unit (GRU), and an adaptive output layer, all connected in sequence. The GNN module uses a message passing framework, and the processing flow is as follows:

[0087] Node encoder: to encode nodes The original node features are mapped to a 64-dimensional latent space through a multilayer perceptron (MLP) to obtain the node features. The initial hidden state is given by the following formula:

[0088] (2);

[0089] in, For nodes The initial node characteristics, For nodes The initial hidden state.

[0090] Edge convolutional layers (two stacked layers): Aggregate neighbor information. For each edge, the hidden states of the nodes at both ends of each edge are fused with the edge features through an MLP to generate the edge message, as shown in the following formula:

[0091] (3);

[0092] in, It is a node and nodes Edge features between them It is the first Layer edge The news.

[0093] Through average aggregation Each node in the layer Update its initial hidden state by receiving messages from all neighbors. And by using residual connection, the first... Layer nodes Updated hidden state The calculation formula is as follows:

[0094] (4);

[0095] in, For nodes The set of neighboring nodes;

[0096] Multi-head attention layer: The multi-head attention mechanism learns the global relationships between nodes, supplementing the shortcomings of local convolution. The multi-head attention mechanism can adopt, but is not limited to, the standard Transformer attention, using two attention heads.

[0097] For each head Calculate the query, key, and value:

[0098] (5);

[0099] (6);

[0100] (7);

[0101] in, For all nodes at the 1st The initial hidden state matrix of the layer, The projection matrix is ​​learnable, and the dimension of each head is . .

[0102] According to each head Given the query, key, and value, calculate the attention weight for each head using the following formula:

[0103] (8);

[0104] Based on the attention weights of each head, multi-head splicing and projection are performed to obtain nodes. attentional latent state :

[0105] (9);

[0106] in, The output projection matrix is ​​a linear transformation matrix of the spliced ​​outputs of the multi-head attention. Its function is to remap the spliced ​​64-dimensional vector to the 64-dimensional latent space.

[0107] GRU (Gated Cyclic Unit): GRU (Gated Cyclic Unit) utilizes historical information to improve timing consistency; based on nodes... The hidden state of attention at the current moment and the previous moment node The hidden states are used for time-series modeling to obtain the current time step. node The hidden state is represented by the following formula:

[0108] (10);

[0109] in, For the current moment The nodes output by the multi-head attention layer The latent state of attention, For the previous moment The hidden state of GRU has a dimension of 64, consistent with other layers.

[0110] The update equation is calculated as follows:

[0111] (11);

[0112] (12);

[0113] (13);

[0114] (14);

[0115] in, To update the door, It updates the weight of the gate. It is the Sigmoid function, with an output range of [0,1]. To reset the door, It resets the weight of the door. It is a candidate hidden state. These are the weights of the candidate hidden states. For element-wise multiplication, For the current moment, This refers to the previous moment.

[0116] Adaptive output layer: current time step node Hidden state The intermediate position correction is obtained by mapping through the adaptive output layer (multilayer perceptron). and rotation intermediate correction amount At the current moment node The hidden states, as well as the intermediate position and intermediate rotation corrections, are used as uncertainty perception features. The fused uncertainty embedding from step S5 is used together with the intermediate correction amount for gating coefficient prediction; the intermediate correction amount is then subjected to gating and uncertainty-aware scaling to obtain the final uncertainty-aware correction amount. .

[0117] Step S4: Uncertainty encoding is performed from three dimensions: physical model, measurement quality, and temporal consistency. Then, multi-source uncertainty fusion is performed on the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding obtained after uncertainty encoding to obtain fused uncertainty embedding.

[0118] This step encodes heterogeneous uncertainty sources into a unified latent space, providing rich judgment criteria for subsequent gating decisions. The physical model here specifically refers to the ESKF model used in step S2. The fusion process encodes and fuses the covariance matrix, measurement quality indicators, and time-series statistical features output by the ESKF. The input features required for uncertainty encoding all originate from the ESKF output of step S2 and the edge features defined earlier: where... , From edge features ; It is calculated from the geometric distribution of the visible nodes at the current moment; , , Intermediate values ​​from the ESKF measurement update step; , These represent the mean and standard deviation of the news sequence within the sliding window.

[0119] Step S4.1: Encode the physical model uncertainty based on the covariance matrix to obtain the physical model uncertainty embedding;

[0120] Calculate the diagonal elements of the covariance matrix. covariance condition number and average uncertainty The calculation is as follows:

[0121] (15);

[0122] (16);

[0123] (17);

[0124] Among them, the covariance condition number Reflecting the anisotropy of the estimate, It is the largest eigenvalue of the covariance matrix. It is the smallest eigenvalue of the covariance matrix. It is the trace operation of a matrix (i.e., the sum of the diagonal elements);

[0125] Uncertainty encoding is performed on the diagonal elements of the covariance, the covariance condition number, and the average uncertainty using a multilayer perceptron to obtain the uncertainty embedding of the physical model. The calculation formula is as follows:

[0126] (18);

[0127] Step S4.2: Encode the measurement quality uncertainty based on the signal strength, ranging innovation, geometric precision factor, and NLOS indicator in the edge features to obtain the measurement quality uncertainty embedding. The calculation formula is as follows:

[0128] (19);

[0129] in, It is an NLOS indicator. It is the geometric precision factor. It's the signal strength. This is new information on ranging.

[0130] Step S4.3: Encode the temporal consistency uncertainty using state jump variables, innovation sequence statistics, and covariance change rate to obtain the temporal uncertainty embedding. The calculation formula is as follows:

[0131] (20);

[0132] in, It is a state jump variable. It is the statistics of new information sequences, It is the rate of change of covariance;

[0133] Step S4.4: Dynamically weight and fuse the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding using an attention mechanism to obtain the fused uncertainty embedding. The calculation formula is as follows:

[0134] (twenty one);

[0135] (twenty two);

[0136] in, It is an embedding of physical model uncertainty Measurement quality uncertainty embedding and temporal uncertainty embedding The weighting coefficients.

[0137] Step S5: Utilize uncertainty-aware features and fused uncertainty embeddings to predict uncertainty gating coefficients. Scale the uncertainty-aware features to obtain position and rotation corrections; specifically:

[0138] Step S5.1: Based on the uncertainty-aware features of the current moment node Hidden states and fusion uncertainty embedding Calculate the gating coefficient for position uncertainty and rotational uncertainty gating coefficient The purpose is to learn when to apply corrections, and the calculation formula is as follows:

[0139] (twenty three);

[0140] in, This is the Sigmoid function, with an output range of [0,1]. It is a linear transformation layer.

[0141] It is understandable that when the gating coefficient is approximately 0, it indicates that the physical model is reliable and the correction of the learning model is suppressed; when the gating coefficient is approximately 1, it indicates that the physical model is invalid and the correction of the learning model is adopted entirely; when the gating coefficient is approximately 0.5, it indicates a compromise between the two, with partial correction.

[0142] Step S5.2: Adjust the correction amplitude according to the uncertainty of the physical model (filter) to obtain the position amplitude. and rotation amplitude The formula is as follows:

[0143] (twenty four);

[0144] (25);

[0145] in, It is a scaling factor. The average position standard deviation output by the physical model. The mean rotational standard deviation, The function limits the range of scaling factors to avoid excessive adjustments.

[0146] It is important to note that a small scaling factor should be used in the early stages of training, and then gradually increased to stabilize the training, as follows:

[0147] (26);

[0148] in, This represents the current number of training steps. This represents the total number of training steps. The initial scaling factor is 0.001, and the final scaling factor is 0.01. The course avoids the disruption of physical model estimation caused by random corrections to the network output in the early stages of training; it gradually increases the task difficulty to improve training stability.

[0149] Specifically, The formula for the average position standard deviation output by the physical model is as follows:

[0150] (27);

[0151] in, It is a sub-block matrix of the corresponding position state in the ESKF posterior covariance matrix;

[0152] The formula for the mean rotational standard deviation is as follows:

[0153] (28);

[0154] in, It is the sub-block matrix corresponding to the rotation state in the ESKF posterior covariance matrix.

[0155] Step S5.3: Scale the intermediate position correction based on the position uncertainty gating coefficient and position amplitude to obtain the position correction; scale the intermediate rotation correction based on the rotation uncertainty gating coefficient and rotation amplitude to obtain the rotation correction. The calculation formula is as follows:

[0156] (29);

[0157] (30);

[0158] in, It is the intermediate correction amount of position in the uncertainty perception feature. It is the rotational intermediate correction amount in the uncertainty perception feature;

[0159] Hyperbolic tangent limiting prevents gradient explosion and ensures numerical stability. Even with a large network output... , By limiting it to the range of [-1,1] and then multiplying it by a small scaling factor, the final correction amount is within a reasonable range.

[0160] Step S6: Based on the position correction and rotation correction, perform manifold update on the state estimation results to obtain the cooperative localization results of the agents.

[0161] The manifold update strategy applies the correction amount output by the GNN to the physical model state, and ensures geometric constraints through the SE(3) manifold operation.

[0162] The correction amount is synthesized and calculated as follows:

[0163] (31);

[0164] The 6 maintenance positives (including translation and rotation) are transformed into transformations on the manifold using the SE(3) exponential mapping, and the rotation matrix is ​​updated using group multiplication to ensure strict geometric constraints. After the update, SO(3) projection is performed to ensure the orthogonality of the rotation matrix. After the manifold update, each agent obtains an enhanced state estimate.

[0165] Based on the position correction, the manifold is updated on the position in the state estimation result to obtain the agent. The predicted global position is calculated as follows:

[0166] (32);

[0167] in, This represents the position in the state estimation results.

[0168] Based on the rotation correction, the manifold is updated on the rotation matrix in the state estimation result to obtain the agent's predicted pose, calculated as follows:

[0169] (33);

[0170] in, This is the rotation matrix in the state estimation result.

[0171] The agent's predicted global position and predicted pose are used as the agent's cooperative localization result.

[0172] It should be noted that the implementation mechanism of multi-agent cooperative localization is as follows:

[0173] Cooperative constraint propagation: Through the dynamic graph structure (step S3), UWB ranging information between agents is propagated in the network through the message passing mechanism of GNN, forming a globally consistent constraint;

[0174] Distributed state update: Each agent independently updates its pose based on its own ESKF estimate, neighbor information, and learned corrections;

[0175] Uncertainty reduction through collaboration: By using relative observation constraints among multiple agents, the overall positioning uncertainty of the system is effectively suppressed. In particular, when the quality of individual sensors deteriorates, high-quality estimations from neighboring agents can be supported by collaborative mechanisms.

[0176] It is understood that the method described in this embodiment processes the training phase as follows:

[0177] Optimize the entire network parameters through end-to-end differentiable training using training data with ground truth labels;

[0178] Gating coefficients are calibrated for loss due to uncertainty. Supervised learning enables it to accurately determine when to trust the physical model or the learning model.

[0179] Course learning scaling factor During training, the value is gradually increased from 0.001 to 0.01 to ensure training stability.

[0180] The end-to-end differential training process includes three types of loss;

[0181] First, the loss of positional accuracy is calculated as follows:

[0182] (34);

[0183] in, It is an intelligent agent The true global position, It is an intelligent agent Predicting global position, It is the Huber loss function, which is more robust to outliers than mean squared error.

[0184] Second, the uncertainty calibration loss is calculated as follows:

[0185] First, define the ideal gating coefficient (post-hoc labeling). The formula is as follows:

[0186] (35);

[0187] in, This represents the actual error. The difference between the position and the true value directly output by ESKF is the physical model error. The temperature coefficient controls the softening degree of the ideal gate coefficient, and is a preset hyperparameter. The larger the value, The closer it gets to a hard 0 / 1 decision.

[0188] Then, based on the ideal gating coefficient, the uncertainty calibration loss is calculated. The formula is as follows:

[0189] (36);

[0190] in, For intelligent agents The gating coefficient, generally referring to the position gating coefficient. With rotational gating coefficient Both share the same calibration loss calculation logic. It represents the total number of agent nodes in the scene. It is the binary cross-entropy loss function, used to measure the difference in distribution between the predicted gating coefficient and the ideal gating coefficient.

[0191] Third, NLOS robustness loss The calculation is as follows:

[0192] (37);

[0193] in, This is the NLOS tolerance margin, a preset hyperparameter used to allow for a certain range of ranging deviations without incurring penalties. It is the set of all edges marked as NLOS in the current dynamic graph of the multi-agent system. It is a single NLOS edge, corresponding to a ranging connection between a pair of agent nodes. It is an intelligent agent With nodes The predicted distance between them is calculated from the predicted location. It is an intelligent agent With nodes The actual distance observation values ​​between them.

[0194] Total loss function:

[0195] (38);

[0196] in, These are the weighting coefficients for the uncertainty calibration loss. These are the weighting coefficients of the NLOS robustness loss. These are the weighting coefficients of the L2 regularization loss, used to control the regularization strength of the model parameters. It is the squared L2 norm of all trainable parameters of the model, i.e., the weight decay term, which is used to prevent overfitting.

[0197] The weight of the uncertainty calibration loss is gradually increased during training, calculated as follows:

[0198] (39);

[0199] in, This represents the current number of training steps. The upper limit of the loss weight is used to calibrate for uncertainty; this is a preset hyperparameter. To warm up the training steps, in The stage weights increase linearly. After holding constant.

[0200] Performance comparison experiments were conducted on the system under standard test scenarios. The test scenario was set up with three agents moving within a 20m × 20m space for 300 seconds, with an IMU sampling frequency of 100Hz, a ranging sampling frequency of 10Hz, and an NLOS ratio of 15%.

[0201] like Figure 2 As shown, compared to the pure physics model, this invention achieves improvements in all key metrics. The pure physics model has a position RMSE of 0.72 meters, while this invention reduces it to 0.45 meters, an improvement of 37.5%. The pure physics model experiences a systematic drift of approximately 0.8 meters after 300 seconds, while this invention reduces it to 0.3 meters, demonstrating a significant drift compensation effect. In scenes with a high NLOS ratio (30%), the pure physics model has a position RMSE of 1.3 meters, while this invention reduces it to 0.8 meters, an improvement of 38%.

[0202] like Figure 3 As shown, this invention also has significant advantages over pure deep learning methods (end-to-end GNN). Pure deep learning methods require 100,000 training samples, while this invention only requires 10,000, reducing data requirements by 90%. Pure deep learning methods experience a 35% performance drop when the number of agents changes, while this invention experiences a drop of less than 15%, improving generalization ability by more than 2 times. Pure deep learning methods have an inference time exceeding 50 milliseconds, while this invention's total time is approximately 4.9 milliseconds, improving real-time performance by more than 10 times.

[0203] The invention demonstrates excellent robustness across various scenarios. In a low-noise-suppression (NLOS) scenario (0% NLOS), the RMSE is 0.42 meters, close to the theoretical lower bound. In a medium NLOS scenario (15% NLOS), the RMSE is 0.45 meters. In a high NLOS scenario (30% NLOS), the RMSE is 0.8 meters. Performance degradation is less than 15% when the number of agents changes (from 3 to 5). Performance degradation is less than 10% when noise levels change (±50%).

[0204] Example 2

[0205] This embodiment provides a cooperative localization system enhanced with an uncertainty-aware gating network, including:

[0206] The data acquisition and processing module is configured to acquire multi-source sensor data from each intelligent agent and preprocess it to obtain preprocessed multi-source sensor data.

[0207] The state estimation module is configured to use filters to perform state estimation on preprocessed multi-source sensor data, and obtain the state estimation results and covariance matrix.

[0208] The uncertainty perception extraction module is configured to construct a dynamic graph of the multi-agent system based on the state estimation results and covariance matrix of each agent, and extract uncertainty perception features.

[0209] The uncertainty fusion module is configured to encode uncertainty from three dimensions: physical model, measurement quality, and temporal consistency. It then performs multi-source uncertainty fusion on the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding obtained after uncertainty encoding to obtain the fused uncertainty embedding.

[0210] The uncertainty scaling module is configured to use uncertainty-aware features and fused uncertainty embeddings to predict uncertainty gating coefficients, scale the uncertainty-aware features, and obtain position correction and rotation correction amounts.

[0211] The cooperative localization module is configured to update the manifold based on the position correction and rotation correction to obtain the cooperative localization result of the agent.

[0212] The examples and application scenarios implemented by the above modules and corresponding steps are the same, but are not limited to the content disclosed in Embodiment 1 above. It should be noted that the above modules, as part of the system, can be executed in a computer system such as a set of computer-executable instructions.

[0213] The descriptions of each embodiment in the above embodiments have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0214] The proposed system can be implemented in other ways. For example, the system embodiments described above are merely illustrative, and the division of modules described above is only a logical functional division. In actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated into another system, or some features may be ignored or not executed.

[0215] Example 3

[0216] This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps in the cooperative localization method enhanced by an uncertainty-aware gating network as described in Embodiment 1 above.

[0217] Example 4

[0218] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps in the cooperative localization method enhanced by an uncertainty-aware gating network as described in Embodiment 1 above.

[0219] Example 5

[0220] This embodiment provides a computer program product or computer program, including computer instructions stored in a computer-readable storage medium. The processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the steps in the uncertainty-aware gating network-enhanced cooperative localization method described in Embodiment 1 above.

[0221] Those skilled in the art will understand that embodiments of the present invention can provide methods, systems, or computer program products. Therefore, the present invention can take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage and optical storage) containing computer-usable program code.

[0222] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0223] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0224] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0225] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0226] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A cooperative localization method enhanced by uncertainty-aware gating networks, characterized in that, include: Acquire multi-source sensor data for each agent and preprocess it to obtain preprocessed multi-source sensor data; The state estimation of the preprocessed multi-source sensor data is performed using a filter to obtain the state estimation results and the covariance matrix; Based on the state estimation results and covariance matrix of each agent, a dynamic graph of the multi-agent system is constructed, and uncertainty perception features are extracted. Uncertainty is encoded from three dimensions: physical model, measurement quality, and temporal consistency. The resulting physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding are then fused from multiple sources to obtain a fused uncertainty embedding. Uncertainty perception features and fused uncertainty embeddings are used to predict uncertainty gating coefficients. The uncertainty perception features are scaled to obtain position correction and rotation correction. Based on the position correction and rotation correction, the manifold is updated on the state estimation results to obtain the cooperative localization results of the agents.

2. The cooperative localization method enhanced by uncertainty-aware gating networks as described in claim 1, characterized in that, The process involves constructing a dynamic graph of the multi-agent system based on the state estimation results and covariance matrix of each agent, and extracting uncertainty-aware features, including: Using agents as nodes, edges are constructed between agents based on the distance measurement connections between them, forming a dynamic graph of a multi-agent system. Based on the dynamic graph of a multi-agent system, an uncertainty perception feature is extracted using a graph neural network model.

3. The cooperative localization method enhanced by uncertainty-aware gating networks as described in claim 2, characterized in that, The dynamic graph of the multi-agent system, which utilizes a graph neural network model to extract uncertainty-aware features, includes: The original node features of the node are mapped to a 64-dimensional latent space to obtain the initial hidden state of the node. The message of generating the edge is generated by fusing the hidden states of the nodes at both ends of each edge with the edge features. The initial hidden state is updated by aggregating the messages of all neighbors of each node in each layer on an average basis. The updated hidden state of the nodes in each layer is obtained by using residual connections. Based on the updated hidden states of nodes in each layer, a multi-head attention mechanism is used to learn the global relationships between nodes and obtain the attention hidden states of nodes. Based on the gated recurrent unit and the hidden state of the node in the previous time step, the attention hidden state of the node is modeled in a time sequence to obtain the hidden state of the node in the current time step. The hidden state of the current node is mapped to obtain the intermediate position correction and the intermediate rotation correction. The hidden state of the current node, as well as the intermediate position correction and the intermediate rotation correction, are used as uncertainty perception features.

4. The cooperative localization method enhanced by uncertainty-aware gating networks as described in claim 1, characterized in that, The uncertainty is encoded from three dimensions: physical model, measurement quality, and temporal consistency. Then, multi-source uncertainty fusion is performed on the resulting physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding to obtain a fused uncertainty embedding, including: The uncertainty of the physical model is encoded based on the covariance matrix to obtain the uncertainty embedding of the physical model. The measurement quality uncertainty is encoded based on the signal strength, ranging information, geometric precision factor, and NLOS indicator in the edge features to obtain the measurement quality uncertainty embedding; Uncertainty encoding of temporal consistency is performed using state jump variables, innovation sequence statistics, and covariance change rate to obtain temporal uncertainty embedding; The fused uncertainty embedding is obtained by dynamically weighting and fusing the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding through an attention mechanism.

5. The cooperative localization method enhanced by an uncertainty-aware gating network as described in claim 1, characterized in that, The process of predicting uncertainty gating coefficients by utilizing uncertainty-aware features and fusing uncertainty embeddings, and scaling the uncertainty-aware features to obtain position and rotation corrections, includes: The position uncertainty gating coefficient and rotation uncertainty gating coefficient are calculated based on the hidden state of the node at the current moment in the uncertainty perception feature and the fused uncertainty embedding. The position and rotation amplitudes are obtained by adjusting the correction amplitude based on the uncertainty of the physical model filter. The position correction is obtained by scaling the intermediate position correction based on the position uncertainty gating coefficient and the position amplitude, and the rotation correction is obtained by scaling the intermediate rotation correction based on the rotation uncertainty gating coefficient and the rotation amplitude.

6. The cooperative localization method enhanced by uncertainty-aware gating networks as described in claim 1, characterized in that, The manifold update of the state estimation results based on position and rotation corrections yields the cooperative localization results of the agents, including: Based on the position correction, the manifold is updated on the position in the state estimation result to obtain the predicted global position of the agent; Based on the rotation correction, the manifold is updated on the rotation matrix in the state estimation result to obtain the predicted pose of the agent. The agent's predicted global position and predicted pose are used as the agent's cooperative localization result.

7. A cooperative positioning system enhanced with an uncertainty-aware gating network, characterized in that, include: The data acquisition and processing module is configured to acquire multi-source sensor data from each intelligent agent and preprocess it to obtain preprocessed multi-source sensor data. The state estimation module is configured to use filters to perform state estimation on preprocessed multi-source sensor data, and obtain the state estimation results and covariance matrix. The uncertainty perception extraction module is configured to construct a dynamic graph of the multi-agent system based on the state estimation results and covariance matrix of each agent, and extract uncertainty perception features. The uncertainty fusion module is configured to encode uncertainty from three dimensions: physical model, measurement quality, and temporal consistency. It then performs multi-source uncertainty fusion on the physical model uncertainty embedding, measurement quality uncertainty embedding, and temporal uncertainty embedding obtained after uncertainty encoding to obtain the fused uncertainty embedding. The uncertainty scaling module is configured to use uncertainty-aware features and fused uncertainty embeddings to predict uncertainty gating coefficients, scale the uncertainty-aware features, and obtain position correction and rotation correction amounts. The cooperative localization module is configured to update the manifold based on the position correction and rotation correction to obtain the cooperative localization result of the agent.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the cooperative localization method enhanced by an uncertainty-aware gating network as described in any one of claims 1-6.

9. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the cooperative localization method enhanced by an uncertainty-aware gating network as described in any one of claims 1-6.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the steps in the cooperative localization method for uncertainty-aware gating network enhancement as described in any one of claims 1-6.

Images