CN110568407A - A method of underwater navigation and positioning based on ultra-short baseline and dead reckoning - Google Patents

Abstract

The invention relates to an underwater navigation positioning method based on an ultra-short baseline and dead reckoning, which comprises the following steps of: s1, determining the positioning principle of the ultra-short baseline system; s2, determining an error model of the ultra-short baseline system; s3, establishing a state model and an observation model; and S4, establishing an extended Kalman filtering algorithm considering the variation of the direction angle measurement deviation. The invention can effectively adapt to the situation of noise change of direction angle measurement, and can keep higher positioning precision under small direction angles.

Description

A method of underwater navigation and positioning based on ultra-short baseline and dead reckoning

technical field

The invention relates to the technical field of underwater positioning and navigation, and more specifically relates to an underwater navigation and positioning method based on ultra-short baseline and dead reckoning.

Background technique

During the operation of underwater vehicles such as ROV (Remote Operated Vehicle) and AUV (Autonomous Underwater Vehicle), it is very important to perform high-precision navigation and positioning on them.

As a multi-source information fusion algorithm, Kalman Filtering has been widely used in the navigation and positioning of underwater vehicles. The extended Kalman filter based on Ultrashort Base Line (USBL) and dead reckoning (DR) can fuse the observation data and dead reckoning information of the ultrashort baseline system to obtain the best underwater target position. Excellent estimate. Accurate filtering model and error statistics are the basis of Kalman filtering optimal estimation. For the planar ultra-short baseline system, the measurement deviation of the azimuth angle is related to the azimuth angle itself. In the traditional extended Kalman filter algorithm based on ultra-short baseline and dead reckoning, the measurement deviation of the direction angle of the ultra-short baseline system is often regarded as a fixed value, which leads to low positioning accuracy of the filter at small direction angles. Considering factors such as ship shaking or autonomous movement of underwater targets that lead to changes in the direction angle measurement deviation, the present invention proposes an underwater navigation and positioning method based on ultra-short baseline and dead reckoning that considers the change in direction angle measurement deviation.

Contents of the invention

(1) Technical problems to be solved

In order to solve the problem of low positioning accuracy of the traditional extended Kalman filter algorithm based on ultra-short baseline and dead reckoning at small azimuth angles, the present invention provides a method based on ultra-short baseline and dead reckoning that considers the change in measurement deviation of azimuth angle. Underwater navigation and positioning method.

(2) Technical solution

In order to achieve the above object, the main technical solutions adopted in the present invention include:

Design an underwater navigation and positioning method based on ultra-short baseline and dead reckoning, including the following steps:

S1. Determine the positioning principle of the ultra-short baseline system;

S2. Determine the error model of the ultra-short baseline system;

S3, establishing a state model and an observation model;

S4. Establishing an extended Kalman filter algorithm considering the variation of the direction angle measurement deviation.

In the above scheme, in the step S4, a Kalman filter algorithm considering the variation of the direction angle measurement deviation is established, including the following steps:

S4-1. According to the posterior estimated position and its posterior covariance at the previous moment, determine the prior estimated position and its prior covariance of the underwater target at this moment through the state model;

S4-2. According to the posterior estimated position at the previous moment, determine the observation noise matrix at this moment through the error model of the ultra-short baseline system;

S4-3. Determine the gain according to the observed noise matrix at this moment;

S4-4. Determine the innovation according to the measurement equation;

S4-5. Determine the posterior estimated position of the underwater target at this moment and its posterior covariance according to the gain and the new information;

S4-6. Repeat the above steps to obtain the real-time position of the underwater vehicle.

