CN110779519A - Underwater vehicle single beacon positioning method with global convergence - Google Patents

Underwater vehicle single beacon positioning method with global convergence Download PDF

Info

Publication number
CN110779519A
CN110779519A CN201911129169.6A CN201911129169A CN110779519A CN 110779519 A CN110779519 A CN 110779519A CN 201911129169 A CN201911129169 A CN 201911129169A CN 110779519 A CN110779519 A CN 110779519A
Authority
CN
China
Prior art keywords
underwater
observation
underwater vehicle
noise
model
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.)
Granted
Application number
CN201911129169.6A
Other languages
Chinese (zh)
Other versions
CN110779519B (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
Active 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

Abstract

The invention relates to the technical field of underwater positioning, in particular to a single beacon positioning method of an underwater vehicle. 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; according to the method, a discrete-state nonlinear single beacon positioning model is converted into a linear time-varying model through state augmentation; performing Kalman filtering prediction on the relative speed of the underwater vehicle and water and the attitude and heading of the vehicle under the obtained satellite coordinate system; and when obtaining the ocean current velocity observation, the depth meter observation and the underwater sound signal transmission time, respectively updating the ocean current velocity, the depth and the underwater sound signal transmission time through Kalman filtering. On the premise of meeting the observability of a positioning model, the method has global exponential convergence.

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:
v c＝[v cxv cyv cz] T
wherein: v. of cx，v cy，v czThe 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:
v w＝[v wxv wyv wz] T
wherein: v. of wx，v wy，v wzThe 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 RE-GDA0002303477560000031
wherein:
Figure RE-GDA0002303477560000032
the relative velocity vector of the underwater vehicle and the water under the satellite coordinate system measured by the Doppler velocimeter,
Figure RE-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 RE-GDA0002303477560000034
the calculation formula of (2) is as follows:
Figure RE-GDA0002303477560000035
wherein: respectively measuring a roll angle, a pitch angle and a course angle of the underwater vehicle by an attitude course reference system;
note v eIs an unknown effective acoustic velocity underwater;
solving for unknown p, v cAnd v eAnd taking into account the corresponding uncertainty, obtaining a kinematic model of the underwater vehicle:
Figure RE-GDA0002303477560000037
Figure RE-GDA0002303477560000038
wherein:
Figure RE-GDA00023034775600000310
is the position uncertainty, omega, of the underwater vehicle in the x, y and z directions c＝[ω cxω cyω cz] TIs the uncertainty of the ocean current in the x, y and z directions; omega eIs 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 signal eWithout 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 T a，T eAnd T aAre all known quantities, and the observation equation is:
Figure RE-GDA00023034775600000311
wherein: v. of tCorresponding 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 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:
wherein: v. of g＝[v gxv gyv gz] 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 v gAnd v wAnd calculating to obtain the observed quantity of the ocean current velocity as follows:
m c＝v g-v w
wherein: m is c＝[m cxm cym cz] TRepresenting the observation of ocean currents in three directions;
the ocean current observation equation is linear and satisfies m c＝v c+v vc；
Wherein: v. of vcObserving the noise vector, v, for the ocean currents vc＝[v vcxv vcyv vcz] TWherein v is vcxObserving noise for the ocean current in the x direction; v. of vcyObserving noise for the ocean current in the y direction; v. of vczObserving 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 m zThen its observation equation is
m z＝ap+v z
Wherein: a ═ 001]，v zNoise 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:
p k+1＝p k+ΔTv c,k+ΔTv w,k+ω p,k
v c,k+1＝v c,k+ω c,k
v e,k+1＝v e,k+ω e,k
wherein: omega p,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 RE-GDA0002303477560000042
wherein v is t,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:
m vc，k＝v c,k+v vc,k
wherein v is vc,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:
m z,k＝ap k+v z,k
wherein v is z,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 RE-GDA0002303477560000051
from the discrete-time kinematics model of the underwater vehicle, one can obtain:
x 1,k+1＝x 1,k+ΔTx 3,kv w,k+ΔTx 2,k+ω 1,k
x 2,k+1＝x 2,k+ω 2,k
x 3,k+1＝x 3,k+ω 3,k
wherein: omega 1,k,ω 2,k,ω 3,kRespectively corresponding process noise;
the discrete state variables are further defined:
Figure RE-GDA0002303477560000052
obtaining:
x 5,k+1＝x 5,k+ω 5,k
Figure RE-GDA0002303477560000054
wherein: omega 4,k,ω 5,k,ω 6,kRespectively corresponding process noise;
defining a state vector and a noise vector:
Figure RE-GDA0002303477560000061
x is then kThe kinematic equation of (a) is:
x k+1＝A kx k+ω k
wherein:
Figure RE-GDA0002303477560000062
wherein: i is 3Identity matrix representing three dimensions, 0 m×nRepresenting a matrix with elements of 0 and dimension of m rows and n columns;
s2, processing an observation model;
from x 1,k,x 2,k,x 3,k,x 5,k,x 6,kThe definition of (a) can be given as:
x 2,k＝v c,kx 3,k
Figure RE-GDA0002303477560000063
Figure RE-GDA0002303477560000064
when the underwater vehicle obtains ocean current observation m vc，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＝x 2,k-m vc,kx 3,k+v s1,k
Figure RE-GDA0002303477560000065
Figure RE-GDA0002303477560000066
wherein: v. of s,1，v s,2And v s,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:
v s1,k＝v vc,kx 3,k
Figure RE-GDA0002303477560000077
Figure RE-GDA0002303477560000078
likewise, from x 1,k,x 3,kThe definition of (a) and the depth gauge observation equation can be obtained:
0＝ax 1,k-m z,kx 3,k+v s4,kwherein: v. of s,4To correspond to observed noise, and v s4,k＝v z,kx 3,k；
From x 4,kThe definition of (a) can be given as:
Figure RE-GDA0002303477560000071
wherein:
Figure RE-GDA0002303477560000072
constructing an observation vector and an observation noise vector as follows:
Figure RE-GDA0002303477560000073
Figure RE-GDA0002303477560000074
Figure RE-GDA0002303477560000075
the corresponding observation equation is:
m 1,k＝C 1,kx k+v 1,k
m 2,k＝C 2,kx k+v 2,k
m 3,k＝C 3,kx k+v 3,k
wherein:
Figure RE-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 RE-GDA0002303477560000081
And the attitude heading of the aircraft obtained by the attitude heading reference system
Figure RE-GDA00023034775600000811
Constructing a rotation matrix
Figure RE-GDA0002303477560000082
By
Figure RE-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 A k；
The state prior estimation of the augmented linear model obtained by the prediction link of Kalman filtering is as follows:
Figure RE-GDA0002303477560000084
Figure RE-GDA0002303477560000085
wherein:
Figure RE-GDA0002303477560000086
and P k|kThe posterior state and the posterior variance at the moment k are respectively;
Figure RE-GDA0002303477560000087
and P k+1|kRespectively a prior state and a prior variance at the moment k + 1; q kThe 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 omega kThe 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 RE-GDA0002303477560000088
according to known v w,k+1And constructing ocean current observation at the k +1 moment:
m vc,k+1＝v g,k+1-v w,k+1
according to m vc,k+1Constructing an observation matrix C 1,k+1And an observation vector m 1,k+1；
According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure RE-GDA0002303477560000089
Figure RE-GDA00023034775600000810
P k+1|k+1＝P k+1|k-K k+1C 1,k+1P k+1|k
wherein: k k+1Is Kalman gain; r 1,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 v 1,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 +1 z,k+1Constructing an observation matrix C based thereon 2,k+1And an observed variable m 2,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 RE-GDA0002303477560000091
P k+1|k+1＝P k+1|k-K k+1C 2,k+1P k+1|k
wherein: r 2,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 v 2,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 moment t,k+1(ii) a Constructing an observation matrix C 3,k+1And an observed variable m 3,k+1According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure RE-GDA0002303477560000094
P k+1|k+1＝P k+1|k-K k+1C 3,k+1P k+1|k
wherein: r 3,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 v 3,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 RE-GDA0002303477560000095
Figure RE-GDA0002303477560000096
Figure RE-GDA0002303477560000097
wherein:
Figure RE-GDA0002303477560000098
and
Figure RE-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 RE-GDA00023034775600000910
wherein: v. of mAnd v MThe 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 RE-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:
v c＝[v cxv cyv cz] T
wherein: v. of cx，v cy，v czThe 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:
v w＝[v wxv wyv wz] T
wherein: v. of wx，v wy，v wzThe 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 RE-GDA0002303477560000111
wherein:
Figure RE-GDA0002303477560000112
the relative velocity vector of the underwater vehicle and the water under the satellite coordinate system measured by the Doppler velocimeter,
Figure RE-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 RE-GDA0002303477560000114
the calculation formula of (2) is as follows:
Figure RE-GDA0002303477560000115
wherein: respectively measuring a roll angle, a pitch angle and a course angle of the underwater vehicle by an attitude course reference system;
note v eIs an unknown effective acoustic velocity underwater;
solving for unknown p, v cAnd v eAnd taking into account the corresponding uncertainty, obtaining a kinematic model of the underwater vehicle:
Figure RE-GDA0002303477560000118
wherein:
Figure RE-GDA00023034775600001110
for the position of an underwater vehicle in the x, y, z directionsUncertainty, ω c＝[ω cxω cyω cz] TIs the uncertainty of the ocean current in the x, y and z directions; omega eIs 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 signal eWithout 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 T a，T eAnd T aAre all known quantities, and the observation equation is:
Figure RE-GDA0002303477560000121
wherein: v. of tCorresponding 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 RE-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 RE-GDA0002303477560000123
wherein: v. of g＝[v gxv gyv gz] 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 v gAnd v wAnd calculating to obtain the observed quantity of the ocean current velocity as follows:
m c＝v g-v w
wherein: m is c＝[m cxm cym cz] TRepresenting the observation of ocean currents in three directions;
the ocean current observation equation is linear and satisfies m c＝v c+v vc；
Wherein: v. of vcObserving the noise vector, v, for the ocean currents vc＝[v vcxv vcyv vcz] TWherein v is vcxObserving noise for the ocean current in the x direction; v. of vcyObserving noise for the ocean current in the y direction; v. of vczObserving 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 m zThen its observation equation is
m z＝ap+v z
Wherein: a ═ 001]，v zNoise 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:
p k+1＝p k+ΔTv c,k+ΔTv w,k+ω p,k
v c,k+1＝v c,k+ω c,k
v e,k+1＝v e,k+ω e,k
wherein: omega p,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:
wherein v is t,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:
m vc，k＝v c,k+v vc,k
wherein v is vc,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:
m z,k＝ap k+v z,k
wherein v is z,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 RE-GDA0002303477560000132
from the discrete-time kinematics model of the underwater vehicle, one can obtain:
x 1,k+1＝x 1,k+ΔTx 3,kv w,k+ΔTx 2,k+ω 1,k
x 2,k+1＝x 2,k+ω 2,k
x 3,k+1＝x 3,k+ω 3,k
wherein: omega 1,k,ω 2,k,ω 3,kRespectively corresponding process noise;
the discrete state variables are further defined:
Figure RE-GDA0002303477560000133
obtaining:
Figure RE-GDA0002303477560000134
x 5,k+1＝x 5,k+ω 5,k
wherein: omega 4,k,ω 5,k,ω 6,kRespectively corresponding process noise;
defining a state vector and a noise vector:
Figure RE-GDA0002303477560000141
x is then kThe kinematic equation of (a) is:
x k+1＝A kx k+ω k
wherein:
Figure RE-GDA0002303477560000142
wherein: i is 3Identity matrix representing three dimensions, 0 m×nRepresenting a matrix with elements of 0 and dimension of m rows and n columns;
s2, processing an observation model;
from x 1,k,x 2,k,x 3,k,x 5,k,x 6,kThe definition of (a) can be given as:
x 2,k＝v c,kx 3,k
Figure RE-GDA0002303477560000143
Figure RE-GDA0002303477560000144
when the underwater vehicle obtains ocean current observation m vc，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＝x 2,k-m vc,kx 3,k+v s1,k
Figure RE-GDA0002303477560000145
Figure RE-GDA0002303477560000146
wherein: v. of s,1，v s,2And v s,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:
v s1,k＝v vc,kx 3,k
likewise, from x 1,k,x 3,kThe definition of (a) and the depth gauge observation equation can be obtained:
0＝ax 1,k-m z,kx 3,k+v s4,k
wherein: v. of s,4To correspond to observed noise, and v s4,k＝v z,kx 3,k；
From x 4,kThe definition of (a) can be given as:
Figure RE-GDA0002303477560000153
wherein:
Figure RE-GDA0002303477560000154
constructing an observation vector and an observation noise vector as follows:
Figure RE-GDA0002303477560000155
Figure RE-GDA0002303477560000156
Figure RE-GDA0002303477560000157
the corresponding observation equation is:
m 1,k＝C 1,kx k+v 1,k
m 2,k＝C 2,kx k+v 2,k
m 3,k＝C 3,kx k+v 3,k
wherein:
Figure RE-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 RE-GDA0002303477560000161
And the attitude heading of the aircraft obtained by the attitude heading reference system Constructing a rotation matrix
Figure RE-GDA0002303477560000162
By
Figure RE-GDA0002303477560000163
Underwater vehicle under local inertial coordinate systemRelative speed with water, and corresponding system matrix A can be constructed k；
The state prior estimation of the augmented linear model obtained by the prediction link of Kalman filtering is as follows:
Figure RE-GDA0002303477560000164
wherein: and P k|kThe posterior state and the posterior variance at the moment k are respectively; and P k+1|kRespectively a prior state and a prior variance at the moment k + 1; q kThe 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 omega kThe 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 RE-GDA0002303477560000168
according to known v w,k+1And constructing ocean current observation at the k +1 moment:
m vc,k+1＝v g,k+1-v w,k+1
according to m vc,k+1Constructing an observation matrix C 1,k+1And an observation vector m 1,k+1；
According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure RE-GDA0002303477560000169
Figure RE-GDA00023034775600001610
P k+1|k+1＝P k+1|k-K k+1C 1,k+1P k+1|k
wherein: k k+1Is Kalman gain; r 1,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 v 1,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 +1 z,k+1Constructing an observation matrix C based thereon 2,k+1And an observed variable m 2,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 RE-GDA0002303477560000171
Figure RE-GDA0002303477560000172
P k+1|k+1＝P k+1|k-K k+1C 2,k+1P k+1|k
wherein: r 2,k+1Is the k +1 time and depth observation phaseThe noise covariance matrix is a symmetric positive definite matrix with specific parameters defined by the observed noise v 2,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 moment t,k+1(ii) a Constructing an observation matrix C 3,k+1And an observed variable m 3,k+1According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure RE-GDA0002303477560000173
Figure RE-GDA0002303477560000174
P k+1|k+1＝P k+1|k-K k+1C 3,k+1P k+1|k
wherein: r 3,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 v 3,k+1The statistical property decision of (a) can be obtained by offline modulation.
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 RE-GDA0002303477560000175
Figure RE-GDA0002303477560000177
wherein:
Figure RE-GDA0002303477560000178
and 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 RE-GDA00023034775600001710
wherein: v. of mAnd v MThe 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 RE-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 RE-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 velocity MAnd v m1600 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 The two calculation methods are as follows:
Figure RE-GDA0002303477560000191
Figure RE-GDA0002303477560000192
wherein:
Figure RE-GDA0002303477560000193
and
Figure RE-GDA0002303477560000194
real and estimated vehicle position coordinates in the ith Monte Carlo simulation,
Figure RE-GDA0002303477560000195
and
Figure RE-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 (10)

