Underwater single beacon semi-definite planning positioning method and system capable of estimating unknown sound speed

Abstract

The present invention belongs to the field of underwater positioning technology, and discloses a semi-definite programming positioning method and system for underwater single beacon that can estimate unknown sound speed. Based on the pseudo-linear positioning equation, a constrained least squares problem is constructed, and auxiliary variables are further introduced to construct linear constraints. Based on the semi-definite relaxation principle, the optimization problem is converted into a semi-definite programming problem, and a numerical solution is performed based on the interior point method to obtain preliminary estimates of the AUV position and sound speed; the preliminary estimate error is explicitly expressed and brought back to the virtual long baseline positioning equation to obtain a weighted least squares estimate of the preliminary estimate error; the estimated error is used to correct the preliminary estimate of the semi-definite programming to obtain a more accurate estimate of the AUV position and sound speed. Compared with the single beacon positioning method that does not consider the unknown sound speed, the method of the present invention can obtain better positioning accuracy and enhance the practical application capability of the underwater single beacon positioning system.

Description

Technical Field

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

[0002] Accurate and real-time position feedback is the basis for underwater vehicles (AUVs) to complete their assigned tasks. Since electromagnetic wave signals attenuate rapidly underwater, acoustic signals are often used as the propagation medium for AUV underwater positioning. Currently, the more common underwater acoustic positioning methods are long baseline positioning and ultra-short baseline positioning. Long baseline positioning requires the deployment, calibration, and recovery of multiple underwater beacons, which is costly and time-consuming, while ultra-short baseline positioning requires detailed calibration of the position and attitude of the acoustic array. In contrast, the underwater single beacon positioning method that combines a single underwater acoustic beacon with dead reckoning information has attracted widespread attention from researchers in recent years due to its advantages such as low cost, good flexibility, and the ability to obtain bounded positioning errors.

[0003] Current underwater single-beacon positioning methods typically treat the water acoustic velocity as a constant known value, directly multiplying the nominal velocity 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, the water acoustic velocity is affected by physical factors such as seawater temperature, salinity, and depth. Its value is usually time-varying and unknown, related to time and space. Errors in the sound velocity setting will cause errors in distance calculation, which in turn will lead to errors in navigation positioning, affecting the practical application capabilities of the single-beacon system. Therefore, research on single-beacon positioning methods that can estimate the unknown water acoustic velocity while estimating the AUV's position has important application prospects.

