TOA and FOA underwater moving target positioning method based on equal gradient sound velocity profile
Through the TOA and FOA underwater moving target positioning method based on the equal gradient sound velocity profile, combined with semi-positive planning and linear weighted least squares optimization, the problem of low positioning accuracy under the influence of sound velocity changes and noise in underwater environment is solved, and high Moving target positioning with accuracy and stability.
Patent Information
- Application Number
- CN202411017598.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-29
- Publication Date
- 2024-12-06
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
The existing underwater moving target positioning methods have problems of low accuracy and high complexity when dealing with changes in sound speed and measuring noise, making it difficult to achieve accurate target positioning in an underwater environment.
Using the TOA and FOA underwater moving target positioning method based on isogradual sound velocity profile, a simplified FOA measurement model is constructed by mixing TOA and FOA measurement information, and combining semi-positive fixed planning and linear weighted least squares optimization strategy to improve positioning accuracy and robustness.
It realizes high-precision moving target positioning in an underwater environment, significantly improves positioning accuracy, and maintains stable performance under high noise conditions, which complies with the positioning accuracy standards of the Krame-Royal Nether.
Smart Images

Figure CN119087342A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a target positioning technology, and in particular to a TOA (Time of Arrival) and FOA (Frequency of Arrival) underwater mobile target positioning method based on an isogradient sound velocity profile, which is suitable for positioning mobile targets in scenarios where the underwater propagation sound velocity is unknown and changes with the water area. Background Art
[0002] In recent years, underwater acoustic sensor networks (UASN) have played an indispensable role in many key fields such as underwater resource exploration, disaster prevention and mitigation, and environmental monitoring. As one of the core and basic technologies, underwater positioning technology based on UASN plays a vital role in both military and civilian fields.
[0003] When it comes to positioning underwater stationary targets, researchers usually use time of arrival (TOA) measurement or angle of arrival (AOA) measurement to develop positioning algorithms. However, with the development of technology and the expansion of application fields, many application scenarios involve the positioning of underwater moving targets, such as marine resource exploration, surveillance and security, underwater communications, ecological research, and seabed exploration. These application scenarios have inspired scholars to conduct in-depth research on more complex mobile target positioning problems.
[0004] In the study of mobile target positioning, Doppler frequency shift (DFS) has become a key element, especially when there is relative motion between the target and the sensor. DFS is often combined with TOA measurements to accurately determine the position and velocity of a moving target. However, the unique signal propagation characteristics of the underwater environment, such as the change in sound wave speed with temperature, pressure and salinity, make many mobile target positioning methods designed for free space environments (such as radar) fail in underwater environments.
[0005] Specifically, the propagation path of underwater sound waves presents a curved characteristic due to the change in sound speed, which makes the simple TOA model unable to be directly applied to the measurement of target distance. At the same time, the curved propagation path also complicates the DFS model, further increasing the difficulty of positioning underwater moving targets.
[0006] In order to overcome the challenges brought by the changing speed of sound in underwater environments, researchers have proposed a variety of methods. One method is to assume that the acoustic signal propagates in a straight line at an unknown speed and jointly estimate the target position through time or frequency measurements. However, this assumption of straight-line propagation may not be accurate enough when locating targets at long distances.
[0007] To improve the above methods, another strategy takes into account the change of sound speed and introduces a positioning method based on sound speed profile (SSP). This method forms a constant TOA surface at a specific target depth through SSP, which helps to accurately locate the target. Although this method has high positioning accuracy in theory, its high computational complexity limits its widespread adoption in practical applications.
[0008] In order to simplify the calculation and improve practicality, the researchers further assumed that SSP has the characteristics of equal gradient, and used the geometric relationship between the sensor and the target to propose a TOA model based on equal gradient SSP.
[0009] From the existing research reviewed above, although considerable progress has been made in the positioning of underwater moving targets, the available positioning methods are not sufficient to describe the changes in the speed of sound in the measurement model. The processing is very complicated when solving the target position from the measurement equation or only locating stationary targets. The processing of positioning moving targets is even more complicated, so a more suitable measurement model is needed for positioning. Summary of the invention
[0010] The technical problem to be solved by the present invention is to provide a TOA and FOA underwater mobile target positioning method based on isogradient sound velocity profile, which uses mixed TOA and FOA measurement information to realize underwater mobile target positioning. The relationship between the proposed FOA measurement model and the mobile target position is more intuitive, making the FOA measurement model easier to handle to form a positioning problem. It has high positioning accuracy and shows good robustness to measurement noise.
[0011] The technical solution adopted by the present invention to solve the above technical problems is: a TOA and FOA underwater mobile target positioning method based on isogradient sound velocity profile, characterized by comprising the following steps:
[0012] Step 1: In an underwater positioning system, establish a three-dimensional coordinate system as a reference coordinate system, set up N fixed beacons with known positions and a moving target with unknown position, and record the true coordinate position of the i-th fixed beacon as s i ,s i =[x i ,y i ,z i ] T , the real coordinate position and real moving speed of the moving target are recorded as u o and v o , Where N>1, beacon refers to the sensor, 1≤i≤N, x i Indicates i The horizontal axis, y i Indicates i The vertical coordinate, zi Indicates i The vertical coordinate of Indicates u o The horizontal axis of Indicates u o The vertical coordinate of Indicates u o The vertical coordinate of Indicates v o The velocity component along the x-axis, Indicates v o The velocity component along the y-axis, Indicates v o The velocity component along the z-axis. The superscript "T" indicates the transpose of a vector or matrix.
[0013] Step 2: In an underwater positioning system, a fixed beacon transmits a signal to a mobile target, and the mobile target receives the signal and extracts the TOA measurement value and the FOA measurement value from the received signal. The TOA measurement value and the FOA measurement value extracted after the signal transmitted by the i-th fixed beacon is received by the mobile target are recorded as t i and f i ; Wherein, i=1,...,N;
[0014] Step 3: Construct the TOA measurement model, which is described as follows: And construct the FOA measurement model, which is described as: in, represents the TOA measurement value of the signal emitted by the i-th fixed beacon arriving at the mobile target without being contaminated by measurement noise, represents the TOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, a represents the underwater sound speed gradient, q i is the intermediate quantity introduced, q i =g i ||u o -s i ||, g i is the intermediate quantity introduced, b represents the speed of sound on the ocean surface, and the symbol “||||” is the symbol of the second norm. Indicates u o The vector consisting of the first and second elements in s i,(1:2) Indicates i The vector consisting of the first and second elements in f i o represents the FOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target without being contaminated by measurement noise, represents the FOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, f c represents the carrier frequency, c represents the speed of sound at the moving target,
[0015] Step 4: Preprocess the TOA measurement model constructed in step 3 as follows: First, the propagation speed c of the signal emitted by the i-th fixed beacon to the mobile target is i Approximately The approximate TOA measurement model is described as: Then, the left and right sides of the approximated TOA measurement model are squared simultaneously, and the second-order noise term is ignored to obtain the transformation model, which is described as:
[0016]
[0017] Step 5: The transformation model is formulated as a constrained weighted least squares problem, described as:
[0018]
[0019] Where x is the optimization variable of the constrained weighted least squares problem, u represents the position variable of the moving target, u=[x u ,y u ,z u ] T , x u represents the horizontal coordinate of u, y u represents the ordinate of u, z u represents the vertical coordinate of u, u (1:2) represents the vector consisting of the first and second elements in u, ((A 1 x+h 1 ) T W 1 (A 1 x+h 1 )) is the objective function of the constrained weighted least squares problem, A 1 is the introduced coefficient matrix, A 1 The vector composed of the i-th row elements of is A 1 (i,:), h 1 is the introduced coefficient vector, h 1 The i-th element in is W 1 is the introduced weight matrix, E[] means to find the mathematical expectation, B 1 is the introduced coefficient matrix, B 1 The element in the i-th row and i-th column is B 1 (i,i), B 1 The rest of the elements in are 0, n t represents the TOA measurement noise vector, n t Subject to the mean of 0 and the covariance matrix is Q t Gaussian distribution of , x(1) represents the first element in x, x(2) represents the second element in x, x(3) represents the third element in x, x(4) represents the fourth element in x, and x(5) represents the fifth element in x;
[0020] Step 6: Introduce a new auxiliary matrix variable X into the constrained weighted least squares problem constructed in step 5, X = xx T ; Then set X = xx T The equivalent is rank(X)=1,X≥0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(X)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as:
[0021]
[0022] Among them, rank() means to find the rank of the matrix, X≥0 means that X is semi-positive definite, tr() is the trace operation of the matrix elements, is the objective function of the convex semi-positive definite programming problem, X and x are the optimization variables of the convex semi-positive definite programming problem, X(1,1) represents the first row and first column element in X, X(2,2) represents the second row and second column element in X, and X(3,3) represents the third row and third column element in X;
[0023] Step 7: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 6 and obtain the estimated value of x, which is recorded as according to Get a rough estimate of u, denoted as in, express The vector consisting of the first to third elements in ;
[0024] Step 8: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as:
[0025]
[0026] Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as:
[0027]
[0028] in,
[0029] q i Medium Replace u o get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between Replace u o get
[0030] Step 9: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 8 are constructed as a constrained weighted least squares problem, which is described as:
[0031]
[0032] Where y is the optimization variable in the constrained weighted least squares problem,
[0033] Δu 1 Represents the position deviation variable of the moving target, Δx u,1 Represents the horizontal coordinate deviation variable of the moving target position, Δy u,1 Represents the position ordinate deviation variable of the moving target, Δz u,1 represents the vertical coordinate deviation variable of the moving target’s position, v represents the moving speed variable of the moving target, v = [v x ,v y ,v z ] T , v xrepresents the velocity component of v along the x-axis, v y represents the velocity component of v along the y-axis, v z represents the velocity component of v along the z-axis, ((A 2 y+h 2 ) T W 2 (A 2 y+h 2 )) is the objective function of the constrained weighted least squares problem, A 2 is the introduced coefficient matrix, The element in the ith row and first column of The element in the ith row and second column of The element in the ith row and third column of The vector composed of the elements in the ith row and the 4th to 9th columns is 0 N×6 represents an all-zero matrix of dimension N×6, The vector composed of the elements in the i-th row and the first to fifth columns is The vector composed of the elements in the ith row and the 6th to 9th columns is h 2 is the introduced coefficient vector, The i-th element of The i-th element of W 2 is the introduced weight matrix, B 2 is the introduced coefficient matrix, The element in the i-th row and i-th column is The rest of the elements in are 0. The element in the i-th row and i-th column is The remaining elements in are all 0, n represents the TOA and FOA composite measurement noise vector, n = [(n t ) T ,(n f ) T ] T , n f represents the FOA measurement noise vector, n f Subject to the mean of 0 and the covariance matrix is Q fGaussian distribution of , y(1) represents the first element of y, y(2) represents the second element of y, y(3) represents the third element of y, y(4) represents the fourth element of y, y(5) represents the fifth element of y, y(6) represents the sixth element of y, y(7) represents the seventh element of y, y(8) represents the eighth element of y, y(9) represents the ninth element of y, y(1:3) represents the vector consisting of the first to third elements of y, and y(4:6) represents the vector consisting of the fourth to sixth elements of y;
[0034] Step 10: Introduce a new auxiliary matrix variable Y into the constrained weighted least squares problem constructed in step 9, Y = yy T ; Then Y = yy T The equivalent is rank(Y)=1,Y≥0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(Y)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as:
[0035]
[0036] Among them, Y ≥ 0 means that Y is positive semi-definite, is the objective function of the convex semi-positive definite programming problem, Y and y are both optimization variables of the convex semi-positive definite programming problem, Y(3,4) represents the 3rd row and 4th column element in Y, Y(1,6) represents the 1st row and 6th column element in Y, Y(3,5) represents the 3rd row and 5th column element in Y, Y(2,6) represents the 2nd row and 6th column element in Y, Y(1,4) represents the 1st row and 4th column element in Y, Y(2,5) represents the 2nd row and 5th column element in Y, and Y(3,6) represents the 3rd row and 6th column element in Y;
[0037] Step 11: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 10 and obtain the estimated value of y, which is recorded as according to Get Δu 1 The estimated values of and v are recorded as and According to and The first step of refining the position estimate of the moving target is obtained, denoted as in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in ;
[0038] Step 12: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as:
[0039]
[0040] Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as:
[0041] ;
[0042] in, use replace get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between express The velocity component along the x-axis, express The velocity component along the y-axis, express The velocity component along the z-axis, Δv o Indicates v o and The deviation between express and The deviation between express and The deviation between express and The deviation between
[0043] Step 13: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 12 are constructed as a linear weighted least squares problem, which is described as:
[0044]
[0045] Among them, Δp is the optimization variable of the linear weighted least squares problem, Δu 2 Denotes the first step refinement of the moving target position estimate deviation variable, Δu 2 =[Δx u,2 ,Δy u,2 ,Δz u,2 ] T , Δx u,2 represents the first step of refining the position estimate of the moving target, the horizontal coordinate deviation variable, Δy u,2 represents the first step of refining the position estimate of the moving target, Δz u,2 represents the vertical coordinate deviation variable of the first step refined position estimate of the moving target, Δv represents the moving speed deviation variable of the moving target, Δv = [Δv x ,Δv y ,Δv z ] T , Δv x represents the velocity component of Δv along the x-axis, Δv y represents the velocity component of Δv along the y-axis, Δv z represents the velocity component of Δv along the z-axis, ((A 3 Δp+h 3 ) T W 3 (A 3 Δp+h 3 )) is the objective function of the linear weighted least squares problem, A 3 is the introduced coefficient matrix, Medium replace get The vector composed of the elements in the i-th row, the first column to the second column is
[0046]
[0047] The vector composed of the elements in the ith row and the third to fourth columns of
[0048]
[0049] The vector composed of the elements in the 5th to 6th columns of the i-th row is
[0050] h 3 is the introduced coefficient vector,
[0051] Medium replace get The i-th element in is W 3 is the introduced weight matrix, B 3 is the introduced coefficient matrix, B 2 Medium replace Get B 3 , Q = blkdiag(Q t ,Q f ), blkdiag(Q t ,Q f ) indicates that Q t ,Q f Assemble into a block diagonal matrix;
[0052] Step 14: Solve the linear weighted least squares problem constructed in step 13 and obtain the optimal solution of Δp, which is recorded as Then we get the final estimated values of u and v, which are recorded as and in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in .
[0053] Compared with the prior art, the advantages of the present invention are:
[0054] 1) Compared with the existing underwater mobile target positioning method, the method of the present invention not only fully considers the characteristics of underwater sound speed change in design, but also establishes a simplified and efficient FOA measurement model equation according to the specific position and speed of the moving target. The model has an isogradient sound speed profile feature, which makes the positioning accuracy high and the positioning process simple.
[0055] 2) By combining the FOA measurement model with the TOA measurement model of the constant gradient sound velocity profile, the positioning accuracy of the method of the present invention is significantly improved compared with the TOA and FOA measurement models based on the constant sound velocity in the existing underwater mobile target positioning method.
[0056] 3) The method of the present invention adopts a semi-positive definite programming method, i.e., a nonlinear optimization strategy, to obtain a rough estimate of the mobile target position, and then uses a linear weighted least squares method to refine the initial solution, i.e., the rough estimate of the mobile target position. Compared with the existing linear optimization method, this nonlinear optimization strategy can still maintain excellent positioning performance and stability in the face of high measurement noise power, reflecting the good robustness of the method of the present invention to measurement noise.
[0057] 4) Experimental verification shows that the proposed method can achieve the target positioning accuracy standard specified by the Cramer-Rao lower bound (CRLB), and still exhibits stable positioning performance when the measurement noise power is large. These results fully demonstrate the robustness and reliability of the proposed method in practical applications, making it an effective tool for solving underwater target positioning problems. BRIEF DESCRIPTION OF THE DRAWINGS
[0058] Figure 1 It is a general implementation block diagram of the method of the present invention;
[0059] Figure 2a A comparison diagram of the mean square error (MSE) of the coordinate position estimation of a mobile target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9, a=0.1, and b=1500 as the TOA measurement noise power changes;
[0060] Figure 2b A comparison diagram of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9, a=0.1, and b=1500 as the TOA measurement noise power changes;
[0061] Figure 3a A comparison diagram of the mean square error (MSE) of the coordinate position estimation of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when a=0.1 and b=1500 as the number of sensors changes;
[0062] Figure 3b A comparison diagram of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when a=0.1 and b=1500 as the number of sensors changes;
[0063] Figure 4a A comparison diagram of mean square error (MSE) of the coordinate position estimation of a moving target at an unknown position with the change of the sound speed gradient by the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9 and b=1500;
[0064] Figure 4b A comparison diagram of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9 and b=1500 as the gradient of the sound speed changes;
[0065] Figure 5a A comparison diagram of the mean square error (MSE) of the coordinate position estimation of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9, a=0.1, and b=1500 as the horizontal distance between the sensor and the moving target changes;
[0066] Figure 5b : This is a comparison chart of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9, a=0.1, and b=1500 as the horizontal distance between the sensor and the moving target changes. DETAILED DESCRIPTION
[0067] The present invention is further described in detail below with reference to the accompanying drawings.
[0068] The present invention proposes a method for locating underwater mobile targets based on TOA and FOA of isogradient sound velocity profiles, and its overall implementation block diagram is as follows: Figure 1 As shown, it includes the following steps:
[0069] Step 1: In an underwater positioning system, establish a three-dimensional coordinate system as a reference coordinate system, set up N fixed beacons with known positions and a moving target with unknown position, and record the true coordinate position of the i-th fixed beacon as s i ,s i =[x i ,y i ,z i ] T , the real coordinate position and real moving speed of the moving target are recorded as u o and v o , Where N>1, beacon refers to the sensor, 1≤i≤N, x i Indicates i The horizontal axis, y i Indicates i The vertical coordinate, z i Indicates i The vertical coordinate of Indicates u o The horizontal axis of Indicates u o The vertical coordinate of Indicates u o The vertical coordinate of Indicates v o The velocity component along the x-axis, Indicates v o The velocity component along the y-axis, Indicates v o The velocity component along the z-axis. The superscript "T" indicates the transpose of a vector or matrix.
[0070] Step 2: In an underwater positioning system, a fixed beacon transmits a signal to a mobile target, and the mobile target receives the signal and extracts the TOA measurement value and the FOA measurement value from the received signal. The TOA measurement value and the FOA measurement value extracted after the signal transmitted by the i-th fixed beacon is received by the mobile target are recorded as t i and f i ; where i=1,...,N.
[0071] Step 3: Construct the TOA measurement model, which is described as follows: And construct the FOA measurement model, which is described as: Among them, t i That is, the TOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target and contaminated by the measurement noise. represents the TOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target without being contaminated by measurement noise, represents the TOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, a represents the underwater sound speed gradient, q i is the intermediate quantity introduced, q i =g i ||u o -s i ||, g i is the intermediate quantity introduced, b represents the speed of sound on the ocean surface, and the symbol “||||” is the symbol of the second norm. Indicates u o The vector consisting of the first and second elements in s i,(1:2) Indicates i The vector consisting of the first and second elements in f i That is, the FOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target contaminated by the measurement noise, f i o represents the FOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target without being contaminated by measurement noise, represents the FOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, f crepresents the carrier frequency, c represents the speed of sound at the moving target,
[0072] Step 4: Preprocess the TOA measurement model constructed in step 3 as follows: First, the propagation speed c of the signal emitted by the i-th fixed beacon to the mobile target is i Approximately The approximate TOA measurement model is described as: Then, the left and right sides of the approximated TOA measurement model are squared simultaneously, and the second-order noise term is ignored to obtain the transformation model, which is described as:
[0073]
[0074] Step 5: The transformation model is formulated as a constrained weighted least squares problem, described as:
[0075]
[0076] Among them, min() is the minimum function, st means "constrained by...", and x is the optimization variable of the constrained weighted least squares problem. u represents the position variable of the moving target, u=[x u ,y u ,z u ] T , x u represents the horizontal coordinate of u, y u represents the ordinate of u, z u represents the vertical coordinate of u, u (1:2) represents the vector consisting of the first and second elements in u, ((A 1 x+h 1 ) T W 1 (A 1 x+h 1 )) is the objective function of the constrained weighted least squares problem, A 1 is the introduced coefficient matrix, A 1 The vector formed by the i-th row elements of is A 1 (i,:), h 1 is the introduced coefficient vector, h 1 The i-th element in is W 1 is the introduced weight matrix, E[] means to find the mathematical expectation, B 1 is the introduced coefficient matrix, B 1 The element in the i-th row and i-th column is B 1 (i,i), B1 The rest of the elements in are 0, n t represents the TOA measurement noise vector, represents the TOA measurement noise of the signal transmitted by the first fixed beacon reaching the mobile target, represents the TOA measurement noise of the signal transmitted by the Nth fixed beacon reaching the mobile target, n t Subject to the mean of 0 and the covariance matrix is Q t Gaussian distribution of x(1) represents the first element in x, x(2) represents the second element in x, x(3) represents the third element in x, x(4) represents the fourth element in x, and x(5) represents the fifth element in x.
[0077] Step 6: Introduce a new auxiliary matrix variable X into the constrained weighted least squares problem constructed in step 5, X = xx T ; Then set X = xx T The equivalent is rank(X)=1,X≥0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(X)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as:
[0078]
[0079] Among them, rank() means to find the rank of the matrix, X≥0 means that X is semi-positive definite, tr() is the trace operation of the matrix elements, is the objective function of the convex semi-positive definite programming problem, X and x are the optimization variables of the convex semi-positive definite programming problem, X(1,1) represents the first row and first column element in X, X(2,2) represents the second row and second column element in X, and X(3,3) represents the third row and third column element in X.
[0080] Step 7: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 6 and obtain the estimated value of x, which is recorded as according to Get a rough estimate of u, denoted as in, express The vector consisting of the 1st to 3rd elements in .
[0081] Step 8: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as:
[0082]
[0083] Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as:
[0084]
[0085] in,
[0086] q i Medium Replace u o get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between Replace u o get
[0087] Step 9: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 8 are constructed as a constrained weighted least squares problem, which is described as:
[0088]
[0089] Where y is the optimization variable in the constrained weighted least squares problem,
[0090] Δu 1 Represents the position deviation variable of the moving target, that is, The corresponding variable, Δx u,1 Represents the horizontal coordinate deviation variable of the moving target position, that is, The corresponding variable, Δy u,1Represents the position ordinate deviation variable of the moving target, that is, The corresponding variable, Δz u,1 Represents the vertical coordinate deviation variable of the moving target position, that is, The corresponding variable, v, represents the moving speed variable of the moving target, v = [v x ,v y ,v z ] T , v x represents the velocity component of v along the x-axis, v y represents the velocity component of v along the y-axis, v z represents the velocity component of v along the z-axis, ((A 2 y+h 2 ) T W 2 (A 2 y+h 2 )) is the objective function of the constrained weighted least squares problem, A 2 is the introduced coefficient matrix, The element in the ith row and first column of The element in the ith row and second column of The element in the ith row and third column of The vector composed of the elements in the ith row and the 4th to 9th columns is 0 N×6 represents an all-zero matrix of dimension N×6, The vector composed of the elements in the i-th row and the first to fifth columns is The vector composed of the elements in the ith row and the 6th to 9th columns is h 2 is the introduced coefficient vector, The i-th element of The i-th element of W 2 is the introduced weight matrix, B 2 is the introduced coefficient matrix, The element in the i-th row and i-th column is The rest of the elements in are 0. The element in the i-th row and i-th column is The remaining elements in are all 0, n represents the TOA and FOA composite measurement noise vector, n = [(n t ) T ,(n f ) T ]T , n f represents the FOA measurement noise vector, represents the FOA measurement noise of the signal transmitted by the first fixed beacon reaching the mobile target, represents the FOA measurement noise of the signal transmitted by the Nth fixed beacon reaching the mobile target, n f Subject to the mean of 0 and the covariance matrix is Q f Gaussian distribution of , y(1) represents the first element of y, y(2) represents the second element of y, y(3) represents the third element of y, y(4) represents the fourth element of y, y(5) represents the fifth element of y, y(6) represents the sixth element of y, y(7) represents the seventh element of y, y(8) represents the eighth element of y, y(9) represents the ninth element of y, y(1:3) represents the vector consisting of the first to third elements of y, and y(4:6) represents the vector consisting of the fourth to sixth elements of y.
[0091] Step 10: Introduce a new auxiliary matrix variable Y into the constrained weighted least squares problem constructed in step 9, Y = yy T ; Then Y = yy T The equivalent is rank(Y)=1,Y≥0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(Y)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as:
[0092]
[0093] Among them, Y ≥ 0 means that Y is positive semi-definite, is the objective function of the convex semi-positive definite programming problem, Y and y are the optimization variables of the convex semi-positive definite programming problem, Y(3,4) represents the 3rd row and 4th column element in Y, Y(1,6) represents the 1st row and 6th column element in Y, Y(3,5) represents the 3rd row and 5th column element in Y, Y(2,6) represents the 2nd row and 6th column element in Y, Y(1,4) represents the 1st row and 4th column element in Y, Y(2,5) represents the 2nd row and 5th column element in Y, and Y(3,6) represents the 3rd row and 6th column element in Y.
[0094] Step 11: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 10 and obtain the estimated value of y, which is recorded as according to Get Δu 1 The estimated values of and v are recorded as and According to and The first step of refining the position estimate of the moving target is obtained, denoted as in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in .
[0095] Step 12: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as:
[0096]
[0097] Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as:
[0098] in, use replace get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between express The velocity component along the x-axis, express The velocity component along the y-axis, express The velocity component along the z-axis, Δv o Indicates v o and The deviation between express and The deviation between express and The deviation between express and The deviation between .
[0099] Step 13: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 12 are constructed as a linear weighted least squares problem, which is described as:
[0100]
[0101] Among them, Δp is the optimization variable of the linear weighted least squares problem, Δu 2 The first step of refining the position estimate deviation variable of the moving target is The corresponding variable, Δu 2 =[Δx u,2 ,Δy u,2 ,Δz u,2 ] T , Δx u,2 represents the first step of refining the position estimate of the moving target, i.e. The corresponding variable, Δy u,2 represents the first step of refining the position estimate of the moving target, i.e. The corresponding variable, Δz u,2 represents the vertical coordinate deviation variable of the first step refined position estimate of the moving target, that is, The corresponding variable, Δv, represents the moving speed deviation variable of the moving target, which is Δv o The corresponding variable, Δv=[Δv x ,Δv y ,Δv z ] T , Δv x represents the velocity component of Δv along the x-axis, Δv y represents the velocity component of Δv along the y-axis, Δv z represents the velocity component of Δv along the z-axis, ((A 3 Δp+h 3 ) T W 3 (A 3 Δp+h 3 )) is the objective function of the linear weighted least squares problem, A 3 is the introduced coefficient matrix, Medium replace get The vector composed of the elements in the i-th row, the first column to the second column is The vector composed of the elements in the ith row and the third to fourth columns of The vector composed of the elements in the 5th to 6th columns of the i-th row is h 3 is the introduced coefficient vector, Medium replace get The i-th element in is W 3 is the introduced weight matrix, B 3 is the introduced coefficient matrix, B 2 Medium replace Get B 3 , Q = blkdiag(Q t ,Q f ), blkdiag(Q t ,Q f ) indicates that Q t ,Q f It is a block diagonal matrix.
[0102] Step 14: Solve the linear weighted least squares problem constructed in step 13 and obtain the optimal solution of Δp, which is recorded as Then we get the final estimated values of u and v, which are recorded as and in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in .
[0103] In order to verify the feasibility and effectiveness of the method of the present invention, a simulation test was carried out on the method of the present invention.
[0104] Assume that there are 9 sensors (beacons) with known positions and 1 mobile target with unknown position in a three-dimensional space, i.e., a three-dimensional coordinate system. The real Cartesian coordinate position of the mobile target in the three-dimensional space The real speed is randomly generated, where rice, rice, rice, m / s, m / s, Meters / second; the real Cartesian coordinate position of the sensor in three-dimensional space with known position s i =[x i ,y i ,z i ] T , i = 1, ..., 9 are randomly generated, where x i ~(-1000,1000) meters, y i ~(-1000,1000) meters, z i ~(0,500) meters. The time measurement noise from the unknown moving target to the sensor is assumed to have a mean of 0 and a covariance matrix of The frequency measurement noise of the moving target at unknown position to the sensor is assumed to have a mean of 0 and a covariance matrix of Gaussian noise, where σ t =0.01σ f , represents the variance of TOA measurement noise, represents the variance of FOA measurement noise, I N Represents the identity matrix of dimension N×N.
[0105] Based on the above parameter settings, the simulation test tested the performance of the method of the present invention under four conditions: different TOA measurement noise power, different number of sensors, change in the gradient of the speed of sound, and change in the horizontal distance between the sensor and the moving target. The mean square error (MSE) describing the positioning performance in the four conditions is the MSE obtained by running 1000 Monte Carlo experiments in 10 random scenarios for each scenario. At the same time, the performance benchmark Cramer-Rao lower bound (CRLB) is introduced for comparison.
[0106] Figure 2a A comparison chart of the mean square error (MSE) of the coordinate position estimation of a moving target with unknown position and the Cramer-Rao lower bound (CRLB) is given when N=9, a=0.1, and b=1500, as the TOA measurement noise power changes. Figure 2b A comparison chart is given of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position when the method of the present invention and the Cramer-Rao lower bound (CRLB) are used in the case of N=9, a=0.1, and b=1500 as the TOA measurement noise power changes. Figure 2a and Figure 2b It can be seen that when the TOA measurement noise power is 10log 10 σt 2 When the TOA measurement noise power is small, the method of the present invention can achieve the positioning accuracy of the Cramer-Rao lower bound.
[0107] Figure 3a A comparison chart of the mean square error (MSE) of the coordinate position estimation of a moving target with unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) with the number of sensors is given. Figure 3b A comparison chart of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position with the Cramer-Rao lower bound (CRLB) when a=0.1 and b=1500 is given as the number of sensors changes. Figure 3a and Figure 3b It can be seen that when the number of sensors changes, the method of the present invention can achieve the positioning accuracy of the Cramer-Rao lower bound when the number of sensors reaches 5. In addition, as the number of sensors increases, the MSE of the position estimation and moving speed estimation of the mobile target at an unknown position by the method of the present invention is significantly reduced, which means that when the number of sensors increases, the positioning accuracy of the mobile target at an unknown position by the method of the present invention is significantly improved.
[0108] Figure 4a A comparison chart of the mean square error (MSE) of the coordinate position estimation of a moving target at an unknown position with the change of the sound speed gradient is given between the method of the present invention and the Cramer-Rao lower bound (CRLB) when N=9 and b=1500. Figure 4b A comparison chart is given of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position when N=9 and b=1500, compared with the Cramer-Rao lower bound (CRLB). Figure 4a and Figure 4b It can be seen from the figure that when the gradient changes with the speed of sound, the method of the present invention can achieve the positioning accuracy of the Cramer-Rao lower bound when the gradient of the speed of sound is less than 0.55.
[0109] Figure 5a A comparison chart of the mean square error (MSE) of the coordinate position estimation of a moving target with an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) is given when N=9, a=0.1, and b=1500 as the horizontal distance between the sensor and the moving target changes. Figure 5b A comparison chart of the mean square error (MSE) of the estimated moving speed of a moving target at an unknown position between the method of the present invention and the Cramer-Rao lower bound (CRLB) is given when N=9, a=0.1, and b=1500 as the horizontal distance between the sensor and the moving target changes. Figure 5a and Figure 5b It can be seen from the figure that as the horizontal distance between the moving target and the sensor changes more and more, the method of the present invention can still achieve the positioning accuracy of the Cramer-Rao lower bound.
[0110] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims
1. A TOA and FOA underwater mobile target positioning method based on isogradient sound velocity profile, characterized in that The following steps are involved: Step 1: In an underwater positioning system, establish a three-dimensional coordinate system as a reference coordinate system, set up N fixed beacons with known positions and a moving target with unknown position, and record the true coordinate position of the i-th fixed beacon as s i ,s i =[x i ,y i ,z i ] T , the real coordinate position and real moving speed of the moving target are recorded as u o and v o , Where N>1, beacon refers to the sensor, 1≤i≤N, x i Indicates i The horizontal axis, y i Indicates i The vertical coordinate, z i Indicates i The vertical coordinate of Indicates u o The horizontal axis of Indicates u o The vertical coordinate of Indicates u o The vertical coordinate of Indicates v o The velocity component along the x-axis, Indicates v o The velocity component along the y-axis, Indicates v o The velocity component along the z-axis. The superscript "T" indicates the transpose of a vector or matrix. Step 2: In an underwater positioning system, a fixed beacon transmits a signal to a mobile target, and the mobile target receives the signal and extracts the TOA measurement value and the FOA measurement value from the received signal. The TOA measurement value and the FOA measurement value extracted after the signal transmitted by the i-th fixed beacon is received by the mobile target are recorded as t i and f i ; Wherein, i=1,...,N; Step 3: Construct the TOA measurement model, which is described as follows: And construct the FOA measurement model, which is described as: in, represents the TOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target without being contaminated by measurement noise, represents the TOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, a represents the underwater sound speed gradient, q i is the intermediate quantity introduced, q i =g i ||u o -s i ||, g i is the intermediate quantity introduced, b represents the speed of sound on the ocean surface, and the symbol "|| ||" is the symbol of the two-norm. Indicates u o The vector consisting of the first and second elements in s i,(1:2) Indicates i The vector consisting of the first and second elements in f i o represents the FOA measurement value of the signal emitted by the i-th fixed beacon reaching the mobile target without being contaminated by measurement noise, represents the FOA measurement noise of the signal emitted by the i-th fixed beacon reaching the mobile target, f c represents the carrier frequency, c represents the speed of sound at the moving target, Step 4: Preprocess the TOA measurement model constructed in step 3 as follows: First, the propagation speed c of the signal emitted by the i-th fixed beacon to the mobile target is i Approximately The approximate TOA measurement model is described as: Then, the left and right sides of the approximated TOA measurement model are squared simultaneously, and the second-order noise term is ignored to obtain the transformation model, which is described as: Step 5: The transformation model is formulated as a constrained weighted least squares problem, described as: Where x is the optimization variable of the constrained weighted least squares problem, u represents the position variable of the moving target, u=[x u ,y u ,z u ] T , x u represents the horizontal coordinate of u, y u represents the ordinate of u, z u represents the vertical coordinate of u, u (1:2) represents the vector consisting of the first and second elements in u, ((A1x+h1) T W1(A1x+h1)) is the objective function of the constrained weighted least squares problem, A1 is the introduced coefficient matrix, and the vector formed by the i-th row elements of A1 is A1(i,:), h1 is the introduced coefficient vector, and the i-th element in h1 is W1 is the introduced weight matrix, E[] means to find the mathematical expectation, B1 is the introduced coefficient matrix, and the element in the i-th row and i-th column of B1 is B1(i,i). The rest of the elements in B1 are 0, n t represents the TOA measurement noise vector, n t Subject to the mean of 0 and the covariance matrix is Q t Gaussian distribution of , x(1) represents the first element in x, x(2) represents the second element in x, x(3) represents the third element in x, x(4) represents the fourth element in x, and x(5) represents the fifth element in x; Step 6: Introduce a new auxiliary matrix variable X into the constrained weighted least squares problem constructed in step 5, X = xx T ; Then set X = xx T The equivalent is rank(X)=1,X=0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(X)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as: Among them, rank() means to find the rank of the matrix, X≥0 means that X is semi-positive definite, tr() is the trace operation of the matrix elements, is the objective function of the convex semi-positive definite programming problem, X and x are the optimization variables of the convex semi-positive definite programming problem, X(1,1) represents the first row and first column element in X, X(2,2) represents the second row and second column element in X, and X(3,3) represents the third row and third column element in X; Step 7: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 6 and obtain the estimated value of x, which is recorded as according to Get a rough estimate of u, denoted as in, express The vector consisting of the first to third elements in ; Step 8: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as: Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as: ; in, q i Medium Replace u o get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between Replace u o get Step 9: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 8 are constructed as a constrained weighted least squares problem, which is described as: Where y is the optimization variable in the constrained weighted least squares problem, Δu1 represents the position deviation variable of the moving target, Δx u,1 Represents the horizontal coordinate deviation variable of the moving target position, Δy u,1 Represents the position ordinate deviation variable of the moving target, Δz u,1 represents the vertical coordinate deviation variable of the moving target’s position, v represents the moving speed variable of the moving target, v = [v x ,v y ,v z ] T , v x represents the velocity component of v along the x-axis, v y represents the velocity component of v along the y-axis, v z represents the velocity component of v along the z-axis, ((A2y+h2) T W2(A2y+h2)) is the objective function of the constrained weighted least squares problem, A2 is the introduced coefficient matrix, The element in the ith row and first column of The element in the ith row and second column of The element in the ith row and third column of The vector composed of the elements in the ith row and the 4th to 9th columns is 0 N×6 represents an all-zero matrix of dimension N×6, The vector composed of the elements in the i-th row and the first to fifth columns is , The vector composed of the elements in the ith row and the 6th to 9th columns is , h2 is the introduced coefficient vector, The i-th element of The i-th element of W2 is the introduced weight matrix, B2 is the introduced coefficient matrix, The element in the i-th row and i-th column is The rest of the elements in are 0. The element in the i-th row and i-th column is The rest of the elements are 0, n represents the TOA and FOA composite measurement noise vector, n = [(n t ) T ,(n f ) T ] T , n f represents the FOA measurement noise vector, n f Subject to the mean of 0 and the covariance matrix is Q f Gaussian distribution of , y(1) represents the first element of y, y(2) represents the second element of y, y(3) represents the third element of y, y(4) represents the fourth element of y, y(5) represents the fifth element of y, y(6) represents the sixth element of y, y(7) represents the seventh element of y, y(8) represents the eighth element of y, y(9) represents the ninth element of y, y(1:3) represents the vector consisting of the first to third elements of y, and y(4:6) represents the vector consisting of the fourth to sixth elements of y; Step 10: Introduce a new auxiliary matrix variable Y into the constrained weighted least squares problem constructed in step 9, Y = yy T ; Then Y = yy T The equivalent is rank(Y)=1,Y≥0; then use the semi-positive definite relaxation technique to relax the non-convex constraint rank(Y)=1, and transform the constrained weighted least squares problem into a convex semi-positive definite programming problem, which can be described as: Among them, Y ≥ 0 means that Y is positive semi-definite, is the objective function of the convex semi-positive definite programming problem, Y and y are the optimization variables of the convex semi-positive definite programming problem, Y(3,4) represents the 3rd row and 4th column element in Y, Y(1,6) represents the 1st row and 6th column element in Y, Y(3,5) represents the 3rd row and 5th column element in Y, Y(2,6) represents the 2nd row and 6th column element in Y, Y(1,4) represents the 1st row and 4th column element in Y, Y(2,5) represents the 2nd row and 5th column element in Y, and Y(3,6) represents the 3rd row and 6th column element in Y; Step 11: Use the interior point method to solve the convex semi-positive definite programming problem obtained in step 10 and obtain the estimated value of y, which is recorded as according to Get the estimated values of Δu1 and v, which are recorded as and According to and The first step of refining the position estimate of the moving target is obtained, denoted as in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in ; Step 12: Use the first-order Taylor expansion to convert the TOA measurement model constructed in step 3 into Expanding at , we get the first-order Taylor expansion related to TOA measurement, expressed as: Similarly, using the first-order Taylor expansion, the FOA measurement model constructed in step 3 is Expanding at , we get the first-order Taylor expansion related to FOA measurement, expressed as: ; in, use replace get express The horizontal axis of express The vertical coordinate of express The vertical coordinate of express The vector consisting of the first and second elements in , Indicates u o and The deviation between express and The deviation between express and The deviation between express and The deviation between express The velocity component along the x-axis, express The velocity component along the y-axis, express The velocity component along the z-axis, Δv o Indicates v o and The deviation between express and The deviation between express and The deviation between express and The deviation between Step 13: The first-order Taylor expansion related to TOA measurement and the first-order Taylor expansion related to FOA measurement obtained in step 12 are constructed as a linear weighted least squares problem, which is described as: Among them, Δp is the optimization variable of the linear weighted least squares problem, Δu2 represents the deviation variable of the first step refinement position estimate of the moving target, Δu2 = [Δx u,2 ,Δy u,2 ,Δz u,2 ] T , Δx u,2 represents the first step of refining the position estimate of the moving target, the horizontal coordinate deviation variable, Δy u,2 represents the first step of refining the position estimate of the moving target, Δz u,2 represents the vertical coordinate deviation variable of the first step refined position estimate of the moving target, Δv represents the moving speed deviation variable of the moving target, Δv = [Δv x ,Δv y ,Δv z ] T , Δv x represents the velocity component of Δv along the x-axis, Δv y represents the velocity component of Δv along the y-axis, Δv z represents the velocity component of Δv along the z-axis, ((A3Δp+h3) T W3(A3Δp+h3)) is the objective function of the linear weighted least squares problem, A3 is the introduced coefficient matrix, Medium replace get The vector composed of the elements in the i-th row, the first column to the second column is , The vector composed of the elements in the ith row and the third to fourth columns of , The vector composed of the elements in the 5th to 6th columns of the i-th row is h3 is the introduced coefficient vector, Medium replace get The i-th element in is W3 is the introduced weight matrix, B3 is the introduced coefficient matrix, which is used in B2 replace Get B3,Q = blkdiag (Q t ,Q f ), blkdiag(Q t ,Q f ) indicates that Q t ,Q f Assemble into a block diagonal matrix; Step 14: Solve the linear weighted least squares problem constructed in step 13 and obtain the optimal solution of Δp, which is recorded as Then we get the final estimated values of u and v, which are recorded as and in, express The vector consisting of the first to third elements in , express The vector consisting of the 4th to 6th elements in .
