[go: up one dir, main page]

CN110779519B - A single beacon localization method for underwater vehicle with global convergence - Google Patents

A single beacon localization method for underwater vehicle with global convergence Download PDF

Info

Publication number
CN110779519B
CN110779519B CN201911129169.6A CN201911129169A CN110779519B CN 110779519 B CN110779519 B CN 110779519B CN 201911129169 A CN201911129169 A CN 201911129169A CN 110779519 B CN110779519 B CN 110779519B
Authority
CN
China
Prior art keywords
observation
underwater
underwater vehicle
model
noise
Prior art date
Legal status (The legal status 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 status listed.)
Expired - Fee Related
Application number
CN201911129169.6A
Other languages
Chinese (zh)
Other versions
CN110779519A (en
Inventor
朱仲本
余相
秦洪德
邓忠超
万磊
田瑞菊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201911129169.6A priority Critical patent/CN110779519B/en
Publication of CN110779519A publication Critical patent/CN110779519A/en
Application granted granted Critical
Publication of CN110779519B publication Critical patent/CN110779519B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

本发明涉及水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；本发明通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；在获得的随体坐标系下水下航行器与水相对速度以及航行器姿态航向时，进行Kalman滤波预测；在获得海流速度观测、深度计观测、水声信号传递时间时，分别通过Kalman滤波进行海流速度、深度、水声信号传递时间更新。在满足定位模型可观测的前提下，本方法具有全局指数收敛性。

Figure 201911129169

The invention relates to the technical field of underwater positioning, in particular to a single beacon positioning method for an underwater vehicle. A single beacon positioning method for an underwater vehicle with global convergence, the underwater vehicle is equipped with a hydrophone, a Doppler speedometer, a depth gauge, an attitude and heading reference system and a GPS; the underwater acoustic beacon broadcasts periodically Underwater acoustic signal; the present invention transforms the discrete state nonlinear single beacon positioning model into a linear time-varying model through state augmentation; in the obtained satellite coordinate system, the relative speed between the underwater vehicle and the water and the attitude and heading of the vehicle are , carry out Kalman filtering prediction; when the current velocity observation, depth gauge observation, and underwater acoustic signal transmission time are obtained, Kalman filtering is used to update the current speed, depth, and underwater acoustic signal transmission time respectively. Under the premise of satisfying the observability of the positioning model, this method has global exponential convergence.

Figure 201911129169

Description

Underwater vehicle single beacon positioning method with global convergence
Technical Field
The invention relates to the technical field of underwater positioning, in particular to a single beacon positioning method of an underwater vehicle.
Background
Accurate position feedback is the basis for an underwater vehicle to accomplish a given underwater task. Because the underwater electromagnetic wave signal is attenuated quickly, the GNSS system widely applied to land and sky positioning cannot be applied underwater. The existing mainstream underwater positioning methods include dead reckoning methods represented by inertial navigation and underwater acoustic positioning methods represented by long baseline positioning. The inertial navigation equipment often generates large accumulated errors along with the increase of time, and cannot be used for underwater positioning for a long time, and the high-precision inertial navigation equipment has extremely high cost, so that the application of the high-precision inertial navigation equipment in an underwater vehicle is limited. The existing mainstream underwater acoustic positioning modes comprise long baseline positioning, ultra-short baseline positioning, single beacon positioning and the like. Both long baseline positioning and ultra-short baseline positioning are mature, but the cost is usually high, and the real-time performance is usually poor, which limits the application of the positioning in underwater vehicles. The emerging underwater single beacon positioning system integrates the dead reckoning data and the ranging information of the single underwater sound beacon, and has great advantages in the aspects of positioning cost and real-time performance. The existing single beacon positioning system takes underwater sound signal transmission time as observation, and obtains geographic slant distance between a beacon and an underwater vehicle as an observation variable on the premise that the underwater sound velocity is known. In practical application, however, the underwater acoustic sound velocity is influenced by factors such as underwater temperature, salinity and density, generally, the underwater acoustic sound velocity is time-varying unknown, and the accurate underwater acoustic sound velocity is difficult to obtain, so that an underwater acoustic ranging error is caused, and the performance of a single-beacon positioning system is influenced; in addition, because the observation equation of the geographic range of inclination is nonlinear, the current underwater single beacon positioning system usually adopts nonlinear Kalman filtering or particle filtering to perform position calculation, and these nonlinear filtering methods only have local convergence, so that under the condition that the initial position error of the underwater vehicle or the position error at a certain moment is large (for example, in the underwater navigation process of the vehicle, the acoustic positioning assistance is lost due to the badness of the underwater environment within a certain time period, a large accumulated error is generated only by dead reckoning of the vehicle, and a large position error occurs after the acoustic positioning assistance is recovered), the convergence of subsequent filtering is difficult to ensure, which also affects the practical application of the existing underwater single beacon positioning system.
Disclosure of Invention
The purpose of the invention is: aiming at the problems of unknown underwater sound velocity, large initial position error of an underwater vehicle or large position error at a certain moment and the like in underwater single beacon positioning, the underwater vehicle single beacon positioning method with global convergence is provided based on a state augmentation method.
The technical scheme of the invention is as follows: an underwater vehicle single beacon positioning method with global convergence is disclosed, wherein the underwater vehicle is provided with a hydrophone, a Doppler velocimeter, a depth meter, an attitude heading reference system and a GPS; the underwater sound beacon broadcasts the underwater sound signal periodically; the method comprises the following steps:
A. establishing an underwater local inertia coordinate system by taking any point in a positioning area as an origin and setting the east, north and sky directions as x, y and z axes respectively;
B. acquiring an initial position of the underwater vehicle in an underwater local inertial coordinate system through a carried GPS;
C. establishing a kinematics model and an observation model of an underwater vehicle, carrying out discretization, and establishing a nonlinear single beacon positioning model;
D. converting a discrete-state nonlinear single beacon positioning model into a linear time-varying model through state augmentation;
E. when the underwater vehicle receives the relative speed of the underwater vehicle and water under a satellite coordinate system obtained by a Doppler velocimeter and the vehicle attitude heading obtained by an attitude heading reference system, Kalman filtering prediction is carried out;
F. when the underwater vehicle receives an absolute speed observation measured by a Doppler velocimeter, the underwater vehicle calculates the self ground speed under an underwater local inertial coordinate system by combining an attitude heading angle of the underwater vehicle obtained by an attitude heading reference system so as to obtain a current speed observation, and updates the current speed through Kalman filtering;
G. when the underwater vehicle receives the observation of the depth meter, depth updating is carried out through Kalman filtering;
H. after the underwater vehicle receives the underwater sound signals, the underwater sound signal transmission time is obtained through the known underwater sound signal emission time, the square of the underwater sound signal transmission time is used as an observation variable, and the underwater sound signal transmission time is updated through Kalman filtering.
On the basis of the above scheme, specifically, in the step C, the method for establishing the kinematic model includes:
the position vector is defined as:
p＝[x y z]T
wherein: x, y and z are space position coordinates of the underwater vehicle in an underwater local inertia coordinate system;
defining the ocean current velocity vector as:
vc＝[vcx vcy vcz]T
wherein: v. ofcx，vcy，vczThe method comprises the following steps of (1) obtaining unknown ocean current velocities in x, y and z directions in an underwater local inertia coordinate system;
defining the underwater vehicle to water velocity vector as:
vw＝[vwx vwy vwz]T
wherein: v. ofwx，vwy，vwzThe relative speeds of the underwater vehicle and the water in the directions of x, y and z in an underwater local inertia coordinate system are respectively obtained by calculation through data measured by an attitude heading reference system and a Doppler velocimeter, and the calculation formula is as follows:
Figure GDA0002303477560000031
wherein:
Figure GDA0002303477560000032
the relative velocity vector of the underwater vehicle and the water under the satellite coordinate system measured by the Doppler velocimeter,
Figure GDA0002303477560000033
the matrix elements of the rotation matrix are related to the attitude angle and the heading angle of the underwater vehicle measured by the attitude heading reference system;
Figure GDA0002303477560000034
the calculation formula of (2) is as follows:
Figure GDA0002303477560000035
wherein:
Figure GDA0002303477560000036
respectively measuring a roll angle, a pitch angle and a course angle of the underwater vehicle by an attitude course reference system;
note veIs an unknown effective acoustic velocity underwater;
solving for unknown p, vcAnd veAnd taking into account the corresponding uncertainty, obtaining a kinematic model of the underwater vehicle:
Figure GDA0002303477560000037
Figure GDA0002303477560000038
Figure GDA0002303477560000039
wherein: omegap＝[ωpx ωpy ωpz]TIs the position uncertainty, omega, of the underwater vehicle in the x, y and z directionsc＝[ωcx ωcy ωcz]TIs the uncertainty of the ocean current in the x, y and z directions; omegaeIs the effective sound speed uncertainty.
On the basis of the above scheme, specifically, in the step C, the method for establishing the observation model includes:
s1, establishing an observation model of underwater acoustic signal transmission time;
recording the time T of the underwater acoustic beacon for transmitting the underwater acoustic signaleWithout loss of generality, record the underwater acoustic beacon atThe space position coordinate in the underwater local inertia coordinate system is s ═ 000]TThe time when the underwater vehicle receives the underwater acoustic signal is Ta，TeAnd TaAre all known quantities, and the observation equation is:
Figure GDA00023034775600000311
wherein: v. oftCorresponding observation noise;
s2, establishing an ocean current flow velocity observation model;
according to the absolute speed of the underwater vehicle under the satellite coordinate system measured by the Doppler velocimeter
Figure GDA00023034775600000312
And (3) calculating to obtain the representation of the absolute speed of the underwater vehicle under an underwater local inertial coordinate system by combining the attitude and the heading of the underwater vehicle measured by the attitude and heading reference system:
Figure GDA0002303477560000041
wherein: v. ofg＝[vgx vgy vgz]TThe method comprises the following steps of (1) obtaining components of the absolute speed of an underwater vehicle in x, y and z directions under a local inertial coordinate system;
according to vgAnd vwAnd calculating to obtain the observed quantity of the ocean current velocity as follows:
mc＝vg-vw
wherein: m isc＝[mcx mcy mcz]TRepresenting the observation of ocean currents in three directions;
the ocean current observation equation is linear and satisfies mc＝vc+vvc；
Wherein: v. ofvcObserving the noise vector, v, for the ocean currentsvc＝[vvcx vvcy vvcz]TWherein v isvcxCurrent in the x directionObserving noise; v. ofvcyObserving noise for the ocean current in the y direction; v. ofvczObserving noise for the ocean current in the z direction;
s3, establishing a depth observation model;
recording the observed quantity of a depth gauge carried by an underwater vehicle as mzThen its observation equation is
mz＝ap+vz
Wherein: a ═ 001]，vzNoise was observed for the depth gauge.
On the basis of the above scheme, specifically, in the step C, the discretization method of the kinematic model and the observation model includes:
s1, discretizing a kinematic model;
taking the variable plus subscript k as a discrete time index, taking Delta T as a discrete interval, and discretizing a kinematic model as follows:
pk+1＝pk+ΔTvc,k+ΔTvw,k+ωp,k
vc,k+1＝vc,k+ωc,k
ve,k+1＝ve,k+ωe,k
wherein: omegap,k,ωc,k,ωe,kRepresenting process noise in discrete states;
s2, discretizing an observation model;
assuming that the underwater vehicle receives the underwater sound signal at the moment k, the discrete underwater sound signal transmission time observation equation is as follows:
Figure GDA0002303477560000042
wherein v ist,kObserving noise for underwater sound signal transfer time;
assuming that the ocean current velocity observation can be obtained at each discrete time point, the observation equation after the dispersion is as follows:
mvc，k＝vc,k+vvc,k
wherein v isvc,kIs k atObserving noise by carving ocean current velocity;
also, assuming that depth gauge observations are available at each discrete time point, the post-discretization observation equation is:
mz,k＝apk+vz,k
wherein v isz,kNoise was observed for the depth gauge at time k.
On the basis of the above scheme, specifically, the step D includes:
s1, processing a kinematic model;
defining discrete state variables:
Figure GDA0002303477560000051
from the discrete-time kinematics model of the underwater vehicle, one can obtain:
x1,k+1＝x1,k+ΔTx3,kvw,k+ΔTx2,k+ω1,k
x2,k+1＝x2,k+ω2,k
x3,k+1＝x3,k+ω3,k
wherein: omega1,k,ω2,k,ω3,kRespectively corresponding process noise;
the discrete state variables are further defined:
Figure GDA0002303477560000052
obtaining:
Figure GDA0002303477560000053
x5,k+1＝x5,k+ω5,k
Figure GDA0002303477560000054
wherein: omega4,k,ω5,k,ω6,kRespectively corresponding process noise;
defining a state vector and a noise vector:
Figure GDA0002303477560000061
x is thenkThe kinematic equation of (a) is:
xk+1＝Akxk+ωk
wherein:
Figure GDA0002303477560000062
wherein: i is3Identity matrix representing three dimensions, 0m×nRepresenting a matrix with elements of 0 and dimension of m rows and n columns;
s2, processing an observation model;
from x1,k,x2,k,x3,k,x5,k,x6,kThe definition of (a) can be given as:
x2,k＝vc,kx3,k
Figure GDA0002303477560000063
Figure GDA0002303477560000064
when the underwater vehicle obtains ocean current observation mvc，kWhen the method is taken as a known quantity, and combined with an ocean current observation equation, the method can obtain the following steps:
0＝x2,k-mvc,kx3,k+vs1,k
Figure GDA0002303477560000065
Figure GDA0002303477560000066
wherein: v. ofs,1，vs,2And vs,3For the corresponding observation noise, the relationship between the observation noise of ocean current and the state of the linear augmentation model is as follows:
vs1,k＝vvc,kx3,k
Figure GDA0002303477560000077
Figure GDA0002303477560000078
likewise, from x1,k,x3,kThe definition of (a) and the depth gauge observation equation can be obtained:
0＝ax1,k-mz,kx3,k+vs4,kwherein: v. ofs,4To correspond to observed noise, and vs4,k＝vz,kx3,k；
From x4,kThe definition of (a) can be given as:
Figure GDA0002303477560000071
wherein:
Figure GDA0002303477560000072
constructing an observation vector and an observation noise vector as follows:
Figure GDA0002303477560000073
Figure GDA0002303477560000074
Figure GDA0002303477560000075
the corresponding observation equation is:
m1,k＝C1,kxk+v1,k
m2,k＝C2,kxk+v2,k
m3,k＝C3,kxk+v3,k
wherein:
Figure GDA0002303477560000076
on the basis of the above scheme, specifically, the method adopted in step E is:
recording the relative speed of the underwater vehicle and water under a satellite coordinate system obtained by the Doppler velocimeter received by the underwater vehicle at the moment k
Figure GDA0002303477560000081
And the attitude heading of the aircraft obtained by the attitude heading reference system
Figure GDA00023034775600000811
Constructing a rotation matrix
Figure GDA0002303477560000082
By
Figure GDA0002303477560000083
Obtaining the relative speed of the underwater vehicle and the water under the local inertial coordinate system, and further constructing a corresponding system matrix Ak；
The state prior estimation of the augmented linear model obtained by the prediction link of Kalman filtering is as follows:
Figure GDA0002303477560000084
Figure GDA0002303477560000085
wherein:
Figure GDA0002303477560000086
and Pk|kThe posterior state and the posterior variance at the moment k are respectively;
Figure GDA0002303477560000087
and Pk+1|kRespectively a prior state and a prior variance at the moment k + 1; qkThe covariance matrix of the process noise at the time k is a symmetric positive definite matrix, and the specific parameters of the covariance matrix are the process noise omegakThe statistical property decision of (a) can be obtained by offline modulation.
On the basis of the above scheme, specifically, the method adopted in step F is:
recording the absolute speed observation of the underwater vehicle under a satellite coordinate system measured by a Doppler velocimeter at the moment of k +1, and calculating the absolute speed of the vehicle under a local inertial coordinate system according to a vehicle attitude heading angle obtained by an attitude heading reference system:
Figure GDA0002303477560000088
according to known vw,k+1And constructing ocean current observation at the k +1 moment:
mvc,k+1＝vg,k+1-vw,k+1
according to mvc,k+1Constructing an observation matrix C1,k+1And an observation vector m1,k+1；
According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000089
Figure GDA00023034775600000810
Pk+1|k+1＝Pk+1|k-Kk+1C1,k+1Pk+1|k
wherein: kk+1Is Kalman gain; r1,k+1A noise covariance matrix related to the observation of the current velocity at the moment k +1 is a symmetric positive definite matrix, and the specific parameters of the matrix are observed by the observation noise v1,k+1The statistical property decision of (a) can be obtained by offline modulation.
On the basis of the above scheme, specifically, the method adopted in step G is:
recording the depth m of the underwater vehicle measured by a depth meter received by the underwater vehicle at the moment of k +1z,k+1Constructing an observation matrix C based thereon2,k+1And an observed variable m2,k+1(ii) a According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000091
Figure GDA0002303477560000092
Pk+1|k+1＝Pk+1|k-Kk+1C2,k+1Pk+1|k
wherein: r2,k+1A noise covariance matrix related to the depth observation at the moment of k +1, which is a symmetric positive definite matrix with specific parameters of observation noise v2,k+1The statistical property decision of (a) can be obtained by offline modulation.
On the basis of the above scheme, specifically, the method adopted in step H is:
recording the underwater sound signal received by the underwater vehicle at the moment k +1, and calculating the transmission time m of the underwater sound signal at the momentt,k+1(ii) a Constructing an observation matrix C3,k+1And an observed variable m3,k+1According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000093
Figure GDA0002303477560000094
Pk+1|k+1＝Pk+1|k-Kk+1C3,k+1Pk+1|k
wherein: r3,k+1A noise covariance matrix related to the observation of the underwater sound signal transmission time at the moment k +1 is a symmetric positive definite matrix, and the specific parameters of the matrix are observed by the observation noise v3,k+1The statistical property decision of (a) can be obtained by offline modulation.
On the basis of the above scheme, further, according to the posterior state estimation of the augmented linear model obtained in the step F, G, H, the posterior state estimation of the original nonlinear model can be calculated, and the calculation method is as follows:
Figure GDA0002303477560000095
Figure GDA0002303477560000096
Figure GDA0002303477560000097
wherein:
Figure GDA0002303477560000098
and
Figure GDA0002303477560000099
the posterior estimation of the effective sound velocity, the position and the ocean current velocity at the moment of k +1 respectively; in order to further ensure the stability of the positioning model, the estimation of the effective sound velocity is limited, that is, the following steps are selected:
Figure GDA00023034775600000910
wherein: v. ofmAnd vMThe lower bound and the upper bound of the effective sound velocity are respectively set according to the actual situation; sat (x, a, b) is the clipping function, whose output is:
Figure GDA0002303477560000101
has the advantages that: according to the method, through state augmentation, a nonlinear underwater acoustic signal transmission time observation model in an underwater single beacon positioning model is converted into a linear time-varying model, the linear time-varying single beacon positioning model is constructed, and position calculation is carried out through Kalman filtering. On the premise of meeting the observability of a positioning model, the method provided by the invention has global convergence. That is, when the initial position error of the underwater vehicle or the position error at a certain moment is large, the proposed method can still ensure the convergence of the positioning error.
Drawings
FIG. 1 is a flow chart of the steps of the present invention;
FIG. 2 is a comparison of the mean square positioning error obtained by the present invention compared to a conventional Extended Kalman Filtering (EKF) based underwater single beacon positioning method for position resolution;
FIG. 3 is a comparison of the mean square effective sound velocity error obtained by the underwater single beacon positioning method based on Extended Kalman Filtering (EKF) for position resolution.
Detailed Description
In embodiment 1, referring to fig. 1, an underwater vehicle single beacon positioning method with global convergence is provided, where the underwater vehicle is equipped with a hydrophone, a doppler velocimeter, a depth meter, an attitude and heading reference system, and a GPS; the underwater sound beacon broadcasts the underwater sound signal periodically; the method comprises the following steps:
A. and establishing an underwater local inertia coordinate system by taking any point in the positioning area as an origin and setting the east, north and sky directions as x, y and z axes respectively.
B. And acquiring the initial position of the underwater vehicle in an underwater local inertial coordinate system through the carried GPS.
C. And establishing a kinematics model and an observation model of the underwater vehicle, carrying out discretization, and establishing a nonlinear single beacon positioning model.
The establishment method of the kinematic model comprises the following steps:
the position vector is defined as:
p＝[x y z]T
wherein: x, y and z are space position coordinates of the underwater vehicle in an underwater local inertia coordinate system;
defining the ocean current velocity vector as:
vc＝[vcx vcy vcz]T
wherein: v. ofcx，vcy，vczThe method comprises the following steps of (1) obtaining unknown ocean current velocities in x, y and z directions in an underwater local inertia coordinate system;
defining the underwater vehicle to water velocity vector as:
vw＝[vwx vwy vwz]T
wherein: v. ofwx，vwy，vwzThe relative speeds of the underwater vehicle and the water in the directions of x, y and z in an underwater local inertia coordinate system are respectively obtained by calculation through data measured by an attitude heading reference system and a Doppler velocimeter, and the calculation formula is as follows:
Figure GDA0002303477560000111
wherein:
Figure GDA0002303477560000112
the relative velocity vector of the underwater vehicle and the water under the satellite coordinate system measured by the Doppler velocimeter,
Figure GDA0002303477560000113
the matrix elements of the rotation matrix are related to the attitude angle and the heading angle of the underwater vehicle measured by the attitude heading reference system;
Figure GDA0002303477560000114
the calculation formula of (2) is as follows:
Figure GDA0002303477560000115
wherein:
Figure GDA0002303477560000116
respectively measuring a roll angle, a pitch angle and a course angle of the underwater vehicle by an attitude course reference system;
note veIs an unknown effective acoustic velocity underwater;
solving for unknown p, vcAnd veAnd taking into account the corresponding uncertainty, obtaining a kinematic model of the underwater vehicle:
Figure GDA0002303477560000117
Figure GDA0002303477560000118
Figure GDA0002303477560000119
wherein: omegap＝[ωpx ωpy ωpz]TIs the position uncertainty, omega, of the underwater vehicle in the x, y and z directionsc＝[ωcx ωcy ωcz]TIs the uncertainty of the ocean current in the x, y and z directions; omegaeIs the effective sound speed uncertainty.
The establishment method of the observation model comprises the following steps:
s1, establishing an observation model of underwater acoustic signal transmission time;
recording the time T of the underwater acoustic beacon for transmitting the underwater acoustic signaleWithout loss of generality, the spatial position coordinate of the underwater acoustic beacon in the underwater local inertial coordinate system is recorded as s ═ 000]TThe time when the underwater vehicle receives the underwater acoustic signal is Ta，TeAnd TaAre all known quantities, and the observation equation is:
Figure GDA0002303477560000121
wherein: v. oftCorresponding observation noise;
s2, establishing an ocean current flow velocity observation model;
according to the absolute speed of the underwater vehicle under the satellite coordinate system measured by the Doppler velocimeter
Figure GDA0002303477560000122
And (3) calculating to obtain the representation of the absolute speed of the underwater vehicle under an underwater local inertial coordinate system by combining the attitude and the heading of the underwater vehicle measured by the attitude and heading reference system:
Figure GDA0002303477560000123
wherein: v. ofg＝[vgx vgy vgz]TThe method comprises the following steps of (1) obtaining components of the absolute speed of an underwater vehicle in x, y and z directions under a local inertial coordinate system;
according to vgAnd vwAnd calculating to obtain the observed quantity of the ocean current velocity as follows:
mc＝vg-vw
wherein: m isc＝[mcx mcy mcz]TRepresenting the observation of ocean currents in three directions;
the ocean current observation equation is linear and satisfies mc＝vc+vvc；
Wherein: v. ofvcObserving the noise vector, v, for the ocean currentsvc＝[vvcx vvcy vvcz]TWherein v isvcxObserving noise for the ocean current in the x direction; v. ofvcyObserving noise for the ocean current in the y direction; v. ofvczObserving noise for the ocean current in the z direction;
s3, establishing a depth observation model;
recording the observed quantity of a depth gauge carried by an underwater vehicle as mzThen its observation equation is
mz＝ap+vz
Wherein: a ═ 001]，vzNoise was observed for the depth gauge.
The discretization method of the kinematic model and the observation model comprises the following steps:
s1, discretizing a kinematic model;
taking the variable plus subscript k as a discrete time index, taking Delta T as a discrete interval, and discretizing a kinematic model as follows:
pk+1＝pk+ΔTvc,k+ΔTvw,k+ωp,k
vc,k+1＝vc,k+ωc,k
ve,k+1＝ve,k+ωe,k
wherein: omegap,k,ωc,k,ωe,kRepresenting process noise in discrete states;
s2, discretizing an observation model;
assuming that the underwater vehicle receives the underwater sound signal at the moment k, the discrete underwater sound signal transmission time observation equation is as follows:
Figure GDA0002303477560000131
wherein v ist,kObserving noise for underwater sound signal transfer time;
assuming that the ocean current velocity observation can be obtained at each discrete time point, the observation equation after the dispersion is as follows:
mvc，k＝vc,k+vvc,k
wherein v isvc,kObserving noise for the ocean current velocity at the moment k;
also, assuming that depth gauge observations are available at each discrete time point, the post-discretization observation equation is:
mz,k＝apk+vz,k
wherein v isz,kNoise was observed for the depth gauge at time k.
D. And converting the discrete-state nonlinear single beacon positioning model into a linear time-varying model through state augmentation.
S1, processing a kinematic model;
defining discrete state variables:
Figure GDA0002303477560000132
from the discrete-time kinematics model of the underwater vehicle, one can obtain:
x1,k+1＝x1,k+ΔTx3,kvw,k+ΔTx2,k+ω1,k
x2,k+1＝x2,k+ω2,k
x3,k+1＝x3,k+ω3,k
wherein: omega1,k,ω2,k,ω3,kRespectively corresponding process noise;
the discrete state variables are further defined:
Figure GDA0002303477560000133
obtaining:
Figure GDA0002303477560000134
x5,k+1＝x5,k+ω5,k
Figure GDA0002303477560000135
wherein: omega4,k,ω5,k,ω6,kRespectively corresponding process noise;
defining a state vector and a noise vector:
Figure GDA0002303477560000141
x is thenkThe kinematic equation of (a) is:
xk+1＝Akxk+ωk
wherein:
Figure GDA0002303477560000142
wherein: i is3Identity matrix representing three dimensions, 0m×nRepresenting a matrix with elements of 0 and dimension of m rows and n columns;
s2, processing an observation model;
from x1,k,x2,k,x3,k,x5,k,x6,kThe definition of (a) can be given as:
x2,k＝vc,kx3,k
Figure GDA0002303477560000143
Figure GDA0002303477560000144
when the underwater vehicle obtains ocean current observation mvc，kWhen the method is taken as a known quantity, and combined with an ocean current observation equation, the method can obtain the following steps:
0＝x2,k-mvc,kx3,k+vs1,k
Figure GDA0002303477560000145
Figure GDA0002303477560000146
wherein: v. ofs,1，vs,2And vs,3For the corresponding observation noise, the relationship between the observation noise of ocean current and the state of the linear augmentation model is as follows:
vs1,k＝vvc,kx3,k
Figure GDA0002303477560000151
Figure GDA0002303477560000152
likewise, from x1,k,x3,kThe definition of (a) and the depth gauge observation equation can be obtained:
0＝ax1,k-mz,kx3,k+vs4,k
wherein: v. ofs,4To correspond to observed noise, and vs4,k＝vz,kx3,k；
From x4,kThe definition of (a) can be given as:
Figure GDA0002303477560000153
wherein:
Figure GDA0002303477560000154
constructing an observation vector and an observation noise vector as follows:
Figure GDA0002303477560000155
Figure GDA0002303477560000156
Figure GDA0002303477560000157
the corresponding observation equation is:
m1,k＝C1,kxk+v1,k
m2,k＝C2,kxk+v2,k
m3,k＝C3,kxk+v3,k
wherein:
Figure GDA0002303477560000158
E. and when the underwater vehicle receives the relative speed of the underwater vehicle and water under the satellite coordinate system obtained by the Doppler velocimeter and the vehicle attitude heading obtained by the attitude heading reference system, Kalman filtering prediction is carried out.
Recording the relative speed of the underwater vehicle and water under a satellite coordinate system obtained by the Doppler velocimeter received by the underwater vehicle at the moment k
Figure GDA0002303477560000161
And the attitude heading of the aircraft obtained by the attitude heading reference system
Figure GDA00023034775600001611
Constructing a rotation matrix
Figure GDA0002303477560000162
By
Figure GDA0002303477560000163
Obtaining the relative speed of the underwater vehicle and the water under the local inertial coordinate system, and further constructing a corresponding system matrix Ak；
The state prior estimation of the augmented linear model obtained by the prediction link of Kalman filtering is as follows:
Figure GDA0002303477560000164
Figure GDA0002303477560000165
wherein:
Figure GDA0002303477560000166
and Pk|kThe posterior state and the posterior variance at the moment k are respectively;
Figure GDA0002303477560000167
and Pk+1|kRespectively a prior state and a prior variance at the moment k + 1; qkThe covariance matrix of the process noise at the time k is a symmetric positive definite matrix, and the specific parameters of the covariance matrix are the process noise omegakThe statistical property decision of (a) can be obtained by offline modulation.
F. When the underwater vehicle receives an absolute speed observation measured by the Doppler velocimeter, the self ground speed under an underwater local inertial coordinate system is calculated by combining an underwater vehicle attitude heading angle obtained by the attitude heading reference system, so that a current speed observation is obtained, and the current speed is updated through Kalman filtering.
Recording the absolute speed observation of the underwater vehicle under a satellite coordinate system measured by a Doppler velocimeter at the moment of k +1, and calculating the absolute speed of the vehicle under a local inertial coordinate system according to a vehicle attitude heading angle obtained by an attitude heading reference system:
Figure GDA0002303477560000168
according to known vw,k+1And constructing ocean current observation at the k +1 moment:
mvc,k+1＝vg,k+1-vw,k+1
according to mvc,k+1Constructing an observation matrix C1,k+1And an observation vector m1,k+1；
According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000169
Figure GDA00023034775600001610
Pk+1|k+1＝Pk+1|k-Kk+1C1,k+1Pk+1|k
wherein: kk+1Is Kalman gain; r1,k+1A noise covariance matrix related to the observation of the current velocity at the moment k +1 is a symmetric positive definite matrix, and the specific parameters of the matrix are observed by the observation noise v1,k+1The statistical property decision of (a) can be obtained by offline modulation.
G. And when the underwater vehicle receives the observation of the depth meter, the depth is updated through Kalman filtering.
Recording the depth m of the underwater vehicle measured by a depth meter received by the underwater vehicle at the moment of k +1z,k+1Constructing an observation matrix C based thereon2,k+1And an observed variable m2,k+1(ii) a According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000171
Figure GDA0002303477560000172
Pk+1|k+1＝Pk+1|k-Kk+1C2,k+1Pk+1|k
wherein: r2,k+1A noise covariance matrix related to the depth observation at the moment of k +1, which is a symmetric positive definite matrix with specific parameters of observation noise v2,k+1The statistical property decision of (a) can be obtained by offline modulation.
H. After the underwater vehicle receives the underwater sound signals, the underwater sound signal transmission time is obtained through the known underwater sound signal emission time, the square of the underwater sound signal transmission time is used as an observation variable, and the underwater sound signal transmission time is updated through Kalman filtering.
Recording the underwater sound signal received by the underwater vehicle at the moment k +1, and calculating the transmission time m of the underwater sound signal at the momentt,k+1(ii) a Constructing an observation matrix C3,k+1And an observed variable m3,k+1According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure GDA0002303477560000173
Figure GDA0002303477560000174
Pk+1|k+1＝Pk+1|k-Kk+1C3,k+1Pk+1|k
wherein: r3,k+1A noise covariance matrix related to the observation of the underwater sound signal transmission time at the moment k +1 is a symmetric positive definite matrix, and the specific parameters of the matrix are observed by the observation noise v3,k+1Determination of statistical characteristics ofAnd (4) modulation under the threading line.
Further, according to the posterior state estimation of the augmented linear model obtained in the step F, G, H, the posterior state estimation of the original nonlinear model can be calculated, and the calculation method is as follows:
Figure GDA0002303477560000175
Figure GDA0002303477560000176
Figure GDA0002303477560000177
wherein:
Figure GDA0002303477560000178
and
Figure GDA0002303477560000179
the posterior estimation of the effective sound velocity, the position and the ocean current velocity at the moment of k +1 respectively; in order to further ensure the stability of the positioning model, the estimation of the effective sound velocity is limited, that is, the following steps are selected:
Figure GDA00023034775600001710
wherein: v. ofmAnd vMThe lower bound and the upper bound of the effective sound velocity are respectively set according to the actual situation; sat (x, a, b) is the clipping function, whose output is:
Figure GDA0002303477560000181
example 2 the method described in example 1 was verified by simulation data.
By way of comparison, the present embodiment also shows the positioning result of the conventional underwater single beacon positioning method based on Extended Kalman Filtering (EKF) to perform position solution. The total simulation time length is 5000 seconds, the simulated effective sound velocity is 1500 m/s in the whole movement process, and the simulated ocean current velocities in the three directions are-0.1 m/s, 0.2 m/s and 0.1 m/s respectively. The simulated underwater sound signal emission period is 10 seconds, namely the transmission time updating period of the underwater sound signal is 10 seconds. The sampling periods of the simulated Doppler velocimeter, the simulated attitude heading reference system and the simulated depth meter are all 0.1 second (equivalent to 10Hz sampling frequency), namely the system discrete period, the ocean current velocity updating period and the aircraft depth updating period are all 0.1 second.
The simulated individual sensor noise parameters are as follows:
Figure GDA0002303477560000182
in the process of numerical verification, the initial parameters of the filter are set as follows: (1) the initial errors of the positions in the x direction and the y direction are both 500 meters; (2) the initial error of the position in the z direction is 50 m; (3) the initial values of the ocean currents in the x direction, the y direction and the z direction are both 0 m/s; (4) the initial value of the effective sound speed is 1450 m/s; (5) the ocean current uncertainty standard deviation is 1 m/s; (6) the uncertainty standard deviation of the water velocity observation of the aircraft is 10-4M/s; (7) the standard deviation of the uncertainty of the effective sound velocity of the proposed method is 10-7Meter/second, standard deviation of effective sound velocity uncertainty of EKF-based traditional underwater single beacon positioning system is 10-5M/s; (8) the standard deviation of the underwater acoustic signal transmission time observation noise is 10-5Second; (9) the standard deviation of ocean current observation noise is 0.002 m/s; (10) the standard deviation of the observation noise of the depth meter is 0.001 meter; (11) upper and lower bounds v of effective acoustic velocityMAnd vm1600 m/s and 1400 m/s, respectively.
The results of 500 independent sub-Monte Carlo simulations were used to verify the proposed method. The evaluation index of the positioning performance of the two positioning methods is mean square positioning error RMSΔHAnd mean square effective acoustic velocity error
Figure GDA0002303477560000183
The two calculation methods are as follows:
Figure GDA0002303477560000191
Figure GDA0002303477560000192
wherein:
Figure GDA0002303477560000193
and
Figure GDA0002303477560000194
real and estimated vehicle position coordinates in the ith Monte Carlo simulation,
Figure GDA0002303477560000195
and
Figure GDA0002303477560000196
the real and estimated effective sound velocities in the ith Monte Carlo simulation, respectively, where M-500 represents the total number of simulations. According to fig. 2 and fig. 3, it can be seen that the proposed method can converge to a smaller value faster under the condition of a larger initial position error, whereas the conventional EKF-based single beacon positioning method cannot converge and filtering divergence occurs.
Although the invention has been described in detail above with reference to a general description and specific examples, it will be apparent to one skilled in the art that modifications or improvements may be made thereto based on the invention. Accordingly, such modifications and improvements are intended to be within the scope of the invention as claimed.

