CN1325932C - Combined navigation positioning method of manned submersible - Google Patents

Abstract

A manned submersible integrated navigation positioning method, take the velocity value of motion sensor and Doppler speed measuring sonar, synthesize through the first-stage Kalman filter, get the optimum manned submersible velocity value; the Kalman filter is used for comprehensively calculating the optimal speed value, the course angle and the longitudinal inclination angle of the manned submersible; taking the ultra-short baseline positioning sonar measurement position value, and integrating the ultra-short baseline positioning sonar measurement position value and the calculated manned submersible vehicle position value through a second-stage Kalman filter to obtain an optimal manned submersible vehicle position value; the method for fusing the time-delay ultra-short baseline measurement data is adopted to realize the correction of the current manned submersible position calculation data by using the past ultra-short baseline measurement manned submersible position data; and determining the coordinates of the manned submersible in an inertial coordinate system without throwing a fixed underwater sound beacon. The invention can comprehensively process the position and attitude information of the manned submersible measured by the multi-path navigation equipment to obtain the optimal navigation information.

Description

The combined navigation locating method of manned underwater vehicle

Technical field

The present invention relates to manned underwater vehicle location technology in the ocean, after specifically the signal of multiple navigator being carried out overall treatment, estimate the combined navigation locating method of the manned underwater vehicle of the most approaching level and smooth real manned underwater vehicle positional information.

Technical background

Manned underwater vehicle navigates by water in the ocean, need know own current position and attitude information in real time.At present, the function of manned underwater vehicle navigation positioning system comprises: the position, the position of manned underwater vehicle in the inertial navigation coordinate system, the attitude of manned underwater vehicle (comprise course, the degree of depth, trim, highly) of determining the relative operation lash ship of manned underwater vehicle.

Manned underwater vehicle and autonomous underwater robot (AUV) are referred to as no cable underwater robot, and present no cable underwater robot navigator fix has multiple way, and the air navigation aid of getting the multichannel navigation signal is also arranged.But they back up by simple often navigation signal, rather than carry out the fusion of signal.

Summary of the invention

The combined navigation locating method that the purpose of this invention is to provide a kind of manned underwater vehicle, the position and the attitude information of the manned underwater vehicle that the multichannel navigator is measured carry out overall treatment, obtain optimum navigation information.

To achieve these goals, technical scheme of the present invention is to operate as follows: 1) get the velocity amplitude of motion sensor and Doppler range rate measurement sonar, and comprehensive by first order Kalman filter, obtain optimum manned underwater vehicle velocity amplitude; 2) manned underwater vehicle optimal velocity value and course angle, the trim angle that is comprehensively gone out by Kalman filter calculated the manned underwater vehicle positional value; 3) get the positional value that the ultra-short baseline fixed sonar is measured, ultra-short baseline fixed sonar positional value of measuring and the manned underwater vehicle positional value of extrapolating is comprehensive by second level Kalman filter, obtain optimum manned underwater vehicle positional value; 4) adopt time-delay ultra-short baseline measurement data fusion method, realize measuring the manned underwater vehicle position data, present manned underwater vehicle position is calculated the rectification of data with ultra-short baseline in the past; 5) need not throw in fixedly acoustic marker and determine manned underwater vehicle coordinate in inertial coordinates system;

Describedly calculate that by the manned underwater vehicle optimal velocity value of extrapolating and course angle, trim angle the step of manned underwater vehicle positional value is specific as follows;

Determining the latitude and longitude coordinates of manned underwater vehicle reference position earlier, adopt fixed coordinate system (E-XYZ), wherein is initial point E with the starting point, and EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Define moving coordinate system (o-xyz) again, moving coordinate system and manned underwater vehicle connect firmly together, and the ox axle is consistent with the main axis of symmetry orientation of manned underwater vehicle, and the oy axle is consistent with auxiliary axis of symmetry orientation, and u, v represent that manned underwater vehicle is along moving coordinate system bidimensional speed; Manned underwater vehicle at next positional value prediction equation constantly is:

X _k+1＝X _k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts

Y _k+1＝Y _k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts (12)

Described θ is the course angle of manned underwater vehicle in fixed coordinate system, and ψ is the trim angle of manned underwater vehicle in fixed coordinate system, and the position of k manned underwater vehicle constantly is X _k, Y _k, the velocity amplitude in the moving coordinate system is u (k), v (k), T _sBe the position time delay;

