CN105547289A - Underwater vehicle combined navigation system and navigation information fusion method - Google Patents

Abstract

The invention discloses an underwater vehicle combined navigation system and a navigation information fusion method. The system comprises a strap-down inertial navigation device, a long base-line localization device, a bathometer and an information fusion module. The strap-down inertial navigation device measures a position, a rate and heading and attitude signals of an aircraft and transmits the data to the information fusion module. The long base-line localization device measures position information of the aircraft and transmits the position information to the information fusion module. The bathometer measures height information of the aircraft and transmits the height information to the information fusion module. The information fusion module fuses the information measured through the strap-down inertial navigation device, the long base-line localization device and the bathometer and outputs the navigation information of the aircraft. The strap-down inertial navigation device is corrected through the output navigation information. The system solves the problems of use of a land navigation technology under water and satisfies underwater navigation device demands of high precision and high crypticity.

Description

A kind of scale underwater vehicle combined navigation system and navigation data fusion method

Technical field

The invention belongs to integrated navigation technology field, particularly a kind of scale underwater vehicle combined navigation system and navigation data fusion method.

Background technology

Underwater navigation location is the basic premise of ocean development activity and Marine High-technology development, ocean development needs obtain on a large scale, high-precision marine environment data, need to carry out sea floor exploration, underwater survey and Underwater Engineering etc., modern naval battle also develops into gradually and relates to space, sky, land, sea, and the three-dimensional warfare in multilayer space, seabed under water, and all these needs the support that sea and underwater navigation are located.

Airmanship is one of technological challenge of facing of underwater hiding-machine.Navigational system must provide remote and long height and position, speed and attitude information.Precision navigation technology is the key of underwater hiding-machine effective operation and safe retrieving, but by the impact of the factors such as the complicacy of underwater environment, device own vol, weight, the energy and the disguise of diving, realize the navigation of high-precision underwater hiding-machine and remain a difficult task.Because electromagnetic wave is decayed greatly in water, can not long-rangely propagate, thus the air navigation aid of some maturations of land under water and inapplicable.

Summary of the invention

In order to solve the technical matters that above-mentioned background technology proposes, the present invention aims to provide a kind of scale underwater vehicle combined navigation system and navigation data fusion method, overcome land navigation technology and apply underwater defect, meet the demand of submarine navigation device high precision, highly concealed type.

In order to realize above-mentioned technical purpose, technical scheme of the present invention is:

A kind of scale underwater vehicle combined navigation system, comprise strapdown inertial navigation device, Long baselines locating device, dive instrument and information fusion module are surveyed in depth measurement, described Long baselines positioning system comprises a receiver and is arranged on multiple transponder arrays of diverse location under water, described strapdown inertial navigation device measures the position of aircraft, speed, course attitude signal also sends information fusion module to, described receiver measures the positional information of aircraft according to the positional information of each transponder array, and send information fusion module to, described depth measurement is surveyed instrument of diving and is measured the elevation information of aircraft and send information fusion module to, described information fusion module is by strapdown inertial navigation device, the information that instrument measurement of diving is surveyed in Long baselines locating device and depth measurement merges, export flight guidance information, and correct strapdown inertial navigation device with the navigation information exported.

Based on a kind of preferred version of technique scheme, a transponder array comprises 3 transponders, and these 3 transponders form equilateral triangle.

Based on a kind of preferred version of technique scheme, it is the equilateral triangle of L that 3 transponders form the length of side, and the length of side wherein, h is the depth difference of submarine navigation device and this transponder array center.

Based on a kind of preferred version of technique scheme, strapdown inertial navigation device comprises three axis accelerometer, three-axis gyroscope, signal conditioning circuit and microprocessor, described three-axis gyroscope gathers the angle information of aircraft and sends microprocessor to, described three axis accelerometer gathers the acceleration information of aircraft, and microprocessor is sent to after signal conditioning circuit process, microprocessor is according to the acceleration received and angle information, obtain the position of aircraft, speed, course attitude information, and send information fusion module to.

