A communication-efficient dual-fusion consistent distributed collaborative positioning method and system

Abstract

The present invention discloses a communication-efficient dual-fusion consistency distributed collaborative positioning method and system, which relates to the technical field of multi-robot collaborative positioning. The technical highlights of the present invention include: for a multi-robot system, each robot performs the following operations in sequence to obtain its own positioning: self-motion prediction: based on self-motion information, an extended Kalman filter is used to predict its own state estimate and covariance estimate; heterogeneous fusion: the robot obtains the state estimate and covariance estimate of the neighboring observed robots, and jointly fuses its own estimate with the estimate from the neighboring observed robots and relative or absolute measurements; homogeneous fusion: the covariance intersection technology is used to combine the posterior estimate obtained by the joint fusion with its own estimate to obtain the final position estimate. The present invention combines heterogeneous and homogeneous fusion, with minimal requirements for storage, communication load and network connectivity, ensuring full utilization of all available information.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-robot collaborative positioning, and in particular to a dual-fusion consistency distributed collaborative positioning method and system with efficient communication. Background Art

[0002] For multi-robot systems, co-localization (CL)—determining the position and orientation of each robot relative to the same reference frame—is crucial for various tasks such as surveillance and reconnaissance, search and rescue, and formation control. One strategy for co-localization is to adopt a centralized framework, in which a fusion center or a designated leader robot is responsible for collecting and processing information from all member robots. Although centralized approaches can effectively track the relationships between robots and achieve considerable accuracy, they are susceptible to single points of failure and lack scalability as the team size increases, making their deployment challenging and unsuitable for practical needs.

[0003] To enhance robustness and scalability, extensive research has focused on distributed architectures, where each robot maintains a local filter and exchanges information with other robots to improve localization accuracy. A key challenge in distributed architectures is handling inter-robot dependencies to prevent double counting. This problem arises when robots treat shared information as irrelevant, resulting in degraded localization accuracy and unreliable results. Summary of the Invention

[0004] In view of the above problems, the present invention proposes a communication-efficient dual-fusion consistent distributed collaborative positioning method and system.

[0005] According to one aspect of the present invention, a communication-efficient dual-fusion consistency distributed collaborative localization method is proposed. The method includes: for n robot systems, at time k, each robot i, i∈{1,2,…,n}, sequentially performs the following operations to obtain its own localization:

[0006] Ego-motion prediction: Based on the ego-motion information, the extended Kalman filter is used to predict the state estimation and covariance estimation of the self-motion;

[0007] Heterogeneous fusion: When detecting relative measurements, robot i starts communicating with nearby observed robots to obtain their state estimates and covariance estimates. Robot i jointly fuses its own estimate with the estimates from nearby observed robots, as well as the relative or absolute measurements. The joint fusion process produces a posteriori estimates for itself and its nearby observed robots.

[0008] Homomorphic fusion: After receiving the posterior state estimates and posterior covariance estimates from neighboring observing robots, robot i combines these posterior estimates with its own estimate using the covariance intersection technique to obtain the final position estimate.

[0009] Furthermore, the specific process of the heterogeneous fusion includes:

[0010] First, determine the optimal joint state estimate

[0011] Then, the covariance intersection technique is used to design the prior covariance, which is the robot's own covariance estimation. The upper bound W i,k :

[0012]

[0013] where ω i 、 Respectively The corresponding weight, Represents the set of neighboring robots observed by the robot; and the prior covariance estimate Replaced by prior covariance The upper bound W i,k , substitute the optimal joint state estimate In the formula, we get:

[0014]

[0015] Among them, ||·|| represents the Mahalanobis distance; Indicates the The prior covariance estimate of the robots, represents the weight corresponding to the covariance, r ii and Represent the prior information, absolute measurement and relative measurement residuals respectively; s i,k Represents robot i and its n i The joint state vector of the neighboring observation robots; R ii and denote the covariance of absolute measurement noise and the covariance of relative measurement noise, respectively;

[0016] Then, for the pairs with prior covariance Upper bound W i,k The optimal joint state estimate Solve and obtain the joint covariance estimate

[0017] Then, extract the joint covariance estimate The corresponding robot obtained by robot i The posterior autocovariance estimate of and estimate the posterior autocovariance by minimizing the The optimal weight vector is obtained by

[0018] Then, the optimal weight vector Substitute back the error state update equation and joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot.

[0019] Furthermore, the determination of the optimal joint state estimate The process includes:

[0020] Joint state vector s i,k The optimal estimate of is obtained by solving the following maximum a posteriori estimation optimization problem:

[0021]

[0022] Where, p(s i,k |v i,k ,y i,k ) means that at a given v i,k ,y i,k Events i,k The probability distribution of v i,k represents the joint prior estimate of robot i and its neighboring observation robots, y i,k represents the entire measurement vector of all absolute and relative measurements of robot i;

[0023] Rewrite the above formula and omit s i,k Irrelevant denominator; rewrite and omit the denominator Take the logarithm of both sides of the formula to obtain the optimal joint state estimate for:

[0024]

