An inertial and acoustic combined navigation method considering sound ray bending

Abstract

The application provides an inertial and acoustic combined navigation method considering sound ray bending, and the method comprises the following steps: step 1, establishing a state equation model of the inertial and acoustic combined navigation; step 2, establishing a bending sound ray measurement model based on an equal-gradient sound velocity profile; and step 3, updating and feeding back errors by using an unscented Kalman filtering method according to the state equation and the measurement equation established in steps 1 and 2. The application can overcome the problem of positioning accuracy decline caused by sound ray bending, and has higher positioning accuracy.

Description

Technical Field

[0001] The present application relates to the field of underwater positioning technology, and in particular to an inertial and acoustic combined navigation method taking into account sound line bending. Background Art

[0002] Autonomous underwater vehicles (AUVs), due to their small size, light weight, and excellent stealth, play a vital role in marine resource development and other tasks, and have become a key vehicle for marine technology applications in various countries. To ensure that AUVs can successfully complete their underwater missions, they must possess long-endurance, high-precision navigation and positioning capabilities. Therefore, underwater navigation and positioning technologies that prioritize high accuracy, autonomy, extended endurance, and reliability have become critical to deep-diving AUV applications.

[0003] The Inertial Navigation System (INS), with its autonomous, passive nature, has long been the core of deep-diving AUV navigation and positioning. INS boasts high short-term accuracy and can continuously provide comprehensive navigation information in all weather conditions, making it an essential navigation method for underwater vehicles. However, INS suffers from the drawbacks of error accumulation and the divergence of navigation errors over time, necessitating periodic error correction using other sensors. Therefore, to meet the demands of long-duration, high-precision navigation and positioning, inertial-based integrated navigation technology is a major growth area.

[0004] Because acoustic signals have the advantage of low propagation attenuation underwater, acoustic positioning technology is currently one of the keys to breakthroughs in abyssal science. Driven by portability and high precision, combined navigation technology based on acoustic systems assisted by inertial navigation has become an important means of achieving underwater AUV navigation and positioning capabilities.

[0005] In underwater acoustic positioning, changes in seawater depth, salinity, etc. can cause non-linear propagation of sound lines. The measurement models in traditional acoustic positioning are all based on the assumption that sound signals propagate in a straight line. This is suitable for small-scale, shallow water environments, but will produce large positioning errors for acoustic navigation and positioning of deep-sea AUVs. Summary of the Invention

[0006] The present application provides an inertial and acoustic combined navigation method that takes into account the curvature of sound lines, which can be used to solve the technical problem that the measurement model uses sound signals propagating in a straight line, resulting in large positioning errors in deep-sea areas.

[0007] This application provides an inertial and acoustic integrated navigation method that takes into account sound line bending, the method comprising:

[0008] Step 1: Establish a state equation model for inertial and acoustic integrated navigation;

[0009] Step 2: Establish a curved sound line measurement model based on the constant gradient sound velocity profile;

[0010] Step 3: Based on the state equation and measurement equation established in steps 1 and 2, the error is updated and fed back using the unscented Kalman filter method.

[0011] Furthermore, in step (1), a state equation model of inertial / acoustic integrated navigation is established:

[0012] Taking the position error, velocity error and attitude error of inertial navigation as the system state quantity, we can get:

[0013]

[0014] Among them, X represents the state quantity set, [φ E φ N φ U ] T Indicates the misalignment angle error of the attitude, [δV E δV N δV U ] T represents the velocity error, [δL δλ δh] T represents the position error, [εx ε y ε z ] T represents the zero bias of the gyroscope, Indicates the zero bias of the accelerometer;

[0015] Therefore, the state equation of integrated navigation is:

[0016]

[0017] Where F(t) represents the state transfer matrix, which is set according to the strapdown inertial navigation error equation, and W(t) represents the process noise matrix.

[0018] Furthermore, step 2, establishing a curved sound line measurement model based on an isogradient sound velocity profile, includes:

[0019] The underwater sound field is approximated as an isogradient sound velocity profile distribution, and the sound velocity model is:

[0020] C(z)=gz+b