Based on a kind of preferred version of technique scheme, signal conditioning circuit comprises the filtering circuit, amplifying circuit and the AD converter that connect successively, thus carries out filtering, amplification and AD conversion processing successively to the information that three axis accelerometer exports.

The present invention also comprises a kind of navigation data fusion method being applied to combinations thereof navigational system, the method comprises the information fusion of strapdown inertial navigation device and Long baselines locating device and utilizes Long baselines locating device to correct strapdown inertial navigation device, and concrete steps are as follows:

(1) the misalignment error equation of strapdown inertial navigation device, velocity error equation and site error equation is listed;

(2) according to the distance between submarine navigation device and each transponder array, utilize sphere to cross method, obtain positioning equation:

||X _G-X _Ti||＝r _i(i＝1,2,…,N)

Wherein, N is transponder array sum, and i represents i-th transponder array, X _gfor aircraft position coordinates to be solved, X _tifor being anchored to the position coordinates of i-th water-bed transponder array, r _ifor the distance between aircraft and i-th transponder array;

(3) constant value drift of the misalignment error equation listed according to step (1), velocity error equation and site error equation and three-axis gyroscope and three axis accelerometer, obtains integrated navigation system state equation;

(4) positional information that the positional information measured by Long baselines locating device and strapdown inertial navigation system export is poor, as the measuring value of integrated navigation system, and instrument error equation;

(5), after the global optimum utilizing Kalman filtering algorithm to obtain strapdown inertial navigation system error state estimates, error correction is carried out to strapdown inertial navigation system.

Adopt the beneficial effect that technique scheme is brought:

The technical matters of initial alignment is re-started before launching subsystem after the present invention is directed to scale underwater vehicle combined navigation system initial alignment and long-time navigation, the advantage that can provide accurate course angle, latitude and longitude information is located in conjunction with strap-down inertial, Long baselines, scale underwater vehicle combined navigation is improved, improves the disguise of integrated navigation system and long-time navigation accuracy.The present invention corrects according to the time integral error of Long baselines locating information to strap-down inertial, and due to strapdown inertial navigation system observability degree in the vertical direction low, vertical direction elevation information exports unstable, so utilize the latent instrument of depth measurement survey to provide the elevation information of aircraft vertical direction.

Accompanying drawing explanation

Fig. 1 is the block diagram of system of the present invention.

Fig. 2 is the composition frame chart of strapdown inertial navigation device in the present invention.

Embodiment

Below with reference to accompanying drawing, technical scheme of the present invention is described in detail.

The block diagram of system of the present invention as shown in Figure 1, comprise strapdown inertial navigation device, Long baselines locating device, dive instrument and information fusion module are surveyed in depth measurement, described Long baselines positioning system comprises a receiver and is arranged on multiple transponder arrays of diverse location under water, described strapdown inertial navigation device measures the position of aircraft, speed, course attitude signal also sends information fusion module to, described receiver measures the positional information of aircraft according to the positional information of each transponder array, and send information fusion module to, described depth measurement is surveyed instrument of diving and is measured the elevation information of aircraft and send information fusion module to, described information fusion module is by strapdown inertial navigation device, the information that instrument measurement of diving is surveyed in Long baselines locating device and depth measurement merges, export flight guidance information, and correct strapdown inertial navigation device with the navigation information exported.

In the present embodiment, Long baselines locating device adopts the sphere method that crosses to realize location, and each transponder array comprises 3 transponders, and it is the equilateral triangle of L that these 3 transponders form length of sides, and the length of side wherein, h is the depth difference of submarine navigation device and this transponder array center.

In the present embodiment, as shown in Figure 2, strapdown inertial navigation device comprises three axis accelerometer, three-axis gyroscope, signal conditioning circuit and microprocessor, described three-axis gyroscope gathers the angle information of aircraft and sends microprocessor to, described three axis accelerometer gathers the acceleration information of aircraft, and microprocessor is sent to after signal conditioning circuit process, microprocessor is according to the acceleration received and angle information, obtain the position of aircraft, speed, course attitude information, and send information fusion module to.Wherein, signal conditioning circuit comprises the filtering circuit, amplifying circuit and the AD converter that connect successively, thus carries out filtering, amplification and AD conversion processing successively to the information that three axis accelerometer exports.