[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 simultaneously estimate the AUV position and sound speed, the obtained AUV positioning position accuracy is poor, and the navigation error is large. Summary of the Invention

[0005] To overcome the problems existing in the related art, the present invention discloses a method and system for underwater single-beacon semidefinite programming positioning that can estimate unknown sound speed. The present invention aims to address the problem of unknown time-varying underwater sound speed in underwater single-beacon positioning by constructing a semidefinite programming solution for underwater single-beacon positioning that can estimate unknown sound speed based on semidefinite relaxation, thereby achieving joint estimation of AUV position and sound speed.

[0006] The technical solution is as follows: an underwater single-beacon semi-definite planning positioning method capable of estimating unknown sound speed includes:

[0007] Based on the relationship between the propagation time of underwater acoustic signals and the speed of sound as well as the position of the AUV, the AUV dead reckoning information is integrated and all the observations of the propagation time of underwater acoustic signals within the time window width N are used to construct a virtual long baseline positioning model.

[0008] By introducing auxiliary variables, the constructed virtual long baseline positioning model is algebraically transformed to obtain the virtual long baseline positioning pseudo linear equation;

[0009] A constrained optimization problem is constructed and solved by semidefinite programming problem through semidefinite relaxation to obtain a preliminary estimate of the single beacon positioning position and sound speed.

[0010] The estimation error of the single beacon positioning position and sound speed preliminary estimation solution is explicitly expressed, and based on weighted least squares, the single beacon positioning preliminary position and sound speed estimation solution is corrected using the virtual long baseline positioning equation to obtain the final position estimate.

[0011] Furthermore, the relationship between the propagation time of the underwater acoustic signal, the speed of sound, and the position of the AUV is constructed, including:

[0012] Taking any point in the positioning area as the origin, the east, north and sky directions are set as X, Y and Z axes respectively, and the geodetic coordinate system for underwater single beacon positioning is established. The position coordinates of the underwater acoustic beacon in the geodetic coordinate system are fixed and known, which can be expressed as:

[0013]

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

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

[0016]

[0017] Where, t k is the real underwater acoustic signal propagation time, c is the real sound speed, x k ,y k ,z k is the real position coordinate of AUV at time k;

[0018] Square both sides to get:

[0019]

[0020] Simplifying, we get:

[0021]

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

[0023]

[0024] Where, is the observed value of underwater acoustic signal propagation time, v k is the observation noise of the underwater acoustic signal propagation time;

[0025] Substituting it into the ranging equation and ignoring the second-order noise term, we get:

[0026]

[0027] Furthermore, before constructing the relationship between the underwater acoustic signal propagation time, sound speed, and AUV position, it is necessary to:

[0028] With the AUV centroid as the origin, the front, right, and bottom directions are the x, y, and z axes respectively, and the AUV body coordinate system is established;

[0029] According to the AUV velocity in the body coordinate system detected by the DVL and the AUV attitude detected by the attitude sensor, a dead reckoning model is constructed. The relationship between the AUV position variables at time k and time ki (i≥1) in the dead reckoning model is:

[0030]

[0031] Where 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, 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.

[0032] Furthermore, a virtual long baseline positioning model is constructed as follows:

[0033] Define the time window width as N, and use the propagation time of the underwater acoustic signal received from time kN to k to solve the position coordinates of the AUV at time k. For time ki, the relationship between the constructed underwater acoustic signal propagation time, sound speed and AUV position is as follows:

[0034]

[0035] Where, is the observation value of underwater acoustic signal propagation time at time ki, v k-i is the observation noise of underwater acoustic signal propagation time at time ki;

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

[0037]

[0038] definition:

[0039]

[0040] Where x b,i is the x-axis position coordinate of the i-th virtual beacon, y b,i is the y-axis position coordinate of the i-th virtual beacon, z b,i is the z-axis position coordinate of the i-th virtual beacon;

[0041] get:

[0042]

[0043] Further, define:

[0044] x b,0 =x b

[0045] y b,0 =y b

[0046] z b,0 =z b

[0047] Where x b,0 、y b,0 、z b,0 Defined as the virtual beacon coordinates at time k-0;

[0048] Then, combined with the observation equation at time k, all N+1 observation equations are expressed as follows:

[0049]

[0050] Furthermore, the pseudo-linear equation of virtual long baseline positioning is obtained, including: expanding the observation equation in the constructed virtual long baseline positioning model to obtain:

[0051]

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

[0053]

[0054] Where 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 all system model parameters, is the unknown quantity to be estimated, and the definitions of each variable include:

[0055]

[0056]

[0057] Where S k is an auxiliary variable, is the observed value of underwater acoustic signal propagation time at time kN, v k-N is the underwater acoustic signal propagation time observation noise at time kN.

[0058] Furthermore, a preliminary estimated solution for the single beacon positioning position and sound speed is obtained, including: according to the constructed virtual long baseline positioning pseudo-linear equation, the estimation problem is transformed into a constrained optimization problem, which is expressed as:

[0059]

[0060] Where, is the estimated value of the unknown quantity, the superscript T represents the transpose, for any vector a, a[m:n] represents the subvector consisting of the mth to nth elements of vector a, that is, for The fifth element of for The first three elements of || || 2 represents the vector two norm; W 1,k is the weight matrix, defined as:

[0061]

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

[0063]

[0064] Where, is the observed variance of the underwater acoustic signal propagation time at time k-1, is the observed variance of the underwater acoustic signal propagation time at time kN, is the observation variance of the underwater acoustic signal propagation time at time k;

[0065] The objective function of the optimization problem is transformed into:

[0066]

[0067] Where tr represents the trace of the matrix;

[0068] Defining auxiliary variables Then the constrained optimization problem is transformed into:

[0069]

[0070] For any matrix A, A[m:n,p:q] represents the submatrix consisting of the elements from the mth to nth rows and the pth to qth columns of matrix A. The last constraint in the above equation is mathematically equivalent to:

[0071]

[0072] rank(Ψ k )=1

[0073] In the formula, rank(Ψ k ) is the matrix Ψ k The rank of the problem is removed, and the rank 1 constraint is removed. The optimization problem is transformed into a semidefinite programming problem, which can be efficiently solved numerically by the interior point method to obtain the optimal solution of the semidefinite programming. and The initial estimates of the AUV position and sound speed are extracted as:

[0074]

[0075] Where, is the initial estimate of the speed of sound, is the preliminary estimated value of the AUV position at time k.

[0076] Furthermore, the noise coefficient matrix B k The calculation depends on the sound velocity value. In the application process, the weight W 1,k Set it as the unit matrix and calculate the preliminary least squares solution Substitute the sound velocity value corresponding to this solution into the noise coefficient matrix B k , calculate the weight W 1,k .

[0077] Furthermore, the initial position and sound velocity estimation solutions of the single beacon positioning are corrected using the virtual long baseline positioning equation, including: defining the estimation error corresponding to the obtained preliminary estimated values ​​of the AUV position and sound velocity; and using the estimated error variables to correct the preliminary estimated values ​​of the semidefinite programming to obtain the final estimated values.

[0078] Furthermore, the estimation error corresponding to the obtained AUV position and the preliminary estimated value of the sound speed is defined as follows:

[0079]

[0080] Where Δx k for The estimation error, Δy k for The estimation error, Δz k for The estimation error, Δc is The estimation error of

[0081] Substituting the above equation into get:

[0082]

[0083] Ignoring the second-order noise term, we can simplify to:

[0084]

[0085] definition:

[0086]

[0087] get:

[0088]

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

[0090] B k v k =g k -F k Δη k

[0091] Where g k With F k is the system model parameter, Δη k is the unknown quantity to be estimated, and each variable is defined as follows;

[0092]

[0093] Based on the weighted least squares estimation principle, the unknown quantity Δη is obtained k The weighted least squares estimation solution of for:

[0094]

[0095] Using the estimated error variable The semidefinite programming preliminary estimate is corrected to obtain the final estimate as follows:

[0096]

[0097] Where, is the final estimate of the x-position, is the final estimated value of the y-direction position, is the final estimated value of the z-direction position, is the final estimate of the speed of sound.

[0098] Another object of the present invention is to provide an underwater single beacon semi-definite planning positioning system capable of estimating an unknown sound speed, the system being implemented by the underwater single beacon semi-definite planning positioning method capable of estimating an unknown sound speed, the system comprising an AUV;

[0099] The AUV is equipped with a hydrophone, DVL, and attitude sensor;

[0100] The underwater acoustic beacon broadcasts underwater acoustic signals periodically. The AUV uses the DVL on board to periodically measure its velocity in the body coordinate system. Combined with the AUV attitude angle observed by the attitude sensor, the AUV velocity in the geodetic coordinate system is calculated to perform dead reckoning and record and store the dead reckoning data.

[0101] After receiving the acoustic signal transmitted by the acoustic beacon, the hydrophone carried by the AUV obtains the propagation time of the acoustic signal based on the known acoustic signal transmission time.

[0102] When the AUV receives a certain number of underwater acoustic signals, it constructs a virtual long baseline array based on the stored dead reckoning data. It also constructs a virtual long baseline positioning problem based on the relationship between the underwater acoustic signal propagation time and the AUV's position and sound speed.

[0103] Auxiliary variables are introduced to obtain a pseudo-linear positioning equation based on algebraic transformation. A constrained least squares problem is constructed based on the pseudo-linear positioning equation. Auxiliary variables are further introduced to construct linear constraints. Based on the semidefinite relaxation principle, the optimization problem is transformed into a semidefinite programming problem. A numerical solution is performed using the interior point method to obtain preliminary estimates of the AUV's position and sound speed.

[0104] The error of the preliminary estimate is explicitly expressed and brought back into the virtual long baseline positioning equation to obtain the weighted least squares estimate of the preliminary estimate error. The estimated error is used to correct the preliminary estimate of the semidefinite programming to obtain more accurate AUV position and sound speed estimates.

[0105] Combining all the above technical solutions, the beneficial effects of the present invention are as follows: the present invention provides an AUV equipped with a hydrophone, a DVL and an attitude sensor; the hydroacoustic beacon periodically broadcasts hydroacoustic signals, and 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; 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; and in combination with the hydroacoustic signal The relationship between the propagation time and the AUV position and sound speed is used to construct a virtual long baseline positioning problem; auxiliary variables are introduced to obtain a pseudo-linear positioning equation based on algebraic transformation; a constrained least squares problem is constructed based on the pseudo-linear positioning equation, and auxiliary variables are further introduced to construct linear constraints. Based on the semidefinite relaxation principle, the optimization problem is converted into a semidefinite programming problem, and numerical solution is performed based on the interior point method to obtain a preliminary estimate of the AUV position and sound speed; the preliminary estimate error is explicitly expressed and brought back to the virtual long baseline positioning equation to obtain a weighted least squares estimate of the preliminary estimate error; the estimated error is used to correct the preliminary estimate of the semidefinite programming to obtain a more accurate estimate of the AUV position and sound speed. The method of the present invention can simultaneously estimate the AUV position and sound speed. Compared with the single beacon positioning method based on the nominal sound speed, the method of the present invention can obtain better positioning accuracy and enhance the practical application capability of the underwater single beacon positioning system.

[0106] During application, the method of the present invention eliminates the need for the AUV to utilize a sound velocity profiler or temperature-salinity-depth sensor to measure the sound velocity profile of the experimental area in real time, thereby reducing the hardware cost of the AUV. Furthermore, compared to positioning methods based on nominal sound velocity, the method of the present invention can achieve better positioning accuracy, improve the validity and value of the data collected by the AUV, and enhance the operational and application value of the AUV itself. The method of the present invention achieves simultaneous estimation of the AUV's position and sound velocity within the framework of virtual long-baseline positioning. Currently, no related technology has emerged, thus filling the technical gap of joint position-sound velocity estimation within the framework of virtual long-baseline positioning. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0108] Figure 1 This is a flow chart of a method for underwater single-beacon semi-definite planning positioning capable of estimating unknown sound speed provided by an embodiment of the present invention;

[0109] Figure 2Schematic diagram of the underwater single beacon positioning geodetic coordinate system and the AUV body coordinate system provided by an embodiment of the present invention;

[0110] Figure 3 This is a motion trajectory diagram of an AUV obtained by simulation according to an embodiment of the present invention;

[0111] Figure 4 This is a comparison chart of positioning errors of different positioning methods provided by an embodiment of the present invention;

[0112] Figure 5 3 is a schematic diagram of the sound speed estimation error of the method proposed in the present invention provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0113] To make the above-mentioned objects, features, and advantages of the present invention more readily apparent, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings. The following description sets forth numerous specific details to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways than those described herein, and those skilled in the art may make similar modifications without departing from the scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0114] The innovation of the present invention is that it realizes the joint estimation of the position and sound speed of the AUV with single beacon positioning under the framework of virtual long baseline positioning. By expanding the sound speed to the unknown quantity to be estimated, the virtual long baseline positioning equation is converted into a pseudo-linear equation form by introducing an auxiliary variable; by introducing a semidefinite relaxation technique to construct a semidefinite programming problem; the preliminary estimate of the AUV position and sound speed is solved by the interior point method; finally, by expressing the preliminary estimate error and bringing it back to the virtual long baseline positioning equation, a more accurate estimate of the AUV position and sound speed is obtained. 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.

