A multi-node dynamic network collaborative positioning method

Abstract

The present invention discloses a multi-node dynamic network collaborative positioning method, including: constructing a dynamic collaborative positioning method for adaptively adjusting the system observation error covariance matrix based on the Kalman filter algorithm based on node observations; constructing a dynamic collaborative positioning method based on the federated Kalman filter based on the variance upper bound technology; and realizing multi-node dynamic network collaborative positioning by combining the dynamic collaborative positioning method for adaptively adjusting the system observation error covariance matrix with the dynamic collaborative positioning method based on the federated Kalman filter. Based on the Kalman filter principle, the present invention integrates the filtering method for adaptively adjusting the observation error covariance matrix with the federated filtering method to realize the normal operation of the collaborative positioning system in situations such as the conversion between anchor nodes and ordinary nodes between mobile nodes, the disappearance and appearance of ranging values ​​between nodes, and the random access and exit of nodes.

Description

Technical Field

[0001] The present invention relates to the technical field of dynamic networks, and in particular to a multi-node dynamic network collaborative positioning method. Background Art

[0002] For collaborative positioning and navigation technologies based on distance measurement, Kalman filtering is currently widely used for engineering implementation. This approach establishes a motion model for mobile nodes and, based on this, designs a multi-node collaborative positioning Kalman filter to achieve optimal position estimation for system nodes. However, in actual applications, nodes are always in motion and often face scene switching issues. This can cause the roles of anchor nodes and ordinary nodes to change, ranging values ​​between nodes to disappear or be generated, and nodes to randomly join and leave the network, causing the Kalman filter algorithm to fail to work properly.

[0003] This patent proposes a multi-node dynamic network collaborative positioning method based on the Kalman filtering principle. By integrating the filtering method of adaptively adjusting the observation error covariance matrix with the federal filtering method, the collaborative positioning system can be operated normally in situations such as the transformation of anchor nodes and ordinary nodes between mobile nodes, the disappearance and appearance of ranging values ​​between nodes, and the random access and exit of nodes. Summary of the Invention

[0004] The purpose of this section is to summarize some aspects of the embodiments of the present invention and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the abstract and title of this application to avoid obscuring the purpose of this section, the abstract and the title of the invention, and such simplifications or omissions should not be used to limit the scope of the present invention.

[0005] In view of the above existing problems, the present invention is proposed.

[0006] Therefore, the technical problem solved by the present invention is that the existing technology may cause the system to not work properly when the scene is switched.

[0007] To solve the above technical problems, the present invention provides the following technical solutions: a multi-node dynamic network collaborative positioning method, comprising:

[0008] Based on the node observations, a dynamic collaborative positioning method is constructed based on the Kalman filter algorithm to adaptively adjust the system observation error covariance matrix;

[0009] A dynamic collaborative positioning method based on federated Kalman filtering is constructed based on the variance upper bound technology;

[0010] Dynamic network collaborative positioning of multiple nodes is achieved by combining the dynamic collaborative positioning method of adaptively adjusting the system observation error covariance matrix with the dynamic collaborative positioning method based on federated Kalman filtering.

[0011] As a preferred solution of the multi-node dynamic network collaborative positioning method described in the present invention, a collaborative positioning network with a fixed number of nodes N is defined. When all ranging information between nodes exists and all nodes can receive BDS satellites, the observation quantity can be expressed as:

[0012]

[0013] in: It is divided into external observations of node i and node j and internal observations of itself.

[0014] As a preferred solution of the multi-node dynamic network collaborative positioning method of the present invention, when the initial state is known, the Kalman filtering process includes:

[0015] A1: Based on the state matrix and the state estimate at time k-1, we can get the prediction at time k:

[0016] x k,k-1 = Φ k,k-1 x k-1

[0017]

[0018] Among them, x k represents the state of the system at time k, x k,k-1 represents the state at time k predicted by time k-1, Φ k,k-1 represents the state transition matrix from time k-1 to time k; Q k represents the state noise covariance matrix of the collaborative positioning system; Γ k-1 is the noise distribution matrix from time k-1 to time k

[0019] A2: Calculate the Kalman filter gain:

[0020]

[0021] Among them, K k represents the Kalman filter gain at time k, H k is the observation transfer matrix at time k; is the covariance matrix of the observation error at time k.

[0022] A3: Calculate the posterior through the Kalman gain:

[0023] x k =x k,k-1 +K k (z k -H k x k,k-1)

[0024] P k =(IK k H k )P k,k-1

