CN1325932C  Assembled navigation positioning method for manned submersible  Google Patents
Assembled navigation positioning method for manned submersible Download PDFInfo
 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
Links
 238000007500 overflow downdraw method Methods 0.000 claims abstract description 8
 238000005259 measurement Methods 0.000 claims abstract 7
 239000003550 marker Substances 0.000 claims description 16
 238000004220 aggregation Methods 0.000 claims description 6
 230000002776 aggregation Effects 0.000 claims description 6
 238000001914 filtration Methods 0.000 claims description 6
 XLYOFNOQVPJJNPUHFFFAOYSAN water Substances data:image/svg+xml;base64,PD94bWwgdmVyc2lvbj0nMS4wJyBlbmNvZGluZz0naXNvLTg4NTktMSc/Pgo8c3ZnIHZlcnNpb249JzEuMScgYmFzZVByb2ZpbGU9J2Z1bGwnCiAgICAgICAgICAgICAgeG1sbnM9J2h0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnJwogICAgICAgICAgICAgICAgICAgICAgeG1sbnM6cmRraXQ9J2h0dHA6Ly93d3cucmRraXQub3JnL3htbCcKICAgICAgICAgICAgICAgICAgICAgIHhtbG5zOnhsaW5rPSdodHRwOi8vd3d3LnczLm9yZy8xOTk5L3hsaW5rJwogICAgICAgICAgICAgICAgICB4bWw6c3BhY2U9J3ByZXNlcnZlJwp3aWR0aD0nMzAwcHgnIGhlaWdodD0nMzAwcHgnIHZpZXdCb3g9JzAgMCAzMDAgMzAwJz4KPCEtLSBFTkQgT0YgSEVBREVSIC0tPgo8cmVjdCBzdHlsZT0nb3BhY2l0eToxLjA7ZmlsbDojRkZGRkZGO3N0cm9rZTpub25lJyB3aWR0aD0nMzAwJyBoZWlnaHQ9JzMwMCcgeD0nMCcgeT0nMCc+IDwvcmVjdD4KPHRleHQgeD0nMTAwLjUwMScgeT0nMTcwJyBjbGFzcz0nYXRvbS0wJyBzdHlsZT0nZm9udC1zaXplOjQwcHg7Zm9udC1zdHlsZTpub3JtYWw7Zm9udC13ZWlnaHQ6bm9ybWFsO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTpub25lO2ZvbnQtZmFtaWx5OnNhbnMtc2VyaWY7dGV4dC1hbmNob3I6c3RhcnQ7ZmlsbDojRTg0MjM1JyA+SDwvdGV4dD4KPHRleHQgeD0nMTI2LjExNCcgeT0nMTg2JyBjbGFzcz0nYXRvbS0wJyBzdHlsZT0nZm9udC1zaXplOjI2cHg7Zm9udC1zdHlsZTpub3JtYWw7Zm9udC13ZWlnaHQ6bm9ybWFsO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTpub25lO2ZvbnQtZmFtaWx5OnNhbnMtc2VyaWY7dGV4dC1hbmNob3I6c3RhcnQ7ZmlsbDojRTg0MjM1JyA+MjwvdGV4dD4KPHRleHQgeD0nMTM4JyB5PScxNzAnIGNsYXNzPSdhdG9tLTAnIHN0eWxlPSdmb250LXNpemU6NDBweDtmb250LXN0eWxlOm5vcm1hbDtmb250LXdlaWdodDpub3JtYWw7ZmlsbC1vcGFjaXR5OjE7c3Ryb2tlOm5vbmU7Zm9udC1mYW1pbHk6c2Fucy1zZXJpZjt0ZXh0LWFuY2hvcjpzdGFydDtmaWxsOiNFODQyMzUnID5PPC90ZXh0Pgo8L3N2Zz4K data:image/svg+xml;base64,PD94bWwgdmVyc2lvbj0nMS4wJyBlbmNvZGluZz0naXNvLTg4NTktMSc/Pgo8c3ZnIHZlcnNpb249JzEuMScgYmFzZVByb2ZpbGU9J2Z1bGwnCiAgICAgICAgICAgICAgeG1sbnM9J2h0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnJwogICAgICAgICAgICAgICAgICAgICAgeG1sbnM6cmRraXQ9J2h0dHA6Ly93d3cucmRraXQub3JnL3htbCcKICAgICAgICAgICAgICAgICAgICAgIHhtbG5zOnhsaW5rPSdodHRwOi8vd3d3LnczLm9yZy8xOTk5L3hsaW5rJwogICAgICAgICAgICAgICAgICB4bWw6c3BhY2U9J3ByZXNlcnZlJwp3aWR0aD0nODVweCcgaGVpZ2h0PSc4NXB4JyB2aWV3Qm94PScwIDAgODUgODUnPgo8IS0tIEVORCBPRiBIRUFERVIgLS0+CjxyZWN0IHN0eWxlPSdvcGFjaXR5OjEuMDtmaWxsOiNGRkZGRkY7c3Ryb2tlOm5vbmUnIHdpZHRoPSc4NScgaGVpZ2h0PSc4NScgeD0nMCcgeT0nMCc+IDwvcmVjdD4KPHRleHQgeD0nMTMuMzEyOCcgeT0nNTMuNTkwOScgY2xhc3M9J2F0b20tMCcgc3R5bGU9J2ZvbnQtc2l6ZToyM3B4O2ZvbnQtc3R5bGU6bm9ybWFsO2ZvbnQtd2VpZ2h0Om5vcm1hbDtmaWxsLW9wYWNpdHk6MTtzdHJva2U6bm9uZTtmb250LWZhbWlseTpzYW5zLXNlcmlmO3RleHQtYW5jaG9yOnN0YXJ0O2ZpbGw6I0U4NDIzNScgPkg8L3RleHQ+Cjx0ZXh0IHg9JzI4LjE1NjknIHk9JzYyLjg2MzYnIGNsYXNzPSdhdG9tLTAnIHN0eWxlPSdmb250LXNpemU6MTVweDtmb250LXN0eWxlOm5vcm1hbDtmb250LXdlaWdodDpub3JtYWw7ZmlsbC1vcGFjaXR5OjE7c3Ryb2tlOm5vbmU7Zm9udC1mYW1pbHk6c2Fucy1zZXJpZjt0ZXh0LWFuY2hvcjpzdGFydDtmaWxsOiNFODQyMzUnID4yPC90ZXh0Pgo8dGV4dCB4PSczNS4wNDU1JyB5PSc1My41OTA5JyBjbGFzcz0nYXRvbS0wJyBzdHlsZT0nZm9udC1zaXplOjIzcHg7Zm9udC1zdHlsZTpub3JtYWw7Zm9udC13ZWlnaHQ6bm9ybWFsO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTpub25lO2ZvbnQtZmFtaWx5OnNhbnMtc2VyaWY7dGV4dC1hbmNob3I6c3RhcnQ7ZmlsbDojRTg0MjM1JyA+TzwvdGV4dD4KPC9zdmc+Cg== O XLYOFNOQVPJJNPUHFFFAOYSAN 0.000 claims 3
 239000011901 water Substances 0.000 description 13
 238000000034 methods Methods 0.000 description 10
 238000004891 communication Methods 0.000 description 5
 239000011159 matrix materials Substances 0.000 description 4
 230000001133 acceleration Effects 0.000 description 2
 230000000694 effects Effects 0.000 description 2
 239000003365 glass fibers Substances 0.000 description 2
 230000002265 prevention Effects 0.000 description 2
 238000007476 Maximum Likelihood Methods 0.000 description 1
 280000638271 Reference Point companies 0.000 description 1
 239000003570 air Substances 0.000 description 1
 239000000969 carriers Substances 0.000 description 1
 238000004590 computer program Methods 0.000 description 1
 238000010586 diagrams Methods 0.000 description 1
 238000005516 engineering processes Methods 0.000 description 1
 230000004927 fusion Effects 0.000 description 1
 238000007499 fusion processing Methods 0.000 description 1
 238000009499 grossing Methods 0.000 description 1
 238000005096 rolling process Methods 0.000 description 1
 230000003068 static Effects 0.000 description 1
 239000002699 waste materials Substances 0.000 description 1
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
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 ultrashort baseline fixed sonar is measured, ultrashort 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 timedelay ultrashort 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 ultrashort 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 (EXYZ), 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 (oxyz) 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
_{s}Be 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 ultrashort baseline fixed sonar measures is the new value of Kalman filter;
The described timedelay ultrashort baseline of step 4) measurement data fusion method is specially:
If the traveltime length of sound wave in water in the manned underwater vehicle positional information that the ultrashort baseline fixed sonar is measured is t=d*T
_{s}, at t=k*T
_{s}The ultrashort baseline acoustic marker sends answer signal to the transmittingreceiving transducer of ultrashort baseline fixed sonar, the time delay t=that manned underwater vehicle transmits in water at sound wave (k+d) * T under water constantly
_{s}Receive k manned underwater vehicle positional information X constantly
_{Km}, Y
_{Km}At t=(k+d) * T
_{s}The location estimation formula of manned underwater vehicle is constantly:
Wherein, d is the time interval, T
_{s}For time measurement unit; K is the k time measurement; X
_{k}, Y
_{k}Be the k estimation of manned underwater vehicle position constantly, u, v are the twodimensional 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:
Then according to k moment measured value X
_{Km}, Y
_{Km}With 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 wherein, Y is every d*T
_{s}Refresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T
_{s}Refresh 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 (EXYZ), 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 timedelay ultrashort 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 transmittingreceiving transducer of ultrashort baseline fixed sonar data handling system and ultrashort 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 transmittingreceiving transducer of ultrashort baseline fixed sonar data handling system, ultrashort 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, ultrashort 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 threedimensional 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, ultrashort baseline fixed sonar (the transmittingreceiving transducer and the acoustic marker that contain ultrashort baseline fixed sonar data handling system, ultrashort 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 abovementioned 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 ultrashort baseline fixed sonar is measured, ultrashort 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 timedelay ultrashort 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 ultrashort 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 threedimensional 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 (EXYZ), 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 (oxyz) 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+1}For:
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 ultrashort baseline fixed sonar is measured, ultrashort 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 ultrashort baseline fixed sonar of Kalman filter can obtain the boat position maximum likelihood estimate of manned underwater vehicle.
The ultrashort 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.Ultrashort 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 discretetime 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 statetransition 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 (k1) } tried to achieve the k1 optimal filtering of state x (k1) constantly and estimated
, then do not obtaining before the new observation data y (k), according to y (k1) try to achieve k constantly the predicted value of state x (k) be:
The prediction of observed reading is estimated as:
When obtaining new observation data y (k), can draw the predicated error of trying to achieve and be by (5) formula:
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:
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(kk1)C
^{T}(k)[C(k)P(kk1)C
^{T}(k)+R(k)]
^{1} (9)
Wherein, P (kk1) is the prediction variance of estimaion error, is expressed as:
P(kk1)＝Ф(k，k1)P(k1k1)Ф
^{T}(k，k1)+Q(k) (10)
P(kk)＝[IK(k)C(k)]P(kk1)[IK(k)C(k)]
^{T}+K(k)R(k)K
^{T}(k) (11)
In the manned underwater vehicle navigation, in fact abovementioned 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 timedelay ultrashort 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 ultrashort baseline in the past;
Obtain the positional information process angle of ultra short baseline locating system from manned underwater vehicle: at first the transmittingreceiving transducer of the ultrashort baseline fixed sonar of ultrashort baseline fixed sonar periodically sends the sound pulse interrogating signal, after ultrashort baseline acoustic marker on the manned underwater vehicle was received interrogating signal, ping was replied in emission.After the transmittingreceiving transducer of short baseline location is received pulse signal, ultrashort 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 ultrashort baseline fixed sonar measurement that therefore each manned underwater vehicle is received all will be outofdate information.
The present invention carries out aggregation of data with the manned underwater vehicle positional value of described ultrashort baseline fixed sonar measurement and the positional value of reckoning, by the current outofdate ultrashort 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 ultrashort 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 traveltime length of sound wave in water in the manned underwater vehicle positional information that the ultrashort baseline fixed sonar is measured is t=d*T
_{s}, at t=k*T
_{s}The ultrashort baseline acoustic marker sends answer signal to the transmittingreceiving transducer of ultrashort 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
_{s}Just can receive k manned underwater vehicle positional information X constantly
_{Km}, Y
_{Km}At this moment, at t=(k+d) * T
_{s}Constantly, the location estimation formula of manned underwater vehicle is:
Wherein, d is the time interval, T
_{s}For time measurement unit; K is the k time measurement; X
_{k}, Y
_{k}Be the k estimation of manned underwater vehicle position constantly, u, v are the twodimensional 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:
Then according to k moment measured value X
_{Km}, Y
_{Km}With 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:
Wherein, X, Y is every d*T
_{s}Refresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T
_{s}Refresh 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 recyclewater 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 recyclewater acoustic marker is also arranged.
The present invention adopts the ultrashort 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 recyclewater acoustic marker.
Consider that lash ship in the manned underwater vehicle operation process is to move at random, do not have relativelystationary 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 (EXYZ), 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 timedelays such as ultrashort 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 ultrashort baseline fixed sonar is measured, ultrashort 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 timedelay ultrashort 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 ultrashort 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 (EXYZ), 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 (oxyz) 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
_{s}Be 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 ultrashort 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 timedelay ultrashort baseline of step 4) measurement data fusion method is specially:
If the traveltime length of sound wave in water in the manned underwater vehicle positional information that the ultrashort baseline fixed sonar is measured is t=d*T
_{s}, at t=k*T
_{s}The ultrashort baseline acoustic marker sends answer signal to the transmittingreceiving transducer of ultrashort baseline fixed sonar, the time delay t=that manned underwater vehicle transmits in water at sound wave (k+d) * T under water constantly
_{s}Receive k manned underwater vehicle positional information X constantly
_{Km}, Y
_{Km}At t=(k+d) * T
_{s}The location estimation formula of manned underwater vehicle is constantly:
Wherein, d is the time interval, T
_{s}For time measurement unit; K is the k time measurement; X
_{k}, Y
_{k}Be the k estimation of manned underwater vehicle position constantly, u, v are the twodimensional 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:
Then according to k moment measured value X
_{Km}, Y
_{Km}With 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:
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
_{s}Refresh once second, and the movement velocity of remaining variables such as manned underwater vehicle and course angle are every T
_{s}Refresh 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 (EXYZ), 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.
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CNB2004100875669A CN1325932C (en)  20041117  20041117  Assembled navigation positioning method for manned submersible 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CNB2004100875669A CN1325932C (en)  20041117  20041117  Assembled navigation positioning method for manned submersible 
Publications (2)
Publication Number  Publication Date 

