CN103292813B - A kind of information filter method improving water surface ship formation navigation accuracy - Google Patents

Abstract

The invention discloses a kind of information filter method improving water surface ship navigation accuracy, relate to a kind of collaborative navigation technology.Achieving low precision water surface ship utilizes other ship high precision navigation information to improve self poisoning precision.Its method is: set up corresponding coordinate system; Above-listed motion model and the observation model writing water surface ship collaborative navigation on the basis of built coordinate system; Nonlinear model is carried out linearization; Information filter is utilized to carry out time renewal and observation renewal.The present invention is applicable to the real-time navigation location of water surface ship.

Description

A kind of information filter method improving water surface ship formation navigation accuracy

Technical field

The invention belongs to field of navigation technology, relate to the air navigation aid that a kind of water surface ship is formed into columns, be specifically related to a kind of collaborative navigation method based on information filter.

Background technology

Along with the intensification of exploitation ocean understanding, water surface ship becomes study hotspot with advantages such as its mobility strong, cost are low.In the cooperative localization of many unmanned boats, it is positioning precision when utilizing the relative observation information raising between unmanned boat self to navigate separately, this advantage is particularly evident in the unmanned boat group of an assembly for sensor inequality, and the unmanned boat equipping low precision can improve its precision by sharing information with high precision unmanned boat.When some ship loses self-contained navigation ability due to sensor or environmental factor, collaborative navigation can recover the homing capability of these ships to a certain extent.Therefore the collaborative navigation studying many waters surface ship has important theory value and practical significance.

At present, relatively less for the technical research of many waters surface ship collaborative navigation, conventional method adopts the kalman filter method of various improvement to carry out data fusion in conjunction with concrete navigation sensor, greatly can improve the navigation accuracy of host node, on this basis the precise information of host node fed back to each node thus improve multi-platform overall navigation ability.But it has its defect in application, will change square parameter that is all and observer state correlation behavior when observing and upgrading, calculated amount is increased, and communication complexity is larger.Information filter obtains increasing application as a kind of new filtering method.If evaluated error is very large, then the set of data of the mean squared error matrix estimated is all very large, mean squared error matrix invert after the set of data will be very little, state estimation comprises the information of state just seldom, so P ^-1measurement can be regarded as quantitative measurement value containing status information.Traditionally, by I=P ^-1be called information matrix.Information filter is applied in the collaborative navigation of many ships by the present invention.

Summary of the invention

In order to overcome the defect existed in prior art, improving the navigation and positioning accuracy that water surface ship is formed into columns, the invention provides a kind of collaborative navigation method based on information filter.Its technical scheme is as follows:

Based on a collaborative navigation method for information filter, comprise the following steps:

Step one, set up coordinate system

Geographic coordinate system (under be designated as t): OX _ty _tz _t

Geographic coordinate system represents sky, the northeast coordinate system of carrier present position, and its initial point is selected in carrier center of gravity place, X _tpoint to east, Y _tpoint to north, Z _talong sensing sky, vertical line direction.

Carrier coordinate system (under be designated as b): OX _by _bz _b

Carrier coordinate system is connected in itself, and its true origin O is positioned at the center of gravity place of carrier, X _bpoint to right along transverse axis, Y _bbefore pointing to along the longitudinal axis, Z _bperpendicular to OX _by _b, and point to along the vertical axes of carrier.

Step 2, set up the motion model of water surface ship formation collaborative navigation.

Note k moment whole system state is i.e. horizontal ordinate, ordinate and course angle.Speed v is in coordinate system, and before pointing to along the longitudinal axis, it is relevant with course angle in the projection of Department of Geography, being projected as of east orientation axle being projected as of north orientation axle then the motion model of reckoning is:

Set up the collaborative navigation mathematical model based on single pilot boat, following the ship equation of motion can be expressed as:

$\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}---\left(1\right)$

V in formula _k, φ _kbe respectively speed, the course angle of following ship, all disturb by white Gaussian noise.The equation of motion is abbreviated as:

x _k+1＝f(x _k，u _k，w _k)＝x _k+Γ(u _k+w _k)(2)

