A Physical Information Augmentation Cluster Collaborative Intelligent Positioning Method and System

Abstract

This invention discloses a swarm cooperative intelligent localization method and system with enhanced physical information, relating to the field of swarm control technology. The method includes: obtaining the posterior state estimate and its corresponding covariance matrix of each agent based on inertial measurement data using an extended Kalman filter algorithm; constructing a dynamic spatiotemporal graph based on effective relative position observation information among the agents; obtaining the input feature vector for each agent based on its posterior state estimate and corresponding covariance matrix; inputting the input feature vector and the dynamic spatiotemporal graph into a decoupled dual-head graph neural network model to obtain the global state correction for the entire agent swarm and the local relative state correction for each agent; and calculating the final state estimate for each agent based on its posterior state estimate, local relative state correction, and global state correction. This invention can significantly improve the accuracy and robustness of cooperative localization.

Description

Technical Field

[0001] This invention relates to the field of cluster control technology, and in particular to a cluster collaborative intelligent positioning method and system with physical information enhancement. Background Technology

[0002] As unmanned systems evolve from individual intelligence to swarm intelligence, multi-agent swarms are demonstrating enormous potential in areas such as large-scale environmental monitoring, collaborative search and rescue, and distributed logistics transportation. The foundation for achieving these high-level collaborative tasks lies in each member of the swarm being able to acquire its own and other members' pose (position and attitude) information in real time, accurately, and reliably. In open outdoor environments, Global Navigation Satellite Systems (GNSS) can provide individuals with absolute positioning information. However, in complex scenarios such as indoor environments, urban canyons, underground spaces, forests, or areas with strong electromagnetic interference, where GNSS is denied or severely attenuated, positioning methods relying on GNSS will fail or experience a sharp decline in accuracy.

[0003] To achieve collaborative operation of multi-agent clusters in complex scenarios, various cooperative localization techniques based on multi-source sensor fusion have been proposed. These techniques mainly rely on inertial measurement units (IMUs), ultra-wideband (UWB) radio, visual sensors, and lidar. Through mutual observation and communication among agents, relative constraints are constructed, thereby estimating the global pose. Specific implementation schemes can be broadly divided into two categories: methods based on classical state estimation theory and data-driven deep learning methods.

[0004] Methods based on classical state estimation theory are centered around the Bayesian filtering framework. Among them, the Extended Kalman Filter (EKF) is widely used due to its high computational efficiency and ease of engineering implementation. The EKF fuses the self-motion information from the inertial measurement unit and the relative observation information from other nodes through a recursive "prediction-update" process. Under the assumption of an ideal linear Gaussian system, the EKF is theoretically the optimal solution. Furthermore, to handle strongly nonlinear scenarios, research has proposed the Unscented Kalman Filter (UKF) and Particle Filter (PF). The Unscented Kalman Filter approximates the state distribution through an unscented transformation, while the Particle Filter approximates the posterior probability using a large number of particles, theoretically capable of handling arbitrarily nonlinear problems.

[0005] Data-driven deep learning methods are centered around neural networks. Graph Neural Networks (GNNs) are naturally well-suited for processing graph-structured data with agents as nodes and communication or observation relationships as edges. Existing data-driven deep learning methods primarily utilize GNNs to directly learn relative pose relationships from raw sensor data (such as signal strength and image features) to construct end-to-end localization models. Data-driven deep learning methods can learn complex nonlinear mappings from data without relying on explicit physical models.

[0006] However, the performance of methods based on classical state estimation theory heavily relies on pre-established, precise motion and observation models. These models are essentially simplifications of the real world and struggle to capture all complex nonlinear dynamics and external disturbances. Secondly, the performance of these methods also depends on the precise setting of the process noise covariance matrix and the observation noise covariance matrix. In practical applications, these noise characteristics are time-varying and non-Gaussian, while currently, these covariance matrices are typically set to fixed constants, leading to model mismatch, which in turn causes estimation error accumulation (drift) and even filter divergence. Data-driven deep learning methods suffer from a lack of interpretability due to the "black box" nature of neural networks, making it difficult to guarantee the physical plausibility of the output. Furthermore, neural networks require massive amounts of data with high-precision ground truth labels for training. In the real physical world, acquiring training data is extremely costly, and the model's generalization ability is fragile in scenarios outside the training data distribution, easily producing erroneous outputs that violate physical laws. Summary of the Invention

[0007] To address some or all of the technical problems existing in the prior art, this invention provides a cluster collaborative intelligent positioning method and system with enhanced physical information, which can integrate the advantages of model prior and data-driven approaches, and has the characteristics of high accuracy, strong robustness, good trainability and low data dependence.

[0008] The technical solution of the present invention is as follows:

[0009] Firstly, a physically-enhanced cluster cooperative intelligent localization method is provided, which is applied to an intelligent agent cluster and includes:

[0010] For each agent, based on the agent's inertial measurement data, the extended Kalman filter algorithm is used to obtain the agent's posterior state estimate and its corresponding covariance matrix.

[0011] Based on the effective relative position observation information between each agent, a dynamic spatiotemporal graph is constructed, wherein the agents correspond to the nodes in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to the edges in the dynamic spatiotemporal graph;

[0012] Based on the posterior state estimate of each agent and its corresponding covariance matrix, the input feature vector corresponding to each agent is obtained. The input feature vector corresponding to each agent and the dynamic spatiotemporal graph are input into the pre-trained decoupled dual-head graph neural network model to obtain the global state correction amount for the entire agent cluster and the local relative state correction amount corresponding to each agent output by the decoupled dual-head graph neural network model.

[0013] The final state estimate of each agent is calculated based on the posterior state estimate of each agent, the local relative state correction of each agent, and the global state correction.

[0014] Furthermore, in some embodiments, the acquisition of the agent's posterior state estimate and its corresponding covariance matrix based on the agent's inertial measurement data through an extended Kalman filter algorithm includes:

[0015] Based on the agent's inertial measurement data and the previous time step's posterior state estimate and its corresponding covariance matrix, the agent's prior state estimate and its corresponding covariance matrix are calculated using preset nonlinear kinematic equations and state transition matrices.

[0016] Based on the effective relative position observation information between the agent and other agents, the Kalman gain is calculated using the preset observation model and its Jacobian matrix. The Kalman gain is then used to correct the prior state estimate of the agent and its corresponding covariance matrix, thus obtaining the posterior state estimate of the agent and its corresponding covariance matrix.

[0017] Furthermore, in some implementations, the input feature vector corresponding to the agent is obtained by concatenating the agent's posterior state estimate and a diagonal matrix with the covariance matrix corresponding to the posterior state estimate as its diagonal elements.

[0018] Furthermore, in some embodiments, the decoupled dual-head graph neural network model includes: a graph attention network unit, a recurrent neural network unit, a global correction head, and a relative correction head;

[0019] The graph attention network unit takes the input feature vector corresponding to the agent and the dynamic spatiotemporal graph as input. The graph attention network unit is used to perform spatial information aggregation on the input feature vector and the dynamic spatiotemporal graph based on the graph attention mechanism to obtain spatial representation features and output them.

[0020] The recurrent neural network unit is connected to the graph attention network unit. The recurrent neural network unit is used to fuse the spatial representation features output by the graph attention network unit with the agent's hidden state acquired at the previous time step to obtain an updated agent hidden state and output it.

[0021] The global correction head is connected to the recurrent neural network unit, and the global correction head is used to predict the global state correction amount for the entire agent cluster based on the agent's hidden state output by the recurrent neural network unit.

[0022] The relative correction head is connected to the recurrent neural network unit, and the relative correction head is used to predict the local relative state correction amount corresponding to each agent based on the agent's hidden state output by the recurrent neural network unit.

[0023] Furthermore, in some embodiments, the global correction head includes a graph pooling layer and a first multilayer perceptron connected in sequence. The graph pooling layer is used to aggregate the agent's hidden state output by the recurrent neural network unit into a single graph-level feature vector, and the first multilayer perceptron is used to process the graph-level feature vector to obtain the global state correction amount.

[0024] Furthermore, in some embodiments, the relative correction head includes a second multilayer perceptron, which is used to process the agent hidden state output by the recurrent neural network unit to obtain the local relative state correction amount corresponding to each agent.

[0025] Furthermore, in some embodiments, the decoupled dual-head graph neural network model is trained in the following manner:

[0026] Step 311: Obtain the training data set, which includes: inertial measurement data of each agent at multiple consecutive time steps, effective relative position observation information between agents at multiple consecutive time steps, and the real state of each agent at multiple consecutive time steps.

[0027] Step 312: Select a training dataset;

[0028] Step 313: Starting from the first time step of the currently selected training data, based on the current decoupled dual-head graph neural network model, obtain the posterior state estimate of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent.

[0029] Step 314: Based on the currently selected training data and the posterior state estimates of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent, calculate the preset loss function and use the loss function to update the parameters of the decoupled dual-head graph neural network model.

[0030] Step 315: Select an unused training data point, return to step 313 to continue iterative optimization, until all training data have been traversed, then proceed to step 316.

[0031] Step 316: Determine whether the preset training stop condition has been met. If not, return to step 312 to continue iterative optimization based on the currently updated decoupled dual-headed graph neural network model. If yes, use the currently updated decoupled dual-headed graph neural network model as the decoupled dual-headed graph neural network model that has completed training.

[0032] Furthermore, in some implementations, the loss function is expressed as:

[0033] ;

[0034] in, Represents the loss function. Indicates monitoring losses, This represents the loss due to self-supervised physical constraints. Indicates weight;

[0035] The supervision loss is expressed as:

[0036] ;

[0037] The self-supervised physical constraint loss is expressed as:

[0038] ;

[0039] This indicates the number of consecutive time steps contained in the training data. This represents the total number of agents in the agent cluster. Let represent the final state estimate of the i-th agent at time step t. This represents the true state of the i-th agent at time step t. This represents the set of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the number of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the corrected state of the j-th agent at time step t. This represents the corrected state of the i-th agent at time step t. , , This represents the posterior state estimate of the j-th agent at time step t. This represents the local relative state correction of the j-th agent at time step t. Let represent the posterior state estimate of the i-th agent at time step t. This represents the local relative state correction of the i-th agent at time step t. This represents the effective relative position observation information between the i-th agent and the j-th agent at time step t.

[0040] Furthermore, in some implementations, the final state estimate of the agent is calculated using the following formula:

[0041] ;

[0042] in, This represents the final state estimate of the i-th agent. Let represent the posterior state estimate of the i-th agent. This represents the local relative state correction amount for the i-th agent. This represents the global state correction amount.

[0043] Secondly, a physical information-enhanced cluster cooperative intelligent positioning system is also provided, which is applied to an intelligent agent cluster and includes:

[0044] The data acquisition module is used to acquire inertial measurement data of the intelligent agents and effective relative position observation information between the intelligent agents;

[0045] The posterior state estimation module is used to obtain the posterior state estimate of the agent and its corresponding covariance matrix based on the agent's inertial measurement data through the extended Kalman filter algorithm.

[0046] The dynamic spatiotemporal graph construction module is used to construct a dynamic spatiotemporal graph based on the effective relative position observation information between each agent, wherein the agent corresponds to the node in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to the edge in the dynamic spatiotemporal graph;

[0047] The state correction acquisition module is equipped with a pre-trained decoupled bi-headed graph neural network model, which is used to obtain the input feature vector corresponding to each agent based on the posterior state estimate of each agent and its corresponding covariance matrix. The input feature vector corresponding to each agent and the dynamic spatiotemporal graph are input to the decoupled bi-headed graph neural network model to obtain the global state correction for the entire agent cluster and the local relative state correction for each agent output by the decoupled bi-headed graph neural network model.

[0048] The final state estimation module is used to calculate the final state estimate of each agent based on the posterior state estimate of each agent, the local relative state correction amount corresponding to each agent, and the global state correction amount.

[0049] The main advantages of the technical solution of this invention are as follows:

[0050] The physical information-enhanced cluster collaborative intelligent localization method and system of this invention significantly improves the collaborative localization accuracy and robustness by deeply cascading the traditional extended Kalman filter with a graph neural network model, decoupling the dual-head graph neural network model to learn and compensate for the model mismatch and nonlinear error of the filtering algorithm, and using global information to suppress common drift. By decoupling the traditional extended Kalman filter from the decoupled dual-head graph neural network model, unstable gradient propagation during model training can be avoided, ensuring that the model can be trained and converged efficiently and stably. By constructing a dynamic spatiotemporal graph using effective relative position observation information as a physical constraint, the interpretability and physical rationality of the final result can be ensured, the generalization ability of the model can be improved, and the dependence on training data with high-precision ground truth labels during model training can be reduced. Attached Figure Description

[0051] The accompanying drawings, which are included to provide a further understanding of embodiments of the invention and constitute a part of this invention, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings:

[0052] Figure 1 A flowchart illustrating a physical information-enhanced cluster collaborative intelligent positioning method provided in an embodiment of the present invention;

[0053] Figure 2 This is a schematic diagram of a physical information-enhanced cluster collaborative intelligent positioning system provided in an embodiment of the present invention. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0055] The technical solutions provided by the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0056] refer to Figure 1 In a first aspect, embodiments of the present invention provide a physically enhanced cluster cooperative intelligent localization method, which is applied to an intelligent agent cluster and includes the following steps:

[0057] Step 1: For each agent, based on the agent's inertial measurement data, obtain the agent's posterior state estimate and its corresponding covariance matrix using the extended Kalman filter algorithm.

[0058] Step 2: Based on the effective relative position observation information between each agent, construct a dynamic spatiotemporal graph, where the agents correspond to nodes in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to edges in the dynamic spatiotemporal graph;

[0059] Step 3: Based on the posterior state estimate of each agent and its corresponding covariance matrix, obtain the input feature vector corresponding to each agent. Input the input feature vector corresponding to each agent and the dynamic spatiotemporal graph into the pre-trained decoupled dual-head graph neural network model to obtain the global state correction amount for the entire agent cluster and the local relative state correction amount corresponding to each agent output by the decoupled dual-head graph neural network model.

[0060] Step 4: Calculate the final state estimate of each agent based on the posterior state estimate of each agent, the local relative state correction amount corresponding to each agent, and the global state correction amount.

[0061] In this embodiment of the invention, an agent corresponds to a node in a dynamic spatiotemporal graph. When there is valid relative position observation information between two agents, an edge is created between the two corresponding nodes in the dynamic spatiotemporal graph, and the valid relative position observation information is used as an attribute of the corresponding edge.

