Double-fusion consistency distributed cooperative positioning method and system with efficient communication

Abstract

The invention discloses a communication-efficient dual-fusion consistency distributed cooperative positioning method and system, and relates to the technical field of multi-robot cooperative positioning. The technical key points of the invention are as follows: for a multi-robot system, each robot sequentially executes the following operations to obtain self-positioning: self-motion prediction: based on self-motion information, using an extended Kalman filter to predict self state estimation and covariance estimation; heterogeneous fusion: the robot obtains state estimation and covariance estimation of the adjacently observed robot, and carries out joint fusion on self estimation, estimation from the adjacently observed robot and relative or absolute measurement; and isomorphic fusion: using a covariance intersection technology to combine posterior estimation obtained by joint fusion with self estimation so as to obtain final position estimation. According to the method, heterogeneous and isomorphic fusion is combined, the requirements on storage, communication load and network connectivity are lowest, and all available information is ensured to be fully utilized.

Description

Technical Field

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

[0002] For multi-robot systems, co-localization (CL), i.e., 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, where 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 interrelationships between robots and achieve considerable accuracy, they are susceptible to single points of failure and lack scalability as the team size increases, which makes their deployment challenging and unable to meet practical needs.

[0003] To enhance robustness and scalability, a lot of 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 of distributed architectures is handling inter-robot correlations to prevent double counting. This problem occurs when robots treat shared information as irrelevant, which leads to degraded localization accuracy and unreliable results. Summary of the invention

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

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

[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 neighboring observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimates with those from neighboring observed robots, as well as relative or absolute measurements, and the joint fusion process produces a posteriori estimates for itself and its neighboring observed robots;

[0008] Homomorphic fusion: 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.

[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, that is, the robot's own covariance estimate 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 prior covariance estimate of the robots, represents the weight corresponding to the covariance, r ii and Respectively represent the prior information, absolute measurement and relative measurement residuals; 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 optimal weight vector is obtained by

[0018] Then, the optimal weight vector Substitute back the error state update equation and the 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] The joint state vector s i,k The optimal estimate of is obtained by solving the following maximum a posteriori estimation optimization problem:

[0021]

[0022] In the formula, 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] In the formula, 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] In the formula, is an extraction matrix, except for the robot The relevant entries are zero except for a d×d identity matrix.

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

[0030]

[0031] In the formula, 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 estimate of each robot and the posterior covariance estimate for:

[0033]

[0034] In the formula, 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 estimation of robot i at time k after homogeneous fusion is:

[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 The obtained a posteriori state estimate corresponding to robot i.

[0038] Furthermore, the weight vector to be optimized is obtained by directly minimizing the final covariance estimate 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 comprising: 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 using an extended Kalman filter based on the ego-motion information;

[0041] A heterogeneous fusion module, which is configured such that when detecting relative measurements, robot i starts communicating 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, and relative or absolute measurements, and the joint fusion process generates a posteriori estimates for itself and its neighboring observed robots;

[0042] A 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 a final position estimate.

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

[0044] The present invention proposes a fully distributed collaborative positioning method based on a dual fusion framework. The method combines heterogeneous and homogeneous fusion, with minimal requirements on storage, communication load and network connectivity, ensuring full utilization of all available information. In addition, a theoretical proof of the consistency of the proposed method is provided; among them, 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) and a universal nonlinear measurement in the case of unknown cross-covariance between prior estimates. Monte Carlo simulations and real experiments were carried out respectively, and the experiments proved that the method proposed in the present invention is superior to 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 It 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 is an example diagram of the root mean square error (RMSE) of the position and orientation of all four robots in the 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 4 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 4 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 scheme of the present invention, exemplary implementations or embodiments of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described implementations or embodiments are only implementations or embodiments of a part of the present invention, not all of them. Based on the implementations or embodiments of the present invention, all other implementations or embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present invention.

[0053] In order to achieve collaborative localization under limited communication capabilities and unreliable network connections, this paper proposes a consistent and efficient distributed method with minimal requirements for storage, communication burden and network connection. The method 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 is only carried out when there are relative measurements between robot pairs. Neighboring robot pairs only exchange local estimates without transmitting relative measurements, thereby reducing the communication burden. In addition, under the dual fusion framework, the observed robot can make full use of all available information of the observing robot, thereby obtaining better estimation performance. The method supports a universal measurement model.

