A Distributed Cooperative Navigation and Positioning Accuracy Evaluation Method Based on Belief Propagation

Abstract

The present invention discloses a method for evaluating the accuracy of distributed cooperative navigation and positioning based on belief propagation. The steps are as follows: S1. Construct a factor graph model of a single node at the current moment. S2. Obtain the belief expression of the state quantity of the node i at the current moment. S3. Determine the covariance matrix of the state quantity of the node i at the current moment to obtain the accuracy of distributed cooperative navigation and positioning of the node i ; The method for evaluating the accuracy of distributed cooperative navigation and positioning based on belief propagation is based on the belief propagation theory, and comprehensively considers the influence of ranging error and the position error of cooperative terminals on the accuracy of distributed cooperative navigation and positioning, solving the problems existing in the existing modeling methods for the accuracy of distributed cooperative navigation and positioning, such as the failure to fully consider the influence of ranging error and the position error of cooperative terminals and poor modeling accuracy; Through experimental verification, this method has high accuracy, is easy to operate, and has good practicability.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-agent cooperative navigation and positioning, and particularly relates to a distributed cooperative navigation and positioning accuracy evaluation method based on belief propagation. Background Art

[0002] An intelligent unmanned cluster system is composed of multiple agents such as unmanned aerial vehicles, unmanned vehicles, and robots through information interaction with each other, and can perform intelligent cooperative control to complete complex tasks that cannot be achieved by a single unmanned system, and has broad application prospects in military and civilian fields. Cooperative navigation technology is a key technology aimed at improving the positioning and navigation accuracy of individuals in a multi-agent system. In a multi-agent system, richer observation information is provided for sub-nodes by sharing navigation states and mutual observations among various sub-nodes in the system. Each node cooperates with each other to achieve an improvement in navigation and positioning accuracy under low-cost conditions.

[0003] Currently, the solutions for cooperative navigation can be divided into two types of algorithms: centralized positioning and distributed positioning. In centralized positioning, a centralized computing unit collects the measurement data of all terminals for unified calculation and simultaneously estimates the coordinates of multiple terminals. This method has problems such as excessive system calculation overhead and poor stability. Distributed positioning takes each terminal as a cooperative node with each other. In each iteration, the terminal calculates and updates its own navigation parameters using the information of adjacent nodes. Therefore, compared with centralized positioning, it has lower computational complexity and communication load, and is the mainstream solution in current cooperative navigation technology.

[0004] Evaluating the accuracy of cooperative navigation and positioning is an important step in designing cooperative navigation algorithms and measuring whether the designed cooperative navigation algorithms are qualified; however, the accuracy of distributed cooperative positioning algorithms is not only related to ranging errors but also related to the position errors of cooperative terminals. Therefore, based on the above description, the modeling of distributed cooperative navigation and positioning accuracy is relatively complex. Directly applying existing navigation and positioning accuracy modeling methods to the modeling of distributed cooperative navigation and positioning accuracy will inevitably have problems of poor modeling accuracy due to the insufficient consideration of the influence of ranging errors and the position errors of cooperative terminals. Therefore, existing navigation and positioning accuracy evaluation methods are not applicable to the evaluation of distributed cooperative navigation and positioning accuracy. Summary of the Invention

[0005] The purpose of the present invention is to provide a distributed cooperative navigation and positioning accuracy evaluation method based on belief propagation that takes into account the influence of both ranging errors and the position errors of cooperative terminals during the modeling process of distributed cooperative navigation and positioning accuracy, so as to solve the problem of poor modeling accuracy of positioning accuracy.

[0006] To this end, the technical solution of the present invention is as follows:

[0007] A method for evaluating the accuracy of distributed cooperative navigation and positioning based on belief propagation, the steps are as follows:

[0008] S1. Construct a factor graph model of a single node at the current moment: For any node i, set the prior factor f prior and the relative ranging factor f m , and define: The state quantity X0 of node i at the initial moment is connected to the state quantity X prior at the current moment through the prior factor f k , so as to transfer the state quantity X0 at the initial moment to the state quantity X k at the current moment; The state quantity X k of node i at the current moment is connected to the relative ranging factor f m , so as to transmit the relative ranging information between the adjacent node and node i to the state quantity X k of node i at the current moment;

[0009] S2. Obtain the belief expression of the state quantity of node i at the current moment:

[0010]

