Underwater vehicle positioning method based on motion compensation model and sound velocity error estimation

Abstract

The invention discloses an underwater vehicle positioning method based on a motion compensation model and sound velocity error estimation, which relates to the technical field of ocean measurement and comprises the following steps: deducing a layered approximate calculation formula of sound ray propagation time and horizontal distance quantity on the basis of a ray acoustic propagation model; constructing a motion compensation model of the submersible vehicle, fitting motion state coordinates of the submersible vehicle by using a primary function about time, and performing linearization calculation of sound propagation time about a to-be-estimated coefficient in a ray acoustic propagation model; establishing a sound velocity error estimation model, enabling the sound velocity error to be equivalent to sound velocity deviation in each positioning period, and carrying out the linearization calculation of the sound propagation time with respect to the sound velocity deviation under a ray acoustic propagation model. According to the invention, the method can not only avoid solving the position coordinates of the transmitting and receiving signals of the transponders to compensate the motion model error of the submersible vehicle, but also can compensate the sound velocity error to improve the positioning precision of the submersible vehicle.

Description

Technical Field

[0001] The present invention relates to the field of ocean measurement technology, and in particular to an underwater submersible positioning method based on a motion compensation model and sound speed error estimation. Background Art

[0002] Marine scientific research, marine exploration, and marine safety require the assistance of new underwater submersibles. Accurate location information is crucial for the correlation of information obtained by underwater submersible detection. However, due to the rapid attenuation of electromagnetic signals in underwater environments, the global navigation satellite system commonly used on the ground and in the air cannot be used underwater. Usually, different positioning methods need to be used according to different working environments. In the past two decades, with the development of hydroacoustic signal processing technology, acoustic-assisted underwater submersible navigation and positioning technology has made great progress. Long baseline positioning is one of the most commonly used acoustic positioning methods for assisting submersibles, and has been successfully used in many marine engineering projects and scientific observation systems.

[0003] The long baseline positioning system usually consists of a transducer installed on the submersible and an acoustic transponder with a known position in a fixed area. In the long baseline acoustic measurement, the submersible sends an interrogation acoustic signal, the transponder receives the signal and returns the response signal, and then measures the round-trip acoustic propagation time observation, combined with the sound speed value, to achieve the underwater submersible position estimation. In 2012, Thomson and Dosso published a paper entitled "AUV localization in an underwater acoustic positioning system" in the magazine "Canadian Acoustics", using the synchronous positioning and map construction method to locate and navigate the underwater submersible, but in order to simplify the algorithm, the above underwater positioning system ignores the movement of the submersible during the interrogation and reception process. That is, it is assumed that the transducer of the submersible remains stationary during the time interval between the transmission and reception of the signal. However, when the acoustic signal propagates over a large range underwater, the movement of the submersible causes the transducer's position at the time of sending the signal and the time of receiving the signal to change significantly. Ignoring the movement of the submersible during the acoustic measurement will affect the accuracy of the positioning model, thereby affecting the positioning results. In addition, due to the influence of water temperature, salinity and depth, the speed of sound changes in time and space, which will affect the propagation path of sound waves in the ocean medium. In current practical applications, the speed of sound is obtained by assuming a constant speed of sound or measuring the velocity profile of the positioning area based on experience. However, there are often errors in the empirical value or measured profile.

[0004] In summary, when an underwater submersible transmits and receives acoustic signals in acoustic measurement, it usually moves at a certain speed, and the positions of the transducer's transmitted and received acoustic signals change. Ignoring the changes in the positions of the underwater submersible's transmitted and received acoustic signals, as well as the ocean sound speed error that changes in time and space, will affect the positioning results of the submersible. Therefore, establishing a compensation model for the submersible motion model and estimating the complex sound speed error are issues that need to be solved urgently. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides an underwater submersible positioning method based on a motion compensation model and sound speed error estimation, comprising the following steps:

[0006] S1. According to Snell's law, the hierarchical approximate formula of the time and horizontal distance of sound propagation is derived and calculated, and the ray acoustic propagation model is constructed;

