CN202442717U

CN202442717U - System for achieving integrated navigation accurate positioning with federal H'8' filter

Info

Publication number: CN202442717U
Application number: CN2012200130961U
Authority: CN
Inventors: 李庆华; 徐元; 刘雪真; 侯萌; 林霏; 甄易
Original assignee: Shandong Institute of Light Industry
Current assignee: Shandong Institute of Light Industry
Priority date: 2012-01-12
Filing date: 2012-01-12
Publication date: 2012-09-19
Anticipated expiration: 2022-01-12

Abstract

The utility model relates to a system for achieving integrated navigation accurate positioning with a federal H'8' filter. The system for achieving the integrated navigation accurate positioning with the federal H'8' filter comprises a radio navigation (RN) nodal point portion and a Bayesian Networks (BN) nodal point portion, wherein the RN nodal point portion is composed of RN nodal points, a wireless sensor network (WSN) module, a supersonic ranging module and a time synchronization module, and the BN nodal point portion is composed of BN nodal points, the WSN module, an inertial navigation system (INS) module, a central data processing module and the time synchronization module. The RN nodal points of the RN nodal point portion are divided into a plurality of clusters, and each cluster is connected with a local filter through the WSN module and the time synchronization module. The local filter of each cluster is connected with a navigational computer though a senior filter, and each local filter is connected with the BN nodal point portion. The system for achieving the integrated navigation accurate positioning with the federal H'8' filter takes full advantage of environmental information of unknown nodal points and achieves fusion of relative navigation information and absolute navigation information.

Description

Application federation H∞System for realizing combined navigation accurate positioning by filter

Technical Field

The utility model relates to a federal H_∞Filter, especially related to application federal H_∞A method and a system for realizing combined navigation accurate positioning by a filter belong to the field of multi-sensor data fusion.

Background

The need for precise location information for unknown nodes has driven the development of precise location techniques, a trend that will remain unchanged for a long period of time in the future. Although the Global Positioning System (GPS) can provide accurate navigation information with continuously stable navigation accuracy, in an environment such as an indoor, high-rise building-intensive urban area, a mine, a tunnel, and the like, a GPS signal is out of lock, and positioning cannot be performed. To solve this problem, many scholars combine an Inertial Navigation System (INS) with a GPS system to form a combined GPS/INS navigation system. Although the INS has the advantages of full autonomy, comprehensive motion information, short time and high precision, the autonomous navigation can be realized, errors are accumulated along with time, and the navigation precision is seriously reduced under the condition of long-endurance operation. Therefore, the compensation of the INS for the GPS navigation information can only be short-term compensation, and the navigation accuracy of the GPS/INS integrated navigation system depends on the navigation accuracy of the GPS, whereas the integrated navigation system cannot provide high-accuracy navigation information in the case of long-term loss of lock of the GPS.

In recent years, Wireless Sensor Networks (WSNs) have shown great potential in the field of short-range positioning due to their low cost, low power consumption and low system complexity. The WSN provides possibility for positioning unknown nodes in areas without GPS signals, i.e. so-called "blind areas", such as indoor, dense urban areas with high-rise buildings, mines, tunnels, etc., but because the communication technology adopted by the WSN is usually short-distance wireless communication technology (such as ZigBee, WIFI, etc.), if it is desired to complete long-distance target tracking and positioning, a large number of network nodes are required to complete together, which increases the network burden of the WSN.

In dealing with the problem of combining navigation data fusion, the most common approach is to use a filter. The best known method is Kalman Filter (KF), however, since KF is designed for linear system, it has only good filtering effect for linear stationary system. In order to realize the filtering problem of the nonlinear system, many scholars convert the nonlinear system into a linear system through a jacobian matrix for filtering, thereby generating an Extended Kalman Filter (EKF). The EKF has good filtering effect on a nonlinear system, and is a filtering method which is widely applied in actual production at present. However, KF and EKF do not completely meet the situation in practical application because they assume that both the state noise and the observation noise of the system are standard white noise in the filtering process. In addition, due to hardware problems of equipment, the situation that the acquisition period of data at each moment is slightly uncertain is caused, data acquisition cannot be carried out according to an ideal situation, and the situation is not considered by the conventional KF and EKF.