[0025] Where, p(s i,k |v i,k ) indicates that given v i,k Events i,k The probability distribution of Indicates a given Under the event The probability distribution of Indicates a given Under the event The probability distribution of Represents robot i and robot iτ Relative measurement of and Denote robot i and robot i respectively τ The superscript i indicates that these states are estimated by robot i.

[0026] Furthermore, the posterior autocovariance estimate The expression is:

[0027]

[0028] Where, is an extraction matrix, except for the robot The relevant entries are all zero except for a d×d identity matrix.

[0029] Furthermore, the error state update equation is:

[0030]

[0031] Where H l 、H ii and Respectively represent the Jacobian matrices corresponding to the motion model, the observation model of absolute measurement, and the observation model of relative measurement; Indicates the current linearization point.

[0032] Furthermore, the posterior state estimation of each robot and the posterior covariance estimate for:

[0033]

[0034] Where, represents the prior state estimate.

[0035] Furthermore, the specific process of the homogeneous fusion includes: using covariance crossover technology to perform homogeneous fusion, and the final positioning of robot i at time k after homogeneous fusion is estimated to be:

[0036]

[0037] In the formula, η represents the weight vector to be optimized, represents the final state estimate; represents the final covariance estimate; represents the set of neighboring robots that can observe robot i; Indicated by robot Obtain the posterior autocovariance estimate corresponding to robot i; Indicated by robot Obtain the posterior state estimate corresponding to robot i.

[0038] Furthermore, the weight vector to be optimized is estimated by directly minimizing the final covariance It can be determined by the trace or logarithmic determinant of .

[0039] According to another aspect of the present invention, a communication-efficient dual-fusion consistency distributed collaborative positioning system is proposed. The system includes: for n robot systems, at time k, each robot i, i∈{1,2,…,n} sequentially uses the following modules to obtain its own positioning:

[0040] an ego-motion prediction module configured to predict an ego-state estimate and a covariance estimate of the ego-motion using an extended Kalman filter based on the ego-motion information;

[0041] a heterogeneous fusion module configured such that upon detecting relative measurements, robot i initiates communication with neighboring observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimate with estimates from neighboring observed robots, as well as relative or absolute measurements, wherein the joint fusion process generates a posteriori estimates for itself and its neighboring observed robots;

[0042] The homogeneous fusion module is configured such that after receiving the posterior state estimates and posterior covariance estimates from neighboring observation robots, robot i combines these posterior estimates with its own estimate using the covariance intersection technique to obtain the final position estimate.

[0043] The beneficial technical effects of the present invention are:

[0044] This paper proposes a fully distributed collaborative localization method based on a dual fusion framework. This method combines heterogeneous and homogeneous fusion, with minimal requirements for storage, communication load, and network connectivity, ensuring full utilization of all available information. Furthermore, a theoretical proof of the consistency of the proposed method is provided; in particular, a heterogeneous fusion method based on maximum a posteriori probability (MAP) is proposed, which fuses prior estimates of multiple heterogeneous states (overlapping or completely independent) with a universal nonlinear measurement in the presence of unknown cross-covariance between the prior estimates. Monte Carlo simulations and real-world experiments were performed, demonstrating that the proposed method outperforms existing methods in terms of accuracy and consistency. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the following detailed description are included in this specification and form a part of this specification, and are used to further illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0046] Figure 1 This is a flow chart of a communication-efficient dual-fusion consistent distributed collaborative positioning method described in an embodiment of the present invention.

[0047] Figure 2 1 is an example diagram of the root mean square error (RMSE) of the positions and orientations of all four robots in a Monte Carlo simulation in an embodiment of the present invention.

[0048] Figure 3 is an example graph of NEES of the average position and orientation of four robots in an embodiment of the present invention; if the NEES of the estimator exceeds the black dashed line, it is overconfident; otherwise, it is consistent.

[0049] Figure 4 This is an example diagram of the trajectories of five robots using different algorithms from 0 to 400 seconds in sub-dataset 9 in an embodiment of the present invention.

[0050] Figure 5 This is an example diagram of the overall communication load of the distributed algorithm on all nine sub-datasets in an embodiment of the present invention.

[0051] Figure 6 This is an example diagram of the overall communication load of the distributed algorithm on all nine sub-datasets in an embodiment of the present invention. DETAILED DESCRIPTION

[0052] In order to enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below with reference to the accompanying drawings. Obviously, the described embodiments or examples are only some of the embodiments or examples of the present invention, and not all of them. Based on the embodiments or examples of the present invention, all other embodiments or examples obtained by those skilled in the art without creative work should fall within the scope of protection of the present invention.

[0053] To achieve collaborative localization under limited communication capabilities and unreliable network connections, this paper proposes a consistent and efficient distributed approach with minimal requirements for storage, communication burden, and network connectivity. This approach adopts a dual fusion framework, involving heterogeneous and homogeneous fusion. Each robot only needs to maintain its own state and does not need to understand the scale of the entire system. Communication between robot pairs only occurs when relative measurements are available. Neighboring robot pairs only exchange local estimates without transmitting relative measurements, thus reducing the communication burden. Furthermore, within the dual fusion framework, the observed robot can fully utilize all available information from the observing robot, resulting in better estimation performance. This approach supports universal measurement models.

