Method for cooperative localization of multiple unmanned systems based on hybrid entropy factor graph and variational inference

Abstract

The application relates to the technical field of multi-unmanned system cooperative positioning, and discloses a mixed entropy factor graph multi-unmanned system cooperative positioning method based on variational inference, which comprises the following steps: an extended factor graph model of multi-unmanned system cooperative positioning is constructed, a mixed correlation entropy cost function is constructed based on the convex combination of two different kernel bandwidth Gaussian kernel functions, and a model node is obtained by abstracting a state variable, an observation constraint and a kernel parameter; after prior distribution initialization of a variable node, position information of a master unmanned system and ranging measurement data of a slave unmanned system are obtained; the information is taken as input, and variational Bayesian inference is adopted to jointly estimate a position state and a kernel parameter; if a condition is met, the optimal position coordinates of the slave unmanned system are determined, and the posterior distribution of the current period is taken as the prior distribution of the next period. The application has higher positioning precision and robustness under time-varying non-Gaussian noise.

Description

Technical Field

[0001] This invention relates to the field of cooperative localization technology for multiple unmanned systems, and more specifically, to a cooperative localization method for multiple unmanned systems based on a hybrid entropy factor graph using variational inference. Background Technology

[0002] In fields such as military, agriculture, and transportation, multiple unmanned systems, such as drones, unmanned vehicles, and underwater unmanned vehicles, can effectively solve problems such as limited operational coverage and difficulties in perception and environmental modeling of individual unmanned systems through mutual communication and cooperation. Accurate positioning information is a key guarantee for multiple unmanned systems to complete collaborative operations. In a typical collaborative positioning architecture, some unmanned systems are equipped with high-precision inertial navigation systems as the master unmanned system, while the remaining slave unmanned systems adopt low-cost navigation and positioning systems. By communicating and ranging with the master unmanned system, the slave unmanned systems use collaborative positioning algorithms to correct their own navigation information, thereby achieving the goal of reducing costs and improving overall positioning accuracy.

[0003] In existing technologies, factor graph-based collaborative localization methods abstract the position and observation information of each unmanned system into factor nodes and utilize sum-product algorithms for message passing and information fusion, becoming one of the mainstream schemes for multi-unmanned system collaborative localization. To further improve the robustness of the algorithm in non-Gaussian noise environments, some existing technologies have introduced a cost function based on correlation entropy to replace the traditional minimum mean square error criterion, achieving better anti-interference effects. In particular, the hybrid entropy method, which uses a convex combination of two Gaussian kernel functions with different kernel bandwidths, can adapt to more complex noise distribution patterns compared to the single kernel bandwidth method. However, all the above-mentioned existing technologies set the kernel bandwidth parameter and mixing coefficient in the hybrid entropy as fixed hyperparameters determined offline in advance. In actual long-endurance operations, the operating environment of unmanned systems is not static. For example, when an unmanned system enters a port area from open water, enters an indoor area from outdoors, or encounters a region with abrupt changes in multipath effects, the peak intensity and tail thickness of the non-Gaussian noise faced by the ranging sensor will dynamically evolve with the changes in the environment. When the preset fixed kernel bandwidth is mismatched with the actual noise distribution at the current moment, the effectiveness of the relevant entropy as a measure of local similarity will be greatly reduced. As a result, although the mixed entropy is maximized numerically during the factor graph message transmission process, the corresponding optimal solution deviates significantly from the actual position of the unmanned system, that is, the positioning accuracy drops sharply.

[0004] Therefore, it is necessary to provide a cooperative localization method for multi-unmanned systems based on variational inference and hybrid entropy factor graphs, which aims to solve the problem that the kernel parameters cannot adaptively adjust to follow noise changes in time-varying non-Gaussian noise environments in current technologies. Summary of the Invention

[0005] In view of this, the present invention proposes a cooperative localization method for multi-unmanned systems based on a hybrid entropy factor graph using variational inference, which aims to solve the problem that the kernel parameters cannot adaptively adjust to follow noise changes in time-varying non-Gaussian noise environments in current technologies.

[0006] This invention proposes a cooperative localization method for multi-unmanned systems based on a hybrid entropy factor graph using variational inference, comprising the following steps: S100. Construct an extended factor graph model for cooperative localization of multiple unmanned systems, and construct a hybrid correlation entropy cost function based on a convex combination of two Gaussian kernel functions with different kernel bandwidths; wherein, the nodes of the extended factor graph model are abstracted from the state variables and observation constraints of the multiple unmanned systems, as well as the kernel parameters of the two Gaussian kernel functions; S200. Perform prior distribution initialization settings on the variable nodes in the nodes respectively. After completing the prior distribution initialization settings, obtain the position information of the master unmanned system and the ranging measurement data of the slave unmanned system based on a fixed sampling period. S300. Using the location information and ranging measurement data as input, based on the extended factor graph model, and with maximizing the mixed correlation entropy as the overall optimization objective, variational Bayesian inference is used to perform joint posterior estimation of the position state of the unmanned system and the kernel parameters to obtain the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period. S400. Determine whether the variational Bayes inference satisfies the preset iteration termination condition. When the preset iteration termination condition is satisfied, determine the optimal position coordinates of the slave system in the current sampling period based on the approximate posterior distribution of the position state, and use the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period as the prior distributions of the corresponding position state and kernel parameters in the next sampling period, respectively.