In formula represent and follow the state of ship in the k moment; u _k=(v _kφ _k) ^t; Γ (u _k+ w _k) be nonlinear terms, w _kfor white Gaussian noise, and:

$Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{\mathrm{\σ}}_{vk}^2& 0\\ 0& {\mathrm{\σ}}_{\mathrm{\φ}k}^2\end{array}\right]---\left(3\right)$

The foundation of step 3, water surface ship collaborative navigation observation equation.

According to following ship from moment t _kto moment t _k+1the distance vector of motion, by t _kthe geometric position that moment follows ship and pilot boat moves to t _k+1moment, t _kmoment, t _k+1two circles that moment take pilot boat as the center of circle, two ship spacing are radius have intersection point.By the geometric relationship in figure, the expression formula of two ship spacings can be obtained:

$\begin{array}{c}{R}_k^2={\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)}^2+{\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)}^2\\ R_{k+1}^2={\left(x_{k+1}^S-x_{k+1}^M\right)}^2+{\left(y_{k+1}^S-y_{k+1}^M\right)}^2\end{array}---\left(4\right)$

List measurement equation as follows:

$\begin{array}{c}{Z}_{k+1}=h(x_{k+1}^S,y_{k+1}^S,x_k^M,y_k^M,x_{k+1}^M,y_{k+1}^M,d_{x_{k,k+1}},d_{y_{k,k+1}})+w_z\\ =\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]+w_z\end{array}---\left(5\right)$

In formula, w _zfor observation noise.

The linearization of step 4, water surface ship collaborative navigation system model.

The later state-transition matrix of linearization is:

${\mathrm{\Φ}}_{k+1,k}=\frac{\∂f}{\∂X_k^T}{|}_{X_k={\hat{X}}_k}\≈I+\mathrm{\Δ}t\·\frac{\∂f}{\∂{X_k}^T}=I+\mathrm{\Δ}t\·\left[\begin{array}{ccc}1& 0& -\mathrm{\Δ}t\·V_k{\mathrm{sin\φ}}_k\\ 0& 1& \mathrm{\Δ}t\·V_k{\mathrm{cos\φ}}_k\\ 0& 0& 1\end{array}\right],$

Linearized system noise excitation matrix is: $G_k=\frac{\∂f}{\∂{u_k}^T}=\left[\begin{array}{cc}\mathrm{\Δ}t\·{\mathrm{cos\φ}}_k& 0\\ \mathrm{\Δ}t\·{\mathrm{sin\φ}}_k& 0\\ 0& \mathrm{\Δ}t\end{array}\right].$

Linearized system measurement matrix can be expressed as:

$H_{k+1}=\frac{\∂Z_{k+1}}{\∂{X_{k+1}}^T}\left[\begin{array}{ccc}2*\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)& 2*\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)& 0\\ 2*\left(x_{k+1}^S-x_{k+1}^M\right)& 2*\left(y_{k+1}^S-y_{k+1}^M\right)& 0\end{array}\right]---\left(6\right)$

Step 5, information filter renewal process

$I_{k/k-1}=Q_{k-1}^{-1}-Q_{k-1}^{-1}{\mathrm{\Φ}}_{k,k-1}{(I_{k-1}+{\mathrm{\Φ}}_{k,k-1}^T{Q}_{k-1}^{-1}{\mathrm{\Φ}}_{k,k-1})}^{-1}{\mathrm{\Φ}}_{k,k+1}^T{Q}_{k-1}^{-1}---\left(7a\right)$

$I_k=I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k---\left(7b\right)$