[0054] The embodiment of the present invention proposes a communication efficient dual fusion consistent distributed collaborative positioning method, such as Figure 1 As shown, the method includes: for n robot systems, at time i, each robot i, i∈{1,2,…,n} performs the following operations in sequence to obtain its own positioning:

[0055] Ego-motion prediction: Based on the ego-motion information, the extended Kalman filter is used to predict the state estimation and covariance estimation of the self-motion;

[0056] Heterogeneous fusion: When detecting relative measurements, robot i starts communicating with nearby observed robots to obtain their state estimates and covariance estimates. Robot i jointly fuses its own estimate with the estimates from nearby observed robots, as well as the relative or absolute measurements. The joint fusion process produces a posteriori estimates for itself and its nearby observed robots.

[0057] Homomorphic fusion: After receiving the posterior state estimates and posterior covariance estimates from neighboring observing robots, robot i combines these posterior estimates with its own estimate using the covariance intersection technique to obtain the final position estimate.

[0058] In this embodiment, preferably, the specific process of the heterogeneous fusion includes:

[0059] First, determine the optimal joint state estimate Then, the covariance intersection technique is used to design the prior covariance, which is the robot's own covariance estimation. The upper bound W i,k ; and estimate the prior covariance Replaced by prior covariance The upper bound W i,k , substitute the optimal joint state estimate In the formula of ; then, for the prior covariance Upper bound W i,k The optimal joint state estimate Solve and obtain the joint covariance estimate Then, extract the joint covariance estimate The corresponding robot obtained by robot i The posterior autocovariance estimate of and estimate the posterior autocovariance by minimizing the The optimal weight vector is obtained by Then, the optimal weight vector Substitute back the error state update equation and joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot.

[0060] In this embodiment, preferably, the determination of the optimal joint state estimation The process includes:

[0061] Joint state vector s i,k The optimal estimate of is obtained by solving the following maximum a posteriori estimation optimization problem:

[0062]

[0063] Where, p(s i,k |v i,k ,y i,k ) means that at a given v i,k ,y i,k Events i,k The probability distribution of v i,k represents the joint prior estimate of robot i and its neighboring observation robots, y i,k represents the entire measurement vector of all absolute and relative measurements of robot i;

[0064] Rewrite the above formula and omit s i,k Irrelevant denominator; rewrite and omit the denominator Take the logarithm of both sides of the formula to obtain the optimal joint state estimate for:

[0065]

[0066] Where, p(s i,k |v i,k ) indicates that given v i,k Events i,k The probability distribution of Indicates a given Under the event The probability distribution of Indicates a given Under the event The probability distribution of Represents robot i and robot i τ The relative measurement of and Denote robot i and robot i respectively τ The superscript i indicates that these states are estimated by robot i.

[0067] In this embodiment, preferably, the specific process of the homogeneous fusion includes: performing homogeneous fusion using covariance crossover technology, and the final positioning of the robot i at time k after homogeneous fusion is estimated to be:

[0068]

[0069] In the formula, η represents the weight vector to be optimized, represents the final state estimate; represents the final covariance estimate; represents the set of neighboring robots that can observe robot i; Indicated by robot Obtain the posterior autocovariance estimate corresponding to robot i; Indicated by robot Obtain the posterior state estimate corresponding to robot i.

[0070] Another embodiment of the present invention proposes a communication-efficient dual-fusion consensus distributed collaborative localization method. This method considers a multi-robot collaborative localization system consisting of n robots, all of which have perception, computation, and communication capabilities. These robots use measurements and information exchanged with other robots to self-localize. Specifically, each robot is equipped with proprioception sensors to obtain self-motion information and carries external perception sensors to perform relative measurements between robots and, when available, absolute measurements from identifiable landmarks. The size n of the multi-robot system may change dynamically over time and does not need to be known by the robots.

[0071] For robot i, i∈{1,…,n}, the motion model can be described by the following general formula:

[0072] x i,k =f i (x i,k-1 ,u i,k ,n i,k ) (1)

[0073] Where k = 1, 2, ... represents the discrete time index, Indicates the state of robot i, u i,k represents the self-motion input, n i,k Represents input noise, which obeys zero-mean Gaussian distribution

[0074] For a pair of robots i and j (i, j∈{1,2,…,n}), robots i and j are called neighbors if there is a relative measurement between them. Specifically, if robot i detects a relative measurement y from robot j ij,k , then robot j is called the observed robot and robot i is called the observing robot. ij,k The observation model can be written as follows:

[0075] y ij,k =h i (x i,k ,x j,k )+w ij,k (2)

[0076] where w ij,k Indicates that it obeys a zero-mean Gaussian distribution measurement noise.

[0077] For robot i, i∈{1,2,…,n}, the absolute measurement y ii,k The observation model can be described as:

[0078] y ii,k =g i (x i,k )+w ii,k (3)

[0079] where w ii,k Represents covariance R ii,k Zero-mean Gaussian noise.