In the above scheme, in the step S1, the transducer O is arranged at the origin of the hull space coordinate system O-XYZ, and the hydrophones S1, S2, S3, and S4 are symmetrically arranged at the center of the hull space coordinate system XOY plane, Among them, the array elements S1 and S3 are located on the X-axis pointing to the bow, and the array elements S2 and S4 are located on the Y-axis perpendicular to the bow; the distance between the array elements is d, that is, the distance between S1 and S3, and the distance between S2 and S4; The underwater target of the transponder is located at point T, and its coordinates are (x, y, z) in the hull space coordinate system; the ultra-short baseline hydroacoustic positioning system uses the delay difference T and phase difference between the signals received by the hydrophone △φ _x , △φ _y solve the distance r of the underwater target:

r=CT/2 (1)

and orientation:

θ _x ＝ arcos(λ△φ _x /2πd), θ _y ＝arcos(λ△φ _y /2πd) (2)

Then calculate the underwater target position according to formula (1) and formula (2):

Among them, in the formulas (1) to (3), C is the underwater sound velocity, and f is the transceiving frequency of the transducer.

In the above scheme, in the step S2, the underwater target position is partially differentiated and the standard deviation is calculated:

Neglecting the acoustic wavelength error σ _λ and the array installation error σ _d , the direction finding error is optimized according to the Cramereau lower bound, and the simplified deviation model of the directional angle θ _x , θ _y is

In the above scheme, in the step S3, it is stipulated that the motion state of the underwater target is determined by three parameters: the course over the ground, the speed over the ground and the heave speed:

In formula (6), x(k), y(k), z(k) are relative transducer positions of the underwater target at time k, COG(k)(CourseOf Ground), SOG(k)(Speed Of Ground) , △z(k) are the course, speed and heave velocity of the underwater target at time k respectively, ε _COG (k), ε _SOG (k), ε _{△ z} (k) are the underwater target at time k The control deviation of the target's course over the ground, the control deviation of the speed over the ground, and the control deviation of the heave speed, Δt is the time interval between k time and k+1 time; the state equation of the underwater target at k+1 time is:

The ultra-short baseline system observes the position of underwater targets,

In formulas (8) to (9), r(k), θ _x (k), and θ _y (k) are the observed slant distance, direction angle in the x direction, and direction in the y direction of the ultra-short baseline system at time k, respectively. angle; _δr (k), are respectively the observation slant distance deviation, the direction angle deviation in the x direction, and the direction angle deviation in the y direction at time k, and (a, b, c) are the absolute coordinates of the transducer, then the measurement equation is:

In formula (10), r(k), θ _x (k), θ _y (k), and dep(k) are the observed slant distance of the ultra-short baseline system at time k, the direction angle in the x direction, and the direction angle in the y direction, respectively. Azimuth angle, measured value of depth gauge; δ _r (k), They are the observed slope distance deviation, the direction angle deviation in the x direction, and the direction angle deviation in the y direction at time k, respectively.

In the above scheme, in the step 4, an extended Kalman filter algorithm considering the variation of the direction angle measurement deviation is established:

F(k+1) is the Jacobian matrix of x(k+1)=g(x(k),u(k),ε(k)) about the underwater target position at k+1 moment, and its expression is :

Q(k) is the noise matrix of the state vector at time k, and the process noise is regarded as a constant value:

H(k+1) is the Jacobian matrix of the observation equation z(k)=h(x(k),δ(k)) relative to the underwater target position coordinates:

l, l ₁ , l ₂ are construction auxiliary variables:

R(k+1) is the noise matrix of the ultra-short baseline system observation vector at time k+1; assuming that each observation vector is independent of each other and the observation slant distance deviation is a constant value, the observation noise matrix is decomposed into the following form:

In the announcement (16), Σ(k+1) is the observation noise adjustment factor, which reflects the influence of the relative acoustic array orientation change of the underwater target on the observation deviation; R is the minimum observation deviation of the system;

The continuity of the motion of the object makes the difference between θ _x and θ _y at two adjacent moments not much different, and the filtered value at the previous moment is used as the true value at this moment:

Obtained according to the following formula:

The filter gain K is corrected as:

The extended Kalman filter framework is tuned to:

(3) Beneficial effects

The beneficial effects of the present invention are: the present invention utilizes the direction angle obtained from filtering at the previous moment to approximately construct the direction angle measurement noise at the next moment, and adaptively changes the relative position of the underwater target and the hydrophone array, that is, the direction Angle change, adjust the weight of the observation value of the ultra-short baseline system in the extended Kalman filter wave value. The present invention can effectively adapt to the situation that the direction angle measurement noise changes, and can maintain high positioning accuracy even under small direction angles.

Description of drawings

Figure 1 is a schematic diagram of the positioning principle of the ultra-short baseline system;

Fig. 2 is a schematic diagram of a filtering framework;

Figure 3 is a schematic diagram of the filtering effect.

Detailed ways

In order to better explain the present invention and facilitate understanding, the present invention will be described in detail below through specific embodiments in conjunction with the accompanying drawings.

The present invention provides an underwater navigation and positioning method based on ultra-short baseline and dead reckoning, which considers the variation of direction angle measurement deviation, comprising the following steps:

S1. Determine the positioning principle of the ultra-short baseline system: as shown in FIG. 1 , the transducer O is arranged at the origin of the hull space coordinate system (O-XYZ). The hydrophones S1, S2, S3, and S4 are arranged symmetrically in the center of the XOY plane of the hull space coordinate system. Among them, array elements S1 and S3 are located on the X-axis pointing to the bow of the ship, and array elements S2 and S4 are located on the Y-axis perpendicular to the bow of the ship. The array element spacing is d, that is, the spacing between S1 and S3, and the spacing between S2 and S4. The underwater target equipped with an acoustic transponder is located at point T, and its coordinates are (x, y, z) in the hull space coordinate system. The ultra-short baseline underwater acoustic positioning system uses the delay difference T and phase difference △φ _x , △φ _y between the signals received by the hydrophone to calculate the distance r of the underwater target:

r=CT/2 (1)

and orientation:

θ _x ＝ arcos(λ△φ _x /2πd), θ _y ＝arcos(λ△φ _y /2πd) (2)

Then calculate the underwater target position according to formula (1) and formula (2):

Among them, in the formulas (1) to (3), C is the underwater sound velocity, and f is the transceiving frequency of the transducer.

S2. Determine the deviation model of the orientation angles θ _x , θ _y .

Partially differentiate the position of the underwater target and find the standard deviation:

Neglecting the acoustic wavelength error σ _λ and array installation error σ _d , the direction-finding error is optimized according to the Cramereau lower bound, and the simplified azimuth deviation model is

S3. Determine the state equation of the underwater target.

It is stipulated that the motion state of the underwater target is determined by three parameters: heading over ground, speed over ground and heave speed:

In formula (6), x(k), y(k), z(k) are relative transducer positions of the underwater target at time k, COG(k)(CourseOf Ground), SOG(k)(Speed Of Ground) , △z(k) are the course, speed and heave velocity of the underwater target at time k respectively, ε _COG (k), ε _SOG (k), ε _{△ z} (k) are the underwater target at time k The control deviation of the target's course over the ground, the control deviation of the ground speed and the control deviation of the heave speed, Δt is the time interval between k time and k+1 time. The state equation of the underwater target at time k+1 is:

Determine the measurement equation. The ultra-short baseline system observes the position of underwater targets,

In formulas (8) to (9), r(k), θ _x (k), and θ _y (k) are the observed slant distance, direction angle in the x direction, and direction in the y direction of the ultra-short baseline system at time k, respectively. angle; _δr (k), are the observed slope distance deviation, the direction angle deviation in the x direction, and the direction angle deviation in the y direction at time k, respectively, and (a, b, c) are the absolute coordinates of the transducer. The measurement equation is:

In formula (12), r(k), θ _x (k), θ _y (k), and dep(k) are the observed slant distance of the ultra-short baseline system at time k, the direction angle in the x direction, and the direction angle in the y direction, respectively. Azimuth angle, measured value of depth gauge; δ _r (k), They are the observed slope distance deviation, the direction angle deviation in the x direction, and the direction angle deviation in the y direction at time k, respectively.

S4. Determine the Kalman filter algorithm considering the variation of the direction angle measurement deviation: as shown in Figure 2

F(k+1) is the Jacobian matrix of x(k+1)=g(x(k),u(k),ε(k)) about the underwater target position at k+1 moment, and its expression is :

Q(k) is the noise matrix of the state vector at time k, and the process noise is regarded as a constant value:

H(k+1) is the Jacobian matrix of the observation equation z(k)=h(x(k),δ(k)) relative to the underwater target position coordinates:

l, l ₁ , l ₂ are construction auxiliary variables:

R(k+1) is the noise matrix of the observation vectors of the ultra-short baseline system at time k+1; each observation vector is set to be independent of each other, and the observation slope distance deviation is a constant value. Decompose the observation noise matrix into the following form:

In the formula (16), Σ(k+1) is the observation noise adjustment factor, which reflects the influence of the position change of the underwater target relative to the acoustic array on the observation deviation; R is the minimum observation deviation of the system.

For Σ(k+1)=diag(1,sin ^-2 θ _x (k+1),sin ^-2 θ _y (k+1),1), it can be known from formula (11) that sin ^{- 2} θ _x (k+1), sin ^-2 θ _y (k+1), due to the continuity of the motion of the object, the difference between θ _x and θ _y at two adjacent moments is not large, and the filter value at the previous moment is considered as The truth value at this moment:

Obtained according to the following formula:

The filter gain K is corrected as:

The Kalman filter framework is adjusted to:

The effect of the present invention will be demonstrated with specific test examples below.

The motion control deviation of the designed underwater target and the observation deviation of the ultra-short baseline system are shown in Table 1.

Table 1 Motion control bias and ultra-short baseline system observation bias of underwater targets

Plan the underwater target at the initial position (-176, 0, -1000) △z=0, r=1000m horizontal circular motion. The ship performs rolling and pitching motions with an amplitude of 12°, a 6° period of 12s, and a period of 5s. The control cycle, the observation cycle is 1s. As shown in Figure 3, the root mean square error of the extended Kalman filter algorithm (CTM-EKF), which considers the variation of the direction angle measurement deviation of the ultra-short baseline system, is lower than that of the traditional Kalman filter algorithm (EKF), and its positioning accuracy is higher .

Accompanying drawing has described the embodiment of the present invention, but the present invention is not limited to above-mentioned specific implementation, and above-mentioned specific implementation is only illustrative, rather than restrictive, and those of ordinary skill in the art are in the present invention Under the enlightenment of the present invention, many forms can also be made without departing from the purpose of the present invention and the scope of protection of the claims, and these all belong to the protection of the present invention.

Claims (6)

1. an underwater navigation positioning method based on an ultra-short baseline and dead reckoning is characterized by comprising the following steps:

S1, determining the positioning principle of the ultra-short baseline system;

s2, determining an error model of the ultra-short baseline system;

S3, establishing a state model and an observation model;

And S4, establishing an extended Kalman filtering algorithm considering the variation of the direction angle measurement deviation.

2. The underwater navigation positioning method based on ultra-short baseline and dead reckoning as claimed in claim 1, wherein: in step S4, a kalman filter algorithm is established that considers the variation of the direction angle measurement deviation, including the following steps:

S4-1, determining the prior estimated position and the prior covariance of the underwater target at the moment through a state model according to the prior estimated position and the prior covariance at the moment;

S4-2, determining an observation noise matrix at the moment through an error model of the ultra-short baseline system according to the posterior estimation position at the last moment;

s4-3, determining gain according to the noise matrix observed at the moment;

S4-4, determining innovation according to a measurement equation;