$K_k=I_k^{-1}{H}_k^T{R}_k^{-1}---\left(7c\right)$

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(7d\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(7e\right)$

Compared with prior art, beneficial effect of the present invention:

The present invention proposes a kind of collaborative navigation method based on information filter, information filter uses information matrix to calculate optimum gain, if known nothing the statistical information of required estimated state initial value, must blindly choose , corresponding P ₀that just should select is very large, may produce spilling during recurrence calculation mean squared error matrix.And adopt information matrix just can avoid occurring this phenomenon.

Accompanying drawing explanation

Fig. 1 is the positioning track figure of reference locus, alone navigation and the collaborative navigation simulating A, B two ships;

With graph of errors comparison diagram during independent navigation when Fig. 2 is B ship each quantity of state collaborative navigation.

Embodiment

Below in conjunction with the drawings and specific embodiments, technical scheme of the present invention is described in more detail.

Based on a collaborative navigation method for information filter, comprise the following steps:

Step one, set up coordinate system

Geographic coordinate system (under be designated as t): OX _ty _tz _t

Geographic coordinate system represents sky, the northeast coordinate system residing for carrier, and its initial point O is selected in carrier center of gravity place, X _tpoint to east, Y _tpoint to north, Z _talong sensing sky, vertical line direction (sky, northeast).

Carrier coordinate system (under be designated as b): OX _by _bz _b

Carrier coordinate system is connected in itself, and its true origin O is positioned at the center of gravity place of carrier, X _bpoint to right along transverse axis, Y _bbefore pointing to along the longitudinal axis, Z _bperpendicular to OX _by _b, and point to along the vertical axes of carrier.

Step 2, set up the motion model of water surface ship formation collaborative navigation.

Note k moment whole system state is i.e. horizontal ordinate x _k, ordinate y _kand course angle before speed v is pointed to along the longitudinal axis in coordinate system, it is relevant with course angle in the projection of Department of Geography, being projected as of east orientation axle being projected as of north orientation axle then follow the ship equation of motion can be expressed as:

$\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}---\left(1\right)$

In formula be respectively speed, course angle that the k moment follows ship, all disturb by white Gaussian noise.Δ t is the sampling period, and ω is course angle change angular speed.The equation of motion is abbreviated as:

x _k+1＝f(x _k，u _k，w _k)＝x _k+Γ(u _k+w _k)(2)

In formula represent and follow the state of ship in the k moment; u _k=(v _kφ _k) ^t; Γ (u _k+ w _k) be nonlinear terms, w _kfor white Gaussian noise, and the variance matrix of noise is:

$Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{\mathrm{\σ}}_{vk}^2& 0\\ 0& {\mathrm{\σ}}_{\mathrm{\φ}k}^2\end{array}\right]---\left(3\right)$

In formula, the element on diagonal line is respectively v _kwith variance.

The foundation of step 3, water surface ship collaborative navigation observation equation.

According to following ship from moment t _kto moment t _k+1the distance vector of motion, by t _kthe geometric position that moment follows ship and pilot boat moves to t _k+1moment, t _kmoment, t _k+1two circles that moment take pilot boat as the center of circle, two ship spacing are radius have intersection point.By geometric relationship, moment t can be obtained _kwith moment t _k+1article two, the expression formula of ship spacing:

$\begin{array}{c}{R}_k^2={\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)}^2+{\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)}^2\\ R_{k+1}^2={\left(x_{k+1}^S-x_{k+1}^M\right)}^2+{\left(y_{k+1}^S-y_{k+1}^M\right)}^2\end{array}---\left(4\right)$

In formula, upper right footmark S and M represents respectively and follows ship and pilot boat, representative follows ship by t _kmoment is to t _k+1moment along the distance of x-axis process, representative follows ship by t _kmoment is to t _k+1moment is along the distance of y-axis process.List measurement equation as follows:

$\begin{array}{c}{Z}_{k+1}=h(x_{k+1}^S,y_{k+1}^S,x_k^M,y_k^M,x_{k+1}^M,y_{k+1}^M,d_{x_{k,k+1}},d_{y_{k,k+1}})+w_z\\ =\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]+w_z\end{array}---\left(5\right)$

In formula, Z _k+1for the observed quantity in k+1 moment, w _zfor observation noise.

The linearization of step 4, water surface ship collaborative navigation system model.

The later state-transition matrix of linearization is:

${\mathrm{\Φ}}_{k+1,k}=\frac{\∂f}{\∂X_k^T}{|}_{X_k={\hat{X}}_k}\≈I+\mathrm{\Δ}t\·\frac{\∂f}{\∂{X_k}^T}=I+\mathrm{\Δ}t\·\left[\begin{array}{ccc}1& 0& -\mathrm{\Δ}t\·V_k{\mathrm{sin\φ}}_k\\ 0& 1& \mathrm{\Δ}t\·V_k{\mathrm{cos\φ}}_k\\ 0& 0& 1\end{array}\right],$

In formula, I is unit battle array.

Linearized system noise excitation matrix is: $G_k=\frac{\∂f}{\∂{u_k}^T}=\left[\begin{array}{cc}\mathrm{\Δ}t\·{\mathrm{cos\φ}}_k& 0\\ \mathrm{\Δ}t\·{\mathrm{sin\φ}}_k& 0\\ 0& \mathrm{\Δ}t\end{array}\right].$

Linearized system measurement matrix can be expressed as:

$H_{k+1}=\frac{\∂Z_{k+1}}{\∂{X_{k+1}}^T}\left[\begin{array}{ccc}2*\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)& 2*\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)& 0\\ 2*\left(x_{k+1}^S-x_{k+1}^M\right)& 2*\left(y_{k+1}^S-y_{k+1}^M\right)& 0\end{array}\right]---\left(6\right)$

Step 5, information filter renewal process

$I_{k/k-1}=Q_{k-1}^{-1}-Q_{k-1}^{-1}{\mathrm{\Φ}}_{k,k-1}{(I_{k-1}+{\mathrm{\Φ}}_{k,k-1}^T{Q}_{k-1}^{-1}{\mathrm{\Φ}}_{k,k-1})}^{-1}{\mathrm{\Φ}}_{k,k+1}^T{Q}_{k-1}^{-1}---\left(7a\right)$

$I_k=I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k---\left(7b\right)$

$K_k=I_k^{-1}{H}_k^T{R}_k^{-1}---\left(7c\right)$

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(7d\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(7e\right)$

In above formula, for the information matrix after state updating, I _kmeasure the information matrix after upgrading, K _kfor filter gain, for the state after state updating, measure the state after upgrading.

In order to further illustrate the beneficial effect of described method, emulate the track of collaborative navigation and graph of errors, simulation result as shown in Figure 1 and Figure 2, and has carried out com-parison and analysis to it.

Com-parison and analysis:

Fig. 1 simulates the positioning result of reference locus, alone navigation and the collaborative navigation of A, B two ships.Know by initially emulating setting, except unmanned boat A can utilize high precision apparatus to correct except cumulative errors when navigating alone, unmanned boat B can only utilize low precision information to carry out reckoning.Simulation result shows, collaborative navigation improves the positioning precision of unmanned boat.

Graph of errors comparison diagram when Fig. 2 gives B ship each quantity of state collaborative navigation and separately during navigation, wherein thinner line represents independent navigation, and thick line represents collaborative navigation.From simulation result, collaborative navigation utilizes information filter can obtain good estimation effect.

The above; be only the present invention's preferably embodiment; protection scope of the present invention is not limited thereto; anyly be familiar with those skilled in the art in the technical scope that the present invention discloses, the simple change of the technical scheme that can obtain apparently or equivalence are replaced and are all fallen within the scope of protection of the present invention.

Claims (1)

1., based on a collaborative navigation method for information filter, it is characterized in that, comprise the following steps:

Step one, set up coordinate system

Geographic coordinate system: OX _ty _tz _t

Geographic coordinate system represents sky, the northeast coordinate system residing for carrier, and its initial point O is selected in carrier center of gravity place, X _tpoint to east, Y _tpoint to north, Z _talong sensing sky, vertical line direction;

Carrier coordinate system: OX _by _bz _b

Carrier coordinate system is connected in itself, and its true origin O is positioned at the center of gravity place of carrier, X _bpoint to right along transverse axis, Y _bbefore pointing to along the longitudinal axis, Z _bperpendicular to OX _by _b, and point to along the vertical axes of carrier;

Step 2, set up the motion model of water surface ship formation collaborative navigation;

Note k moment whole system state is i.e. horizontal ordinate x _k, ordinate y _kand course angle before speed v is pointed to along the longitudinal axis in coordinate system, it is relevant with course angle in the projection of Department of Geography, being projected as of east orientation axle being projected as of north orientation axle then follow the ship equation of motion can be expressed as:

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}\right.---\left(1\right)$

