Underwater single beacon semi-definite programming 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 semi-definite programming positioning method and system capable of estimating an unknown sound velocity. Constructing a constrained least square problem based on a pseudo-linear positioning equation, further introducing an auxiliary variable, constructing a linear constraint condition, converting an optimization problem into a semi-definite programming problem based on a semi-definite relaxation principle, and performing numerical solution based on an interior point method to obtain an AUV position and sound velocity preliminary estimation value; explicitly expressing the initial estimation value error, and bringing the initial estimation value error back to the virtual long baseline positioning equation to obtain a weighted least square estimation value of the initial estimation error; and correcting the initial estimation value of the semi-definite programming by using the estimated error to obtain a more accurate AUV position and sound velocity estimation value. Compared with a single-beacon positioning method which does not consider sound velocity unknown, the method provided by the invention can obtain better positioning precision, 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 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 underwater positioning of AUVs. Currently, the more common methods of hydroacoustic positioning 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 a detailed calibration of the position and attitude of the acoustic array. In contrast, the underwater single beacon positioning method that combines a single hydroacoustic beacon with dead reckoning information has received 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] The current underwater single beacon positioning method usually regards the water acoustic speed as a constant known value, 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, the water acoustic speed is affected by physical factors such as the temperature, salinity and depth of the seawater. Its value is usually related to time and space, and is 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, the research on the single beacon positioning method that can estimate the unknown water acoustic speed while estimating the position of the AUV 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 estimate the AUV position and sound speed at the same time, the obtained AUV positioning position accuracy is poor, and the navigation error is large. Summary of the invention

[0005] In order to overcome the problems existing in the related art, the embodiments disclosed in the present invention provide a method and system for underwater single beacon semidefinite planning positioning that can estimate unknown sound speed. The purpose of the present invention is to solve the problem of unknown time-varying underwater sound speed in underwater single beacon positioning, and to construct an underwater single beacon semidefinite planning positioning solution that can estimate unknown sound speed based on semidefinite relaxation, so as to realize the 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 comprises:

[0007] Based on the relationship between the propagation time of the hydroacoustic signal and the speed of sound as well as the position of the AUV, the AUV dead reckoning information is integrated and all the propagation time observations of the hydroacoustic signal 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 a semidefinite programming problem is constructed through semidefinite relaxation to solve it, and a preliminary estimate of the single beacon positioning position and sound speed is obtained;

[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 a virtual long baseline positioning equation to obtain a final position estimate.

[0011] Furthermore, the relationship between the propagation time of the hydroacoustic signal and 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 underwater single beacon positioning geodetic coordinate system is established. The position coordinates of the hydroacoustic 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 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;

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

[0016]

[0017] Where, t k is the real propagation time of underwater acoustic signal, c is the real speed of sound, 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 the underwater acoustic signal contains observation noise, and the observation value of the propagation time of the underwater acoustic signal is recorded as:

[0023]

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

[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 propagation time of the underwater acoustic signal, the speed of sound, and the position of the AUV, 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 speed of the AUV 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] 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.

[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 hydroacoustic signal received from kN to k to solve the position coordinates of the AUV at time k. For time ki, the relationship between the constructed hydroacoustic signal propagation time, sound speed and AUV position is as follows:

[0034]

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

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

[0037]

[0038] definition:

[0039]

[0040] In the formula, x b,i is the x-axis position coordinate of the ith 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] In the formula, 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] Further, 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] 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 various variables include:

[0055]

[0056]

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

[0058] Furthermore, a preliminary estimated solution of 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] In the formula, is the estimated value of the unknown quantity, the superscript T represents the transpose, and 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(·) 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:

[0063]

[0064] In the formula, 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 observed 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 of 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 1, and the rank 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] In the formula, 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 virtual long baseline positioning equation is used to correct the preliminary position and sound speed estimation solutions of the single beacon positioning, including: defining the estimation error corresponding to the obtained preliminary estimated values ​​of the AUV position and sound speed; and using the estimated error variable to correct the preliminary estimated value of the semidefinite programming to obtain the final estimated value.

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

[0079]

[0080] In the formula, Δ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, the simplification yields:

[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] In the formula, 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 initial estimate of the semidefinite programming is corrected to obtain the final estimate expression:

[0096]

[0097] In the formula, is the final estimate of the x-position, is the final estimate of the y-direction position, is the final estimate 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 is implemented by the underwater single beacon semi-definite planning positioning method capable of estimating an unknown sound speed, the system comprises an AUV;

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

[0100] The hydroacoustic beacon broadcasts hydroacoustic signals periodically. The AUV uses the DVL carried by the AUV 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 earth coordinate system is calculated to perform dead reckoning, and the dead reckoning data is recorded and stored.

[0101] After receiving the acoustic signal emitted by the acoustic beacon, the hydrophone carried by the AUV obtains the acoustic signal propagation time based on the known acoustic signal emission time;

[0102] 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; it constructs a virtual long baseline positioning problem in combination with the relationship between the propagation time of the hydroacoustic signal and the position and speed of sound of the AUV;

[0103] Auxiliary variables are introduced to obtain the 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 transformed into a semidefinite 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;