S4-5, determining the posterior estimation position and the posterior covariance of the underwater target at the moment according to the gain and the innovation;

and S4-6, repeating the steps to obtain the real-time position of the underwater vehicle.

3. The underwater navigation positioning method based on ultra-short baseline and dead reckoning as claimed in claim 1, wherein: in the step S1, the transducer O is arranged at the origin position of a ship body space coordinate system O-XYZ, and the hydrophones S1, S2, S3 and S4 are symmetrically arranged at the center of an XOY plane of the ship body space coordinate system, wherein S1 and S3 array elements are positioned on an X axis pointing to the ship bow, and S2 and S4 array elements are positioned on a Y axis perpendicular to the ship bow; the array element spacing is d, namely the spacing between S1 and S3, and the spacing between S2 and S4; an underwater target carrying an acoustic transponder is located at a point T, and the coordinates of the underwater target are (x, y, z) under a ship body space coordinate system; ultra-short baseline hydrophone positioning system using time delay difference T and phase difference delta phi between signals received by hydrophones_x,△φ_yResolving the distance r of the underwater target:

r＝CT/2 (1)

and orientation:

θ_x＝arcos(λ△φ_x/2πd),θ_y＝arcos(λ△φ_y/2πd) (2)

And then resolving the position of the underwater target according to a formula (1) and a formula (2):

in the formulas (1) to (3), C is the underwater sound velocity, and f is the transducer transmitting and receiving frequency.

4. the underwater navigation positioning method based on ultra-short baseline and dead reckoning as claimed in claim 1, wherein: in step S2, the underwater target position is partially differentiated and standard deviation is calculated:

neglecting the acoustic wavelength error σ_λmounting error sigma of array_dthe direction error is optimized according to the lower boundary of the Cramer-Lo, and the simplified direction angle theta_x,θ_yThe deviation model is

5. The underwater navigation positioning method based on ultra-short baseline and dead reckoning as claimed in claim 1, wherein: in step S3, the motion state of the underwater target is determined by three parameters, namely, the heading to the ground, the speed to the ground and the heave speed:

in the formula (6), x (k), y (k) and z (k) are the relative transducer position Of the underwater target at the time k, COG (k) (Course Of group), SOG (k) (Speed Of group), and Deltaz (k) are the Ground Course, Ground Speed and heave Speed Of the underwater target at the time k respectively, and epsilon_COG(k),ε_SOG(k),ε_△z(k) Respectively controlling deviation of the underwater target to the ground course, deviation of the underwater target to the ground speed and deviation of the heave speed at the moment k, wherein delta t is a time interval between the moment k and the moment k + 1; the state equation of the underwater target at the k +1 moment is as follows:

the ultra-short baseline system observes the position of an underwater target,

in the formulae (8) to (9), r (k) and θ_x(k)、θ_y(k) The observation slant distance, the direction angle in the x direction and the direction angle in the y direction of the ultra-short baseline system at the moment k are respectively; delta_r(k)、the observed slope deviation at time k, the directional angle deviation in the x direction, and the directional angle deviation in the y direction, respectively, (a, b, c) are the absolute coordinates of the transducer, and the measurement equation is:

in the formula (10), r (k) and θ_x(k)、θ_y(k) dep (k) is the observation slope distance of the ultra-short baseline system at the moment k, the direction angle in the x direction, the direction angle in the y direction and the depth meter measured value respectively; delta_r(k)、 The observed slope distance deviation at time k, the direction angle deviation in the x direction, and the direction angle deviation in the y direction are respectively.

6. an underwater navigation positioning method based on ultra-short baseline and dead reckoning as claimed in claim 1 or 2, characterized in that: in step 4, an extended kalman filter algorithm considering the variation of the measurement deviation of the direction angle is established:

f (k +1) is the jacobian moment of x (k +1) ═ g (x (k), u (k), epsilon (k)) for the underwater target position at time k + 1.