V in formula _k, be respectively speed, course angle that the k moment follows ship, v _k+1, all disturb by white Gaussian noise; Δ t is the sampling period, and ω is course angle change angular speed; The equation of motion is abbreviated as:

x _k+1＝f(x _k，u _k，w _k)＝x _k+Г(u _k+w _k)(2)

In formula represent and follow the state of ship in the k moment; u _k=(v _kφ _k) ^t; Г (u _k+ w _k) be nonlinear terms, w _kfor white Gaussian noise, and the variance matrix of noise is:

In formula, the element on diagonal line is respectively v _kwith variance;

The foundation of step 3, water surface ship collaborative navigation observation equation;

According to following ship from moment t _kto moment t _k+1the distance vector of motion, by t _kthe geometric position that moment follows ship and pilot boat moves to t _k+1moment, t _kmoment, t _k+1two circles that moment take pilot boat as the center of circle, two ship spacing are radius have intersection point; By geometric relationship, moment t can be obtained _kwith moment t _k+1article two, the expression formula of ship spacing:

$R_k^2={(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)}^2+{(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)}^2$ (4)

$R_{k+1}^2={(x_{k+1}^S-x_{k+1}^M)}^2+{(y_{k+1}^S-y_{k+1}^M)}^2$

In formula, upper right footmark S and M represents respectively and follows ship and pilot boat, representative follows ship by t _kmoment is to t _k+1moment along the distance of x-axis process, representative follows ship by t _kmoment is to t _k+1moment is along the distance of y-axis process; List measurement equation as follows:

$\begin{array}{c}{Z}_{k+1}=h(x_{k+1}^S,y_{k+1}^S,x_k^M,y_k^M,x_{k+1}^M,y_{k+1}^M,d_{x_{k,k+1}},d_{y_{k,k+1}})+w_z\\ =\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]+w_z\end{array}---\left(5\right)$

In formula, Z _k+1for the observed quantity in k+1 moment, w _zfor observation noise;

The linearization of step 4, water surface ship collaborative navigation system model;

The later state-transition matrix of linearization is:

${\mathrm{\Φ}}_{k+1,k}=\frac{\∂f}{\∂X_k^T}{|}_{X_k={\hat{X}}_k}\≈I+\mathrm{\Δ}t\·\frac{\∂f}{\∂{X_k}^T}=I+\mathrm{\Δ}t\·\left[\begin{array}{ccc}1& 0& -\mathrm{\Δ}t\·v_k{\mathrm{sin\φ}}_k\\ 0& 1& \mathrm{\Δ}t\·v_k{\mathrm{cos\φ}}_k\\ 0& 0& 1\end{array}\right],$

In formula, I is unit battle array;

Linearized system noise excitation matrix is: $G_k=\frac{\∂f}{\∂{u_k}^T}=\left[\begin{array}{cc}\mathrm{\Δ}t\·{\mathrm{cos\φ}}_k& 0\\ \mathrm{\Δ}t\·{\mathrm{sin\φ}}_k& 0\\ 0& \mathrm{\Δ}t\end{array}\right];$

Linearized system measurement matrix can be expressed as:

$H_{k+1}=\frac{\∂Z_{k+1}}{\∂{X_{k+1}}^T}=\left[\begin{array}{ccc}2*\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)& 2*\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)& 0\\ 2*\left(x_{k+1}^S-x_{k+1}^M\right)& 2*\left(y_{k+1}^S-y_{k+1}^M\right)& 0\end{array}\right]---\left(6\right)$

Step 5, information filter renewal process

$I_k=I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k---\left(7b\right)$

$K_k=I_k^{-1}{H}_k^T{R}_k^{-1}---\left(7c\right)$

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(7d\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(7e\right)$

In above formula, I _k/k-1for the information matrix after state updating, I _kmeasure the information matrix after upgrading, K _kfor filter gain, for the state after state updating, measure the state after upgrading.