Underwater single beacon virtual long baseline positioning method and system capable of estimating unknown sound velocity

Abstract

The invention belongs to the technical field of underwater positioning, and discloses an underwater single beacon virtual long baseline positioning method and system capable of estimating an unknown sound velocity. The method comprises the following steps: constructing a virtual long baseline array by combining stored dead reckoning data; constructing a virtual long baseline positioning problem by combining the relationship between the underwater acoustic signal propagation time and the position and sound velocity of the AUV; auxiliary variables are introduced, a pseudo-linear positioning equation is obtained based on algebraic conversion, and an AUV position initial solution is obtained based on a weighted least square method; and constructing a second-step linear estimation problem by exploring the relationship between the auxiliary variable and the AUV position, and solving by using weighted least squares to obtain a more accurate AUV position estimation solution. Compared with a single-beacon positioning method which does not consider sound velocity unknown, the method provided by the invention can obtain more ideal position estimation performance, and the practical application capability of an underwater single-beacon positioning system is enhanced.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater positioning, and in particular relates to an underwater single-beacon virtual long baseline positioning method and system capable of estimating unknown sound speed. Background Art

[0002] Underwater vehicles (AUVs) need to obtain accurate position feedback underwater to ensure the accuracy of their own motion control and the smooth completion of underwater operations. Since electromagnetic wave signals attenuate rapidly underwater, acoustic signals are currently used as the propagation medium for AUV underwater positioning. Compared with the more common underwater long baseline positioning and ultra-short baseline positioning methods, the underwater single beacon positioning method that combines a single hydroacoustic beacon with dead reckoning information has attracted widespread attention from researchers due to its low cost, good flexibility, and the ability to obtain bounded positioning errors. Among them, the single beacon positioning position solution method designed by constructing a virtual long baseline array has good flexibility and positioning accuracy, and therefore has good application prospects.

[0003] The current underwater single beacon positioning method usually regards the water acoustic speed as known, and directly multiplies the nominal sound speed value by the propagation time of the water acoustic signal detected by the hydrophone to obtain the distance between the AUV and the beacon as the observed value. However, in actual underwater positioning applications, affected by factors such as seawater temperature, salinity and depth, the water acoustic speed is usually time-varying and unknown. The setting error of the sound speed will cause the calculation error of the distance, which will lead to the error of navigation positioning, affecting the actual application capability of the single beacon system. Therefore, it is of great scientific and engineering significance to study the single beacon positioning method that can simultaneously estimate the AUV position and water acoustic speed.

[0004] Through the above analysis, the problems and defects of the existing technology are as follows: the existing single beacon positioning technology based on virtual long baseline cannot estimate the AUV position and sound speed at the same time, and the positioning accuracy of the AUV position is poor, resulting in large errors in navigation positioning. Summary of the invention

[0005] In order to overcome the problems existing in the related art, the embodiments disclosed in the present invention provide an underwater single-beacon virtual long baseline positioning method and system that can estimate unknown sound speed.

[0006] The technical solution is as follows: an underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed. This method aims at the problem of unknown time-varying underwater sound speed in underwater single beacon positioning. Based on the two-step weighted least squares principle, a single beacon virtual long baseline positioning solution capable of estimating unknown sound speed is constructed to complete the joint estimation of AUV position and sound speed, including the following steps:

[0007] The AUV dead reckoning information is integrated and all the acoustic signal propagation time observations within the time window width N are used to construct a virtual long baseline positioning model.

[0008] By introducing auxiliary variables and performing algebraic transformation, the pseudo-linear equation for virtual long baseline positioning is obtained, which is solved based on the weighted least squares method to obtain preliminary estimates of the sound speed and AUV position.

[0009] According to the relationship between the auxiliary variables and the AUV position, combined with the results of the preliminary estimation, the square of the AUV position and the square of the sound speed are used as unknown quantities to construct the second-step linear estimation problem, which is solved based on the weighted least squares method to obtain a more accurate least squares solution.

[0010] Combine the estimated value of the second step linear estimation problem with the preliminary estimate of the AUV position to calculate the final sound speed estimate and AUV position estimate.

[0011] Furthermore, before constructing the virtual long baseline positioning model, the following operations need to be performed:

[0012] Take any point in the positioning area as the origin, set the east, north and sky directions as X, Y and Z axes respectively, and establish the underwater single beacon positioning geodetic coordinate system; take the AUV centroid as the origin, set the front, right and bottom directions as x, y and z axes respectively, and establish the AUV body coordinate system;

[0013] According to the speed of the AUV in the body coordinate system detected by the DVL and the attitude of the AUV detected by the attitude sensor, a dead reckoning model is constructed;

[0014] The hydroacoustic beacon periodically emits hydroacoustic signals. The coordinates of the beacon position are known. The AUV is equipped with a hydrophone to measure the propagation time of the hydroacoustic signal. This is used as the observation quantity to construct the relationship between the propagation time of the hydroacoustic signal, the speed of sound, and the position of the AUV.