[0007] Furthermore, when the nodes of the expansion factor graphical model are abstracted from the state variables and observation constraints of the multi-unmanned system, as well as the kernel parameters of two Gaussian kernel functions, they include: The position state variables of the unmanned system will be abstracted into position state variable nodes of the extended factor graph model; The kernel parameters of the two Gaussian kernel functions are abstracted as kernel parameter variable nodes in the extended factor graph model; where, The kernel parameter variable nodes include the kernel bandwidth variable nodes of each of the two Gaussian kernel functions and the mixed coefficient variable nodes of the convex combination of the two Gaussian kernel functions. The recursive constraints of the motion state of the unmanned system are abstracted into motion factor nodes of the extended factor graph model; The ranging constraints of the master unmanned system and the slave unmanned system are abstracted into the ranging factor nodes of the extended factor graph model; A kernel parameter prior factor node is added based on the kernel parameter, and the kernel parameter prior factor node is connected to the corresponding kernel parameter variable node.

[0008] Furthermore, when performing prior distribution initialization settings for the variable nodes in the aforementioned nodes, the process includes: An initial Gaussian prior distribution of the position state is set for the position state variable nodes of the unmanned system, and the hyperparameters of the initial Gaussian prior distribution of the position state are determined based on the initial dead reckoning information of the unmanned system. Set a beta conjugate prior distribution for the mixing coefficient variable nodes of the convex combination of two Gaussian kernel functions; Set inverse gamma conjugate prior distributions for the kernel bandwidth variable nodes of the two Gaussian kernel functions.

[0009] Furthermore, when using variational Bayesian inference to perform joint posterior estimation of the position state of the unmanned system and the kernel parameters, it includes: By employing a coordinate ascent method and using the lower bound of evidence as a basis for alternating iterative solutions, the approximate posterior distribution of the positional state and the approximate posterior distribution of the kernel parameters in the current sampling period are obtained.

[0010] Furthermore, each round of alternating iterations in the process of solving the problem based on the lower bound of evidence includes: The mean of the approximate posterior distribution of the mixed coefficient variable nodes is used as the mixing coefficient of the mixed correlation entropy cost function in the current alternating iteration round. The mean of the approximate posterior distribution of the two kernel bandwidth variable nodes is used as the two kernel bandwidths of the mixed correlation entropy cost function in the current alternating iteration round to determine the mixed correlation entropy cost function in the current alternating iteration round. Based on the mixed correlation entropy cost function in the current alternating iteration round and the extended factor graph model, the approximate posterior distribution of the position state is updated. The approximate posterior distribution of the fixed updated position state remains unchanged, and the approximate posterior distribution of the two kernel bandwidth variable nodes remains unchanged. Based on the beta conjugate prior distribution of the mixed coefficient variable nodes, the approximate posterior distribution of the mixed coefficient is updated to obtain the mixed coefficient. The approximate posterior distribution of the fixed updated position state remains unchanged, as does the approximate posterior distribution of the fixed updated mixing coefficient. Based on the inverse gamma conjugate prior distribution of each kernel bandwidth variable node, the approximate posterior distribution of the kernel bandwidth of each of the two Gaussian kernel functions is updated respectively.

[0011] Furthermore, when updating the approximate posterior distribution of the position state based on the mixed correlation entropy cost function and the extended factor graph model in the current alternating iteration rounds, it includes: Based on the mixed-relevance entropy cost function in the current alternating iteration round, a sum-product operation is performed on the extended factor graph model to obtain the Gaussian message distribution of the positional state variable nodes; wherein... The Gaussian message distribution includes location state mean information and location state variance information; The mean information of the position state is used as the mean parameter of the approximate posterior distribution of the position state, and the variance information of the position state is used as the variance parameter of the approximate posterior distribution of the position state. Based on the mean parameter and the variance parameter, the updated approximate posterior distribution of the position state is determined.

[0012] Furthermore, when updating the approximate posterior distribution of the mixing coefficients based on the beta conjugate prior distribution of the mixing coefficient variable nodes, the following steps are included: Obtain the beta distribution of the mixed coefficient variable node in the current alternating iteration round, wherein the beta distribution includes a first current value of a first shape parameter and a second current value of a second shape parameter; Obtain the mixing coefficients corresponding to the mixing coefficient variable nodes, use the beta distribution as the probability measure, take the expected value of the first natural logarithm of the mixing coefficients as the first statistic under the probability measure of the beta distribution, and obtain the second natural logarithm of the difference between 1 and the mixing coefficients, and take the expected value of the second natural logarithm as the second statistic. The value of the first statistic is added to the first current value of the first shape parameter to obtain the updated first shape parameter; the value of the second statistic is added to the second current value of the second shape parameter to obtain the updated second shape parameter. Based on the updated first shape parameter and the updated second shape parameter, an updated beta distribution is constructed, and the updated beta distribution is used as the approximate posterior distribution of the updated mixed coefficient variable nodes.

[0013] Furthermore, when updating the approximate posterior distributions of the kernel bandwidths of the two Gaussian kernel functions based on the inverse gamma conjugate prior distributions of each kernel bandwidth variable node, the following steps are taken: Obtain the inverse gamma distribution of each core bandwidth variable node in the current alternating iteration round, wherein the inverse gamma distribution includes the third current value of the third shape parameter and the fourth current value of the scale parameter; Obtain the kernel bandwidth corresponding to each kernel bandwidth variable node, use the approximate posterior distribution of the updated position state as a probability measure, and obtain the mathematical expectation of the squared term of the ranging residual under the probability measure of the approximate posterior distribution of the updated position state to obtain the third statistic corresponding to each kernel bandwidth; wherein, the squared term of the ranging residual is the square of the difference between the distance estimate and the distance observation between the master unmanned system and the slave unmanned system. The value of the third statistic is added to the fourth current value of the scale parameter to obtain the updated scale parameter; Based on the third current value and the updated scale parameter, an updated inverse gamma distribution is constructed, and the updated inverse gamma distribution is used as the approximate posterior distribution of the corresponding updated kernel bandwidth variable node.