[0115] In embodiment 1, an underwater single-beacon semi-definite planning positioning system for estimating unknown sound speed provided by an embodiment of the present invention includes an underwater vehicle AUV;

[0116] The AUV is equipped with a hydrophone, a Doppler Velocity Log (DVL), and an attitude sensor. The hydroacoustic beacon periodically broadcasts acoustic signals. The AUV periodically observes its velocity in the body coordinate system through the DVL it carries. Combined with the AUV's attitude angle observed by the attitude sensor, the AUV's velocity in the geodetic coordinate system is calculated, and dead reckoning is performed. The dead reckoning data is then recorded and stored.

[0117] After receiving the acoustic signal transmitted by the acoustic beacon, the hydrophone carried by the AUV obtains the propagation time of the acoustic signal based on the known acoustic signal transmission time.

[0118] When the AUV receives a certain number of underwater acoustic signals, it constructs a virtual long baseline array based on the stored dead reckoning data. It also constructs a virtual long baseline positioning problem based on the relationship between the underwater acoustic signal propagation time and the AUV's position and sound speed.

[0119] Auxiliary variables are introduced to obtain a pseudo-linear positioning equation based on algebraic transformation. A constrained least squares problem is constructed based on the pseudo-linear positioning equation. Auxiliary variables are further introduced to construct linear constraints. Based on the semidefinite relaxation principle, the optimization problem is transformed into a semidefinite programming problem. A numerical solution is performed using the interior point method to obtain preliminary estimates of the AUV's position and sound speed.