[0011] In the formula, BI(X k ) is the belief of the state quantity X k of the node at the current moment, that is, t = k; Q is the covariance matrix of the prior factor f prior ; g(X k-1 ) is the predicted value of the state quantity at the current moment obtained by node i based on the state quantity X k-1 at the previous moment; R1 is the covariance matrix of the relative ranging factor f m ; z UWB is the measured value of the relative ranging information at the current moment; h(X k ) is the observed value of the relative ranging information at the current moment;

[0012] S3. Determine the covariance matrix of the state quantity of node i at the current moment to obtain the accuracy of distributed cooperative navigation and positioning of node i; The specific steps of this step S3 are:

[0013] S301. Based on the standard form of probability density, define the probability density expression of the state quantity X k of node i at the current moment as:

[0014]

[0015] In the formula, μ is the mean value of the current state X k of the node; λ is a scale factor, and its value is a constant; ∑ X is the covariance matrix of the state quantity X k of node i at the current moment;

[0016] S302: The state quantity X of node i at the current moment k The confidence expression of BI(X k )=P(X k ), and the covariance matrix ∑ of the state quantity of node i at the current moment is obtained after sorting X , whose expression is:

[0017] ∑ X =(J1 T Q -1 J1+J2 T R -1 J2) -1 ,

[0018] In the formula, Q -1 is the inverse matrix of Q at the current moment; R -1 is the inverse matrix of R; the recursive formula of Q is: Q k =FQ k-1 F T +GPG T , where Q k is the prior factor f at the current moment prior The covariance matrix, Q k-1 is the prior factor f at the previous moment prior The covariance matrix of ; the expression of F is: v k-1 is the observed value of the rate at the previous moment; t s is the dead reckoning solution period; θ k is the heading angle at the current moment; the expression of G is: P is the random noise of gyroscope angular rate; R is the relative ranging error noise of UWB;

[0019] S303: The state quantity X of node i at the current moment calculated in step S302 k The covariance matrix ∑ X Model ‖∑ X ‖, which is the distributed collaborative navigation positioning accuracy result of node i at the current moment.

[0020] Further, in step S302, the expression of P is: 100(° / h 3 / 2 ).

[0021] Further, in step S302, the expression of R is: 0.1m 2。

[0022] Compared with the prior art, the distributed cooperative navigation positioning accuracy evaluation method based on belief propagation is based on the belief propagation theory, and comprehensively considers the influence of ranging error and the position error of cooperative terminals on the distributed cooperative navigation positioning accuracy, solving the problems existing in the existing distributed cooperative navigation positioning accuracy modeling methods, such as the failure to fully consider the influence of ranging error and the position error of cooperative terminals and the poor modeling accuracy; through experimental verification, this method has high accuracy, is easy to operate, and has good practicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a flowchart of the distributed cooperative navigation positioning accuracy evaluation method based on belief propagation of the present invention;

[0024] Figure 2 is to construct a factor graph model of a single node at the current moment in step S1 of the distributed cooperative navigation positioning accuracy evaluation method based on belief propagation of the present invention;

[0025] Figure 3 is a schematic diagram of the simulation scenario set in the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0026] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the following embodiments are by no means any limitation to the present invention.

[0027] The distributed cooperative navigation positioning accuracy evaluation method based on belief propagation of the present application proposes a navigation positioning accuracy evaluation method for the cooperative navigation method in a communication-constrained environment based on graph optimization (hereinafter referred to as the cooperative navigation method for short); the above-mentioned cooperative navigation method is specifically described in the published patent CN114838732A.

[0028] See Figure 1 , the specific implementation steps of the distributed cooperative navigation positioning accuracy evaluation method based on belief propagation are as follows:

[0029] S1. Construct a factor graph model of a single node at the current moment:

[0030] For any node i, set the prior factor f prior and the relative ranging factor f m , and define:

[0031] The state quantity X0 of node i at the initial moment (t = 0) is connected through the prior factor f prior to the state quantity X k at the current moment (t = k), so as to transfer the state quantity X0 at the initial moment to the state quantity X k ;

[0032] The state quantity X of node i at the current time (t=k) k and the relative distance factor f m The relative distance information between the adjacent nodes and node i is transmitted to the state quantity X of node i at the current moment. k ;

[0033] The neighboring nodes of node i refer to all other nodes except node i in the collaborative navigation system.

[0034] like Figure 2 The figure shows a schematic diagram of constructing a factor graph model of a single node at the current moment based on node i.