[0014] Furthermore, determining whether variational Bayesian inference satisfies the preset iteration termination condition includes: When the increase in the lower bound of the evidence for a consecutive preset number of adjacent rounds is less than or equal to the preset convergence threshold, it is determined that the variational Bayesian inference satisfies the preset iteration termination condition. Otherwise, the deterministic variational Bayesian inference does not satisfy the preset iteration termination condition.

[0015] Furthermore, when a preset iteration termination condition is met, determining the optimal position coordinates of the unmanned system in the current sampling period based on the approximate posterior distribution of the position state includes: When the preset iteration termination condition is met, the mean vector of the approximate posterior distribution of the position state is obtained, and the mean vector is used as the optimal position coordinates of the unmanned system in the current sampling period.

[0016] Compared with existing technologies, the beneficial effects of this invention are as follows: It constructs an extended factor graph model for collaborative localization of multiple unmanned systems, and builds a hybrid correlation entropy cost function based on a convex combination of two Gaussian kernel functions with different kernel bandwidths. The nodes of the extended factor graph model are abstracted from the state variables and observation constraints of the multiple unmanned systems, as well as the kernel parameters of the two Gaussian kernel functions. Incorporating the kernel parameters as nodes into the extended factor graph model makes them endogenous variables with probabilistic attributes, rather than fixed constants. This allows the kernel parameters to be jointly estimated with the position and state of the unmanned system within the same probabilistic framework, fundamentally solving the technical deficiency in traditional factor graph methods where kernel parameters, as offline fixed hyperparameters, cannot adaptively adjust with environmental changes. Prior distribution initialization is performed on the variable nodes in the nodes. After the prior distribution initialization is completed, the position information of the master unmanned system and the ranging measurement data of the slave unmanned system are acquired based on a fixed sampling period. This initialization provides initial prior information for subsequent online inference. The acquired position information of the master unmanned system and the ranging measurement data of the slave unmanned system provide a data foundation for subsequent joint posterior estimation of the position state of the slave unmanned system and the kernel parameters based on variational Bayesian inference. Using the position information and ranging measurement data as input, based on the extended factor graph model, and with maximizing the mixed correlation entropy as the overall optimization objective, variational Bayesian inference is used to perform joint posterior estimation of the position state of the slave unmanned system and the kernel parameters. This yields the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters for the current sampling period. Bayesian inference is then used to perform joint posterior estimation of the position state and kernel parameters of the slave unmanned system, while simultaneously updating the posterior distributions of the position state and kernel parameters, allowing the kernel parameters to adaptively adjust to changes in the current noise environment. The process involves determining whether the variational Bayesian inference meets a preset iteration termination condition. When this condition is met, the optimal position coordinates of the unmanned system in the current sampling period are determined based on the approximate posterior distribution of the position state. By setting a preset iteration termination condition, convergence can be achieved promptly while maintaining estimation accuracy, avoiding the consumption of computational resources. The approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period are used as the prior distributions of the corresponding position state and kernel parameters in the next sampling period, respectively. By passing the posterior distribution of the current sampling period to the prior distribution of the next sampling period, the estimation of the kernel parameters can be continuously accumulated over time, ensuring that the system maintains its adaptive tracking capability to time-varying noise environments throughout the entire mission cycle.

[0017] In summary, this invention incorporates kernel parameters into the extended factor graph model and uses variational inference to jointly estimate the position state. It iteratively updates the kernel broadband parameters and mixing coefficients online, enabling the kernel parameters to adaptively adjust with changes in environmental noise. This significantly improves the robustness of system state estimation and positioning accuracy in non-Gaussian noise environments. Attached Figure Description

[0018] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 A flowchart of a multi-unmanned system cooperative localization method based on variational inference and a hybrid entropy factor graph provided in an embodiment of the present invention; Figure 2 The diagram below illustrates the logic of the hybrid entropy factor graph-based multi-unmanned system cooperative localization method based on variational inference provided in this embodiment of the invention. Detailed Implementation

[0019] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the disclosure to those skilled in the art. It should be noted that, unless otherwise specified, embodiments and features in the embodiments of the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0020] In some embodiments of this application, see Figure 1-2 As shown, this embodiment provides a cooperative localization method for multi-unmanned systems based on a hybrid entropy factor graph using variational inference, including the following steps: S100. Construct an extended factor graph model for cooperative localization of multiple unmanned systems, and construct a hybrid correlation entropy cost function based on a convex combination of two Gaussian kernel functions with different kernel bandwidths; wherein, the nodes of the extended factor graph model are abstracted from the state variables and observation constraints of the multiple unmanned systems, as well as the kernel parameters of the two Gaussian kernel functions.

[0021] Specifically, when the nodes of the expansion factor graph model are abstracted from the state variables and observation constraints of the multi-unmanned system, as well as the kernel parameters of two Gaussian kernel functions, they include: The position state variables of the unmanned system will be abstracted into position state variable nodes of the extended factor graph model; The kernel parameters of the two Gaussian kernel functions are abstracted as kernel parameter variable nodes in the extended factor graph model; where, The kernel parameter variable nodes include the kernel bandwidth variable nodes of each of the two Gaussian kernel functions and the mixed coefficient variable nodes of the convex combination of the two Gaussian kernel functions. The recursive constraints of the motion state of the unmanned system are abstracted into motion factor nodes of the extended factor graph model; The ranging constraints of the master unmanned system and the slave unmanned system are abstracted into the ranging factor nodes of the extended factor graph model; A kernel parameter prior factor node is added based on the kernel parameter, and the kernel parameter prior factor node is connected to the corresponding kernel parameter variable node.