[0025] As a preferred solution of the multi-node dynamic network collaborative positioning method described in the present invention, wherein: the observation quantity corresponds to the system observation error covariance matrix R k The diagonal is the observation error variance, the off-diagonal elements are 0, and the dimension is Matrix of

[0026] When the set of observation information obtained by the system at time k is E, the condition for whether the system observation quantity exists at time k is expressed as matrix A k , the matrix A k Expressed as:

[0027]

[0028] in,

[0029] As a preferred solution of the multi-node dynamic network collaborative positioning method of the present invention, the Kalman gain formula is rewritten as follows:

[0030]

[0031] Among them, anti(·) is the reverse calculation of the matrix diagonal elements,

[0032] i is the position of the element in the matrix, x +∞ is a value approaching positive infinity.

[0033] As a preferred solution of the multi-node dynamic network collaborative positioning method of the present invention, when a certain observation value disappears in the system observation value, the state prediction value is used to replace the observation value. The state prediction value is expressed as:

[0034]

[0035] Among them, rep(·) is the replacement calculation of the observation,

[0036] As a preferred solution of the multi-node dynamic network collaborative positioning method described in the present invention, the Kalman filter algorithm for collaborative positioning of nodes and dynamic changes between nodes is rewritten to include:

[0037] Compute state prediction:

[0038] x k,k-1=Φ k,k-1 x k-1

[0039]

[0040] Calculate the Kalman filter gain:

[0041]

[0042] Update system observations

[0043]

[0044] Compute the state posteriors:

[0045]

[0046] P k =(IK k H k )P k,k-1

[0047] As a preferred solution of the multi-node dynamic network collaborative positioning method described in the present invention, the dynamic collaborative positioning method based on federal Kalman filtering consists of a reference system and two new and old subsystems. By using a feedback-free mode, each self-filter is independent and parallel, and the variance upper bound technology is adopted to obtain the local state estimation and error mean square error of the system respectively, and then input them into the main system and globally filtered as the observation quantity of the main system to obtain the global state estimation of the collaborative system. After the output of the new system is stable, the old system filter is turned off to achieve a smooth transition of the system state after the node is joined.

[0048] As a preferred solution of the multi-node dynamic network collaborative positioning method of the present invention, the process of the multi-node dynamic network collaborative positioning includes:

[0049] First, the state quantity and estimated error covariance matrix are initialized, and information about all nodes and nodes in the system is obtained. Then, it is determined whether a federated filter exists. When a federated filter exists, the new filter updates the state prediction and the predicted state error covariance matrix. When the federated filter does not exist, the number of nodes changes, and a federated Kalman filter is constructed. The new and old filters update the state prediction and the predicted state error covariance matrix to determine whether the number of observations at the nodes and between the nodes has changed.

[0050] As a preferred solution of the multi-node dynamic network collaborative positioning method of the present invention, the process of the multi-node dynamic network collaborative positioning includes:

[0051] When the number of observations at a node or between nodes changes, the reliability of the sensor signal is evaluated, and the observation judgment matrix, observation error covariance matrix, and observation vector are updated. When the number of observations at a node or between nodes does not change, the observation error covariance matrix and observation vector are directly updated, and then the Kalman filter gain is calculated.

[0052] The state posterior estimate and state estimate covariance matrix are updated through the result of the Kalman filter gain, and then it is determined whether a federal filter exists. When a federal filter exists, it is determined whether the old filter solution conditions are met. If so, the state is directly output. If not, a global filter is used for fusion, and then the state is output. When no federal filter exists, the state is directly output to obtain the node's positioning information.

[0053] Beneficial effects of the present invention: A multi-node dynamic network collaborative positioning method provided by the present invention is based on the Kalman filtering principle. By integrating the filtering method of adaptively adjusting the observation error covariance matrix with the federal filtering method, the collaborative positioning system can be operated normally in situations such as the transformation of anchor nodes and ordinary nodes between mobile nodes, the disappearance and appearance of ranging values ​​between nodes, and the random access and exit of nodes. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort. Among them:

[0055] Figure 1 This is an overall flow chart of a multi-node dynamic network collaborative positioning method according to an embodiment of the present invention;

[0056] Figure 2 A dynamic collaborative positioning federated Kalman filter diagram of a multi-node dynamic network collaborative positioning method according to an embodiment of the present invention;

