Autonomous underwater vehicle cluster node self-positioning method

Abstract

The invention discloses an autonomous underwater vehicle cluster node self-positioning method, which belongs to the technical field of underwater detection, and comprises the following steps: inputting preprocessed acoustic environment characteristic data into a Bi-LSTM-MLP model for training, generating a predicted sound velocity model, and calculating propagation time by adopting a bidirectional time difference method; dividing the vertical depth of the water body into a plurality of discrete layers through a sound velocity model, establishing a relationship between a multi-layer propagation angle and the sound velocity by using a Snell law, and calculating the propagation angle of each layer in combination with the propagation time; calculating the horizontal distance of the propagation path of each layer by using the geometrical relationship between the horizontal distance of each layer and the thickness, and accumulating to obtain the horizontal distance of the total propagation path; determining the spatial position of the AUV by combining the horizontal distances from the underwater AUV to the three water surface AUVs and combining depth information through a three-circle positioning method; according to the cluster node self-positioning method for the autonomous underwater vehicle, the adaptability and robustness of a positioning algorithm to a complex marine environment are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of underwater detection technology, and in particular to a self-positioning method for underwater autonomous vehicle cluster nodes. Background Art

[0002] The node positioning technology of underwater autonomous vehicle (AUV) groups has attracted much attention due to its wide application in military and civilian fields. This technology is of great significance for achieving high-precision seabed mapping, deep-sea resource detection and development, underwater security monitoring, and analysis of unknown target behavior. However, traditional AUV positioning technology is easily disturbed by factors such as underwater sound speed changes, signal propagation delays, and propagation path refraction, which greatly reduces the positioning accuracy. Therefore, the underwater AUV positioning method combined with the acoustic wave refraction effect has become a research hotspot for improving the AUV self-positioning accuracy, and has promoted the in-depth development of related technologies.

[0003] Time difference of arrival (TDOA) has always been one of the important methods in the field of AUV (autonomous underwater vehicle) positioning. This technology determines the position of the AUV by measuring the time difference between signals arriving at multiple sensors. It not only has high positioning accuracy, but also effectively overcomes the challenges brought by the propagation delay of underwater acoustic signals. In recent years, LiuYing et al. (2023) proposed a TDOA measurement method based on the multipath channel effect of the underwater environment. This method introduces a calibration source to improve the positioning performance. Compared with traditional technologies, its mean square error is significantly reduced. In some scenarios, the efficiency exceeds 90% under low noise conditions and exceeds 80% under medium noise conditions.

[0004] In addition, ZhangTao et al. proposed an AUV positioning method based on the strapdown inertial navigation system (SINS) / long baseline (LBL) tightly coupled algorithm. This method can effectively and regularly compensate for the accumulated AUV position error by introducing the LBL positioning information centered on the SINS. However, the positioning accuracy of the above methods is easily affected by the uncertainty of the sound propagation speed, which leads to a decrease in positioning accuracy.

[0005] In response to the above problems, a self-positioning method for underwater AUV cluster nodes based on sound speed matching and two-way time difference fusion is proposed. This method innovatively uses the Bi-LSTM-MLP model to accurately model the spatiotemporal variation characteristics of sound speed in complex ocean environments, and constructs a multi-level relationship between propagation angle and sound speed based on Snell's law, thereby accurately simulating the impact of sound wave refraction effect on the propagation path. Through this improvement, the method in this paper can effectively reduce the interference of sound speed change and sound wave refraction effect on positioning accuracy. Summary of the invention

[0006] The purpose of the present invention is to provide a self-positioning method for underwater autonomous vehicle cluster nodes to solve the problems mentioned in the above background technology.

[0007] To achieve the above object, the present invention provides a method for self-positioning of underwater autonomous vehicle cluster nodes, comprising the following steps:

[0008] S1. Input the preprocessed acoustic environment feature data into the Bi-LSTM-MLP model for training to generate a predicted sound speed model. The Bi-LSTM-MLP model includes a Bi-LSTM layer consisting of two independent LSTMs and an MLP layer consisting of an input layer, one or more hidden layers, and an output layer.