[0120] The error of the preliminary estimate is explicitly expressed and brought back into the virtual long baseline positioning equation to obtain the weighted least squares estimate of the preliminary estimate error. The estimated error is used to correct the preliminary estimate of the semidefinite programming to obtain more accurate AUV position and sound speed estimates.

[0121] Example 2, as Figure 1 As shown, the underwater single beacon semi-definite planning positioning method for estimating unknown sound speed provided by an embodiment of the present invention includes:

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

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

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

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

[0126] S5, by introducing auxiliary variables and performing algebraic transformation, the pseudo linear equation of virtual long baseline positioning is obtained;

[0127] S6, construct a constrained optimization problem, and solve it by constructing a semidefinite programming problem through semidefinite relaxation to obtain a preliminary estimate of the single beacon positioning position and sound speed.

[0128] In S7, the estimation error of the preliminary estimation solution of the single beacon positioning position and sound speed is explicitly expressed. Based on weighted least squares, the preliminary estimation solution of the single beacon positioning position and sound speed is corrected using the virtual long baseline positioning equation to obtain the final position and sound speed estimation.

[0129] In step S1, the established geodetic coordinate system and body coordinate system are as follows: Figure 2 As shown;

[0130] In step S2, the dead reckoning model is constructed as follows:

[0131] The horizontal coordinate of AUV at time k is expressed as:

[0132]

[0133] Among them, x k represents the x-direction coordinate at time k, y k represents the y-direction coordinate at time k, z k represents the z-direction coordinate at time k, and the speed of the AUV detected by DVL at time k in the body coordinate system is recorded as:

[0134]

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

[0136]

[0137] Where, are the forward, rightward and downward speeds of the AUV respectively, θ k ,ψ k They are the roll angle, pitch angle and heading angle of the AUV respectively.

[0138] The present invention mainly considers the problem of unknown sound speed in single beacon positioning, and designs a solution method under the virtual long baseline positioning framework, which can obtain better AUV position estimation accuracy than the single beacon positioning method that does not consider the unknown sound speed. The method of the present invention does not require setting a nominal sound speed value or measuring the sound speed value in advance.