[0007] S2. Use the basis function about time to fit the motion state coordinates of the underwater submersible, perform linear calculation of the sound propagation time about the coefficient to be estimated in the ray acoustic propagation model, and construct an underwater submersible motion compensation model;

[0008] S3. In each positioning cycle, the sound speed error is equivalent to the sound speed deviation. The ray acoustic propagation model is used to perform linear calculation of the sound propagation time with respect to the sound speed deviation, and a sound speed error estimation model is established.

[0009] The technical solution further defined in the present invention is:

[0010] Furthermore, in step S2, the underwater vehicle motion compensation model estimates the position of the transducer at the time when it receives the acoustic signal returned by the seabed transponder by adding the motion interpolation analysis of the underwater vehicle during the propagation of the acoustic signal; if the underwater vehicle-borne transducer is at the time of transmitting the signal T t The three-dimensional position vectors in the x-direction, y-direction and z-direction are x t =[x t ,y t , z t ] T , the position vectors of the jth transponder deployed on the seabed in the x-direction, y-direction and z-direction are X j =[X j , Y j , Z j ] T , j = 1, ..., J, where J is the total number of deployed seabed transponders; at time T r The x-, y- and z-direction positions of the transducer receiving the acoustic signal returned by the jth seafloor transponder at time r，j =[x r，j ,yr，j , z r，j ] T , the corresponding sound propagation time is measured as t j .

[0011] As described above, in the underwater submersible positioning method based on the motion compensation model and the sound speed error estimation, in step S2, the motion state of the underwater submersible is simulated using the linear fitting method of the basis function, and the problem of the transducer position coordinates to be estimated is converted into the problem of estimating the basis function coefficients, thereby estimating the position x of the transducer at the moment of receiving the acoustic signal returned by the seabed transponder. r，j ; The position coordinates x(t) and y(t) of the transducer at time t are expressed by the linear combination of basis functions as follows:

[0012]

[0013] in, represents the kth basis function of the x coordinate in each positioning cycle, a x，k Represents the coefficient corresponding to the basis function; represents the kth basis function of the y coordinate in each positioning cycle, a y,k represents the coefficient corresponding to the basis function; k=1, ..., K, indicating that the number of basis functions in the positioning period is K.

[0014] As described above, in the underwater submersible positioning method based on the motion compensation model and the sound speed error estimation, in step S3, in each positioning cycle, the sound speed error is modeled as the sound speed deviation e c , at this time, the underwater submersible positioning model is expressed as:

[0015] f(m,x j , c(z)) = t j , j = 1, ..., J

[0016] Among them, X j represents the jth seafloor transponder position, c(z) represents the sound velocity profile; m = [a x,k , a y，k , e c ] T represents the parameter to be estimated, a x，k represents the coefficient of the kth basis function corresponding to the x coordinate in each positioning cycle, a y，k represents the coefficient of the kth basis function corresponding to the y coordinate in each positioning cycle; f represents the sound path calculation model calculated using the ray acoustic model.

[0017] As described above, in the underwater submersible positioning method based on the motion compensation model and the sound speed error estimation, in step S3, the solution of the sound path calculation model f calculated by the ray acoustic model includes the calculation of the sound propagation time and the horizontal distance, which specifically includes the following steps:

[0018] S3.1. Use the basis functions and their initial coefficients to calculate the position x of the transducer in the x-direction, y-direction, and z-direction at the time of transmitting and receiving the signal. t =[x t ,y t , z t ] T and x r =[x r ,y r , z r ] T ;

[0019] S3.2, combined with the position vector X=[X, Y, Z] of the seabed transponder in the x-direction, y-direction and z-direction T , the acoustic travel time observations are calculated using the ray acoustic propagation model;

[0020] S3.3. According to the differential calculation, the sound line propagation path is layered in depth and in reverse, and the sound propagation time and horizontal distance on each layered microelement are calculated. Finally, the calculation formulas for the two-way sound propagation time t and the horizontal distance r are obtained respectively:

[0021]

[0022] Among them, z t and z r represents the depth value of the transducer at the time of transmitting and receiving the signal, Z represents the depth value of the seabed transponder, c(z′) represents the sound velocity value at the depth z′, and n t and n r They respectively represent the sound line parameters of the round trip process of the two-way sound path.