[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 neighboring observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimates with those from neighboring observed robots, as well as relative or absolute measurements, and the joint fusion process produces a posteriori estimates for itself and its neighboring observed robots;

[0057] Homomorphic fusion: 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.

[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, that is, the robot's own covariance estimate The upper bound W i,k ; and the prior covariance estimate 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 optimal weight vector is obtained by Then, the optimal weight vector Substitute back the error state update equation and the joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot.

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

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

[0062]

[0063] In the formula, 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] In the formula, 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.

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

[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 The obtained a posteriori state estimate corresponding to robot i.

[0070] Another embodiment of the present invention proposes a communication efficient dual fusion consistent distributed collaborative positioning method, in which a multi-robot collaborative positioning system consisting of n robots is considered, and all robots have perception, computing and communication capabilities. These robots use measurements and information exchanged with other robots for self-positioning. Specifically, each robot is equipped with a proprioceptive sensor to obtain self-motion information, and carries an external perception sensor to perform relative measurements between robots, and absolute measurements from identifiable landmarks when available. The scale n of the multi-robot system may change dynamically over time and does not need to be known to the robots.

[0071] For a 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, represents 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. Relative measurement y 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 It means that it follows a zero-mean Gaussian distribution. of 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 a covariance R ii,k Zero-mean Gaussian noise.

[0080] The present invention aims to introduce a consistent distributed co-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. This estimator only keeps track of 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 states 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] where F i,k and G i,k is the Jacobian matrix obtained by calculation:

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

[0086] 3) Homomorphic fusion: After receiving the states 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 in a recursive manner.

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

[0089] 2) Heterogeneous Fusion Algorithm

[0090] In this section, heterogeneous fusion is formulated as a maximum a posteriori (MAP) estimation problem and its analytical solution is derived. Specifically, in order to account for the unknown cross-covariance between different robot prior estimates, the covariance intersection (CI) technique is used to introduce an upper bound on the prior covariance. Then, the unknown parameters involved in the state and covariance estimates as well as the covariance bound are determined in turn.

[0091] A. Heterogeneous fusion formula based on MAP

[0092] At time k, assume that robot i obtains relative measurement information from its neighboring observation robots and absolute measurement information from 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 by y i,k, can be written as follows:

[0093]

[0094] in and 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 represents 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 maintains only its own state estimate.

[0100] Let 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 cross-covariance i,k Perform joint fusion to obtain the joint state estimate s i,k .

[0103] The 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. Based on 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 due to the joint prior covariance matrix is unknown in (6), so it is often not feasible to achieve an optimal estimate by minimizing (10). 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), by Replace with W i,k ,get

[0118]

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

[0120]

[0121] and r ii and Respectively represent the prior information, absolute measurement and relative measurement residuals, 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 by a first-order Taylor expansion and asymptotically moves to the minimum point. The current linearization point is denoted as:

[0124]

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

[0126]

[0127] Among them, H j , H ii and is the corresponding Jacobian matrix, δs i,k is the state correction 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 each robot’s local state estimate is reduced. Therefore, we choose to minimize the autocovariance estimate of each robot individually 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 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. In order to enhance the robustness and controllability of the optimization process, the trusted region is dynamically adjusted according to 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 proposed in (25) can be solved within a limited number of iterations.

[0139] According to (25), we can A weight vector is obtained to minimize its corresponding autocovariance estimate. Then, the state and covariance estimate of each robot are updated separately using the corresponding weight vector as shown below.

[0140]

[0141] where δs i,k (·)and They are given in (22) and (23) respectively. By letting And for all Repeating the above update process, it can be iterated 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 the above heterogeneous fusion, robot i can generate state and covariance estimates for itself and its neighboring observed robots. The state and covariance estimates of these neighboring observed robots are then transmitted back to the corresponding robot for further fusion, which will be discussed in the subsequent subsections. It should be noted that the measurement data do not need to be transmitted because they have been incorporated into the posterior estimates during the heterogeneous fusion process. This greatly reduces the communication load.

[0143] 3) Homogeneous Fusion Algorithm

[0144] At time k, assume 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 neighboring observation robot i ι The received state and covariance estimates are expressed as and in Then, robot i performs homogeneous fusion and converts the internal estimation With the received estimate Combined to get the final estimated result.

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

[0146]

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

[0148]

[0149] For the uniform fusion problem in equations (28) and (29), we can directly minimize 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 η * This can be achieved by substituting back into (28) and (29).

[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 result obtained at time step k-1 is consistent, that is,

[0161]

[0162] For any i∈{1,2,…,n}. According to (4), applying the linearized state model, the self-motion prediction 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 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 through 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 Be consistent Then, since the covariance intersection fusion strategy maintains consistency, the estimated pair obtained by (28) and (29) 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 performed 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 show 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 placed at random positions. The robots move with random linear and angular velocities drawn from uniform distributions in the range [-0.1, 0.1] m / s and [-0.5, 0.5] rad / s, respectively. Each robot obtains ego-motion 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. In addition, 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] 50 trials were performed, each lasting 120 s with a simulation step of 1.0 s. All estimators were implemented using the same parameters and simulated data. The initial state estimates of these estimators were initialized by adding an offset of 0.6 m around the true position and 0.2 rad around the true orientation. To fully verify the performance of the proposed method, one iteration (DFC1) and two iterations (DFC2) were implemented, respectively.