[0139] Combining the velocity of the AUV in the body coordinate system and the AUV attitude angle, the velocity of the AUV in the earth coordinate system at time k can be obtained as:

[0140]

[0141] Where R k is the rotation matrix, defined as:

[0142]

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

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

[0145] Among them, v k-1 Δt represents the discrete time interval of dead reckoning;

[0146] Expressing the above in component form is:

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

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

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

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

[0151]

[0152] Where 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, 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.

[0153] In step S3, the relationship between the constructed underwater acoustic signal propagation time, sound speed, and AUV position is as follows:

[0154] The coordinates of the underwater acoustic beacon are fixed and known, which can be recorded as:

[0155]

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

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

[0158]

[0159] Where, t k is the real underwater acoustic signal propagation time, c is the real sound speed, x k ,y k ,z k is the real position coordinate of AUV at time k;

[0160] Square both sides to get:

[0161]

[0162] Simplifying, we get:

[0163]

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

[0165]

[0166] Where, is the observed value of underwater acoustic signal propagation time, v k is the observation noise of the underwater acoustic signal propagation time;

[0167] Substituting it into the ranging equation and ignoring the second-order noise term, we get:

[0168]

[0169] It can be understood that by expressing the sound speed in the observation equation, it is convenient to realize the joint estimation of the AUV position and the sound speed in the later stage, thereby obtaining a more accurate AUV position coordinate.

[0170] In step S4, the constructed virtual long baseline positioning model is as follows:

[0171] The time window width is defined as N, and the propagation time of the underwater acoustic signal received in the time period from kN to k is used to solve the position coordinates of the AUV at time k. For time ki, the relationship between the propagation time of the underwater acoustic signal constructed by the present invention, the sound speed, and the AUV position is as follows:

[0172]

[0173] Where, is the observation value of underwater acoustic signal propagation time at time ki, vk-i is the observation noise of underwater acoustic signal propagation time at time ki;

[0174] Substitute the AUV’s dead reckoning model, i.e. the position relationship between time k and time ki, and we get:

[0175]

[0176] definition:

[0177]

[0178] Where x b,i is the x-axis position coordinate of the i-th virtual beacon, y b,i is the y-axis position coordinate of the i-th virtual beacon, z b,i is the z-axis position coordinate of the i-th virtual beacon;

[0179] get:

[0180]

[0181] Further, define:

[0182] x b,0 =x b

[0183] y b,0 =y b

[0184] z b,0 =z b

[0185] Where x b,0 、y b,0 、z b,0 Defined as the virtual beacon coordinates at time k-0;

[0186] Then, combined with the observation equation at time k, all N+1 observation equations are expressed as follows:

[0187]

[0188] Existing virtual long baseline positioning methods all construct equations based on distance as an observation, while the method of the present invention is constructed based on signal propagation time as an observation. It is necessary to express the speed of sound in the equation to facilitate the simultaneous estimation of the speed of sound in the later stage. In this step, the speed of sound is regarded as an unknown quantity to be estimated, and the propagation time of the underwater acoustic signal is used as the observation. This is the main difference between the present invention and the existing virtual long baseline positioning method. Through this modeling, the subsequent AUV position-sound speed synchronous estimation can be achieved, enhancing the positioning accuracy and application capabilities of the method of the present invention. The existing virtual long baseline positioning method directly uses distance as an observation and needs to set the nominal value of the sound speed. If the nominal value of the sound speed is not set accurately, it will cause errors in ranging and positioning. The method of the present invention overcomes this defect and directly uses the signal propagation time as the observation to synchronously estimate the sound speed and position during the positioning process.

[0189] In step S5, the method for obtaining the virtual long baseline positioning pseudo linear equation is as follows:

[0190] By expanding the observation equation in step S4, the present invention innovatively obtains:

[0191]

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

[0193]

[0194] Where 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 all system model parameters, is the unknown quantity to be estimated, and the definitions of each variable include:

[0195]

[0196] Where S k is an auxiliary variable, is the observed value of underwater acoustic signal propagation time at time kN, v k-N is the underwater acoustic signal propagation time observation noise at time kN.

[0197] Furthermore, a preliminary estimated solution for the single beacon positioning position and sound speed is obtained, including: according to the constructed virtual long baseline positioning pseudo-linear equation, the estimation problem is transformed into a constrained optimization problem, which is expressed as:

[0198]

[0199] Where, is the estimated value of the unknown quantity, the superscript T represents the transpose, for any vector a, a[m:n] represents the subvector consisting of the mth to nth elements of vector a, that is, for The fifth element of for The first three elements of || || 2 represents the vector two norm; W 1,k is the weight matrix, defined as:

[0200]

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

[0202]

[0203] Where, is the observed variance of the underwater acoustic signal propagation time at time k-1, is the observed variance of the underwater acoustic signal propagation time at time kN, is the observation variance of the underwater acoustic signal propagation time at time k;

[0204] The objective function of the optimization problem is transformed into:

[0205]