[0062] In this embodiment of the invention, the input of the decoupled dual-head graph neural network model is the input feature vector corresponding to each agent and the dynamic spatiotemporal graph, and the output of the decoupled dual-head graph neural network model is a global state correction amount for the entire agent cluster and a local relative state correction amount corresponding to each agent.

[0063] In this embodiment of the invention, the covariance matrix is ​​also the uncertainty information of the state estimation.

[0064] In this embodiment of the invention, the intelligent agent is a drone.

[0065] The physical information-enhanced cluster collaborative intelligent localization method provided in this invention significantly improves the collaborative localization accuracy and robustness by deeply cascading the traditional extended Kalman filter with a graph neural network model, decoupling the dual-head graph neural network model to learn and compensate for the model mismatch and nonlinear error of the filtering algorithm, and using global information to suppress common drift. By decoupling the traditional extended Kalman filter from the decoupled dual-head graph neural network model, unstable gradient propagation during model training can be avoided, ensuring that the model can be trained and converged efficiently and stably. By constructing a dynamic spatiotemporal graph using effective relative position observation information as a physical constraint, the interpretability and physical rationality of the final result can be ensured, the generalization ability of the model can be improved, and the dependence of the model on training data with high-precision ground truth labels during training can be reduced.

[0066] Furthermore, in this embodiment of the invention, the extended Kalman filter algorithm adopts the existing standard extended Kalman filter algorithm.

[0067] In this embodiment of the invention, based on the inertial measurement data of the agent, the posterior state estimate of the agent and its corresponding covariance matrix are obtained through the extended Kalman filter algorithm, specifically including the following steps:

[0068] Step 101: Based on the agent's inertial measurement data and the posterior state estimate of the previous moment and its corresponding covariance matrix, the prior state estimate of the agent and its corresponding covariance matrix are calculated through the preset nonlinear kinematic equations and state transition matrix.

[0069] Step 102: Based on the effective relative position observation information between the agent and other agents, calculate the Kalman gain using the preset observation model and its Jacobian matrix, and use the Kalman gain to correct the prior state estimate of the agent and its corresponding covariance matrix to obtain the posterior state estimate of the agent and its corresponding covariance matrix.

[0070] In this embodiment of the invention, the state of the agent includes: the agent's coordinates in the three-dimensional world coordinate system, the agent's yaw angle, and the zero velocity bias and zero angular velocity bias of the agent's inertial measurement unit, which can be specifically expressed as:

[0071] ;

[0072] in, Indicates the state of the agent. This represents the x-axis coordinate of the intelligent agent in a three-dimensional world coordinate system. This represents the y-axis coordinate of the intelligent agent in a three-dimensional world coordinate system. This represents the z-axis coordinate of the agent in the three-dimensional world coordinate system. Indicates the yaw angle of the agent. This indicates that the velocity of the inertial measurement unit of the intelligent agent is zero bias. This indicates that the angular velocity of the inertial measurement unit of the intelligent agent is zero bias. It represents the set of real numbers, and the superscript T indicates the transpose operation of the matrix.

[0073] In this embodiment of the invention, the inertial measurement data of the intelligent agent includes: the linear velocity and angular velocity measured by the inertial measurement unit of the intelligent agent, which can be specifically expressed as:

[0074] ;

[0075] in, Inertial measurement data representing the intelligent agent, Represents the linear velocity of the agent. This represents the angular velocity of the intelligent agent.

[0076] Based on the existing standard extended Kalman filter algorithm, the prior state estimate of the agent and its corresponding covariance matrix are calculated using the following formula:

[0077] ;

[0078] ;

[0079] in, Let represent the prior state estimate of the agent at time t. Representing the nonlinear kinematic equations, This represents the posterior state estimate of the agent at time t-1. This represents the inertial measurement data of the agent at time t. Let represent the covariance matrix corresponding to the prior state estimate of the agent at time t. This represents the covariance matrix corresponding to the posterior state estimate of the agent at time t-1. Represents the state transition matrix. express The transpose of the matrix, This represents the process noise covariance matrix.

[0080] In this embodiment of the invention, the nonlinear kinematic equations are the same as those used in the existing standard extended Kalman filter algorithm, and will not be elaborated here. According to the extended Kalman filter theory, the state transition matrix is ​​obtained by calculating the Jacobian matrix of the nonlinear kinematic equations at the latest state estimation point.

[0081] In this embodiment of the invention, the process noise covariance matrix is ​​set according to actual needs and is a preset fixed value.

[0082] It should be noted that at the initial time step, the corresponding posterior state estimate of the previous time step and its corresponding covariance matrix are both initialized to zero.

[0083] Based on the existing standard extended Kalman filter algorithm, assuming there is effective relative positional observation information between the i-th agent and the j-th agent, the posterior state estimate of the i-th agent and its corresponding covariance matrix can be calculated by the following formula:

[0084] ;

[0085] ;

[0086] ;

[0087] ;

[0088] ;

[0089] in, This represents the posterior state estimate of the agent at time t. Indicates Kalman gain, This represents the observation residual between the i-th agent and the j-th agent. Let represent the covariance matrix corresponding to the posterior state estimate of the agent at time t. Represents the identity matrix. The Jacobian matrix represents the observation model. express The transpose of the matrix, Represents the observation noise covariance matrix. This represents the effective relative position observation information between the i-th agent and the j-th agent. This represents the predicted relative position observation information between the i-th agent and the j-th agent. Represents the observation model, Let represent the prior state estimate of the i-th agent at time t. This represents the prior state estimate of the j-th agent at time t. This represents the position value in the prior state estimate of the j-th agent at time t. Let represent the position value in the prior state estimate of the i-th agent at time t, and the superscript -1 indicates the inverse operation of the matrix.

[0090] In this embodiment of the invention, the observation noise covariance matrix is ​​set according to actual needs and is a preset fixed value.

[0091] In this embodiment of the invention, by limiting the standard extended Kalman filter algorithm to a fixed, non-differentiable feature extractor, it is decoupled from the subsequent trainable decoupled dual-head graph neural network model. This fundamentally avoids the unstable propagation of gradients in the recursive filtering loop, thereby ensuring that the model can be trained and converged efficiently and stably.

[0092] Furthermore, in this embodiment of the invention, the input feature vector corresponding to the agent is obtained by concatenating the agent's posterior state estimate and a diagonal matrix with the covariance matrix corresponding to the posterior state estimate as its diagonal elements, which can be specifically expressed as:

[0093] ;

[0094] in, This represents the input feature vector corresponding to the i-th agent. Let represent the posterior state estimate of the i-th agent. Let represent the covariance matrix corresponding to the posterior state estimate of the i-th agent. Indicated by A diagonal matrix with diagonal elements. This indicates a splicing operation.

[0095] In this embodiment of the invention, by fusing the state estimation and its corresponding uncertainty information into the input features of the decoupled dual-headed graph neural network model, the decoupled dual-headed graph neural network model can intelligently and collaboratively correct errors, thereby improving positioning accuracy and robustness.

[0096] Furthermore, in this embodiment of the invention, the decoupled dual-head graph neural network model includes: a graph attention network unit, a recurrent neural network unit, a global correction head, and a relative correction head;

[0097] The graph attention network unit takes the input feature vector corresponding to the agent and the dynamic spatiotemporal graph as input. The graph attention network unit is used to aggregate spatial information of the input feature vector and the dynamic spatiotemporal graph based on the graph attention mechanism to obtain spatial representation features and output them.

[0098] The recurrent neural network unit is connected to the graph attention network unit. The recurrent neural network unit is used to fuse the spatial representation features output by the graph attention network unit with the agent's hidden state acquired in the previous time step to obtain the updated agent's hidden state and output it.

[0099] The global correction head is connected to the recurrent neural network unit. The global correction head is used to predict the global state correction amount for the entire agent cluster based on the agent's hidden state output by the recurrent neural network unit.

[0100] The relative correction head is connected to the recurrent neural network unit. The relative correction head is used to predict the local relative state correction amount for each agent based on the hidden state of the agent output by the recurrent neural network unit.

[0101] In this embodiment of the invention, the graph attention network unit employs a graph attention network, such as GATv2. The graph attention network aggregates spatial information from a dynamic spatiotemporal graph. The attention mechanism of the graph attention network enables each agent to dynamically calculate weights and aggregate information based on the state and uncertainty of its neighboring agents, thereby forming a spatial representation feature with context awareness of the local environment.

[0102] In this embodiment of the invention, the recurrent neural network unit adopts a recurrent neural network (GRU). The recurrent neural network fuses information in the temporal dimension based on the spatial representation features output by the graph attention network unit and the agent's hidden state acquired in the previous time step, enabling the model to understand the evolution law and continuity of the agent cluster state, and finally output the updated agent hidden state.

[0103] Each agent has corresponding spatial representation features and agent hidden state.

[0104] In this context, the hidden state of the agent in the previous moment is initialized to zero, meaning that at the initial moment, the hidden state of the agent in the previous moment is zero.

[0105] In this embodiment of the invention, the global correction head includes a graph pooling layer and a first multilayer perceptron connected in sequence. The graph pooling layer is used to aggregate the hidden states of the agent output by the recurrent neural network unit into a single graph-level feature vector. The first multilayer perceptron is used to process the graph-level feature vector to obtain the global state correction amount.