Claims (7)

1.一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；其特征在于，所述方法包括以下步骤：1. A single beacon positioning method for an underwater vehicle with global convergence, the underwater vehicle is equipped with a hydrophone, a Doppler speedometer, a depth gauge, an attitude and heading reference system and a GPS; an underwater acoustic beacon period sex broadcasting underwater acoustic signal; it is characterized in that, described method comprises the following steps: A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；A. Take any point in the positioning area as the origin, and set the three directions of east, north, and sky as the x, y, and z axes, respectively, to establish the underwater local inertial coordinate system; B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；B. Obtain the initial position of the underwater vehicle in the underwater local inertial coordinate system through the onboard GPS; C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；C. Establish and discretize the kinematic model and observation model of the underwater vehicle, and build a nonlinear single beacon positioning model; 运动学模型的建立方法为：The method of establishing the kinematic model is as follows: 定义位置向量为：Define the position vector as: p＝[x y z]T p=[xyz] T 其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；Among them: x, y, z are the spatial position coordinates of the underwater vehicle in the underwater local inertial coordinate system; 定义海流速度向量为：Define the current velocity vector as: vc＝[vcx vcy vcz]T v c = [v cx v cy v cz ] T 其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；Where: v cx , v cy , v cz are the unknown current velocities in the three directions of x, y, and z in the underwater local inertial coordinate system; 定义水下航行器对水速度向量为：Define the underwater vehicle speed vector as: vw＝[vwx vwy vwz]T v w = [v wx v wy v wz ] T 其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：Among them: v wx , v wy , v wz are the relative velocities of the underwater vehicle and the water in the three directions of x, y, and z in the underwater local inertial coordinate system, respectively, measured by the attitude and heading reference system and the Doppler velocimeter The data is calculated, and the calculation formula is:
Figure FDA0002969125650000011
Figure FDA0002969125650000011
 其中：