[0206] Where tr represents the trace of the matrix;

[0207] Defining auxiliary variables Then the constrained optimization problem is transformed into:

[0208]

[0209] For any matrix A, A[m:n,p:q] represents the submatrix consisting of the elements from the mth to nth rows and the pth to qth columns of matrix A. The last constraint in the above equation is mathematically equivalent to:

[0210]

[0211] rank(Ψ k )=1

[0212] In the formula, rank(Ψ k ) is the matrix Ψ kThe rank of the problem is removed, and the rank 1 constraint is removed. The optimization problem is transformed into a semidefinite programming problem, which can be efficiently solved numerically by the interior point method to obtain the optimal solution of the semidefinite programming. and The initial estimates of the AUV position and sound speed are extracted as:

[0213]

[0214]

[0215] Where, is the initial estimate of the speed of sound, is the preliminary estimated value of the AUV position at time k.

[0216] In step S7, the method for correcting the preliminary position and sound speed estimation solution of single beacon positioning is as follows:

[0217] The estimated error corresponding to the preliminary estimated values ​​of the AUV position and sound speed obtained in step S6 is defined as:

[0218]

[0219] Where Δx k for The estimation error, Δy k for The estimation error, Δz k for The estimation error, Δc is The estimation error of

[0220] Substituting the above equation into get:

[0221]

[0222] Ignoring the second-order noise term, we can simplify to:

[0223]

[0224] definition:

[0225]

[0226] get:

[0227]

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

[0229] B k v k =gk -F k Δη k

[0230] Where g k With F k is the system model parameter, Δη k is the unknown quantity to be estimated, and each variable is defined as follows;

[0231]

[0232] Based on the weighted least squares estimation principle, the unknown quantity Δη is obtained k The weighted least squares estimation solution of for:

[0233]

[0234] Using the estimated error variable The semidefinite programming preliminary estimate is corrected to obtain the final estimate as follows:

[0235]

[0236] Where, is the final estimate of the x-position, is the final estimated value of the y-direction position, is the final estimated value of the z-direction position, is the final estimate of the speed of sound.

[0237] By further correcting the preliminary estimated value and directly utilizing the nonlinear relationship in the virtual long baseline positioning equation, the approximate error of constructing the pseudo-linear equation by introducing auxiliary variables in steps S5-S6 is eliminated, thus obtaining a more accurate estimation result.

[0238] Illustratively, the pseudo code of the underwater single-beacon semi-definite planning positioning method for estimating unknown sound speed provided in an embodiment of the present invention may be the steps provided in Table 1.

[0239] Table 1 Pseudo code of underwater single beacon semidefinite programming positioning method for estimating unknown sound speed

[0240]

[0241] To further illustrate the effects of the present invention, the following simulation experiment was conducted. This example demonstrates the positioning performance of three single-beacon positioning methods using distance as an observation value. Because the underwater acoustic velocity is time-varying and unknown in real applications, the nominal velocity values ​​of the three comparison methods deviate from the true values. By multiplying the nominal velocity value by the observed underwater acoustic signal propagation time, the observed distance between the AUV and the beacon can be calculated, and the distance is used to estimate the AUV's position coordinates. Method 1 solves the virtual long baseline localization problem based on weighted least squares (R-WLS), method 2 solves the virtual long baseline localization problem based on the semidefinite programming method in the literature Yanshen Du, Ping Wei, and Huaguo Zhang.Semidefinite programming approaches for source localization problems.In 2014IEEE International Conference on Communiction Problem-solving, pages 339–342, 2014. (R-SDR1), and method 3 solves the virtual long baseline localization problem based on the semidefinite programming method in the literature KWCheung, WKMa, and H.C.So.Accurate approximation algorithm for toa-based maximum likelihood mobile location using semidefinite programming.In 2004IEEE International Conference on Acoustics, Speech, and Signal Processing, volume 2, pages ii–145, 2004. (R-SDR2). The method of the present invention directly uses the underwater sound propagation time as the observation quantity, and can estimate the unknown sound speed, and constructs a semidefinite programming problem based on semidefinite relaxation to solve it.

[0242] 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 dive trajectory of the AUV. The control inputs of the AUV are 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.2m / s, and the initial velocity in other directions is 0. The AUV motion trajectory obtained by simulation is as follows Figure 3 shown.

[0243] 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 and pitch angle observations 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.

[0244] For all methods, the width of the sliding window is set to 15, that is, the current moment and the 15 underwater acoustic ranging observations before the current moment are used to solve the position coordinates of the AUV at the current moment. The semidefinite programming problem proposed in the present invention and the three methods R-SDR1 and R-SDR2 are all solved using the CVX toolbox of Matlab software, using the sedumi solver, and the solution accuracy is set to the highest. See the literature Michael Grant and Stephen Boyd. CVX: Matlab software for disciplined convex programming, version 2.0beta. https: / / cvxr.com / cvx, September 2013. and the literature 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:

[0245]

[0246] 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.

[0247] according to Figure 4 As shown in Table 2, the method proposed in the present invention can achieve 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. This shows that the sound speed setting error has a certain impact on the single beacon positioning accuracy, and the method of the present invention can reduce the influence of this error source to a certain extent. Figure 5 is the sound speed estimation error of the method proposed in the present invention. It can be seen that the method proposed in the present invention can obtain a lower sound speed estimation error, 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 1.746950m / 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 arrangement 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.

[0248] Table 2 Comparison of average positioning errors of the same method

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

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

Claims

1. A method for underwater single beacon semi-definite programming positioning capable of estimating unknown sound speed, characterized in that: The method includes: Based on the relationship between the propagation time of underwater acoustic signals and the speed of sound as well as the position of the AUV, the AUV dead reckoning information is integrated and all the observations of the propagation time of underwater acoustic signals within the time window width N are used to construct a virtual long baseline positioning model. By introducing auxiliary variables, the constructed virtual long baseline positioning model is algebraically transformed to obtain the virtual long baseline positioning pseudo linear equation; A constrained optimization problem is constructed and solved by semidefinite programming problem through semidefinite relaxation to obtain a preliminary estimate of the single beacon positioning position and sound speed. The estimation error of the single beacon positioning position and sound speed preliminary estimation solution is explicitly expressed, and based on weighted least squares, the single beacon positioning preliminary position and sound speed estimation solution is corrected using the virtual long baseline positioning equation to obtain the final position estimate.

2. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 1 is characterized in that: The relationship between the propagation time of the underwater acoustic signal, the speed of sound, and the position of the AUV is constructed, including: Taking any point in the positioning area as the origin, the east, north and sky directions are set as X, Y and Z axes respectively, and the geodetic coordinate system for underwater single beacon positioning is established. The position coordinates of the underwater acoustic beacon in the geodetic coordinate system are fixed and known, which can be expressed as: Where, P b is the coordinate vector of the underwater beacon position, x b is the east coordinate of the acoustic beacon, y b is the north coordinate of the acoustic beacon, z b The celestial coordinates of the hydroacoustic beacon; The observed propagation time of the underwater acoustic signal obtained at time k is expressed as: Where, t k is the real underwater acoustic signal propagation time, c is the real sound speed, x k ,y k ,z k is the real position coordinate of AUV at time k; Square both sides to get: Simplifying, we get: The propagation time of underwater acoustic signal contains observation noise, and the observed value of the propagation time of underwater acoustic signal is recorded as: Where, is the observed value of underwater acoustic signal propagation time, v k is the observation noise of the underwater acoustic signal propagation time; Substituting it into the ranging equation and ignoring the second-order noise term, we get:

3. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 2 is characterized in that: Before constructing the relationship between the underwater acoustic signal propagation time, sound speed, and AUV position, it is necessary to: With the AUV centroid as the origin, the front, right, and bottom directions are the x, y, and z axes respectively, and the AUV body coordinate system is established; According to the AUV velocity in the body coordinate system detected by the DVL and the AUV attitude detected by the attitude sensor, a dead reckoning model is constructed. The relationship between the AUV position variables at time k and time ki (i≥1) in the dead reckoning model is: Where 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, 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.

4. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 3 is characterized in that: Construct a virtual long baseline positioning model as follows: The time window width is defined as n, and the propagation time of the underwater acoustic signal received from time kN to k is used to solve the position coordinates of the AUV at time k. For time ki, the relationship between the constructed underwater acoustic signal propagation time, sound speed and AUV position is as follows: Where, is the observation value of underwater acoustic signal propagation time at time ki, v k-i is the observation noise of underwater acoustic signal propagation time at time ki; Substitute the AUV’s dead reckoning model, i.e. the position relationship between time k and time ki, and we get: definition: Where x b,i is the x-axis position coordinate of the i-th virtual beacon, y b,i is the y-axis position coordinate of the i-th virtual beacon, z b,i is the z-axis position coordinate of the i-th virtual beacon; get: Further, define: x b,0 =x b and b,0 =and b With b,0 =z b Where x b,0 、y b,0 、z b,0 Defined as the virtual beacon coordinates at time k-0; Then, combined with the observation equation at time k, all N+1 observation equations are expressed as follows:

5. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 4 is characterized in that: Obtaining the pseudo-linear equation of virtual long baseline positioning includes: expanding the observation equation in the constructed virtual long baseline positioning model to obtain: All N+1 equations are integrated into a vector matrix form: Where 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 all system model parameters, is the unknown quantity to be estimated, and the definitions of each variable include: Where S k is an auxiliary variable, is the observed value of underwater acoustic signal propagation time at time kN, v k-N is the underwater acoustic signal propagation time observation noise at time kN.

6. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 5 is characterized in that: Obtain a preliminary estimate of the single beacon positioning position and sound speed, including: according to the constructed virtual long baseline positioning pseudo-linear equation, the estimation problem is transformed into a constrained optimization problem, which is expressed as: Where, is the estimated value of the unknown quantity. The superscript T represents the transpose. For any vector a, a[m:n] represents the subvector consisting of the mth to nth elements of vector a. || || 2 represents the vector two norm; W 1,k is the weight matrix, defined as: In the formula, E(·) is the expected value of the random variable, the superscript -1 represents the inverse of the matrix, It is approximately equal to Q k is the observation variance matrix of the underwater acoustic signal propagation time, defined as The specific form is: Where, is the observed variance of the underwater acoustic signal propagation time at time k-1, is the observed variance of the underwater acoustic signal propagation time at time kN, is the observation variance of the underwater acoustic signal propagation time at time k; The objective function of the optimization problem is transformed into: Where tr represents the trace of the matrix; Defining auxiliary variables Then the constrained optimization problem is transformed into: For any matrix A, A[m:n,p:q] represents the submatrix consisting of the elements from the mth to nth rows and the pth to qth columns of matrix A. The last constraint in the above equation is mathematically equivalent to: rank(Ψ k )=1 In the formula, rank(Ψ k ) is the matrix Ψ k The rank of the problem is removed, and the rank 1 constraint is removed. The optimization problem is transformed into a semidefinite programming problem, which can be efficiently solved numerically by the interior point method to obtain the optimal solution of the semidefinite programming. and The initial estimates of the AUV position and sound speed are extracted as: Where, is the initial estimate of the speed of sound, is the preliminary estimated value of the AUV position at time k.

7. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 6, characterized in that: Noise figure matrix B k The calculation depends on the sound velocity value. In the application process, the weight W is first 1,k Set it as the unit matrix and calculate the preliminary least squares solution Substitute the sound velocity value corresponding to this solution into the noise coefficient matrix B k , calculate the weight W 1,k .

8. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 2 is characterized in that: The initial position and sound velocity estimates of the single beacon positioning are corrected using the virtual long baseline positioning equation, including: defining the estimation error corresponding to the initial estimated values ​​of the AUV position and sound velocity; and using the estimated error variables to correct the initial estimate of the semidefinite programming to obtain the final estimate.

9. The underwater single beacon semi-definite programming positioning method capable of estimating unknown sound speed according to claim 8, characterized in that: The estimated error corresponding to the initial estimated value of the AUV position and sound speed is defined as: Where Δx k for The estimation error, Δy k for The estimation error, Δz k for The estimation error, Δc is The estimation error of Substituting the above equation into get: Ignoring the second-order noise term, we can simplify to: definition: get: All N+1 equations are integrated into a vector matrix form: B k v k =g k -F k See you later. k Where g k With F k is the system model parameter, Δη k is the unknown quantity to be estimated, and each variable is defined as follows; Based on the weighted least squares estimation principle, the unknown quantity Δη is obtained k The weighted least squares estimation solution of for: Using the estimated error variable The semidefinite programming preliminary estimate is corrected to obtain the final estimate as follows: Where, is the final estimate of the x-position, is the final estimated value of the y-direction position, is the final estimated value of the z-direction position, is the final estimate of the speed of sound.

10. An underwater single-beacon semi-definite planning positioning system capable of estimating unknown sound speed, characterized in that: The system is implemented by the underwater single beacon semi-definite planning positioning method capable of estimating unknown sound speed as described in any one of claims 1 to 9, and the system includes an AUV; The AUV is equipped with a hydrophone, DVL, and attitude sensor; The underwater acoustic beacon broadcasts underwater acoustic signals periodically. The AUV uses the DVL on board to periodically measure its velocity in the body coordinate system. Combined with the AUV attitude angle observed by the attitude sensor, the AUV velocity in the geodetic coordinate system is calculated to perform dead reckoning and record and store the dead reckoning data. After receiving the acoustic signal transmitted by the acoustic beacon, the hydrophone carried by the AUV obtains the propagation time of the acoustic signal based on the known acoustic signal transmission time. When the AUV receives a certain number of underwater acoustic signals, it constructs a virtual long baseline array based on the stored dead reckoning data; Combining the relationship between underwater acoustic signal propagation time and AUV position and sound speed, a virtual long baseline positioning problem is constructed; Auxiliary variables are introduced to obtain a pseudo-linear positioning equation based on algebraic transformation. A constrained least squares problem is constructed based on the pseudo-linear positioning equation. Auxiliary variables are further introduced to construct linear constraints. Based on the semidefinite relaxation principle, the optimization problem is transformed into a semidefinite programming problem. A numerical solution is performed using the interior point method to obtain preliminary estimates of the AUV's position and sound speed. The error of the preliminary estimate is explicitly expressed and brought back into the virtual long baseline positioning equation to obtain the weighted least squares estimate of the preliminary estimate error. The estimated error is used to correct the preliminary estimate of the semidefinite programming to obtain more accurate AUV position and sound speed estimates.

Images