CN103017755A - Measuring method for underwater navigation attitudes - Google Patents

Abstract

The invention relates to an information measuring method and in particular relates to a measuring method for underwater combined navigation attitudes of a strap-down inertial navigation system/a doppler log by the doppler log under an underwater working mode when a submarine navigates underwater. The measuring method comprises the following steps of: connecting the strap-down inertial navigation system and the doppler log of the submarine; measuring the attitude value, the speed value and the position value of the submarine in a recursive manner; recording the speed value at a moment that the submarine starts diving; searching an east parameter beta x of an ocean current model and a north parameter beta y of the ocean current model within a navigation sea area; measuring the speed of the ocean current in the recursive manner; real-timely acquiring the water speed obtained by the doppler log; real-timely measuring attitude errors of the strap-down inertial navigation system; and measuring the accurate attitude value of the strap-down inertial navigation system. After compensating the speed of the ocean current, the measuring method provided by the invention improves the course error angle precision, the pitching error angle precision and rolling error angle precision in different degrees, and improves the combined navigation precision.

Description

A kind of underwater navigation attitude measurement method

Technical field

What the present invention relates to is a kind of information measuring method, and during specifically a kind of latent device underwater navigation, Doppler log is under the water conservancy project operation mode, strapdown inertial navigation system Doppler log integrated navigation attitude measurement method under water.

Background technology

Strapdown inertial navitation system (SINS) (SINS) has advantages of independent work, but its navigation error accumulates in time; GPS can provide accurate positional information, but its signal can decay in water, can't be used by the latent device of underwater operation; Doppler log (DVL) can provide the velocity information of the device of diving, but its operating distance is limited, when latent device and seabed distance during less than its operating distance, DVL works in ground mode, ground velocity is provided, when latent device and seabed distance during greater than its operating distance, DVL works in aqueous mode, and speed through water is provided.Be subject to the DVL self performance, the body of diving is long-pending, the restriction of the aspects such as power consumption, and in most cases, DVL can only provide speed through water.Above-mentioned single navigational system is because the deficiency that self exists can't satisfy the requirement of the device navigation of diving.Therefore, usually above-mentioned three kinds of system in combination are got up to use, just can learn from other's strong points to offset one's weaknesses, improve navigation accuracy.

When diving the device underwater operation, can't use gps signal, DVL can only provide speed through water simultaneously.Application number 200810137469.4, name is called " the AUV integrated navigation system is without the mark changing method "; Be published in the article " based on the hardware-in-the-loop simulation research of SINS/GPS/DVL integrated navigation location " on " computer measurement and control " in 2011, above-mentioned achievement and existing patent and article are all in the latent device underwater navigation stage, with the observed quantity of speed through water as the device junction filter of diving, go to estimate the error of SINS, can since the existence of ocean current speed so that filtering divergence, estimate unsuccessfully (wave filter needs ground velocity as observed quantity, deducts ocean current speed and speed through water equals ground velocity).This paper has designed auxiliary integrated navigation scheme, utilizes the ocean current model recursion Flow Velocity of going abroad, and obtains the ground velocity value in conjunction with the DVL speed through water, and input filter is finished filtering and estimated task.

Summary of the invention

The object of the present invention is to provide a kind of Doppler log of measuring accuracy that improves to strapdown inertial navitation system (SINS) under the water conservancy project operation mode/Doppler log integrated navigation attitude measurement method.

The object of the present invention is achieved like this:

The present invention in conjunction with the ocean current rate pattern, works in the integrated navigation state by strapdown inertial navigation system, the Doppler log of the device equipment of diving, and measures in real time the attitude information of the device of diving, and comprises the steps:

(1) connects strapdown inertial navigation system and the Doppler log that the device of diving is equipped with data cable;

(2) sampled value of gyro and accelerometer in the latent device strapdown inertial navigation system of collection, recursion is measured attitude value, velocity amplitude and the positional value of the device of diving;