[0009] S2, using the two-way time difference method to calculate the propagation time to avoid the error caused by the clock deviation between the surface AUV and the underwater AUV;

[0010] S3, divide the vertical depth of the water body into several discrete layers through the sound speed model, use Snell's law to establish the relationship between the multi-level propagation angle and the sound speed, and calculate the propagation angle of each layer in combination with the propagation time;

[0011] S4. Calculate the horizontal distance of each layer of propagation path by using the geometric relationship between the horizontal distance and thickness of each layer, and accumulate the horizontal distance of the total propagation path;

[0012] S5. Combining the horizontal distances from the underwater AUV to the three surface AUVs, the spatial position of the AUV is determined by combining the depth information with the three-circle positioning method.

[0013] Preferably, the specific process of S1 is: the input data passes through the Bi-LSTM layer to output the hidden state y, the hidden state y is input to the hidden layer of the MLP layer, and finally the predicted value of the sound speed is output to obtain the predicted sound speed model.

[0014] Preferably, the specific workflow of the Bi-LSTM layer is:

[0015] LSTM consists of a memory unit and three unit gates: input gate, forget gate, and output gate. Its core idea is to model the long-term dependency relationship in the time series through the gating mechanism. The workflow is as follows:

[0016] f t =σ(W f ·[h t-1 ,x t ]+b f ) (1)

[0017] i t =σ(W i ·[h t-1 ,x t]+b i ) (2)

[0018]

[0019] o t =σ(W o ·[h t-1 ,x t ]+b o ) (5)

[0020] h t =o t ·tanh C t ) (6)

[0021] Among them, x t is the input of the current time step; is the candidate memory cell state; f t ,i t , o t are the outputs of the forget, input, and output gates in the LSTM unit respectively; W f , W i , W o , W c The weight matrices corresponding to the forget, input, output gates, and memory cells; C t , C t-1 is the memory cell state at the current time step and the previous time step; h t ,h t-1 is the hidden state of the current time step and the previous time step; b f , b i , b c , b o is the corresponding deviation;

[0022] Bi-LSTM processes the forward (from the past to the future) and reverse (from the future to the past) sequence data through two independent LSTM networks, and finally outputs the concatenation result y of the forward and reverse LSTM hidden states. The specific process is as follows:

[0023]

[0024] in, is the hidden state of the forward LSTM, is the hidden state of the reverse LSTM, and both are calculated by formula (6).

[0025] Preferably, each layer of the MLP layer is composed of a number of neurons (also called nodes), and the calculation formula is:

[0026] a l =fW l ·al-1 +b l (10)

[0027] Among them, a l represents the activation value of the lth layer, W l and b l are weight matrices and biases respectively, and f is a nonlinear activation function;

[0028] The hidden state y enters the MLP hidden layer, and the activation value of each layer is calculated by formula (10), and the predicted value of the final output sound speed is calculated by formula (11)

[0029]

[0030] Among them, W1 and W2 are the weight matrices of the fully connected layer, and b1 and b2 are bias terms.

[0031] Preferably, the specific steps of S2 are as follows:

[0032] S21, surface AUV in T 0 The underwater AUV sends a signal at all times. After receiving the signal, the underwater AUV returns a response signal after a delay of Δt. The surface AUV 1 Receive the response signal at all times;

[0033] S22, surface AUV recording signal sending time T 0 and the time T when the response signal is received 1 First, calculate the total round-trip propagation time of the signal, subtract the processing delay of the underwater AUV from the total propagation time, and get the actual propagation time of the signal. The formula is as follows:

[0034] T=T 1 -(T 0 +Δt)(12)

[0035] Where T is the actual propagation time of the signal.

[0036] Preferably, the Shell law in S3 is expressed as:

[0037]

[0038] Among them, β i is the propagation angle of the i-th layer, v i is the average sound speed in the ith layer.

[0039] Preferably, the specific process of S4 calculating the horizontal distance of the total propagation path is:

[0040] x i =d i tanβi (14)

[0041]

[0042] Among them, x i is the horizontal propagation distance of the sound wave at the ith position, d i is the thickness of the i-th layer, t i is the propagation time of the sound wave in the i-th layer, x is the total horizontal distance of propagation, and t is the total time of propagation.