[0023] As described above, in the underwater vehicle positioning method based on the motion compensation model and sound speed error estimation, in step S3.3, n t and n r They represent the sound line parameters of the round trip process of the two-way sound path. According to Snell's refraction law, the two sound line parameters remain unchanged on the entire sound line, and their sizes are calculated using the horizontal distance r of the transmitted sound line. t The horizontal distance r that the return sound ray propagates r Solved by Newton's method, n t and n r At the initial value n 0，t and n 0，r The iterative calculation formula is:

[0024]

[0025] Among them, r(n 0,t ) and r(n0,r ) indicates the use of the sound line parameter n 0，t and n 0，r The calculated horizontal distance that the sound ray travels.

[0026] As described above, in the underwater submersible positioning method based on the motion compensation model and the sound speed error estimation, in step S3, since using the sound path calculation model f to solve the model parameter m is a nonlinear problem, a Taylor series expansion calculation is performed on f at the initial value point m0, and the first-order function form is retained, and the calculation formula is obtained as follows:

[0027] f(m)≈f(m0)+J(m-m0)

[0028] Among them, the Jacobian matrix

[0029] As described above, in the underwater submersible positioning method based on the motion compensation model and sound speed error estimation, in step S3, the Bayesian criterion is used to solve the model parameter m. When the prior information of the estimated model parameters and the observation data error conform to the Gaussian distribution, the estimate corresponding to the maximum posterior probability density is the optimal solution of the model parameters, and the linearized Bayesian iterative calculation formula of m and the corresponding posterior covariance matrix are obtained.

[0030] As described above, in the underwater submersible positioning method based on the motion compensation model and sound speed error estimation, the Bayesian criterion used in step S3 is:

[0031]

[0032] The posterior probability density function is shown as follows:

[0033] P(m|t)∝P(t|m)P(m)

[0034] Where t represents the acoustic observation vector consisting of a set of acoustic propagation time observations t, P(m|t) represents the posterior probability density of the model parameter m given the known observation data t, P(m) represents the prior probability density of the model parameter m, P(t|m) represents the probability density distribution function of t given m, and P(t) represents the prior probability density distribution of the observation data.

[0035] As described above, in the underwater submersible positioning method based on the motion compensation model and the sound speed error estimation, in step S3, when the prior information of the estimated model parameters and the observed data error conform to the Gaussian distribution, the mismatch function Θ(m) is expressed as:

[0036]

[0037] The estimate corresponding to the maximum posterior probability density is the optimal solution for the model parameters, and is called the maximum a posteriori estimate. Therefore, the optimal solution is obtained by minimizing the mismatch function Θ(m), and making: Therefore, the linearized Bayesian iterative calculation formula of m and the corresponding posterior covariance matrix are obtained as follows:

[0038]

[0039] in, Represents the covariance matrix of the model parameter priors, C t Represents the covariance matrix of the sound travel time observations.

[0040] The beneficial effects of the present invention are:

[0041] (1) In the present invention, based on the ray acoustic propagation model, a hierarchical approximate calculation formula for the time and horizontal distance of sound line propagation is derived, thereby solving the influence of the bending of sound line propagation;

[0042] (2) In the present invention, the motion state coordinates of the submersible are fitted using a basis function with respect to time to compensate for the motion error of the underwater submersible during acoustic measurement;

[0043] (3) In the present invention, the sound speed error is equivalent to the sound speed deviation in each positioning cycle, and a sound speed error estimation model is established, thereby solving the problem that the sound speed error that changes in complex time and space affects the positioning accuracy of the submarine. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0045] Figure 2 A distribution diagram of the seabed transponders and underwater submersible tracks in a simulation scenario in an embodiment of the present invention;

[0046] Figure 3 Schematic diagram of ocean sound speed data in a simulation scenario of an embodiment of the present invention, wherein (a) is a schematic diagram of ocean sound speed simulation with depth stratification, and (b) is a schematic diagram of sound speed error with the number of observations;