(3) the latent device of record begins the velocity amplitude that dive moment strapdown inertial navigation system is measured, as the initial value of ocean current recursion measuring speed, i.e. V _CE(0), V _CN(0), V wherein _CE(0) is the east orientation initial value of ocean current recursion measuring speed, V _CN(0) is the north orientation initial value of ocean current recursion measuring speed;

(4) in electronic chart or ocean current database, search in the device navigation marine site of diving ocean current model east orientation parameter beta _xWith ocean current model north orientation parameter beta _y

(5) in latent device dive and underwater navigation stage, recursion is measured the speed of ocean current, and the increment of ocean current in Kalman filtering update cycle T is

V _CE(k+1)＝V _CE(k)-β _xV _CE(k)T ，

V _CN(k+1)＝V _CN(k)-β _yV _CN(k)T

V wherein _CE(k) the east orientation speed of measuring for the ocean current recursion, V _CN(k) the north orientation speed of measuring for the ocean current recursion, k=0,1,2 ..., k is the sampling time;

(6) Real-time Collection Doppler log speed through water value: V _DE(k) the east orientation speed of measuring for the Doppler log recursion, V _DN(k) the north orientation speed of measuring for the Doppler log recursion, the speed of the ocean current that integrating step (5) measures consists of the observed quantity of Kalman filter:

$\left[\begin{array}{c}{V}_{\mathrm{CE}}\left(k\right)+V_{\mathrm{DE}}\left(k\right)\\ V_{\mathrm{CN}}\left(k\right)+V_{\mathrm{DN}}\left(k\right)\end{array}\right];$

(7) utilize the observed quantity of Kalman filter, structure strapdown inertial navigation system/Doppler log integrated navigation Kalman filter, real-time estimation goes out the strapdown inertial navigation system attitude error;

(8) utilize strapdown inertial navigation system measurement attitude value in the strapdown inertial navigation system attitude error correction step (2) of estimating, the device strapdown inertial navigation system attitude of being dived accurately value.

2. a kind of underwater navigation attitude measurement method according to claim 1, it is characterized in that: the attitude value of described latent device comprises course angle ψ, pitch angle θ, roll angle γ; The velocity amplitude of described latent device is v=[v _Ev _Nv _U] ^Tv _EBe the east orientation speed of the device of diving, v _NBe the north orientation speed of the device of diving, v _UFor the sky of the device of diving to speed; The positional value of described latent device comprises latitude L, longitude λ.

3. a kind of underwater navigation attitude measurement method according to claim 1 and 2 is characterized in that: describedly estimate the strapdown inertial navigation system attitude error that and comprise course angle error delta ψ, pitch angle error delta θ, roll angle error delta γ.

4. a kind of underwater navigation attitude measurement method according to claim 1 and 2 is characterized in that: the described device strapdown inertial navigation system attitude value of diving accurately comprises accurately course angle ψ-δ ψ; Pitch angle θ-δ θ accurately; Roll angle γ-δ γ accurately.

Beneficial effect of the present invention is:

The present invention has designed a kind of SINS/DVL, and Integrated navigation mode is for the method for attitude measurement under water, and in the Integrated navigation mode, the ocean current model provides ocean current speed under water, can provide comparatively accurately the ground velocity value to assist SINS in conjunction with the DVL speed through water.Simulated conditions is set to before and after the compensation ocean current: random drift is 0.001 °/h, gyroscope constant value drift ε _x=ε _y=ε _z=0.01 °/h, scale coefficient error is 10 ^-4The random normal value of accelerometer is biased to

Random drift is 10 ^-5G.Boats and ships work in the state of at the uniform velocity sailing through to.

Show by the accompanying drawing simulation result: adopt designed assembled scheme, can overcome DVL can't provide the impact of ground velocity information for filtering performance, and wherein, east orientation misalignment steady-state error precision has improved 0.007 ⁰, north orientation misalignment steady-state error precision slightly is improved, but not obvious, and the sky has improved 0.06 to misalignment steady-state error precision ⁰, before the ocean current velocity compensation of comparing, three misalignment steady-state errors all have improvement in various degree, have improved the integrated navigation alignment precision, have reached the purpose of this programme design.