[0021] Where C(z) represents the sound velocity at depth z, g represents the sound velocity gradient, and b represents the sound velocity at the water surface.

[0022] Based on the sound velocity model, the ray tracing theory is used to model the sound propagation. The ray tracing theory based on the Snell criterion is expressed as:

[0023]

[0024] Among them, θ and z represent the angle of a given point along the ray and the depth of a given point, θ A and θ B Represents the ray angles at the transponder and acoustic transducer respectively; z A and z B Indicates the depth of the transponder and acoustic transducer;

[0025] Based on the ray tracing theory, the sound wave propagation time t under the curved sound line is expressed as:

[0026]

[0027] Among them, θ B =β0-α0,θ A =β0+α0,β0=arctan[(z B -z A ) / r],α0=arctan[gr / (2b+g(z B +z A ))], r represents the horizontal distance between the transponder and the acoustic transducer;

[0028] The angle measurements (α, β) obtained by the acoustic system are expressed as follows:

[0029]

[0030] Where [xyz] represents the three-dimensional position coordinates of the transponder in the acoustic transducer coordinate system;

[0031] The nonlinear measurement equation is established as follows:

[0032] Z=h(X)

[0033] Among them, Z represents the observed quantity, that is, Represents the nonlinear measurement equation about the state X.

[0034] Furthermore, in step 3, based on the state equation and measurement equation established in steps 1 and 2, the error is updated and fed back using the unscented Kalman filter method, including:

[0035] The state equation of step 1 is:

[0036]

[0037] The measurement equation in step 2:

[0038] Z=h(X)

[0039] According to the new measurement equation, the unscented Kalman filter is used for integrated navigation. The steps are as follows:

[0040] Step a: Use unscented transformation to obtain a set of sampling points of the current state The known state mean is And variance P:

[0041]

[0042] Where n represents the state dimension and a is the scaling parameter. (*) i Represents the i-th column of the square root of the matrix.

[0043] Calculate the corresponding weight ω of the sampling point i :

[0044] ω0=a / (n+a)i=0

[0045] ω i =a / (2n+2a)i=1,2,…2n

[0046] Step b, state one-step prediction:

[0047] in, Represents the state estimation at time k-1, namely attitude error, velocity error, position error, accelerometer bias error, gyroscope bias error, F k-1 Represents the state transition matrix from time k-1 to time k;

[0048] Step c, state one-step prediction mean square error:

[0049] Among them, Q k-1 represents the process noise matrix, P k-1 represents the root mean square error at time k-1;

[0050] Step d: According to the one-step prediction value, input P k|(k-1) and Use the unscented transformation of step a to generate the sampling points and weights of the current state:

[0051]

[0052] Step e: Substitute the sampling points predicted in step (d) into the measurement equation to obtain the predicted measurement value Z k|(k-1),i :

[0053]

[0054] Step f, obtain the observed predicted value of the sampling point set through step e, and obtain the predicted mean of the system through weighted summation and variance P zz 、P zx :

[0055]

[0056] Among them, R k represents the measurement noise matrix;

[0057] Step g, calculate the gain matrix of the Kalman filter:

[0058]

[0059] Step h, calculate the state estimate and the state estimation mean square error P k :

[0060]

[0061] The present invention adopts the above technical solution and has the following beneficial effects: the inertial / acoustic combined navigation method established according to the above steps can overcome the problem of decreased positioning accuracy caused by the bending of sound lines. Compared with the traditional positioning model that assumes that sound lines propagate along straight lines, the method of the present invention has higher positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 A schematic diagram of sound line bending provided in an embodiment of the present application. DETAILED DESCRIPTION

[0063] In order to make the objectives, technical solutions and advantages of this application clearer, the implementation methods of this application will be further described in detail below with reference to the accompanying drawings.

[0064] This paper addresses the problem of inertial / acoustic combined navigation under curved sound rays. It designs an underwater sound ray measurement model based on constant gradient sound velocity profiles and combines it with an unscented Kalman filter to achieve underwater / acoustic combined navigation. The specific method is as follows:

[0065] Step 1: Establish the state equation model of inertial / acoustic integrated navigation, specifically:

[0066] Taking the position error, velocity error, and attitude error of the inertial navigation as the system state quantities, we can get:

[0067]

[0068] Among them, X represents the state quantity set, [φ E φ N φ U ]T misalignment angle error representing attitude, [δV E δV N δV U ] T velocity error representing, [δL δλ δh] T position error representing, [ε x ε y ε z ] T gyroscope bias representing, accelerometer bias representing.

[0069] Therefore, the state equation of integrated navigation is:

[0070]

[0071] wherein F(t) represents state transition matrix, which is set according to strapdown inertial navigation error equation, and W(t) represents process noise matrix.

[0072] Step 2: Establish a curved sound ray measurement model based on an equal gradient sound velocity profile, specifically:

[0073] The underwater acoustic field is approximated as an equal gradient sound velocity profile distribution, and the sound velocity can be modeled as:

[0074] C(z)=gz+b

[0075] wherein C(z) represents the sound velocity at depth z, g represents the sound velocity gradient, and b represents the water surface sound velocity size.

[0076] Based on the above sound velocity model, ray tracing theory is used to model sound ray propagation, and the ray tracing theory based on Snell's rule can be expressed as:

[0077]

[0078] wherein θ and z represent the angle along the ray and the depth of the given point, respectively, θ A and θ B represent the ray angles at the transponder and the acoustic transducer, respectively. A and z B represent the depths of the transponder and the acoustic transducer.

[0079] Based on the above ray tracing theory, the sound wave propagation time t under curved sound ray is established as:

[0080]

[0081] wherein θ B =β0-α0, θ A =β0+α0, β0=arctan[(zB -z A ) / r],α0=arctan[gr / (2b+g(z B +z A ))], r represents the horizontal distance between the transponder and the acoustic transducer.

[0082] The angle measurements (α, β) obtained by the acoustic system are expressed as follows:

[0083]

[0084] Where [xyz] represents the three-dimensional position coordinates of the transponder in the acoustic transducer coordinate system.

[0085] The nonlinear measurement equation is established as follows:

[0086] Z=h(X)

[0087] Among them, Z represents the observed quantity, that is, Represents the nonlinear measurement equation about the state X.

[0088] Step 3: Based on the state equation and measurement equation established in steps 1-2, the unscented Kalman filter method is used to update and feedback the error, specifically:

[0089] The state equation of the synthesis step (1) is:

[0090]

[0091] The measurement equation in step (2) is:

[0092] Z=h(X)

[0093] According to the new measurement equation, the unscented Kalman filter is used for integrated navigation. The steps are as follows:

[0094] (a) Use unscented transformation to obtain a set of sampling points of the current state The known state mean is And variance P:

[0095]

[0096] Where n represents the state dimension and a is the scaling parameter. (*) i Represents the i-th column of the square root of the matrix.

[0097] Calculate the corresponding weights ω of these sampling points i :

[0098] ω0=a / (n+a)i=0

[0099] ω i=a / (2n+2a)i=1,2,…2n

[0100] (b) One-step state prediction:

[0101] in, Represents the state estimation at time k-1, namely attitude error, velocity error, position error, accelerometer bias error, gyroscope bias error, F k-1 Represents the state transition matrix from time k-1 to time k.

[0102] (c) State one-step prediction mean square error:

[0103] Among them, Q k-1 represents the process noise matrix, P k-1 represents the root mean square error at time k-1.

[0104] (d) According to the one-step prediction value, input P k|(k-1) and Use the unscented transformation of step (a) to generate the sampling points and weights of the current state:

[0105]

[0106] (e) Substitute the sampling points predicted in step (d) into the measurement equation to obtain the predicted measurement value Z k|(k-1),i :

[0107]

[0108] (f) Obtain the observed predicted values ​​of the sampling point set through step (e), and obtain the predicted mean of the system through weighted summation and variance P zz 、P zx :

[0109]

[0110] Among them, R k represents the measurement noise matrix.

[0111] (g) Calculate the gain matrix of the Kalman filter

[0112]

[0113] (h) Computational state estimation and the state estimation mean square error P k :

[0114]