[0022] In this embodiment, a multi-unmanned system formation comprises two master unmanned systems and one slave unmanned system. When constructing the extended factor graph model, two position state variable nodes are constructed, representing the x-coordinate and y-coordinate of the slave unmanned system in the two-dimensional plane, respectively. Two kernel bandwidth variable nodes are constructed, representing the kernel bandwidth of the first Gaussian kernel function and the kernel bandwidth of the second Gaussian kernel function, respectively. A mixture coefficient variable node is constructed, representing the convex combination coefficients. A motion factor node is constructed, connecting the position state variable nodes of the previous and current time steps, with the constraint relationship being the kinematic recursive equation of the slave unmanned system. Two ranging factor nodes are constructed, corresponding to the ranging links between the two master unmanned systems and the slave unmanned system, respectively. Each ranging factor node connects the position information node of the corresponding master unmanned system, the position state variable node of the slave unmanned system, and all kernel parameter variable nodes, with the constraint relationship defined by the mixture correlation entropy cost function. Simultaneously, prior factor nodes are added to the kernel bandwidth variable node and the mixture coefficient variable node, each prior factor node being individually connected to its corresponding kernel parameter variable node, used to introduce the prior distribution information of the kernel parameters into the factor graph.

[0023] Understandably, the position-state variables of the unmanned system are abstracted into position-state variable nodes, and the two kernel bandwidths and mixing coefficients are abstracted into kernel parameter variable nodes, thus incorporating the kernel parameters into the factor graph. Since the kernel parameters are constructed as random variables with probability distributions rather than fixed constants, prior factor nodes are added to introduce prior information. An extended factor graph model incorporating kernel parameters is constructed, providing a foundation for subsequent variational inference and alternating iterative calculations.

[0024] S200. Perform prior distribution initialization settings on the variable nodes in the nodes respectively. After completing the prior distribution initialization settings, obtain the position information of the master unmanned system and the ranging measurement data of the slave unmanned system based on a fixed sampling period.

[0025] Specifically, when performing prior distribution initialization settings for the variable nodes in the aforementioned nodes, the following is included: An initial Gaussian prior distribution of the position state is set for the position state variable nodes of the unmanned system, and the hyperparameters of the initial Gaussian prior distribution of the position state are determined based on the initial dead reckoning information of the unmanned system. Set a beta conjugate prior distribution for the mixing coefficient variable nodes of the convex combination of two Gaussian kernel functions; Set inverse gamma conjugate prior distributions for the kernel bandwidth variable nodes of the two Gaussian kernel functions.

[0026] In this embodiment, the initial position of the unmanned system is provided by the onboard GPS upon system startup. Taking a specific scenario as an example, the initial position of the unmanned system is (2.5m, 3.8m), and the horizontal positioning accuracy of the GPS is 0.8m. For the position state variable nodes, the mean of the Gaussian prior distribution is set to (2.5m, 3.8m), and the variance is set to 0.64m. 2 For the mixed coefficient variable nodes, due to the lack of prior knowledge, both shape parameters of the beta prior distribution are set to 2, so that it presents a symmetrical uniform distribution in the interval [0, 1]. For the two kernel bandwidth variable nodes, based on the prior calibration information of the sensor, the shape parameter of the inverse gamma prior of the first kernel bandwidth is set to 3 and the scale parameter is set to 2 (corresponding to a mean of approximately 1), and the shape parameter of the inverse gamma prior of the second kernel bandwidth is set to 3 and the scale parameter is set to 32 (corresponding to a mean of approximately 16), so as to reflect the different scales of the two kernel bandwidths.

[0027] It is understandable that the values ​​of the position state variable nodes of the unmanned system are defined over the entire real number domain. Therefore, a Gaussian distribution is chosen as its prior distribution. The mean and variance of this Gaussian prior distribution are determined based on the initial dead reckoning information at the start of the unmanned system. The mean and variance of the Gaussian prior distribution are the hyperparameters. The mixing coefficient variable nodes represent mixing coefficients whose values ​​are physically constrained to the interval [0, 1]. Because these mixing coefficients are weights of a convex combination, they must be between 0 and 1. The beta distribution is precisely defined in the interval [0, 1], and as a conjugate prior of the multinomial and multi-class distributions, it offers computational convenience in Bayesian inference. Therefore, a beta conjugate prior distribution is chosen. The shape of the beta conjugate prior distribution is controlled by two shape parameters. The kernel bandwidth variable nodes represent kernel bandwidth parameters that must be greater than zero. The inverse gamma distribution is defined over the positive real number domain, and as a conjugate prior of the Gaussian distribution variance, it has an analytical update form in Bayesian inference. Therefore, an inverse gamma conjugate prior distribution is chosen. By selecting conjugate prior distributions for different parameters, the posterior and prior distributions belong to the same distribution family in the subsequent variational Bayes update steps, thereby obtaining an analytical update expression and avoiding complex numerical calculations.

[0028] S300. Using the location information and ranging measurement data as input, based on the extended factor graph model, and with maximizing the mixed correlation entropy as the overall optimization objective, variational Bayesian inference is used to perform joint posterior estimation of the position state of the unmanned system and the kernel parameters to obtain the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period.