Described course angle θ, trim angle ψ, bidimensional velocity amplitude u, v are that the aggregation of data of navigator data by Kalman filter of manned underwater vehicle obtains; The positional value of the manned underwater vehicle that wherein said ultra-short baseline fixed sonar measures is the new value of Kalman filter;

The described time-delay ultra-short baseline of step 4) measurement data fusion method is specially:

If the travel-time length of sound wave in water in the manned underwater vehicle positional information that the ultra-short baseline fixed sonar is measured is t=d*T _s, at t=k*T _sThe ultra-short baseline acoustic marker sends answer signal to the transmitting-receiving transducer of ultra-short baseline fixed sonar, the time delay t=that manned underwater vehicle transmits in water at sound wave (k+d) * T under water constantly _sReceive k manned underwater vehicle positional information X constantly _Km, Y _KmAt t=(k+d) * T _sThe location estimation formula of manned underwater vehicle is constantly:

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1){\]T}_s$

$Y_{k+d}=Y_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (13)；

Wherein, d is the time interval, T _sFor time measurement unit; K is the k time measurement; X _k, Y _kBe the k estimation of manned underwater vehicle position constantly, u, v are the two-dimensional motion speed of manned underwater vehicle at surface level, and θ is the course angle of manned underwater vehicle, and ψ is the trim angle of manned underwater vehicle; Order:

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (14)；

Then according to k moment measured value X _Km, Y _KmWith k moment estimated value X _k, Y _k, application card Kalman Filtering formula obtains k optimal estimation constantly

, the optimal estimation value that further obtains (k+d) moment manned underwater vehicle position is:

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)+\mathrm{\Δ}{X}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+\mathrm{\Δ}{Y}_d$ (15)；

X wherein, Y is every d*T _sRefresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T _sRefresh once second; The described fixing acoustic marker need not thrown in determines that manned underwater vehicle definite method of coordinate in inertial coordinates system is that stationary coordinate with manned underwater vehicle are transformed in the inertial navigation coordinate system, represents the coordinate of manned underwater vehicle with longitude and latitude.Be specially: determining fixed coordinate system (E-XYZ), wherein is initial point E with the lash ship, and EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Stationary coordinate are transformed in the inertial coordinates system, represent the coordinate of manned underwater vehicle with longitude and latitude; The latitude and longitude coordinates value calculating formula of manned underwater vehicle in inertial coordinates system is:

(16)；

Wherein, (λ ₀, L ₀) be the latitude and longitude coordinates of operation lash ship reference position, X, Y are the coordinate figure of manned underwater vehicle in fixed coordinate system, R is an earth radius.

The manned underwater vehicle combined navigation locating method that the present invention proposes is a navigation signal of choosing multichannel, then the multichannel navigation signal is carried out aggregation of data, obtain the new all excellent navigation data estimated value of the navigation signal than each road, the navigation data that estimates is promptly smoothly again more near actual value.

The present invention has following advantage:

1, the present invention proposes a kind of Combinated navigation method of manned underwater vehicle.This method is that the navigation signal on each road is undertaken comprehensively by digital filter, rather than the simple backup of navigation signal.The navigation signal of the manned underwater vehicle that this Combinated navigation method obtains forms continuous, stable navigation data, promptly smoothly has criticality again, the maximum real navigation positional value near manned underwater vehicle.

2, the present invention proposes time-delay ultra-short baseline measurement data fusion method.

3, the present invention uses a kind of method of novel definite manned underwater vehicle coordinate in inertial coordinates system, removes from the fixing trouble of acoustic marker of marine input.

Description of drawings

Fig. 1 manned underwater vehicle navigation positioning system is formed synoptic diagram.

Fig. 2 guidance system data flow structure figure.

Fig. 3 Kalman filter structural drawing.

Embodiment

Below in conjunction with accompanying drawing the present invention is described in further detail.