[0080] The present invention aims to introduce a consistent distributed collaborative localization algorithm that requires as little storage, communication load and network connection as possible. Specifically, the algorithm requires that the self-motion information u i,k , available absolute measurement y ii,k , relative measurement y ij,k (j∈{1,2,…,n}\{i}) and the information exchanged with other robots to achieve x i,k For a system of n robots, at time k, each robot i, i∈{1,2,…,n} maintains a local estimator that only tracks its own state estimate. and covariance To minimize communication overhead and reduce reliance on network connectivity, communication occurs only between robot pairs with relative measurements, and robot pairs only exchange their state and covariance estimates. Specifically, for a pair of robots i and j (i, j∈{1,2,…,n}), if relative measurements from robot j are detected, robot i receives the prior state and covariance estimates of robot j for heterogeneous fusion, and then feeds the posterior state and covariance estimates back to robot j for homogeneous fusion.

[0081] Each robot uses its ego-motion information, available absolute or relative measurements, and information received from other robots to localize itself. During the estimation process, each robot sequentially performs the following operations:

[0082] 1) Self-motion prediction: Once the self-motion information is obtained, the robot immediately predicts its state and covariance using the classic extended Kalman filter. Specifically, for each robot i, i∈{1,2,…,n}, let and Represent the state and covariance estimation at the previous time k-1 respectively. After sensing the latest control input u through its onboard sensor i,k After that, the robot makes predictions as follows:

[0083]

[0084] Among them F i,k and G i,k The Jacobian matrix is ​​calculated as:

[0085] 2) Heterogeneous Fusion: When detecting relative measurements, the robot communicates with its neighboring robots to obtain their state and covariance estimates. The robot then jointly fuses its own estimates with those from its neighboring robots, along with relative and absolute measurements (if available). This joint fusion process produces a posteriori estimates for itself and its neighboring robots. The neighboring robots' estimates are then communicated back to the corresponding robot for subsequent homogeneous fusion.

[0086] 3) Homomorphic fusion: After receiving state and covariance estimates from neighboring observed robots, the robot combines these received estimates with its own estimate using the covariance intersection (CI) technique to obtain the final estimate.

[0087] For all robots, these three update operations are performed independently and recursively.

[0088] The embodiments of the present invention are described in detail below.

[0089] 2) Heterogeneous fusion algorithm

[0090] In this section, we formulate heterogeneous fusion as a maximum a posteriori (MAP) estimation problem and derive its analytical solution. Specifically, to account for the unknown cross-covariance between the prior estimates of different robots, we introduce an upper bound on the prior covariance using the covariance intersection (CI) technique. The unknown parameters involved in the state and covariance estimates, as well as the covariance bound, are then determined sequentially.

[0091] A. MAP-based heterogeneous fusion formula

[0092] At time k, it is assumed that robot i obtains relative measurement information from its neighboring observation robots and absolute measurement information from the landmarks. Let represents the set of neighboring observation robots of robot i, n i express The entire measurement vector of all absolute and relative measurements of robot i is stacked, denoted as y i,k, which can be written as follows:

[0093]

[0094] in and y ii,k They are expressed as formula (2) and (3) respectively.

[0095] At the same time, robot i communicates with its neighboring observation robots to obtain their prior states and covariance estimates in Let v i,k Denote the joint prior estimate of robot i and its neighboring observation robots as follows:

[0096]

[0097] in is the prior estimate of robot i, It's a robot τ A priori estimate of , and v i,k The joint prior covariance matrix of It can be expanded as follows:

[0098]

[0099] in and Represents robots i and i τ The prior covariance estimate of and represents the cross-covariance between the prior estimates of robot κ and robot ι Note that the autocovariance and (highlighted in green) in Eq. (6) is maintained by the corresponding robot and is therefore known, while the cross covariance (highlighted in grey) is unknown because each robot’s local estimator only maintains its own state estimate.

[0100] Let s i,k Represents the robot i and its n i The joint state vector of the neighboring observation robots is as follows:

[0101]

[0102] in and Denote robot i and robot i respectively τThe superscript i indicates that these states are estimated by robot i. The heterogeneous fusion of robot i aims to transform the previously estimated v i,k (including itself and its neighboring observation robots) with the absolute and relative measurements y detected without prior known cross covariance i,k Perform joint fusion to obtain the joint state estimate s i,k .

[0103] Joint state vector s i,k The best estimate of can be obtained by solving the following MAP optimization problem:

[0104]

[0105] According to Bayes' rule, the maximum a posteriori estimation formula MA can be rewritten as:

[0106]

[0107] Since the denominator p(y i,k |v i,k ) and s i,k is irrelevant and can be omitted. According to the assumption that the measurement noise is uncorrelated, p(y i,k |s i,k ) can be factored as follows:

[0108]

[0109] Substituting (9) into (8) and taking the logarithm of both sides of (8), the optimal joint state estimate can be obtained by equivalently minimizing the sum of negative log-likelihoods

[0110]

[0111] B. MAP-based heterogeneous fusion solution