Disclosure of Invention

The utility model aims at overcoming the defects and providing an application federal H_∞The filter realizes the method and the system for the combined navigation accurate positioning. The method overcomes the problem that the traditional filtering methods (such as KF and EKF) have specific requirements on system noise, and is more suitable for multi-sensor data fusion in practical application. Meanwhile, the method makes full use of the environmental information of the unknown nodes, and realizes the fusion of the relative navigation information and the absolute navigation information. On a single cluster basisBy Federal H_∞And filtering, namely fusing the data of the local filter to obtain navigation information with higher precision than any single cluster.

The utility model discloses the technical scheme who takes does:

application federation H_∞The system for realizing the combined navigation accurate positioning by the filter comprises an RN node part and a BN node part, wherein the RN node part consists of an RN node, a wireless WSN module, an ultrasonic ranging module and a time synchronization module; the BN node part consists of a BN node, a wireless WSN module, an INS navigation module, a central data processing module and a time synchronization module; the RN nodes of the RN node part are divided into a plurality of clusters, each cluster is connected with a local filter through a wireless WSN module and a time synchronization module, and the local filter of each cluster is connected with a navigation computer through a main filter; each local filter is connected to the BN node portion.

The positioning method of the system comprises the following steps:

(1) clustering reference nodes in the WSN network: in the WSN, Nodes with known positions are defined as Reference Nodes (RNs), Nodes with unknown positions are defined as unknown Nodes (Blind Nodes, BN), the RN Nodes in a certain area (for example, within the range of 10m multiplied by 10 m) are classified into a cluster, one node in each cluster is randomly selected as a cluster head node of the cluster, and the WSN is responsible for collecting data collected by all the RN Nodes in the cluster and exchanging the information with other clusters;

(2) judging the coverage area of the BN node in which cluster, and selecting a positioning mode according to the number of the clusters which can be covered: the method comprises the following steps that information collected by the INS is the position and the speed of a current unknown node BN obtained by resolving data measured by a gyroscope and a meter at each moment, information collected by the WSN is the distance information between the BN node and an RN node at each moment, the BN node obtains signals transmitted by the RN node through the WSN, the BN node is judged to be in the coverage range of which cluster according to the received signals of the RN node, and a positioning mode is selected according to the number of the clusters which can be covered: if only one cluster can cover the BN node, single cluster positioning is carried out, and if more than one cluster can cover the BN node, namely the BN node is positioned at the position where the clusters are overlapped, multi-cluster fusion positioning is carried out;

(3) single cluster positioning or multi-cluster fusion positioning:

single cluster positioning: the INS measures the distance between the RN node and the BN node, and the distance information is input into a local H_∞The filter is used for predicting the position error and the speed error of the BN measured by the INS, and subtracting the information acquired by the INS from the predicted value to obtain navigation information with corrected result;

and (3) multi-cluster fusion positioning: a. in each cluster covering the BN node, the INS measures the distance between the RN node and the BN node in the cluster, the distance information is input into a local filter of the cluster for processing, and the position error and the speed error of the BN measured by the INS in each cluster are estimated; b. based on the error estimate obtained by the local filter, distributed H is used_∞The filter is used for carrying out data fusion on the data obtained by the local filter to obtain the optimal estimated value of the position error and the speed error measured by the INS in the system; c. and finally, the navigation information (the position and the speed of the BN) measured by the INS is differed from the optimal estimated value to obtain the navigation information with the corrected result.

The method for estimating the position error and the speed error of the BN measured by the INS in each cluster in step a of the step (3) above comprises:

(ii) knowing the position error (e) of the node in both x and y directions of the local relative coordinate system at each instant_x,e_y) And speed error (e)_vx,e_vy) The system equation for constructing a local filter as a state vector is shown in formula (i):

