CN102323586B - UUV (unmanned underwater vehicle) aided navigation method based on current profile - Google Patents

Abstract

The invention provides a UUV (unmanned underwater vehicle) aided navigation method based on current profile. The method comprises the following steps: firstly setting up a Kalman filter model of current profile data, secondly determining the observation noise variance R and process noise variance Q in a Kalman equation according to variations of the environment and the UUV own velocity and finally setting up a UUV velocity-current profile relation database, utilizing the relation database and the current profile information measured by an ADCP (acoustic Doppler current profile) to reckon the UUV velocity at the time of DVL (Doppler velocity log) failure and further obtaining the navigation position of the UUV by deadreckoning. The method has the following beneficial effects: when the UUV performs a task under water, once the sonar data of the DVL have failure, the UUV velocity can be reckoned by using the current profile information measured by the ADCP, so that the UUV can continue performing the ocean exploration task in the complex ocean environments such as great depth, thus improving the environmental adaptation capability of the UUV.

Description

A kind of UUV auxiliary navigation method based on ocean current profile

Technical field

What the present invention relates to is a kind of UUV air navigation aid, relates in particular to a kind of based on ocean current profile auxiliary navigation method under the DVL sonar data failure conditions of UUV lift-launch.

Background technology

Along with the application of unmanned vehicles UUV (Unmanned Underwater Vehicle) in ocean development under water is more and more extensive, also more and more higher to the accuracy requirement of UUV navigational system.The tradition dead reckoning is one of air navigation aid at present the most frequently used among the UUV, and this method depends on the speed data of Doppler anemometer (DVL) collection and the attitude data that attitude sensor gathers.But, in the situations such as maximum magnitude that UUV is turning, seawater is followed the tracks of at the bottom of more deeply surpassing DVL, may lose efficacy, DVL was in case lost efficacy or because hydraulic performance decline causes the real-time speed of UUV to obtain, can only adopt the lost efficacy velocity information of former bats of DVL, and the speed of UUV is real-time change under complicated marine environment, thereby the navigation on UUV produces larger impact, and the UUV positional information deviation of reckoning can be increasing.Therefore when DVL lost efficacy, seek a kind of can effectively avoiding and lost efficacy or fault causes the problem of the larger navigation error of dead reckoning generation because of speed pickup, just seem particularly urgent.

At present, attempt to seek a kind of means that effectively under UUV tachogenerator failure conditions, can assist the UUV navigation both at home and abroad.Such as army not at document " the underwater navigation information fusion method based on Ocean Current Database is inquired into " (Journal of System Simulation, 2002, underwater navigation information fusion method based on Ocean Current Database has been proposed the 10th phase of the 14th volume), the method needs the Ocean current information of a large amount of working sea area, workload is large, is difficult to promote.2009, the people such as Wu Taiqi of Chinese Navy engineering university are in document " structure of navigation simulation and analysis platform under the gravity field auxiliary water " (naval vessel science and technology, 2009, the 2nd phase of the 31st volume) proposed to utilize gravity field to carry out the method for unmanned vehicles assisting navigation under water in, the method need to increase the software and hardwares such as gravity sensor, gravity datum diagram data, has increased the complicacy of UUV system.2007, Norway

In document " Comparison of Mathematical Models for the HUGIN 4500 AUV Based on Experimental Data " (International Symposium on Underwater Technology), proposed to utilize the UUV kinetic model to carry out the method for assisting navigation Deng the people, and the parameter that the UUV kinetic model relates to is more, be difficult to accurately determine the kinetic model of UUV, thereby can't guarantee the precision of navigating.

Summary of the invention

The object of the present invention is to provide a kind of DVL (Doppler anemometer that carries as UUV, the ocean current profile data that can utilize ADCP (acoustic Doppler current profiler, Acoustic Doppler Current Profile) to record when Doppler Velocity Log) losing efficacy assist self navigate in the UUV of ocean current profile auxiliary navigation method.

The object of the present invention is achieved like this: comprise the following steps:

(1) ocean current profile data and DVL speed are obtained

