Robust cooperative positioning method for multi-epoch observed quantity association under model non-adaptation

Abstract

The invention provides a multi-epoch associated robust cooperative positioning method under model non-adaptation. The method comprises the steps of observed quantity model modeling, multi-epoch observed quantity association, observed quantity hidden variable modeling and distributed robust positioning. Considering that the environment where the agent is located in a short time is similar, and strong correlation exists between non-adaptive models of different positioning epochs and observed quantity deviation, the state of the agent changing along with time is restrained through a state transition equation, and hidden variables are introduced to represent the deviation and noise of the multi-epoch observed quantity. Observed quantity deviation and positioning performance deterioration caused by non-adaptation of an observation model are eliminated, and positioning robustness is enhanced.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation and positioning, and in particular relates to a robust collaborative positioning method for multi-epoch observation association under model non-adaptation. Background Art

[0002] Multi-agent systems (MAS), including unmanned ground vehicles (UGVs), unmanned aerial vehicles (UAVs), and multi-robot systems, have been widely used in civil and military fields such as smart cities and transportation and rescue. High-precision and high-reliability positioning is crucial to MAS and is a prerequisite for its tasks such as perception, planning, and control. Unlike single-agent systems, agents in MAS can measure each other with neighboring agents and share states and sensor data to overcome the limitations of individual sensor capabilities and the impact of environmental changes. Therefore, collaborative positioning can provide more accurate and reliable positioning results than single-agent single-point positioning.

[0003] In practical applications, factors such as non-line of sight (NLOS), sensor failure, and platform vibration cause the observations obtained by the agent to contain non-ideal errors, which in turn leads to the mismatch between the ideal observation model and the actual observations. Therefore, it is necessary to study robust collaborative positioning to improve positioning reliability and robustness.

[0004] Commonly used robust collaborative positioning methods include fault detection and elimination (FDE), M estimation and measurement error modeling. FDE judges the observations by constructing detection statistics and improves positioning robustness by eliminating abnormal observations. However, FDE is difficult to strike a balance between low false alarm rate and low missed detection rate. M estimation reduces the impact of abnormal observations by adjusting the weights of different observations, but it ignores the characteristics of non-adaptive models, so the positioning performance is limited. Measurement error modeling models the statistical characteristics of observation errors caused by non-adaptive models and improves positioning accuracy by simultaneously estimating proxy states and error model statistics. However, existing measurement error modeling methods assume that the observations between different positioning epochs are independent of each other, and the independent estimation results are not robust. Summary of the invention

[0005] In view of this, the present invention provides a robust collaborative positioning method for multi-epoch association under model non-adaptation. Considering that the environments in which the agents are located are similar in a short period of time, there is a strong correlation between the non-adaptive models and observation deviations of different positioning epochs. Therefore, the present invention estimates the agent state and error by associating multi-epoch observation information to enhance the robustness of positioning.

[0006] In order to solve the above technical problems, the present invention is implemented as follows.

[0007] A robust collaborative positioning method for multi-epoch observation correlation under model non-adaptation, comprising:

[0008] Step 1: Observation model building: constructing measurement bias latent variables and the measurement noise latent variable To characterize the deviation and noise of multi-epoch observations; introduce the measurement deviation latent variable and measurement noise latent variable into the ideal observation model to obtain the modified observation model of t epochs

[0009] Step 2: Correlation of multi-epoch observations: Based on the similarity of the agent's environment in a short period of time and the correlation of observations in different epochs, the agent's kinematic model from epoch t-1 to epoch t is used to infer the agent's state in historical epochs and substitute it into the observation model And perform observation correlation for a period of time to obtain the multi-epoch observation model after correlation Realize the observation of multi-epoch association of t-epoch state;

[0010] Step 3: Modeling latent variables of observed quantities: Use Gauss-Wishart distribution to model latent variables and obtain latent variable distribution

[0011] Step 4: Distributed robust positioning: Based on the multi-epoch observation model and latent variable distribution The marginal posteriors of each agent state and the latent variables are estimated simultaneously to achieve robust co-localization.

[0012] Preferably, the observation model is constructed in step 1 as follows:

[0013] In the multi-agent system, each agent is equipped with sensors, and the relative observations between the agent and the adjacent nodes are obtained at each epoch. At epoch t, ​​assuming that agent i is adjacent to node j, and node j is an anchor point or agent, the modified epoch t observation model with measurement bias latent variables and measurement noise latent variables is introduced. for:

[0014]

[0015] in, is the actual observation obtained by the agent through the sensor; The ideal observation model is known for the agent; To measure the bias latent variable; is the measurement noise, where represents a Gaussian distribution, is the introduced measurement noise hidden variable; is the state vector to be estimated of agent i, is the location of agent i, is the posture of agent i; is the state vector to be estimated of agent i, is the position of node j, is the posture of node j.

[0016] Preferably, the step 2 of associating multi-epoch observations is as follows:

[0017] Kinematic model of agent i from epoch t-1 to epoch t It is expressed as:

[0018]

[0019] in, is the input vector, which consists of the proxy sensor observations, where is the linear velocity of the agent, is the rotation speed; δt is the time interval between adjacent epochs;

[0020] Based on kinematic model Establishing the conversion relationship between multi-epoch proxy states Indicates that the agent is in epoch t, ​​based on the state Input Vector The proxy state of t-1 epoch is obtained by inverse calculation according to the kinematic model, and then the observed quantities of multiple epochs are associated with the state of t epoch;

[0021] For agent i, it obtains the observations of neighboring node j in the time period [t-L+1,…,t] with a length of L. Then the multi-epoch observation model obtained by correlating the observations in this time period is It is expressed as:

[0022]

[0023] in,

[0024]

[0025] Among them, the variable They represent the relative observation set between agent i and adjacent node j, the ideal measurement model set, and the measurement deviation latent variable set in a time period of L; Indicates that the agent is in epoch t, ​​based on the state Input Vector The state of the t-L+1 epoch obtained by inverse calculation based on the kinematic model; represents the relative observation noise set between agent i and neighboring node j in a period of L; noise The information matrix uses variables express.

[0026] Preferably, in step 3, Gauss-Wishart distribution is used Model latent variables and obtain latent variable distribution for:

[0027]

[0028] in, represents a Gaussian distribution, represents the Wishart distribution, Γ(·) represents the Gamma distribution, tr(·) represents the trace of the matrix, and d represents the relative observation set in a time period of length L. Dimensions of; Operator are the hyperparameters of the latent variables, which determine the specific shape of the distribution of the latent variables.

[0029] Preferably, step 5 determines the closed-form solution of the agent state and the marginal posterior distribution of the latent variable under variational message passing, so that all agents in the MAS estimate their own states in a distributed manner in a closed-form solution to achieve robust collaborative positioning.

[0030] Preferably, the distributed robust positioning step of step 5 specifically includes:

[0031] According to the multi-epoch observation model, the Gaussian likelihood function of the multi-epoch observation is determined as:

[0032]

[0033] At epoch t, ​​the prior distribution of the agent state is expressed as in is the agent prior state mean, is the agent prior state information matrix;

[0034] At epoch t, ​​the prior distribution of the latent variable of the observation associated with agent i is expressed as:

[0035]

[0036] in, Represents latent variable hyperparameters The value of prior information of ;

[0037] According to the Bayesian formula, the joint posterior distribution of all variables is:

[0038]

[0039] Where N is the total number of agents, represents the set of neighboring agents of agent i at epoch t;

[0040] The marginal posterior of any agent state is obtained by directly integrating Equation (I) with respect to all other variables;

[0041] The distributed estimation of agent state edge posterior is realized based on iterative variational message passing (VMP) framework.

[0042] Assume that after the kth iteration, the agent state and the latent variable variational posterior are:

[0043]

[0044] in, is the variational posterior mean of the proxy state, is the agent state variational posterior information matrix, Hidden variable hyperparameter estimation results after the kth iteration;

[0045] The ideal measurement model set Linearization is:

[0046]

[0047] in, Ideal measurement model The Jacobian matrix of the estimated result of the agent i state in the kth round of iteration is: Ideal measurement model The Jacobian matrix of the estimated result of agent j's state at the kth iteration. is the observation residual calculated based on the k-th round of iterative estimation results;

[0048] Then, under the VMP framework, the state variation posterior calculation method of agent i in the k+1 round iteration is:

[0049]

[0050] in

[0051]

[0052] in, Denotes the set θ t remove The following subset;

[0053] Hidden variables The state variation posterior calculation method in k+1 rounds of iterations is:

[0054]

[0055] in

[0056]

[0057] in, Representing a collection remove The subsequent subset. Representing a collection remove The following subset;