[0035] S2, obtain the confidence expression of the state quantity of node i at the current moment;

[0036] According to the belief propagation theory, the state quantity X of node i at the current moment is k The confidence level is:

[0037] BI(X k )=BI(f prior ,X k )BI(f m ,X k ),

[0038] In the formula, BI(X k ) is the state quantity X of the node at the current time (t=k) k The confidence level, BI(f prior ,X k ) is the initial state quantity X0 passed to the current state quantity X at the current time (t = k) k The confidence connection relationship, BI(f m ,X k ) is the relative distance information between the adjacent nodes and the constructed single node, which is transmitted to the current state quantity X k The confidence connection relationship;

[0039] Among them, BI(f prior ,X k ) is expanded as:

[0040]

[0041] Where Q is the prior factor f prior The covariance matrix of g(X k-1 ) is the state quantity X of node i based on the previous moment (t=k-1) k-1 The predicted value of the state quantity at the current time (t=k) is obtained, wherein the predicted value is obtained by the aforementioned collaborative navigation method, that is, the navigation result finally obtained by the aforementioned collaborative navigation method after optimization;

[0042] BI(f m ,X k ) is expanded as:

[0043]

[0044] Where R1 is the relative ranging factor f m The covariance matrix of UWB is the relative distance information measurement value at the current moment, obtained by the ultra-wideband distance sensor configured on the node; h(X k ) is the relative distance information observation value at the current time (t = k), and its value is based on z UWB Add noise disturbance to obtain; In this embodiment, in order to simplify the calculation, h(X k ) and z UWB equal;

[0045] Therefore, the confidence expression of the state quantity of node i at the current moment is summarized as:

[0046]

[0047] S3, determining the covariance matrix of the state quantity of node i at the current moment to obtain the distributed collaborative navigation positioning accuracy of node i;

[0048] Specifically, the implementation steps of step S3 are as follows:

[0049] S301, based on the standard form of probability density, define the state quantity X of node i at the current moment k The probability density expression of is:

[0050]

[0051] Where μ is the current state of the node X k The mean of ;λ is the proportional factor, which is a constant;∑ X is the state X of node i at the current moment k The covariance matrix of

[0052] S302: The state quantity X of node i at the current moment k The confidence expression of BI(X k )=P(X k ), and the covariance matrix ∑ of the state quantity of node i at the current moment is obtained after sorting X , whose expression is:

[0053] ∑ X =(J1 T Q -1 J1+J2T R -1 J2) -1 ,

[0054] In the formula, Q -1 is the inverse matrix of Q, and R -1 is the inverse matrix of R;

[0055] Among them,

[0056] Based on Step S1 and Step S2, Q is the covariance matrix of the prior factor f prior The prior factor f prior is the connection quantity from the state quantity X0 at the initial moment to the state quantity X k at the current moment. Therefore, Q is a quantity related to time, that is, Q at the current moment k is recursively obtained from Q at the previous moment k-1 The recurrence formula of Q is:

[0057] Q k = FQ k-1 F T + GPG T ,

[0058] In the formula, the expression of F is: Among them, v k-1 is the speed observation value at the previous moment (t = k - 1), and its specific value is obtained by the odometer installed on node i; t s is the dead reckoning solution period, and its value is the same as the dead reckoning interval time adopted in the aforementioned cooperative navigation method; θ k is the heading angle at the current moment (t = k); in this embodiment, t s is 0.1 s;

[0059] The expression of G is: t s is the dead reckoning solution period, and its value is the same as the dead reckoning solution period adopted in the aforementioned cooperative navigation method; in this embodiment, t s is 0.1 s;

[0060] P is the gyroscope angular rate random noise, which conforms to Gaussian noise. The expression of P is: Its physical meaning is: P is Gaussian noise with a mean of 0 and a variance of ; in this embodiment, is 100 (° / h 3 / 2 );

[0061] R is the relative ranging error noise of UWB, which conforms to Gaussian noise. The expression of R is: Its physical meaning is: R is Gaussian noise with a mean of 0 and a variance of Gaussian noise; in this embodiment, 0.1m 2 ;

[0062] Q k is the prior factor f at the current time (t=k) prior The covariance matrix, Q k-1 is the prior factor f at the previous moment (t=k-1) prior The covariance matrix of; at the initial moment (t=0), Q0 is 0, then the F, G and P at each moment calculated in sequence based on the aforementioned collaborative navigation method can be correspondingly calculated to obtain the Q value at each moment;