[0112] It is worth noting that since the joint prior covariance matrix is unknown in (6), so achieving an optimal estimate by minimizing (10) is often infeasible. Therefore, we turn to seeking a suboptimal but consistent estimate by choosing an upper bound on the prior covariance (6).

[0113] By using the covariance intersection (CI) technique, we can design a priori covariance The upper bound of is as follows:

[0114]

[0115] in

[0116]

[0117] In ω i ∈[0,1], Under the conditions, Substituting (11) into (10), we can get Replaced with W i,k ,get

[0118]

[0119] where ||·|| represents the Mahalanobis distance,

[0120]

[0121] and r ii and Represent the prior information, absolute measurement and relative measurement residuals respectively, that is,

[0122]

[0123] For the nonlinear optimization problem described in (13), an iterative solution using the classical Gauss-Newton method is provided. This method iteratively approximates the residual via a first-order Taylor expansion and asymptotically moves to the minimum point. The current linearization point is denoted as:

[0124]

[0125] Initialize by using the previous state estimates of robot i and its neighboring observation robots. Then, the order of the residual is Taylor expansion (15)-(17) in Nearby:

[0126]

[0127] Among them H j 、H ii and is the corresponding Jacobian matrix, δs i,k is the state correction that needs to be optimized. Substitute (19)-(21) into (13), and then calculate the nonlinear cost function with respect to δs i,k Taking the derivative and setting it to zero gives the error state update equation.

[0128]

[0129] Among them, the weight vector ω to be optimized is given by The columns are concatenated as follows:

[0130]

[0131] The joint covariance estimation is done by updating

[0132]

[0133] The updated error state (22) and covariance (23) depend on the weights in (11). These weights are usually determined by optimizing the covariance estimate, i.e., minimizing The trace or logarithmic determinant of . However, in heterogeneous state fusion, this does not ensure that the uncertainty of the local state estimate of each robot is reduced. Therefore, we choose to minimize the autocovariance estimate of each robot separately to determine the corresponding weight vector.

[0134] make Indicates the robot i obtains the corresponding robot The posterior autocovariance estimate of . It can be extracted from (23) as follows.

[0135]

[0136] in is an extraction matrix, except for the robot The relevant entries are all zero except for a d×d identity matrix. The optimal weight vector corresponding to the state estimate obtained by robot i It can be estimated by minimizing the autocovariance , that is,

[0137]

[0138] ω satisfies (12). Since the cost function described in (25) is non-convex, a trusted region constraint algorithm is used to solve this optimization problem. To enhance the robustness and controllability of the optimization process, the trusted region is dynamically adjusted based on the cost loss and the distance between the actual objective function and its approximation at the current iteration step. In addition, to start the optimization process, the initial trusted region is set to 0.1×dim(ω). The optimization problem posed in (25) can be solved within a finite number of iterations.

[0139] According to (25), we can Obtain a weight vector to minimize its corresponding autocovariance estimate. Then, use the corresponding weight vector to update the state and covariance estimate of each robot separately, as shown below.

[0140]

[0141] where δs i,k (·)and are given in (22) and (23) respectively. By letting And for all Repeating the above update process, it is possible to iterate in heterogeneous fusion to achieve better convergence and accuracy. It is worth noting that in order to improve computational efficiency, the state and covariance estimation tasks of the above different robots are executed in parallel by using multiple threads at the same time.

[0142] Through this heterogeneous fusion, robot i generates state and covariance estimates for itself and its neighboring observed robots. These state and covariance estimates for neighboring observed robots are then transmitted back to the corresponding robot for further fusion, as discussed in the following subsections. It is important to note that the measurement data does not need to be transmitted, as they are already incorporated into the posterior estimates during the heterogeneous fusion process. This significantly reduces the communication overhead.

[0143] 3) Homogeneous Fusion Algorithm

[0144] At time k, it is assumed that robot i obtains its own estimation result through heterogeneous fusion At the same time, robot i receives estimates of the states and covariances from its neighboring observed robots. Let represents the set of neighboring observation robots of robot i, m i for The cardinality of the robot i will be observed from the neighboring ι The received state and covariance estimates are expressed as and in Then, robot i performs homogeneous fusion and combines the internal estimation With the received estimate Combined to obtain the final estimation result.

[0145] Specifically, the classic covariance intersection (CI) technique is applied to achieve consistent estimation fusion in the presence of unknown cross-correlations, as shown below:

[0146]

[0147] The weight vector η is given by Stacked in columns, i.e. Weight Need to optimize, meet

[0148]

[0149] For the uniform fusion problem in equations (28) and (29), the covariance estimate The weight vector η is determined by the trace or logarithmic determinant of .

[0150]

[0151] The cost function in Eq. (31) is convex, which enables the application of convex optimization techniques (e.g., gradient descent and interior point methods). The final estimation result can be obtained by substituting the optimal weight vector η * Substitute back into (28) and (29) to achieve this.

[0152] The distributed dual fusion CL algorithm proposed in the embodiment of the present invention is summarized in Algorithm 1.

[0153]

[0154]

[0155] We now prove the consistency of the proposed decentralized collaborative localization algorithm.