[0015] Furthermore, the dead reckoning model constructed, that is, the relationship between the AUV position variables at time k and time ki, where i ≥ 1, is:

[0016]

[0017]

[0018] In the formula, x k ,y k ,z k is the real position coordinate of AUV at time k, x k-i is the x-direction position coordinate of the AUV at time ki, y k-i is the y-direction position coordinate of the AUV at time ki, z k-iis the z-direction position coordinate of the AUV at time ki, Δt is the discrete time interval, i is the time index variable, and v x,k-t is the x-direction velocity of AUV at time kt, v y,k-t is the y-direction velocity of AUV at time kt, v z,k-t is the z-direction velocity of AUV at time kt;

[0019] Construct a hydroacoustic observation model under linear acoustic profile conditions, including:

[0020] The coordinates of the hydroacoustic beacon are fixed and known, recorded as:

[0021]

[0022] Where P b is the coordinate vector of the hydroacoustic beacon position, x b is the east coordinate of the hydroacoustic beacon, y b is the north coordinate of the hydroacoustic beacon, z b The celestial coordinates of the hydroacoustic beacon;

[0023] The observed value of the propagation time of the underwater acoustic signal obtained at time k is expressed as:

[0024]

[0025] Squaring both sides gives:

[0026]

[0027] Simplifying, we get:

[0028]

[0029] The propagation time of the underwater acoustic signal contains observation noise, and the observation value of the propagation time of the underwater acoustic signal is recorded as:

[0030]

[0031] In the formula, t k is the actual underwater acoustic signal propagation time, v k is the observation noise of the propagation time of the underwater acoustic signal;

[0032] Substituting it into the distance measurement equation, we get:

[0033]

[0034] Furthermore, the constructed virtual long baseline positioning model includes: combining the observation equation at time k, and obtaining all N+1 observation equations to be comprehensively expressed as:

[0035]

[0036] Substitute the AUV’s dead reckoning model, that is, the position relationship between time k and time ki, and we get:

[0037]

[0038] in,

[0039]

[0040] In the formula, x b,i ,y b,i ,z b,i Respectively represent the x, y, and z axis position coordinates of the i-th virtual beacon.

[0041] Further, the method for calculating the sound speed and the preliminary estimate of the AUV position includes:

[0042] Expand all N+1 observation equations to obtain:

[0043]

[0044] All N+1 equations are integrated into a vector matrix form:

[0045]

[0046] In the formula, B k is the noise coefficient matrix, v k is the noise vector of the underwater acoustic signal propagation time, h k ,G k are system model parameters, is the unknown quantity to be estimated, and the definitions of each variable are as follows:

[0047]

[0048]

[0049] get The weighted least squares solution of is:

[0050]

[0051] Among them, the weight matrix W 1,k Defined as:

[0052]

[0053] In the formula, E(·) represents the expected value of the random variable, and the superscript -1 represents the inverse of the matrix. It is approximately equal to Q k is the observed variance matrix of the underwater acoustic signal propagation time, defined as The specific form is:

[0054]

[0055] The weighted least squares method The estimated variance of is:

[0056]

[0057] Where D(·) represents the variance of the random variable.

[0058] Furthermore, the method for calculating the initial estimate of the speed of sound and the position of the AUV further includes: firstly, the weight W 1,k Set it as the unit matrix to calculate the preliminary least squares solution, and then bring the position coordinates corresponding to this solution into the noise coefficient matrix B k , calculate the weight W 1,k and the weighted least squares solution

[0059] Furthermore, the steps to obtain a more accurate weighted least squares solution include:

[0060] The estimated value that will be obtained Expressed as the true value and the estimated error:

[0061]

[0062] in, To estimate the error, define a new unknown quantity X k as follows:

[0063]

[0064] according to The first three elements of X k The relationship between them is:

[0065]

[0066] Among them, for any vector a, a[m:n] represents the subvector composed of the mth to nth elements of vector a, and a⊙b represents the vector formed by multiplying the corresponding elements of vector a and vector b; according to The fourth element of k The relationship between them is:

[0067]

[0068] according to The fifth element of k The relationship between them is:

[0069]

[0070] Combining the above three equations, we get:

[0071]

[0072] in,

[0073]

[0074]

[0075] χ k The weighted least squares estimate solution of is:

[0076]

[0077] Weight matrix W 2,k Set to:

[0078]

[0079] Further, the steps for calculating the estimated value of the AUV single beacon positioning position and the estimated value of the sound speed are as follows:

[0080] The estimated value obtained Take the square root and combine the estimated values The sign corresponding to the position variable is used to obtain the estimated position of the AUV. The calculation formula is as follows:

[0081]

[0082] Where sign(a) represents the sign of each element of vector a. If b = sign(a), then

[0083]

[0084] Since the speed of sound should be a positive number, the estimated value of the speed of sound is as follows:

[0085]

[0086] Another object of the present invention is to provide an underwater single beacon virtual long baseline positioning system capable of estimating an unknown sound speed, the system implements the underwater single beacon virtual long baseline positioning method capable of estimating an unknown sound speed, the system comprises an AUV, the AUV is equipped with a hydrophone, a DVL and an attitude sensor; the hydroacoustic beacon periodically broadcasts a hydroacoustic signal, the AUV periodically observes its velocity in the body coordinate system through the carried DVL, and calculates the velocity of the AUV in the geodetic coordinate system in combination with the AUV attitude angle observed by the attitude sensor, performs dead reckoning, and records and stores the dead reckoning data; the AUV receives the hydroacoustic signal emitted by the hydroacoustic beacon Finally, the propagation time of the hydroacoustic signal is obtained in combination with the known emission time of the hydroacoustic signal; when the AUV receives a certain number of hydroacoustic signals, a virtual long baseline array is constructed in combination with the stored dead reckoning data; a virtual long baseline positioning problem is constructed in combination with the relationship between the propagation time of the hydroacoustic signal and the AUV position and sound speed; auxiliary variables are introduced, and the pseudo-linear positioning equation is obtained based on algebraic transformation, and the preliminary solution of the AUV position and sound speed is obtained based on the weighted least squares method; and then the second-step linear estimation problem is constructed through the relationship between the auxiliary variables and the AUV position, and the weighted least squares method is used to solve it to obtain a more accurate AUV position and sound speed estimation solution.

[0087] Furthermore, the underwater single beacon virtual long baseline positioning method for estimating an unknown sound speed operated by the underwater single beacon virtual long baseline positioning system for estimating an unknown sound speed is carried on a computer-readable storage medium, and the computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the steps in the above-mentioned underwater single beacon virtual long baseline positioning method for estimating an unknown sound speed can be implemented.

[0088] In combination with all the above technical solutions, the beneficial effects of the present invention are as follows:

[0089] The AUV provided by the present invention is equipped with a hydrophone, a DVL and an attitude sensor; the hydroacoustic beacon periodically broadcasts a hydroacoustic signal, the AUV periodically observes its velocity in a body coordinate system through the carried DVL, and calculates the velocity of the AUV in a geodetic coordinate system in combination with the AUV attitude angle observed by the attitude sensor, performs dead reckoning, and records and stores the dead reckoning data; after receiving the hydroacoustic signal emitted by the hydroacoustic beacon, the AUV obtains the propagation time of the hydroacoustic signal in combination with the known emission time of the hydroacoustic signal; when the AUV receives a certain number of hydroacoustic signals, a virtual long baseline array is constructed in combination with the stored dead reckoning data; a virtual long baseline positioning problem is constructed in combination with the relationship between the propagation time of the hydroacoustic signal and the position and sound speed of the AUV; an auxiliary variable is introduced, a pseudo-linear positioning equation is obtained based on algebraic transformation, and a preliminary solution of the AUV position is obtained based on the weighted least squares method; and then a second-step linear estimation problem is constructed by exploring the relationship between the auxiliary variable and the AUV position, and the weighted least squares method is used to solve the problem, so as to obtain a more accurate AUV position estimation solution. The method of the present invention can realize the joint estimation of the AUV position and sound speed. Compared with the single beacon positioning method based on known sound speed, the method of the present invention can obtain more ideal position estimation performance and enhance the practical application capability of the underwater single beacon positioning system.

[0090] In the application process of the present invention, the AUV does not need to use the temperature-salinity-depth sensor to detect the sound velocity profile of the experimental area in real time, which can reduce the hardware cost of the AUV. At the same time, compared with the positioning method based on the nominal sound velocity, the method of the present invention can obtain better positioning accuracy, improve the validity and value of the data collected by the AUV, and enhance the operation and application value of the AUV itself. The present invention realizes the simultaneous estimation of the AUV position and sound velocity under the framework of virtual long baseline positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0091] The accompanying drawings herein are incorporated in and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the description, serve to explain the principles of the present disclosure;

[0092] Figure 1 It is a flow chart of an underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed provided by an embodiment of the present invention;

[0093] Figure 2 It is a schematic diagram of the underwater single beacon positioning geodetic coordinate system and the AUV body coordinate system provided by an embodiment of the present invention;

[0094] Figure 3 The motion trajectory of the AUV obtained by simulation;

[0095] Figure 4 This is a comparison chart of positioning errors of different positioning methods;

[0096] Figure 5This is a sound speed estimation error diagram of the method proposed in the present invention. DETAILED DESCRIPTION

[0097] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific implementation disclosed below.