[0043] Preferably, the formula for solving the three-circle positioning method in S5 is:

[0044] Z=(∥xy-A∥-X 1 ) 2 +(∥xy-B∥-X 2 ) 2 +(∥xy-C∥-X 3 ) 2 (19)

[0045]

[0046] Among them, X 1 , X 2 , X 3 is the horizontal distance from three surface AUVs to one underwater AUV, xy = (x, y) is the optimal solution for the target position, A = (Ax, Ay), B = (Bx, By), C = (Cx, Cy) are the three known circle center coordinates, ∥xy-A∥ represents the Euclidean distance from the target position to point A, and Z is the sum of the distances from the target position to the three circle centers.

[0047] Therefore, the present invention adopts the above-mentioned underwater autonomous vehicle cluster node self-positioning method, which has the following beneficial effects:

[0048] (1) The present invention effectively combines sound velocity modeling with positioning algorithms, significantly improving positioning accuracy and system stability in complex underwater environments. It also accurately measures propagation time through sound velocity prediction and two-way time difference method, significantly improving the adaptability and robustness of positioning algorithms to complex marine environments.

[0049] (2) The present invention does not require the underwater AUV to carry expensive sensors or positioning systems, which makes it suitable for the efficient positioning requirements of AUV clusters.

[0050] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 This is a flow chart of a method for self-positioning of underwater autonomous vehicle cluster nodes according to the present invention;

[0052] Figure 2 A Bi-LSTM-MLP model structure diagram of a self-positioning method for underwater autonomous vehicle cluster nodes of the present invention;

[0053] Figure 3 This is a LSTM model structure diagram of a self-positioning method for underwater autonomous vehicle cluster nodes of the present invention;

[0054] Figure 4 A schematic diagram of a two-way time difference method for a self-positioning method of a cluster node of an underwater autonomous vehicle according to the present invention;

[0055] Figure 5 A propagation path diagram of a self-positioning method for a cluster node of an underwater autonomous vehicle according to the present invention;

[0056] Figure 6 The present invention provides a three-circle intersection positioning diagram for a method for self-positioning a cluster node of an underwater autonomous vehicle. DETAILED DESCRIPTION

[0057] The following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0058] Example

[0059] like Figure 1 As shown, the present invention provides a method for self-positioning of underwater autonomous vehicle cluster nodes, comprising the following steps:

[0060] S1. Input the pre-processed acoustic environment feature data into the Bi-LSTM-MLP model for training to generate a predicted sound speed model. The Bi-LSTM model structure is as follows: Figure 2 As shown in FIG. 1 , the Bi-LSTM-MLP model includes a Bi-LSTM layer consisting of two independent LSTMs and an MLP layer consisting of an input layer, one or more hidden layers, and an output layer.

[0061] The input data passes through the Bi-LSTM layer to output the hidden state y, which is then input into the hidden layer of the MLP layer. Finally, the predicted value of the sound speed is output to obtain the predicted sound speed model.

[0062] The specific workflow of the Bi-LSTM layer is:

[0063] like Figure 3As shown in the figure, LSTM consists of a memory unit and three unit gates: input gate, forget gate, and output gate. The core idea is to model the long-term dependency relationship in the time series through the gating mechanism. The workflow is as follows:

[0064] f t =σ(W f ·[h t-1 ,x t ]+b f ) (1)

[0065] i t =σ(W i ·[h t-1 ,x t ]+b i ) (2)

[0066]

[0067] o t =σ(W o ·[h t-1 ,x t ]+b o ) (5)

[0068] h t =o t ·tanh C t ) (6)

[0069] Among them, x t is the input of the current time step; is the candidate memory cell state; f t ,i t , o t are the outputs of the forget, input, and output gates in the LSTM unit respectively; W f , W i , W o , W c The weight matrices corresponding to the forget, input, output gates, and memory cells; C t , C t-1 is the memory cell state at the current time step and the previous time step; h t ,h t-1 is the hidden state of the current time step and the previous time step; b f , b i , b c , b o is the corresponding deviation;