[0156] Theorem 1: Assume that the initial estimate For each robot i, i∈{1,2,…,n} is consistent, that is:

[0157]

[0158] Then, the local estimate obtained by Algorithm 1 is Under the condition of using the linearized system model, it remains consistent for any k ≥ 0, that is:

[0159]

[0160] Proof: The proposed algorithm involves three update steps: ego-motion prediction, heterogeneous fusion, and homogeneous fusion. We will show that the results obtained in each update step are consistent, and finally prove that the entire algorithm is consistent. The consistency of ego-motion prediction is proved by induction. Assume that the estimation results obtained at time step k-1 are consistent, that is,

[0161]

[0162] For any i∈{1,2,…,n}. According to (4), applying the linearized state model, the self-motion prediction is obtained The actual covariance of is

[0163]

[0164] Comparing (5) and (35), the prior covariance estimate satisfy If (34) holds. Therefore, the prior estimate obtained based on ego-motion prediction is consistent given (32).

[0165] Now prove that robot i is good at (in ) The estimated The same linearized measurement model is also applied, and the robot The posterior state updated by (26) The actual covariance of is as follows:

[0166]

[0167] in

[0168]

[0169] robot The posterior covariance estimate of can be rewritten through (27) as:

[0170]

[0171] in

[0172]

[0173] According to (11), we have This means Because the prior estimate obtained by self-motion prediction is Therefore, comparing (36) and (37), we can conclude that:

[0174]

[0175] For any This proves that robot i is a robot Obtained estimates are consistent.

[0176] Next, we prove that the estimate obtained by uniform fusion As you can see, the robot The estimate obtained for robot i is consistent Then, since the covariance intersection fusion strategy maintains consistency, the estimated pair obtained by (28) and (29) is is also consistent. We then complete the proof by noting that for any k ≥ 0, given the initial conditions (32), (33) hold.

[0177] The technical effects of the embodiments of the present invention are further verified through experiments.

[0178] First, Monte Carlo simulations are performed to demonstrate the performance of the proposed distributed dual fusion consistency cooperative localization method (DFT). A comparative analysis of DFT and state-of-the-art distributed co-localization methods is performed. For a fair comparison, distributed algorithms that meet the following criteria are selected: the robots do not need to know the size of the entire system, communication occurs only between robot pairs with relative measurements, and the algorithms ensure consistency. To this end, DFT is compared with the uncorrelated minimum variance estimator (DMV) and the distributed extended information filter (DEIF). In addition, a centralized estimator (Centralized) is used as a baseline, as well as single robot localization (SL, a local estimator based on ego-motion information and absolute measurements) and traditional distributed co-localization (Naive, measurement updates are performed without considering cross-correlations) to demonstrate the importance of relative measurements in improving localization performance.

[0179] Consider a two-dimensional planar scenario involving four differentially driven robots. The robots are initially randomly positioned. The robots move with random linear and angular velocities drawn from a uniform distribution in the range [-0.1, 0.1] m / s and [-0.5, 0.5] rad / s, respectively. Each robot obtains egomotion observations (linear and angular velocities) with standard deviations of 0.08 m / s and 0.03 rad / s, respectively. In addition, each robot has random access to azimuth and range measurements relative to the other robots. The measurement noise is assumed to follow a Gaussian distribution with standard deviations of 0.05 rad and 0.1 m, respectively. Furthermore, all robots intermittently obtain absolute observations of six randomly placed landmarks. The relative measurement of any pair of robots has a detection probability of 60%, while the probability of detecting each landmark is 12%.

[0180] Fifty trials were performed, each lasting 120 seconds with a simulation step of 1.0 seconds. All estimators were implemented using the same parameters and simulated data. The initial state estimates for these estimators were initialized by adding a 0.6m offset around the true position and a 0.2rad offset around the true orientation. To fully validate the performance of the proposed method, one iteration (DFC1) and two iterations (DFC2) were implemented.

[0181] Figure 2 and Figure 3The root mean square error (RMSE) and normalized estimation error square (NEES) for all four robots are shown separately. In addition, the average RMSE and NEES for position and orientation are summarized in Table 1. In the table, the best RMSE value of the estimator other than the centralized estimator is highlighted in bold, and the second best value is highlighted in blue. As expected, the centralized estimator shows the best performance. Compared to SL relying only on ego-motion information and absolute measurements, cooperative localization using relative observations significantly improves localization accuracy.

[0182] Among these distributed co-localization estimators, DFC achieved the best accuracy even with just one iteration, as shown in Table 1. This is because the dual fusion framework enables the robot to fully utilize available information while limiting communication overhead and network connectivity, significantly improving performance. Furthermore, DFC jointly integrates prior estimates and nonlinear measurements while explicitly addressing the independence of measurement noise, resulting in substantial performance improvements compared to DEIF. Furthermore, by leveraging relinearization and iterative optimization, the DFC estimator implemented with two iterations performed even better.

[0183] Table 1 Average root mean square error and normalized square error in Monte Carlo simulation experiments

[0184]

