CN1325932C - Assembled navigation positioning method for manned submersible - Google Patents

Assembled navigation positioning method for manned submersible Download PDF

Info

Publication number
CN1325932C
CN1325932C CNB2004100875669A CN200410087566A CN1325932C CN 1325932 C CN1325932 C CN 1325932C CN B2004100875669 A CNB2004100875669 A CN B2004100875669A CN 200410087566 A CN200410087566 A CN 200410087566A CN 1325932 C CN1325932 C CN 1325932C
Authority
CN
China
Prior art keywords
underwater vehicle
manned underwater
manned
coordinate
value
Prior art date
Application number
CNB2004100875669A
Other languages
Chinese (zh)
Other versions
CN1779485A (en
Inventor
于开洋
Original Assignee
中国科学院沈阳自动化研究所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 中国科学院沈阳自动化研究所 filed Critical 中国科学院沈阳自动化研究所
Priority to CNB2004100875669A priority Critical patent/CN1325932C/en
Publication of CN1779485A publication Critical patent/CN1779485A/en
Application granted granted Critical
Publication of CN1325932C publication Critical patent/CN1325932C/en

Links

Abstract

The present invention relates to an assembled navigation positioning method for manned diving devices. When speed values of a motion sensor and a Doppler speed measuring sonar are taken to be synthesized by a first stage Kalman filter, the optimal speed value of a manned diving device is obtained; through the optimum speed value of the manned diving device which is synthesized by the Kalman filter, course angles and trim angles, the position value of the manned diving device is calculated. The position measuring value of a positioning sonar with ultra short baselines is taken; the position measuring value of the positioning sonar with ultra short baselines, and the calculated position value of the manned diving device are synthesized by a second stage Kalman filter to obtain the optimal position value of the manned diving device; a time delay ultra short baseline measurement data fusion method is adopted to realize that the past ultra short baseline is used to measure the position data of the manned diving device and the calculated position data of the existing manned diving device is corrected; the coordinates of the manned diving device in an inertial coordinate system can be determined without using a fixation acoustic beacon. The present invention can carry out the synthesized processing of the position and attitude information of the manned diving device which are obtained through the measurement of multiway 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 + Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ) ] T s
Y k + d = Y k + Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
Δ X d = Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ) ] T s
Δ Y d = Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
X ^ ( k + d | k ) = X ^ ( k | k ) + Δ X d
Y ^ ( k + d | k ) = Y ^ ( k | k ) + Δ 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, (λ 0, L 0) 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:
x ^ ( k | k - 1 ) = Φ ( k , k - 1 ) x ^ ( k - 1 | k - 1 ) - - - ( 5 )
The prediction of observed reading is estimated as:
y ^ ( k | k - 1 ) = C ( k ) x ^ ( k | k - 1 ) - - - ( 6 )
When obtaining new observation data y (k), can draw the predicated error of trying to achieve and be by (5) formula:
y ~ ( k | k - 1 ) = y ( k ) - y ^ ( k | k - 1 ) - - - ( 7 )
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:
x ^ ( k | k ) = x ^ ( k | k - 1 ) + K ( k ) y ~ ( k | k - 1 ) - - - ( 8 )
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 + Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ) T s
Y k + d = Y k + Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
Δ X d = Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ) ] T s
Δ Y d = Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
X ^ ( k + d | k ) = X ^ ( k | k ) = ΔX d
Y ^ ( k + d | k ) = Y ^ ( k | k ) + Δ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, (λ 0, L 0) 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 0, L 0).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 1, L 1), 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 (λ 0, L 0) usefulness (λ 1, L 1) 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 (λ 0, L 0) 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 + Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ] T s
Y k + d = Y s + Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
Δ X d = Σ i = 1 d [ u ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( k + i - 1 ) - v ( k + i - 1 ) sin ( k + i - 1 ) cos ψ ( k + i - 1 ) ] T s
Δ Y d = Σ i = 1 d [ u ( k + i - 1 ) sin θ ( k + i - 1 ) cos ψ ( k + i - 1 ) + v ( k + i - 1 ) cos θ ( k + i - 1 ) cos ψ ( 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:
X ^ ( k + d | k ) = X ^ ( k | k ) + Δ X d
Y ^ ( k + d | k ) = Y ^ ( k | k ) + Δ Y d - - - ( 15 ) .
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, (λ 0, L 0) 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.
CNB2004100875669A 2004-11-17 2004-11-17 Assembled navigation positioning method for manned submersible CN1325932C (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CNB2004100875669A CN1325932C (en) 2004-11-17 2004-11-17 Assembled navigation positioning method for manned submersible

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CNB2004100875669A CN1325932C (en) 2004-11-17 2004-11-17 Assembled navigation positioning method for manned submersible

Publications (2)

Publication Number Publication Date
CN1779485A CN1779485A (en) 2006-05-31
CN1325932C true CN1325932C (en) 2007-07-11

Family

ID=36769881

Family Applications (1)

Application Number Title Priority Date Filing Date
CNB2004100875669A CN1325932C (en) 2004-11-17 2004-11-17 Assembled navigation positioning method for manned submersible

Country Status (1)

Country Link
CN (1) CN1325932C (en)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101344586B (en) * 2008-08-29 2011-07-20 华南理工大学 Method and apparatus for three-dimensional multi-movement objective positioning by using multi-frequency sound wave
JP5968894B2 (en) * 2010-10-25 2016-08-10 ロッキード マーティン コーポレイションLockheed Martin Corporation Underwater vehicle position and orientation estimation based on correlated sensor data
CN102231082B (en) * 2011-04-08 2013-06-12 中国船舶重工集团公司第七○二研究所 Underwater object detection and autonomous underwater vehicle (AUV) automatic collision prevention method and system based on mini sonar
CN102288170B (en) * 2011-07-14 2013-06-05 浙江大学 Correction method of electronic compass in underwater vehicle
CN102608596B (en) * 2012-02-29 2013-06-05 北京航空航天大学 Information fusion method for airborne inertia/Doppler radar integrated navigation system
CN103376452B (en) * 2012-04-18 2014-12-10 中国科学院沈阳自动化研究所 Method for correction of underwater robot position error with single acoustic beacon
CN102636771A (en) * 2012-04-25 2012-08-15 西北工业大学 AUV (Autonomous Underwater Vehicle) underwater acoustic locating method based on double mobile beacons
CN103064422A (en) * 2012-11-23 2013-04-24 中国科学院沈阳自动化研究所 Non-cable self-control underwater navigation body hovering and controlling method
CN103033814B (en) * 2012-12-21 2014-08-13 中国船舶重工集团公司第七一○研究所 Multi-source data mixed platform and method for sonar detection positioning
CN104280024B (en) * 2013-07-05 2017-04-19 中国科学院沈阳自动化研究所 Device and method for integrated navigation of deepwater robot
CN104280026B (en) * 2013-07-08 2017-11-14 中国科学院沈阳自动化研究所 Deepwater robot Long baselines Combinated navigation method based on AUKF
CN104280025B (en) * 2013-07-08 2017-06-23 中国科学院沈阳自动化研究所 Deepwater robot ultra-short baseline Combinated navigation method based on uncented Kalman filter
CN103968830B (en) * 2014-05-14 2015-12-02 哈尔滨工程大学 Many ways guidance device during a kind of UUV approximately level tracking lash ship navigation and method
CN105628016B (en) * 2014-10-30 2018-09-21 中国科学院沈阳自动化研究所 A kind of navigation locating method based on ultra-short baseline
CN104765017B (en) * 2015-04-22 2017-03-15 国家深海基地管理中心 Manned underwater vehicle ultra short baseline locating system sea rating test method
CN105022053B (en) * 2015-07-31 2018-08-17 哈尔滨工程大学 Parametric sonar emits the real-time Kalman filter recursion modulator approach of signal
CN105093180B (en) * 2015-09-16 2018-07-20 欧舶智能科技(上海)有限公司 A kind of underwater robot location system and method
CN109085774A (en) * 2018-06-11 2018-12-25 同济大学 The underwater safety device of seabed electromechanical equipment
CN111245522A (en) * 2018-11-12 2020-06-05 北京环佳通信技术有限公司 Underwater command system

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
基于超短基线多普勒的水下机器人位置估计 邢志伟,张禹,封锡盛,机器人,第25卷第3期 2003 *
基于超短基线的缆控水下机器人动力定位 黄俊峰,邢志伟,李一平,控制工程,第9卷第6期 2002 *
水下机器人神经网络自适应逆控制 邢志伟,封锡盛,控制工程,第10卷第3期 2003 *
自治水下机 张禹,李开周,邢志伟,封锡盛,计算机仿真,第21期 2004 *
超短基线定位系统在ROV动力定位中应用的可行性研究 邢志伟,于开洋,王晓辉,机器人,第24卷第6期 2002 *
超短基线定位系统在ROV动力定位中应用的可行性研究 邢志伟,于开洋,王晓辉,机器人,第24卷第6期 2002;基于超短基线的缆控水下机器人动力定位 黄俊峰,邢志伟,李一平,控制工程,第9卷第6期 2002;基于超短基线多普勒的水下机器人位置估计 邢志伟,张禹,封锡盛,机器人,第25卷第3期 2003;水下机器人神经网络自适应逆控制 邢志伟,封锡盛,控制工程,第10卷第3期 2003;自治水下机 张禹,李开周,邢志伟,封锡盛,计算机仿真,第21期 2004 *

Also Published As

Publication number Publication date
CN1779485A (en) 2006-05-31

Similar Documents

Publication Publication Date Title
JP2018128469A (en) Systems and methods for navigating autonomous unmanned underwater vehicles
Melo et al. Survey on advances on terrain based navigation for autonomous underwater vehicles
Paull et al. AUV navigation and localization: A review
McEwen et al. Performance of an AUV navigation system at Arctic latitudes
Whitcomb et al. Advances in Doppler-based navigation of underwater robotic vehicles
Larsen Synthetic long baseline navigation of underwater vehicles
Newman et al. Pure range-only sub-sea SLAM
Ruiz et al. Concurrent mapping and localization using sidescan sonar
Durrant-Whyte et al. Localization of autonomous guided vehicles
US9223002B2 (en) System and method for determining the position of an underwater vehicle
Donovan Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter
US9372255B2 (en) Determining a position of a submersible vehicle within a body of water
Whitcomb et al. Advances in underwater robot vehicles for deep ocean exploration: Navigation, control, and survey operations
CN104316045B (en) A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water
Tan et al. Cooperative path planning for range-only localization using a single moving beacon
US7970491B2 (en) Robot localization system
CN102829777B (en) Autonomous underwater vehicle combined navigation system and method
Grenon et al. Enhancement of the inertial navigation system for the Morpheus autonomous underwater vehicles
JP3319759B2 (en) Method and apparatus for monitoring and remotely controlling a mobile unmanned underwater vehicle
EP1275012B1 (en) Methods and systems for navigating under water
Whitcomb et al. Combined Doppler/LBL based navigation of underwater vehicles
Ribas et al. Underwater SLAM for structured environments using an imaging sonar
Webster et al. Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles
CN100495066C (en) Underwater GPS positioning navigation method and system without high stable frequency scale
Jalving et al. A toolbox of aiding techniques for the HUGIN AUV integrated inertial navigation system

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
C17 Cessation of patent right
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20070711

Termination date: 20091217