CN103017755B - A kind of underwater navigation attitude measurement method - Google Patents

Abstract

What the present invention relates to is a kind of information measuring method, specifically a kind of dive device underwater navigation time, Doppler log under water conservancy project operation mode, strapdown inertial navigation system Doppler log integrated navigation attitude measurement method under water.The present invention includes following steps: the strapdown inertial navigation system and the Doppler log that connect device equipment of diving; Recursion measures the dive attitude value of device, velocity amplitude and positional value; The latent device of record starts dive moment velocity amplitude; Search in latent device navigation marine site, ocean current model east orientation parameter beta _xwith ocean current model north orientation parameter beta _y; Recursion measures the speed of ocean current; Real-time Collection Doppler log speed through water value; Real-time estimation goes out strapdown inertial navigation system attitude error; Measure device strapdown inertial navigation system attitude value of diving accurately.The present invention is after compensation ocean current speed, and course, pitching and roll error angular accuracy have raising in various degree, improve integrated 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, specifically a kind of dive device underwater navigation time, Doppler log under 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 the advantage of independent work, but its navigation error accumulates in time; GPS can provide accurate positional information, but its signal can be decayed in water, cannot be used by the latent device of underwater operation; Doppler log (DVL) can provide the velocity information of latent device, but its operating distance is limited, when latent device and seabed distance are less than its operating distance, DVL works in ground mode, ground velocity is provided, when latent device and seabed distance are greater than its operating distance, DVL works in aqueous mode, provides speed through water.Be limited to DVL self performance, latent body amasss, the restriction of the aspects such as power consumption, and in most cases, DVL can only provide speed through water.The deficiency that above-mentioned single navigational system exists due to self, cannot meet the requirement of latent device navigation.Therefore, combine above-mentioned three kinds of systems use usually, just can learn from other's strong points to offset one's weaknesses, and improves navigation accuracy.

During latent device underwater operation, cannot use gps signal, DVL can only provide speed through water simultaneously.Application number 200810137469.4, name is called " AUV integrated navigation system is without mark changing method "; Within 2011, be published in the article " based on the research of SINS/GPS/DVL integrated navigation and location hardware-in-the-loop simulation " on " computer measurement and control ", above-mentioned achievement and existing patent and article are all in the latent device underwater navigation stage, observed quantity using speed through water as latent device junction filter, go the error estimating SINS, filtering divergence can be made due to the existence of ocean current speed, estimate unsuccessfully (wave filter needs ground velocity as observed quantity, and speed through water equals ground velocity deduct ocean current speed).Devise auxiliary integrated navigation scheme herein, utilize ocean current model recursion to go abroad Flow Velocity, obtain ground velocity value in conjunction with DVL speed through water, input filter, complete filtering and estimate task.

Summary of the invention

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

The object of the present invention is achieved like this:

Strapdown inertial navigation system, Doppler log that the present invention is equipped by device of diving, in conjunction with ocean current rate pattern, work in integrated navigation state, measures the attitude information of device of diving in real time, comprise the steps:

(1) strapdown inertial navigation system and the Doppler log of device equipment of diving is connected with data cable;

(2) gather the sampled value of dive gyro and accelerometer in device strapdown inertial navigation system, recursion measures the attitude value of device of diving, velocity amplitude and positional value;

(3) the latent device of record starts 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), wherein V _cE(0) be the east orientation initial value of ocean current recursion measuring speed, V _cN(0) be the north orientation initial value of ocean current recursion measuring speed;

(4) in electronic chart or ocean current database, search in latent device navigation marine site, 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 measures 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，

Wherein V _cEk east orientation speed that () measures for ocean current recursion, V _cNk north orientation speed that () measures for ocean current recursion, k=0,1,2 ..., k is the sampling time;

(6) Real-time Collection Doppler log speed through water value: V _dEk east orientation speed that () measures for Doppler log recursion, V _dNk north orientation speed that () measures for Doppler log recursion, integrating step (5) measures the speed of the ocean current obtained, and forms the observed quantity of Kalman filter:

$\left[\begin{array}{c}{V}_{CE}\left(k\right)+V_{DE}\left(k\right)\\ V_{CN}\left(k\right)+V_{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 strapdown inertial navigation system attitude error;

(8) strapdown inertial navigation system in strapdown inertial navigation system attitude error correction step (2) estimating is utilized to measure attitude value, device strapdown inertial navigation system attitude value of being dived accurately.

2. a kind of underwater navigation attitude measurement method according to claim 1, 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 _efor the east orientation speed of latent device, v _nfor the north orientation speed of latent device, v _ufor the sky of latent device is 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 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: described device strapdown inertial navigation system attitude value of diving accurately comprises course angle ψ-δ ψ accurately; Pitch angle θ-δ θ accurately; Roll angle γ-δ γ accurately.

Beneficial effect of the present invention is:

The present invention devises a kind of SINS/DVL, and Integrated navigation mode is for the method for attitude measurement under water, and under water in Integrated navigation mode, ocean current model provides ocean current speed, ground velocity value comparatively accurately can be provided to carry out auxiliary SINS in conjunction with DVL speed through water.Before and after compensating ocean current, simulated conditions is set to: random drift is 0.001 °/h, gyroscope constant value drift ε _x=ε _y=ε _z=0.01 °/h, scale coefficient error is 10 ^-4; The random constant value of accelerometer is biased to ▽ _x=▽ _y=10 ^-4g, random drift is 10 ^-5g.Vessel operation is at the uniform velocity sailing through to state.

Shown by accompanying drawing simulation result: the assembled scheme designed by employing, can overcome DVL cannot provide ground velocity information for the impact of filtering performance, wherein, east orientation misalignment steady-state error precision improves 0.007 °, north orientation misalignment steady-state error precision slightly improves, but it is not obvious, it improves 0.06 ° to misalignment steady-state error precision, before ocean current velocity compensation of comparing, three misalignment steady-state errors all have improvement in various degree, improve integrated navigation alignment precision, reach the object of this programme design.

Accompanying drawing explanation

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

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

Specific embodiments

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

SINS and DVL of step 1, device equipment of diving is connected by data cable, guarantees that their information transmission is unobstructed;

Step 2, gather the sampled value of dive gyro and accelerometer in device SINS, recursion measures the attitude value of underwater research vehicle, velocity amplitude and positional value.

Described latent device SINS measures 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, device of diving start the dive moment, and record SINS measured velocity value, as the initial value of ocean current recursion measuring speed, i.e. V _cE(0), V _cN(0), wherein V _cE(0) be the east orientation initial value of ocean current recursion measuring speed, V _cN(0) be the north orientation initial value of ocean current recursion measuring speed;

Step 4, by electronic chart or ocean current database, search latent device navigation marine site in, ocean current model east orientation parameter beta _xwith the north orientation parameter beta of ocean current model _y;

Step 5, device of diving are in dive and underwater navigation stage, and recursion measures 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

Wherein V _cEk east orientation speed that () measures for ocean current recursion, V _cNk north orientation speed that () measures for ocean current recursion, k=0,1,2,

Step 6, Real-time Collection DVL speed through water value: V _dEk east orientation speed that () measures for DVL recursion, V _dNk north orientation speed that () measures for DVL recursion, integrating step 5 measures the ocean current value obtained, and forms the observed quantity of Kalman filter

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

Step 7, the observed quantity utilized in step 6, structure SINS/DVL integrated navigation Kalman filter, 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, step 7 is utilized to estimate SINS in the attitude error correction step 2 of SINS to measure attitude value, device SINS attitude value of being dived accurately,

Course angle ψ-δ ψ accurately,

Pitch angle θ-δ θ accurately,

Roll angle γ-δ γ accurately.

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

$\left\{\begin{array}{c}\stackrel{\·}{X}=FX+GW\\ Z=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, and be the white noise sequence of 7 × 1 dimensions, O is measurement noise, is 2 × 1 dimension white noise sequences.

Choose state variable

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

In 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(1,3)=\frac{T}{R};$

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

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

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

$F(4,1)=-(2w_{ie}T\cos {\mathrm{Lv}}_E+\frac{{v_E}^2}{R}T{\sec }^{2}L),F(4,3)=-(2w_{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(5,4)=-\frac{T}{R},F(5,5)=1,F(5,6)=w_{ie}\sin L+\frac{v_E}{R}\tan L,F(5,7)=-(w_{ie}T\cos L+\frac{v_{E}T}{R});$

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

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

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

F(7,7)＝1

One step transfer matrix F is except element defined above, and all the other elements are all zero.

Choose observed quantity $Z_k=\left[\begin{array}{c}{V}_{CE}\left(k\right)+V_{DE}\left(k\right)\\ V_{CN}\left(k\right)+V_{DN}\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, ▽ _e, ▽ _nthe accelerometer bias of east orientation and north orientation, ε _e, ε _n, ε _ueast orientation, north orientation and sky respectively to gyroscopic drift.Earth rotation angular speed w _ie=7.2921158 × 10 ^-5radian, earth radius R=6378393 rice.

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

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 ^-4; The random constant value of accelerometer is biased to ▽ _x=▽ _y=10 ^-4g, random drift is 10 ^-5g.Latent device works at the uniform velocity sails through to state, speed of a ship or plane 10kn, 135 °, course.Ocean current speed V _cx(0)=1kn, V _cy(0)=2kn, the parameter beta of ocean current model _x=5.1296 × 10 ^-5β _y=4.4569 × 10 ^-5;

We can see, after compensating ocean current speed, course, pitching and roll error angular accuracy have raising in various degree, the ocean current speed demonstrating this programme recursion can well compensate DVL to water relative velocity, improve integrated navigation precision.

Claims (1)

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

(1) strapdown inertial navigation system and the Doppler log of device equipment of diving is connected with data cable;

(2) gather the sampled value of dive gyro and accelerometer in device strapdown inertial navigation system, recursion measures the attitude value of device of diving, velocity amplitude and positional value;

(3) the latent device of record starts 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), wherein V _cE(0) be the east orientation initial value of ocean current recursion measuring speed, V _cN(0) be the north orientation initial value of ocean current recursion measuring speed;

(4) in electronic chart or ocean current database, search in latent device navigation marine site, 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 measures 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，

Wherein V _cEk east orientation speed that () measures for ocean current recursion, V _cNk north orientation speed that () measures for ocean current recursion, k=0,1,2 ..., k is the sampling time;

(6) Real-time Collection Doppler log speed through water value: V _dEk east orientation speed that () measures for Doppler log recursion, V _dNk north orientation speed that () measures for DVL recursion, integrating step (5) measures the ocean current value obtained, and forms the observed quantity of Kalman filter:

$\left[\begin{array}{c}{V}_{CE}\left(k\right)+V_{DE}\left(k\right)\\ V_{CN}\left(k\right)+V_{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 strapdown inertial navigation system attitude error;

(8) strapdown inertial navigation system in strapdown inertial navigation system attitude error correction step (2) estimating is utilized to measure attitude value, device strapdown inertial navigation system attitude value of being dived accurately; ; 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] ^twherein v _efor the east orientation speed of latent device, v _nfor the north orientation speed of latent device, v _ufor the sky of latent device is to speed; The positional value of described latent device comprises latitude L, longitude λ; Describedly estimate the strapdown inertial navigation system attitude error and comprise course angle error delta ψ, pitch angle error delta θ, roll angle error; Described device strapdown inertial navigation system attitude value of diving accurately comprises course angle ψ-δ ψ accurately; Pitch angle θ-δ θ accurately; Roll angle γ-δ γ accurately; In described step (7), the system state equation that Kalman filter is used and measurement equation are:

$\left\{\begin{array}{c}\stackrel{\·}{X}=FX+GW\\ Z=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, and be the white noise sequence of 7 × 1 dimensions, O is measurement noise, is 2 × 1 dimension white noise sequences;

Choose state variable

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

In 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(1,3)=\frac{T}{R};$

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

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

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

$F(4,1)=-(2w_{ie}T\cos {\mathrm{Lv}}_E+\frac{{v_E}^2}{R}T{\sec }^{2}L),F(4,3)=-(2w_{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(5,4)=-\frac{T}{R},F(5,5)=1,F(5,6)=w_{ie}\sin L+\frac{v_E}{R}\tan L,F(5,7)=-(w_{ie}T\cos L+\frac{v_{E}T}{R});$

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

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

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

F(7,7)＝1

One step transfer matrix F is except element defined above, and all the other elements are all zero;

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

Then measuring battle array is

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

Wherein, ▽ _e, ▽ _nthe accelerometer bias of east orientation and north orientation, ε _e, ε _n, ε _ueast orientation, north orientation and sky respectively to gyroscopic drift; Earth rotation angular speed w _ie=7.2921158 × 10 ^-5radian, earth radius R=6378393 rice;

Gyroscope constant value drift ε _x=ε _y=ε _z=0.01 °/h, random drift is 0.001 °/h, and scale coefficient error is 10 ^-4; The random constant value of accelerometer is biased to ▽ _x=▽ _y=10 ^-4g, random drift is 10 ^-5g; Latent device works at the uniform velocity sails through to state, speed of a ship or plane 10kn, 135 °, course; Ocean current speed V _cx(0)=1kn, V _cy(0)=2kn, the parameter beta of ocean current model _x=5.1296 × 10 ^-5, β _y=4.4569 × 10 ^-5.