1. 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; characterized in that 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.
2. The single beacon positioning method for the underwater vehicle with global convergence as claimed in claim 1, wherein in the step C, the kinematic model is established by:
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:
v c＝[v cxv cyv cz] T
wherein: v. of cx，v cy，v czThe 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:
v w＝[v wxv wyv wz] T
wherein: v. of wx，v wy，v wzRespectively x, y, z in underwater local inertial coordinate systemThe relative speed between the underwater vehicle and water in the three directions is obtained by calculation through data measured by the attitude heading reference system and the Doppler velocimeter, and the calculation formula is as follows:
Figure FDA0002277790400000021
wherein:
Figure FDA0002277790400000022
the relative velocity vector of the underwater vehicle and the water under the satellite coordinate system measured by the Doppler velocimeter, 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 FDA0002277790400000024
the calculation formula of (2) is as follows:
Figure FDA0002277790400000025
wherein:
Figure FDA0002277790400000026
theta and psi are respectively a roll angle, a pitch angle and a heading angle of the underwater vehicle and are measured by an attitude heading reference system;
note v eIs an unknown effective acoustic velocity underwater;
solving for unknown p, v cAnd v eAnd taking into account the corresponding uncertainty, obtaining a kinematic model of the underwater vehicle:
Figure FDA0002277790400000027
Figure FDA0002277790400000028
Figure FDA0002277790400000029
wherein:
Figure FDA00022777904000000210
is the position uncertainty, omega, of the underwater vehicle in the x, y and z directions c＝[ω cxω cyω cz] TIs the uncertainty of the ocean current in the x, y and z directions; omega eIs the effective sound speed uncertainty.
3. The single beacon positioning method for the underwater vehicle with global convergence as claimed in claim 2, wherein in the step C, the observation model is established by:
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 signal eWithout 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 T a，T eAnd T aAre all known quantities, and the observation equation is:
Figure FDA00022777904000000211
wherein: v. of tCorresponding 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 The attitude and heading of the underwater vehicle measured by combining the attitude heading reference systemAnd (3) calculating to obtain the representation of the absolute speed of the underwater vehicle under an underwater local inertial coordinate system:
Figure FDA0002277790400000031
wherein: v. of g＝[v gxv gyv gz] 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 v gAnd v wAnd calculating to obtain the observed quantity of the ocean current velocity as follows:
m c＝v g-v w
wherein: m is c＝[m cxm cym cz] TRepresenting the observation of ocean currents in three directions;
the ocean current observation equation is linear and satisfies m c＝v c+v vc；
Wherein: v. of vcObserving the noise vector, v, for the ocean currents vc＝[v vcxv vcyv vcz] TWherein v is vcxObserving noise for the ocean current in the x direction; v. of vcyObserving noise for the ocean current in the y direction; v. of vczObserving 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 m zThen its observation equation is
m z＝ap+v z
Wherein: a ═ 001]，v zNoise was observed for the depth gauge.
4. The single beacon positioning method for the underwater vehicle with global convergence of claim 3, wherein in the step C, the kinematic model and the observation model discretization method comprise 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:
p k+1＝p k+ΔTv c,k+ΔTv w,k+ω p,k
v c,k+1＝v c,k+ω c,k
v e,k+1＝v e,k+ω e,k
wherein: omega p,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 FDA0002277790400000032
wherein v is t,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:
m vc，k＝v c,k+v vc,k
wherein v is vc,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:
m z,k＝ap k+v z,k
wherein v is z,kNoise was observed for the depth gauge at time k.
5. The single beacon location method for an underwater vehicle with global convergence of claim 4, wherein said step D comprises:
s1, processing a kinematic model;
defining discrete state variables:
Figure FDA0002277790400000041
from the discrete-time kinematics model of the underwater vehicle, one can obtain:
x 1,k+1＝x 1,k+ΔTx 3,kv w,k+ΔTx 2,k+ω 1,k
x 2,k+1＝x 2,k+ω 2,k
x 3,k+1＝x 3,k+ω 3,k
wherein: omega 1,k,ω 2,k,ω 3,kRespectively corresponding process noise;
the discrete state variables are further defined:
Figure FDA0002277790400000042
obtaining:
Figure FDA0002277790400000043
x 5,k+1＝x 5,k+ω 5,k
wherein: omega 4,k,ω 5,k,ω 6,kRespectively corresponding process noise;
defining a state vector and a noise vector:
Figure FDA0002277790400000051
x is then kThe kinematic equation of (a) is:
x k+1＝A kx k+ω k
wherein:
Figure FDA0002277790400000052
wherein: i is 3Identity matrix representing three dimensions, 0 m×nRepresenting a matrix with elements of 0 and dimension of m rows and n columns;
s2, processing an observation model;
from x 1,k,x 2,k,x 3,k,x 5,k,x 6,kThe definition of (a) can be given as:
x 2,k＝v c,kx 3,k
Figure FDA0002277790400000053
Figure FDA0002277790400000054
when the underwater vehicle obtains ocean current observation m vc，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＝x 2,k-m vc,kx 3,k+v s1,k
Figure FDA0002277790400000055
Figure FDA0002277790400000056
wherein: v. of s,1，v s,2And v s,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:
v s1,k＝v vc,kx 3,k
Figure FDA0002277790400000061
Figure FDA0002277790400000062
likewise, from x 1,k,x 3,kThe definition of (a) and the depth gauge observation equation can be obtained:
0＝ax 1,k-m z,kx 3,k+v s4,k
wherein: v. of s,4To correspond to observed noise, and v s4,k＝v z,kx 3,k；
From x 4,kThe definition of (a) can be given as:
wherein:
Figure FDA0002277790400000064
constructing an observation vector and an observation noise vector as follows:
Figure FDA0002277790400000065
Figure FDA0002277790400000067
the corresponding observation equation is:
m 1,k＝C 1,kx k+v 1,k
m 2,k＝C 2,kx k+v 2,k
m 3,k＝C 3,kx k+v 3,k
wherein:
Figure FDA0002277790400000068
C 2，k＝[a 0 1×3-m z，k0 0 0]。
C 3，k＝[0 1×30 1×30 1 0 0]
6. the single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 5, wherein 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 FDA0002277790400000071
And the attitude heading of the aircraft obtained by the attitude heading reference system
Figure FDA0002277790400000072
Constructing a rotation matrix
Figure FDA0002277790400000073
By
Figure FDA0002277790400000074
Obtaining the relative speed of the underwater vehicle and the water under the local inertial coordinate system, and further constructing a corresponding system matrix A k；
The state prior estimation of the augmented linear model obtained by the prediction link of Kalman filtering is as follows:
Figure FDA0002277790400000075
Figure FDA0002277790400000076
wherein:
Figure FDA0002277790400000077
and P k|kThe posterior state and the posterior variance at the moment k are respectively;
Figure FDA0002277790400000078
and P k+1|kRespectively a prior state and a prior variance at the moment k + 1; q kThe 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 omega kThe statistical property decision of (a) can be obtained by offline modulation.
7. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 6, wherein 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 FDA0002277790400000079
according to known v w,k+1And constructing ocean current observation at the k +1 moment:
m vc,k+1＝v g,k+1-v w,k+1
according to m vc,k+1Constructing an observation matrix C 1,k+1And an observation vector m 1,k+1；
According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure FDA00022777904000000711
P k+1|k+1＝P k+1|k-K k+1C 1,k+1P k+1|k
wherein: k k+1Is Kalman gain; r 1,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 v 1,k+1The statistical property decision of (a) can be obtained by offline modulation.
8. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 7, wherein 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 +1 z,k+1From which an observation matrix C is constructed 2,k+1And an observed variable m 2,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 FDA0002277790400000081
Figure FDA0002277790400000082
P k+1|k+1＝P k+1|k-K k+1C 2,k+1P k+1|k
wherein: r 2,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 v 2,k+1The statistical property decision of (a) can be obtained by offline modulation.
9. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 7, wherein 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 moment t,k+1(ii) a Constructing an observation matrix C 3,k+1And an observed variable m 3,k+1According to the updating link of Kalman filtering, the state posterior estimation of the obtained augmented linear model is as follows:
Figure FDA0002277790400000083
P k+1|k+1＝P k+1|k-K k+1C 3,k+1P k+1|k
wherein: r 3,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 v 3,k+1The statistical property decision of (a) can be obtained by offline modulation.
10. The single beacon localization method of claim 5-9, wherein the initial non-linear model a posteriori state estimates are computed from the augmented linear model a posteriori state estimates from step F, G, H by:
Figure FDA0002277790400000085
Figure FDA0002277790400000087
wherein:
Figure FDA0002277790400000088
and the posterior estimation of the effective sound velocity, the position and the ocean current velocity at the moment of k +1 respectively; to further ensure the positioning dieAnd (3) the stability of the model, namely, the estimation of the effective sound velocity is limited, namely:
Figure FDA00022777904000000810
wherein: v. of mAnd v MThe 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 FDA0002277790400000091
CN201911129169.6A 2019-11-18 2019-11-18 Underwater vehicle single beacon positioning method with global convergence Active CN110779519B (en)