[0029] Specifically, when using variational Bayesian inference to perform joint posterior estimation of the position state of the unmanned system and the kernel parameters, the following is included: By employing a coordinate ascent method and using the lower bound of evidence as a basis for alternating iterative solutions, the approximate posterior distribution of the positional state and the approximate posterior distribution of the kernel parameters in the current sampling period are obtained.

[0030] Understandably, by employing a coordinate ascent approach, the joint optimization is decomposed into three dimensions: position state update, mixture coefficient update, and kernel bandwidth update, each requiring separate solutions. Each problem involves only one variable, reducing computational complexity and facilitating rapid, real-time execution on the onboard processor of the unmanned system. The Evidence Lower Bound (ELBO) is the core objective function in variational Bayesian inference. By iteratively maximizing the ELBO in each iteration, the algorithm's convergence is guaranteed.

[0031] Specifically, each round of alternating iterations in the process of solving the problem based on the lower bound of evidence includes: The mean of the approximate posterior distribution of the mixed coefficient variable nodes is used as the mixing coefficient of the mixed correlation entropy cost function in the current alternating iteration round. The mean of the approximate posterior distribution of the two kernel bandwidth variable nodes is used as the two kernel bandwidths of the mixed correlation entropy cost function in the current alternating iteration round to determine the mixed correlation entropy cost function in the current alternating iteration round. Based on the mixed correlation entropy cost function in the current alternating iteration round and the extended factor graph model, the approximate posterior distribution of the position state is updated. The approximate posterior distribution of the fixed updated position state remains unchanged, and the approximate posterior distribution of the two kernel bandwidth variable nodes remains unchanged. Based on the beta conjugate prior distribution of the mixed coefficient variable nodes, the approximate posterior distribution of the mixed coefficient is updated to obtain the mixed coefficient. The approximate posterior distribution of the fixed updated position state remains unchanged, as does the approximate posterior distribution of the fixed updated mixing coefficient. Based on the inverse gamma conjugate prior distribution of each kernel bandwidth variable node, the approximate posterior distribution of the kernel bandwidth of each of the two Gaussian kernel functions is updated respectively.

[0032] Understandably, in each iteration, the mixed correlation entropy cost function for the current round is first constructed based on the mean of the current approximate posterior distributions of the mixed coefficient variable nodes and the two kernel bandwidth variable nodes. Then, the sum-product algorithm is executed on the extended factor graph model to update the approximate posterior distribution of the position state. After this update, the updated position state and the two kernel bandwidths are fixed, and the approximate posterior distribution of the mixed coefficients is updated using the beta conjugate prior distribution. Finally, the updated position state and the updated mixed coefficients are fixed, and the approximate posterior distributions of the two kernel bandwidths are updated using the inverse gamma conjugate prior distributions respectively. This sequence strictly follows the requirements of the coordinate ascent method, that is, only one subset of variables is updated each time, while the remaining variables remain unchanged, and each variable is updated once in each iteration, ensuring the convergence of the algorithm.

[0033] Specifically, when updating the approximate posterior distribution of the position state based on the mixed correlation entropy cost function and the extended factor graph model in the current alternating iteration round, the following steps are included: Based on the mixed-relevance entropy cost function in the current alternating iteration round, a sum-product operation is performed on the extended factor graph model to obtain the Gaussian message distribution of the positional state variable nodes; wherein... The Gaussian message distribution includes location state mean information and location state variance information; The mean information of the position state is used as the mean parameter of the approximate posterior distribution of the position state, and the variance information of the position state is used as the variance parameter of the approximate posterior distribution of the position state. Based on the mean parameter and the variance parameter, the updated approximate posterior distribution of the position state is determined.

[0034] Understandably, by obtaining the Gaussian message distribution through the sum-product algorithm, this distribution contains both the mean and variance information of the location state, thus providing both the optimal estimate of the location and a measure of its uncertainty. Using the mean of the Gaussian message distribution as the mean parameter of the approximate posterior distribution of the location state allows the approximate posterior distribution to inherit the inference results from the sum-product algorithm. The message passing in the sum-product algorithm on the factor graph is the optimal inference given the current mixed entropy cost function and prior information; therefore, the mean of the resulting Gaussian message distribution is the optimal location estimate given all currently available information. Using the variance of the Gaussian message distribution as the variance parameter of the approximate posterior distribution of the location state allows the approximate posterior distribution to retain the uncertainty information of the estimate.

[0035] Specifically, when updating the approximate posterior distribution of the mixing coefficients based on the beta conjugate prior distribution of the mixing coefficient variable nodes, the following steps are included: Obtain the beta distribution of the mixed coefficient variable node in the current alternating iteration round, wherein the beta distribution includes a first current value of a first shape parameter and a second current value of a second shape parameter; Obtain the mixing coefficients corresponding to the mixing coefficient variable nodes, use the beta distribution as the probability measure, take the expected value of the first natural logarithm of the mixing coefficients as the first statistic under the probability measure of the beta distribution, and obtain the second natural logarithm of the difference between 1 and the mixing coefficients, and take the expected value of the second natural logarithm as the second statistic. The value of the first statistic is added to the first current value of the first shape parameter to obtain the updated first shape parameter; the value of the second statistic is added to the second current value of the second shape parameter to obtain the updated second shape parameter. Based on the updated first shape parameter and the updated second shape parameter, an updated beta distribution is constructed, and the updated beta distribution is used as the approximate posterior distribution of the updated mixed coefficient variable nodes.