[0047] Figure 4 Schematic diagrams of root mean square error results of underwater submersible positioning using different methods in embodiments of the present invention, wherein (a) is a schematic diagram of the root mean square error results of underwater submersible positioning when a motion compensation model is used, (b) is a schematic diagram of the root mean square error results of underwater submersible positioning when a sound speed error compensation model is used, and (c) is a schematic diagram of the root mean square error results of underwater submersible positioning when the method of this embodiment is used;

[0048] Figure 5Schematic diagram of the root mean square error of AUV positioning under different observation error conditions of the method of the present invention, wherein (a) is a schematic diagram of the root mean square error results of underwater submersible positioning when the standard deviation of the sound propagation time measurement error of the method of the present invention is 100us, and (b) is a schematic diagram of the root mean square error results of underwater submersible positioning when the standard deviation of the sound propagation time measurement error of the method of the present invention is 500us. DETAILED DESCRIPTION

[0049] The present embodiment provides an underwater submersible positioning method based on a motion compensation model and sound speed error estimation, which is used to solve the underwater submersible motion model error compensation and complex change sound speed error estimation problems. First, a ray acoustic propagation model is used to derive a hierarchical approximation formula for calculating the time and horizontal distance of sound line propagation. Then, an underwater submersible motion compensation model is constructed, and a basis function about time is used to fit the motion state coordinates of the underwater submersible. The sound propagation time is linearly calculated with respect to the coefficients to be estimated in the ray acoustic propagation model. Then, a sound speed error estimation model is established, and the sound speed error is equivalent to the sound speed deviation in each positioning cycle. The ray acoustic propagation model is used to linearize the sound propagation time with respect to the sound speed deviation, and the maximum a posteriori estimation result of the iterative calculation model parameters is obtained, and finally the underwater submersible positioning result is output.

[0050] like Figure 1 As shown, the underwater submersible positioning method based on motion compensation model and sound speed error estimation provided in this embodiment includes the following steps:

[0051] S1. According to Snell's law, the hierarchical approximate formulas of the time and horizontal distance of sound propagation are derived and calculated, and the ray acoustic propagation model is constructed.

[0052] Underwater submersible positioning is based on long-baseline acoustic positioning, which usually consists of a transducer installed on the underwater submersible and an acoustic transponder with a known position in a fixed area. When performing a mission, the underwater submersible usually needs to navigate according to a pre-set trajectory, that is, the position of the transducer changes at the time of transmitting and receiving signals; in the long-baseline acoustic measurement, the underwater submersible sends an interrogation acoustic signal, the transponder receives the signal and returns a response signal, and then the round-trip sound propagation time observation is measured, combined with the sound speed value, to realize the underwater submersible position estimation; the sound speed profile data is measured using a sound speed profiler.

[0053] S2. Use the basis function about time to fit the motion state coordinates of the underwater submersible, perform linear calculation of the sound propagation time with respect to the coefficients to be estimated in the ray acoustic propagation model, and construct an underwater submersible motion compensation model.

[0054] If the submersible transducer transmits the signal at time T t The three-dimensional position vectors in the x-direction, y-direction and z-direction are xt =[x t ,y t , z t ] T , the position vectors of the jth transponder deployed on the seabed in the x-direction, y-direction and z-direction are X j =[X j , Y j , Z j ] T , j = 1, ..., J, where J is the total number of deployed seabed transponders; at time T r The x-, y- and z-direction positions of the transducer receiving the acoustic signal returned by the jth seafloor transponder at time r，j =[x r，j ,y r，j , z r,j ] T , the corresponding sound propagation time is measured as t j .

[0055] In order to estimate the position x of the transducer when it receives the acoustic signal returned by the seabed transponder r，j , the linear fitting method of basis function is used to simulate the motion state of the underwater submersible, and the problem of estimating the position coordinates of the transducer to be estimated is converted into the problem of estimating the coefficients of the basis function; the position coordinates x(t) and y(t) of the transducer at time t are expressed by the linear combination of basis functions as follows:

[0056]

[0057] in, represents the kth basis function of the x coordinate in each positioning cycle, a x，k Represents the coefficient corresponding to the basis function; represents the kth basis function of the y coordinate in each positioning cycle, a y，k represents the coefficient corresponding to the basis function; k=1, ..., K, indicating that the number of basis functions in the positioning period is K.