The control computer control ADCP that utilizes UUV to carry launches the sound wave of certain frequency, utilizes Doppler effect to obtain the ocean current profile that the UUV water layer thickness is close in the bottom;

Utilize the sound wave of the control computer control DVL emission certain frequency that UUV carries, utilize Doppler effect to obtain the speed of DVL, the i.e. headway of UUV;

(2) the Kalman filtering of ocean current profile

According to the characteristics of ocean current environment and UUV self model, set up ocean current profile data Kalman filtering data model; And determine observation noise variance R in the ocean current Kalman filtering data model with reference to the order of magnitude of ocean current high frequency residual error, the size of the UUV velocity information Dynamic Regulating Process noise variance Q value that records according to DVL simultaneously;

(3) " UUV speed-Ocean current information " concerns storehouse foundation

When DVL navigation sonar data are effective, utilize the course information of UUV speed that DVL records, UUV that the OCTANS attitude sensor measures, the ocean current profile data that ADCP measures are set up " UUV speed-Ocean current information " and are concerned the storehouse;

(4) ocean current profile assisting navigation

When DVL navigation sonar data failure, utilize " UUV speed-Ocean current information " concern the storehouse, and the ocean current profile information that gets of ADCP sonar Real-time Measuring, the navigation speed of reckoning UUV; Again according to the longitude and latitude of initial UUV and the positional information that the dead reckoning algorithm obtains calculating UUV.

The acquisition methods of the positional information of described UUV is:

The UUV navigation speed of calculating the component of due east direction and direct north as shown in the formula:

$\left\{\begin{array}{c}{v}_E=v_F\sin H-v_L\cos H\\ v_N=v_F\cos H+v_L\sin H\end{array}\right.$

V in the formula _E, v _N---be respectively the carrier headway at the component of due east direction and direct north;

v _F, v _L---be respectively carrier forward speed and the left-hand speed relative to the earth that DVL records;

H---the course angle of UUV carrier for just, for negative, is recorded by compass clockwise counterclockwise;

The UUV position is calculated by following formula:

$\left\{\begin{array}{c}J={\mathrm{<figure-callout\; id="0"\; label="J"\; filenames="HDA0000075726600000012.png,HDA0000075726600000021.png"\; state="\{\{state\}\}">J</figure-callout>}}_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{E,(i-1)}\mathrm{\&Delta;t}/R_{M,(i-1)}\\ W={\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000075726600000012.png,HDA0000075726600000021.png"\; state="\{\{state\}\}">W</figure-callout>}}_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{N,(i-1)}\mathrm{\&Delta;t}/R_{N,(i-1)}\end{array}\right.$

Δ t in the formula---the DVL sampling period;

J, W---be respectively n constantly longitude and the latitude of UUV carrier position;

J ₀, W ₀---be respectively longitude and the latitude of initial time carrier, recorded by the GPS receiver;

v _{E, (i-1)}, v _{N, (i-1)}---be respectively i-1 moment UUV headway at the component of due east direction and direct north;

R _{M (i-1)}, R _{N, (i-1)}---be respectively i-1 constantly earth meridian radius-of-curvature and latitude circle radius-of-curvature.

The relative prior art of the present invention has following advantage and effect:

When beneficial effect of the present invention is that UUV executes the task under water, in case the DVL sonar data lost efficacy, can use ADCP ocean current profile information to calculate UUV speed, so that UUV can continue to carry out the seafari task under the complicated marine environment such as the large degree of depth, improve the adaptive capacity to environment of UUV.

Description of drawings

Fig. 1 is the auxiliary UUV navigation flowcharts of ocean current profile of the present invention;

Fig. 2 is the auxiliary ocean current profile filter effect figure of high frequency residual error of the present invention;

Fig. 3 is that process noise variance Q of the present invention is on the impact of ocean current profile Kalman filtering;

Fig. 4 is that the present invention utilizes DVL speed dynamically to determine the Kalman filter effect figure of Q value;

Fig. 5 is that UUV of the present invention concerns schematic diagram with ship coordinate system and east northeast fixed coordinate system;

Fig. 6 is UUV track plot in the examination experiment of the present invention sea;

Auxiliary UUV navigation effect figure when Fig. 7 is invalid when ocean current profile is long in the examination experiment of the present invention sea.

Embodiment

The present invention is described in further detail below in conjunction with accompanying drawing.

(1) ocean current profile data and DVL speed are obtained

UUV is certain depth when navigation under water, ADCP (transducer head is installed downwards) is under the control computer control, the sound wave of emission certain frequency, utilize Doppler effect to obtain the ocean current profile that the UUV water layer thickness is close in the bottom, the ocean current profile data communication device cross serial ports (or network interface) be transferred to control computing machine storage medium store, just can get access to the ocean current profile data by ADCP like this.

The control computer control DVL that utilizes UUV to carry launches the sound wave of certain frequency, utilize Doppler effect to obtain the speed of DVL, the velocity information of DVL is stored by the storage medium that serial ports or (or network interface) are transferred to the control computing machine, just can get access to like this headway of UUV by DVL.

(2) the Kalman Real-Time Filtering of ocean current profile

The ocean current profile data are processed the Kalman Real-Time Filtering technology of selecting, to increase stability and the confidence level of data.

If the state equation of linear discrete system and observation equation are:

$\left\{\begin{array}{c}{X}_k={\mathrm{\&phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k,k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(1\right)$

In the formula, X _kAnd Z _kRespectively k state vector and measurement vector constantly; φ _{K, k-1}For n * n ties up nonsingular state-transition matrix; Γ _{K, k-1}That n * p maintains system process noise input matrix; H _kThat m * n ties up observing matrix; W _{K, k-1}That p maintains system stochastic process noise sequence; V _kThat m maintains system random observation noise sequence.If linear off-line system is white noise sequence, then stochastic discrete system Kalman filtering equations can be expressed as several recurrence equations:

Status predication: ${\hat{X}}_{k,k-1}={\mathrm{\&phi;}}_{k,k-1}{\hat{X}}_{k-1}---\left(2\right)$

State estimation: ${\hat{X}}_k=X_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(3\right)$

The filter gain equation: $K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}---\left(4\right)$

Predicated error variance equation: $P_{k,k-1}={\mathrm{\&phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k,k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T---\left(5\right)$

Estimation error variance equation: P _k=[1-K _kH _k] P _{K, k-1}(6)

In sea area among a small circle, can regard the ocean current profile data in the shorter time as one group of stationary random sequence, ocean current profile is a gradual data variable, for the data of this feature, when carrying out the Kalman filtering operation, its model can be simplified.Because ocean current is gradual process, so the transition matrix φ of system _{K, k-1}=1, think that namely current ocean current data equate with previous moment ocean current data; The data that sonar sensor obtains are states of system, so observing matrix H _k=1, like this, the system state dimension is 1, obtains the process noise input matrix Γ of system _{K, k-1}=1.Like this, ocean current profile Kalman filtering equations just is reduced to:

Status predication: ${\hat{X}}_{k,k-1}={\hat{X}}_{k-1}---\left(7\right)$

State estimation: ${\hat{X}}_k=X_{k,k-1}+K_k(Z_k-{\hat{X}}_{k,k-1})---\left(8\right)$

Filter gain equation: K _k=P _{K, k-1}[P _{K, k-1}+ R] ^-1(9)

Predicated error variance equation: P _{K, k-1}=P _K-1+ Q (10)

Estimation error variance equation: P _k=[1-K _k] P _{K, k-1}(11)

From formula (7)-(12) ocean current profile Kalman Filtering Formula, can find out, as long as determine observation noise variance R and the process noise variance Q of ADCP sonar system, just can progressively calculate filtered ocean current profile data.

1) observation noise variance R's determines

At first analyze the observation noise variance R in the ocean current data Kalman equation: as shown in Figure 3, the ocean current data of using wavelet transformation tool analysis ADCP sonar to collect, extract the high frequency residual error part of data, determine the size of R value according to the order of magnitude of high frequency residual error data, again with R in people's formula (9), the gain coefficient K in the calculating K alman equation.

2) process noise variance Q's determines

When the DVL sonar data is effective, adopt the UUV percentage speed variation to come the size of dynamic deterministic process noise variance Q.When the UUV percentage speed variation is larger, dynamically increase the size of Q value, to improve the real-time of Current Observations data; When the UUV percentage speed variation hour, the dynamic size of less Q value then is with the stability of increase Current Observations data.

Analyze Kalman ocean current data filtering curve that process noise variance Q:Q value in the ocean current data Kalman equation obtains when bigger than normal and less than normal as shown in Figure 2, this patent adopts the DVL sonar data to dynamically associate the method for Q value again.When the UUV velocity variations is large, dynamically increase the Q value; Otherwise dynamically less Q value then.The Kalman ocean current filtering data that employing the method obtains as shown in Figure 3.For in people's formula (10) and the formula (11), variance P is predicted in the estimation in the calculating K alman equation with the Q value.

(3) " UUV speed-Ocean current information " concerns storehouse foundation

When the DVL sonar data is effective, the UUV course information that records according to its UUV velocity information that records and compass, the Ocean current information that ADCP is recorded is from being transformed under the east northeast fixed coordinate system, as shown in Figure 2 with the ship coordinate system.Conversion formula is as follows:

$\left\{\begin{array}{c}{V}_E=V_u\&times;\cos \left(\mathrm{\&theta;}\right)-V_v\&times;\sin \left(\mathrm{\&theta;}\right)\\ V_N=V_u\&times;\sin \left(\mathrm{\&theta;}\right)+V_v\&times;\cos \left(\mathrm{\&theta;}\right)\end{array}\right.---\left(12\right)$

In the formula, V _EBe ξ direction current speed in the east northeast fixed coordinate system, V _NBe ζ direction current speed in the east northeast fixed coordinate system, V _uFor with x direction current speed in the ship coordinate system, V _vFor with y direction current speed in the ship coordinate system.After the current speed coordinate system conversion, just UUV speed and ocean current profile can be carried out relatedly, set up " UUV speed-Ocean current information " and concern the storehouse.

(4) ocean current profile assisting navigation

When DVL navigation sonar data failure, utilize " UUV speed-Ocean current information " concern the storehouse, and the ocean current profile information that gets of ADCP sonar Real-time Measuring, the navigation speed of reckoning UUV.According to the longitude and latitude of initial UUV and the positional information that the dead reckoning algorithm obtains calculating UUV, calculate that specifically process is as follows again:

The UUV navigation speed of calculating the component of due east direction and direct north as shown in the formula:

$\left\{\begin{array}{c}{v}_E=v_F\sin H-v_L\cos H\\ v_N=v_F\cos H+v_L\sin H\end{array}\right.---\left(13\right)$

V in the formula _E, v _N---be respectively the carrier headway at the component of due east direction and direct north;

v _F, v _L---be respectively carrier forward speed and the left-hand speed relative to the earth that DVL records;

H---the course angle of UUV carrier for just, for negative, can be recorded by compass clockwise counterclockwise.

The UUV position is calculated by following formula:

$\left\{\begin{array}{c}J={\mathrm{<figure-callout\; id="0"\; label="J"\; filenames="HDA0000075726600000012.png,HDA0000075726600000021.png"\; state="\{\{state\}\}">J</figure-callout>}}_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{E,(i-1)}\mathrm{\&Delta;t}/R_{M,(i-1)}\\ W={\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000075726600000012.png,HDA0000075726600000021.png"\; state="\{\{state\}\}">W</figure-callout>}}_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{N,(i-1)}\mathrm{\&Delta;t}/R_{N,(i-1)}\end{array}\right.---\left(13\right)$

Δ t in the formula---the DVL sampling period;

J, W---be respectively n constantly longitude and the latitude of UUV carrier position;

J ₀, W ₀---be respectively longitude and the latitude of initial time carrier, can be recorded by the GPS receiver;

v _{E, (i-1)}, v _{N, (i-1)}---be respectively i-1 moment UUV headway at the component of due east direction and direct north;

R _{M (i-1)}, R _{N, (i-1)}---be respectively i-1 constantly earth meridian radius-of-curvature and latitude circle radius-of-curvature.

Embodiments of the invention are below described.

In order to verify the validity of the inventive method, carried out DLV assisting navigation sea and had a try and test, one section U-shaped flight path choosing among the present invention in the UUV planning describes, and 1 hour 18 minutes this flight path time spent, the flight path track is as shown in Figure 6.Suppose that DVL can not get valid data for a long time, the ocean current profile assisting navigation will adopt the related good DVL-ocean current profile associated data of last bat to navigate always so, Fig. 7 is the ocean current profile data navigation figure of DVL when can not get valid data for a long time, the auxiliary UUV of ocean current profile omnidistance 6008m that navigates this time, relative deviation 2.35%, the result shows that ocean current profile auxiliary navigation method DVL had great using value when losing efficacy.

Claims (2)

1. the UUV auxiliary navigation method based on ocean current profile is characterized in that comprising the following steps:

(1) ocean current profile data and DVL speed are obtained

The control computer control ADCP that utilizes UUV to carry launches the sound wave of certain frequency, utilizes Doppler effect to obtain the ocean current profile that the UUV water layer thickness is close in the bottom;

Utilize the sound wave of the control computer control DVL emission certain frequency that UUV carries, utilize Doppler effect to obtain the speed of DVL, the i.e. headway of UUV;

(2) the Kalman filtering of ocean current profile

According to the characteristics of ocean current environment and UUV self model, set up ocean current profile data Kalman filtering data model; And determine observation noise variance R in the ocean current Kalman filtering data model with reference to the order of magnitude of ocean current high frequency residual error, the size of the UUV velocity information Dynamic Regulating Process noise variance Q value that records according to DVL simultaneously;

(3) " UUV speed-Ocean current information " concerns storehouse foundation

When DVL navigation sonar data are effective, utilize the course information of UUV speed that DVL records, UUV that the OCTANS attitude sensor measures, the ocean current profile data that ADCP measures are set up " UUV speed-Ocean current information " and are concerned the storehouse;

(4) ocean current profile assisting navigation

When DVL navigation sonar data failure, utilize " UUV speed-Ocean current information " concern the storehouse, and the ocean current profile information that gets of ADCP sonar Real-time Measuring, the navigation speed of reckoning UUV; Again according to the longitude and latitude of initial UUV and the positional information that the dead reckoning algorithm obtains calculating UUV.

2. a kind of UUV auxiliary navigation method based on ocean current profile according to claim 1 is characterized in that the acquisition methods of the positional information of described UUV is:

The UUV navigation speed of calculating the component of due east direction and direct north as shown in the formula:

$\left\{\begin{array}{c}{v}_E=v_F\sin H-v_L\cos H\\ v_N=v_F\cos H+v_L\sin H\end{array}\right.$

V in the formula _E, v _N---be respectively UUV carrier headway at the component of due east direction and direct north;

v _F, v _L---be respectively UUV carrier forward speed and the left-hand speed relative to the earth that DVL records;

H---the course angle of UUV carrier for just, for negative, is recorded by compass clockwise counterclockwise;

The UUV position is calculated by following formula:

$\left\{\begin{array}{c}J=J_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{E,(i-1)}\mathrm{\&Delta;t}/R_{M,(i-1)}\\ W=W_0+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{v}_{N,(i-1)}\mathrm{\&Delta;t}/R_{N,(i-1)}\end{array}\right.$

Δ t in the formula---the DVL sampling period;

J, W---be respectively n constantly longitude and the latitude of UUV carrier position;

J ₀, W ₀---be respectively longitude and the latitude of initial time carrier, recorded by the GPS receiver;

v _{E, (i-1)}, v _{N, (i-1)}---be respectively i-1 moment UUV carrier headway at the component of due east direction and direct north;

R _{M, (i-1)}, R _{N, (i-1)}---be respectively i-1 constantly earth meridian radius-of-curvature and latitude circle radius-of-curvature.

Images