[0057] Figure 3 This is a dynamic collaborative positioning error diagram of a multi-node dynamic network collaborative positioning method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0058] To make the above-mentioned objects, features, and advantages of the present invention more clearly understood, the following detailed description of the specific embodiments of the present invention is given in conjunction with the accompanying drawings. It is obvious that the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary persons in this field without creative work should fall within the scope of protection of the present invention.

[0059] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0060] Secondly, the term "one embodiment" or "embodiment" herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in various places throughout this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive of other embodiments.

[0061] The present invention is described in detail with reference to schematic diagrams. For ease of illustration, cross-sectional views of device structures may be partially enlarged and not to scale when describing embodiments of the present invention. Furthermore, the schematic diagrams are merely illustrative and should not limit the scope of the present invention. Furthermore, in actual production, the three-dimensional dimensions of length, width, and depth should be included.

[0062] In the description of the present invention, it should be noted that the terms "upper, lower, inner, and outer" and other references to orientations or positional relationships are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate and simplify the description of the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limitations on the present invention. Furthermore, the terms "first, second, or third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0063] In this disclosure, unless otherwise specified or limited, the terms "mounted," "connected," and "connected" should be interpreted broadly. For example, they may refer to fixed, removable, or integral connections. They may also refer to mechanical, electrical, or direct connections, indirect connections through an intermediary, or internal communication between two components. Those skilled in the art will understand the specific meanings of these terms in this disclosure.

[0064] Example 1

[0065] Reference Figure 1 —2 is the first embodiment of the present invention, which provides a multi-node dynamic network collaborative positioning method, including:

[0066] S1: Based on the node observation, a dynamic collaborative positioning method is constructed based on the Kalman filter algorithm to adaptively adjust the system observation error covariance matrix;

[0067] Furthermore, a collaborative positioning network with a fixed number of nodes N is defined. When all ranging information between nodes exists and all nodes can receive BDS satellites, the observation quantity can be expressed as:

[0068]

[0069] in: It is divided into external observations of node i and node j and internal observations of itself.

[0070] Furthermore, when the initial state is known, the Kalman filter process includes:

[0071] A1: Based on the state matrix and the state estimate at time k-1, we can get the prediction at time k:

[0072] x k,k-1 =Φ k,k-1 x k-1

[0073]

[0074] Among them, x k represents the state of the system at time k, x k,k-1 represents the state at time k predicted by time k-1, Φ k,k-1 represents the state transition matrix from time k-1 to time k; Q k represents the state noise covariance matrix of the collaborative positioning system; Γ k-1 is the noise distribution matrix from time k-1 to time k

[0075] A2: Calculate the Kalman filter gain:

[0076]

[0077] Among them, K k represents the Kalman filter gain at time k, H k is the observation transfer matrix at time k; is the covariance matrix of the observation error at time k.

[0078] A3: Calculate the posterior through the Kalman gain:

[0079] x k =x k,k-1 +X k (z k -H k x k,k-1 )

[0080] P k =(IK k H k )P k,k-1

[0081] Furthermore, the observations correspond to the system observation error covariance matrix R k The diagonal is the observation error variance, the off-diagonal elements are 0, and the dimension is Matrix of

[0082] When the set of observation information obtained by the system at time k is E, the condition for whether the system observation quantity exists at time k is expressed as matrix A k , matrix A k Expressed as:

[0083]

[0084] in,

[0085] Furthermore, the Kalman gain formula is rewritten as follows:

[0086]

[0087] Among them, anti(·) is the reverse calculation of the matrix diagonal elements, i is the position of the element in the matrix, x +∞ is a value approaching positive infinity.

[0088] In the system observation, when a certain observation disappears, the state prediction value is used to replace the observation value. The state prediction value is expressed as:

[0089]

[0090] Among them, rep(·) is the replacement calculation of the observation,

[0091] Furthermore, the Kalman filter algorithm for collaborative positioning of nodes and dynamic changes between nodes is rewritten to include:

[0092] Compute state prediction:

[0093] x k,k-1 =Φ k,k-1 x k-1

[0094]

[0095] Calculate the Kalman filter gain:

[0096]

[0097] Update system observations

[0098]

[0099] Compute the state posteriors:

[0100]

[0101] P k =(IK k H k )P k,k-1

[0102] S2: A dynamic collaborative localization method based on federated Kalman filtering is constructed based on the variance upper bound technique;