[0058] S3. In each positioning cycle, the sound speed error is equivalent to the sound speed deviation. The ray acoustic propagation model is used to perform linear calculation of the sound propagation time with respect to the sound speed deviation, and a sound speed error estimation model is established.

[0059] In each positioning cycle, the sound speed error is modeled as the sound speed deviation e c , at this time, the underwater submersible positioning model is expressed as:

[0060] f(m,X j , c(z)) = t j , j = 1, ..., J

[0061] Among them, Xj represents the jth seafloor transponder position, c(z) represents the sound velocity profile; m = [a x,k , a y，k , e c ] T represents the parameter to be estimated, a x，k represents the coefficient of the kth basis function corresponding to the x coordinate in each positioning cycle, a y，k represents the coefficient of the kth basis function corresponding to the y coordinate in each positioning cycle; f represents the sound path calculation model calculated using the ray acoustic model.

[0062] Calculate the position x of the transducer in the x-direction, y-direction, and z-direction at the moment of transmitting and receiving the signal t =[x t ,y t , z t ] T and x r =[x r ,y r , z r ] T , then combine the position vector X = [X, Y, Z] of the seabed transponder in the x-direction, y-direction and z-direction T , the sound propagation time observation is calculated using the ray acoustic propagation model. According to Snell's law, the sound ray parameter n is:

[0063]

[0064] Among them, θ k Indicates the sound line at depth z k The sound ray grazing angle at k ) represents the depth z k The corresponding sound velocity value.

[0065] Finally, according to the idea of ​​differential calculation, the sound propagation path is layered in depth and retrograde, and the sound propagation time and horizontal distance on each layered microelement are calculated. Finally, the calculation formulas for the two-way sound propagation time t and horizontal distance r are obtained respectively:

[0066]

[0067] Among them, z t and z r represents the depth value of the transducer at the time of transmitting and receiving the signal, Z represents the depth value of the seabed transponder, c(z′) represents the sound velocity value at the depth z′, and n t and n rThey represent the sound line parameters of the round trip process of the two-way sound path. According to the Shell refraction law, the two sound line parameters remain unchanged along the entire sound line, and their sizes are calculated using the horizontal distance r of the transmitted sound line. t The horizontal distance r that the return sound ray propagates r Solved by Newton's method, n t and n r At the initial value n 0,t and n 0，r The iterative calculation formula is:

[0068]

[0069] Among them, r(n 0,t ) and r(n 0，r ) indicates the use of the sound line parameter n 0，t and n 0，r The calculated horizontal distance that the sound ray travels.

[0070] And the calculation formulas of n0 are:

[0071]

[0072] In the formula, the calculation formula of c0 is:

[0073]

[0074] In order to effectively calculate the integral of the sound propagation time t and the horizontal distance r, the layered approximation method is used to calculate the sound line path; assuming that the sound velocity gradient of each layer is constant, the sound velocity at any depth z in the layer can be calculated using the depth z i The speed of sound c i It is expressed as c(z)=c i +v i (zz i ), where v i is the absolute sound velocity gradient in the i-th layer, and its magnitude can be calculated as:

[0075]

[0076] In addition, the relative sound velocity gradient u in the i-th layer is defined as i for:

[0077]

[0078] In order to solve the sound propagation path in each sound speed layer, the sound line trajectory in the layer is approximated as a circular arc. The curvature radius of the circular arc in the i-th sound speed layer is:

[0079] R=1 / |ucosθ|

[0080] The calculation formula for the sound propagation path length ds in each layer is:

[0081]

[0082] If the sound line is at depth z i and z i+1 The sound ray grazing angles are θ i and θ i+1 , the sound propagation time calculation formula in the i-th layer is:

[0083]

[0084] The two-way sound propagation time t is calculated by cumulative calculation as follows:

[0085]

[0086] In the formula, and w Z Respectively represent the depth z t 、z r and the number of layers of Z. In addition,

[0087] Similarly, the two-way sound propagation distance r is calculated by accumulating and The calculation formula is:

[0088]