[0098] The innovation of the present invention is that the present invention innovatively realizes the joint estimation of the AUV position and sound speed in single beacon positioning under the framework of virtual long baseline positioning. By expanding the sound speed as the unknown quantity to be estimated, the virtual long baseline positioning equation is converted into a pseudo-linear equation form by introducing auxiliary variables; the preliminary estimates of the AUV position and sound speed are obtained by weighted least squares method; by exploring the relationship between the auxiliary variables and the AUV position, the second-step linear estimation problem is constructed, and the weighted least squares method is used to solve it to obtain more accurate estimates of the AUV position and sound speed. The method of the present invention can simultaneously estimate the position and sound speed of the AUV, thereby enhancing the practical application capability of the AUV.

[0099] Aiming at the problem of unknown time-varying underwater acoustic speed in underwater single beacon positioning, the present invention constructs a single beacon virtual long baseline positioning solution based on the two-step weighted least squares principle, which can estimate the unknown sound speed, and realizes the joint estimation of AUV position and sound speed.

[0100] Embodiment 1, as Figure 1 As shown, the underwater single beacon virtual long baseline positioning method for estimating unknown sound speed provided by an embodiment of the present invention includes:

[0101] S1, take any point in the positioning area as the origin, set the east, north and sky directions as X, Y and Z axes respectively, and establish the underwater single beacon positioning geodetic coordinate system; take the AUV centroid as the origin, and set the front, right and bottom directions as x, y and z axes respectively, and establish the AUV body coordinate system;

[0102] S2, constructing a dead reckoning model based on the velocity of the AUV in the body coordinate system detected by the DVL and the attitude of the AUV detected by the attitude sensor;

[0103] S3, the hydroacoustic beacon periodically emits hydroacoustic signals. The coordinates of the beacon position are known. The AUV is equipped with a hydrophone, which can measure the propagation time of the hydroacoustic signal. This is used as the observation quantity to construct the relationship between the propagation time of the hydroacoustic signal, the speed of sound, and the position of the AUV.

[0104] S4, integrating AUV dead reckoning information and using all acoustic signal propagation time observations within the time window width N to construct a virtual long baseline positioning model;

[0105] S5, by introducing auxiliary variables and performing algebraic transformation, the pseudo-linear equation of virtual long baseline positioning is obtained, which is solved based on the weighted least squares method to obtain the preliminary estimated values ​​of the sound speed and the AUV position;

[0106] S6, based on the relationship between the auxiliary variables and the AUV position, taking the preliminary estimation results as observations, the square of the AUV position and the square of the sound speed as unknowns, construct the second-step linear estimation problem, and solve it based on the weighted least squares method to obtain a more accurate least squares solution;

[0107] S7, combining the estimated value of the second step linear estimation problem with the preliminary estimated value of the AUV position, and calculating the final sound speed estimate and the AUV position estimate.

[0108] Exemplarily, in step S1, the established geodetic coordinate system and body coordinate system are as follows: Figure 2 shown.

[0109] Exemplarily, in step S2, the dead reckoning model constructed is as follows:

[0110] The coordinates of the AUV at time k are expressed as:

[0111]

[0112] In the formula, x k is the x-coordinate at time k, y k is the y-direction coordinate at time k, z k is the z-direction coordinate at time k, and the velocity of the AUV detected by DVL at time k in the body coordinate system is recorded as:

[0113]

[0114] The AUV attitude angle detected by the attitude sensor is recorded as:

[0115]

[0116] In the formula, are the forward, rightward and downward speeds of the AUV respectively, θ k ,ψ k are the roll angle, pitch angle and heading angle of the AUV respectively. Combining the speed of the AUV in the body coordinate system and the AUV attitude angle, the speed of the AUV in the earth coordinate system at time k can be obtained as:

[0117]

[0118] Among them, R k is the rotation matrix, defined as:

[0119]

[0120] According to the position and speed of AUV at time k-1, the position coordinates of AUV at time k are obtained as follows:

[0121] P k =P k-1 +v k-1 Δt

[0122] Where Δt represents the discrete time interval of dead reckoning;

[0123] Expressing the above in component form is:

[0124] x k =x k-1 +v x,k-1 Δt

[0125] y k =y k-1 +v y,k-1 Δt

[0126] z k =z k-1 +v z,k-1 Δt

[0127] The relationship between the AUV position variables at time k and time ki (i ≥ 1) is obtained as follows:

[0128]

[0129]

[0130] In the formula, x k-i is the x-direction position coordinate of the AUV at time ki, y k-i is the y-direction position coordinate of the AUV at time ki, z k-i is the z-direction position coordinate of the AUV at time ki, Δt is the discrete time interval, i is the time index variable, and v x,k-t is the x-direction velocity of AUV at time kt, v y,k-t is the y-direction velocity of AUV at time kt, v z,k-t is the z-direction velocity of the AUV at time kt.

[0131] Exemplarily, in step S3, the constructed hydroacoustic observation model under the linear acoustic profile condition is as follows:

[0132] The coordinates of the hydroacoustic beacon are fixed and known, which can be recorded as:

[0133]