[0106] In this embodiment of the invention, the global correction head is used to correct the collective drift error of the entire agent cluster. The graph pooling layer first performs a graph pooling operation to aggregate the hidden states of all agents into a single graph-level feature vector that represents the overall state of the cluster. Then, the first multilayer perceptron predicts a global state correction amount shared by all agents based on the graph-level feature vector. This global state correction amount aims to treat the entire agent cluster as a rigid body and translate it in the world coordinate system to compensate for the common drift caused by the accumulation of zero bias of the inertial measurement unit.

[0107] In this embodiment of the invention, the relative correction head includes a second multilayer perceptron, which is used to process the hidden state of the agent output by the recurrent neural network unit to obtain the local relative state correction amount corresponding to each agent.

[0108] In this embodiment of the invention, the relative correction head is used to correct local geometric distortions within the agent cluster. The second multilayer perceptron predicts the local relative state correction amount for each agent based on the hidden state of each agent. This local relative state correction amount is intended to fine-tune the position of each agent relative to its neighboring agents to ensure that the internal configuration of the agent cluster is more in line with the geometric constraints of the physical world.

[0109] Furthermore, in this embodiment of the invention, the decoupled dual-head graph neural network model is trained in the following manner:

[0110] Step 311: Obtain the training data set, which includes: inertial measurement data of each agent at multiple consecutive time steps, effective relative position observation information between agents at multiple consecutive time steps, and the real state of each agent at multiple consecutive time steps.

[0111] Step 312: Select a training dataset;

[0112] Step 313: Starting from the first time step of the currently selected training data, based on the current decoupled dual-head graph neural network model, obtain the posterior state estimate of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent.

[0113] Step 314: Based on the currently selected training data and the posterior state estimates of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent, calculate the preset loss function and use the loss function to update the parameters of the decoupled dual-head graph neural network model.

[0114] Step 315: Select an unused training data point, return to step 313 to continue iterative optimization, until all training data have been traversed, then proceed to step 316.

[0115] Step 316: Determine whether the preset training stop condition has been met. If not, return to step 312 to continue iterative optimization based on the currently updated decoupled dual-headed graph neural network model. If yes, use the currently updated decoupled dual-headed graph neural network model as the decoupled dual-headed graph neural network model that has completed training.

[0116] In this embodiment of the invention, at the start of training, the parameters of the decoupled dual-head graph neural network model are initialized parameters, and during the training process, the model parameters are continuously updated and optimized.

[0117] In this embodiment of the invention, the training dataset is obtained from historical data.

[0118] In this embodiment of the invention, in step 313, the posterior state estimate of each agent at each time step in multiple consecutive time steps, the local relative state correction amount of each agent, the global state correction amount, and the final state estimate of each agent are obtained in accordance with the manner described in steps 1-4 above.

[0119] In this embodiment of the invention, the training stop condition is set according to actual needs, for example, it is set to the number of training iterations reaching a set number.

[0120] In this embodiment of the invention, the gradient of the loss function with respect to the parameters of the decoupled dual-headed graph neural network model is calculated, and the parameters of the decoupled dual-headed graph neural network model are updated and optimized using the gradient descent method.

[0121] Furthermore, in this embodiment of the invention, the decoupled dual-head graph neural network model employs the following loss function during training:

[0122] ;

[0123] in, Represents the loss function. Indicates monitoring losses, This represents the loss due to self-supervised physical constraints. Indicates the weight.

[0124] Among them, weight Configure according to actual needs.

[0125] In this embodiment of the invention, the supervision loss is expressed as:

[0126] ;

[0127] in, This indicates the number of consecutive time steps contained in the training data. This represents the total number of agents in the agent cluster. Let represent the final state estimate of the i-th agent at time step t. This represents the true state of the i-th agent at time step t.

[0128] In this embodiment of the invention, the supervisory loss is used to ensure the absolute accuracy of the final state estimation result. The gradient of the supervisory loss is backpropagated to the entire decoupled dual-head graph neural network model. However, since the goal of this loss is to minimize the final global error, this loss is the main driving force guiding the global correction head to learn.

[0129] In this embodiment of the invention, the self-supervised physical constraint loss is expressed as:

[0130] ;

[0131] in, This indicates the number of consecutive time steps contained in the training data. This represents the set of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the number of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the corrected state of the j-th agent at time step t. This represents the corrected state of the i-th agent at time step t. , , This represents the posterior state estimate of the j-th agent at time step t. This represents the local relative state correction of the j-th agent at time step t. Let represent the posterior state estimate of the i-th agent at time step t. This represents the local relative state correction of the i-th agent at time step t. This represents the effective relative position observation information between the i-th agent and the j-th agent at time step t.

[0132] In this embodiment of the invention, the self-supervised physical constraint loss is a self-supervised loss term that does not depend on the truth label. This loss does not act on the final state, but rather on an intermediate state containing local relative state corrections, i.e., a corrected state. The calculation of this corrected state is independent of the global state correction but related to the local relative state correction. The gradient generated by this loss term is primarily backpropagated to the relative correction head. Through this self-supervised physical constraint loss, the learning objective can be decoupled at the loss function level, allowing the relative correction head to focus on learning how to maintain a high degree of self-consistency between the internal geometry and physical observations of the agent cluster, without being interfered with by the global localization task, thereby ensuring both global accuracy and local consistency.