Priority Applications (1)

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

Applications Claiming Priority (1)

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

Publications (2)

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

Family

ID=69391536

Family Applications (1)

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

Country Status (1)

Country Link
CN (1) CN110779519B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112285652A (en) * 2020-10-28 2021-01-29 浙江大学 Underwater glider positioning method utilizing single beacon virtual arrival time difference
CN112698273A (en) * 2020-12-15 2021-04-23 哈尔滨工程大学 Multi-AUV single-standard distance measurement cooperative operation method
CN113093092A (en) * 2021-04-01 2021-07-09 哈尔滨工程大学 Underwater robust self-adaptive single beacon positioning method
CN113324546A (en) * 2021-05-24 2021-08-31 哈尔滨工程大学 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure
CN116702479A (en) * 2023-06-12 2023-09-05 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle

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 哈尔滨工程大学 Distributed underwater network localization method based on mobile beacon
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 kind of localization method and system for submarine target

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 哈尔滨工程大学 Distributed underwater network localization method based on mobile beacon
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 kind of localization method and system for submarine target

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
H. QIN: "H. Qin Unscented Kalman Filter Based Single Beacon Underwater Localization with Unknown Effective Sound Velocity", 《2018 OCEANS - MTS/IEEE KOBE TECHNO-OCEANS (OTO), KOBE》 *
梁国龙: "单信标导航精度分析与航路规划", 《水下无人系统学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112285652A (en) * 2020-10-28 2021-01-29 浙江大学 Underwater glider positioning method utilizing single beacon virtual arrival time difference
CN112698273A (en) * 2020-12-15 2021-04-23 哈尔滨工程大学 Multi-AUV single-standard distance measurement cooperative operation method
CN113093092A (en) * 2021-04-01 2021-07-09 哈尔滨工程大学 Underwater robust self-adaptive single beacon positioning method
CN113324546A (en) * 2021-05-24 2021-08-31 哈尔滨工程大学 Multi-underwater vehicle collaborative positioning self-adaptive adjustment robust filtering method under compass failure
CN116702479A (en) * 2023-06-12 2023-09-05 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle
CN116702479B (en) * 2023-06-12 2024-02-06 哈尔滨工程大学 Unknown input and position estimation method and system for underwater vehicle