[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 with SL relying only on ego-motion information and absolute measurements, cooperative localization using relative observations significantly improves the localization accuracy.

[0182] Among these distributed co-localization estimators, DFC shows the best accuracy even with only one iteration, as shown in Table 1. This is because the dual fusion framework enables the robot to fully utilize the available information with limited communication overhead and network connectivity, thus significantly improving the performance. In addition, DFC jointly integrates the prior estimate and nonlinear measurement while explicitly handling the independence of measurement noise, which brings substantial performance improvement compared to DEIF. In addition, by leveraging relinearization and iterative optimization, the DFC estimator implemented with two iterations performs better.

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

[0184]

[0185] The proposed co-localization algorithm is then validated on the publicly available UTIAS multi-robot co-localization and mapping dataset. This dataset consists of nine sub-datasets collected using five two-wheel differential drive robots under nine different motion trajectories. Each sub-dataset is an organic collection of self-motion 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 centralized estimators. The algorithms are tested on nine sub-datasets, using all available relative measurements for measurement updates. Each algorithm estimates the position and orientation of five robots simultaneously. The initial estimates of these estimators are initialized by adding a bias of 0.6m to the true position and 0.2rad to the true orientation. To ensure a fair comparison, all estimators are implemented with 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 errors of position and orientation, where the best performing 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 the 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, about 20% less, compared to DEIF and DMV, because it does not need to exchange relative measurements.

[0192] The embodiment of the present invention proposes a communication efficient dual fusion consistency distributed method for collaborative positioning, which combines heterogeneous and homogeneous fusion; a decentralized heterogeneous fusion algorithm based on MAP is proposed to fuse prior estimates of multiple heterogeneous states and universal nonlinear measurements in the presence of unknown cross-correlations. The dual fusion method enables all available information to be fully utilized without the need to exchange measurements, thereby significantly reducing the communication load. A large number of Monte Carlo simulations and actual experiments show that the proposed method is superior to existing cutting-edge distributed collaborative positioning methods.

[0193] Another embodiment of the present invention provides a communication efficient dual fusion consistency distributed collaborative positioning system, the system comprising: 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 using an extended Kalman filter based on the ego-motion information;

[0195] A heterogeneous fusion module, which is configured such that when detecting relative measurements, robot i starts communicating 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, and relative or absolute measurements, and the joint fusion process generates a posteriori estimates for itself and its neighboring observed robots;

[0196] A 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 a 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 explained 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 according to a limited number of embodiments, it will be apparent to those skilled in the art, with the benefit of the above description, that other embodiments are contemplated within the scope of the invention thus described. The disclosure of the present invention 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 in that: 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 neighboring observed robots to obtain their state estimates and covariance estimates; robot i jointly fuses its own estimates with those from neighboring observed robots, as well as relative or absolute measurements, and the joint fusion process produces a posteriori estimates for itself and its neighboring observed robots; Homomorphic fusion: 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.

2. A communication efficient dual fusion consistent distributed collaborative positioning method according to claim 1, characterized in that: The specific process of the heterogeneous fusion includes: First, determine the optimal joint state estimate Then, the covariance intersection technique is used to design the prior covariance, that is, the robot's own covariance estimate The upper bound W i,k : 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: Among them, ||·|| represents the Mahalanobis distance; represents the prior covariance estimate of the lth robot, ω l Represents the weight corresponding to the covariance, r l 、r ii and Respectively represent the prior information, absolute measurement and relative measurement residuals; 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 posterior autocovariance estimate corresponding to robot l obtained by robot i is and estimate the posterior autocovariance by minimizing The optimal weight vector is obtained by Then, the optimal weight vector Substitute back the error state update equation and the joint covariance estimate , obtain the posterior state estimate and posterior covariance estimate of each robot.

3. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 2, characterized in that: Determining the optimal joint state estimate The process includes: The joint state vector s i,k The optimal estimate of is obtained by solving the following maximum a posteriori estimation optimization problem: In the formula, 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: In the formula, 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.

4. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 3 is characterized in that: The posterior autocovariance estimate The expression is: In the formula, is an extraction matrix that is zero except for the entry associated with robot l, which is a d×d identity matrix.

5. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 4, characterized in that: The error state update equation is: In the formula, 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.

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

7. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 6, characterized in that: 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 as: 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; represents the posterior autocovariance estimate corresponding to robot i obtained by robot l; represents the posterior state estimate corresponding to robot i obtained by robot l.

8. The communication efficient dual fusion consistent distributed collaborative positioning method according to claim 7, 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 .

9. A communication efficient dual fusion consistent distributed collaborative positioning system, characterized in that: For a system of n robots, at time k, each robot i, i∈{1,2,…,n} sequentially uses the following modules to obtain its own positioning: an ego-motion prediction module configured to predict an ego-state estimate and a covariance estimate using an extended Kalman filter based on the ego-motion information; A heterogeneous fusion module, which is configured such that when detecting relative measurements, robot i starts communicating 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, and relative or absolute measurements, and the joint fusion process generates a posteriori estimates for itself and its neighboring observed robots; A 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 a final position estimate.

Images