[0058] By iteratively updating the marginal posteriors of the proxy states and the hidden variables, we obtain the marginal posteriors of each proxy state and the hidden variables. The mean of the marginal posteriors is the distributed robust collaborative localization result.

[0059] Beneficial effects:

[0060] The present invention is applicable to the scenario of non-adaptive observation model. First, the correlation between multi-epoch non-adaptive models is considered, the agent's state changing over time is constrained by the state transfer equation, and hidden variables are introduced to characterize the deviation and noise of multi-epoch observations, so as to eliminate the observation deviation and positioning performance deterioration caused by the non-adaptive observation model. In addition, the closed-form solution of the marginal posterior distribution of the agent state and the hidden variable of the observation is derived under variational message passing, so that all agents in the MAS can estimate their own state in a distributed manner in a closed-form solution to achieve robust collaborative positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Schematic diagram of the robust co-localization method for correlation of multi-epoch observations under model non-adaptation. DETAILED DESCRIPTION

[0062] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0063] The present invention considers the correlation between multi-epoch non-adaptive models, constrains the agent's state over time through the state transfer equation, and introduces high-dimensional latent variables to characterize the deviation and noise of multi-epoch observations. Furthermore, the present invention proposes a distributed estimation method for agent states and latent variables to achieve robust positioning.

[0064] The structural block diagram of the present invention is as follows Figure 1 As shown, it includes four steps: observation model building, multi-epoch observation correlation, observation latent variable modeling, and distributed robust positioning.

[0065] The specific steps of the present invention are as follows:

[0066] Without loss of generality, the multi-agent system is described as follows: Assume a MAS system with N mobile agents and M stationary anchors in a 2D scene. At epoch t, ​​the position of agent i is expressed as The posture is represented by Therefore, the estimated state vector of each agent can be expressed as The anchor point is regarded as the spatial reference of the system. Taking anchor point i as an example, its state is also expressed as Unlike proxies, anchor states Precisely known.

[0067] Step 1: Observation model building

[0068] Each proxy is equipped with sensors, and the relative observations between the adjacent anchor points / proxy can be obtained at each epoch. Due to the existence of non-ideal error factors, the actual observations are not compatible with the ideal observation model. Therefore, it is necessary to first correct the ideal observation model and construct an observation model that takes error factors into account.

[0069] At epoch t, ​​assuming that proxy i is adjacent to anchor point / proxy j, the relative distance and azimuth observations considering the error factor can be expressed as

[0070]

[0071] in, is the actual observation obtained by the proxy sensor. The ideal observation model is known for the agent. The first latent variable introduced in the present invention characterizes the unknown measurement deviation caused by multiple non-ideal error factors, and is called the measurement deviation latent variable. is the measurement noise, where represents a Gaussian distribution, The second latent variable introduced in the present invention characterizes the unknown measurement noise information matrix and is called the measurement noise latent variable. By correcting the ideal measurement model, we can fully consider the impact of non-ideal error factors on the observed quantity, thereby improving the positioning accuracy.

[0072] Step 2: Correlation of multi-epoch observations

[0073] Since the environments of the agents are similar in a short period of time, there is correlation between the observations at different positioning epochs. The present invention uses the kinematic model of the agent to constrain the state of the agent at multiple epochs, and then associates the observations at multiple epochs.

[0074] Kinematic model of agent i from epoch t-1 to epoch t It can be expressed as

[0075]

[0076] in, is the input vector, which consists of the sensor observations of agent i, where is the linear velocity of agent i at epoch t-1, is the corresponding rotation speed. δt is the time interval between adjacent epochs.

[0077] Based on kinematic model The conversion relationship between multi-epoch proxy states can be established, and then the multi-epoch observations can be associated with the t-epoch state. Taking agent i as an example, assuming that it can obtain the observations of the adjacent agent / anchor point j in the time period of length L [t-L+1,…,t], then the multi-epoch observation model can be obtained after the observations in this time period are associated as follows:

[0078]

[0079] in

[0080]

[0081] in, Indicates that the agent is in epoch t, ​​based on the state Input Vector The proxy state of epoch t-1 is obtained by inferring the kinematic model (2). Representation based on state A sequence of L input vectors before t epoch The state of the t-L+1 epoch is obtained by back-calculation. They represent the relative observation set, ideal measurement model set, and measurement deviation latent variable set of agents i and j during this period respectively. Represents the relative observation noise set between agent i and neighboring node j in a long period of time L; hidden variable Represents the set of all relative observation noises within a time period The information matrix of is called the measurement noise latent variable set.