Figure FDA0002969125650000012
为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，
Figure FDA0002969125650000013
为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；
Figure FDA0002969125650000014
的计算公式为：in:
Figure FDA0002969125650000012
is the relative velocity vector of the underwater vehicle and the water in the satellite coordinate system measured by the Doppler velocimeter,
Figure FDA0002969125650000013
is the rotation matrix that rotates with the body coordinate system to the local inertial coordinate system, and its matrix elements are related to the attitude angle and heading angle of the underwater vehicle measured by the attitude and heading reference system;
Figure FDA0002969125650000014
The calculation formula is:
Figure FDA0002969125650000015
Figure FDA0002969125650000015
 其中：
Figure FDA0002969125650000016
θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；in:
Figure FDA0002969125650000016
θ and ψ are the roll angle, pitch angle and heading angle of the underwater vehicle, respectively, which are measured by the attitude and heading reference system; 记ve为水下未知的有效声速；Let ve be the unknown effective speed of sound underwater; 求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型： Solve the time derivatives of the unknown p, vc and ve, and consider the corresponding uncertainties to obtain the kinematic model of the underwater vehicle:
Figure FDA0002969125650000021
Figure FDA0002969125650000021
Figure FDA0002969125650000022
Figure FDA0002969125650000022
Figure FDA0002969125650000023
Figure FDA0002969125650000023
 其中：ωp＝[ωpx ωpy ωpz]T为水下航行器在x，y，z方向的位置不确定性，ωc＝[ωcxωcy ωcz]T为x，y，z方向的海流不确定性；ωe为有效声速不确定性；Where: ω p =[ω px ω py ω pz ] T is the position uncertainty of the underwater vehicle in the x, y, z directions, ω c =[ω cx ω cy ω cz ] T is x, y, z The current uncertainty in the direction; ω e is the effective sound velocity uncertainty; 观测模型的建立方法为：The method of establishing the observation model is as follows: S1.建立水声信号传递时间的观测模型；S1. Establish an observation model of the transmission time of underwater acoustic signals; 记水声信标发射水声信号的时刻为Te，不失一般性，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[0 0 0]T，水下航行器接收到该水声信号的时刻为Ta，Te以及Ta均为已知量，观测方程为：Denote the moment when the underwater acoustic beacon transmits the underwater acoustic signal as T e , without loss of generality, denote the spatial position coordinate of the underwater acoustic beacon in the underwater local inertial coordinate system as s=[0 0 0] T , underwater navigation The moment when the receiver receives the underwater acoustic signal is T a , both T e and T a are known quantities, and the observation equation is:
Figure FDA0002969125650000024
Figure FDA0002969125650000024
 其中：vt为对应的观测噪声；Where: v t is the corresponding observation noise; S2.建立海流流速观测模型；S2. Establish an ocean current velocity observation model; 根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度
Figure FDA0002969125650000025
结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：The absolute speed of the underwater vehicle in the satellite coordinate system measured by the Doppler velocimeter
Figure FDA0002969125650000025
Combined with the attitude and heading of the underwater vehicle measured by the attitude and heading reference system, the representation of the absolute speed of the underwater vehicle in the underwater local inertial coordinate system is calculated:
Figure FDA0002969125650000026
Figure FDA0002969125650000026
 其中：vg＝[vgx vgy vgz]T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；Wherein: v g =[v gx v gy v gz ] T is the component of the absolute velocity of the underwater vehicle in the three directions of x, y and z in the local inertial coordinate system; 根据vg以及vw，计算得到海流速度观测量为：According to v g and v w , the observed current velocity is calculated as: mc＝vg-vw m c =v g -v w 其中：mc＝[mcx mcy mcz]T表示三个方向的海流观测量；Where: m c =[m cx m cy m cz ] T represents the ocean current observations in three directions; 海流观测方程为线性，满足mc＝vc+vvc；The ocean current observation equation is linear and satisfies m c =v c +v vc ; 其中：vvc为海流观测噪声向量，vvc＝[vvcx vvcy vvcz]T，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；Where: v vc is the current observation noise vector, v vc =[v vcx v vcy v vcz ] T , where v vcx is the current observation noise in the x direction; v vcy is the current observation noise in the y direction; v vcz is the current observation noise in the z direction Current observation noise; S3.建立深度观测模型；S3. Establish a depth observation model; 记水下航行器所搭载的深度计观测量为mz，则其观测方程为Note that the observation volume of the depth gauge carried by the underwater vehicle is m z , then its observation equation is mz＝ap+vz m z =ap+v z 其中：a＝[0 0 1]，vz为深度计观测噪声；Where: a=[0 0 1], v z is the observation noise of the depth gauge; 运动学模型以及观测模型离散化方法为：The kinematic model and the discretization method of the observation model are: S1.运动学模型离散化；S1. The kinematic model is discretized; 以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：Taking the variable plus the subscript symbol k as the discrete time index and ΔT as the discrete interval, the kinematic model is discrete as: pk+1＝pk+ΔTvc,k+ΔTvw,k+ωp,k p k+1 = p k +ΔTv c,k +ΔTv w,k +ωp ,k vc,k+1＝vc,k+ωc,k v c,k+1 =v c,k +ωc ,k ve,k+1＝ve,k+ωe,k v e,k+1 = v e,k +ω e,k 其中：ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；Where: ω p,k ,ω c,k ,ω e,k represent the process noise in discrete state; S2.观测模型离散化；S2. The observation model is discretized; 假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：Assuming that the underwater vehicle receives the underwater acoustic signal at time k, the observation equation of the discrete underwater acoustic signal transmission time is:
Figure FDA0002969125650000031
Figure FDA0002969125650000031
 其中，vt,k为水声信号传递时间观测噪声；Among them, v t,k is the observation noise of underwater acoustic signal transmission time; 假设在每一个离散时间点处均得到海流速度观测，离散后观测方程为：Assuming that the current velocity observations are obtained at each discrete time point, the observation equation after dispersion is: mvc，k＝vc,k+vvc,k m vc,k =v c,k +v vc,k 其中，vvc,k为k时刻海流速度观测噪声；Among them, vvc,k is the observation noise of the current velocity at time k; 同样，假设在每一个离散时间点处均得到深度计观测，离散后观测方程为：Similarly, assuming that the depth gauge observation is obtained at each discrete time point, the observation equation after discrete is: mz,k＝apk+vz,k m z,k =ap k +v z,k 其中，vz,k为k时刻深度计观测噪声；Among them, v z,k is the observation noise of the depth meter at time k; D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；D. Transform the discrete state nonlinear single beacon localization model into a linear time-varying model through state augmentation; E.水下航行器接收到多普勒测速仪所获得的随体坐标系下水下航行器与水相对速度以及姿态航向参考系统所获得的航行器姿态航向时，进行Kalman滤波预测；E. When the underwater vehicle receives the relative speed between the underwater vehicle and the water in the satellite coordinate system obtained by the Doppler speedometer and the attitude and heading of the aircraft obtained by the attitude and heading reference system, Kalman filtering is performed to predict; F.水下航行器接收到多普勒测速仪所测得的绝对速度观测时，结合姿态航向参考系统所获得的水下航行器姿态航向角计算水下局部惯性坐标系下自身对地速度，进而获得海流速度观测，通过Kalman滤波进行海流速度更新；F. When the underwater vehicle receives the absolute speed observation measured by the Doppler speedometer, it calculates its own ground speed in the underwater local inertial coordinate system in combination with the attitude and heading angle of the underwater vehicle obtained by the attitude and heading reference system. Then, the current velocity observation is obtained, and the current velocity is updated by Kalman filtering; G.水下航行器接收到深度计观测时，通过Kalman滤波进行深度更新；G. When the underwater vehicle receives the depth meter observation, the depth is updated through Kalman filtering; H.水下航行器收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将水声信号传递时间的平方作为观测变量，通过Kalman滤波进行水声信号传递时间更新。H. After the underwater vehicle receives the underwater acoustic signal, the transmission time of the underwater acoustic signal is obtained through the known transmission time of the underwater acoustic signal, and the square of the transmission time of the underwater acoustic signal is used as the observation variable, and the transmission time of the underwater acoustic signal is calculated by Kalman filtering. renew. 2.如权利要求1的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤D包括：2. a kind of underwater vehicle single beacon positioning method with global convergence as claimed in claim 1, is characterized in that, described step D comprises: S1.运动学模型处理；S1. Kinematics model processing; 定义离散状态变量：Define discrete state variables:
Figure FDA0002969125650000041
Figure FDA0002969125650000041
 根据离散时间的水下航行器运动学模型，得到：According to the discrete-time underwater vehicle kinematics model, we get: x1,k+1＝x1,k+ΔTx3,kvw,k+ΔTx2,k+ω1,k x 1,k+1 =x 1,k +ΔTx 3,k v w,k +ΔTx 2,k +ω 1,k x2,k+1＝x2,k+ω2,k x 2,k+1 =x 2,k +ω 2,k x3,k+1＝x3,k+ω3,k x 3,k+1 =x 3,k +ω3 ,k 其中：ω1,k,ω2,k,ω3,k分别为对应的过程噪声；Among them: ω 1,k ,ω 2,k ,ω 3,k are the corresponding process noises; 进一步定义离散状态变量：Further define discrete state variables:
Figure FDA0002969125650000042
Figure FDA0002969125650000042
 得到：get:
Figure FDA0002969125650000043
Figure FDA0002969125650000043
 x5,k+1＝x5,k+ω5,k x 5,k+1 = x 5,k +ω 5,k
Figure FDA0002969125650000044
Figure FDA0002969125650000044
 其中：ω4,k,ω5,k,ω6,k分别为对应的过程噪声；Among them: ω 4,k ,ω 5,k ,ω 6,k are the corresponding process noises respectively; 定义状态向量及噪声向量：Define the state vector and noise vector:
Figure FDA0002969125650000045
Figure FDA0002969125650000045
 则xk的运动学方程为：Then the kinematic equation of x k is: xk+1＝Akxk+ωk x k+1 =A k x k +ω k 其中：in:
Figure FDA0002969125650000051
Figure FDA0002969125650000051
 其中：I3表示三维的单位矩阵，0m×n表示元素均为0，维度为m行n列的矩阵；Among them: I 3 represents a three-dimensional unit matrix, 0 m×n represents a matrix whose elements are all 0, and the dimension is m rows and n columns; S2.观测模型处理；S2. Observation model processing; 由x1,k,x2,k,x3,k,x5,k,x6,k的定义得到：From the definitions of x 1,k ,x 2,k ,x 3,k ,x 5,k ,x 6,k : x2,k＝vc,kx3,k x 2,k = v c,k x 3,k