[0103] Furthermore, the dynamic collaborative positioning method based on the federated Kalman filter consists of a reference system and two new and old subsystems, such as Figure 2 As shown in the figure, by using the feedback-free mode, each self-filter is independent and parallel, and the variance upper bound technique is adopted to obtain the local state estimation and error mean square error of the system respectively, which are then input into the main system and used as the observation quantity of the main system for global filtering to obtain the global state estimation of the collaborative system. After the output of the new system is stable, the old system filter is turned off to achieve a smooth transition of the system state after the node is added.

[0104] It should be noted that for collaborative node exits, based on the variance upper bound technique, the initial value of the new Kalman filter equation can use the state estimate and state error covariance value of the old Kalman filter. For node access, the initial state of the node and the covariance of the state error are unknown. Generally speaking, the initial state of the new node is taken within a rough range, and the covariance of its corresponding state error is set to a larger value. Therefore, the new Kalman filter equation will be interfered with by the initial state error of the new node in the initial stage, causing fluctuations in the states and state error covariances of other nodes. Therefore, in order to reduce the fluctuations in positioning results caused by new nodes and achieve smooth switching between the two Kalman filters, a dynamic collaborative positioning method based on the federated Kalman filter is established during this period. After the new Kalman filter stabilizes, the original Kalman filter solution is stopped.

[0105] S3: Dynamic network collaborative positioning of multiple nodes is achieved by combining the dynamic collaborative positioning method of adaptively adjusting the system observation error covariance matrix with the dynamic collaborative positioning method based on federated Kalman filtering.

[0106] Furthermore, the process of multi-node dynamic network co-location includes:

[0107] First, the state quantity and estimated error covariance matrix are initialized, and information about all nodes and nodes in the system is obtained. Then, it is determined whether a federated filter exists. When a federated filter exists, the new filter updates the state prediction and the predicted state error covariance matrix. When the federated filter does not exist, the number of nodes changes, and a federated Kalman filter is constructed. The new and old filters update the state prediction and the predicted state error covariance matrix to determine whether the number of observations at the nodes and between the nodes has changed.

[0108] When the number of observations at a node or between nodes changes, the reliability of the sensor signal is evaluated, and the observation judgment matrix, observation error covariance matrix, and observation vector are updated; when the number of observations at a node or between nodes does not change, the observation error covariance matrix and observation vector are directly updated, and then the Kalman filter gain is calculated.

[0109] The state posterior estimate and state estimate covariance matrix are updated through the result of the Kalman filter gain, and then it is determined whether a federal filter exists. When a federal filter exists, it is determined whether the old filter solution conditions are met. If so, the state is directly output. If not, a global filter is used for fusion, and then the state is output. When no federal filter exists, the state is directly output to obtain the node's positioning information.

[0110] Example 2

[0111] Reference Figure 3 , is an embodiment of the present invention, which provides a multi-node dynamic network collaborative positioning method. In order to verify the beneficial effects of the present invention, scientific demonstration is carried out through specific implementation data.

[0112] This embodiment tests the collaborative positioning algorithm in a dynamic network environment. The simulation time is 1800 seconds, and a dynamic change is performed every 300 seconds. The specific change settings are as follows. The simulation results are shown in Table 1.

[0113] Table 1 Dynamic network change settings

[0114]

[0115] like Figure 3As shown in the figure, within the range of 0-300s, the system has two anchor nodes and full ranging information, and its positioning error is maintained at a small level; within 300-600s, the system anchor node disappears, and in the BDS signal denial environment, the system error becomes divergent; within 600-900s, the system ranging information is reduced by 2, and the positioning error divergence speed increases due to fewer constraints; within 900-1200s, one of the nodes randomly exits, the number of system nodes becomes 4, and the system state quantity and observation quantity change at the same time. At this time, the system process is carried out by establishing a federal Kalman filter. The system has 6 ranging values ​​and is fully connected between nodes, so the overall error divergence rate is smaller than that of 600-900s. During the period of 1200-1500s, new nodes are connected to the system and the number of anchor nodes increases, so its positioning error is greatly reduced and remains stable overall. However, relying on only one anchor node cannot completely correct the system's early solution error, so a fixed error still exists in this stage. After 1500s, a new anchor node is added, and the error in the previous stage is corrected. The system positioning accuracy is basically the same as that of the first node.

[0116] In summary, the system's positioning performance in different situations verifies the effectiveness of the dynamic collaborative positioning algorithm.