[0185] The proposed collaborative localization algorithm was then validated on the publicly available UTIAS multi-robot collaborative localization and mapping dataset. This dataset consists of nine sub-datasets collected using five two-wheeled differential drive robots under nine different motion trajectories. Each sub-dataset is a collection of egomotion information (linear and angular velocity), relative range-orientation measurements between robots, and relative range-orientation measurements from robots to landmarks.

[0186] The proposed distributed co-localization algorithm is compared with existing methods, including SL, DEIF, DMV, and a centralized estimator. These algorithms are tested on nine sub-datasets, utilizing all available relative measurements for measurement updates. Each algorithm simultaneously estimates the position and orientation of five robots. The initial estimates of these estimators are initialized by adding a 0.6m bias to the true position and a 0.2rad bias to the true orientation. To ensure a fair comparison, all estimators are implemented using the same parameters.

[0187] Figure 4 shows the trajectory of the robot on sub-dataset 9, Figure 5The statistical position and orientation errors of these algorithms on all sub-datasets are shown. Table 2 summarizes the root mean square error of position and orientation, where the best result among the distributed algorithms is highlighted in bold and the second best result is shown in blue. It can be seen that the centralized estimator has the smallest localization error. Among the distributed algorithms, DFC outperforms on almost all sub-datasets (except sub-dataset 5). As expected, the DFC algorithm with two iterations performs better due to relinearization and iterative optimization.

[0188] Table 2 Average root mean square error in actual experiments

[0189]

[0190]

[0191] Figure 6 The communication costs of these decentralized estimators in experiments on 9 sub-datasets are shown. It can be seen that the proposed decentralized estimator DFC requires smaller communication capacity than DEIF and DMV, which is reduced by about 20%, because it does not need to exchange relative measurements.

[0192] This embodiment of the present invention proposes a communication-efficient dual-fusion consensus distributed method for collaborative localization that combines heterogeneous and homogeneous fusion. It also proposes a decentralized heterogeneous fusion algorithm based on MAP to fuse prior estimates of multiple heterogeneous states and universal nonlinear measurements in the presence of unknown cross-correlations. This dual fusion method enables full utilization of all available information without the need to exchange measurements, significantly reducing communication overhead. Extensive Monte Carlo simulations and real-world experiments demonstrate that the proposed method outperforms existing cutting-edge distributed collaborative localization methods.

[0193] Another embodiment of the present invention provides a communication-efficient dual-fusion consistency distributed collaborative positioning system, which includes: for n robot systems, at time k, each robot i, i∈{1,2,…,n} sequentially uses the following modules to obtain its own positioning:

[0194] an ego-motion prediction module configured to predict an ego-state estimate and a covariance estimate of the ego-motion using an extended Kalman filter based on the ego-motion information;

[0195] a heterogeneous fusion module configured such that upon detecting relative measurements, robot i initiates communication with neighboring observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimate with estimates from neighboring observed robots, as well as relative or absolute measurements, wherein the joint fusion process generates a posteriori estimates for itself and its neighboring observed robots;

[0196] The homogeneous fusion module is configured such that after receiving the posterior state estimates and posterior covariance estimates from neighboring observation robots, robot i combines these posterior estimates with its own estimate using the covariance intersection technique to obtain the final position estimate.

[0197] The functions of a communication-efficient dual-fusion consistent distributed collaborative positioning system described in an embodiment of the present invention can be described by the aforementioned communication-efficient dual-fusion consistent distributed collaborative positioning method. Therefore, for the parts not described in detail in the system embodiment, please refer to the above method embodiment and will not be repeated here.

[0198] Although the present invention has been described with respect to a limited number of embodiments, those skilled in the art, having benefit of the foregoing description, will appreciate that other embodiments are contemplated within the scope of the invention thus described. This disclosure is intended to be illustrative rather than restrictive of the scope of the invention, which is defined by the appended claims.

Claims

1. A communication efficient dual fusion consistent distributed collaborative positioning method, characterized by: For a system of n robots, at time k, each robot i, i∈{1,2,…,n} performs the following operations in sequence to obtain its own localization: Ego-motion prediction: Based on the ego-motion information, the extended Kalman filter is used to predict the state estimation and covariance estimation of the self-motion; Heterogeneous fusion: When detecting relative measurements, robot i starts communicating with nearby observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimates with estimates from nearby observed robots, as well as relative or absolute measurements. The joint fusion process generates a posteriori estimates for itself and its nearby observed robots, which include a posteriori state estimates and a posteriori covariance estimates. The specific process of heterogeneous fusion includes: first, determining the optimal joint state estimate Then, the covariance intersection technique is used to design the prior covariance, which is the robot's own covariance estimation. The upper bound W i,k : Among them, ω i 、 Respectively The corresponding weight; Represents the set of neighboring robots observed by the robot; and the prior covariance estimate Replaced by prior covariance The upper bound W i,k , substitute the optimal joint state estimate In the formula, we get: Among them, ||·|| represents the Mahalanobis distance; Indicates the The prior covariance estimate of the robots, represents the weight corresponding to the covariance, r ii and Represent the prior information, absolute measurement and relative measurement residuals respectively; s i,k Represents robot i and its n i The joint state vector of the neighboring observation robots; R ii and denote the covariance of absolute measurement noise and the covariance of relative measurement noise, respectively; Then, for the pairs with prior covariance Upper bound W i,k The optimal joint state estimate Solve and obtain the joint covariance estimate Then, extract the joint covariance estimate The corresponding robot obtained by robot i The posterior covariance estimate of and estimate the posterior covariance by minimizing The optimal weight vector is obtained by Then, the optimal weight vector Substitute back the error state update equation and joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot; Homogeneous fusion: After receiving the posterior state estimates and posterior covariance estimates from neighboring observing robots, robot i combines these posterior estimates with the posterior estimates obtained through its own heterogeneous fusion step using the covariance intersection technique to obtain the final position estimate.