CN1779485A CN1779485A (en)  20060531 
CN1325932C true CN1325932C (en)  20070711 
Family
ID=36769881
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CNB2004100875669A CN1325932C (en)  20041117  20041117  Assembled navigation positioning method for manned submersible 
Country Status (1)
Country  Link 

CN (1)  CN1325932C (en) 
Families Citing this family (19)
Publication number  Priority date  Publication date  Assignee  Title 

CN101344586B (en) *  20080829  20110720  华南理工大学  Method and apparatus for threedimensional multimovement objective positioning by using multifrequency sound wave 
JP5968894B2 (en) *  20101025  20160810  ロッキード マーティン コーポレイションＬｏｃｋｈｅｅｄ Ｍａｒｔｉｎ Ｃｏｒｐｏｒａｔｉｏｎ  Underwater vehicle position and orientation estimation based on correlated sensor data 
CN102231082B (en) *  20110408  20130612  中国船舶重工集团公司第七○二研究所  Underwater object detection and autonomous underwater vehicle (AUV) automatic collision prevention method and system based on mini sonar 
CN102288170B (en) *  20110714  20130605  浙江大学  Correction method of electronic compass in underwater vehicle 
CN102608596B (en) *  20120229  20130605  北京航空航天大学  Information fusion method for airborne inertia/Doppler radar integrated navigation system 
CN103376452B (en) *  20120418  20141210  中国科学院沈阳自动化研究所  Method for correction of underwater robot position error with single acoustic beacon 
CN102636771A (en) *  20120425  20120815  西北工业大学  AUV (Autonomous Underwater Vehicle) underwater acoustic locating method based on double mobile beacons 
CN103064422A (en) *  20121123  20130424  中国科学院沈阳自动化研究所  Noncable selfcontrol underwater navigation body hovering and controlling method 
CN103033814B (en) *  20121221  20140813  中国船舶重工集团公司第七一○研究所  Multisource data mixed platform and method for sonar detection positioning 
CN104280024B (en) *  20130705  20170419  中国科学院沈阳自动化研究所  Device and method for integrated navigation of deepwater robot 
CN104280026B (en) *  20130708  20171114  中国科学院沈阳自动化研究所  Deepwater robot Long baselines Combinated navigation method based on AUKF 
CN104280025B (en) *  20130708  20170623  中国科学院沈阳自动化研究所  Deepwater robot ultrashort baseline Combinated navigation method based on uncented Kalman filter 
CN103968830B (en) *  20140514  20151202  哈尔滨工程大学  Many ways guidance device during a kind of UUV approximately level tracking lash ship navigation and method 
CN105628016B (en) *  20141030  20180921  中国科学院沈阳自动化研究所  A kind of navigation locating method based on ultrashort baseline 
CN104765017B (en) *  20150422  20170315  国家深海基地管理中心  Manned underwater vehicle ultra short baseline locating system sea rating test method 
CN105022053B (en) *  20150731  20180817  哈尔滨工程大学  Parametric sonar emits the realtime Kalman filter recursion modulator approach of signal 
CN105093180B (en) *  20150916  20180720  欧舶智能科技（上海）有限公司  A kind of underwater robot location system and method 
CN109085774A (en) *  20180611  20181225  同济大学  The underwater safety device of seabed electromechanical equipment 
CN111245522A (en) *  20181112  20200605  北京环佳通信技术有限公司  Underwater command system 

2004
 20041117 CN CNB2004100875669A patent/CN1325932C/en not_active IP Right Cessation
NonPatent Citations (6)
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)  20060531 
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 Dopplerbased navigation of underwater robotic vehicles  
Larsen  Synthetic long baseline navigation of underwater vehicles  
Newman et al.  Pure rangeonly subsea SLAM  
Ruiz et al.  Concurrent mapping and localization using sidescan sonar  
DurrantWhyte 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 rangeonly 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 singlebeacon onewaytraveltime 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 nonpayment of annual fee 
Granted publication date: 20070711 Termination date: 20091217 