Inertia and acoustics integrated navigation method considering sound ray bending

Abstract

The invention provides an inertia and acoustics integrated navigation method considering sound ray bending. The method comprises the following steps: step 1, establishing a state equation model of inertia and acoustics integrated navigation; step 2, establishing a curved sound ray measurement model based on an equal gradient sound velocity profile; and step 3, according to the state equations and the measurement equations established in the step 1 to the step 2, performing error updating and feedback by using an unscented Kalman filtering method. According to the invention, the problem that the positioning precision is reduced due to bending of sound rays can be solved, and the positioning precision is higher.

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 the bending of sound lines. Background Art

[0002] Autonomous Underwater Vehicle (AUV) plays an important role in tasks such as marine resource development due to its small size, light weight, good concealment and other advantages. It has now become an important carrier for marine technology applications in various countries. In order to ensure that underwater vehicles can successfully complete relevant underwater tasks, they must have long-term, high-precision navigation and positioning capabilities. Therefore, underwater navigation and positioning technology guided by high accuracy, good autonomy, long flight time and high reliability has become the key to deep-diving AUV applications.

[0003] The Inertial Navigation System (INS) has always been the core of deep-diving AUV navigation and positioning due to its autonomous and passive advantages. Inertial navigation has the advantages of high short-term accuracy and can provide all navigation information continuously in all weather conditions. It is an indispensable navigation method for underwater vehicles. However, inertial navigation has the disadvantages of error accumulation and long-term navigation error divergence, and needs to be supplemented by other sensors for periodic error correction. Therefore, in order to meet the needs of long-duration and high-precision navigation and positioning, inertial-based combined navigation technology is the main technical growth point.

[0004] Since acoustic signals have the advantage of low propagation attenuation underwater, acoustic positioning technology is one of the keys to breakthroughs in deep-sea science. Guided by portability and high precision, combined navigation technology based on acoustic system-assisted inertial navigation has become an important means to achieve underwater AUV navigation and positioning functions.

[0005] In underwater acoustic positioning, changes in seawater depth, salinity, etc. will 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 to propagate along a straight line, resulting in large positioning errors in deep sea areas.

[0007] The present application provides an inertial and acoustic combined navigation method taking into account the bending of sound lines, the method comprising:

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

[0009] Step 2, establishing a curved sound line measurement model based on an isogradient sound velocity profile;

[0010] Step 3: Based on the state equation and measurement equation established in steps 1 to 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 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 speed 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] Among them, 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 is modeled as:

[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 on the water surface;

[0022] Based on the sound speed 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 Respectively represent the ray angles at the transponder and acoustic transducer; z A and z B Indicates the depth of the transponder, 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 measurement (α, β) obtained by the acoustic system is 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, according to the state equation and measurement equation established in steps 1 to 2, an unscented Kalman filter method is used to update and feedback the error, including:

[0035] The state equation of step 1 is:

[0036]

[0037] The measurement equation in step 2 is:

[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 the variance P:

[0041]

[0042] Where n is 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 transfer 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 the sound line. Compared with the traditional positioning model that assumes that the sound line propagates in a straight line, 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 the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0064] Aiming at the problem of inertial / acoustic combined navigation under curved sound lines, the present invention designs an underwater sound line measurement model based on isogradient sound velocity profile and combines it with an unscented Kalman filter to realize 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 quantity, we get:

[0067]

[0068] Among them, X represents the set of state quantities, [φ E φ N φ U ]T Indicates the misalignment angle error of the attitude, [δV E δV N δV U ] T represents the speed 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.

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

[0070]

[0071] Among them, 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.

[0072] Step 2: Establish a curved sound line measurement model based on the constant gradient sound velocity profile, specifically:

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

[0074] C(z)=gz+b

[0075] Among them, C(z) represents the sound speed at depth z, g represents the sound speed gradient, and b represents the sound speed on the water surface.

[0076] Based on the above sound speed model, the ray tracing theory is used to model the sound propagation. The ray tracing theory based on the Snell criterion can be expressed as:

[0077]

[0078] 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. A and z B Indicates the depth of the transponder, acoustic transducer.

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

[0080]

[0081] Among them, θ 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 measurement (α, β) obtained by the acoustic system is 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 the variance P:

[0095]

[0096] Where n is 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 value of the sampling point set through step (e), and obtain the system predicted mean 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) Calculate state estimate 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 protection scope of the present application.

Claims

1. An inertial and acoustic combined navigation method considering sound line bending, characterized in that: The method comprises: Step 1, establish a state equation model for inertial and acoustic integrated navigation; Step 2, establishing a curved sound line measurement model based on an isogradient sound velocity profile; Step 3: Based on the state equation and measurement equation established in steps 1 to 2, the error is updated and fed back using the unscented Kalman filter method.

2. The method according to claim 1, characterized in that 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 get: Among them, X represents the set of state quantities, [φ E φ N δ U ] T Indicates the misalignment angle error of the attitude, [δV E δV N δV U ] T represents the speed 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: Among them, 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, characterized in that 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 is modeled as: 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 on the water surface; Based on the sound speed 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 Respectively represent the ray angles at the transponder and acoustic transducer; z A and z B Indicates the depth of the transponder, 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 measurement (α, β) obtained by the acoustic system is 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.

4. The method according to claim 1, characterized in that In step 3, according to the state equation and measurement equation established in steps 1 to 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 is: 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 the variance P: Where n is 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)i=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 transfer 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