[0082] According to the multi-epoch observation model (Equation 3), the Gaussian likelihood function of the multi-epoch observation (3) after association can be written as

[0083]

[0084] Where d represents the relative observation set within this time period Dimensions, Operators

[0085] Step 3: Observational latent variable modeling

[0086] In the Gaussian likelihood function (5) of the multi-epoch observations after correlation, the observation latent variable and Determines the mean and covariance of the Gaussian distribution. Therefore, the modeling of latent variables will directly affect the likelihood function. The present invention uses Gauss-Wishart (GW) distribution to model latent variables. Thus, the GW distribution of latent variables can be expressed as:

[0087]

[0088] Among them, Γ(·) represents the Gamma distribution, represents a Gaussian distribution, represents the Wishart distribution, and tr(·) represents the trace of the matrix. As can be seen from formula (6), the present invention uses the hyperparameter To determine the specific shape of the distribution of latent variables.

[0089] Step 4: Distributed Robust Localization

[0090] By associating multi-epoch observations and introducing observation latent variables, the present invention aims to simultaneously estimate the marginal posteriori of each agent state and the marginal posteriori of the observation latent variables, thereby achieving robust collaborative positioning.

[0091] Assuming that at epoch t, ​​the prior distribution of the agent state can be expressed as in is the agent prior state mean, is the agent prior state information matrix. Similarly, at epoch t, ​​the prior distribution of the observed latent variables related to agent i can be expressed as

[0092] According to the Bayesian formula, the joint posterior distribution of all variables can be written as

[0093]

[0094] The marginal posterior for any agent state can be obtained by directly integrating (7) with respect to all other variables.

[0095] The present invention implements distributed estimation of agent state edge posterior based on iterative variational message passing (VMP) framework. Assume that after the kth iteration, the agent state variational posterior and the latent variable variational posterior are:

[0096]

[0097] in, is the variational posterior mean of the proxy state, is the agent state variational posterior information matrix, Hidden variable hyperparameter estimation results after the kth iteration.

[0098] At the same time, the multi-epoch ideal observation model It can be linearized into

[0099]

[0100] in, Ideal measurement model The Jacobian matrix of the estimated result of the agent i state in the kth round of iteration is: Ideal measurement model The Jacobian matrix of the estimated result of agent j's state at the kth iteration. is the observation residual calculated based on the k-th round of iterative estimation results.

[0101] Then, under the VMP framework, the state variation posterior calculation method of agent i in the k+1 round iteration is:

[0102]

[0103] in

[0104]

[0105] in, Denotes the set θ t remove The following subset;

[0106] Furthermore, the observed latent variables The state variation posterior calculation method in k+1 rounds of iterations is:

[0107]

[0108] in

[0109]

[0110] in, Representing a collection remove The subsequent subset. Representing a collection remove The subsequent subset.

[0111] Thus, the iterative update calculation of the marginal posteriors of the agent state and the observed latent variables is realized. After obtaining (10) and (12), return to step (8) and perform the next iteration again until the iteration converges. Thus, the marginal posteriors of each agent state and the marginal posteriors of the observed latent variables are obtained. The mean of the marginal posteriors is the distributed robust collaborative localization result.

[0112] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. A robust collaborative positioning method for multi-epoch observation association under model non-adaptation, characterized in that: include: Step 1: Observation model building: constructing measurement bias latent variables and the measurement noise latent variable To characterize the deviation and noise of multi-epoch observations; introduce the measurement deviation latent variable and measurement noise latent variable into the ideal observation model to obtain the modified observation model of t epochs Step 2: Correlation of multi-epoch observations: Based on the similarity of the agent's environment in a short period of time and the correlation of observations in different epochs, the agent's kinematic model from epoch t-1 to epoch t is used to infer the agent's state in historical epochs and substitute it into the observation model And perform observation correlation for a period of time to obtain the multi-epoch observation model after correlation Realize the observation of multi-epoch association of t-epoch state; Step 3: Modeling latent variables of observed quantities: Use Gauss-Wishart distribution to model latent variables and obtain latent variable distribution Step 4: Distributed robust positioning: Based on the multi-epoch observation model and latent variable distribution The marginal posteriors of each agent state and the latent variables are estimated simultaneously to achieve robust co-localization.