in the formula (I), X_c，kIs the state variable of the system, T is the sampling period of the system, (omega)_x,k,ω_y,k) And (ω)_vx,k,ω_vy，k) For the position error noise and velocity error noise at each time of the system equation, as random values, ω_c，kIs the noise vector of the system, A_cIs a system matrix;

measuring the position and the speed of the BN node at the current moment by adopting the INS, measuring the position and the speed of the BN node at the current moment by adopting the WSN, and subtracting the position of the BN node at the current moment measured by the WSN from the position of the BN node at the current moment measured by the INS to obtain a position error; subtracting the speed of the BN node at the current moment measured by the WSN from the speed of the BN node at the current moment measured by the INS to obtain a speed error, and outputting the calculated position error and the calculated speed error as an observation vector, wherein the observation vector is shown in the following formula (II):

in the formula (II), y_c，kFor observation of the vector, X_c，kIs the state variable of the system, (Δ r)_x，k，Δr_y,k) And (Δ v)_x，k，Δv_y,k) For the position error and velocity error at each time of the observation equation, (upsilon)_x，k，υ_y,k) And (upsilon)_vx，k，υ_vy,k) For the position error noise and velocity error noise at each moment of the observation equation, as random values, upsilon_c，kAs a noise vector of the system, C_cIs an observation matrix;

using the above formula (I) and formula (II), adding the linear relation between the quantity to be estimated and the state vector of each cluster, namely z_c，k＝L_cx_c，kObtaining a system state equation of each cluster as shown in a formula (III),

wherein x is_c，k∈RⁿIs the system state of the c-th cluster, A_cFor each local filter, a system matrix, B_cFor each local filter, taking the system noise matrix, C_cFor each local filter, D_cFor each local filter, taking the unit matrix, w_c，kAnd xi_c，kRespectively representing the system noise and the observation noise of each cluster as random values;

fourthly, according to the system state and the observation equation of each cluster, applying x in the formula (III)_c，k+1And y_c，kThe following formula is defined:

\{\begin{matrix} {\hat{x}}_{c, k + 1} = A_{c} {\hat{x}}_{c, k} + K_{c} (y_{c, k} - C_{c} {\hat{x}}_{c, k}) \\ {\hat{z}}_{c, k} = L_{c} {\hat{x}}_{c, k} \end{matrix} - - - (IV)

wherein

And

are respectively x_c，kAnd z_c，kObserved value of, K_cFor Kalman gain, define

{\tilde{x}}_{c, k} = x_{c, k} - {\hat{x}}_{, c},

{\tilde{z}}_{c, k} = z_{c, k} - {\hat{z}}_{c, k},

To give formula (V):

wherein,

and (4) obtaining the position error and speed error estimated vector of BN measured by INS in each cluster through a formula (V).

The distributed H in the step b of the step (3) is_∞The method for the filter to perform data fusion on the data obtained by the local filter is as follows:

constructing global system state estimates

Satisfying the formula (VI), solving the weight matrix through inequalities (VII-1) and (VII-2) described below, substituting the weight matrix into the formula (VI) to finally obtain the optimal error estimation of the main filter,

wherein,

in the formula M_cThe weight matrix representing the fusion information output of each distribution subsystem, and the sum of the weight matrix is a unit matrix I; using the following inequality, when there is a feasible solution, and the solution is greater than 0, looking at the energy performance index gamma of the

filter

_c1,2, N andselecting a group of parameters which enable the energy performance index to be minimum as a final weight matrix; if a feasible solution exists for the set of linear matrix inequalities (VII) (VII-1-2 is all inclusive)

Q_c,M_cN, then system (v) is progressively stable and H is present_∞

Performance index gamma

_c1,2, N andγ_cthe size of (2) determines each cluster H_∞The degree of interference suppression by the filter is,

is determined by the size of Federal H_∞Degree of interference suppression of filter, gamma_cAnd

the larger the interference suppression degree, the less robust the filter. (I is a unit matrix, L is defined as above)

In the formula

\tilde{P} = [\begin{matrix} P_{1} & 0 & . . . & 0 \\ 0 & P_{2} & . . . & 0 \\ 0 & 0 & . . . & 0 \\ 0 & 0 & . . . & P_{N} \end{matrix}],

\tilde{A} = diag (A - P_{1}^{- 1} Q_{1} C_{1}, A - P_{2}^{- 1} Q_{2} C_{2}, . . ., A - B_{N}^{- 1} Q_{N} C_{N})

\tilde{K} = diag (B_{1} - P_{1}^{- 1} Q_{1} D_{11}, B_{1} - P_{2}^{- 1} Q_{2} D_{21}, . . ., B_{1} - P_{N}^{- 1} Q_{N} D_{N}),

(the parameters are derived from the distributed subsystem equation and H_∞Parameter matrix of filter, N distribution subsystems in total, subscript indicating serial number of distribution subsystem).

In order to reduce WSN's network burden, the utility model provides a earlier with WSN network clustering, at each in cluster, make up WSN and INS, utilize the relative navigation information based on WSN's wireless location technique measurement unknown node, the absolute navigation information who obtains on relative navigation information and the INS passes through distributing type H_∞And the filter performs data fusion to obtain ideal navigation information. The method takes the position error and the speed error of an unknown node in two directions at each moment as a state vector, takes the position error and the speed error calculated according to data acquired by two systems at each moment as an observation vector output, and reconstructs an asymptotically stable system, namely H, from the state vector and the observation vector output_∞And a filter. By H_∞The filter estimates the optimal state vector at each time instant. The method overcomes the noise of the traditional filtering method (such as KF and EKF) on the systemThe method has the problem of specific requirements, and is more suitable for multi-sensor data fusion in practical application. Meanwhile, the method makes full use of the environmental information of the unknown nodes, and realizes the fusion of the relative navigation information and the absolute navigation information. On a single cluster basis, using multiple filter federates H_∞And filtering, namely fusing the data of the local filter to obtain navigation information with higher precision than any single cluster.

Drawings

FIG. 1 shows the utility model applies Federal H_∞The filter realizes an integrated navigation accurate positioning system;

FIG. 2 shows a single cluster utilization H_∞A schematic diagram of a filtering combination navigation method;

FIG. 3 is a multi-cluster application federation H_∞A schematic diagram of a filtering combination navigation method;

FIG. 4 is a flow chart of a method of the present invention;

FIG. 5 shows the distribution of reference nodes and unknown nodes in example 1;

fig. 6 is a flowchart of data fusion of data obtained by the local filter in embodiment 1;

wherein: 1, RN node a, 2, RN node b, 3, RN node c,4, BN node part, 5, ultrasonic ranging module, 6, time synchronization module, 7, wireless WSN module, 8, INS navigation module, 9, local filter, 10, main filter, 11, navigation computer, 12, navigation information, 13, central data processing module, 14, position difference, speed difference, 15, unknown node, 16, cluster head node, 17, reference node, 18, start node, 19, optimal error evaluation, 20, end node, 21,

cluster

1, 22, cluster 2,

23. cluster 2 measured location, 24 cluster 1 measured BN location, 25 INS measured trajectory.

Detailed Description

The invention will be described in detail with reference to the accompanying drawings

Application federation H_∞The system for realizing the combined navigation accurate positioning by the filter comprises an RN node part and a BN node part, wherein the RN node part consists of an RN node, a wireless WSN module 7, an ultrasonic ranging module 5 and a time synchronization module 6; the BN node part consists of a BN node, a wireless WSN module 7, an INS navigation module 8, a central data processing module and a time synchronization module 6; the RN nodes of the RN node part are divided into a plurality of clusters, each cluster is connected with a local filter 9 through a wireless WSN module 7 and a time synchronization module 6, and the local filter 9 of each cluster is connected with a navigation computer through a main filter 10; each local filter is connected to the BN node portion.

A specific implementation example is given below.

In the environment shown in the following figure, the reference nodes are divided into two clusters (as shown in fig. 5), where cluster 1 and cluster 2 each include 4 reference nodes. The BN node moves along the illustrated trajectory.

As shown in fig. 6, the method of this embodiment includes the following steps:

(1) and acquiring the table adding information through the INS, resolving after acquiring the table adding information, and obtaining the position and speed information of the BN node measured by the INS, wherein the position information of the BN measured by the INS at the first moment of the intersection part is (51.8732, 22.0200).

(2) And the BN node acquires the signal transmitted by the RN node through the WSN. And judging the coverage range of the BN node in which cluster according to the received signal of the RN node, and selecting a positioning mode according to the number of the clusters which can be covered, wherein both the cluster 1 and the cluster 2 can cover the BN node in the example.

(3) And (3) if more than one cluster capable of covering the BN node is measured in the step (2), performing multi-cluster fusion positioning. In a cluster capable of covering a BN node, firstly measuring the distance between an RN node and the BN node, and obtaining the position information of the BN node measured by two cluster WSNs through the distance information, wherein the measured BN position of cluster 1 is (29.2414, 10.3006); cluster 2 measured BN position (29.0979, 11.1479).

And subtracting the position information of the BN obtained by the WSN of the two clusters from the position information of the BN obtained by the measurement of the INS to obtain the estimated error of the INS in the two clusters, namely cluster 1 (22.6317, 11.7195) and cluster 2 (22.7752, 10.8722). Inputting the error information into local part H_∞And the filter is used for predicting the INS error.

First, the P, Q, K matrix satisfying the linear matrix inequality in FIG. 6 is obtained.

For cluster 1, the resulting arrays are:

γ₁＝1.9804×10^-12,

for cluster 2, the resulting arrays are:

γ₂＝6.3708×10^-14,

the weights M of the final main filter for the error optimum estimation measured by the local filters in the individual clusters are obtained by the matrix, wherein the weight M1 of cluster 1 is

M_{1} = [\begin{matrix} 0.9998 & - 0.0001 & - 0.0007 & - 0.0001 \\ - 0.0001 & 0.9998 & - 0.0001 & - 0.0007 \\ - 0.0007 & - 0.0001 & 0.9962 & - 0.0001 \\ - 0.0001 & - 0.0007 & - 0.0001 & 0.9962 \end{matrix}]

Weight M2 for cluster 2 is

M_{2} = [\begin{matrix} 0.0002 & 0.0001 & 0.0007 & 0.0001 \\ 0.0001 & 0.0002 & 0.0001 & 0.0007 \\ 0.0007 & 0.0001 & 0.0038 & 0.0001 \\ 0.0001 & 0.0007 & 0.0001 & 0.0038 \end{matrix}] .

Performing data fusion on data obtained by the local filter on the basis of INS error estimation obtained by the local filter, and performing X-ray fusion on the data_k＝M₁X_C1,k+M₂X_C2,kAn optimal error estimate for the main filter is obtained (22.5978, 11.6314). The navigation information (51.8732, 22.0200) measured by the INS itself at this time is subtracted by the optimal error estimate of the main filter to obtain a corrected result (29,2754, 10.3886).

The above example is the estimation of the position error of the BN node measured by the INS by the distributed filter, and the estimation of the speed error of the BN node is similar to the estimation of the position error, and will not be described in detail here.

Claims

1. Application federation H_∞The system for realizing the combined navigation accurate positioning by the filter comprises an RN node part and a BN node part, and is characterized in that the RN node part consists of an RN node, a wireless WSN module, an ultrasonic distance measurement module and a time synchronization module; the BN node part consists of a BN node, a wireless WSN module, an INS navigation module, a central data processing module and a time synchronization module; the RN nodes of the RN node part are divided into a plurality of clusters, each cluster is connected with a local filter through a wireless WSN module and a time synchronization module, and the local filters of each cluster are connected with a navigation computer through a main filter(ii) a Each local filter is connected to the BN node portion.