[0115] The above-described embodiments of the present application do not constitute a limitation on the scope of protection of the present application.

Claims

1. A combined inertial and acoustic navigation method taking into account sound ray curvature, characterized in that: The method comprises: Step 1: Establish a state equation model for inertial and acoustic integrated navigation; Step 2: Establish a curved sound line measurement model based on the constant gradient sound velocity profile; Step 3, based on the state equation and measurement equation established in steps 1 and 2, the error is updated and fed back using the unscented Kalman filter method; Step 2, establishing a curved sound line measurement model based on an isogradient sound velocity profile, includes: The underwater sound field is approximated as an isogradient sound velocity profile distribution, and the sound velocity model is: C(z)=gz+b Where C(z) represents the sound velocity at depth z, g represents the sound velocity gradient, and b represents the sound velocity at the water surface. Based on the sound velocity model, the ray tracing theory is used to model the sound propagation. The ray tracing theory based on the Snell criterion is expressed as: Among them, θ and z represent the angle of a given point along the ray and the depth of a given point, θ A and θ B Represents the ray angles at the transponder and acoustic transducer respectively; z A and z B Indicates the depth of the transponder and acoustic transducer; Based on the ray tracing theory, the sound wave propagation time t under the curved sound line is expressed as: Among them, θ B =β0-α0,θ A =β0+α0,β0=arctan[(z B -z A ) / r],α0=arctan[gr / (2b+g(z B +z A ))], r represents the horizontal distance between the transponder and the acoustic transducer; The angle measurements (α, β) obtained by the acoustic system are expressed as follows: Where [xyz] represents the three-dimensional position coordinates of the transponder in the acoustic transducer coordinate system; The nonlinear measurement equation is established as follows: Z=h(X) Among them, Z represents the observed quantity, that is, Represents the nonlinear measurement equation about the state X.

2. The method according to claim 1, wherein In step (1), the state equation model of inertial / acoustic integrated navigation is established: Taking the position error, velocity error and attitude error of inertial navigation as the system state quantity, we can get: Among them, X represents the state quantity set, [φ E φ N φ U ] T Indicates the misalignment angle error of the attitude, [δV E δV N δV U ] T represents the velocity error, [δL δλ δh] T represents the position error, [ε x ε y ε z ] T represents the zero bias of the gyroscope, Indicates the zero bias of the accelerometer; Therefore, the state equation of integrated navigation is: Where F(t) represents the state transfer matrix, which is set according to the strapdown inertial navigation error equation, and W(t) represents the process noise matrix.

3. The method according to claim 1, wherein In step 3, based on the state equation and measurement equation established in steps 1 and 2, the error is updated and fed back using the unscented Kalman filter method, including: The state equation of step 1 is: The measurement equation in step 2: Z=h(X) According to the new measurement equation, the unscented Kalman filter is used for integrated navigation. The steps are as follows: Step a: Use unscented transformation to obtain a set of sampling points of the current state The known state mean is And variance P: Where n represents the state dimension and a is the scaling parameter; (*) i represents the i-th column of the square root of the matrix; Calculate the corresponding weight ω of the sampling point i : ω0=a / (n+a)=0 ω i =a / 2n+2a)i=1,2,…2n Step b, state one-step prediction: in, Represents the state estimation at time k-1, namely attitude error, velocity error, position error, accelerometer bias error, gyroscope bias error, F k-1 Represents the state transition matrix from time k-1 to time k; Step c, state one-step prediction mean square error: Among them, Q k-1 represents the process noise matrix, P k-1 represents the root mean square error at time k-1; Step d: According to the one-step prediction value, input P k|(k-1) and Use the unscented transformation of step a to generate the sampling points and weights of the current state: Step e: Substitute the sampling points predicted in step (d) into the measurement equation to obtain the predicted measurement value Z k|(k-1),i : Step f, obtain the observed predicted value of the sampling point set through step e, and obtain the predicted mean of the system through weighted summation and variance P zz 、P zx : Among them, R k represents the measurement noise matrix; Step g, calculate the gain matrix of the Kalman filter: Step h, calculate the state estimate and the state estimation mean square error P k :

Images