CN104180804A - Single reference node underwater vehicle integrated navigation method based on underwater information network - Google Patents

Abstract

The invention discloses a single reference node underwater vehicle integrated navigation method based on an underwater information network. The method comprises the following steps: realizing positioning of the underwater vehicle by utilizing an underwater single network node with a known position as a reference node, utilizing a series of distance information acquired in the underwater vehicle navigation process through communication ranging, combining the operating speed, displacement, course angle and other information of the underwater vehicle in each communication ranging time interval; and correcting an accumulative error of a serial inertial navigation system/Doppler velocity measurement system (SINS/DVL) equipped in the underwater vehicle by utilizing the position information acquired by positioning, thereby realizing high-precision integrated navigation. According to the method, the accumulative error of the SINS/DVL can be corrected at fixed time, and the correction period is set in advance, so that long-term high-precision integrated navigation of the underwater vehicle is realized, and the method has high actual application value.

Description

Based on single reference mode scale underwater vehicle combined navigation method of underwater information network

Technical field

The invention relates to navigator fix technology, especially about a kind of single reference mode scale underwater vehicle combined navigation system and navigation locating method based on underwater information network.

Background technology

Ocean can be described as the key areas of human survival and sustainable development, and the utilization to Yu Haiyang and exploitation have become one of Fundamentals that determine the national ups and downs.In the field such as utilization and exploitation of ocean, due to radiowave, attenuation ratio is more serious under water, cannot meet the requirement of underwater communication, thereby the water sound communication technique taking sound wave as carrier has obtained developing extremely widely and applying.

In recent decades, underwater information network is as a large branch of wireless sensor network technology application, taking underwater sound communication and network technology as basic, enters into brand-new developing stage in the research field of underwater acoustic technology.Underwater information network, is deployed in underwater environment exactly, using sound wave as information carrier, underwater acoustic channel is as the wireless sensor network of communication channel, is the extension of land wireless sensor network to submersible service.Dispose under water sensor network nodes, can detect underwater environment on the one hand, carry out the collection of underwater information, on the other hand can be using the sensor network nodes of known location as with reference to node, be used for the submarine navigation device in this marine site etc. to position, thereby auxiliary submarine navigation device is realized high precision navigation.

Make submarine navigation device complete some predetermined task and missions, naturally be unable to do without underwater navigation technology.Compare with land navigation and air navigation, underwater navigation has the features such as longevity of service, disguise require high, circumstance complication and information source is few, and therefore with respect to land navigation and air navigation, the research of underwater navigation and application have larger difficulty.

At present, underwater information network and Technology for Target Location being combined, is the emerging research direction of acoustic positioning technique.It is a gordian technique of the numerous application of underwater information network that the target of surveyed area is positioned, and is also the basis of other application.

In the present invention, using the single underwater information network node of known position information as with reference to node, while is in conjunction with the flight path of submarine navigation device, be used for the submarine navigation device in this marine site to position, and utilize positioning result timing to revise the cumulative errors of submarine navigation device SINS/DVL navigational system, thereby realize the long high-precision independent navigation of submarine navigation device.

Utilize underwater information network node to realize the target localization in monitoring marine site, combine with the SINS/DVL navigational system of submarine navigation device self configuration simultaneously, thereby revise the cumulative errors of SINS/DVL navigational system, realize high-precision integrated navigation, this in navigator fix research, has considerable meaning under water.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, a kind of high-precision independent combined navigation method is for a long time provided, the method can improve the navigation accuracy of the submarine navigation device of long-time navigation greatly, and has independence.

The present invention adopts following technical scheme for achieving the above object:

Based on a single reference mode scale underwater vehicle combined navigation method for underwater information network, comprise the steps:

A, submarine navigation device navigates by water under water in whole process and to navigate by the SINS/DVL navigational system of self assembling.SINS/DVL navigational system can be carried out to submarine navigation device the high precision navigator fix of short time, but As time goes on, has certain cumulative errors.For the submarine navigation device of long-time navigation, there is certain navigation deviation in SINS/DVL navigational system, need to regularly introduce other positioning systems and carry out assisting navigation location, thereby revise the navigation cumulative errors of this navigational system, realize long high-precision independent navigation.