Manned underwater vehicle is no cable underwater vehicle, and manned underwater vehicle and operation lash ship are got in touch by underwater sound communication machine.The navigation positioning system of manned underwater vehicle by waterborne, two parts are formed under water, above water comprises: the monitor surface computing machine (contains computer data processing system, the manned underwater vehicle state information display system), the transmitting-receiving transducer of ultra-short baseline fixed sonar data handling system and ultra-short baseline fixed sonar, GPS (GPS) receiver, the system waterborne of underwater sound communication machine.The GPS receiver can be determined the coordinate of lash ship in inertial coordinates system, sends the coordinate of lash ship in inertial coordinates system to underwater manned submersible by underwater sound communication machine.The transmitting-receiving transducer of ultra-short baseline fixed sonar data handling system, ultra-short baseline fixed sonar and the acoustic marker on the underwater manned submersible are formed independent acoustic positioning system, can measure manned underwater vehicle and lash ship relative position.The underwater portion of navigation positioning system comprises: underwater navigation computing machine and various underwater navigation equipment and sensor, navigator is based on motion sensor, auxiliary with Doppler range rate measurement sonar, digital compass, optical fibre gyro, ultra-short baseline fixed sonar subsea beacon, depthometer, collision prevention and survey high sonar, inclinator etc. and form integrated navigation systems.Each road navigation data is input to the underwater navigation computing machine, and it is comprehensive to carry out optimal data by Kalman filter (for computer program of the present invention), for the manned underwater vehicle driver provides high precision, continuous navigation data.

Manned underwater vehicle navigation positioning system waterborne and under water two parts form as shown in Figure 1, wherein:

Motion sensor is an inertial navigation set, is used to measure the three-dimensional motion parameter of carrier, and 3 axial velocities of underwater vehicle and component of acceleration are provided, and yawing, rolling, 3 angular velocity components of pitching are provided.

The navigation data that described manned underwater vehicle navigation positioning system is obtained is based on motion sensor data, and is auxiliary with Doppler range rate measurement sonar, digital compass, GPS receiver, ultra-short baseline fixed sonar (the transmitting-receiving transducer and the acoustic marker that contain ultra-short baseline fixed sonar data handling system, ultra-short baseline fixed sonar), optical fibre gyro, inclinator, depthometer, collision prevention and survey navigator data such as high sonar.In order to carry out the location estimation of manned underwater vehicle accurately, the present invention screens above-mentioned data and is comprehensive.The data flow architecture figure of Fig. 2 manned underwater vehicle navigational system.

The combined navigation locating method of manned underwater vehicle of the present invention: 1) get the velocity amplitude of motion sensor and Doppler range rate measurement sonar, comprehensive by first order Kalman filter, obtain optimum manned underwater vehicle velocity amplitude; 2) manned underwater vehicle optimal velocity value and course angle, the trim angle that is comprehensively gone out by Kalman filter calculated the manned underwater vehicle positional value; 3) get the positional value that the ultra-short baseline fixed sonar is measured, ultra-short baseline fixed sonar positional value of measuring and the manned underwater vehicle positional value of extrapolating is comprehensive by second level Kalman filter, obtain optimum manned underwater vehicle positional value; 4) adopt time-delay ultra-short baseline measurement data fusion method, realize measuring the manned underwater vehicle position data, present manned underwater vehicle position is calculated the rectification of data with ultra-short baseline in the past; 5) need not throw in fixedly acoustic marker and determine manned underwater vehicle coordinate in inertial coordinates system.Be specially:

1) get the velocity amplitude of motion sensor and Doppler range rate measurement sonar, comprehensive by first order Kalman filter, obtain optimum manned underwater vehicle velocity amplitude;

The precision of the output valve of motion sensor and the latitude at place have relation, need be input to the latitude value at motion sensor place in the motion sensor.Motion sensor can be exported the three-dimensional acceleration and the velocity amplitude of manned underwater vehicle, but the precision of velocity amplitude is not very high, and the Doppler range rate measurement sonar is not because the influence of environment is always to measure the manned underwater vehicle speed data, and in some cases, the precision of its data is not fine.Need be undertaken aggregation of data to the measured velocity value of motion sensor and Doppler range rate measurement sonar by first order Kalman filter.Velocity amplitude and the horizontal course angle of manned underwater vehicle and the reckoning that trim angle one is used from manned underwater vehicle of the manned underwater vehicle after comprehensive.

2) manned underwater vehicle optimal velocity value and course angle, the trim angle that is comprehensively gone out by Kalman filter calculated the manned underwater vehicle positional value;