[0036] It is understandable that the first shape parameter and the second shape parameter jointly control the shape of the beta distribution, and the first current value and the second current value are the specific values ​​of the first shape parameter and the second shape parameter at the beginning of the current iteration, respectively. In variational Bayesian inference, each mixing coefficient variable node has a current approximate posterior distribution at the beginning of each iteration. This distribution comes from the result of updating the node at the end of the previous iteration, where it comes from the prior distribution set in the initialization during the first iteration. Obtaining the beta distribution of the mixing coefficient variable node in the current alternating iteration is essentially obtaining all the prior content of the mixing coefficient at the current moment. The first statistic captures all the information about the mixing coefficient on the natural logarithmic scale in the measurement data. The second statistic captures all the information about 1 minus the mixing coefficient on the natural logarithmic scale in the measurement data, and together with the first statistic, constitutes the complete information of the beta distribution. The update process is simplified to the addition operation of the shape parameters, making the computational cost of the mixing coefficient update step extremely small.

[0037] Specifically, when updating the approximate posterior distributions of the kernel bandwidths of the two Gaussian kernel functions based on the inverse gamma conjugate prior distributions of each kernel bandwidth variable node, the following steps are taken: Obtain the inverse gamma distribution of each core bandwidth variable node in the current alternating iteration round, wherein the inverse gamma distribution includes the third current value of the third shape parameter and the fourth current value of the scale parameter; Obtain the kernel bandwidth corresponding to each kernel bandwidth variable node, use the approximate posterior distribution of the updated position state as a probability measure, and obtain the mathematical expectation of the squared term of the ranging residual under the probability measure of the approximate posterior distribution of the updated position state to obtain the third statistic corresponding to each kernel bandwidth; wherein, the squared term of the ranging residual is the square of the difference between the distance estimate and the distance observation between the master unmanned system and the slave unmanned system. The value of the third statistic is added to the fourth current value of the scale parameter to obtain the updated scale parameter; Based on the third current value and the updated scale parameter, an updated inverse gamma distribution is constructed, and the updated inverse gamma distribution is used as the approximate posterior distribution of the corresponding updated kernel bandwidth variable node.

[0038] Understandably, by using the approximate posterior distribution of the updated position state as a probability measure to calculate the expected value of the squared residual term, the kernel bandwidth update fully considers the uncertainty of the position state estimation. When the position state estimation accuracy is high, the expected value of the squared residual term approaches the squared residual term based on the mean; when the position state estimation accuracy is low, the expected value of the squared residual term will be greater than the squared residual term based on the mean, thus adaptively increasing the estimated value of the kernel bandwidth and reflecting the impact of position state uncertainty on the noise metric. By adding the third statistic to the current value of the scale parameter while keeping the shape parameter unchanged, the posterior distribution of the kernel bandwidth is updated. This utilizes the mathematical property that the inverse gamma distribution is the conjugate prior of the Gaussian distribution variance, ensuring that the posterior distribution and the prior distribution belong to the same distribution family, maintaining the probabilistic consistency of the kernel bandwidth during the iteration process. Independent update operations are performed on the two kernel bandwidths respectively, allowing each kernel bandwidth to adapt to noise changes from different sources or at different scales. The first and second kernel bandwidths each have independent inverse gamma distribution parameters. An update of one kernel bandwidth will not affect the estimate of the other kernel bandwidth, thus maintaining the independence and diversity of the two Gaussian kernels in the mixed entropy cost function.

[0039] S400. Determine whether the variational Bayes inference satisfies the preset iteration termination condition. When the preset iteration termination condition is satisfied, determine the optimal position coordinates of the slave system in the current sampling period based on the approximate posterior distribution of the position state, and use the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period as the prior distributions of the corresponding position state and kernel parameters in the next sampling period, respectively.

[0040] Specifically, determining whether variational Bayesian inference satisfies the preset iteration termination condition includes: When the increase in the lower bound of the evidence for a consecutive preset number of adjacent rounds is less than or equal to the preset convergence threshold, it is determined that the variational Bayesian inference satisfies the preset iteration termination condition. Otherwise, the deterministic variational Bayesian inference does not satisfy the preset iteration termination condition.

[0041] In this embodiment, in order to cope with the increased computational load due to excessive iterations, when the number of iterations reaches a preset iteration threshold, the iteration is terminated regardless of whether the increase in the number of consecutive preset number of adjacent rounds of the lower bound of evidence is less than or equal to the preset convergence threshold.

[0042] Understandably, during the alternating iteration process, the lower bound of evidence (ELBO) is a scalar indicator that measures the quality of the current variational distribution approximation, and theoretically, it monotonically increases with the number of iterations. To enhance the robustness of convergence determination, convergence is only determined when the increase in a consecutive preset number of adjacent iterations is less than or equal to a preset convergence threshold, thereby avoiding false convergence caused by numerical fluctuations in a single iteration.

[0043] Specifically, when a preset iteration termination condition is met, determining the optimal position coordinates of the unmanned system in the current sampling period based on the approximate posterior distribution of the position state includes: When the preset iteration termination condition is met, the mean vector of the approximate posterior distribution of the position state is obtained, and the mean vector is used as the optimal position coordinates of the unmanned system in the current sampling period.

[0044] It is understandable that the approximate posterior distribution of the position state is a Gaussian distribution, and its mean vector, under the maximum a posteriori criterion, is the optimal position estimate. Therefore, the mean vector can be directly extracted from this Gaussian distribution as the optimal position coordinates of the unmanned system. This operation requires no additional computation and only involves reading the mean parameter from the data structure.