[0063] S303: The state quantity X of node i at the current moment calculated in step S302 k The covariance matrix ∑ X Model ‖∑ X ‖, which is the distributed collaborative navigation positioning accuracy result of node i at the current moment.

[0064] Furthermore, in order to verify the feasibility of this method in practical applications and the accuracy of precision evaluation, simulation experiments are used to verify the results.

[0065] like Figure 3The following shows the simulation scenario applied in this embodiment; in this simulation scenario, in the initial state, the scenario is divided into an indoor scenario located inside the room and an outdoor scenario located outside the room; in the indoor scenario, there is an autonomous vehicle and an investigator, and in the outdoor scenario, there is an autonomous vehicle; among them, the indoor scenario is arranged as follows: there is a first entrance / exit on the north side near the west side of the room, and there are three entrance / exit doors spaced from west to east on the south side of the room, namely the second entrance / exit, the third entrance / exit, and the fourth entrance / exit. There is a shielding wall opposite to the first entrance / exit near the east side of the room, and the four entrance / exit doors are in the normally open state; the three communication base stations are respectively named Anchor Point 1, Anchor Point 2, and Anchor Point 3. Anchor Point 1 and Anchor Point 2 are set on the south side outside the room, and the distances from their installation positions to the room are the same. Anchor Point 1 is opposite to the midpoint of the connection line between the second entrance / exit and the third entrance / exit, and Anchor Point 2 is opposite to the midpoint of the connection line between the third entrance / exit and the fourth entrance / exit. Anchor Point 3 is set on the west side outside the room and is opposite to the first entrance / exit; Anchor Point 1, Anchor Point 2, and Anchor Point 3 can all be used to establish real-time communication with the autonomous vehicle and the investigator when the communication is unobstructed; the autonomous vehicle outside the room is named Node 1, and its initial position is on the west side of the second entrance / exit; the movement trajectory of Node 1 is set to first drive from the outdoor initial position into the room and then drive along the south wall from the west side to the east side; during the entire movement process, Node 1 can establish communication with the two communication base stations on the south side outside the room (i.e., Anchor Point 1 and Anchor Point 2); the autonomous vehicle inside the room is named Node 2, and its initial position is on the east side of the first entrance / exit and near the north wall; the movement trajectory of Node 2 is set to drive from the north side inside the room to the south side inside the room; during the entire movement process, Node 2 can establish communication with the communication base station on the west side outside the room (i.e., Anchor Point 3); the investigator inside the room is named Node 3, and its initial position is on the west side of the shielding wall and near the north wall; the movement trajectory of Node 3 is set to drive a certain distance southward from the north side inside the room to the end of the shielding wall and then turn and drive towards the east side inside the room; during the entire movement process, Node 3 cannot establish communication with any communication base station. The above three nodes are all equipped with inertial sensors, odometers, and ultra-wideband ranging sensors.

[0066] According to the aforementioned cooperative navigation method, it can be determined that the communication accuracy relationship among the three nodes is: Node 1 > Node 2 > Node 3; furthermore, the information transmission mode is set as: Node 2 can receive the relative distance information between itself and Node 1, and Node 3 can receive the relative distance information between Node 1 and Node 2.

[0067] During this test process, first, the aforementioned cooperative navigation method is used to predict the navigation result, and then the method of the present application is used to simulate and calculate the positioning accuracy of the prediction result to obtain the positioning accuracy simulation value; at the same time, as a control experiment of the present application, with reference to Figure 3Set the same real scenarios in the laboratory and conduct physical tests; similarly, high-precision navigation devices are configured for each node in the real scenarios to obtain positioning results as reference values, and by comparing the reference values with the navigation values of each node, the actual positioning accuracy values are calculated. It should be noted that since each node is in motion all the time, the positioning accuracy of each node also changes with time; for the convenience of comparison, the maximum deviation values of the positioning accuracy obtained by each node in the simulation scenario and the actual scenario are used as the positioning accuracy of the node.

[0068] The test results of each node in the above simulation scenario and physical scenario are specifically shown in Table 1 below.

[0069] Node Name Node 1 Node 2 Node 3 Simulated Value of Positioning Accuracy 0.02m 0.1m 0.4m Actual Value of Positioning Accuracy 0.022m 0.102m 0.399m

