Robust distributed cooperative positioning method for mobile unmanned system

Abstract

The invention relates to a robust distributed cooperative positioning method for a mobile unmanned system, and belongs to the technical field of navigation positioning. Comprising the steps that GNSS hidden variables are introduced into a GNSS observed quantity model, a paraphrasing function of the GNSS hidden variables is obtained, and the GNSS hidden variables comprise unknown GNSS pseudo-range measurement deviation and unknown GNSS measurement noise accuracy caused by non-ideal error factors; introducing UWB hidden variables into the UWB observed quantity model and obtaining a paraphrasing function of the UWB hidden variables, wherein the UWB hidden variables comprise unknown observed quantity deviation and unknown UWB ranging noise accuracy caused by non-ideal error factors; modeling is carried out for GNSS hidden variables and UWB hidden variables; based on a hidden variable model, establishing joint posterior distribution of all vehicles and hidden variables as a distributed cooperative positioning problem objective function; and adopting an alternative estimation method of an agent state and a hidden variable for the joint posterior distribution to realize distributed positioning of the mobile unmanned system.

Description

Technical Field

[0001] The present invention relates to a robust distributed cooperative positioning method for mobile unmanned systems, belonging to the technical field of navigation and positioning. Background Art

[0002] Mobile unmanned systems, including unmanned ground vehicles (UGVs), unmanned aerial vehicles (UAVs), multi-robots, etc., have been widely used in civil and military fields such as smart cities, transportation and rescue. High-precision and high-reliability positioning is crucial for mobile unmanned systems and is a prerequisite for tasks such as perception, planning and control. Existing global navigation satellite system (GNSS) solutions, such as precise point positioning (PPP), are vulnerable to non-line-of-sight (NLOS) and multipath environments in urban areas, resulting in abnormal observables and deteriorated positioning accuracy. In contrast, agents in mobile unmanned systems can communicate with adjacent agents through ultra-wideband (UWB) sensors to obtain relative observables and exchange information such as positions, so as to obtain additional observables and overcome the limitations of local measurements. Therefore, cooperative positioning based on GNSS and UWB observables is expected to provide continuous, accurate and reliable position information for mobile unmanned systems.

[0003] In practical applications, factors such as non-line-of-sight (NLOS), sensor failures and platform vibrations make the observables obtained by agents contain non-ideal errors, which in turn lead to uncertainties in the ideal observation model. Therefore, it is necessary to study robust cooperative positioning to improve the reliability and robustness of positioning.

[0004] Commonly used robust cooperative positioning methods include fault detection and exclusion (FDE), M-estimation and measurement noise modeling. FDE judges the observables by constructing a detection statistic and improves the positioning robustness by eliminating abnormal observables. However, it is difficult for FDE to achieve a balance between low false alarm rate and low miss detection rate. M-estimation reduces the impact of abnormal observables by adjusting the weights of different observables, but it ignores the characteristics of noise, so the positioning performance is limited. Measurement noise modeling models the observable noise using a specific distribution cluster and improves the positioning robustness by jointly estimating the agent state and the noise model statistic. However, existing measurement noise modeling methods all assume that the noise mean is 0, so they cannot effectively cope with the situation where there are significant measurement biases in the observables. Summary of the Invention

[0005] In view of this, the present invention proposes a robust distributed cooperative positioning method for mobile unmanned systems. The present invention simultaneously models the unknown measurement biases and the statistical characteristics of measurement noise contained in GNSS and UWB observables through latent variables, and uses the Gaussian-gamma conjugate distribution to model the latent variables. Further, the present invention proposes an alternating estimation method for the agent state and the latent variables to achieve distributed robust positioning of mobile unmanned systems.

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

[0007] A robust distributed cooperative positioning method for mobile unmanned systems, the specific process is as follows:

[0008] Step 1, introduce GNSS latent variables into the GNSS observation model and obtain its likelihood function. The GNSS latent variables include unknown GNSS pseudorange measurement bias b i,n and unknown GNSS measurement noise precision λ i,n ; introduce UWB latent variables into the UWB observation model and obtain its likelihood function. The UWB latent variables include unknown observation bias b i,j and unknown UWB ranging noise precision λ i,j ;

[0009] Step 2, model the GNSS latent variables and model the UWB latent variables;

[0010] Step 3, based on the latent variable model, establish the joint posterior distribution of all vehicles and latent variables as the objective function of the distributed cooperative positioning problem;

[0011] Step 4, adopt an alternating estimation method of surrogate states and latent variables for the joint posterior distribution to achieve distributed positioning of the mobile unmanned system.

[0012] Optionally, the present invention models the latent variable model based on the Gaussian-Gamma conjugate distribution,

[0013] The model of the GNSS latent variable is:

[0014] f(b i,n , λ i,n ) = NGa(μ i,n , κ i,n , α i,n , β i,n )

[0015] Among them, NGa(·) represents the Gaussian-Gamma conjugate distribution, and the variables μ i,n , κ i,n , α i,n and β i,n are hyperparameters that control the shape of the conjugate distribution f(b i,n , λ i,n ) of the GNSS latent variable;

[0016] The model of the UWB latent variable is:

[0017] f(b i,j , λ i,j ) = NGa(μ i,j, κ i,j , α i,j , β i,j )

[0018] Among them, the variable μ i,j , κ i,j , α i,j and β i,j are hyperparameters for controlling the shape of the conjugate distribution of the UWB latent variable f(b i,j , λ i,j ).

[0019] Optionally, the joint posterior distribution of the vehicle and the latent variable in the present invention is:

[0020] Let the GNSS and UWB observation sets of all agents be ρ = [ρ1,..., ρ M T , the latent variable sets of the GNSS and UWB observations of all agents be b = [b1,..., b M T and λ = [λ1,..., λ M T , and the state vector to be estimated for all agents be θ = [θ1,..., θ M T ; Assuming that the likelihood functions of different observations are conditionally independent, the joint likelihood function of ρ is expressed as:

[0021]

[0022] Among them, the function f(ρ i,j |θ i , θ j , b i,j , λ i,j ) represents the GNSS observation likelihood function, f(ρ i,n |θ i , b i,n , λ i,n ) represents the UWB observation likelihood function, and the set is used to represent the set of satellites visible to agent i, s represents the satellite constellation s ∈ {G, C, E, R}, where G, C, E, and R represent the GPS, Beidou, Galileo, and GLONASS navigation satellite constellations respectively, and the set represents the set of adjacent agents visible to agent i, ρ i,n represents the pseudorange observation, ρ i,j represents the UWB ranging observation, and M represents the number of agents;

[0023] The goal of robust cooperative positioning of mobile unmanned systems is to simultaneously estimate the joint posterior distribution f(θ, b, λ|ρ) of all vehicles and latent variables ​​​​

[0024]

[0025] Among them, f 0 (θ i ) represents the combined prior information of agent i, and f 0 (b i,n , λ i,n ) represents the prior information of agent i about the GNSS latent variable, and f 0 (b i,j , λ i,j ) represents the prior information of agent i about the UWB latent variable.

[0026] Optionally, the marginal posterior of the state of agent i in the (l + 1)-th iteration of the present invention, that is, the positioning of agent i is q l+1 (θ i ) is as follows:

[0027]

[0028] Among them, the superscript l represents the l-th iteration, g i,n is the Jacobian vector of the GNSS observation with respect to the agent state, g i,j,1 and g i,j,2 are the Jacobian vectors of the UWB observation with respect to the state of agent i and the state of agent j respectively. The superscript T represents the transpose, r i,n and r i,j are the residuals of the GNSS and UWB observations respectively, is the prior variance of the state quantity, represents the posterior mean of the state variable of agent j in the l-th iteration, is the prior mean of the state variable.

[0029] Optionally, the estimated result of the marginal posterior of the latent variable in the (l + 1)-th iteration of the present invention is:

[0030]

[0031]

[0032] Optionally, the present invention calculates and updates iteratively according to the agent state, the observation latent variable, and the marginal posterior. When the iteration converges, that is, the marginal posterior of the state vector of each agent and the marginal posterior of the observation latent variable are obtained, thus realizing distributed robust cooperative positioning.

[0033] Optionally, the convergence in the present invention is: if the difference between the errors of two adjacent iterations is less than a set threshold, it is considered that the iteration converges.

[0034] Beneficial effects

[0035] The present invention is applicable to robust cooperative positioning in scenarios with non-ideal observed quantities. First, the uncertainties of GNSS and UWB observed quantities are considered, latent variables of the observed quantities are introduced to characterize the biases and noise characteristics of the observed quantities caused by non-ideal factors, and an objective function for robust cooperative positioning is constructed. Finally, closed-form solutions for the posterior distributions of the agent state vectors and the latent variables of the observed quantities are derived within the variational message passing framework, enabling all agents in the mobile unmanned system to distributively estimate their own states in a closed-form solution manner and achieve robust cooperative positioning. Brief Description of the Drawings

[0036] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings based on these drawings without creative efforts.

[0037] Figure 1 A robust distributed cooperative positioning method for mobile unmanned systems. Detailed Embodiments

[0038] The embodiments of the present invention will be described in detail below with reference to the drawings.

[0039] It should be noted that, without conflict, the following embodiments and the features in the embodiments may be combined with each other; and, based on the embodiments in the present disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of the present disclosure.

[0040] It should be noted that the following describes various aspects of embodiments within the scope of the appended claims. It should be apparent that the aspects described herein may be embodied in a wide variety of forms, and any specific structure and / or function described herein is illustrative only. Based on the present disclosure, those skilled in the art should understand that one aspect described herein may be implemented independently of any other aspect, and two or more of these aspects may be combined in various ways. For example, any number of aspects described herein may be used to implement a device and / or practice a method. Additionally, this device and / or this method may be implemented using other structures and / or functions in addition to one or more of the aspects described herein.

[0041] As Figure 1 shown, the embodiments of the present application propose a robust distributed cooperative positioning method for mobile unmanned systems, including four steps: GNSS and UWB observed quantity modeling, latent variable modeling of the observed quantities, positioning problem construction, and distributed cooperative positioning.

[0042] Without loss of generality, the multi-agent system is described as follows: Suppose a mobile unmanned system with M mobile agents. Each agent is equipped with a GNSS receiver that can receive multi-frequency and multi-constellation GNSS signals. In addition, each agent is equipped with a UWB sensor that can perform relative measurements and communication with adjacent agents. The effective range of UWB is d th . At the t-th positioning epoch, use to represent the set of connections between all agents and satellites, and use to represent the set of connections between agents.

[0043] For agent i, use the set to represent the set of satellites visible to agent i, where s represents the satellite constellation s ∈ {G, C, E, R}, where G, C, E, and R represent the GPS, Beidou, Galileo, and GLONASS navigation satellite constellations respectively. The set represents the set of adjacent agents visible to agent i. The position of agent i is represented as The clock offset between agent i and the satellite system s is δ i,s . The state vector to be estimated for agent i can be represented as where c represents the speed of light. The set of state vectors of all agents is represented as θ.

[0044] (1) GNSS Observation Modeling and UWB Observation Modeling

[0045] A. GNSS Observation Modeling

[0046] The pseudo-range observation ρ between agent i and satellite i,n can be modeled as

[0047]

[0048] where, represents the position of satellite n at the time of transmitting the navigation signal. δ n represents the clock offset between satellite n and the satellite constellation s, I i,n and T i,n represent the measurement biases caused by ionospheric and tropospheric delays. ∈ i,n represents Gaussian measurement noise, and its distribution is N(,) represents the Gaussian distribution, and σ i,n is the unknown noise standard deviation. In addition, h(θ i , a n ) = ‖a n - p i ‖ + cδ i,s represents the relationship with the state vector θ i of agent i and the satellite position a nThe related ideal GNSS observable model. In addition, introduce the latent variable b i,n to represent the unknown GNSS pseudorange measurement deviation caused by non-ideal error factors, and further introduce the latent variable to represent the unknown measurement noise precision.

[0049] After each agent obtains the pseudorange observable ρ i,n , the following steps are required to correct the observable for positioning. First, calculate the satellite signal transmission time position a n and the satellite clock offset δ n based on the satellite ephemeris parameters. In addition, by combining the pseudorange observables belonging to different frequencies, the ionospheric delay error I i,n can be eliminated. Further, based on the satellite elevation angle and the Saastamoinen model, the tropospheric delay error T i,n is eliminated. Thus, the pseudorange observable after error elimination can be expressed as

[0050] ρ i,n = h(θ i , a n ) + b i,n + ∈ i,n (2)

[0051] The likelihood function of this observable can be expressed as

[0052]

[0053] B. UWB observable modeling

[0054] For adjacent agents i and j, the UWB ranging observable ρ i,j between them can be expressed as

[0055]

[0056] where ∈ i,j is Gaussian measurement noise and follows the distribution h(θ i , θ j ) = ‖p i - p j ‖ represents the ideal UWB ranging model. Similar to the GNSS observable, introduce the latent variable b i,j to represent the unknown observable deviation caused by non-ideal error factors, and introduce the latent variable to represent the unknown UWB ranging noise precision. Based on (4), the UWB observable likelihood function can be expressed as

[0057]

[0058] (2) Observed Variable Latent Variable Modeling

[0059] Latent variable b i,n , λ i,n represent the unknown bias and noise precision of GNSS pseudorange observations. Latent variable b i,j , λ i,j represent the unknown bias and noise precision of UWB ranging observations. The latent variable distribution will directly affect the observed variable likelihood function and the final positioning result. In the present invention, the Gaussian-Gamma conjugate distribution is used to model the latent variable, that is:

[0060]

[0061]

[0062] where N(·) represents the Gaussian distribution, Ga(·) represents the Gamma distribution, and Γ(·) represents the Gamma function. Variables μ i,n , κ i,n , α i,n and β i,n are hyperparameters that control the shape of the GNSS latent variable conjugate distribution f(b i,n , λ i,n ). The use of the Gaussian-Gamma distribution has mathematical closure and physical mechanism compatibility. The Gaussian-Gamma, as the conjugate prior of a Gaussian process with unknown mean and precision, ensures a closed-form for parameter updates, thus improving computational efficiency. In addition, the Gaussian component is used to model the bias b i,n , representing the central tendency of the observed variable bias. The Gamma component is used to model the noise precision to capture the variability of the noise and satisfy the domain of definition [0, ∞) of the noise precision. Similarly, variables μ i,j , κ i,j , α i,j and β i,j are hyperparameters that control the shape of the UWB latent variable conjugate distribution f(b i,j , λ i,j ).

[0063] (3) Positioning Problem Construction

[0064] At positioning epoch t, each agent has prior information about its own state variables where is the prior mean of the state variable, and P i 0 is the prior variance. The prior information can be calculated based on the posterior estimation result of the state variable at the previous positioning epoch t - 1 and the state transition model. The joint prior information of all agents is expressed as In addition, each agent has prior information about the observed variable latent variable

[0065]

[0066] Among them, and are respectively the hyperparameter prior information.

[0067] Thus, the joint prior distribution of all latent variables can be expressed as

[0068]

[0069] Define the set of all GNSS and UWB observables related to agent i as ρ i , and the set of latent variables of all GNSS and UWB observables related to agent i as b i , λ i . Define the set of GNSS and UWB observables of all agents as ρ = [ρ1,..., ρ M T , the set of latent variables of all agents' GNSS and UWB observables as b = [b1,..., b M T and λ = [λ1,..., λ M T . By assuming that the likelihood functions of different observables are conditionally independent, the joint likelihood function of ρ can be written as:

[0070]

[0071] Thus, the goal of robust cooperative localization for mobile unmanned systems is to simultaneously estimate the joint posterior distribution f(θ, b, λ|ρ) of all vehicles and latent variables, that is

[0072]

[0073] For each agent, its goal is to estimate the marginal posterior distribution of its own state vector.

[0074] (4) Distributed cooperative localization

[0075] To achieve the marginal posterior estimation of each agent's state vector, the present invention proposes an iterative distributed cooperative localization method based on variational message passing. Assume that after the l-th iteration, the estimated result of the marginal posterior q l (θ j ) of agent i is

[0076]

[0077] Among them, and ​​​They are the posterior mean and variance estimated in the $l$-th round of iteration respectively. The result of the marginal posterior estimation of the latent variable is

[0078]

[0079] For the initial iteration ($l = 0$), we have $q$ l (b i,n , λ i,n ) = f(b i,n , λ i,n ), $q$ l (b i,j , λ i,j ) = f(b i,j , λ i,j ) and $q$ l (θ j ) = f(θ j ). In addition, in the initial iteration, based on the prior information, the GNSS and UWB observation models are linearized, and we can obtain

[0080]

[0081] where, is the Jacobian vector of the GNSS observation with respect to the state of agent $i$. and are the Jacobian vectors of the UWB observation with respect to the state of agent $i$ and the state of agent $j$ respectively. $r$ i,n and $r$ i,j are the GNSS and UWB observation residuals respectively.

[0082] Based on the results of the $l$-th round of iteration (12), (13) and the linearization result of the observations (14), in the distributed cooperative positioning method of the present invention, the result of the marginal posterior estimation of the state of agent $i$ in the $l$-th round of iteration can be calculated as follows:

[0083]

[0084] where denotes taking the expectation of $f(θ, b, λ|ρ)$ with respect to $q$ l (b, λ). The solution of equation (15) can be expressed as

[0085]

[0086] where and are the posterior mean and variance obtained in the $(l + 1)$-th round of iteration respectively, and the specific forms can be written as

[0087]

[0088] After each agent obtains the estimation result of its own state vector in the (l + 1)-th round of iteration, it updates the posterior of the latent variable related to it, and the calculation method is

[0089]

[0090] The solution of formula (18) can be expressed as

[0091]

[0092] where and are the updated hyperparameter values of the latent variables of GNSS and UWB observations, and their specific forms can be written as

[0093]

[0094] Thus, the iterative update calculation of the marginal posterior of the agent state and the latent variable of the observation is realized. When the iteration converges, the marginal posterior of each agent state vector and the marginal posterior of the latent variable of the observation are obtained, thus realizing distributed robust cooperative positioning.

[0095] In summary, the above is only a preferred embodiment of the present invention, and is not intended to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robust distributed cooperative localization method for mobile unmanned systems, characterized in that, The specific process is as follows: Step 1: Introduce GNSS latent variables into the GNSS observation model and obtain its likelihood function. The GNSS latent variables include the unknown GNSS pseudorange measurement bias b caused by non-ideal error factors i,n and the unknown GNSS measurement noise precision λ i,n ; Introduce UWB latent variables into the UWB observation model and obtain its likelihood function. The UWB latent variables include the unknown observation bias b caused by non-ideal error factors i,j and the unknown UWB ranging noise precision λ i,j ; Step 2: Model the GNSS latent variables and the UWB latent variables; Step 3: Based on the latent variable model, establish the joint posterior distribution of all vehicles and latent variables as the objective function of the distributed cooperative localization problem; Step 4: Use the alternating estimation method of the surrogate state and latent variables for the joint posterior distribution to achieve distributed localization of the mobile unmanned system.

2. The robust distributed cooperative positioning method for mobile unmanned systems according to claim 1, wherein, Model the latent variable model based on the Gaussian-Gamma conjugate distribution, The model of the GNSS latent variable is: f(b i,n ,λ i,n ) = NGa(μ i,n ,κ i,n ,α i,n ,β i,n ) Among them, NGa(·) represents the Gaussian-Gamma conjugate distribution, and the variables μ i,n , κ i,n , α i,n and β i,n are hyperparameters for controlling the shape of the conjugate distribution f(b i,n , λ i,n ) of the GNSS latent variable; The model of the UWB latent variable is: f(b i,j ,λ i,j ) = NGa(μ i,j ,κ i,j ,α i,j ,β i,j ) Among them, the variable μ i,j , κ i,j , α i,j and β i,j are hyperparameters that control the shape of the conjugate distribution f(b i,j , λ i,j ) of the UWB latent variable.

3. The robust distributed cooperative localization method for mobile unmanned systems according to claim 2, characterized in that, The joint posterior distribution of the vehicle and latent variables is: Let the set of GNSS and UWB observations of all agents be ρ = [ρ1,..., ρ M T , and the set of latent variables of GNSS and UWB observations of all agents be b = [b1,..., b M T and λ = [λ1,..., λ M T , and the state vectors to be estimated for all agents be θ = [θ1,..., θ M T ; Assuming that the likelihood functions of different observations are conditionally independent, the joint likelihood function of ρ is expressed as:​​​​ Among them, the function f(ρ i,j |θ i , θ j , b i,j , λ i,j ) represents the likelihood function of GNSS observables, and f(ρ i,n |θ i , b i,n , λ i,n ) represents the likelihood function of UWB observables. The set is used to represent the set of satellites visible to agent i, s represents the satellite constellation s ∈ {G, C, E, R}, where G, C, E, and R represent the GPS, Beidou, Galileo, and GLONASS navigation satellite constellations respectively. The set represents the set of neighboring agents visible to agent i. ρ i,n represents the pseudorange observable, and ρ i,j represents the UWB ranging observable. M represents the number of agents; The goal of robust cooperative localization of the mobile unmanned system is to simultaneously estimate the joint posterior distribution f(θ, b, λ|ρ) of all vehicles and latent variables Among them, f 0 (θ i ) represents the combined prior information of agent i, f 0 (b i,n , λ i,n ) represents the prior information of agent i about the GNSS latent variable, f 0 (b i,j , λ i,j ) represents the prior information of agent i about the UWB latent variable.

4. The robust distributed cooperative localization method for mobile unmanned systems according to claim 3, characterized in that The posterior of the state margin of agent i after the (l + 1)-th round of iteration, i.e., the localization of agent i is q l+1 (θ i ): where the superscript \(l\) represents the \(l\)-th iteration, \(g\) i,n is the Jacobian vector of the GNSS measurement with respect to the agent state, \(g\) i,j,1 and \(g\) i,j,2 are the Jacobian vectors of the UWB measurement with respect to the state of agent \(i\) and the state of agent \(j\) respectively. The superscript \(T\) represents the transpose, \(r\) i,n and \(r\) i,j are the GNSS and UWB measurement residuals respectively, is the prior variance of the state quantity, represents the posterior mean of the state variable of agent \(j\) at the \(l\)-th iteration, is the prior mean of the state variable.

5. The robust distributed cooperative positioning method for mobile unmanned systems according to claim 4, wherein The result of the latent variable marginal posterior estimation in the (l + 1)-th iteration is:

6. The robust distributed cooperative positioning method for mobile unmanned systems according to claim 4, wherein According to the surrogate state and the observed variable latent variable and marginal posterior iterative update calculation, when the iteration converges, the marginal posterior of each surrogate state vector and the marginal posterior of the observed variable latent variable are obtained, thus realizing distributed robust cooperative localization.

7. The robust distributed cooperative localization method for mobile unmanned systems according to claim 6, wherein The convergence is defined as: if the error difference between two adjacent iterations is less than the set threshold, the iteration is considered to converge.

Images