2. A communication efficient dual fusion consistent distributed collaborative positioning method according to claim 1, characterized in that: Determining the optimal joint state estimate The process includes: joint state vector s i,k The optimal estimate of is obtained by solving the following maximum a posteriori estimation optimization problem: Where, p(s i,k |v i,k ,y i,k ) means that at a given v i,k ,y i,k Events i,k The probability distribution of v i,k represents the joint prior estimate of robot i and its neighboring observation robots, y i,k represents the entire measurement vector of all absolute and relative measurements of robot i; Rewrite the above formula and omit s i,k Irrelevant denominator; rewrite and omit the denominator Take the logarithm of both sides of the formula to obtain the optimal joint state estimate for: Where, p(s i,k |v i,k ) indicates that given v i,k Events i,k The probability distribution of Indicates a given Under the event The probability distribution of Indicates a given Under the event The probability distribution of Represents robot i and robot i τ Relative measurement of and Denote robot i and robot i respectively τ The superscript i indicates that these states are estimated by robot i.

3. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 2, characterized in that: The posterior covariance estimate The expression is: Where, is an extraction matrix, except for the robot The relevant entries are all zero except for a d×d identity matrix.

4. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 3 is characterized in that: The error state update equation is: Where H l 、H ii and Respectively represent the Jacobian matrices corresponding to the motion model, the observation model of absolute measurement, and the observation model of relative measurement; Indicates the current linearization point.

5. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 4 is characterized in that: The posterior state estimate of each robot and the posterior covariance estimate for: Where, represents the prior state estimate.

6. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 5, characterized in that: The specific process of isomorphic fusion includes: using covariance crossover technology to perform isomorphic fusion. After isomorphic fusion, the final positioning of robot i at time k is estimated to be: In the formula, η represents the weight vector to be optimized, represents the final state estimate; represents the final covariance estimate; represents the set of neighboring robots that can observe robot i; Indicated by robot Obtain the posterior covariance estimate corresponding to robot i; Indicated by robot Obtain the posterior state estimate corresponding to robot i.

7. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 6, characterized in that: The weight vector to be optimized is estimated by directly minimizing the final covariance It can be determined by the trace or logarithmic determinant of .

8. A communication efficient dual fusion consistency distributed collaborative positioning system, characterized by: For a system of n robots, at time k, each robot i, i∈{1,2,…,n} sequentially utilizes the following modules to obtain its own localization: an ego-motion prediction module configured to predict an ego-state estimate and a covariance estimate of the ego-motion using an extended Kalman filter based on the ego-motion information; The heterogeneous fusion module is configured such that when detecting relative measurements, robot i starts communicating with neighboring robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimates with estimates from neighboring robots, as well as relative or absolute measurements, and the joint fusion process generates a posteriori estimates for itself and its neighboring robots, the a posteriori estimates including a posteriori state estimates and a posteriori covariance estimates; the specific process of the heterogeneous fusion includes: first, determining the optimal joint state estimate Then, the covariance intersection technique is used to design the prior covariance, which is the robot's own covariance estimation. The upper bound W i,k : Among them, ω i 、 Respectively The corresponding weight; Represents the set of neighboring robots observed by the robot; and the prior covariance estimate Replaced by prior covariance The upper bound W i,k , substitute the optimal joint state estimate In the formula, we get: Among them, ||·|| represents the Mahalanobis distance; Indicates the The prior covariance estimate of the robots, represents the weight corresponding to the covariance, r ii and Represent the prior information, absolute measurement and relative measurement residuals respectively; s i,k Represents robot i and its n i The joint state vector of the neighboring observation robots; R ii and denote the covariance of absolute measurement noise and the covariance of relative measurement noise, respectively; Then, for the pairs with prior covariance Upper bound W i,k The optimal joint state estimate Solve and obtain the joint covariance estimate Then, extract the joint covariance estimate The corresponding robot obtained by robot i The posterior covariance estimate of and estimate the posterior covariance by minimizing The optimal weight vector is obtained by Then, the optimal weight vector Substitute back the error state update equation and joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot; A homogeneous fusion module is configured such that upon receiving the posterior state estimates and posterior covariance estimates from neighboring observation robots, robot i combines these posterior estimates with the posterior estimates obtained through its own heterogeneous fusion step using the covariance intersection technique to obtain the final position estimate.

Images