Description of drawings

Fig. 1 is to the process flow diagram of integrated navigation attitude measurement method under water of SINS/DVL under the water conservancy project operation mode based on DVL;

Fig. 2, adopt after method of the present invention (compensation ocean current) and the conventional method (not compensating ocean current) in two kinds of situations of misalignment course, pitching and roll error angle result.

Specific embodiments

A kind of DVL is to the SINS/DVL integrated navigation attitude measurement method under the water conservancy project operation mode, it is characterized in that utilizing the strapdown inertial navigation system SINS of latent device equipment, Doppler log DVL is in conjunction with the ocean current rate pattern, work in the integrated navigation state, measure in real time the attitude information of the device of diving.The implementation step is as follows:

SINS and the DVL of step 1, the device equipment of diving link by data cable, guarantee that their communication is unobstructed;

The sampled value of gyro and accelerometer among step 2, the latent device SINS of collection, recursion is measured attitude value, velocity amplitude and the positional value of underwater research vehicle.

Described latent device SINS measures the attitude value and comprises: course angle ψ, pitch angle θ, roll angle γ,

Described latent device SINS measured velocity value is v=[v _Ev _Nv _U] ^T,

Described latent device SINS measuring position value is latitude L, longitude λ;

Step 3, the device of diving begin dive constantly, and record SINS measured velocity value is as the initial value of ocean current recursion measuring speed, i.e. V _CE(0), V _CN(0), V wherein _CE(0) is the east orientation initial value of ocean current recursion measuring speed, V _CN(0) is the north orientation initial value of ocean current recursion measuring speed;

Step 4, by in electronic chart or the ocean current database, search in the device navigation marine site of diving ocean current model east orientation parameter beta _xNorth orientation parameter beta with the ocean current model _y

Step 5, the device of diving are in dive and underwater navigation stage, and recursion is measured the speed of ocean current, and wherein, the increment of ocean current in Kalman filtering update cycle T is

V _CE(k+1)＝V _CE(k)-β _xV _CE(k)T

V _CN(k+1)＝V _CN(k)-β _yV _CN(k)T

V wherein _CE(k) the east orientation speed of measuring for the ocean current recursion, V _CN(k) the north orientation speed of measuring for the ocean current recursion, k=0,1,2,

Step 6, Real-time Collection DVL speed through water value: V _DE(k) the east orientation speed of measuring for the DVL recursion, V _DN(k) the north orientation speed of measuring for the DVL recursion, the ocean current value that integrating step 5 measures consists of the observed quantity of Kalman filter

$\left[\begin{array}{c}{V}_{\mathrm{CE}}\left(k\right)+V_{\mathrm{DE}}\left(k\right)\\ V_{\mathrm{CN}}\left(k\right)+V_{\mathrm{DN}}\left(k\right)\end{array}\right]$

Step 7, utilize the observed quantity in the step 6, structure SINS/DVL integrated navigation Kalman filter, the Kalman filter real-time estimation goes out the attitude error of SINS, comprises course angle error delta ψ, pitch angle error delta θ, roll angle error delta γ;

Step 8, utilize step 7 to estimate that SINS measures the attitude value in the attitude error correction step 2 of SINS, the device SINS attitude of being dived accurately value,

Course angle ψ-δ ψ accurately,

Pitch angle θ-δ θ accurately,

Roll angle γ-δ γ accurately.

This patent also comprises following technical characterictic: in the step 7, system state equation and measurement equation that Kalman filter is used are as follows:

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}\\ Z=\mathrm{HX}+O\end{array}\right.$

Wherein, X represents 7 dimension status switches, and F is the Matrix of shifting of a step of 7 * 7 dimensions, and G is 7 * 7 dimension unit matrix, and H is that 2 * 7 dimensions measure battle array, and W is system noise, is the white noise sequence of 7 * 1 dimensions, and O is measurement noise, is 2 * 1 dimension white noise sequences.