[0089] Since using the sound path calculation model f to solve the model parameter m is a nonlinear problem, the Taylor series expansion calculation is performed on f at the initial value point m0, and the first-order function form is retained, and the calculation formula is obtained as follows:

[0090] f(m)≈f(m0)+J(m-m0)

[0091] Among them, the Jacobian matrix in and The calculation formulas are:

[0092]

[0093] Considering therefore and The calculation formula is:

[0094]

[0095] According to the linear combination of basis functions, the position coordinate relationship is: and The calculation formulas are:

[0096]

[0097] The Jacobian matrix J The calculation formula is:

[0098]

[0099] because and It can be written as:

[0100]

[0101] like In the constant sound velocity gradient layer, γ i The calculation formula is:

[0102]

[0103] therefore The calculation formula is:

[0104]

[0105] In the formula, and w Z Respectively represent the depth z t 、z r and the number of layers corresponding to Z, and

[0106] In order to solve the model parameter m, the Bayesian criterion used is:

[0107]

[0108] Where t represents the acoustic observation vector consisting of a set of sound propagation time observations t, P(m|t) represents the posterior probability density (PPD) of the model parameter m given the known observation data t, P(m) represents the prior probability density of the model parameter m, P(t|m) represents the probability density distribution function of t given m, and P(t) represents the prior probability density distribution of the observation data.

[0109] The posterior probability density function can be written as:

[0110] P(m|t)∝P(t|m)P(m).

[0111] When the prior information of the estimated model parameters and the observed data errors conform to the Gaussian distribution, the mismatch (negative likelihood) function Θ(m) is expressed as:

[0112]

[0113] The estimate corresponding to the maximum PPD is the optimal solution for the model parameters, and is called the maximum a posteriori estimate. Therefore, the optimal solution is obtained by minimizing the mismatch function Θ(m), and making: Therefore, the linearized Bayesian iterative calculation formula of m and the corresponding posterior covariance matrix are obtained as follows:

[0114]

[0115] in, Represents the covariance matrix of the model parameter priors, C t Represents the covariance matrix of the sound travel time observations.

[0116] Monte Carlo (MC) simulation is used to analyze the performance of the proposed method. In the simulation, the seabed transponders are placed at positions [1000, 0, 1600] m, [0, -1000, 1600] m, [-1000, -0, 1600] m and [0, 1000, 1600] m, as shown in the attached figure. Figure 2 As shown in the figure, the underwater submersible moves at a speed of 1 m / s, moving from the position [2000, 2000, 10]m from east to west to the position [-2000, 2000, 10]m, and then repeats the movement from west to east from the position [2000, 1990, 10]m. The final track covers the following figure: Figure 2 The area shown.

[0117] The acoustic measurement period of the transducer on the underwater submersible is 10s, and the measurement error of the sound propagation time obeys a Gaussian distribution with a mean of zero and a standard deviation of 50us. In addition, in order to simulate and analyze the influence of the sound velocity error on the positioning error of the underwater submersible, the following is used Figure 3 The sound velocity profile shown in Figure (a) simulates the ocean sound velocity that varies with depth, and this sound velocity profile is used as the background sound velocity profile to superimpose all sound propagation time observations as shown in the attached figure. Figure 3 The change of sound velocity error shown in Figure (b).

[0118] As attached Figure 4 The motion compensation model shown in Figure (a) is a calculation process that uses basis functions to fit the position coordinates of the underwater vehicle. The results show that after motion model compensation, the underwater vehicle positioning results in most areas still have a root mean square error of more than 2m. This result is affected by the coupling of the static approximation model error and the ocean sound speed error. Therefore, using only the motion compensation model cannot completely compensate for the static approximation model error of the autonomous underwater vehicle (AUV).

[0119] As attached Figure 4The sound speed error compensation model shown in Figure (b) adds ocean sound speed error estimation to the static approximate model, which shows that the sound speed error compensation has a better compensation effect on the inside of the four seabed transponders and the outside of the circular ring with a radius of 2000m. There is still obvious positioning error in the circular ring area, which is mainly affected by the static approximate model.