Figure FDA0002969125650000052
Figure FDA0002969125650000052
Figure FDA0002969125650000053
Figure FDA0002969125650000053
 当水下航行器获得海流观测mvc，k时，将其看作已知量，结合海流观测方程，得到：When the underwater vehicle obtains the current observation m vc,k , it is regarded as a known quantity, combined with the current observation equation, we get: 0＝x2,k-mvc,kx3,k+vs1,k 0=x 2,k -m vc,k x 3,k +v s1,k
Figure FDA0002969125650000054
Figure FDA0002969125650000054
Figure FDA0002969125650000055
Figure FDA0002969125650000055
 其中：vs,1，vs,2及vs,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：Among them: v s,1 , v s,2 and v s,3 are the corresponding observation noises, and the relationship between them and the current observation noise and the state of the linear augmentation model is: vs1,k＝vvc,kx3,k v s1,k =v vc,k x 3,k
Figure FDA0002969125650000056
Figure FDA0002969125650000056
Figure FDA0002969125650000057
Figure FDA0002969125650000057
 同样，由x1,k,x3,k的定义以及深度计观测方程，得到：Similarly, from the definition of x 1,k ,x 3,k and the observation equation of the depth gauge, we get: 0＝ax1,k-mz,kx3,k+vs4,k 0=ax 1,k -m z,k x 3,k +v s4,k 其中：vs,4为对应观测噪声，且vs4,k＝vz,kx3,k；Where: v s,4 is the corresponding observation noise, and v s4,k =v z,k x 3,k ; 由x4,k的定义得到：From the definition of x 4,k we get:
Figure FDA0002969125650000061
Figure FDA0002969125650000061
 其中：in:
Figure FDA0002969125650000062
Figure FDA0002969125650000062
 构造观测向量及观测噪声向量为：The constructed observation vector and observation noise vector are:
Figure FDA0002969125650000063
Figure FDA0002969125650000063
Figure FDA0002969125650000064
Figure FDA0002969125650000064
Figure FDA0002969125650000065
Figure FDA0002969125650000065
 对应的观测方程为：The corresponding observation equation is: m1,k＝C1,kxk+v1,k m 1,k =C 1,k x k +v 1,k m2,k＝C2,kxk+v2,k m 2,k =C 2,k x k +v 2,k m3,k＝C3,kxk+v3,k m 3,k =C 3,k x k +v 3,k 其中：in:
Figure FDA0002969125650000066
Figure FDA0002969125650000066
 C2,k＝[a 01×3 -mz,k 0 0 0]C 2,k =[a 0 1×3 -m z,k 0 0 0] C3,k＝[01×3 01×3 0 1 0 0]。C 3,k = [0 1×3 0 1×3 0 1 0 0]. 3.如权利要求2所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤E所采用的方法为：3. a kind of underwater vehicle single beacon positioning method with global convergence as claimed in claim 2, is characterized in that, the method that described step E adopts is: 记水下航行器在k时刻接收到多普勒测速仪所获得的随体坐标系下航行器与水相对速度
Figure FDA0002969125650000067
以及姿态航向参考系统所获得的航行器姿态航向
Figure FDA0002969125650000068
构造旋转矩阵
Figure FDA0002969125650000069
由
Figure FDA00029691256500000610
得到局部惯性坐标系下水下航行器与水的相对速度，进而构造出相应的系统矩阵Ak；Record the relative speed between the underwater vehicle and the water in the satellite coordinate system obtained by the underwater vehicle receiving the Doppler velocimeter at time k
Figure FDA0002969125650000067
and the attitude and heading of the aircraft obtained by the attitude and heading reference system
Figure FDA0002969125650000068
Construct a rotation matrix
Figure FDA0002969125650000069
Depend on
Figure FDA00029691256500000610
Obtain the relative velocity of the underwater vehicle and the water in the local inertial coordinate system, and then construct the corresponding system matrix A k ; 由Kalman滤波的预测环节，得到增广线性模型状态先验估计为：From the prediction link of Kalman filtering, the state a priori estimate of the augmented linear model is obtained as:
Figure FDA0002969125650000071
Figure FDA0002969125650000071
Figure FDA0002969125650000072
Figure FDA0002969125650000072
 其中：
Figure FDA0002969125650000073
与Pk|k分别为k时刻的后验状态和后验方差；
Figure FDA0002969125650000074
与Pk+1|k分别为k+1时刻的先验状态和先验方差；Qk为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数由过程噪声ωk的统计特性决定，通过线下调制获得。in:
Figure FDA0002969125650000073
and P k|k are the posterior state and posterior variance at time k, respectively;
Figure FDA0002969125650000074
and P k+1|k are the prior state and prior variance at time k+1, respectively; Q k is the process noise covariance matrix at time k, which is a symmetric positive definite matrix, and its specific parameters are determined by the statistical properties of the process noise ω k Decision, obtained through offline modulation. 4.如权利要求3所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤F所采用的方法为：4. a kind of underwater vehicle single beacon positioning method with global convergence as claimed in claim 3 is characterized in that, the method that described step F adopts is: 记水下航行器在k+1时刻接收到多普勒测速仪测得的随体坐标系下绝对速度观测，根据姿态航向参考系统获得的航行器姿态航向角，计算得到航行器在局部惯性坐标系下的绝对速度：Note that the underwater vehicle receives the absolute velocity observation in the satellite coordinate system measured by the Doppler speedometer at time k+1, and calculates the local inertial coordinates of the vehicle according to the attitude and heading angle of the vehicle obtained by the attitude and heading reference system. The absolute speed under the system:
Figure FDA0002969125650000075
Figure FDA0002969125650000075
 根据已知的vw,k+1，构造出k+1时刻的海流观测：According to the known v w,k+1 , the current observation at time k+1 is constructed: mvc,k+1＝vg,k+1-vw,k+1 m vc,k+1 =v g,k+1 -v w,k+1 根据mvc,k+1构造观测矩阵C1,k+1及观测向量m1,k+1；Construct observation matrix C 1,k+1 and observation vector m 1,k+1 according to m vc,k +1; 根据Kalman滤波的更新环节，得到增广线性模型状态后验估计为：According to the update link of the Kalman filter, the state posterior estimate of the augmented linear model is obtained as:
Figure FDA0002969125650000076
Figure FDA0002969125650000076
Figure FDA0002969125650000077
Figure FDA0002969125650000077
 Pk+1|k+1＝Pk+1|k-Kk+1C1,k+1Pk+1|k P k+1|k+1 =P k+1|k -K k+1 C 1,k+1 P k+1|k 其中：Kk+1为Kalman增益；R1,k+1为k+1时刻与海流速度观测相关的噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v1,k+1的统计特性决定，通过线下调制获得。Among them: K k+1 is the Kalman gain; R 1,k+1 is the noise covariance matrix related to the current velocity observation at time k+1, which is a symmetric positive definite matrix, and its specific parameters are determined by the observation noise v 1,k+1 Statistical characteristics are determined and obtained through offline modulation. 5.如权利要求4所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤G所采用的方法为：5. a kind of underwater vehicle single beacon positioning method with global convergence as claimed in claim 4, is characterized in that, the method that described step G adopts is: 记水下航行器在k+1时刻接收到深度计测得的航行器深度mz,k+1，据此构造观测矩阵C2,k+1及观测变量m2,k+1；根据Kalman滤波的更新环节，得到增广线性模型状态后验估计为：Note that the underwater vehicle receives the depth m z,k+1 of the vehicle measured by the depth gauge at time k+1, and constructs the observation matrix C 2,k+1 and the observation variable m 2,k+1 accordingly; according to Kalman In the update link of filtering, the state posterior estimate of the augmented linear model is obtained as:
Figure FDA0002969125650000078
Figure FDA0002969125650000078
Figure FDA0002969125650000079
Figure FDA0002969125650000079
 Pk+1|k+1＝Pk+1|k-Kk+1C2,k+1Pk+1|k P k+1|k+1 =P k+1|k -K k+1 C 2,k+1 P k+1|k 其中：R2,k+1为k+1时刻与深度观测相关的噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v2,k+1的统计特性决定，通过线下调制获得。Among them: R 2,k+1 is the noise covariance matrix related to the depth observation at time k+1, which is a symmetric positive definite matrix, and its specific parameters are determined by the statistical characteristics of the observation noise v 2,k+1 , obtained through offline modulation . 6.如权利要求5所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤H所采用的方法为：6. a kind of underwater vehicle single beacon positioning method with global convergence as claimed in claim 5, is characterized in that, the method that described step H adopts is: 记水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；构造观测矩阵C3,k+1及观测变量m3,k+1，根据Kalman滤波的更新环节，得到增广线性模型状态后验估计为：Note that the underwater vehicle receives the underwater acoustic signal at time k+1, and calculate the transmission time m t,k+1 of the underwater acoustic signal at this moment; construct the observation matrix C 3,k+1 and the observation variable m 3,k+ 1. According to the update link of the Kalman filter, the state posterior estimate of the augmented linear model is obtained as:
Figure FDA0002969125650000081
Figure FDA0002969125650000081
Figure FDA0002969125650000082
Figure FDA0002969125650000082
 Pk+1|k+1＝Pk+1|k-Kk+1C3,k+1Pk+1|k P k+1|k+1 =P k+1|k -K k+1 C 3,k+1 P k+1|k 其中：R3,k+1为k+1时刻与水声信号传递时间观测相关的噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v3,k+1的统计特性决定，通过线下调制获得。Among them: R 3,k+1 is the noise covariance matrix related to the observation of the underwater acoustic signal transmission time at time k+1, which is a symmetric positive definite matrix, and its specific parameters are determined by the statistical characteristics of the observation noise v 3,k+1 . Obtained offline modulation. 7.如权利要求6所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，根据所述步骤F、G、H所得到的增广线性模型后验状态估计，计算得到原始非线性模型后验状态估计，计算方法为：7. The method for positioning a single beacon of an underwater vehicle with global convergence as claimed in claim 6, characterized in that, according to the augmented linear model posterior state estimation obtained in the steps F, G, and H , the posterior state estimate of the original nonlinear model is obtained by calculation, and the calculation method is:
Figure FDA0002969125650000083
Figure FDA0002969125650000083
Figure FDA0002969125650000084
Figure FDA0002969125650000084
Figure FDA0002969125650000085
Figure FDA0002969125650000085
 其中：
Figure FDA0002969125650000086
与
Figure FDA0002969125650000087
分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：in:
Figure FDA0002969125650000086
and
Figure FDA0002969125650000087
are the posterior estimates of the effective sound speed, position and current speed at time k+1 respectively; in order to further ensure the stability of the positioning model, the estimation of the effective sound speed is limited, that is, choose:
Figure FDA0002969125650000088
Figure FDA0002969125650000088
 其中：vm与vM分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：Among them: v m and v M are the lower and upper bounds of the effective sound speed, respectively, which are set according to the actual situation; sat(x, a, b) is the limiting function, and its output is:
Figure FDA0002969125650000089
Figure FDA0002969125650000089
CN201911129169.6A 2019-11-18 2019-11-18 A single beacon localization method for underwater vehicle with global convergence Expired - Fee Related CN110779519B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911129169.6A CN110779519B (en) 2019-11-18 2019-11-18 A single beacon localization method for underwater vehicle with global convergence

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911129169.6A CN110779519B (en) 2019-11-18 2019-11-18 A single beacon localization method for underwater vehicle with global convergence

Publications (2)

Publication Number Publication Date
CN110779519A CN110779519A (en) 2020-02-11
CN110779519B true CN110779519B (en) 2021-04-27

Family

ID=69391536

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911129169.6A Expired - Fee Related CN110779519B (en) 2019-11-18 2019-11-18 A single beacon localization method for underwater vehicle with global convergence

Country Status (1)

Country Link
CN (1) CN110779519B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112285652B (en) * 2020-10-28 2022-06-07 浙江大学 Underwater glider positioning method using single beacon virtual arrival time difference
CN112698273B (en) * 2020-12-15 2022-08-02 哈尔滨工程大学 A collaborative operation method for multi-AUV single-marker ranging
CN113093092B (en) * 2021-04-01 2022-06-14 哈尔滨工程大学 Underwater robust self-adaptive single beacon positioning method
CN113324546B (en) * 2021-05-24 2022-12-13 哈尔滨工程大学 Robust filtering method for self-adaptive adjustment of multi-submersible cooperative positioning under compass failure
CN116702479B (en) * 2023-06-12 2024-02-06 哈尔滨工程大学 A method and system for unknown input and position estimation of underwater vehicles
CN118896619B (en) * 2024-10-09 2024-12-31 国网电力空间技术有限公司 Positioning method and device for airborne electromagnetic interference resistance

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106028278A (en) * 2016-05-04 2016-10-12 哈尔滨工程大学 A Distributed Underwater Network Positioning Method Based on Mobile Beacons
CN110207695A (en) * 2019-05-28 2019-09-06 哈尔滨工程大学 It is a kind of suitable for deep-sea AUV without velocity aid list beacon localization method
EP2169422B1 (en) * 2008-09-24 2019-09-11 LEONARDO S.p.A. System and method for acoustic tracking an underwater vehicle trajectory
CN110412546A (en) * 2019-06-27 2019-11-05 厦门大学 A method and system for locating underwater targets

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2169422B1 (en) * 2008-09-24 2019-09-11 LEONARDO S.p.A. System and method for acoustic tracking an underwater vehicle trajectory
CN106028278A (en) * 2016-05-04 2016-10-12 哈尔滨工程大学 A Distributed Underwater Network Positioning Method Based on Mobile Beacons
CN110207695A (en) * 2019-05-28 2019-09-06 哈尔滨工程大学 It is a kind of suitable for deep-sea AUV without velocity aid list beacon localization method
CN110412546A (en) * 2019-06-27 2019-11-05 厦门大学 A method and system for locating underwater targets

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
H. Qin Unscented Kalman Filter Based Single Beacon Underwater Localization with Unknown Effective Sound Velocity;H. Qin;《2018 OCEANS - MTS/IEEE Kobe Techno-Oceans (OTO), Kobe》;20181206;1-6 *
单信标导航精度分析与航路规划;梁国龙;《水下无人系统学报》;20190430;第27卷(第2期);181-188 *

Also Published As

Publication number Publication date
CN110779519A (en) 2020-02-11

Similar Documents

Publication Publication Date Title
CN110779518B (en) A single beacon localization method for underwater vehicle with global convergence
CN110779519B (en) A single beacon localization method for underwater vehicle with global convergence
Wang et al. Student’s t-based robust Kalman filter for a SINS/USBL integration navigation strategy
CN109459040B (en) Multi-AUV co-localization method based on RBF neural network-assisted volumetric Kalman filter
CN110794409B (en) Underwater single beacon positioning method capable of estimating unknown effective sound velocity
CN109324330B (en) USBL/SINS compact integrated navigation and positioning method based on hybrid derivative-free extended Kalman filter
CN110749891B (en) An adaptive underwater single beacon localization method for estimating unknown effective sound velocity
Song et al. Neural-network-based AUV navigation for fast-changing environments
CN110646783B (en) An underwater beacon positioning method for an underwater vehicle
CN109000642A (en) A kind of improved strong tracking volume Kalman filtering Combinated navigation method
CN105424036B (en) A kind of inexpensive underwater hiding-machine terrain aided inertia combined navigation localization method
CN108594272B (en) An integrated navigation method for anti-spoofing jamming based on robust Kalman filter
CN105823480A (en) Underwater moving target positioning algorithm based on single beacon
CN107063245B (en) SINS/DVL combined navigation filtering method based on 5-order SSRCKF
CN104316025B (en) System for estimating height of sea wave based on attitude information of ship
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN112729291A (en) SINS/DVL ocean current velocity estimation method for deep-submergence long-endurance submersible
CN106441291B (en) An integrated navigation system and navigation method based on strong tracking SDRE filter
CN111156986B (en) A Spectral Redshift Autonomous Integrated Navigation Method Based on Robust Adaptive UKF
Xu et al. Accurate two-step filtering for AUV navigation in large deep-sea environment
CN116222578A (en) Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing
Davari et al. Multirate adaptive Kalman filter for marine integrated navigation system
CN110207698B (en) Polar region grid inertial navigation/ultra-short baseline tight combination navigation method
CN113093092B (en) Underwater robust self-adaptive single beacon positioning method
CN113155134B (en) A Tracking and Prediction Method of Underwater Acoustic Channel Based on Inertial Information Aid

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210427