Manned underwater vehicle was determined reference position earlier before dead reckoning, promptly determine the latitude and longitude coordinates of manned underwater vehicle reference position, adopt fixed coordinate system (E-XYZ), be initial point E with the manned underwater vehicle starting point wherein, EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Define moving coordinate system (o-xyz) again, moving coordinate system and manned underwater vehicle connect firmly together; The ox axle is consistent with the main axis of symmetry orientation of manned underwater vehicle, and the oy axle is consistent with auxiliary axis of symmetry orientation, u, v represent the bidimensional speed (be diaxon speed) of manned underwater vehicle along moving coordinate system, the navigator data of manned underwater vehicle are by the aggregation of data of Kalman filter, can obtain the course angle θ of manned underwater vehicle, trim angle ψ, and in moving coordinate system the bidimensional velocity amplitude u of manned underwater vehicle, v; If know that the position of k manned underwater vehicle constantly is X _k, Y _k, the velocity amplitude in the moving coordinate system is u (k), v (k), and the course angle of manned underwater vehicle in fixed coordinate system is θ, and trim angle is ψ, and the position time delay is T _s, can get next position X constantly by formula (12) _K+1, Y _K+1For:

X _k+1＝X _k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts (12)

Y _k+1＝Y _k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts

Just can extrapolate the positional value of manned underwater vehicle by formula (12) in per moment.

3) get the positional value that the ultra-short baseline fixed sonar is measured, ultra-short baseline fixed sonar positional value of measuring and the manned underwater vehicle positional value of extrapolating is comprehensive by second level Kalman filter, obtain optimum manned underwater vehicle positional value;

The positional value of the manned underwater vehicle that ultra short baseline locating system is measured is as the new value of Kalman filter, the dead reckoning value of measured value and manned underwater vehicle by the comprehensive ultra-short baseline fixed sonar of Kalman filter can obtain the boat position maximum likelihood estimate of manned underwater vehicle.

The ultra-short baseline fixed sonar directly carries out position measurement can guarantee measuring error bounded, very big but it measures noise.Only come the computed position data according to formula (12), tentative data is level and smooth, but because the existence of measuring error, position estimation error presents increase tendency, and grows with time, and finally makes position estimation error be tending towards infinitely great.Ultra-short baseline fixed sonar measurement data, be fused in the reckoning of manned underwater vehicle position by Kalman filter, combine the data smoothing and the direct boundedness of measuring (underwater sound) process error of indirect measurement (reckoning value), obtained best estimation effect.

Kalman filtering is a kind of optimum linearity estimator based on iteration and recursive process, and it can resolve into three subprocess: prediction, measurement, correction.If the state equation of discrete-time system and output equation are respectively:

x(k+1)＝Ф(k+1，k)x(k)+Г(k+1，k)w(k) (2)

y(k)＝C(k)x(k)+v(k)

Wherein, Ф (k+1 k) is state-transition matrix, and C (k) is an observing matrix, w (k), and v (k) is respectively plant noise and observation noise, and has:

E[w(k)]＝0，Cov[w(k)，w(j)]＝Q(k)δ _kj，k，j≥0 (3)

E[v(k)]＝0，Cov[w(k)，w(j)]＝R(k)δ _kj，k，j≥0 (4)

Suppose according to observation data y={y (1), y (2) ..., y (k-1) } tried to achieve the k-1 optimal filtering of state x (k-1) constantly and estimated

, then do not obtaining before the new observation data y (k), according to y (k-1) try to achieve k constantly the predicted value of state x (k) be:

$\hat{x}\left(k\right|k-1)=\mathrm{\Φ}(k,k-1)\hat{x}(k-1|k-1)---\left(5\right)$

The prediction of observed reading is estimated as:

$\hat{y}\left(k\right|k-1)=C\left(k\right)\hat{x}\left(k\right|k-1)---\left(6\right)$

When obtaining new observation data y (k), can draw the predicated error of trying to achieve and be by (5) formula:

$\tilde{y}\left(k\right|k-1)=y\left(k\right)-\hat{y}\left(k\right|k-1)---\left(7\right)$

Can prove, when formula (2) (3) when satisfying, Be a white noise sequence, promptly afterwards with previous irrelevant, therefore, Just can be considered as the new value that k obtains constantly, but can be used for the status predication value Proofread and correct, then the estimated value of k moment state then becomes:

$\hat{x}\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)\tilde{y}\left(k\right|k-1)---\left(8\right)$

Wherein K (k) is the filter gain matrix, and it reaches according to variance of estimaion error, and minimum criterion determines.Expression formula is:

K(k)＝P(k|k-1)C ^T(k)[C(k)P(k|k-1)C ^T(k)+R(k)] ^-1 (9)

Wherein, P (k|k-1) is the prediction variance of estimaion error, is expressed as:

P(k|k-1)＝Ф(k，k-1)P(k-1|k-1)Ф ^T(k，k-1)+Q(k) (10)

P(k|k)＝[I-K(k)C(k)]P(k|k-1)[I-K(k)C(k)] ^T+K(k)R(k)K ^T(k) (11)

In the manned underwater vehicle navigation, in fact above-mentioned Kalman filtering algorithm can be regarded as a kind of data fusion process.The computation process of Kalman filter is referring to Fig. 3.

4) adopt time-delay ultra-short baseline measurement data fusion method, realize measuring the manned underwater vehicle position data, present manned underwater vehicle position is calculated the rectification of data with ultra-short baseline in the past;

Obtain the positional information process angle of ultra short baseline locating system from manned underwater vehicle: at first the transmitting-receiving transducer of the ultra-short baseline fixed sonar of ultra-short baseline fixed sonar periodically sends the sound pulse interrogating signal, after ultra-short baseline acoustic marker on the manned underwater vehicle was received interrogating signal, ping was replied in emission.After the transmitting-receiving transducer of short baseline location is received pulse signal, ultra-short baseline fixed sonar data handling system calculate manned underwater vehicle relatively with the positional value of lash ship, by underwater sound communication machine waterborne the positional information of the manned underwater vehicle of acquisition is sent to the underwater sound communication machine that is installed on the underwater manned submersible then.This process is because manned underwater vehicle is different with the distance of lash ship waterborne, needs the time of several seconds even tens seconds, and the manned underwater vehicle positional information of the ultra-short baseline fixed sonar measurement that therefore each manned underwater vehicle is received all will be out-of-date information.

The present invention carries out aggregation of data with the manned underwater vehicle positional value of described ultra-short baseline fixed sonar measurement and the positional value of reckoning, by the current out-of-date ultra-short baseline fixed sonar location measurement information that obtains, realization is optimized estimation to current manned underwater vehicle positional information, solves the evaluated error problem that location measurement information lags behind and causes.The measured value of ultra-short baseline fixed sonar is fused in the position reckoning of manned underwater vehicle, avoids long manned underwater vehicle position to calculate, make the manned underwater vehicle position data depart from actual position value.Specific implementation process is:

If the travel-time length of sound wave in water in the manned underwater vehicle positional information that the ultra-short baseline fixed sonar is measured is t=d*T _s, at t=k*T _sThe ultra-short baseline acoustic marker sends answer signal to the transmitting-receiving transducer of ultra-short baseline fixed sonar under water constantly, because the time delay that sound wave transmits in water, manned underwater vehicle will be at t=(k+d) * T _sJust can receive k manned underwater vehicle positional information X constantly _Km, Y _KmAt this moment, at t=(k+d) * T _sConstantly, the location estimation formula of manned underwater vehicle is:

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)T_s$

$Y_{k+d}=Y_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (13)；

Wherein, d is the time interval, T _sFor time measurement unit; K is the k time measurement; X _k, Y _kBe the k estimation of manned underwater vehicle position constantly, u, v are the two-dimensional motion speed of manned underwater vehicle at surface level, and θ is the course angle of manned underwater vehicle, and ψ is the trim angle of manned underwater vehicle.Order:

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (14)；

Then according to k moment measured value X _Km, Y _KmWith k moment estimated value X _k, Y _k, use the Kalman filtering formula of (8)～(11), obtain k optimal estimation constantly

, the optimal estimation value that then can get (k+d) moment manned underwater vehicle position is:

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)={\mathrm{\ΔX}}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+{\mathrm{\ΔY}}_d$ (15)；

Wherein, X, Y is every d*T _sRefresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T _sRefresh once second.

5) need not throw in fixedly acoustic marker and determine manned underwater vehicle coordinate in inertial coordinates system.

Manned underwater vehicle not only needs the relative position between definite manned underwater vehicle and the operation lash ship when marine navigation, also need to determine the coordinate of manned underwater vehicle in inertial coordinates system.Owing to arrange the process complexity of underwater sound basic matrix during long baseline positioning system is used, and after operation was finished, the recycle-water acoustic array was lost easily, caused very big waste.Use the ultra short baseline locating system of fixed beacon method can determine that the scope of inertial coordinate is little, so the movable scope of manned underwater vehicle is also limited.If ultra short baseline locating system is thrown in fixed beacon, the possibility of the acoustic marker of losing when the recycle-water acoustic marker is also arranged.