The present invention also comprises a kind of navigation data fusion method being applied to combinations thereof navigational system, the method comprises the information fusion of strapdown inertial navigation device and Long baselines locating device and utilizes Long baselines locating device to correct strapdown inertial navigation device, and concrete steps are as follows:

First, the misalignment error equation of strapdown inertial navigation device, velocity error equation and site error equation is listed:

${\stackrel{\·}{\mathrm{\φ}}}^n={\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n-({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×{\mathrm{\φ}}^n-{\mathrm{\ϵ}}^p$

$\mathrm{\δ}{\stackrel{\·}{V}}^n=f^n\×{\mathrm{\φ}}^n+{\▿}^p-(2{\mathrm{\δ\ω}}_{ie}+{\mathrm{\δ\ω}}_{en})\×{\stackrel{\·}{V}}^n-(2{\mathrm{\ω}}_{ie}+{\mathrm{\ω}}_{en})\×{\mathrm{\δV}}^n$

$\begin{array}{cc}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_N}& \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_E}\sec L+\frac{V_E}{R_E}\sec L\tan L\mathrm{\δ}L\end{array}$

Wherein, λ, L are respectively longitude in coordinate system and latitude; V _efor east orientation speed; V _nfor north orientation speed; it is attitude error in navigational coordinate system; the projection of terrestrial coordinate system relative inertness coordinate system angular velocity of rotation in navigational coordinate system; the navigational coordinate system relatively projection of spherical coordinate system angular velocity of rotation in navigational coordinate system; ε is gyro drift error; f ⁿthe projection of three-dimensional acceleration in navigational coordinate system; ▽ is accelerometer constant value drift error; R _nit is north orientation earth radius; R _eit is east orientation earth radius.

In Long baselines location, suppose that transponder arrays column position coordinate is for (x, y, z), submarine navigation device receives transponder signal at N number of measurement point, and carries out direction finding to it.It is (x that each measurement point utilizes navigational system and depth transducer to record bottoming coordinate _0i, y _0i, z _0i) (i=1,2 ... N).If with N point position for reference, relative position (x can be obtained _i, y _i, z _i):

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=\left[\begin{array}{c}{x}_{0i}\\ y_{0i}\\ Z_{0i}\end{array}\right]-\left[\begin{array}{c}{x}_{0N}\\ y_{0N}\\ Z_{0N}\end{array}\right]$

Relative position coordinates (the x of these points _i, y _i, z _i) can be recorded by dead reckoning or inertial navigation.Owing to being that cumulative errors is less at local Measuring of sea area, measuring accuracy is higher.

Assumed speed of sound is uniformly distributed, and sound wave is linearly propagated, and so measured ripple reaches the true bearing that orientation is exactly transponder.Now can obtain according to above equation

x-x _i＝Rcosα _i

y-y _i＝Rcosβ _i

z-z _i＝Rcosγ _i

Wherein, R is the distance that aircraft arrives transponder, α _i, β _i, γ _iit is the angle under earth coordinates between transponder orientation and each coordinate axis.Meet between three angles

cos ²α _i+cos ²β _i+cos ²γ _i＝1。

Can be obtained by above formula:

$\left\{\begin{array}{c}x-k_{1i}y=x_i-k_{1i}{y}_i\\ y-k_{2i}z=y_i-k_{2i}{z}_i\\ z-k_{3i}x=z_i-k_{3i}{x}_i\end{array}\right.$

I=1 in formula, 2 ... N, $k_{1i}=\frac{{\mathrm{cos\α}}_i}{{\mathrm{cos\β}}_i}{k}_{2i}=\frac{{\mathrm{cos\β}}_i}{{\mathrm{cos\γ}}_i}{k}_{3i}=\frac{{\mathrm{cos\γ}}_i}{{\mathrm{cos\α}}_i},$

Above formula is write as matrix form can obtain:

A _xyX _xy＝V _xy

In formula, $A_{xy}=\left[\begin{array}{c}1-k_{11}\\ 1-k_{12}\\ .\\ .\\ .\\ 1-k_{1N}\end{array}\right],V_{xy}=\left[\begin{array}{c}{x}_1-k_{11}{y}_1\\ x_2-k_{12}{y}_2\\ .\\ .\\ .\\ x_N-k_{1N}{y}_N\end{array}\right]$

So have $X_{xy}={\left(A_{xy}^T{A}_{xy}\right)}^{-1}{A}_{xy}^T{V}_{xy}$

In like manner A can be obtained _yzx _yz=V _yz

In formula, $A_{yz}=\left[\begin{array}{c}1-k_{21}\\ 1-k_{22}\\ .\\ .\\ .\\ 1-k_{2N}\end{array}\right],V_{yz}=\left[\begin{array}{c}{y}_1-k_{21}{z}_1\\ y_2-k_{22}{z}_2\\ .\\ .\\ .\\ y_N-k_{2N}{z}_N\end{array}\right]$

$X_{yz}={\left(A_{yz}^T{A}_{yz}\right)}^{-1}{A}_{yz}^T{V}_{yz}$

A _zxX _zx＝V _zx

$A_{zx}=A_{xy}=\left[\begin{array}{c}1-k_{31}\\ 1-k_{32}\\ .\\ .\\ .\\ 1-k_{3N}\end{array}\right]V_{zx}=\left[\begin{array}{c}{z}_1-k_{31}{x}_1\\ z_2-k_{32}{x}_2\\ .\\ .\\ .\\ z_N-k_{3N}{x}_N\end{array}\right]$

$X_{zx}={\left(A_{zx}^T{A}_{zx}\right)}^{-1}{A}_{zx}^T{V}_{zx}$

So the coordinate of transponder can be obtained by above-mentioned formula respectively.And then the position coordinates of N measurement point moment submarine navigation device is obtained according to formula

Long baselines location navigation is primarily of two kinds of mathematical models: sphere crosses and hyperboloid crosses.The present invention adopts sphere Convergence method.What suppose to obtain is distance between submarine navigation device and each transponder, then have following location navigation equation:

||X _G-X _Ti||＝r _i(i＝1,2,…,N)

Wherein, X _gfor aircraft position coordinates to be solved; X _tifor being anchored to the position coordinates of i-th transponder array in seabed.Suppose that calibration is complete; r _ifor the distance between aircraft and seabed transponder, total N number of array transponder.Sphere Convergence method uses range information, and it is accurate synchronous to require aircraft and seabed transponder to have.Long baselines operating distance is far away, and positioning precision is high, does not also need to measure aircraft attitude.

The misalignment error equation of SINS, velocity error equation and site error equation and gyroscope and accelerometer comprehensively can be obtained integrated navigation system state equation:

${\stackrel{\·}{X}}_k=F_k{X}_k+G_k{W}_k$

System status parameters is,

Wherein, F is Kalman filtering state-transition matrix; G is Kalman filtering gain matrix; W is stochastic error; for mathematical platform misalignment; [δ V _xδ V _yδ V _z] be velocity error; [δ L δ λ δ h] is three site errors; [ε _xε _yε _z] be three gyroscope constant value drifts; [▽ _x▽ _y▽ _z] be three accelerometer constant value drifts.

Long baselines is located the positional information that the positional information determined and strap-down inertial export poor, as the measuring value of integrated navigation system.Instrument error equation,

$Z_k=H_k{\hat{X}}_k+V_k$

Wherein, H _kfor measurement matrix; V _kfor residual vector; for state parameter vector estimated value.

Utilize Kalman filter, after obtaining global optimum's estimation of strapdown inertial navigation system error state, error correction is carried out to strap-down inertial, thus improves the navigation accuracy of whole integrated navigation system.Kalman filter is as follows:

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]\\ P_k=\[I-K_k{H}_k\]P_{k,k-1}\end{array}\right.$

Wherein, Φ is state matrix; P is systematic variance battle array; Q is system noise acoustic matrix; K is system-gain matrix; Γ represents that system noise drives matrix; R represents measurement noise serial variance battle array.

Above embodiment is only and technological thought of the present invention is described, can not limit protection scope of the present invention with this, and every technological thought proposed according to the present invention, any change that technical scheme basis is done, all falls within scope.

Claims (6)

1. a scale underwater vehicle combined navigation system, it is characterized in that: comprise strapdown inertial navigation device, Long baselines locating device, dive instrument and information fusion module are surveyed in depth measurement, described Long baselines positioning system comprises a receiver and is arranged on multiple transponder arrays of diverse location under water, described strapdown inertial navigation device measures the position of aircraft, speed, course attitude signal also sends information fusion module to, described receiver measures the positional information of aircraft according to the positional information of each transponder array, and send information fusion module to, described depth measurement is surveyed instrument of diving and is measured the elevation information of aircraft and send information fusion module to, described information fusion module is by strapdown inertial navigation device, the information that instrument measurement of diving is surveyed in Long baselines locating device and depth measurement merges, export flight guidance information, and correct strapdown inertial navigation device with the navigation information exported.

2. a kind of scale underwater vehicle combined navigation system according to claim 1, is characterized in that: a transponder array comprises 3 transponders, and these 3 transponders form equilateral triangles.

3. a kind of scale underwater vehicle combined navigation system according to claim 2, is characterized in that: it is the equilateral triangle of L that 3 transponders form the length of sides, and the length of side wherein, h is the depth difference of submarine navigation device and this transponder array center.

4. a kind of scale underwater vehicle combined navigation system according to claim 1, it is characterized in that: described strapdown inertial navigation device comprises three axis accelerometer, three-axis gyroscope, signal conditioning circuit and microprocessor, described three-axis gyroscope gathers the angle information of aircraft and sends microprocessor to, described three axis accelerometer gathers the acceleration information of aircraft, and microprocessor is sent to after signal conditioning circuit process, microprocessor is according to the acceleration received and angle information, obtain the position of aircraft, speed, course attitude information, and send information fusion module to.

5. a kind of scale underwater vehicle combined navigation system according to claim 4, it is characterized in that: described signal conditioning circuit comprises the filtering circuit, amplifying circuit and the AD converter that connect successively, thus filtering, amplification and AD conversion processing are carried out successively to the information that three axis accelerometer exports.

6. one kind is applied to the navigation data fusion method of claim 1 integrated navigation system, it is characterized in that: the method comprises the information fusion of strapdown inertial navigation device and Long baselines locating device and utilizes Long baselines locating device to correct strapdown inertial navigation device, and concrete steps are as follows:

(1) the misalignment error equation of strapdown inertial navigation device, velocity error equation and site error equation is listed;

(2) according to the distance between submarine navigation device and each transponder array, utilize sphere to cross method, obtain positioning equation:

||X _G-X _Ti||＝r _i(i＝1,2,…,N)

Wherein, N is transponder array sum, and i represents i-th transponder array, X _gfor aircraft position coordinates to be solved, X _tifor being anchored to the position coordinates of i-th water-bed transponder array, r _ifor the distance between aircraft and i-th transponder array;

(3) constant value drift of the misalignment error equation listed according to step (1), velocity error equation and site error equation and three-axis gyroscope and three axis accelerometer, obtains integrated navigation system state equation;

(4) positional information that the positional information measured by Long baselines locating device and strapdown inertial navigation system export is poor, as the measuring value of integrated navigation system, and instrument error equation;

(5), after the global optimum utilizing Kalman filtering algorithm to obtain strapdown inertial navigation system error state estimates, error correction is carried out to strapdown inertial navigation system.