Wherein, SINS/DVL navigational system principle is:

The error model of comprehensive SINS system and DVL system, sets up state equation and the measurement equation of SINS/DVL system, utilizes Kalman filtering to carry out the optimal estimation of this system, thereby realizes the navigation of SINS/DVL navigational system.

B, after long-time navigation, there is certain cumulative errors in SINS/DVL navigational system, therefore need timing to carry out auxiliary positioning to it, utilizes auxiliary positioning modified result navigation cumulative errors, and position fixing process is as follows:

B-1, in submarine navigation device navigation process, at set intervals, by the range finding platform on submarine navigation device, and complete communication distance measuring between grid of reference node, thereby obtain delay inequality, calculate range information, for solving of tested point position provides sufficient condition, improve positioning precision.

B-2, the information such as travelling speed in conjunction with submarine navigation device within each communication distance measuring time interval, displacement, course angle, the location of realizing submarine navigation device.

The method of a kind of communication distance measuring in step B-1, adopts the method for two-way communication range finding to complete the subaqueous sound ranging between reference point and tested point in the present invention, concrete steps are:

First, tested point underwater sound modem produces the position pulse being made up of information code and synchronous head signal, after modulation, launch this position pulse signal to reference point (location aware) under water, in this transmitted signal, comprise transmitting time t1, in the time that reference point is received this signal under water, carry out synchronous demodulation, and utilize synchronizing process to obtain signal transmission time delay difference τ 1, and τ 1 modulation again together with the signal of receiving, frequency conversion postbacks to node to be measured as replying, this postbacks and in signal, also comprises the depth information that postbacks time t3 and reference point, tested point transducer is received when this postbacks signal, carry out again synchronous demodulation, obtain this transmission time delay difference τ 2, demodulation simultaneously obtains τ 1, in communication distance measuring process, signal transmission time delay difference τ 1, τ 2, ask coherent detection to try to achieve by LFM signal.

If the time error of signal transmitting terminal and receiving end is Δ τ, One Way Delay between sending node and receiving node is poor is τ.By signal processing, can obtain following time relationship:

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

Can be obtained fom the above equation:

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

By the delay inequality τ obtaining, be multiplied by the equivalent bulk sound velocity in this waters again, obtain the distance between 2, complete range finding.

Dead reckoning in twice communication distance measuring time interval in step B-2, concrete steps are:

If ψ, s are respectively the displacements of course angle and the oxy plane of submarine navigation device, and s _j=m _jcos θ _j, m and θ are respectively submarine navigation device at the displacement of three-dimensional system of coordinate and the angle of pitch of submarine navigation device, can obtain by the real-time measurements and calculations of inertial navigation instrument.

Twice communication distance measuring time can be expressed as interior displacement:

$\left\{\begin{array}{c}{a}_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\cos {\mathrm{\ψ}}_j\\ b_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\sin {\mathrm{\ψ}}_j\end{array}\right.---\left(3\right)$

Wherein 1 " i " n, s _iand ψ _ican resolve and obtain by SINS/DVL navigational system.A _i, b _ibe respectively the submarine navigation device i moment compared with the initial time of location, the axial increment of x and the axial increment of y.

Comprehensive B-1 and B-2, the positioning principle in step B, concrete steps are:

First, submarine navigation device is modeled as in the position of the i time sampling instant:

$\left\{\begin{array}{c}{x}_i=x_0+a_i\\ y_i=y_0+b_i\end{array}\right.---\left(4\right)$

Wherein, (x ₀, y ₀) be the navigation reference position of submarine navigation device, (x _i, y _i) be the position of submarine navigation device the i time sampling instant.

When the i time sampling, the distance d between submarine navigation device and grid of reference node _ibe expressed as:

$d_i={[{(x_i-x_b)}^2+{(y_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(5\right)$

Wherein, z _imeasure in real time by depth transducer.

By x _i, y _iuse x ₀, y ₀, a _iand b _irepresent, in substitution formula (5), the distance expression that can obtain between submarine navigation device and grid of reference node is simultaneously:

$d_i={[{(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(6\right)$

Can be expressed as again:

$d_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2---\left(7\right)$

Wherein, d _ifor the distance between target to be measured of a certain moment and grid of reference node, obtain by the product of the velocity of sound and sound wave signal propagation time.(x ₀, y ₀) be submarine navigation device location initial time two-dimensional coordinate value, the depth value z of submarine navigation device _irecord by depth transducer.(x _b, y _b, z _b) be the coordinate with reference to underwater information network node, be known quantity.

Find range by two-way communication, the distance between target to be measured and grid of reference node can be expressed as:

d _i＝cτ _i (8)

i＝1,2,…,n

By in formula (8) substitution formula (7):

$\begin{array}{c}{c}^2{{\mathrm{\τ}}_i}^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(9\right)$

Wherein, (x ₀, y ₀) be unknown quantity to be asked, its surplus is known quantity.

For the positioning system based on two-way communication range finding, location initial time, and the location model expression formula of sampling instant is afterwards as follows:

$\begin{array}{c}\left\{\begin{array}{cc}{c}^2{\mathrm{\τ}}_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2& 1)\\ c^2{{\mathrm{\τ}}_0}^2={(x_0-x_b)}^2+{(y_0-y_b)}^2+{(z_0-z_b)}^2& 2)\end{array}\right.\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(10\right)$

After above formula bracket is launched, and by 1 in formula (10)) formula deducts 2) formula, after arrangement, can obtain following system of linear equations:

$\begin{array}{c}2a_i{x}_0+2b_i{y}_o=c^2({\mathrm{\τ}}_i^2-{\mathrm{\τ}}_0^2)-a_i^2+2a_i{x}_b-b_i^2+2b_i{y}_b+{(z_0-z_b)}^2-{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(11\right)$

Launch:

$\left\{\begin{array}{c}2a_1{x}_0+2b_1{y}_o=c^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ 2a_2{x}_0+2b_2{y}_o=c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ 2a_n{x}_0+2b_n{y}_o=c^2\left({\mathrm{\τ}}_n^2{\mathrm{\τ}}_0^2\right)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right.---\left(12\right)$

Arrangement is:

$\left[\begin{array}{c}2a_{1}2b_1\\ {2a}_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(13\right)$

Write as matrix form:

CX＝D (14)

Wherein:

$X={\left[\begin{array}{cc}{x}_0& y_0\end{array}\right]}^T---\left(15\right)$

$C=\left[\begin{array}{c}2a_{1}2b_1\\ 2a_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]---\left(16\right)$

$D=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(17\right)$

In system of equations CX=D, variable in C is derived and is obtained by the track of inertial reference calculation, because sampling is shorter computing time, can think that inertial navigation has sufficiently high precision, there is not error disturbance in C, and delay inequality variable τ in D need to be by measuring, there is error disturbance, meet the precondition of least square, therefore can utilize least square method, obtain solution of equations and be:

X＝(C ^TC) ^-1C ^TD (18)

By above formula, can, in the hope of the positional information of tested point, realize location.

C, timing positions computing to submarine navigation device, utilizes the positional information of location gained to revise the cumulative errors of the SINS/DVL navigational system of submarine navigation device configuration, thereby realizes high-precision integrated navigation.

Utilize SINS/DVL navigational system to navigate, after a period of time, cumulative errors increases, timing positions computing to submarine navigation device, utilize the positional information of the tested point that position fixing process tries to achieve to revise the cumulative errors of SINS/DVL navigational system, thereby realize high-precision independent combined navigation.

Compared with prior art, tool of the present invention has the following advantages and remarkable result:

For current conventional SINS/DVL navigational system, can carry out to submarine navigation device the high precision navigator fix of short time, but As time goes on, have certain cumulative errors.For the submarine navigation device of long-time navigation, there is certain navigation deviation in SINS/DVL navigational system, need to regularly introduce other positioning systems and carry out auxiliary positioning, thereby revise this system navigation cumulative errors, the long-time high precision navigation of guarantee submarine navigation device like this.A kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network that the present invention proposes, on the basis of SINS/DVL navigation, timing utilizes single underwater information network reference node, and the flight path information of submarine navigation device positions, utilize the cumulative errors of positioning result correction SINS/DVL navigational system, thereby ensureing, on the concealed basis of submarine navigation device, to realize the long-time high-precision independent combined navigation of submarine navigation device.The present invention easily realizes, and has higher actual application value.

Brief description of the drawings

Fig. 1 is that real trace and SINS/DVL resolve track emulation figure;

Fig. 2 is integrated navigation schematic diagram of the present invention;

Fig. 3 is two-way communication range finding schematic diagram of the present invention;

Fig. 4 is the coherent detection envelope output of LFM signal;

Fig. 5 is flight path dead reckoning figure of the present invention;

Fig. 6 is integrated navigation correction track comparison diagram.

Specific embodiments

Below in conjunction with accompanying drawing, technical scheme of the present invention is described in further details.

As shown in Figure 1, real trace and SINS/DVL resolve track emulation figure.Simulation parameter is: submarine navigation device navigation coordinate is that initial coordinate is (0,0,20), and initial velocity is 3m/s.Initial angle is: course angle 40 is spent, and the angle of pitch 0 is spent, and roll angle 0 is spent.Gyroscope and accelerometer are once sampled every 1s, and DVL carries out a speed correction every 60s, and hours underway is 4h altogether.The phantom error parameter of inertial navigation instrument and DVL is: 0.1 °/h of gyro drift error, gyroscope is often worth 0.05 °/h of noise error, accelerometer bias error 10 ^-4g, accelerometer noise error 10 ^-5g, DVL offset error 0.05m/s, DVL scale coefficient error 0.01.

Submarine navigation device navigates by the SINS/DVL navigational system of self assembling.Can find out by analogous diagram, SINS/DVL navigational system can be carried out to submarine navigation device the high precision navigator fix of short time, but As time goes on, has certain cumulative errors.For the submarine navigation device of long-time navigation, there is certain navigation deviation in SINS/DVL navigational system, need to regularly introduce other positioning systems and carry out assisting navigation location, thereby revise the navigation cumulative errors of this navigational system, realize long high-precision independent navigation.

As shown in Figure 2, integrated navigation schematic diagram of the present invention.Utilize the node of single network under water of location aware, and a series of range informations that obtain by communication distance measuring in submarine navigation device navigation process, the simultaneously information such as the travelling speed within each communication distance measuring time interval, displacement, course angle in conjunction with submarine navigation device, realize the location of submarine navigation device, then utilize the positional information of location gained to revise the cumulative errors of the SINS/DVL navigational system of submarine navigation device configuration, thereby realize high-precision integrated navigation.

As shown in Figure 3, two-way communication range finding schematic diagram of the present invention.In position fixing process, need to measure the distance between tested point and reference point, the present invention utilizes two-way communication to find range to record the distance between tested point and reference point.Detailed process is:

First, tested point underwater sound modem produces the position pulse being made up of information code and synchronous head signal, after modulation, launches this position pulse signal to reference point (location aware) under water, comprises transmitting time t in this transmitted signal ₁, in the time that reference point is received this signal under water, carry out synchronous demodulation, and utilize synchronizing process to obtain signal transmission time delay difference τ ₁, and τ ₁modulation again together with the signal of receiving, frequency conversion postbacks to node to be measured as replying, and this postbacks in signal and also comprises and postback time t ₃with the depth information of reference point, tested point transducer receives when this postbacks signal, then carries out synchronous demodulation, obtains this transmission time delay difference τ ₂, demodulation simultaneously obtains τ ₁.

If the time error of signal transmitting terminal and receiving end is Δ τ, One Way Delay between sending node and receiving node is poor is τ.By signal processing, can obtain following time relationship:

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

Can be obtained fom the above equation:

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

By the delay inequality τ obtaining, be multiplied by the equivalent bulk sound velocity in this waters again, obtain the distance between 2, complete range finding.

As shown in Figure 4, the coherent detection envelope of LFM signal output.The simulation parameter of LFM signal is: f ₀=4kHz, f _s=20kHz, B=5kHz, T=400ms, sampling number N=8000, snr of received signal is 15dB, transmission channel is underwater sound multipath channel.In communication distance measuring process, signal transmission time delay difference τ ₁, τ ₂, ask coherent detection to try to achieve by LFM signal.

As shown in Figure 5, flight path dead reckoning figure of the present invention.Wanted a complete position fixing process, must be in conjunction with submarine navigation device the information such as the travelling speed within each communication distance measuring time interval, displacement, course angle.Dead reckoning in twice communication distance measuring time interval, concrete steps are:

If ψ, s are respectively the displacements of course angle and the oxy plane of submarine navigation device, and s _j=m _jcos θ _j, m and θ are respectively submarine navigation device at the displacement of three-dimensional system of coordinate and the angle of pitch of submarine navigation device, can obtain by the real-time measurements and calculations of inertial navigation instrument.

Twice communication distance measuring time can be expressed as interior displacement:

$\left\{\begin{array}{c}{a}_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\cos {\mathrm{\ψ}}_j\\ b_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\sin {\mathrm{\ψ}}_j\end{array}\right.---\left(3\right)$

Wherein 1 " i " n, s _iand ψ _ican resolve and obtain by SINS/DVL navigational system.A _i, b _ibe respectively the submarine navigation device i moment compared with the initial time of location, the axial increment of x and the axial increment of y.

As shown in Figure 6, integrated navigation correction track comparison diagram.Utilize above information, at a time submarine navigation device is positioned, positioning principle concrete steps are:

First, submarine navigation device is modeled as in the position of the i time sampling instant:

$\left\{\begin{array}{c}{x}_i=x_0+a_i\\ y_i=y_0+b_i\end{array}\right.---\left(4\right)$

Wherein, (x ₀, y ₀) be the navigation reference position of submarine navigation device, (x _i, y _i) be the position of submarine navigation device the i time sampling instant.

When the i time sampling, the distance d between submarine navigation device and grid of reference node _ibe expressed as:

$d_i={[{(x_i-x_b)}^2+{(y_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(5\right)$

Wherein, z _imeasure in real time by depth transducer.

By x _i, y _iuse x ₀, y ₀, a _iand b _irepresent, in substitution formula (5), the distance expression that can obtain between submarine navigation device and grid of reference node is simultaneously:

$d_i={[{(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(6\right)$

Can be expressed as again:

$d_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2---\left(7\right)$

Wherein, d _ifor the distance between target to be measured of a certain moment and grid of reference node, obtain by the product of the velocity of sound and sound wave signal propagation time.(x ₀, y ₀) be submarine navigation device location initial time two-dimensional coordinate value, the depth value z of submarine navigation device _irecord by depth transducer.(x _b, y _b, z _b) be the coordinate with reference to underwater information network node, be known quantity.

Find range by two-way communication, the distance between target to be measured and grid of reference node can be expressed as:

d _i＝cτ _i (8)

i＝1,2,…,n

By in formula (8) substitution formula (7):

$\begin{array}{c}{c}^2{{\mathrm{\τ}}_i}^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(9\right)$

Wherein, (x ₀, y ₀) be unknown quantity to be asked, its surplus is known quantity.

For the positioning system based on two-way communication range finding, location initial time, and the location model expression formula of sampling instant is afterwards as follows:

$\begin{array}{c}\left\{\begin{array}{cc}{c}^2{\mathrm{\τ}}_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2& 1)\\ c^2{{\mathrm{\τ}}_0}^2={(x_0-x_b)}^2+{(y_0-y_b)}^2+{(z_0-z_b)}^2& 2)\end{array}\right.\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(10\right)$

After above formula bracket is launched, and by 1 in formula (10)) formula deducts 2) formula, after arrangement, can obtain following system of linear equations:

$\begin{array}{c}2a_i{x}_0+2b_i{y}_o=c^2({\mathrm{\τ}}_i^2-{\mathrm{\τ}}_0^2)-a_i^2+2a_i{x}_b-b_i^2+2b_i{y}_b+{(z_0-z_b)}^2-{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(11\right)$

Launch:

$\left\{\begin{array}{c}2a_1{x}_0+2b_1{y}_o=c^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ 2a_2{x}_0+2b_2{y}_o=c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ 2a_n{x}_0+2b_n{y}_o=c^2\left({\mathrm{\τ}}_n^2{\mathrm{\τ}}_0^2\right)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right.---\left(12\right)$

Arrangement is:

$\left[\begin{array}{c}2a_{1}2b_1\\ {2a}_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(13\right)$

Write as matrix form:

CX＝D (14)

Wherein:

$X={\left[\begin{array}{cc}{x}_0& y_0\end{array}\right]}^T---\left(15\right)$

$C=\left[\begin{array}{c}2a_{1}2b_1\\ 2a_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]---\left(16\right)$

$D=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(17\right)$

In system of equations CX=D, variable in C is derived and is obtained by the track of inertial reference calculation, because sampling is shorter computing time, can think that inertial navigation has sufficiently high precision, there is not error disturbance in C, and delay inequality variable τ in D need to be by measuring, there is error disturbance, meet the precondition of least square, therefore can utilize least square method, obtain solution of equations and be:

X＝(C ^TC) ^-1C ^TD (18)

By above formula, can, in the hope of the positional information of tested point, realize location.

After submarine navigation device navigation a period of time, cumulative errors increases, timing positions submarine navigation device, after position fixing process completes, utilize the positional information of the tested point that position fixing process tries to achieve, cumulative errors to SINS/DVL navigational system is revised, thereby realizes high-precision independent combined navigation.

Simulation parameter in Fig. 6 is: SINS/DVL navigational parameter is identical with Fig. 1 parameter, and total hours underway is 4h, carries out a timing error correction every 1.5h.The coordinate of selecting grid of reference node is (10000,12000,40), only considers time determination error and sound ray bending when supposing to locate in position fixing process.Emulation by Fig. 6 can be found out: the single reference mode scale underwater vehicle combined navigation technology based on underwater information network of the present invention's research, can be used for timing revises the cumulative errors of SINS/DVL navigational system, thereby ensureing, on the basis of submarine navigation device disguise and independence, to realize the long-time high-precision integrated navigation of submarine navigation device.

Claims (6)

1. the single reference mode scale underwater vehicle combined navigation method based on underwater information network, it comprises the steps:

A, submarine navigation device navigates by water under water in whole process and slightly to navigate by the SINS/DVL navigational system of self assembling;

B, timing is carried out auxiliary positioning to described submarine navigation device, and position fixing process is as follows:

B-1, in submarine navigation device navigation process, at set intervals, by the range finding platform on submarine navigation device, and complete communication distance measuring between grid of reference node, thereby obtain delay inequality, calculate range information, for solving of tested point position provides sufficient condition, improve positioning precision;

B-2, the information such as travelling speed in conjunction with submarine navigation device within each communication distance measuring time interval, displacement, course angle, the location of realizing submarine navigation device;

C, timing positions computing to submarine navigation device, utilizes the positional information of location gained to revise the cumulative errors of the SINS/DVL navigational system of submarine navigation device configuration, thereby realizes high-precision integrated navigation.

2. a kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network according to claim 1, is characterized in that:

SINS/DVL navigational system in described steps A is the error model of comprehensive SINS system and DVL system, set up state equation and the measurement equation of SINS/DVL system, utilize Kalman filtering to carry out the optimal estimation of this system, thereby realize the thick navigation of SINS/DVL navigational system.

3. a kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network according to claim 1, is characterized in that:

Described communication distance measuring in step B-1 specifically comprises the following steps:

Tested point underwater sound modem produces the position pulse being made up of information code and synchronous head signal, after modulation, launches this position pulse signal to reference point under water, and the location aware of wherein said reference point under water, comprises transmitting time t in this transmitted signal ₁;

In the time that reference point is received this signal under water, carry out synchronous demodulation, and utilize synchronizing process to obtain signal transmission time delay difference τ ₁, and τ ₁modulation again together with the signal of receiving, frequency conversion postbacks to node to be measured as replying, and this postbacks in signal and also comprises and postback time t ₃depth information with reference point;

Tested point transducer receives when this postbacks signal, then carries out synchronous demodulation, obtains this transmission time delay difference τ ₂, demodulation simultaneously obtains τ ₁;

In communication distance measuring process, signal transmission time delay difference τ ₁, τ ₂, ask coherent detection to try to achieve by LFM signal;

If the time error of signal transmitting terminal and receiving end is Δ τ, One Way Delay between sending node and receiving node is poor is τ, by signal processing, can obtain following time relationship:

$\left\{\begin{array}{c}{\mathrm{\τ}}_1=\mathrm{\τ}+\mathrm{\Δ\τ}\\ {\mathrm{\τ}}_2=\mathrm{\τ}-\mathrm{\Δ\τ}\end{array}\right.---\left(1\right)$

Can be obtained fom the above equation:

$\left\{\begin{array}{c}\mathrm{\τ}=\frac{{\mathrm{\τ}}_1+{\mathrm{\τ}}_2}{2}\\ \mathrm{\Δ\τ}=\frac{{\mathrm{\τ}}_1-{\mathrm{\τ}}_2}{2}\end{array}\right.---\left(2\right)$

By the delay inequality τ obtaining, be multiplied by the equivalent bulk sound velocity in this waters again, obtain the distance between 2, complete range finding.

4. a kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network according to claim 1, is characterized in that:

Dead reckoning in twice communication distance measuring time interval in step B-2, concrete steps are:

If ψ, s are respectively the displacements of course angle and the oxy plane of submarine navigation device, and s _j=m _jcos θ _j, m and θ are respectively submarine navigation device at the displacement of three-dimensional system of coordinate and the angle of pitch of submarine navigation device, can obtain by the real-time measurements and calculations of inertial navigation instrument;

Twice communication distance measuring time can be expressed as interior displacement:

$\left\{\begin{array}{c}{a}_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\cos {\mathrm{\ψ}}_j\\ b_i=\underset{j=0}{\overset{i-1}{\mathrm{\Σ}}}{s}_j\sin {\mathrm{\ψ}}_j\end{array}\right.---\left(3\right)$

Wherein 1 " i " n, s _iand ψ _ican resolve and obtain by SINS/DVL navigational system, a _i, b _ibe respectively the submarine navigation device i moment compared with the initial time of location, the axial increment of x and the axial increment of y.

5. a kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network according to claim 1, is characterized in that:

Positioning principle in step B, concrete steps are:

First, submarine navigation device is modeled as in the position of the i time sampling instant:

$\left\{\begin{array}{c}{x}_i=x_0+a_i\\ y_i=y_0+b_i\end{array}\right.---\left(4\right)$

Wherein, (x ₀, y ₀) be the navigation reference position of submarine navigation device, (x _i, y _i) be the position of submarine navigation device the i time sampling instant;

When the i time sampling, the distance d between submarine navigation device and grid of reference node _ibe expressed as:

$d_i={[{(x_i-x_b)}^2+{(y_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(5\right)$

Wherein, z _imeasure in real time by depth transducer.

By x _i, y _iuse x ₀, y ₀, a _iand b _irepresent, in substitution formula (5), the distance expression that can obtain between submarine navigation device and grid of reference node is simultaneously:

$d_i={[{(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2]}^{\frac{1}{2}}---\left(6\right)$

Can be expressed as again:

$d_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2---\left(7\right)$

Wherein, d _ifor the distance between target to be measured of a certain moment and grid of reference node, obtain (x by the product of the velocity of sound and sound wave signal propagation time ₀, y ₀) be submarine navigation device location initial time two-dimensional coordinate value, the depth value z of submarine navigation device _irecord (x by depth transducer _b, y _b, z _b) be the coordinate with reference to underwater information network node, be known quantity;

Find range by two-way communication, the distance between target to be measured and grid of reference node can be expressed as:

d _i＝cτ _i (8)

i＝1,2,…,n

By in formula (8) substitution formula (7):

$\begin{array}{c}{c}^2{{\mathrm{\τ}}_i}^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(9\right)$

Wherein, (x ₀, y ₀) be unknown quantity to be asked, its surplus is known quantity;

For the positioning system based on two-way communication range finding, location initial time, and the location model expression formula of sampling instant is afterwards as follows:

$\begin{array}{c}\left\{\begin{array}{cc}{c}^2{\mathrm{\τ}}_i^2={(x_0+a_i-x_b)}^2+{(y_0+b_i-y_b)}^2+{(z_i-z_b)}^2& 1)\\ c^2{{\mathrm{\τ}}_0}^2={(x_0-x_b)}^2+{(y_0-y_b)}^2+{(z_0-z_b)}^2& 2)\end{array}\right.\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(10\right)$

After above formula bracket is launched, and by 1 in formula (10)) formula deducts 2) formula, after arrangement, can obtain following system of linear equations:

$\begin{array}{c}2a_i{x}_0+2b_i{y}_o=c^2({\mathrm{\τ}}_i^2-{\mathrm{\τ}}_0^2)-a_i^2+2a_i{x}_b-b_i^2+2b_i{y}_b+{(z_0-z_b)}^2-{(z_i-z_b)}^2\\ i=\mathrm{1,2}\·\·\·n\end{array}---\left(11\right)$

Launch:

$\left\{\begin{array}{c}2a_1{x}_0+2b_1{y}_o=c^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ 2a_2{x}_0+2b_2{y}_o=c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ 2a_n{x}_0+2b_n{y}_o=c^2\left({\mathrm{\τ}}_n^2{\mathrm{\τ}}_0^2\right)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right.---\left(12\right)$

Arrangement is:

$\left[\begin{array}{c}2a_{1}2b_1\\ {2a}_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(13\right)$

Write as matrix form:

CX＝D (14)

Wherein:

$X={\left[\begin{array}{cc}{x}_0& y_0\end{array}\right]}^T---\left(15\right)$

$C=\left[\begin{array}{c}2a_{1}2b_1\\ 2a_{2}2b_2\\ \·\\ \·\\ \·\\ 2a_{n}2b_n\end{array}\right]---\left(16\right)$

$D=\left[\begin{array}{c}{c}^2({\mathrm{\τ}}_1^2-{\mathrm{\τ}}_0^2)-a_1^2+2a_1{x}_b-b_1^2+2b_1{y}_b+{(z_0-z_b)}^2-{(z_1-z_b)}^2\\ c^2({\mathrm{\τ}}_2^2-{\mathrm{\τ}}_0^2)-a_2^2+2a_2{x}_b-b_2^2+2b_2{y}_b+{(z_0-z_b)}^2-{(z_2-z_b)}^2\\ \·\\ \·\\ \·\\ c^2({\mathrm{\τ}}_n^2-{\mathrm{\τ}}_0^2)-a_n^2+2a_n{x}_b-b_n^2+2b_n{y}_b+{(z_0-z_b)}^2-{(z_n-z_b)}^2\end{array}\right]---\left(17\right)$

In system of equations CX=D, variable in C is derived and is obtained by the track of inertial reference calculation, because sampling is shorter computing time, can think that inertial navigation has sufficiently high precision, there is not error disturbance in C, and delay inequality variable τ in D need to be by measuring, there is error disturbance, meet the precondition of least square, therefore can utilize least square method, obtain solution of equations and be:

X＝(C ^TC) ^-1C ^TD (18)

By above formula, can, in the hope of the positional information of tested point, realize location.

6. a kind of single reference mode scale underwater vehicle combined navigation method based on underwater information network according to claim 1, is characterized in that:

In step C, the cumulative errors of SINS/DVL navigational system is revised in timing, and concrete steps are:

Utilize SINS/DVL navigational system to navigate, timing positions computing to submarine navigation device, utilizes the positional information of the tested point that position fixing process tries to achieve to revise the cumulative errors of SINS/DVL navigational system, thereby realizes high-precision independent combined navigation.