The present invention adopts the ultra-short baseline fixed sonar to determine the coordinate of manned underwater vehicle in inertial coordinates system, not be used in sea or seabed and throws in fixed beacon.The scope of activities of manned underwater vehicle is very big like this, and does not have the trouble of recycle-water acoustic marker.

Consider that lash ship in the manned underwater vehicle operation process is to move at random, do not have relatively-stationary reference point, manned underwater vehicle is relatively and the relative position of lash ship constantly can only to calculate each.So the present invention need be transformed into the stationary coordinate of manned underwater vehicle in the inertial navigation coordinate system, represents the coordinate of manned underwater vehicle with longitude and latitude.Be specially:

Determining fixed coordinate system (E-XYZ), wherein is initial point E with the lash ship, and EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Stationary coordinate are transformed in the inertial coordinates system, represent the coordinate of manned underwater vehicle with longitude and latitude; The latitude and longitude coordinates value calculating formula of manned underwater vehicle in inertial coordinates system is:

(16)；

Formula (16) be by the positional value of the relative operation lash ship of manned underwater vehicle ask the latitude and longitude coordinates value of manned underwater vehicle in the inertial navigation coordinate system (λ, L).Wherein, (λ ₀, L ₀) be the latitude and longitude coordinates of operation lash ship reference position, X, Y are the coordinate figure of manned underwater vehicle in fixed coordinate system, R is an earth radius.

Manned underwater vehicle in the inertial navigation coordinate system latitude and longitude coordinates (λ, computation process L):

At first receive the latitude and longitude coordinates (λ in some moment of lash ship by the GPS receiver on the lash ship ₀, L ₀).The position in this moment of lash ship is as the initial point in the fixed coordinate system, and (X Y) calculates this latitude and longitude coordinates (λ constantly of manned underwater vehicle with formula (16) to the coordinate figure of manned underwater vehicle in fixed coordinate system that is comprehensively gone out by Kalman filter ₁, L ₁), being new fixed coordinate system initial point (0,0) with this point, the while is as the initial value of manned underwater vehicle reckoning.After having determined the reckoning initial value of manned underwater vehicle, begin to carry out reckoning with the coordinate transform formula (12) of manned underwater vehicle, obtain manned underwater vehicle in new stationary coordinate coordinate figure (x, y).By formula (16) can extrapolate each constantly manned underwater vehicle latitude and longitude coordinates (λ, L).At this moment coordinate (λ ₀, L ₀) usefulness (λ ₁, L ₁) replace, (X is Y) with (x y) replaces.When manned underwater vehicle was determined initial value or reset values, manned underwater vehicle will be in static relatively state, to reduce the error that produces owing to time-delays such as ultra-short baseline and underwater sound communications.Lash ship is receiving certain constantly reliable lash ship longitude and latitude (λ ₀, L ₀) and the coordinate figure of manned underwater vehicle in fixed coordinate system (X, Y) after, just can move.

The manned underwater vehicle position measurement delay process method that the present invention proposes is equally applicable to other underwater robot, comprises autonomous underwater robot (AUV) and remote underwater robot (ROV).

Claims (7)

1. the combined navigation locating method of a manned underwater vehicle is characterized in that operating as follows: 1) get the velocity amplitude of motion sensor and Doppler range rate measurement sonar, and comprehensive by first order Kalman filter, obtain optimum manned underwater vehicle velocity amplitude; 2) manned underwater vehicle optimal velocity value and course angle, the trim angle that is comprehensively gone out by Kalman filter calculated the manned underwater vehicle positional value; 3) get the positional value that the ultra-short baseline fixed sonar is measured, ultra-short baseline fixed sonar positional value of measuring and the manned underwater vehicle positional value of extrapolating is comprehensive by second level Kalman filter, obtain optimum manned underwater vehicle positional value; 4) adopt time-delay ultra-short baseline measurement data fusion method, realize measuring the manned underwater vehicle position data, present manned underwater vehicle position is calculated the rectification of data with ultra-short baseline in the past; 5) need not throw in fixedly acoustic marker and determine manned underwater vehicle coordinate in inertial coordinates system.