Choose state variable

X＝[δL δλ δv _E δv _N δψ δθ δγ] ^T

In the above-mentioned state variable, each element is respectively latitude error, trueness error, east orientation velocity error, north orientation velocity error, course angle error, pitch angle error, the roll angle error of SINS.

One step transfer matrix F Partial Elements is as follows:

F(1,1)＝1, $F\left(\mathrm{1,3}\right)=\frac{T}{R};$

$F\left(\mathrm{2,1}\right)=\frac{v_E}{R}T\sec L\tan L,$ F(2,2)＝1, $F\left(\mathrm{2,3}\right)=\frac{\sec L}{R}T;$

$F\left(\mathrm{3,1}\right)=2w_{\mathrm{ie}}T\cos L{v}_N+\frac{v_E{v}_N}{R}T{\sec }^{2}L$ $F\left(\mathrm{3,3}\right)=1+\frac{v_N}{R}T\tan L,$ $F\left(\mathrm{3,4}\right)={2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L,$

F(3,6)＝-gT,F(3,7)＝f _NT;

$F\left(\mathrm{4,1}\right)=-({2w}_{\mathrm{ie}}T\cos {\mathrm{lv}}_E+\frac{{v_E}^2}{R}T{\sec }^{2}L),$ $F(4,3)=-({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L),$ F(4，4)＝1,

F(4,5)＝gT,F(4,7)＝-f _ET;

$F\left(\mathrm{5,4}\right)=-\frac{T}{R},$ F(5,5)＝1, $F\left(\mathrm{5,6}\right)=w_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L,$ $F\left(\mathrm{5,7}\right)=-(w_{\mathrm{ie}}T\cos L+\frac{v_{E}T}{R});$

F(6,1)＝-w _ieTsinL, $F\left(\mathrm{6,3}\right)=\frac{T}{R},$ $F\left(\mathrm{6,5}\right)=-(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L),$

F(6,6)＝1, $F\left(\mathrm{6,7}\right)=-\frac{v_N}{R}T;$

$F\left(\mathrm{7,1}\right)=w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T{\sec }^{2}L,$ $F\left(\mathrm{7,3}\right)=\frac{T}{R}\tan L,$ $F\left(\mathrm{7,5}\right)=w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T,$ $F\left(\mathrm{7,6}\right)=\frac{v_N}{R}T,$

F(7,7)＝1

One step transfer matrix F is except the element of above-mentioned definition, and all the other elements all are zero.

Choose observed quantity $Z_k=\left[\begin{array}{c}{V}_{\mathrm{CE}}\left(k\right)+V_{\mathrm{DE}}\left(k\right)\\ V_{\mathrm{CN}}\left(k\right)+V_{\mathrm{DV}}\left(k\right)\end{array}\right]$

Then measuring battle array is

$H=\left[\begin{array}{ccccccc}0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]$

Wherein,

The accelerometer bias of east orientation and north orientation, ε _E, ε _N, ε _UBe respectively east orientation, north orientation and day to gyroscopic drift.Earth rotation angular speed w _Ie=7.2921158 * 10 ^-5Radian, earth radius R=6378393 rice.

Utilize under the dynamic condition SINS/DVL for DVL under the aqueous mode, the test findings that attitude information is measured.

The error of test condition SINS is set to: gyroscope constant value drift ε _x=ε _y=ε _z=0.01 °/h, random drift is 0.001 °/h, and scale coefficient error is 10 ^-4The random normal value of accelerometer is biased to

Random drift is 10 ^-5G.Latent device works in the state of at the uniform velocity sailing through to, speed of a ship or plane 10kn, 135 ° in course.Ocean current speed V _Cx(0)=and 1kn, V _Cy(0)=and 2kn, the parameter beta of ocean current model _x=5.1296 * 10 ^-5β _x=4.4569 * 10 ^-5

We can see, after the compensation ocean current speed, course, pitching and roll error angular accuracy have raising in various degree, the ocean current speed of having verified this programme recursion can well compensate DVL to water to speed, improved the integrated navigation precision.

Claims (5)

1. underwater navigation attitude measurement method, strapdown inertial navigation system, Doppler log by the device equipment of diving in conjunction with the ocean current rate pattern, work in the integrated navigation state, measure in real time the attitude information of the device of diving, and it is characterized in that, comprise the steps:

(1) connects strapdown inertial navigation system and the Doppler log that the device of diving is equipped with data cable;

(2) sampled value of gyro and accelerometer in the latent device strapdown inertial navigation system of collection, recursion is measured attitude value, velocity amplitude and the positional value of the device of diving;

(3) the latent device of record begins the velocity amplitude that dive moment strapdown inertial navigation system is measured, as the initial value of ocean current recursion measuring speed, i.e. V _CE(0), V _CN(0), V wherein _CE(0) is the east orientation initial value of ocean current recursion measuring speed, V _CN(0) is the north orientation initial value of ocean current recursion measuring speed;

(4) in electronic chart or ocean current database, search in the device navigation marine site of diving ocean current model east orientation parameter beta _xWith ocean current model north orientation parameter beta _y

(5) in latent device dive and underwater navigation stage, recursion is measured the speed of ocean current, and the increment of ocean current in Kalman filtering update cycle T is

V _CE(k+1)＝V _CE(k)-β _xV _CE(k)T

V _CE(k+1)＝V _CN(k)-β _yV _CN(k)T，

V wherein _CE(k) the east orientation speed of measuring for the ocean current recursion, V _CN(k) the north orientation speed of measuring for the ocean current recursion, k=0,1,2 ..., k is the sampling time;

(6) Real-time Collection Doppler log speed through water value: V _DE(k) the east orientation speed of measuring for the Doppler log recursion, V _DN(k) the north orientation speed of measuring for the DVL recursion, the ocean current value that integrating step (5) measures consists of the observed quantity of Kalman filter:

$\left[\begin{array}{c}{V}_{\mathrm{CE}}\left(k\right)+V_{\mathrm{DE}}\left(k\right)\\ V_{\mathrm{CN}}\left(k\right)+V_{\mathrm{DN}}\left(k\right)\end{array}\right]$

(7) utilize the observed quantity of Kalman filter, structure strapdown inertial navigation system/Doppler log integrated navigation Kalman filter, real-time estimation goes out the strapdown inertial navigation system attitude error;

(8) utilize strapdown inertial navigation system measurement attitude value in the strapdown inertial navigation system attitude error correction step (2) of estimating, the device strapdown inertial navigation system attitude of being dived accurately value.

2. a kind of underwater navigation attitude measurement method according to claim 1, it is characterized in that: the attitude value of described latent device comprises course angle ψ, pitch angle θ, roll angle γ; The velocity amplitude of described latent device is v=[v _Ev _Nv _U] ^TV wherein _EBe the east orientation speed of the device of diving, v _NBe the north orientation speed of the device of diving, v _UFor the sky of the device of diving to speed; The positional value of described latent device comprises latitude L, longitude λ.

3. a kind of underwater navigation attitude measurement method according to claim 1 and 2 is characterized in that: describedly estimate the strapdown inertial navigation system attitude error that and comprise course angle error delta ψ, pitch angle error delta θ, roll angle error delta γ.

4. a kind of underwater navigation attitude measurement method according to claim 1 and 2 is characterized in that: the described device strapdown inertial navigation system attitude value of diving accurately comprises accurately course angle ψ-δ ψ; Pitch angle θ-δ θ accurately; Roll angle Υ-δ γ accurately.

5. a kind of underwater navigation attitude measurement method according to claim 3 is characterized in that: the described device strapdown inertial navigation system attitude value of diving accurately comprises accurately course angle ψ-δ ψ; Pitch angle θ-δ θ accurately; Roll angle Υ-δ γ accurately.

Images