[0070] On this basis, Bi-LSTM processes the forward (from past to future) and reverse (from future to past) sequence data through two independent LSTM networks. Each LSTM network runs independently according to the above process. The final output is combined by connecting the hidden state vectors of the forward and reverse networks, thereby capturing the contextual information of each time point in the sequence at the same time, and finally outputting the concatenation result y of the forward and reverse LSTM hidden states. The specific process is as follows:

[0071]

[0072] in, is the hidden state of the forward LSTM, is the hidden state of the reverse LSTM. The two are calculated by formula (6). y is the final bidirectional LSTM output, that is, the result of concatenating the hidden states of the forward and reverse LSTM.

[0073] Each layer of the MLP layer is composed of several neurons (also called nodes), and information is transmitted between neurons in a fully connected manner. The core feature of MLP is that it can extract the features of input data through hidden layers and model complex nonlinear relationships using nonlinear activation functions.

[0074] The input data starts from the input layer and passes through the hidden layer and output layer in sequence. The calculation formula for each layer is:

[0075] a l =fW l ·a l-1 +b l (10)

[0076] Among them, a l represents the activation value of the lth layer, W l and b l are the weight matrix and bias respectively, and f is the nonlinear activation function. By changing the type of nonlinear activation function to meet the needs of different tasks, the fitting accuracy can be improved.

[0077] The hidden state y enters the MLP hidden layer, and the activation value of each layer is calculated by formula (10), and the predicted value of the final output sound speed is calculated by formula (11)

[0078]

[0079] Among them, W1 and W2 are the weight matrices of the fully connected layer, and b1 and b2 are bias terms.

[0080] S2. The two-way time difference method is used to calculate the propagation time to avoid the error caused by the clock deviation between the surface AUV and the underwater AUV.

[0081] The core idea of ​​the Two-Way Time Difference method is to eliminate the error caused by clock deviation in the one-way propagation process through the propagation time difference of two-way signals, thereby achieving high-precision time measurement.

[0082] The basic workflow of the two-way time difference method is as follows: Figure 4 shown.

[0083] S21, surface AUV in T 0 The underwater AUV sends a signal at all times. After receiving the signal, the underwater AUV returns a response signal after a delay of Δt. The surface AUV 1 Receive the response signal at all times;

[0084] S22, surface AUV recording signal sending time T 0 and the time T when the response signal is received 1 First, calculate the total round-trip propagation time of the signal, subtract the processing delay of the underwater AUV from the total propagation time, and get the actual propagation time of the signal. The formula is as follows:

[0085] T=T 1 -(T 0 +Δt)(12)

[0086] Where T is the actual propagation time of the signal.

[0087] Since both the sending time and the receiving time are recorded by the surface AUV, there is no need to consider the synchronization with the underwater AUV, thus avoiding the error caused by clock offset between the two. Through the two-way time difference method, the system can significantly reduce the measurement error caused by clock asynchrony, and there is no need to perform complex clock synchronization on each node, which has higher robustness and reliability.

[0088] S3. The vertical depth of the water body is divided into several discrete layers through the sound speed model. The relationship between the multi-level propagation angle and the sound speed is established using Snell's law, and the propagation angle of each layer is calculated in combination with the propagation time.

[0089] Snell's law (also called the law of refraction) describes the refraction law of light waves or other types of waves when they pass from one medium to another. Similarly, when seawater is divided into multiple layers according to depth, each layer of water can be regarded as a different medium due to the differences in physical parameters such as temperature and salinity of each layer of water. Therefore, the propagation path of sound waves in each water layer also shows similar refraction laws, such as Figure 5 shown.

[0090] In the case of sound wave propagation, Shell's law is expressed as:

[0091]

[0092] Among them, β i is the propagation angle of the i-th layer, v i is the average sound speed in the ith layer.

[0093] S4. Calculate the horizontal distance of each layer's propagation path using the geometric relationship between the horizontal distance and thickness of each layer, and accumulate the horizontal distance of the total propagation path.

[0094] The specific process of calculating the horizontal distance of the total propagation path is:

[0095] Depend on Figure 5 The geometric relationship shows that:

[0096] x i =d i tanβ i (14)

[0097] The propagation time of each layer is:

[0098]

[0099] The total horizontal distance propagated is:

[0100]