2. by the combined navigation locating method of the described manned underwater vehicle of claim 1, it is characterized in that: describedly calculate that by the manned underwater vehicle optimal velocity value of extrapolating and course angle, trim angle the step of manned underwater vehicle positional value is specific as follows;

Determining the latitude and longitude coordinates of manned underwater vehicle reference position earlier, adopt fixed coordinate system (E-XYZ), wherein is initial point E with the starting point, and EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Define moving coordinate system (o-xyz) again, moving coordinate system and manned underwater vehicle connect firmly together, and the ox axle is consistent with the main axis of symmetry orientation of manned underwater vehicle, and the oy axle is consistent with auxiliary axis of symmetry orientation, and u, v represent that manned underwater vehicle is along moving coordinate system bidimensional speed; Manned underwater vehicle at next positional value prediction equation constantly is:

X _k+1＝X _k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts

Y _k+1＝Y _k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts (12)

Described θ is the course angle of manned underwater vehicle in fixed coordinate system, and ψ is the trim angle of manned underwater vehicle in fixed coordinate system, and the position of k manned underwater vehicle constantly is X _k, Y _k, the velocity amplitude in the moving coordinate system is u (k), v (k), T _sBe the position time delay.

3. by the combined navigation locating method of the described manned underwater vehicle of claim 2, it is characterized in that: described course angle θ, trim angle ψ, bidimensional velocity amplitude u, v are that the aggregation of data of navigator data by Kalman filter of manned underwater vehicle obtains.

4. by the combined navigation locating method of the described manned underwater vehicle of claim 1, it is characterized in that: the positional value of the manned underwater vehicle that wherein said ultra-short baseline fixed sonar measures is the new value of Kalman filter.

5. by the combined navigation locating method of the described manned underwater vehicle of claim 1, it is characterized in that: the described time-delay ultra-short baseline of step 4) measurement data fusion method is specially:

If the travel-time length of sound wave in water in the manned underwater vehicle positional information that the ultra-short baseline fixed sonar is measured is t=d*T _s, at t=k*T _sThe ultra-short baseline acoustic marker sends answer signal to the transmitting-receiving transducer of ultra-short baseline fixed sonar, the time delay t=that manned underwater vehicle transmits in water at sound wave (k+d) * T under water constantly _sReceive k manned underwater vehicle positional information X constantly _Km, Y _KmAt t=(k+d) * T _sThe location estimation formula of manned underwater vehicle is constantly:

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1\right]T_s$

$Y_{k+d}=Y_s+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s---\left(13\right);$

Wherein, d is the time interval, T _sFor time measurement unit; K is the k time measurement; X _k, Y _kBe the k estimation of manned underwater vehicle position constantly, u, v are the two-dimensional motion speed of manned underwater vehicle at surface level, and θ is the course angle of manned underwater vehicle, and ψ is the trim angle of manned underwater vehicle; Order:

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s---\left(14\right);$

Then according to k moment measured value X _Km, Y _KmWith k moment estimated value X _k, Y _k, application card Kalman Filtering formula obtains K optimal estimation constantly , the optimal estimation value that further obtains (k+d) moment manned underwater vehicle position is:

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)+\mathrm{\Δ}{X}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+\mathrm{\Δ}{Y}_d---\left(15\right).$

6. by the combined navigation locating method of the described manned underwater vehicle of claim 5, it is characterized in that: X wherein, Y is every d*T _sRefresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T _sRefresh once second.

7. press the combined navigation locating method of the described manned underwater vehicle of claim 1, it is characterized in that: the described fixing acoustic marker need not thrown in determines that manned underwater vehicle definite method of coordinate in inertial coordinates system is that stationary coordinate with manned underwater vehicle are transformed in the inertial navigation coordinate system, represents the coordinate of manned underwater vehicle with longitude and latitude; Be specially: determining fixed coordinate system (E-XYZ), wherein is initial point E with the lash ship, and EZ points to the earth's core, and surface level adopts the east northeast coordinate system; Stationary coordinate are transformed in the inertial coordinates system, represent the coordinate of manned underwater vehicle with longitude and latitude; The latitude and longitude coordinates value calculating formula of manned underwater vehicle in inertial coordinates system is:

Wherein, (λ ₀, L ₀) be the latitude and longitude coordinates of operation lash ship reference position, X, Y are the coordinate figure of manned underwater vehicle in fixed coordinate system, R is an earth radius.

Images