[0134] Where P b is the coordinate vector of the hydroacoustic beacon position, x b is the east coordinate of the hydroacoustic beacon, y b is the north coordinate of the hydroacoustic beacon, z b The celestial coordinates of the hydroacoustic beacon;

[0135] The observed value of the propagation time of the underwater acoustic signal obtained at time k is expressed as:

[0136]

[0137] Squaring both sides gives:

[0138]

[0139] Simplifying, we get:

[0140]

[0141] In real applications, the propagation time of the underwater acoustic signal contains observation noise. The observed value of the propagation time of the underwater acoustic signal is recorded as:

[0142]

[0143] In the formula, t k is the actual underwater acoustic signal propagation time, v k is the observation noise of the propagation time of the underwater acoustic signal;

[0144] Substituting it into the distance measurement equation, we get:

[0145]

[0146] Exemplarily, in step S4, the constructed virtual long baseline positioning model includes: combining the observation equation at time k, obtaining all N+1 observation equations to be comprehensively expressed as:

[0147]

[0148] Substitute the AUV’s dead reckoning model, that is, the position relationship between time k and time ki, and we get:

[0149]

[0150] in,

[0151]

[0152] In the formula, x b,i ,y b,i,z b,i Respectively represent the x, y, and z axis position coordinates of the i-th virtual beacon.

[0153] Exemplarily, in step S5, the method for calculating the initial estimate of the speed of sound and the position of the AUV is as follows:

[0154] Expand the observation equation in step S4 to obtain:

[0155]

[0156] All N+1 equations are integrated into a vector matrix form:

[0157]

[0158] In the formula, B k is the noise coefficient matrix, v k is the noise vector of the underwater acoustic signal propagation time, h k ,G k are system model parameters, is the unknown quantity to be estimated, and the definitions of each variable are as follows:

[0159]

[0160]

[0161] get The weighted least squares solution of is:

[0162]

[0163] Among them, the weight matrix W 1,k Defined as:

[0164]

[0165] In the formula, E(·) represents the expected value of the random variable, and the superscript -1 represents the inverse of the matrix. It is approximately equal to Q k is the observed variance matrix of the underwater acoustic signal propagation time, defined as The specific form is:

[0166]

[0167] The weighted least squares method The estimated variance of is:

[0168]

[0169] Where D(·) represents the variance of the random variable.

[0170] Noise coefficient matrix B k The calculation of depends on the position estimation of AUV. In the application process, the weight W is first 1,k Set it as the unit matrix to calculate the preliminary least squares solution, and then bring the position coordinates corresponding to this solution into the noise coefficient matrix B k , calculate the weight W 1,k and the weighted least squares solution

[0171] Exemplarily, in step S6, the present invention innovatively proposes that the steps of obtaining a more accurate weighted least squares solution are as follows:

[0172] The estimated value obtained in step S5 Expressed as the true value and the estimated error:

[0173]

[0174] in, To estimate the error, define a new unknown quantity χ k as follows:

[0175]

[0176] according to The first three elements of k The relationship between them is:

[0177]

[0178] Among them, for any vector a, a[m:n] represents the subvector composed of the mth to nth elements of vector a, and a⊙b represents the vector formed by multiplying the corresponding elements of vector a and vector b; according to The fourth element of k The relationship between them is:

[0179]

[0180] according to The fifth element of k The relationship between them is:

[0181]

[0182] Combining the above three equations, we get:

[0183]

[0184] in,

[0185]

[0186]

[0187] χ k The weighted least squares estimation solution of is:

[0188]

[0189] Weight matrix W 2,k Set to:

[0190]

[0191] Exemplarily, in step S7, the steps of calculating the estimated value of the AUV single beacon positioning position and the estimated value of the sound speed are as follows:

[0192] The estimated value obtained Take the square root and combine the estimated values The symbols corresponding to the position variables can be used to obtain the estimated position of the AUV as follows:

[0193]

[0194] Where sign(a) represents the sign of each element of vector a. If b = sign(a), then

[0195]

[0196] Since the speed of sound should be a positive number, the estimated value of the speed of sound is as follows:

[0197]

[0198] It can be understood that the present invention constructs the relationship between the auxiliary variables and the unknown quantities and uses weighted least squares to correct the weighted least squares solution of the previous step.

[0199] Illustratively, the pseudo code flow of the underwater single beacon virtual long baseline positioning method for estimating unknown sound speed provided by an embodiment of the present invention is shown in Table 1.

[0200] Table 1 Pseudocode flow of underwater single beacon virtual long baseline positioning method for estimating unknown sound speed

[0201]

[0202] Embodiment 2, the underwater single beacon virtual long baseline positioning system capable of estimating unknown sound speed provided by the embodiment of the present invention comprises:

[0203] The AUV is equipped with a hydrophone, a DVL and an attitude sensor; the hydroacoustic beacon broadcasts hydroacoustic signals periodically, and the AUV periodically observes its velocity in the body coordinate system through the DVL it carries, and calculates the velocity of the AUV in the geodetic coordinate system in combination with the AUV attitude angle observed by the attitude sensor, performs dead reckoning, and records and stores the dead reckoning data; after receiving the hydroacoustic signal emitted by the hydroacoustic beacon, the AUV obtains the propagation time of the hydroacoustic signal in combination with the known emission time of the hydroacoustic signal; when the AUV receives a certain number of hydroacoustic signals, it constructs a virtual long baseline array in combination with the stored dead reckoning data; the virtual long baseline positioning problem is constructed in combination with the relationship between the propagation time of the hydroacoustic signal and the position and sound speed of the AUV; an auxiliary variable is introduced, and a pseudo-linear positioning equation is obtained based on algebraic transformation, and a preliminary solution of the AUV position and sound speed is obtained based on the weighted least squares method; then, by exploring the relationship between the auxiliary variable and the AUV position, a second-step linear estimation problem is constructed, and the weighted least squares method is used to solve it to obtain a more accurate AUV position and sound speed estimation solution.

[0204] To further illustrate the relevant effects of the embodiments of the present invention, the following numerical simulation is performed: This embodiment simultaneously demonstrates the positioning performance of three single beacon positioning methods using distance as the observation value. Since the underwater acoustic speed is time-varying and unknown in actual applications, there is a certain deviation between the nominal sound speed values ​​and the true values ​​of the three comparison methods. By multiplying the nominal sound speed value by the observed underwater acoustic signal propagation time, the distance observation value between the AUV and the beacon can be calculated, and the distance is used to estimate the AUV position coordinates. Method 1 is based on weighted least squares to solve the virtual long baseline positioning problem (denoted as R-WLS), method 2 is based on the semidefinite programming method in the literature Yanshen Du, Ping Wei, and Huaguo Zhang. Semidefinite programming approaches for source localization problems. In 2014 IEEE International Conference on Communiction Problem-solving, pages 339–342, 2014. to solve the virtual long baseline positioning problem (denoted as R-SDR1), and method 3 is based on the semidefinite programming method in the literature KW Cheung, WK Ma, and H.C. So. Accurate approximation algorithm for to a-based maximum likelihood mobile location using semidefinite programming. In 2004 IEEE International Conference on Acoustics, Speech, and Signal Processing, volume 2, pages ii–145, 2004. to solve the virtual long baseline positioning problem (denoted as R-SDR2). The method of the present invention directly uses the underwater acoustic propagation time as an observed quantity and can estimate the unknown sound speed based on a two-step weighted least squares solution.

[0205] The simulation parameters are set as follows: AUV dead reckoning interval is 0.1 seconds, acoustic signal transmission interval is 20 seconds, and acoustic beacon is fixed at [120 20 80] T m, and the total simulation time is 1000 seconds. The AUV dynamics model is used to generate the spiral diving motion trajectory of the AUV. The control input of the AUV is horizontal rudder 3°, vertical rudder -3°, and propeller speed 1000 rpm. The initial position of the AUV is [100 100 10] T m, the forward initial velocity is 1.2 m / s, and the initial velocity in other directions is 0. The AUV motion trajectory obtained by simulation is as follows Figure 3 shown.

[0206] The standard deviation of the simulated DVL velocity observation noise in three directions is [0.01 0.01 0.01] T m / s, the standard deviation of the attitude sensor roll angle and pitch angle observation is 0.03°, the standard deviation of the heading angle observation is 0.3°, and the standard deviation of the underwater acoustic signal propagation time is 10us. The simulated sound speed value is 1530m / s, while the nominal sound speed value used by the three distance observation-based methods is 1500m / s.

[0207] For all methods, the width of the sliding window is set to 15, that is, the current moment and the 15 hydroacoustic ranging observations before the current moment are used to solve the position coordinates of the AUV at the current moment. The R-SDR1 and R-SDR2 methods are solved using the CVX toolbox of Matlab software, using the sedumi solver, and the solution accuracy is set to the highest. The references are: Michael Grant and Stephen Boyd. CVX: Matlab software for disciplined convex programming, version 2.0 beta. https: / / cvxr.com / cvx, September 2013. and Michael Grant and Stephen Boyd. Graph implementations for nonsmooth convex programs, Recent Advances in Learning and Control (a tribute to M. Vidyasagar), V. Blondel, S. Boyd, and H. Kimura, editors, pages 95-110, Lecture Notes in Control and Information Sciences, Springer, 2008. http: / / stanford.edu / ~boyd / graph_dcp.html. The positioning error and average positioning error of the four methods are used as performance evaluation criteria. The calculation formulas for positioning error and average positioning error are:

[0208]

[0209] Among them, K represents the number of single beacon positioning settlements. The positioning errors of the four methods are as follows: Figure 4 The average positioning error is shown in Table 2.