[0101] The total propagation time is:

[0102]

[0103] Combining the above equations, we can get:

[0104]

[0105] Among them, x i is the horizontal propagation distance of the sound wave at the ith position, d i is the thickness of the i-th layer, t i is the propagation time of the sound wave in the i-th layer, x is the total horizontal distance of propagation, and t is the total time of propagation.

[0106] By solving the above equation, the propagation angle β can be obtained i and the total horizontal distance x traveled.

[0107] Considering that the measured propagation time T is the total time for the signal to go back and forth, during the signal propagation process, the surface AUV will have a relative horizontal displacement relative to the underwater AUV. Let it be Δx, then:

[0108] Δx=x f -x b (twenty one)

[0109] T=T f +T b (twenty two)

[0110] where x f , T f is the total horizontal distance and time of the signal transmitted from the surface AUV to the underwater AUV, x b , T b is the total horizontal distance and time for the signal to propagate from the underwater AUV to the surface AUV.

[0111] If:

[0112]

[0113] Then, formula (21) and formula (22) can be written as:

[0114]

[0115] Among them, β fi , v fi , d fi is the incident angle, average sound velocity, and thickness of the i-th layer during the process of the signal being sent from the surface AUV to the underwater AUV; β bi , ν bi , d bi is the incident angle, average sound velocity, and thickness of the i-th layer in the process of the signal from the underwater AUV to the surface AUV. By solving formulas (25) and (26), we can get the f1 and β b1 The nonlinear equation of x f and x b .

[0116] S5. Combining the horizontal distances from the underwater AUV to the three surface AUVs, the spatial position of the AUV is determined by combining the depth information with the three-circle positioning method.

[0117] The horizontal distance X from three surface AUVs to an underwater AUV 1 , X 2 , X 3 And the depth meter information, construct three circles on a depth plane. The radius of the three circles is the horizontal distance X 1 , X 2 , X 3 , the center of the circle is the coordinates of the three surface AUVs, as follows Figure 6 As shown, the intersection of the three circles is the position of the underwater AUV.

[0118] In the actual process, some errors are difficult to avoid, and the intersection of the three circles may not be unique. Therefore, it is necessary to use optimization methods such as the least squares method to obtain the optimal solution within a reasonable error range.

[0119] The formula for solving the three-circle positioning method is:

[0120] Z=(∥xy-A∥-X 1 ) 2 +(∥xy-B∥-X 2 ) 2 +(∥xy-C∥-X 3 ) 2 (19)

[0121]

[0122] Among them, X 1 , X 2 , X 3 is the horizontal distance from three surface AUVs to one underwater AUV, xy = (x, y) is the optimal solution for the target position, A = (Ax, Ay), B = (Bx, By), C = (Cx, Cy) are the three known coordinates of the center of the circle, ∥xy-A∥ represents the Euclidean distance from the target position to point A, ∥xy-B∥, ∥xy-C∥ are the same, Z is the sum of the distances from the target position to the three centers of the circle, when the Z value is the smallest, xy is the optimal solution for the target position.

[0123] Therefore, the present invention adopts the above-mentioned underwater autonomous vehicle cluster node self-positioning method, accurately measures the propagation time through sound speed prediction and two-way time difference method, significantly improves the adaptability and robustness of the positioning algorithm to complex ocean environments, and the final average positioning error is about 1.2 meters, and the maximum positioning error does not exceed 2.5 meters.

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for self-positioning of underwater autonomous vehicle cluster nodes, characterized in that: The following steps are involved: S1. Input the preprocessed acoustic environment feature data into the Bi-LSTM-MLP model for training to generate a predicted sound speed model. The Bi-LSTM-MLP model includes a Bi-LSTM layer composed of two independent LSTMs and an MLP layer composed of an input layer, one or more hidden layers, and an output layer. S2, using the two-way time difference method to calculate the propagation time to avoid the error caused by the clock deviation between the surface AUV and the underwater AUV; S3, divide the vertical depth of the water body into several discrete layers through the sound speed model, use Snell's law to establish the relationship between the multi-level propagation angle and the sound speed, and calculate the propagation angle of each layer in combination with the propagation time; S4. Calculate the horizontal distance of each layer of propagation path by using the geometric relationship between the horizontal distance and thickness of each layer, and accumulate the horizontal distance of the total propagation path; S5. Combining the horizontal distances from the underwater AUV to the three surface AUVs, the spatial position of the AUV is determined by combining the depth information with the three-circle positioning method.

2. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 1, characterized in that: The specific process of S1 is as follows: the input data passes through the Bi-LSTM layer to output the hidden state y, the hidden state y is input to the hidden layer of the MLP layer, and finally the predicted value of the sound speed is output to obtain the predicted sound speed model.

3. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 2, characterized in that: The specific workflow of the Bi-LSTM layer is as follows: LSTM consists of a memory unit and three unit gates: input gate, forget gate, and output gate. The workflow is as follows: f t =σ(W f ·[h t-1 ,x t ]+b f )(1) i t =σ(W i ·[h t-1 ,x t ]+b i )(2) the t =σ(W o ·[h t-1 ,x t ]+b o (5) h t =o t tanhC t )(6) Where x t is the input of the current time step; is the candidate memory cell state; f t ,i t , o t are the outputs of the forget, input, and output gates in the LSTM unit respectively; W f , W i , W o , W c The weight matrices corresponding to the forget, input, output gates, and memory cells; C t , C t-1 is the memory cell state at the current time step and the previous time step; h t ,h t-1 is the hidden state of the current time step and the previous time step; b f , b i , b c , b o is the corresponding deviation; Bi-LSTM processes the forward and reverse sequence data through two independent LSTM networks respectively, and finally outputs the concatenation result y of the forward and reverse LSTM hidden states. The specific process is as follows: in, is the hidden state of the forward LSTM, is the hidden state of the reverse LSTM, and both are calculated by formula (6).

4. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 3, characterized in that: The calculation formula for each layer of the MLP layer is: a (l) =f(W (l) ·a (l-1) +b (l) ) (10) Among them, a (l) represents the activation value of the lth layer, W (l) and b (l) are weight matrices and biases respectively, and f is a nonlinear activation function; The hidden state y enters the MLP hidden layer, and the activation value of each layer is calculated by formula (10), and the predicted value of the final output sound speed is calculated by formula (11) Among them, W1 and W2 are the weight matrices of the fully connected layer, and b1 and b2 are bias terms.

5. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 1, characterized in that: The specific steps of S2 are as follows: S21, the surface AUV sends a signal to the underwater AUV at time T0. After receiving the signal, the underwater AUV returns a response signal after a delay of Δt. The surface AUV receives the response signal at time T1. S22, the surface AUV records the signal sending time T0 and the time T1 of receiving the response signal. First, the total propagation time of the signal round trip is calculated, and the processing delay of the underwater AUV is subtracted from the total propagation time to obtain the actual propagation time of the signal. The formula is as follows: T=T1-(T0+Δt)(12) Where T is the actual propagation time of the signal.

6. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 1, characterized in that: The expression of Shell's law in S3 is: Among them, β i is the propagation angle of the i-th layer, v i is the average sound speed in the i-th layer.

7. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 6, characterized in that: The specific process of S4 calculating the horizontal distance of the total propagation path is: x i =d i tanβ i (14) Among them, x i is the horizontal propagation distance of the sound wave at the ith position, d i is the thickness of the i-th layer, t i is the propagation time of the sound wave in the i-th layer, x is the total horizontal distance of propagation, and t is the total time of propagation.

8. The method for self-positioning of underwater autonomous vehicle cluster nodes according to claim 7, characterized in that: The formula for solving the three-circle positioning method in S5 is: Z=(∥xy-A∥-X1) 2 +(∥xy-B∥-X2) 2 +(∥xy-C∥-X3) 2 (19) Among them, X1, X2, and X3 are the horizontal distances from three surface AUVs to an underwater AUV, xy = (x, y) is the optimal solution for the target position, A = (Ax, Ay), B = (Bx, By), and C = (Cx, Cy) are the three known coordinates of the center of the circle, ∥xy-A∥ represents the Euclidean distance from the target position to point A, and Z is the sum of the distances from the target position to the three centers of the circle.

Images