[0133] Furthermore, in this embodiment of the invention, the final state estimate of the agent is calculated using the following formula:

[0134] ;

[0135] in, This represents the final state estimate of the i-th agent. Let represent the posterior state estimate of the i-th agent. This represents the local relative state correction amount for the i-th agent. This represents the global state correction amount.

[0136] In this embodiment of the invention, by superimposing the posterior state estimate, the local relative state correction, and the global state correction as the final state estimate, the accurate separation and coordinated compensation of systematic errors are achieved, thereby significantly improving the overall accuracy and geometric consistency of positioning.

[0137] refer to Figure 2 Secondly, embodiments of the present invention also provide a physically enhanced cluster cooperative intelligent positioning system, which is applied to an intelligent agent cluster and includes:

[0138] The data acquisition module 100 is used to acquire the inertial measurement data of the intelligent agents and the effective relative position observation information between the intelligent agents;

[0139] The posterior state estimation module 200 is used to obtain the posterior state estimate of the agent and its corresponding covariance matrix based on the agent's inertial measurement data through the extended Kalman filter algorithm.

[0140] The dynamic spatiotemporal graph construction module 300 is used to construct a dynamic spatiotemporal graph based on the effective relative position observation information between each agent, wherein the agent corresponds to the node in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to the edge in the dynamic spatiotemporal graph;

[0141] The state correction acquisition module 400 is equipped with a pre-trained decoupled dual-head graph neural network model, which is used to obtain the input feature vector corresponding to each agent based on the posterior state estimate of each agent and its corresponding covariance matrix. The input feature vector corresponding to each agent and the dynamic spatiotemporal graph are input to the decoupled dual-head graph neural network model to obtain the global state correction for the entire agent cluster and the local relative state correction for each agent output by the decoupled dual-head graph neural network model.

[0142] The final state estimation module 500 is used to calculate the final state estimate of each agent based on the posterior state estimate of each agent, the local relative state correction amount corresponding to each agent, and the global state correction amount.

[0143] In this embodiment of the invention, each of the above modules is a device corresponding to the above method steps. The specific working principle and beneficial effects of each module can be found in the above method, and will not be repeated here.

[0144] It should be noted that, in this document, relational terms such as “first” and “second” are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms “comprising,” “including,” or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus.

[0145] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A physical information enhanced swarm cooperative intelligent positioning method, characterized in that, The method is applied to an intelligent agent cluster, and the method includes: For each agent, based on the agent's inertial measurement data, the extended Kalman filter algorithm is used to obtain the agent's posterior state estimate and its corresponding covariance matrix. Based on the effective relative position observation information between each agent, a dynamic spatiotemporal graph is constructed, wherein the agent corresponds to a node in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to an edge in the dynamic spatiotemporal graph; Based on the posterior state estimate of each agent and its corresponding covariance matrix, the input feature vector corresponding to each agent is obtained. The input feature vector corresponding to each agent and the dynamic spatiotemporal graph are input into the pre-trained decoupled dual-head graph neural network model to obtain the global state correction amount for the entire agent cluster and the local relative state correction amount corresponding to each agent output by the decoupled dual-head graph neural network model. The final state estimate of each agent is calculated based on the posterior state estimate of each agent, the local relative state correction of each agent, and the global state correction.

2. The physically information-augmented swarm-cooperative intelligent positioning method according to claim 1, wherein, The agent-based inertial measurement data is used to obtain the agent's posterior state estimate and its corresponding covariance matrix through an extended Kalman filter algorithm, including: Based on the agent's inertial measurement data and the previous time step's posterior state estimate and its corresponding covariance matrix, the agent's prior state estimate and its corresponding covariance matrix are calculated using preset nonlinear kinematic equations and state transition matrices. Based on the effective relative position observation information between the agent and other agents, the Kalman gain is calculated using the preset observation model and its Jacobian matrix. The Kalman gain is then used to correct the prior state estimate of the agent and its corresponding covariance matrix, thus obtaining the posterior state estimate of the agent and its corresponding covariance matrix.

3. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 1, characterized in that, The input feature vector corresponding to the agent is obtained by concatenating the agent's posterior state estimate and a diagonal matrix with the covariance matrix corresponding to the posterior state estimate as the diagonal element.

4. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 1, characterized in that, The decoupled dual-head graph neural network model includes: a graph attention network unit, a recurrent neural network unit, a global correction head, and a relative correction head; The graph attention network unit takes the input feature vector corresponding to the agent and the dynamic spatiotemporal graph as input. The graph attention network unit is used to perform spatial information aggregation on the input feature vector and the dynamic spatiotemporal graph based on the graph attention mechanism to obtain spatial representation features and output them. The recurrent neural network unit is connected to the graph attention network unit. The recurrent neural network unit is used to fuse the spatial representation features output by the graph attention network unit with the agent's hidden state acquired at the previous time step to obtain an updated agent hidden state and output it. The global correction head is connected to the recurrent neural network unit, and the global correction head is used to predict the global state correction amount for the entire agent cluster based on the agent's hidden state output by the recurrent neural network unit. The relative correction head is connected to the recurrent neural network unit, and the relative correction head is used to predict the local relative state correction amount corresponding to each agent based on the agent's hidden state output by the recurrent neural network unit.

5. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 4, characterized in that, The global correction head includes a graph pooling layer and a first multilayer perceptron connected in sequence. The graph pooling layer is used to aggregate the agent's hidden state output by the recurrent neural network unit into a single graph-level feature vector. The first multilayer perceptron is used to process the graph-level feature vector to obtain the global state correction amount.

6. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 4, characterized in that, The relative correction head includes a second multilayer perceptron, which processes the hidden state of the agent output by the recurrent neural network unit to obtain the local relative state correction amount corresponding to each agent.

7. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 1 or 4, characterized in that, The decoupled dual-head graph neural network model is trained in the following way: Step 311: Obtain the training data set, which includes: inertial measurement data of each agent at multiple consecutive time steps, effective relative position observation information between agents at multiple consecutive time steps, and the real state of each agent at multiple consecutive time steps. Step 312: Select a training dataset; Step 313: Starting from the first time step of the currently selected training data, based on the current decoupled dual-head graph neural network model, obtain the posterior state estimate of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent. Step 314: Based on the currently selected training data and the posterior state estimates of each agent at each time step in multiple consecutive time steps, the local relative state correction of each agent, the global state correction, and the final state estimate of each agent, calculate the preset loss function and use the loss function to update the parameters of the decoupled dual-head graph neural network model. Step 315: Select an unused training data point, return to step 313 to continue iterative optimization, until all training data have been traversed, then proceed to step 316. Step 316: Determine whether the preset training stop condition has been met. If not, return to step 312 to continue iterative optimization based on the currently updated decoupled dual-headed graph neural network model. If yes, use the currently updated decoupled dual-headed graph neural network model as the decoupled dual-headed graph neural network model that has completed training.

8. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 7, characterized in that, The loss function is expressed as: ； in, Represents the loss function. Indicates monitoring losses, This represents the loss due to self-supervised physical constraints. Indicates weight; The supervision loss is expressed as: ； The self-supervised physical constraint loss is expressed as: ； This indicates the number of consecutive time steps contained in the training data. This represents the total number of agents in the agent cluster. Let represent the final state estimate of the i-th agent at time step t. This represents the true state of the i-th agent at time step t. This represents the set of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the number of edges in the dynamic spatiotemporal graph acquired at time step t. This represents the corrected state of the j-th agent at time step t. This represents the corrected state of the i-th agent at time step t. , , This represents the posterior state estimate of the j-th agent at time step t. This represents the local relative state correction of the j-th agent at time step t. Let represent the posterior state estimate of the i-th agent at time step t. This represents the local relative state correction of the i-th agent at time step t. This represents the effective relative position observation information between the i-th agent and the j-th agent at time step t.

9. The physical information-enhanced cluster cooperative intelligent positioning method according to claim 1, characterized in that, The final state estimate of the agent is calculated using the following formula: ； in, This represents the final state estimate of the i-th agent. Let represent the posterior state estimate of the i-th agent. This represents the local relative state correction amount for the i-th agent. This represents the global state correction amount.

10. A cluster-based collaborative intelligent positioning system with enhanced physical information, characterized in that, The system is applied to an intelligent agent cluster, and the system includes: The data acquisition module is used to acquire inertial measurement data of the intelligent agents and effective relative position observation information between the intelligent agents; The posterior state estimation module is used to obtain the posterior state estimate of the agent and its corresponding covariance matrix based on the agent's inertial measurement data through the extended Kalman filter algorithm. The dynamic spatiotemporal graph construction module is used to construct a dynamic spatiotemporal graph based on the effective relative position observation information between each agent, wherein the agent corresponds to the node in the dynamic spatiotemporal graph, and the effective relative position observation information corresponds to the edge in the dynamic spatiotemporal graph; The state correction acquisition module is equipped with a pre-trained decoupled bi-headed graph neural network model, which is used to obtain the input feature vector corresponding to each agent based on the posterior state estimate of each agent and its corresponding covariance matrix. The input feature vector corresponding to each agent and the dynamic spatiotemporal graph are input to the decoupled bi-headed graph neural network model to obtain the global state correction for the entire agent cluster and the local relative state correction for each agent output by the decoupled bi-headed graph neural network model. The final state estimation module is used to calculate the final state estimate of each agent based on the posterior state estimate of each agent, the local relative state correction amount corresponding to each agent, and the global state correction amount.

Images