[0045] It is understandable that the extended factor graph model for collaborative localization of multiple unmanned systems is constructed by building a hybrid correlation entropy cost function based on a convex combination of two Gaussian kernel functions with different kernel bandwidths. The nodes of the extended factor graph model are abstracted from the state variables and observation constraints of the multiple unmanned systems, as well as the kernel parameters of the two Gaussian kernel functions. Incorporating the kernel parameters as nodes into the extended factor graph model makes them endogenous variables with probabilistic attributes, rather than fixed constants. This allows the kernel parameters to be jointly estimated with the position and state of the unmanned system within the same probabilistic framework, fundamentally solving the technical deficiency in traditional factor graph methods where kernel parameters, as offline fixed hyperparameters, cannot adaptively adjust to environmental changes. Prior distribution initialization is performed on the variable nodes in the nodes. After the prior distribution initialization is completed, the position information of the master unmanned system and the ranging measurement data of the slave unmanned system are acquired based on a fixed sampling period. This initialization provides initial prior information for subsequent online inference. The acquired position information of the master unmanned system and the ranging measurement data of the slave unmanned system provide a data foundation for subsequent joint posterior estimation of the position state of the slave unmanned system and the kernel parameters based on variational Bayesian inference. Using the position information and ranging measurement data as input, based on the extended factor graph model, and with maximizing the mixed correlation entropy as the overall optimization objective, variational Bayesian inference is used to perform joint posterior estimation of the position state of the slave unmanned system and the kernel parameters. This yields the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters for the current sampling period. Bayesian inference is then used to perform joint posterior estimation of the position state and kernel parameters of the slave unmanned system, while simultaneously updating the posterior distributions of the position state and kernel parameters, allowing the kernel parameters to adaptively adjust to changes in the current noise environment. The process involves determining whether the variational Bayesian inference meets a preset iteration termination condition. When this condition is met, the optimal position coordinates of the unmanned system in the current sampling period are determined based on the approximate posterior distribution of the position state. By setting a preset iteration termination condition, convergence can be achieved promptly while maintaining estimation accuracy, avoiding the consumption of computational resources. The approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period are used as the prior distributions of the corresponding position state and kernel parameters in the next sampling period, respectively. By passing the posterior distribution of the current sampling period to the prior distribution of the next sampling period, the estimation of the kernel parameters can be continuously accumulated over time, ensuring that the system maintains its adaptive tracking capability to time-varying noise environments throughout the entire mission cycle.

[0046] In summary, this invention incorporates kernel parameters into the extended factor graph model and uses variational inference to jointly estimate the position state. It iteratively updates the kernel broadband parameters and mixing coefficients online, enabling the kernel parameters to adaptively adjust with changes in environmental noise. This significantly improves the robustness of system state estimation and positioning accuracy in non-Gaussian noise environments.

[0047] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program goods. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program goods embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0048] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program goods according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and 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.

[0049] 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.

[0050] 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.

[0051] 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 it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A cooperative localization method for multi-unmanned systems based on a hybrid entropy factor graph using variational inference, characterized in that, Includes the following steps: An extended factor graph model for cooperative localization of multi-unmanned systems is constructed, and a hybrid correlation entropy cost function is built based on a convex combination of two Gaussian kernel functions with different kernel bandwidths. The nodes of the extended factor graph model are abstracted from the state variables and observation constraints of the multi-unmanned system, as well as the kernel parameters of the two Gaussian kernel functions. Prior distribution initialization settings are performed on the variable nodes in the nodes respectively. After the prior distribution initialization settings are completed, the position information of the master unmanned system and the ranging measurement data of the slave unmanned system are obtained based on a fixed sampling period. Using the location information and ranging measurement data as input, based on the extended factor graph model, and with maximizing the mixed correlation entropy as the overall optimization objective, variational Bayesian inference is used to perform joint posterior estimation of the position state and the kernel parameters of the unmanned system, to obtain the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period. Determine whether the variational Bayesian inference meets the preset iteration termination condition. When the preset iteration termination condition is met, determine the optimal position coordinates of the slave system in the current sampling period based on the approximate posterior distribution of the position state. Then, use the approximate posterior distribution of the position state and the approximate posterior distribution of the kernel parameters in the current sampling period as the prior distributions of the corresponding position state and kernel parameters in the next sampling period, respectively.

2. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 1, characterized in that, When the nodes of the expansion factor graphical model are abstracted from the state variables and observation constraints of the multi-unmanned system, as well as the kernel parameters of two Gaussian kernel functions, they include: The position state variables of the unmanned system will be abstracted into position state variable nodes of the extended factor graph model; The kernel parameters of the two Gaussian kernel functions are abstracted as kernel parameter variable nodes in the extended factor graph model; where, The kernel parameter variable nodes include the kernel bandwidth variable nodes of each of the two Gaussian kernel functions and the mixed coefficient variable nodes of the convex combination of the two Gaussian kernel functions. The recursive constraints of the motion state of the unmanned system are abstracted into motion factor nodes of the extended factor graph model; The ranging constraints of the master unmanned system and the slave unmanned system are abstracted into the ranging factor nodes of the extended factor graph model; A kernel parameter prior factor node is added based on the kernel parameter, and the kernel parameter prior factor node is connected to the corresponding kernel parameter variable node.

3. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 2, characterized in that, When performing prior distribution initialization settings for the variable nodes in the aforementioned nodes, the following is included: An initial Gaussian prior distribution of the position state is set for the position state variable nodes of the unmanned system, and the hyperparameters of the initial Gaussian prior distribution of the position state are determined based on the initial dead reckoning information of the unmanned system. Set a beta conjugate prior distribution for the mixing coefficient variable nodes of the convex combination of two Gaussian kernel functions; Set inverse gamma conjugate prior distributions for the kernel bandwidth variable nodes of the two Gaussian kernel functions.

4. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 3, characterized in that, When using variational Bayesian inference to perform joint posterior estimation of the position state of the unmanned system and the kernel parameters, the following steps are included: By employing a coordinate ascent method and using the lower bound of evidence as a basis for alternating iterative solutions, the approximate posterior distribution of the positional state and the approximate posterior distribution of the kernel parameters in the current sampling period are obtained.

5. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 4, characterized in that, Each round of alternating iterations in the process of solving the problem based on the lower bound of evidence includes: The mean of the approximate posterior distribution of the mixed coefficient variable nodes is used as the mixing coefficient of the mixed correlation entropy cost function in the current alternating iteration round. The mean of the approximate posterior distribution of the two kernel bandwidth variable nodes is used as the two kernel bandwidths of the mixed correlation entropy cost function in the current alternating iteration round to determine the mixed correlation entropy cost function in the current alternating iteration round. Based on the mixed correlation entropy cost function in the current alternating iteration round and the extended factor graph model, the approximate posterior distribution of the position state is updated. The approximate posterior distribution of the fixed updated position state remains unchanged, and the approximate posterior distribution of the two kernel bandwidth variable nodes remains unchanged. Based on the beta conjugate prior distribution of the mixed coefficient variable nodes, the approximate posterior distribution of the mixed coefficient is updated to obtain the mixed coefficient. The approximate posterior distribution of the fixed updated position state remains unchanged, as does the approximate posterior distribution of the fixed updated mixing coefficient. Based on the inverse gamma conjugate prior distribution of each kernel bandwidth variable node, the approximate posterior distribution of the kernel bandwidth of each of the two Gaussian kernel functions is updated respectively.

6. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 5, characterized in that, When updating the approximate posterior distribution of the position state based on the mixed correlation entropy cost function and the extended factor graph model in the current alternating iteration rounds, the following are included: Based on the mixed-relevance entropy cost function in the current alternating iteration round, a sum-product operation is performed on the extended factor graph model to obtain the Gaussian message distribution of the positional state variable nodes; wherein... The Gaussian message distribution includes location state mean information and location state variance information; The mean information of the position state is used as the mean parameter of the approximate posterior distribution of the position state, and the variance information of the position state is used as the variance parameter of the approximate posterior distribution of the position state. Based on the mean parameter and the variance parameter, the updated approximate posterior distribution of the position state is determined.

7. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 6, characterized in that, When updating the approximate posterior distribution of the mixing coefficients based on the beta conjugate prior distribution of the mixing coefficient variable nodes, the following steps are included: Obtain the beta distribution of the mixed coefficient variable node in the current alternating iteration round, wherein the beta distribution includes the first current value of the first shape parameter and the second current value of the second shape parameter; Obtain the mixing coefficients corresponding to the mixing coefficient variable nodes, use the beta distribution as the probability measure, take the expected value of the first natural logarithm of the mixing coefficients as the first statistic under the probability measure of the beta distribution, and obtain the second natural logarithm of the difference between 1 and the mixing coefficients, and take the expected value of the second natural logarithm as the second statistic. The value of the first statistic is added to the first current value of the first shape parameter to obtain the updated first shape parameter; the value of the second statistic is added to the second current value of the second shape parameter to obtain the updated second shape parameter. Based on the updated first shape parameter and the updated second shape parameter, an updated beta distribution is constructed, and the updated beta distribution is used as the approximate posterior distribution of the updated mixed coefficient variable nodes.

8. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 7, characterized in that, When updating the approximate posterior distributions of the kernel bandwidths of the two Gaussian kernel functions based on the inverse gamma conjugate prior distributions of each kernel bandwidth variable node, the following steps are taken: Obtain the inverse gamma distribution of each core bandwidth variable node in the current alternating iteration round, wherein the inverse gamma distribution includes the third current value of the third shape parameter and the fourth current value of the scale parameter; Obtain the kernel bandwidth corresponding to each kernel bandwidth variable node, use the approximate posterior distribution of the updated position state as a probability measure, and obtain the mathematical expectation of the squared term of the ranging residual under the probability measure of the approximate posterior distribution of the updated position state to obtain the third statistic corresponding to each kernel bandwidth; wherein, the squared term of the ranging residual is the square of the difference between the distance estimate and the distance observation between the master unmanned system and the slave unmanned system. The value of the third statistic is added to the fourth current value of the scale parameter to obtain the updated scale parameter; Based on the third current value and the updated scale parameter, an updated inverse gamma distribution is constructed, and the updated inverse gamma distribution is used as the approximate posterior distribution of the corresponding updated kernel bandwidth variable node.

9. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 8, characterized in that, Determining whether variational Bayesian inference satisfies the preset iteration termination condition includes: When the increase in the lower bound of the evidence for a consecutive preset number of adjacent rounds is less than or equal to the preset convergence threshold, it is determined that the variational Bayesian inference satisfies the preset iteration termination condition. Otherwise, the deterministic variational Bayesian inference does not satisfy the preset iteration termination condition.

10. The method for cooperative localization of multi-unmanned systems based on variational inference and hybrid entropy factor graphs according to claim 9, characterized in that, When a preset iteration termination condition is met, determining the optimal position coordinates of the unmanned system in the current sampling period based on the approximate posterior distribution of the position state includes: When the preset iteration termination condition is met, the mean vector of the approximate posterior distribution of the position state is obtained, and the mean vector is used as the optimal position coordinates of the unmanned system in the current sampling period.

Images