[0070] It can be clearly seen from the test results in Table 1 that the positioning accuracies of each node obtained by using the method of this application are as follows: the positioning accuracy of Node 1 is 0.02 m, the positioning accuracy of Node 2 is 0.1 m, and the positioning accuracy of Node 3 is 0.4 m; these three positioning accuracy results are consistent with the signal reception capabilities of the above three nodes, that is, Node 1 > Node 2 > Node 3, which verifies that the method of this application can truly reflect the node positioning accuracy set in the simulation, that is, the method of this application is effective; and by comparing the positioning accuracy obtained by the method of this application with the positioning accuracy obtained from the actual physical test, the maximum deviation between the simulation value and the actual value of the positioning accuracy is only 2 mm, with high precision, which further proves the effectiveness and accuracy of the method of this application.

[0071] In summary, the distributed cooperative navigation positioning accuracy evaluation method based on belief propagation of this application is not only easy to operate, but also has high precision, and has both effectiveness and accuracy.

[0072] The parts not detailedly disclosed in the present invention belong to the well-known technologies in the art. Although the above-described illustrative specific embodiments of the present invention have been described to facilitate the understanding of the present invention by those skilled in the art of this technology, it should be clear that the present invention is not limited to the scope of the specific embodiments. For those of ordinary skill in the art of this technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions and creations using the concept of the present invention are within the scope of protection.

Claims

1. A distributed cooperative navigation and positioning accuracy evaluation method based on belief propagation, characterized in that The steps are as follows: S1. Construct the factor graph model of a single node at the current moment: For any node i, set the prior factor f prior and the relative ranging factor f m , and define that the state quantity X0 of node i at the initial moment is connected to the state quantity X prior at the current moment through the prior factor f k , so as to transfer the state quantity X0 at the initial moment to the state quantity X k at the current moment; the state quantity X k of node i at the current moment is connected to the relative ranging factor f m , so as to transfer the relative ranging information between the adjacent node and node i to the state quantity X k of node i at the current moment; S2. Obtain the confidence expression of the state quantity of node i at the current moment: where BI(X k ) is the confidence of the node at the current moment, i.e., the state quantity X k at t = k; Q is the covariance matrix of the prior factor f prior ; g(X k-1 ) is the predicted value of the state quantity at the current moment obtained by node i based on the state quantity X k-1 at the previous moment; R1 is the covariance matrix of the relative ranging factor f m ; z UWB is the measured value of the relative ranging information at the current moment; h(X k ) is the observed value of the relative ranging information at the current moment; S3. Determine the covariance matrix of the state quantity of node i at the current moment to obtain the distributed cooperative navigation and positioning accuracy of node i; the specific steps of this step S3 are: S301. Define the probability density expression of the state quantity X of node i at the current moment based on the standard form of probability density k as follows: where μ is the mean of the current state X of the node k ; λ is a scaling factor with a constant value; ∑ X is the covariance matrix of the state quantity X of node i at the current moment k ; S302: The state quantity X of node i at the current moment k The confidence expression of BI(X k )=P(X k ), and the covariance matrix ∑ of the state quantity of node i at the current moment is obtained after sorting X , whose expression is: ∑ X =(J1 T Q -1 H1 + H2 T R -1 H2) -1 , Wherein, Q -1 is the inverse matrix of Q at the current moment; R -1 is the inverse matrix of R; The recurrence formula of Q is: Q k = FQ k-1 F T + GPG T , where Q k is the covariance matrix of the prior factor f prior at the current moment, and Q k-1 is the covariance matrix of the prior factor f prior at the previous moment; The expression of F is: v k-1 is the speed observation value at the previous moment; t s is the dead reckoning solution period; θ k is the heading angle at the current moment; The expression of G is: P is the random noise of the gyroscope angular rate; R is the relative ranging error noise of UWB; S303. The covariance matrix ∑ of the state quantity X of node i calculated in step S302 at the current moment k ; X The norm ‖∑ X ‖ is the distributed cooperative navigation and positioning accuracy result of node i at the current moment.

2. The distributed cooperative navigation and positioning accuracy evaluation method based on belief propagation according to claim 1, characterized in that In step S302, the expression of P is: is 100 (° / h 3 / 2 ).

3. The distributed collaborative navigation and positioning accuracy evaluation method based on belief propagation according to claim 1, characterized in that In step S302, the expression of R is: is 0.1 m 2 .

Images