[0104] The error of the preliminary estimate is explicitly expressed and brought back to 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] In combination with 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 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 earth 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; 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; an auxiliary variable is 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 an auxiliary variable is further introduced to construct a linear constraint condition. Based on the semidefinite relaxation principle, the optimization problem is converted into a semidefinite programming problem, and a 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 preliminary estimate of the semidefinite programming is corrected using the estimated error to obtain a more accurate estimate of the AUV position and sound speed. The method of the present invention can estimate the AUV position and sound speed at the same time. 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 the application of the method of the present invention, there is no need for the AUV to use a sound velocity profiler or a 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 method of the present invention realizes the simultaneous estimation of the AUV position and sound velocity under the framework of virtual long baseline positioning. At present, no related technology has appeared, which fills the technical gap of the joint estimation of position and sound velocity under the framework of virtual long baseline positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] 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;

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

[0109] Figure 2It 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;

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

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

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

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

[0114] The innovation of the present invention is that the present invention realizes the joint estimation of the position and sound speed of the AUV positioned by a single beacon under the framework of virtual long baseline positioning. By expanding the sound speed to the unknown quantity to be estimated, an auxiliary variable is introduced to transform the virtual long baseline positioning equation into a pseudo-linear equation form; a semidefinite planning problem is constructed by introducing a semidefinite relaxation technique; the preliminary estimation values ​​of the AUV position and sound speed are solved by the interior point method; finally, by expressing the preliminary estimation error display and bringing it back to the virtual long baseline positioning equation, a more accurate estimation value of the AUV position and sound speed is obtained. The method of the present invention can estimate the position and sound speed of the AUV at the same time, enhancing the practical application capability of the AUV.

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

[0116] The AUV is equipped with a hydrophone, a DVL (Doppler Velocity Log) and an attitude sensor. The hydroacoustic beacon broadcasts hydroacoustic signals periodically. The AUV periodically observes its velocity in the body coordinate system through the DVL carried by it, and calculates the velocity of the AUV in the geodetic coordinate system by combining the AUV attitude angle observed by the attitude sensor, performs dead reckoning, and records and stores the dead reckoning data.

[0117] After receiving the acoustic signal emitted by the acoustic beacon, the hydrophone carried by the AUV obtains the acoustic signal propagation time based on the known acoustic signal emission time;

[0118] 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; it constructs a virtual long baseline positioning problem in combination with the relationship between the propagation time of the hydroacoustic signal and the position and speed of sound of the AUV;

[0119] Auxiliary variables are introduced to obtain the 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 transformed into a semidefinite 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;

[0120] The error of the preliminary estimate is explicitly expressed and brought back to 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] Embodiment 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, 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;

[0123] 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;

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

[0125] 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;

[0126] S5, by introducing auxiliary variables and performing algebraic transformation, the pseudo-linear equation for 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] S7, explicitly represents the estimation error of the preliminary estimation solution of the single beacon positioning position and sound speed, and based on the weighted least squares, uses the virtual long baseline positioning equation to correct the preliminary estimation solution of the single beacon positioning position and sound speed 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 constructed is as follows:

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

[0132]

[0133] Among them, x k represents the x-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 velocity 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] In the formula, 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 framework of virtual long baseline positioning, 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 need to set a nominal sound speed value, nor does it need to measure 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] In the formula, R k is the rotation matrix, defined as:

[0142]

[0143] According to the position and speed 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] 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.

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

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

[0155]

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

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

[0158]

[0159] In the formula, t k is the real propagation time of underwater acoustic signal, c is the real speed of sound, 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 the underwater acoustic signal contains observation noise, and the observation value of the propagation time of the underwater acoustic signal is recorded as:

[0165]

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

[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, so as to obtain more accurate AUV position coordinates.

[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 hydroacoustic 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 hydroacoustic signal constructed by the present invention, the speed of sound, and the position of the AUV is as follows:

[0172]

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

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

[0175]

[0176] definition:

[0177]

[0178] In the formula, x b,i is the x-axis position coordinate of the ith 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] In the formula, 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] The existing virtual long baseline positioning methods all construct equations based on distance as observation, while the method of the present invention is constructed based on signal propagation time as observation quantity, and the sound speed needs to be expressed in the equation to facilitate the simultaneous estimation of the sound speed in the later stage. In this step, the sound speed is regarded as an unknown quantity to be estimated, and the underwater acoustic signal propagation time is used as the observation quantity. 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, and the positioning accuracy and application ability of the method of the present invention are enhanced. The existing virtual long baseline positioning method directly uses distance as observation, and the nominal value of the sound speed needs to be set. Inaccurate setting of the nominal value of the sound speed will cause ranging errors and positioning errors. The method of the present invention overcomes this defect, directly uses the signal propagation time as the observation quantity, and synchronously estimates 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] 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 various variables include:

[0195]

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

[0197] Furthermore, a preliminary estimated solution of 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] In the formula, is the estimated value of the unknown quantity, the superscript T represents the transpose, and 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(·) 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:

[0202]