[0120] As attached Figure 4 As shown in Figure (c), the root mean square error of AUV positioning is almost within 0.1m. Figure 4 Figure (a) and attached Figure 4 The result shown in Figure (b) shows that the method of this embodiment can well compensate for the influence of the two errors on the AUV positioning result by adding sound speed error estimation to the motion compensation model.

[0121] In addition, the method of this embodiment uses different observation errors for simulation calculation analysis; it is assumed that the acoustic observation error obeys a zero-mean Gaussian distribution, as shown in the attached Figure 5 Figures (a) and (b) show the root mean square error results of underwater vehicle positioning when the standard deviation of the sound propagation time measurement error of the method of this embodiment is 100us and 500us respectively. Other simulation conditions and attached Figure 4 The conditions for the results shown are the same.

[0122] Attached Figure 5 The maximum RMS error of the underwater vehicle positioning with a time error of 100us shown in Figure (a) does not exceed 4m, and is less than 2m in most areas. This result is significantly worse than that of the Figure 4 The results are shown in Figure (c); Figure 5 As shown in Figure (b), the maximum value of the root mean square error of the submersible positioning with a time error of 500us does not exceed 10m, and is less than 6m in most areas. That is, as the quality of the observation data deteriorates, the root mean square error of the underwater submersible positioning increases.

[0123] The method of this embodiment can not only avoid solving the position coordinates of the transmitted and received signals of several transponders to compensate for the error of the submarine motion model, but also can compensate for the sound speed error to improve the positioning accuracy of the submarine.

[0124] In addition to the above embodiments, the present invention may also have other implementation modes. Any technical solution formed by equivalent replacement or equivalent transformation falls within the protection scope required by the present invention.

Claims

1. A method for positioning an underwater vehicle based on a motion compensation model and sound velocity error estimation, characterized in that: The following steps are involved: S1. According to Snell's law, the hierarchical approximate formula of the time and horizontal distance of sound propagation is derived and calculated, and the ray acoustic propagation model is constructed; S2. Use the basis function about time to fit the motion state coordinates of the underwater submersible, perform linear calculation of the sound propagation time about the coefficient to be estimated in the ray acoustic propagation model, and construct an underwater submersible motion compensation model; S3. In each positioning cycle, the sound speed error is equivalent to the sound speed deviation. The ray acoustic propagation model is used to perform linear calculation of the sound propagation time with respect to the sound speed deviation, and a sound speed error estimation model is established.

2. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 1 is characterized in that: In step S2, the underwater vehicle motion compensation model estimates the position of the transducer at the time when the transducer receives the acoustic signal returned by the seabed transponder by adding the motion interpolation analysis of the underwater vehicle during the acoustic signal propagation process; if the underwater vehicle-borne transducer transmits the signal at the time T t The three-dimensional position vectors in the x-direction, y-direction and z-direction are x t =[x t ,y t , z t ] T , the position vectors of the jth transponder deployed on the seabed in the x-direction, y-direction and z-direction are X j =[X j , Y j , Z j ] T , j = 1, ..., J, where J is the total number of deployed seabed transponders; at time T r The x-, y- and z-direction positions of the transducer receiving the acoustic signal returned by the jth seafloor transponder at time r，j =[x r，j ,y r，j , z r，j ] T , the corresponding sound propagation time is measured as t j .

3. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 2 is characterized in that: In step S2, the motion state of the underwater submersible is simulated by using a linear fitting method of basis functions, and the problem of estimating the position coordinates of the transducer to be estimated is converted into the problem of estimating the coefficients of the basis functions, thereby estimating the position x of the transducer at the moment when it receives the acoustic signal returned by the seabed transponder. r，j ; The position coordinates x(t) and y(t) of the transducer at time t are expressed by the linear combination of basis functions as follows: in, represents the kth basis function of the x coordinate in each positioning cycle, a x，k Represents the coefficient corresponding to the basis function; represents the kth basis function of the y coordinate in each positioning cycle, ay, k represents the coefficient corresponding to the basis function; k=1, ..., K, represents the number of basis functions in the positioning cycle is K.

4. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 1 is characterized in that: In step S3, in each positioning cycle, the sound speed error is modeled as the sound speed deviation e c , at this time, the underwater submersible positioning model is expressed as: f(m,X j ,c(z))=t j ,j=1,...,J Among them, X j represents the jth seafloor transponder position, c(z) represents the sound velocity profile; m = [a x,k , a y，k , e c ] T represents the parameter to be estimated, a x，k represents the coefficient of the kth basis function corresponding to the x coordinate in each positioning cycle, a y,k represents the coefficient of the kth basis function corresponding to the y coordinate in each positioning cycle; f represents the sound path calculation model calculated using the ray acoustic model.

5. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 4 is characterized in that: In step S3, the solution of the sound path calculation model f calculated by the ray acoustic model includes the calculation of the sound propagation time and the horizontal distance, which specifically includes the following steps: S3.

1. Use the basis functions and their initial coefficients to calculate the position x of the transducer in the x-direction, y-direction, and z-direction at the time of transmitting and receiving the signal. t =[x t ,y t , z t ] T and x r =[x r ,y r , z r ] T ; S3.2, combined with the position vector X=[X, Y, Z] of the seabed transponder in the x-direction, y-direction and z-direction T , the acoustic travel time observations are calculated using the ray acoustic propagation model; S3.

3. According to the differential calculation, the sound line propagation path is layered in depth and in reverse, and the sound propagation time and horizontal distance on each layered microelement are calculated. Finally, the calculation formulas for the two-way sound propagation time t and the horizontal distance r are obtained respectively: Among them, z t and z r represents the depth value of the transducer at the time of transmitting and receiving the signal, Z represents the depth value of the seabed transponder, c(z′) represents the sound velocity value at the depth z′, and n t and n r They respectively represent the sound line parameters of the round trip process of the two-way sound path.

6. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 5 is characterized in that: In step S3.3, n t and n r They represent the sound line parameters of the round trip process of the two-way sound path. According to Snell's refraction law, the two sound line parameters remain unchanged on the entire sound line, and their sizes are calculated using the horizontal distance r of the transmitted sound line. t The horizontal distance r that the return sound ray propagates r Solved by Newton's method, n t and n r At the initial value n 0，t and n 0，r The iterative calculation formula is: Among them, r(n 0,t ) and r(n 0,r ) indicates the use of the sound line parameter n 0，t and n 0，r The calculated horizontal distance that the sound ray travels.

7. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 1 is characterized in that: In step S3, since using the acoustic path calculation model f to solve the model parameter m is a nonlinear problem, a Taylor series expansion calculation is performed on f at the initial value point m0, and the first-order function form is retained, and the calculation formula is obtained as follows: f(m)≈f(m0)+J(m-m0) Here, the Jacobian matrix is ​​.

8. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 7 is characterized in that: In step S3, the Bayesian criterion is used to solve the model parameter m. When the prior information of the estimated model parameter and the observed data error conform to the Gaussian distribution, the estimate corresponding to the maximum posterior probability density is the optimal solution of the model parameter, and the linearized Bayesian iterative calculation formula of m and the corresponding posterior covariance matrix are obtained.

9. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 8, characterized in that: In step S3, the Bayesian criterion used is: The posterior probability density function is shown as follows: P(m|t)∝P(t|m)P(m) Where t represents the acoustic observation vector consisting of a set of acoustic propagation time observations t, P(m|t) represents the posterior probability density of the model parameter m given the known observation data t, P(m) represents the prior probability density of the model parameter m, P(t|m) represents the probability density distribution function of t given m, and P(t) represents the prior probability density distribution of the observation data.

10. The underwater submersible positioning method based on motion compensation model and sound speed error estimation according to claim 8, characterized in that: In step S3, when the prior information of the estimated model parameters and the observed data error conform to the Gaussian distribution, the mismatch function Θ(m) is expressed as: The estimate corresponding to the maximum posterior probability density is the optimal solution for the model parameters, and is called the maximum a posteriori estimate. Therefore, the optimal solution is obtained by minimizing the mismatch function Θ(m), and making: Therefore, the linearized Bayesian iterative calculation formula of m and the corresponding posterior covariance matrix are obtained as follows: in, Represents the covariance matrix of the model parameter priors, C t Represents the covariance matrix of the sound travel time observations.

Images