[0210] according to Figure 4As shown in Table 2, the method proposed in the present invention can obtain better positioning accuracy than the traditional single beacon positioning method based on distance observation, and its positioning error is lower than that of other comparison methods at most times. This shows that considering the unknown nature of the sound speed can improve the position estimation performance of single beacon positioning to a certain extent. Figure 5 The sound speed estimation error of the method proposed in the present invention is shown. It can be seen that at most positioning moments, the method proposed in the present invention can obtain good sound speed estimation accuracy, which further leads to better distance calculation accuracy and position estimation accuracy. The average sound speed estimation error of the method proposed in the present invention is 2.334561m / s, which is much better than the error corresponding to the nominal sound speed value (30m / s), that is, the method of the present invention can effectively eliminate the influence of the sound speed setting error. It should be noted that for the single beacon positioning of AUV, the spatial geometric relationship of the virtual long baseline beacon is directly related to the movement of the AUV itself. However, due to the weak maneuverability of most AUV bodies, the beacon layout of the virtual long baseline positioning is difficult to be as ideal as the real long baseline positioning system. This also leads to the instability of the results of solving the single beacon positioning problem based on the virtual long baseline positioning framework, that is, for certain specific moments, the positioning error suddenly becomes large, which is very important for Figure 4 This is particularly evident in the R-SDR1 method.

[0211] Although the present invention has been described in detail above by general description and specific embodiments, it is obvious to those skilled in the art that some modifications or improvements can be made to the present invention. Therefore, these modifications or improvements made without departing from the spirit of the present invention all belong to the scope of protection claimed by the present invention.

[0212] Table 2 Comparison of average positioning errors of different methods

[0213] Positioning method Average error (meters) Proposed method 0.351293 R-WLS 1.018677 R-SDR1 8.697602 R-SDR2 6.321329

[0214] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered within the protection scope of the present invention.

Claims

1. An underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed, characterized in that: This method aims to solve the problem of unknown time-varying underwater sound speed in underwater single beacon positioning. Based on the two-step weighted least squares principle, a single beacon virtual long baseline positioning solution is constructed to estimate the unknown sound speed, and the joint estimation of AUV position and sound speed is completed. The method includes the following steps: The AUV dead reckoning information is integrated and all the acoustic signal propagation time observations within the time window width N are used to construct a virtual long baseline positioning model. By introducing auxiliary variables and performing algebraic transformation, the pseudo-linear equation for virtual long baseline positioning is obtained, which is solved based on the weighted least squares method to obtain preliminary estimates of the sound speed and AUV position. According to the relationship between the auxiliary variables and the AUV position, combined with the results of the preliminary estimation, the square of the AUV position and the square of the sound speed are used as unknown quantities to construct the second-step linear estimation problem, which is solved based on the weighted least squares method to obtain a more accurate least squares solution. Combine the estimated value of the second step linear estimation problem with the preliminary estimate of the AUV position to calculate the final sound speed estimate and AUV position estimate.

2. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 1 is characterized in that: Before constructing the virtual long baseline positioning model, the following steps must be performed: Take any point in the positioning area as the origin, set the east, north and sky directions as X, Y and Z axes respectively, and establish the underwater single beacon positioning geodetic coordinate system; take the AUV centroid as the origin, set the front, right and bottom directions as x, y and z axes respectively, and establish the AUV body coordinate system; According to the speed of the AUV in the body coordinate system detected by the DVL and the attitude of the AUV detected by the attitude sensor, a dead reckoning model is constructed; The hydroacoustic beacon periodically emits hydroacoustic signals. The coordinates of the beacon position are known. The AUV is equipped with a hydrophone to measure the propagation time of the hydroacoustic signal. This is used as the observation quantity to construct the relationship between the propagation time of the hydroacoustic signal, the speed of sound, and the position of the AUV.

3. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 2 is characterized in that: The constructed dead reckoning model, i.e., the relationship between the AUV position variables at time k and time ki, where i ≥ 1, is: In the formula, x k ,y k , z k is the real position coordinate of AUV at time k, x k-i is the x-direction position coordinate of the AUV at time ki, y k-i is the y-direction position coordinate of the AUV at time ki, z k-i is the z-direction position coordinate of the AUV at time ki, Δt is the discrete time interval, i is the time index variable, and v x，k-t is the x-direction velocity of AUV at time kt, v y，k-t is the y-direction velocity of AUV at time kt, v z，k-t is the z-direction velocity of AUV at time kt; Construct a hydroacoustic observation model under linear acoustic profile conditions, including: The coordinates of the hydroacoustic beacon are fixed and known, recorded as: Where P b is the coordinate vector of the hydroacoustic beacon position, x b is the east coordinate of the hydroacoustic beacon, y b is the north coordinate of the hydroacoustic beacon, z b The celestial coordinates of the hydroacoustic beacon; The observed value of the propagation time of the underwater acoustic signal obtained at time k is expressed as: Squaring both sides gives: Simplifying, we get: The propagation time of the underwater acoustic signal contains observation noise, and the observation value of the propagation time of the underwater acoustic signal is recorded as: Where, t k is the actual underwater acoustic signal propagation time, v k is the observation noise of the underwater acoustic signal propagation time; put it into the ranging equation, and we get:

4. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 1, characterized in that: The constructed virtual long baseline positioning model includes: combining the observation equation at time k, and obtaining all N+1 observation equations as follows: Substitute the AUV’s dead reckoning model, that is, the position relationship between time k and time ki, and we get: in, In the formula, x b，i ,y b，i , z b，i Respectively represent the x, y, and z axis position coordinates of the i-th virtual beacon.

5. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 1, characterized in that: Methods for calculating the speed of sound and a preliminary estimate of the AUV's position include: Expand all N+1 observation equations to obtain: All N+1 equations are integrated into a vector matrix form: In the formula, B k is the noise coefficient matrix, v k is the noise vector of the underwater acoustic signal propagation time, h h , G k are system model parameters, is the unknown quantity to be estimated, and the definitions of each variable are as follows: get The weighted least squares solution of is: Among them, the weight matrix W 1，k Defined as: In the formula, E(·) represents the expected value of the random variable, and the superscript -1 represents the inverse of the matrix. It is approximately equal to Q k is the observed variance matrix of the underwater acoustic signal propagation time, defined as The specific form is: The weighted least squares method The estimated variance of is: Where D(·) represents the variance of the random variable.

6. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 5, characterized in that: The method for calculating the initial estimate of the speed of sound and the position of the AUV further includes: firstly setting the weight W 1，k Set it as the unit matrix to calculate the preliminary least squares solution, and then bring the position coordinates corresponding to this solution into the noise coefficient matrix B k , calculate the weight W 1，k and the weighted least squares solution 7. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 1, characterized in that: The steps to obtain a more accurate weighted least squares solution include: The estimated value that will be obtained Expressed as the true value and the estimated error: in, To estimate the error, define a new unknown quantity χ k as follows: according to The first three elements of k The relationship between them is: Among them, for any vector a, a[m:n] represents the subvector composed of the mth to nth elements of vector a, and a⊙b represents the vector formed by multiplying the corresponding elements of vector a and vector b; according to The fourth element of k The relationship between them is: according to The fifth element of k The relationship between them is: Combining the above three equations, we get: in, χ k The weighted least squares estimate solution of is: Weight matrix W 2,k Set to:

8. The underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed according to claim 1, characterized in that: The steps to calculate the estimated value of the AUV single beacon positioning position and the estimated value of the sound speed are as follows: The estimated value obtained Take the square root and combine the estimated values The sign corresponding to the position variable is used to obtain the estimated position of the AUV. The calculation formula is as follows: Where sign(a) represents the sign of each element of vector a. If b = sign(a), then Since the speed of sound should be a positive number, the estimated value of the speed of sound is as follows:

9. An underwater single beacon virtual long baseline positioning system capable of estimating unknown sound speed, characterized in that: The system is implemented by the underwater single beacon virtual long baseline positioning method capable of estimating unknown sound speed as claimed in any one of claims 1 to 8, the system comprises an AUV, the AUV is equipped with a hydrophone, a DVL and an attitude sensor; the hydroacoustic beacon periodically broadcasts a hydroacoustic signal, the AUV periodically observes its velocity in the body coordinate system through the carried DVL, calculates the velocity of the AUV in the geodetic coordinate system in combination with the AUV attitude angle observed by the attitude sensor, performs dead reckoning, and records and stores the dead reckoning data; After receiving the hydroacoustic signal emitted by the hydroacoustic beacon, the AUV obtains the propagation time of the hydroacoustic signal in combination with the known hydroacoustic signal emission time; when the AUV receives a certain number of hydroacoustic signals, it constructs a virtual long baseline array in combination with the stored dead reckoning data; Combining the relationship between the propagation time of the underwater acoustic signal and the AUV position and sound speed, a virtual long baseline positioning problem is constructed; auxiliary variables are introduced, and the pseudo-linear positioning equation is obtained based on algebraic transformation, and the preliminary solution of the AUV position and sound speed is obtained based on the weighted least squares method; then, through the relationship between the auxiliary variables and the AUV position, the second-step linear estimation problem is constructed, and the weighted least squares method is used to solve it to obtain a more accurate AUV position and sound speed estimation solution.

10. The underwater single beacon virtual long baseline positioning system capable of estimating unknown sound speed according to claim 9, characterized in that: The underwater single beacon virtual long baseline positioning method for estimating an unknown sound speed operated by the underwater single beacon virtual long baseline positioning system for estimating an unknown sound speed is carried on a computer-readable storage medium, and the computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the steps in the underwater single beacon virtual long baseline positioning method for estimating an unknown sound speed can be implemented.

Images