[0203] In the formula, 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 observed variance of the propagation time of the underwater acoustic signal 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 of 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 1, and the rank 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] In the formula, 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 the sound speed obtained in step S6 is defined as:

[0218]

[0219] In the formula, Δ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, the simplification yields:

[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] In the formula, 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 initial estimate of the semidefinite programming is corrected to obtain the final estimate expression:

[0235]

[0236] In the formula, is the final estimate of the x-position, is the final estimate of the y-direction position, is the final estimate 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 introducing auxiliary variables to construct the pseudo-linear equation in steps S5-S6 is eliminated, thus obtaining a more accurate estimation result.

[0238] Exemplarily, 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 the underwater single beacon semi-definite programming positioning method for estimating unknown sound speed

[0240]

[0241] To further illustrate the relevant effects of the embodiments of the present invention, the following simulation experiments are conducted: This embodiment simultaneously demonstrates the positioning performance of three single beacon positioning methods using distance as the observation value. Since the underwater sound velocity is time-varying and unknown in actual applications, there is a certain deviation between the nominal sound velocity values ​​and the true values ​​of the three comparison methods. By multiplying the nominal sound velocity 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, 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 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.

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

[0244] 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 semidefinite programming problem proposed in the present invention and the three methods R-SDR1 and R-SDR2 are all solved by 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.0 beta. 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 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. This shows that the sound speed setting error has a certain influence 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 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. A method for underwater single beacon semi-definite planning positioning capable of estimating unknown sound speed, characterized in that: The method includes: Based on the relationship between the propagation time of the hydroacoustic signal and the speed of sound as well as the position of the AUV, the AUV dead reckoning information is integrated and all the propagation time observations of the hydroacoustic signal 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 a semidefinite programming problem is constructed through semidefinite relaxation to solve it, and a preliminary estimate of the single beacon positioning position and sound speed is obtained; 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 a virtual long baseline positioning equation to obtain a final position estimate.

2. The underwater single beacon semi-definite planning positioning method capable of estimating unknown sound speed according to claim 1 is characterized in that: The relationship between the propagation time of the hydroacoustic signal and 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 underwater single beacon positioning geodetic coordinate system is established. The position coordinates of the hydroacoustic beacon in the geodetic coordinate system are fixed and known, which can be expressed 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: Where, t k is the real propagation time of underwater acoustic signal, c is the real speed of sound, 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 the underwater acoustic signal contains observation noise, and the observation value of the propagation time of the underwater acoustic signal is recorded as: In the formula, is the observed value of the propagation time of the underwater acoustic signal, v k is the observation noise of the propagation time of the underwater acoustic signal; 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 propagation time of the hydroacoustic signal, the speed of sound, and the position of the AUV, 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 speed of the AUV 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: 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.

4. The underwater single beacon semi-definite planning 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: Define the time window width as n, and use the propagation time of the hydroacoustic signal received from kN to k to solve the position coordinates of the AUV at time k. For time ki, the relationship between the constructed hydroacoustic signal propagation time, sound speed and AUV position is as follows: In the formula, is the observed value of the underwater acoustic signal propagation time at time ki, v k-i is the observation noise of the underwater acoustic signal propagation time at time ki; Substitute the AUV’s dead reckoning model, that is, the position relationship between time k and time ki, and we get: definition: In the formula, x b,i is the x-axis position coordinate of the ith 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 =from b In the formula, 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 planning 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: 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 various variables include: In the formula, S k is an auxiliary variable, is the observed value of the propagation time of the underwater acoustic signal at time kN, v k-N is the propagation time observation noise of the underwater acoustic signal 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, expressed as: In the formula, 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(·) 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: In the formula, 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 observed variance of the propagation time of the underwater acoustic signal 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: Among them, for any matrix A, A[m:n,p:q] represents the submatrix consisting of the elements of 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 1, and the rank 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: In the formula, 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 is characterized in that: Noise coefficient 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 planning positioning method capable of estimating unknown sound speed according to claim 2 is characterized in that: The virtual long baseline positioning equation is used to correct the preliminary position and sound speed estimation solution of the single beacon positioning, including: defining the estimation error corresponding to the preliminary estimated value of the AUV position and sound speed obtained; using the estimated error variable to correct the preliminary estimated value of the semi-definite programming to obtain the final estimated value.

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 the sound speed is defined as: In the formula, Δ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, the simplification yields: 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 In the formula, 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 initial estimate of the semidefinite programming is corrected to obtain the final estimate expression: In the formula, is the final estimate of the x-position, is the final estimate of the y-direction position, is the final estimate 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 claimed 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 hydroacoustic beacon broadcasts hydroacoustic signals periodically. The AUV uses the DVL carried by the AUV 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 earth coordinate system is calculated to perform dead reckoning, and the dead reckoning data is recorded and stored. After receiving the acoustic signal emitted by the acoustic beacon, the hydrophone carried by the AUV obtains the acoustic signal propagation time based on the known acoustic 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 underwater acoustic signals and the position and speed of sound of the AUV, a virtual long baseline positioning problem is constructed. Auxiliary variables are introduced to obtain the 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 transformed into a semidefinite 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 error of the preliminary estimate is explicitly expressed and brought back to 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