2. The method according to claim 1, characterized in that The observation model is constructed in step 1 as follows: In the multi-agent system, each agent is equipped with sensors, and the relative observations between the agent and the adjacent nodes are obtained at each epoch. At epoch t, ​​assuming that agent i is adjacent to node j, and node j is an anchor point or agent, the modified epoch t observation model with measurement bias latent variables and measurement noise latent variables is introduced. for: in, is the actual observation obtained by the agent through the sensor; The ideal observation model is known for the agent; To measure the bias latent variable; is the measurement noise, where represents a Gaussian distribution, is the introduced measurement noise hidden variable; is the state vector to be estimated of agent i, is the location of agent i, is the posture of agent i; is the state vector to be estimated of agent i, is the position of node j, is the posture of node j.

3. The method according to claim 2, characterized in that The step 2 for correlating multi-epoch observations is as follows: Kinematic model of agent i from epoch t-1 to epoch t It is expressed as: in, is the input vector, which consists of the proxy sensor observations, where is the linear velocity of the agent, is the rotation speed; δt is the time interval between adjacent epochs; Based on kinematic model Establishing the conversion relationship between multi-epoch proxy states Indicates that the agent is in epoch t, ​​based on the state Input Vector The proxy state of t-1 epoch is obtained by inverse calculation according to the kinematic model, and then the observed quantities of multiple epochs are associated with the state of t epoch; For agent i, it obtains the observations of neighboring node j in the time period [t-L+1,…,t] with a length of L. Then the multi-epoch observation model obtained by correlating the observations in this time period is It is expressed as: in, Among them, the variable They represent the relative observation set between agent i and neighboring node j, the ideal measurement model set, and the measurement deviation latent variable set in a time period of L; Indicates that the agent is in epoch t, ​​based on the state Input Vector The state of the t-L+1 epoch obtained by inverse calculation based on the kinematic model; represents the relative observation noise set between agent i and neighboring node j in a period of L; noise The information matrix uses variables express.

4. The method according to claim 3, characterized in that In step 3, the Gauss-Wishart distribution is used Model latent variables and obtain latent variable distribution for: in, represents a Gaussian distribution, represents the Wishart distribution, Γ(·) represents the Gamma distribution, tr(·) represents the trace of the matrix, and d represents the relative observation set in a time period of length L. Dimensions of; Operator are the hyperparameters of the latent variables, which determine the specific shape of the distribution of the latent variables.

5. The method according to claim 1, characterized in that The step 5 determines the closed-form solution of the agent state and the marginal posterior distribution of the hidden variable under variational message passing, so that all agents in the MAS estimate their own states in a distributed manner in a closed-form solution to achieve robust collaborative positioning.

6. The method according to claim 4, characterized in that The distributed robust positioning step of step 5 specifically includes: According to the multi-epoch observation model, the Gaussian likelihood function of the multi-epoch observation is determined as: At epoch t, ​​the prior distribution of the agent state is expressed as in is the agent prior state mean, is the agent prior state information matrix; At epoch t, ​​the prior distribution of the latent variable of the observation associated with agent i is expressed as: in, Represents latent variable hyperparameters The value of prior information of ; According to the Bayesian formula, the joint posterior distribution of all variables is: Where N is the total number of agents, represents the set of neighboring agents of agent i at epoch t; The marginal posterior of any agent state is obtained by directly integrating Equation (I) with respect to all other variables; The distributed estimation of agent state edge posterior is realized based on iterative variational message passing (VMP) framework. Assume that after the kth iteration, the agent state and the latent variable variational posterior are: in, is the variational posterior mean of the proxy state, is the agent state variational posterior information matrix, Hidden variable hyperparameter estimation results after the kth iteration; The ideal measurement model set Linearization is: in, Ideal measurement model The Jacobian matrix of the estimated result of the agent i state in the kth round of iteration is: Ideal measurement model The Jacobian matrix of the estimated result of agent j’s state in the kth iteration; is the observation residual calculated based on the k-th round of iterative estimation results; Then, under the VMP framework, the state variation posterior calculation method of agent i in the k+1 round iteration is: in in, Denotes the set θ t remove The following subset; Hidden variables The state variation posterior calculation method in k+1 rounds of iterations is: in in, Representing a collection remove The following subset; Representing a collection remove The following subset; By iteratively updating the marginal posteriors of the proxy states and the hidden variables, we obtain the marginal posteriors of each proxy state and the hidden variables. The mean of the marginal posteriors is the distributed robust collaborative localization result.

Images