Also Published As

Publication number Publication date
CN110779519B (en) 2021-04-27

Similar Documents

Publication Publication Date Title
CN110779518B (en) Underwater vehicle single beacon positioning method with global convergence
CN110779519B (en) Underwater vehicle single beacon positioning method with global convergence
CN109459040B (en) Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering
CN110794409B (en) Underwater single beacon positioning method capable of estimating unknown effective sound velocity
CN110749891B (en) Self-adaptive underwater single beacon positioning method capable of estimating unknown effective sound velocity
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
CN104406605B (en) Airborne many navigation sources integrated navigation analogue systems
JP3362925B2 (en) Onboard navigation system for aircraft
CN110646783B (en) Underwater beacon positioning method of underwater vehicle
CN102353378B (en) Adaptive federal filtering method of vector-form information distribution coefficients
CN110095800A (en) A kind of self-adapted tolerance federated filter Combinated navigation method of multi-source fusion
CN110231636B (en) Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system
CN110823217A (en) Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering
CN102252677A (en) Time series analysis-based variable proportion self-adaptive federal filtering method
Song et al. Neural-network-based AUV navigation for fast-changing environments
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN111221018A (en) GNSS multi-source information fusion navigation method for inhibiting marine multipath
CN104062672A (en) SINSGPS integrated navigation method based on strong tracking self-adaptive Kalman filtering
CN106840211A (en) A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters
CN111142143A (en) Multi-source information fusion-based approach segment flight technical error estimation method
Khalaf et al. Novel adaptive UKF for tightly-coupled INS/GPS integration with experimental validation on an UAV
CN106441291A (en) Integrated navigation system and method based on strong-tracking SDRE filtering
Geng et al. Hybrid derivative-free EKF for USBL/INS tightly-coupled integration in AUV
Davari et al. Multirate adaptive Kalman filter for marine integrated navigation system
RU2654965C1 (en) Integrated strap-down astro-inertial navigation system

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