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

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_cx v_cy v_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_wx v_wy v_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:

wherein:

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;

the calculation formula of (2) is as follows:

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:

wherein: omega_p＝[ω_px ω_py ω_pz]^TIs 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, 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 T_a，T_eAnd T_aAre all known quantities, and the observation equation is:

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_gx v_gy v_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_cx m_cy m_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_vcx v_vcy v_vcz]^TWherein v is_vcxCurrent in the x directionObserving noise; 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:

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,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:

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:

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:

obtaining:

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:

x is then_kThe kinematic equation of (a) is:

x_k+1＝A_kx_k+ω_k

wherein:

wherein: i is₃Identity 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

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

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,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:

wherein:

constructing an observation vector and an observation noise vector as follows:

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:

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

And the attitude heading of the aircraft obtained by the attitude heading reference system

Constructing a rotation matrix

By

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:

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.

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:

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:

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:

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:

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:

wherein:

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:

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:

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_cx v_cy v_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_wx v_wy v_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:

wherein:

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;

the calculation formula of (2) is as follows:

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:

wherein: omega_p＝[ω_px ω_py ω_pz]^TIs 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.

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:

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_gx v_gy v_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_cx m_cy m_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_vcx v_vcy v_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:

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:

obtaining:

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:

x is then_kThe kinematic equation of (a) is:

x_k+1＝A_kx_k+ω_k

wherein:

wherein: i is₃Identity 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

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

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:

wherein:

constructing an observation vector and an observation noise vector as follows:

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:

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

And the attitude heading of the aircraft obtained by the attitude heading reference system

Constructing a rotation matrix

By

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:

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:

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:

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:

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.

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:

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+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:

wherein:

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:

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:

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:

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:

wherein:

and

real and estimated vehicle position coordinates in the ith Monte Carlo simulation,

and

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. 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;

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_cx v_cy v_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_wx v_wy v_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:

wherein:

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;

the calculation formula of (2) is as follows:

wherein:

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:

wherein: omega_p＝[ω_px ω_py ω_pz]^TIs 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 velocity 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:

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_gx v_gy v_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_cx m_cy m_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_vcx v_vcy v_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_zObserving noise for a depth meter;

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 is 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 obtained at each discrete time point, the post-discretization observation equation is:

m_z,k＝ap_k+v_z,k

wherein v is_z,kObserving noise for a depth meter at the k moment;

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 location method for an underwater vehicle with global convergence of claim 1, wherein said step D comprises:

s1, processing a kinematic model;

defining discrete state variables:

according to the discrete-time underwater vehicle kinematic model, obtaining:

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:

obtaining:

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:

x is then_kThe kinematic equation of (a) is:

x_k+1＝A_kx_k+ω_k

wherein:

wherein: i is₃Identity 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) yields:

x_2,k＝v_c,kx_3,k

when the underwater vehicle obtains ocean current observation m_vc，kWhen the ocean current wave is regarded as a known quantity, the ocean current observation equation is combined to obtain:

0＝x_2,k-m_vc,kx_3,k+v_s1,k

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,kAnd the depth gauge observation equation, to yield:

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) yields:

wherein:

constructing an observation vector and an observation noise vector as follows:

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:

C_2,k＝[a 0_1×3 -m_z,k 0 0 0]

C_3,k＝[0_1×3 0_1×3 0 1 0 0]。

3. the single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 2, 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

And the attitude heading of the aircraft obtained by the attitude heading reference system

Constructing a rotation matrix

By

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:

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_kIs determined by the statistical properties of the signal obtained by the modulation under the line.

4. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 3, 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:

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 augmented linear model is obtained as follows:

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+1Is determined by the statistical properties of the signal obtained by the modulation under the line.

5. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 4, 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 augmented linear model is obtained as follows:

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+1Is determined by the statistical properties of the signal obtained by the modulation under the line.

6. The single beacon positioning method for an underwater vehicle with global convergence as claimed in claim 5, 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 augmented linear model is as follows:

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+1Is determined by the statistical properties of the signal obtained by the modulation under the line.

7. The single beacon localization method of claim 6, wherein the initial non-linear model a posteriori state estimates are computed from the augmented linear model a posteriori state estimates obtained in step F, G, H by:

wherein:

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:

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:

Images