[0117] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A multi-node dynamic network collaborative positioning method, characterized in that: include: Based on the node observations, a dynamic collaborative positioning method is constructed based on the Kalman filter algorithm to adaptively adjust the system observation error covariance matrix; A dynamic collaborative positioning method based on federated Kalman filtering is constructed based on the variance upper bound technology; Dynamic network collaborative positioning of multiple nodes is achieved by combining the dynamic collaborative positioning method of adaptively adjusting the system observation error covariance matrix with the dynamic collaborative positioning method based on federated Kalman filtering; The dynamic collaborative positioning method based on federated Kalman filtering consists of a reference system and two subsystems, old and new. By using a feedback-free mode, each self-filter is independent and parallel, and the variance upper bound technique is adopted to obtain the local state estimate and error mean square error of the system. The local state estimate and error mean square error are then input into the main system and used as the observation quantity of the main system for global filtering to obtain the global state estimate of the collaborative system. After the output of the new system stabilizes, the old system filter is turned off to achieve a smooth transition of the system state after the node is added. The process of dynamic network collaborative positioning of multiple nodes includes: First, the state quantity and estimated error covariance matrix are initialized, and information about all nodes and nodes in the system is obtained. Then, it is determined whether a federated filter exists. If a federated filter exists, the new filter updates the state prediction and the predicted state error covariance matrix. If a federated filter does not exist, the number of nodes changes, and a federated Kalman filter is constructed. The new and old filters update the state prediction and the predicted state error covariance matrix, and it is determined whether the number of observations at the nodes and between the nodes has changed. The process of dynamic network collaborative positioning of multiple nodes includes: When the number of observations at a node or between nodes changes, the reliability of the sensor signal is evaluated, and the observation judgment matrix, observation error covariance matrix, and observation vector are updated. When the number of observations at a node or between nodes does not change, the observation error covariance matrix and observation vector are directly updated, and then the Kalman filter gain is calculated. The state posterior estimate and state estimate covariance matrix are updated through the result of the Kalman filter gain, and then it is determined whether a federal filter exists. When a federal filter exists, it is determined whether the old filter solution conditions are met. If so, the state is directly output. If not, a global filter is used for fusion, and then the state is output. When no federal filter exists, the state is directly output to obtain the node's positioning information.

2. The multi-node dynamic network collaborative positioning method according to claim 1, wherein: Define a collaborative positioning network with a fixed number of nodes N. When all ranging information between nodes exists and all nodes can receive BDS satellites, the observation quantity can be expressed as: in: It is divided into external observations of node i and node j and internal observations of itself.

3. The multi-node dynamic network collaborative positioning method according to claim 2, wherein: When the initial state is known, the Kalman filter process includes: A1: Based on the state matrix and the state estimate at time k-1, we can get the prediction at time k: x k,k-1 = Φ k,k-1 x k-1 Among them, x k represents the state of the system at time k, x k,k-1 represents the state at time k predicted by time k-1, Φ k,k-1 represents the state transition matrix from time k-1 to time k; Q k represents the state noise covariance matrix of the collaborative positioning system; Γ k-1 is the noise distribution matrix from time k-1 to time k; A2: Calculate the Kalman filter gain: Among them, K k represents the Kalman filter gain at time k, H k is the observation transfer matrix at time k; is the covariance matrix of observation error at time k; A3: Calculate the posterior through the Kalman gain: x k =x k,k-1 +X k (z k -H k x k,k-1 ) P k =(I-K k H k )P k,k-1 。 4. The multi-node dynamic network collaborative positioning method according to claim 3, wherein: The observation quantity corresponds to the system observation error covariance matrix R k The diagonal is the observation error variance, the off-diagonal elements are 0, and the dimension is Matrix of When the set of observation information obtained by the system at time k is E, the condition for whether the system observation quantity exists at time k is expressed as matrix A k , the matrix A k Expressed as: in, 5. The multi-node dynamic network collaborative positioning method according to claim 4, characterized in that: Rewrite the Kalman gain formula as follows: Among them, anti(·) is the reverse calculation of the matrix diagonal elements, i is the position of the element in the matrix, x +∞ is a value approaching positive infinity.

6. The multi-node dynamic network collaborative positioning method according to claim 5, characterized in that: In the system observation, when a certain observation disappears, the state prediction value is used to replace the observation value. The state prediction value is expressed as: Among them, rep(·) is the replacement calculation of the observation, 7. The multi-node dynamic network collaborative positioning method according to claim 6, characterized in that: The Kalman filter algorithm for collaborative positioning of nodes and dynamic changes between nodes is rewritten to include: Compute state prediction: x k,k-1 =Φ k,k-1 x k-1 Calculate the Kalman filter gain: Update system observations Compute the state posteriors: P k =(I-K k H k )P k,k-1 。

Images