A Real-time Cooperative Localization Method for Multiple AUVs Based on Time Difference of Underwater Acoustic Communication

Abstract

The present invention discloses a multi-aUV real-time cooperative positioning method based on the time difference of underwater acoustic communication, which relates to the field of AUV positioning. The present invention is oriented to the underwater anchorless scenario, and uses the time difference of underwater acoustic communication between the master and slave aUVs to eliminate the influence caused by the clock asynchronism between the aUVs. On this basis, a cooperative positioning model with dynamic spatio-temporal correlation is established by using the position information of the master and slave aUVs and the time difference of underwater acoustic communication, and a non-linear measurement equation about the azimuth and propagation time difference of the slave aUV is reconstructed. The measurement equation is linearized by using the Taylor formula, and a closed-form expression about the position of the slave aUV is obtained according to the weighted least squares theory. On this basis, aiming at the inaccurate positioning caused by model errors and various noises, the explicit expression of the position error compensation amount relative to the position estimation value of the slave aUV is derived by using the perturbation method, further improving the cooperative positioning accuracy of the slave aUV. The present invention solves the problem of difficult cooperative positioning of the aUV system in the underwater anchorless scenario.

Description

Technical Field

[0001] The present invention relates to the field of underwater vehicle positioning, and in particular to a multi-underwater vehicle real-time cooperative positioning method based on the time difference of underwater acoustic communication. Background Art

[0002] Underwater unmanned vehicles (referred to as underwater vehicles) clusters have been widely used in the fields of underwater military exploration, deep-sea ocean exploration, biological monitoring, etc. due to their advantages of strong mobility, flexible action range, high robustness, etc. Accurately obtaining the position of the underwater vehicle itself is an important prerequisite for the multi-underwater vehicle cluster to efficiently execute tasks. In the existing underwater vehicle positioning methods, anchor nodes are usually deployed in a certain fixed sea area, and the underwater vehicle positioning is realized through multiple anchor nodes with known positions. Typically, such as the long baseline positioning system. However, the coverage range of the anchor nodes is very limited for a vast sea area, and the deployment positions of the anchor nodes need to be determined in advance. Due to reasons such as the harsh underwater environment and the limited battery capacity that the anchor nodes can carry by themselves, the anchor nodes cannot work for a long time. On the other hand, when facing the task execution within the moving range, the long baseline positioning system cannot achieve high-precision positioning of the underwater vehicle within the mobile area. Another method is to realize real-time positioning according to the navigation equipment equipped on the underwater vehicle. Although it has the advantages of flexibility and mobility and does not require any anchor nodes, the high-precision navigation equipment is expensive, and there is an accumulated error that makes the positioning error of the underwater vehicle increase infinitely. With the progress of underwater acoustic communication technology and the intelligent development of underwater vehicles, multi-underwater vehicle cooperative positioning has the potential to achieve high-precision positioning, and thus has been widely studied.

[0003] Whether it is a baseline-based positioning system or a multi-underwater vehicle cooperative positioning system, it is based on distance measurement, and accurate distance measurement is premised on the clock synchronization between underwater vehicles, between anchor nodes, and between anchor nodes and underwater vehicles. However, in the underwater environment, due to the manufacturing process of the clock-related components of the underwater vehicle itself and the influence of factors such as underwater temperature, salinity, and depth, it is difficult for the clocks of multiple underwater vehicles to remain synchronized, especially during long-term navigation. On the other hand, the complexity of the underwater acoustic channel and the delay effect of underwater acoustic communication not only reduce the cooperative update frequency of the underwater vehicle system, but also the movement of the underwater vehicle caused further reduces the cooperative positioning accuracy. Therefore, in the case of positioning without anchor node assistance and non-synchronized clocks between underwater vehicles, how to dynamically and real-time achieve high-precision cooperative positioning of underwater vehicles urgently needs to be studied and solved.

[0004] The patent with the application number 202210102709.7 and the name of "A UUV Cluster Cooperative Localization Method Integrated with Ultra-Short Baseline System" provides a method based on distance measurement and ultra-short baseline system. Although it can improve the cooperative localization accuracy of the master-slave submersible system, the ultra-short baseline system needs to be strictly calibrated in advance. For distance measurement, not only do the two communication parties need to ensure clock synchronization, but also limited by the large delay of underwater acoustic communication, the two-way underwater acoustic communication brings deviations in the UUV position and an increase in distance error. The patent with the application number 202111610043.8 and the name of "A Cooperative Navigation and Localization Method for AUV Cluster Deep-Water Operation with Dual-Master AUV Switching" provides a method for the master-slave submersible system. By using the master AUV to regularly float to the water surface to correct its own position and then improving the localization accuracy of the slave AUV through the multi-submersible cooperative localization method, this not only reduces the system cooperative update frequency but also cannot solve the problem of clock asynchronization between the master and slave AUVs. The patent with the application number 202210213443.3 and the name of "A Distributed Underwater Bearing-Only Localization Method for Small Platforms Based on Neural Networks" provides a single-submersible localization method using the bearing information between the reference node and the submersible to be located. However, the accuracy of the bearing information depends on the structure of the measurement array. Due to the limited battery of the submersible itself, the array cannot work for a long time, and the multi-element array seriously affects the maneuverability of the submersible due to its volume and other reasons. In summary, the above methods all have deficiencies in facing the submersible localization problem, cannot achieve real-time localization, and do not specifically consider the impact of the cumulative error of the navigation equipment equipped on the submersible itself on the multi-submersible cooperative localization result.

[0005] Therefore, those skilled in the art are committed to developing a multi-submersible real-time cooperative localization method based on the time difference of underwater acoustic communication. The present invention faces the dynamic real-time cooperative localization requirements of multi-submersibles with non-synchronized clocks in an underwater non-anchored scenario and proposes a multi-submersible real-time cooperative localization method based on the time difference of underwater acoustic communication. Different from the traditional localization methods that mainly use the relative distance measurement between submersibles, in the face of an underwater non-anchored scenario, for a submersible system with only a master submersible and a slave submersible and their clocks being non-synchronized, the time difference of underwater acoustic communication between adjacent moments of the master and slave submersibles and the navigation information of the submersibles themselves are used to achieve real-time cooperative localization of the slave submersible, so as to improve the localization accuracy of the submersible system. Summary of the Invention

[0006] In view of the above-mentioned defects of the prior art, the technical problems to be solved by the present invention are the difficulties in keeping the clocks synchronized between multi-submersibles, the limited measurement performance of submersibles, and the difficulty of real-time localization of submersibles in an underwater non-anchored scenario.

[0007] To achieve the above object, the present invention provides a multi-submersible real-time cooperative localization method based on the time difference of underwater acoustic communication, including the following steps:

[0008] Step 1: Initialize the main submersible and the slave submersible equipment;

[0009] Step 2: The main submersible and the slave submersible respectively record the initial time information and the initial position information, establish a communication link and complete one-way underwater acoustic communication, that is, the main submersible sends information and the slave submersible receives information. Specifically: the main submersible sends the actual value of its own position (the position representation method in the three-dimensional Cartesian coordinate system, the same below) and the time of sending the information After receiving the message, the slave submersible records the actual position of the received information and the reception time

[0010] Step 3: At the (i + 1) (i ≥ 0) moment, the main submersible and the slave submersible complete one-way underwater acoustic communication, and the slave submersible obtains the time information of the main submersible the actual position information of the main submersible its own time information and the rough position prediction value obtained according to the low-precision navigation equipment equipped on itself

[0011] Step 4: At the (i + 1) (i ≥ 0) moment, use the time information of the main submersible and the slave submersible at the i moment and the (i + 1) moment to obtain the underwater acoustic communication time difference (Time Difference of Communication, TDOC) Δτ i+1 ;

[0012] Step 5: Use the position estimated value of the slave submersible at the i moment and the rough position prediction value at the (i + 1) moment to obtain the rough measurement values of the heading angle, pitch angle and speed of the slave submersible and obtain the correction value based on the rough position prediction value of the slave submersible at the (i + 1) moment

[0013] Step 6: According to the position information of the main submersible and the slave submersible at the i moment and the (i + 1) moment and the underwater acoustic communication time difference Δτ i+1 , establish a collaborative positioning model with dynamic spatio-temporal association and construct a nonlinear measurement equation about the heading angle of the slave submersible the pitch angle of the slave submersible and the propagation time difference Δt between the main submersible and the slave submersible i ; linearize the nonlinear measurement equation on the basis of fully considering various noises;

[0014] Step 7: Convert the linearized measurement equation into a weighted least squares problem to obtain the position estimated value of the slave submersible at the (i + 1) moment ​Display expression;

[0015] Step 8: Analyze the source of positioning error to obtain the relative position estimation value of the submersible at the (i + 1)th moment Position error compensation amount Display expression;

[0016] Step 9: According to the position estimation value of the submersible at the (i + 1)th moment And its position error compensation amount Obtain the high-precision position estimation value of the submersible at the (i + 1)th moment

[0017] Step 10: Repeat steps 3 to 9 to estimate the position of the submersible in real time until the navigation ends.

[0018] Furthermore, the device initialization in step 1 includes determining whether the navigation device, depth sensor, and underwater acoustic communication machine are working properly.

[0019] Furthermore, in step 4, the underwater acoustic communication time difference Wherein, And And Are the times of the master submersible and the submersible at the (i + 1)th moment and the ith moment respectively.

[0020] Furthermore, in step 5, when the heading angle of the submersible The pitch angle of the submersible Error And Are relatively small compared to the true value, the correction value Can be obtained based on the rough predicted position value of the submersible at the (i + 1)th moment

[0021] Furthermore, in step 6, the Taylor formula is used to linearize the non-linear measurement equation.

[0022] Furthermore, step 6 includes the following steps:

[0023] Step 6.1: Use the true position values of the master and submersibles and the true value of the underwater acoustic communication time difference to construct a collaborative positioning model with dynamic spatio-temporal correlation;

[0024] Step 6.2: Construct the propagation time difference measurement equation;

[0025] Step 6.3: Construct the azimuth (heading angle, pitch angle) measurement equation;

[0026] Step 6.4: Construct the linearized measurement equation.

[0027] Furthermore, in step 6, the propagation time difference Δti can be expressed as where c is the speed of sound, is the distance between the position of the submersible at time (i + 1) and the position of the master submersible at time i, is the distance between the position of the submersible at time (i + 1) and the position of the slave submersible at time i, R d is the distance between the master submersible and the position of the slave submersible at time i.

[0028] Furthermore, in step 6, the mathematical relationship between the propagation time difference and the underwater acoustic communication time difference is where, Δt i is the propagation time difference, Δτ i+1 is the communication time difference, c is the speed of sound, is the distance between the position of the slave submersible at time (i + 1) and the position of the slave submersible at time i, is the distance between the master submersible and the position of the slave submersible at time (i + 1), L d is the distance between the position of the slave submersible at time (i + 1) and the position of the master submersible at time i.

[0029] Furthermore, step 8 analyzes the influence of various errors on the positioning performance based on the Cramer-Rao bound principle.

[0030] Furthermore, the sources of positioning errors analyzed in step 8 include model errors, measurement noise, position errors, and high-order truncation errors introduced by linearization.

[0031] Furthermore, step 8 uses the perturbation method and the law of large numbers to derive an explicit expression for the position error compensation amount.

[0032] Furthermore, the collaborative positioning method is based on the Bayesian theory framework.

[0033] In a preferred embodiment of the present invention, when the submersible performs a special task in an unknown area, due to the lack of anchor nodes within the maneuvering range of the submersible, the weak observability of the system composed of only one master submersible and one slave submersible, and the limited type of measurable information of the submersible, there is a problem of difficult real-time collaborative positioning for the submersible in an underwater anchor-free scenario. The present invention constructs a collaborative positioning model with dynamic spatio-temporal association by using the underwater acoustic communication time difference information between adjacent two moments of the submersible and the navigation information of the submersible itself. On this basis, an observation equation about the position of the submersible is constructed, and the weighted least squares theory is used to achieve real-time collaborative positioning of the submersible system in an anchor-free environment. Taking a three-dimensional scenario as an example, the premise for realizing the explicit expression of position solution is to obtain at least three observation equations about the position. Observation equations based on the heading angle, pitch angle, and propagation time difference are constructed by using the underwater acoustic communication time difference and the navigation information of the submersible itself, and the nonlinear observation equations are linearized according to the Taylor formula, and finally real-time collaborative positioning is achieved based on the weighted least squares theory.

[0034] In the submersible system in a complex underwater anchorless scenario, synchronous distance measurement is an effective type of information. However, the positioning accuracy of the method based on synchronous distance measurement is limited, and the submersible system cannot maintain clock synchronization for a long time, resulting in a further decline in positioning accuracy. The present invention proposes a measurement method based on the time difference of underwater acoustic communication, and uses time correlation to expand the navigation information of the submersible, solving the problem of insufficient information in the case of asynchronous clocks in the submersible system, thus laying a foundation for realizing a high-precision cooperative positioning method. Using the information type of the time difference of underwater acoustic communication between two adjacent moments of the submersible to eliminate the problem of difficult long-term synchronization in the underwater anchorless scenario of the submersible system, and using time correlation, the submersible's own navigation information is expanded into azimuth (heading angle, pitch angle) information to make full use of limited information.

[0035] On the basis of obtaining the positioning result, a simple and effective cooperative positioning accuracy compensation method is provided to make up for the problem of low cooperative positioning accuracy of the submersible system caused by factors such as noise. Based on the analysis of the Cramer-Rao bound principle, the present invention concludes that the cooperative positioning error of the submersible system in the anchorless environment comes from model error, measurement noise and position noise. On the basis of the positioning result, based on the perturbation analysis method and Taylor formula, an explicit expression of the positioning error compensation amount is derived to further improve the cooperative positioning accuracy.

[0036] Compared with the prior art, the present invention has the following obvious substantial features and remarkable advantages:

[0037] 1. Aiming at the problem of cooperative positioning of the submersible system in the underwater anchorless scenario, the present invention uses limited information, that is, only uses the time difference of underwater acoustic communication and the submersible's own navigation information to construct a cooperative positioning model with dynamic spatio-temporal correlation, and further constructs an observation equation to realize the real-time cooperative positioning of the submersible system.

[0038] 2. The present invention uses the time difference of underwater acoustic communication to eliminate the requirement of clock synchronization that is difficult to meet in practice for the submersible system. And using the submersible's own navigation information and time information to expand the azimuth (heading angle, pitch angle) information, realizing the full use of limited information of the submersible, laying a foundation for realizing a high-precision cooperative positioning method.

[0039] 3. Aiming at the complex underwater anchorless scenario, based on the Cramer-Rao bound theory, the reasons for the low cooperative positioning accuracy are analyzed, including model error, measurement noise and position noise and the high-order truncation error introduced in the linearization process. Further, based on the weighted least squares theory, combined with the perturbation analysis method and the law of large numbers, an explicit expression of the positioning error compensation amount based on the positioning result is derived using the Taylor consensus to improve the cooperative positioning accuracy.

[0040] The following will further illustrate the concept, specific structure and technical effects of the present invention in conjunction with the accompanying drawings, so as to fully understand the purpose, features and effects of the present invention. Description of the Drawings

[0041] Figure 1 It is a schematic diagram of a collaborative positioning model with dynamic spatio-temporal correlation in a preferred embodiment of the present invention;

[0042] Figure 2 It is a flowchart of a collaborative positioning algorithm in a preferred embodiment of the present invention. Detailed Embodiment

[0043] The following introduces multiple preferred embodiments of the present invention with reference to the accompanying drawings of the specification, making its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0044] In the drawings, components with the same structure are denoted by the same numerical labels, and components with similar structures or functions are denoted by similar numerical labels. The size and thickness of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the size and thickness of each component. To make the illustration clearer, the thickness of some components is appropriately exaggerated in the drawings.

[0045] Facing the current development status of the limited range of action and lack of flexibility of the marine positioning system, in the underwater non-anchoring scenario, for the dual-submersible operation system widely used at present, the main and slave submersibles are respectively set. To meet the high-precision self-information requirements during collaborative work, a real-time collaborative positioning method for multiple submersibles based on the time difference of underwater acoustic communication is provided.

[0046] The collaborative positioning algorithm provided by the present invention is for a positioning system composed of a main submersible and a slave submersible. Under the condition of ensuring normal maneuverability, each submersible is equipped with a navigation device, a depth sensor and an underwater acoustic communication machine. Specifically, the main submersible is equipped with a high-precision navigation device, that is, the main submersible can obtain high-precision self-position information. The slave submersible is only equipped with a navigation device with lower precision, that is, the rough predicted value of its own position information is realized through dead reckoning based on the measured attitude and motion information. The depth sensor is used to obtain the navigation depth of the submersible itself, and the underwater acoustic communication machine is used to realize the underwater acoustic communication between the main and slave submersibles.

[0047] The collaborative positioning method provided by the present invention is based on the Bayesian theory framework. It uses the time difference of underwater acoustic communication between the master and slave submersibles to eliminate the influence caused by the clock asynchronization between submersibles. On this basis, a collaborative positioning model with dynamic spatio-temporal correlation is established using the position information and communication time difference of the master and slave submersibles. The measurement equations regarding azimuth (heading angle, pitch angle) and propagation time difference are reconstructed. The Taylor formula is used to linearize the non-linear measurement equations, and the real-time estimation of the slave submersible's position is realized according to the weighted least squares theory. The perturbation method is used to realize the explicit expression of the position error compensation amount relative to the estimated value of the slave submersible's position, further improving the collaborative positioning accuracy of the slave submersible.

[0048] The multi-submersible collaborative positioning method is executed as follows:

[0049] (1) Step 1: The master and slave submersibles complete device initialization, that is, determine whether the navigation device, depth sensor, and underwater acoustic communication machine are working properly;

[0050] (2) Step 2: The master and slave submersibles respectively record the initial time information and initial position information, establish a communication link and complete one-way underwater acoustic communication, that is, the master submersible sends information and the slave submersible receives information. Specifically: The master submersible sends the actual value of its own position (the position representation method in the three-dimensional Cartesian coordinate system, the same below) and the time of sending information After receiving the message, the slave submersible records the actual position of the received information and the receiving time Next, the real-time collaborative positioning process of the submersible starts;

[0051] (3) Step 3: At the (i + 1) (i≥0) moment, the master and slave submersibles complete one-way underwater acoustic communication. The slave submersible obtains the time information of the master submersible the actual position information of the master submersible its own time information and the rough prediction value of the position obtained according to the low-precision navigation device equipped on itself

[0052] (4) Step 4: At the (i + 1) (i≥0) moment, use the time information of the master and slave submersibles at the i-th moment and the (i + 1)-th moment to obtain the time difference of underwater acoustic communication (Time Difference of Communication, TDOC) Δτ i+1 :

[0053]

[0054] (5) Step 5: Use the estimated value of the slave submersible's position at the i-th moment and the rough prediction value of the position at the (i + 1)-th moment to obtain the rough measurement values of the heading angle, pitch angle, and speed of the slave submersible When the error and are relatively small compared to the true value, a rough predicted value of the position of the slave submersible at time (i + 1) can be obtained Obtain the correction value to achieve the first stage of reducing the position error caused by the cumulative navigation error introduced by the low-precision navigation equipment of the slave submersible. Its calculation formula can be expressed as:

[0055] And

[0056]

[0057]

[0058]

[0059] Among them, and are respectively and variances, T s is the time interval between two adjacent moments, represents taking the mean value of (*).

[0060] (6) Step 6: According to the underwater acoustic communication time difference Δτ between the master and slave submersibles at time i i+1 to eliminate the influence caused by the clock non-synchronization between submersibles. On this basis, according to the position information of the master and slave submersibles at time i and (i + 1) establish a collaborative positioning model with dynamic spatio-temporal correlation and construct a nonlinear measurement equation regarding the heading angle of the slave submersible the pitch angle of the slave submersible and the propagation time difference Δt between the master and slave submersibles i ; on the basis of fully considering various noises, linearize the nonlinear measurement equation regarding the measurement vector . Specifically, Step 6 includes the following four parts:

[0061] (61) Step 61: According to the position information of the master and slave submersibles at time i and (i + 1) and the underwater acoustic communication time difference Δτ i+1 , establish a collaborative positioning model with dynamic spatio-temporal correlation, and its schematic diagram is as shown in Figure 1 .

[0062] (62) Step 62: Construct the propagation time difference measurement equation.

[0063] Corresponding to Figure 1Two propagation loops in it, and the corresponding theoretical propagation time delay and can be expressed as:

[0064]

[0065]

[0066] Furthermore, substituting the true value of the time difference of underwater acoustic communication gives

[0067]

[0068] where c is the speed of sound in water.

[0069] (63) Step 63: Construct the angle measurement equation.

[0070] For the slave vehicle, from time i to time (i + 1), the theoretical angle measurement equation for the slave vehicle can be expressed as:

[0071]

[0072] (64) Step 64: Construct the linearized measurement equation.

[0073] Define the position error of the master and slave vehicles the error of the time difference of underwater acoustic communication the propagation time difference error the angle error All are additive zero-mean Gaussian white noises, then the relationship between the true value and the actual measured value can be expressed as

[0074]

[0075]

[0076]

[0077]

[0078]

[0079]

[0080] It should be noted that and for Δτ i+1 , and and and and other physical quantities all have the above relationship, which will not be elaborated. In addition, for the convenience of subsequent description, define

[0081] Substitute it into Steps 62 and 63, and linearize the non - linear measurement equations (propagation time difference measurement equation, azimuth measurement equation) using Taylor's formula. While neglecting the second - order error terms, convert the linearized measurement equations into a weighted least squares problem (Weighted least squares, WLS), which can be expressed as

[0082]

[0083] where,

[0084]

[0085]

[0086] Furthermore,

[0087]

[0088] where,

[0089]

[0090] and C i has a dimension of 3×9, and the specific element values are

[0091]

[0092]

[0093]

[0094]

[0095]

[0096]

[0097]

[0098]

[0099]

[0100] and the remaining elements of C i are zero.

[0101] (7) Step 7: Convert the linearized range equation into a weighted least squares problem, and obtain the position estimate value of the submersible at the (i + 1) - th moment which can be expressed as

[0102]

[0103] Among them,

[0104]

[0105] (8) Step 8: According to the analysis theory of the Hybrid Cramer-Rao Lower Bound (HCRLB), the error in the position estimation of the submersible at the (i + 1)-th moment includes three aspects: (1) the model deviation caused by constructing a collaborative positioning model with dynamic spatio-temporal correlation using the information of its own low-precision navigation equipment; (2) the measurement information error and the submersible position error; (3) the truncation error introduced by neglecting the second-order and higher-order errors in the process of linearizing the non-linear measurement equation using the Taylor formula. Using the perturbation analysis method and the Taylor formula, the position error compensation amount of the relative position estimation value at the (i + 1)-th moment can be expressed as

[0106]

[0107] Among them,

[0108]

[0109]

[0110] Furthermore, assuming that the noises are independent of each other, according to the law of large numbers, by taking the mean value, the solution of the position error compensation amount of the relative position estimation value at the (i + 1)-th moment is realized, that is

[0111]

[0112] Among them,

[0113]

[0114]

[0115]

[0116]

[0117] and q * is the column vector composed of the diagonal elements of the covariance matrix Q * corresponding to (*).

[0118] (9) Step 9: According to the position estimation value of the submersible at the (i + 1)-th moment, the machine position error compensation amount At time (i + 1), from the high-precision estimated value of the submersible position is

[0119]

[0120] (10) Step 10: Repeat steps 3 to 9 to obtain the position estimation of the submersible in real time until the navigation ends.

[0121] Figure 2 This is the specific implementation process of a multi-submersible real-time cooperative positioning method based on the time difference of underwater acoustic communication provided by the present invention. The present invention aims at the positioning problem in the process of multi-submersible cooperative task execution. Therefore, in actual use, only one master submersible and any number of slave submersibles are required, and there is no requirement for clock synchronization of the submersibles. Here, one master submersible and one slave submersible are taken as an example.

[0122] To use the cooperative positioning method provided by the present invention, each submersible is equipped with a navigation device, a depth sensor, and an underwater acoustic communication machine while ensuring normal maneuverability. Specifically, the master submersible is equipped with a high-precision navigation device, that is, the master submersible can obtain relatively high-precision own position information. The slave submersible is only equipped with a navigation device with relatively low precision, that is, the rough estimation of its own position information is realized by dead reckoning based on the measured attitude and motion information. The depth sensor is used to obtain the navigation depth of the submersible itself, and the underwater acoustic communication machine is used to realize the one-way underwater acoustic communication between the master and slave submersibles.

[0123] As Figure 1 shown, the specific implementation steps of the present invention are as follows:

[0124] Step 1: The master and slave submersibles complete device initialization, that is, determine whether the navigation device, depth sensor, and underwater acoustic communication machine are working properly;

[0125] Step 2: The master submersible and the slave submersible respectively record the initial time information and initial position information, establish a communication link and complete one-way underwater acoustic communication, that is, the master submersible sends information and the slave submersible receives information. Specifically: the master submersible sends the actual value of its own position (the position representation method in the three-dimensional Cartesian coordinate system, the same below) and the time of sending information After receiving the message, the slave submersible records the actual position of the received information and the reception time

[0126] Step 3: At time (i + 1) (i≥0), the master submersible and the slave submersible complete one-way underwater acoustic communication, and the slave submersible obtains the time information of the master submersible the actual position information of the master submersible its own time information and the rough predicted value of the position obtained according to the low-precision navigation device equipped by itself

[0127] Step 4: At the (i + 1) (i ≥ 0) moment, obtain the time difference of underwater acoustic communication (TDOC) Δτ by using the time information of the master submersible and the slave submersible at the i-th moment and the (i + 1)-th moment i+1 is

[0128] Step 5: Use the estimated value of the position of the slave submersible at the i-th moment and the rough predicted value of the position at the (i + 1)-th moment to obtain the rough measurement values of the heading angle, pitch angle and speed of the slave submersible When the error of and is relatively small with respect to the true value, a correction value can be obtained based on the rough predicted value of the position of the slave submersible at the (i + 1)-th moment to improve the accuracy of the measurement vector composed of the angle and the propagation time difference; accuracy;

[0129] Step 6: According to the position information of the master submersible and the slave submersible at the i-th moment and the (i + 1)-th moment and the time difference of underwater acoustic communication Δτ i+1 , establish a collaborative positioning model with dynamic spatio-temporal correlation and construct a non-linear measurement equation for the heading angle of the slave submersible, the pitch angle of the slave submersible, and the propagation time difference Δt i between the master submersible and the slave submersible; on the basis of fully considering various noises, linearize the non-linear measurement equation by using the Taylor formula;

[0130] Step 7: Convert the linearized measurement equation into a weighted least squares problem to obtain the display expression of the estimated value of the position of the slave submersible at the (i + 1)-th moment

[0131] Step 8: Theoretical analysis shows that the position estimation error comes from model error, measurement noise, position error and high-order truncation error introduced by linearization. Using the perturbation method and the law of large numbers, derive and obtain the position error compensation amount of the relative position estimated value of the slave submersible at the (i + 1)-th moment. The display expression is

[0132]

[0133] Step 9: According to the estimated value of the position of the slave submersible at the (i + 1)-th moment And its position error compensation amount Obtain a high-precision estimated value of the position of the submersible at the (i + 1)-th moment Is

[0134] Step 10: Repeat the above steps 3 to 9 to estimate the position of the submersible in real time until the navigation ends.

[0135] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations based on the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field according to the concept of the present invention through logical analysis, reasoning, or limited experiments on the basis of the prior art should be within the protection scope determined by the claims.

Claims

1. A real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication, characterized in that, it includes the following steps: Step 1, initialize the master underwater vehicle and slave underwater vehicle devices; Step 2: The master submersible and the slave submersible respectively record the initial time information and the initial position information, establish a communication link and complete one-way underwater acoustic communication, that is, the master submersible sends information and the slave submersible receives information. Specifically: The master submersible sends the actual value of its own position and the time when the information is sent After receiving the message, the slave submersible records the actual position of the received information and the reception time Step 3: At the (i + 1)th (i ≥ 0) moment, the master submersible and the slave submersible complete one-way underwater acoustic communication, and the slave submersible obtains the time information of the master submersible The actual position information of the master submersible Its own time information And the rough position prediction value obtained according to the low-precision navigation equipment equipped on itself Step 4. At the moment of (i + 1) (i ≥ 0), obtain the underwater acoustic communication time difference Δτ using the time information of the master submersible and the slave submersible at the moment of i and the moment of (i + 1). i+1 ; Step 5: Use the estimated position value of the submersible at time i and the rough predicted position value at time (i + 1) to obtain the rough measurement values of the heading angle, pitch angle, and speed of the submersible and obtain the correction value based on the rough predicted position value of the submersible at time (i + 1) ​ Step 6: Based on the position information of the master submersible and the slave submersible at the i-th moment and the (i + 1)-th moment and the underwater acoustic communication time difference Δτ i+1 , establish a collaborative positioning model with dynamic spatio-temporal correlation and construct a nonlinear measurement equation regarding the heading angle of the slave submersible the pitch angle of the slave submersible and the propagation time difference Δt between the master submersible and the slave submersible i ; linearize the nonlinear measurement equation on the basis of fully considering various noises; Step 7: Convert the linearized measurement equation into a weighted least squares problem to obtain the display expression of the position estimated value of the submersible at the (i + 1)-th moment ; Step 8. Analyze the source of the positioning error to obtain the position error compensation amount of the relative position estimated value of the submersible at the (i + 1)-th moment and the display expression of ; Step 9: Obtain the high-precision estimated position of the slave submersible at the (i + 1)-th moment according to the estimated value of the position of the slave submersible at the (i + 1)-th moment and its position error compensation amount to obtain the high-precision estimated value of the position of the slave submersible at the (i + 1)-th moment Step 10, repeatedly execute the said Step 3 to the said Step 9, and estimate the position of the slave underwater vehicle in real time until the navigation ends.

2. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, the device initialization in the said Step 1 includes determining whether the navigation device, depth sensor and underwater acoustic communication machine are working properly.

3. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, In the said step 4, the time difference of underwater acoustic communication wherein, and and are respectively the times of the master submersible and the slave submersible at the (i + 1)-th moment and the i-th moment.

4. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, In step 5, a correction value can be obtained based on the rough predicted value of the position of the submersible at time (i + 1). Obtain a correction value 5. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, the said Step 6 linearizes the nonlinear measurement equation by using the Taylor formula.

6. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, the said Step 6 includes the following steps: Step 6.1, use the positions and time difference of underwater acoustic communication of the master and slave underwater vehicles at the i-th moment and the (i + 1)-th moment to construct a cooperative positioning model with dynamic spatio-temporal correlation; Step 6.2, construct a propagation time difference measurement equation; Step 6.3, construct an azimuth (heading angle, pitch angle) measurement equation; Step 6.4, construct a linearized measurement equation.

7. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, The propagation time difference Δt in step 6 i can be expressed as where c is the speed of sound, is the distance between the position of the submersible at time (i + 1) and the position of the main submersible at time i, is the distance between the position of the submersible at time (i + 1) and the position of the slave submersible at time i, and R d is the distance between the main submersible and the slave submersible at time i.

8. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, The mathematical relationship between the propagation time difference and the underwater acoustic communication time difference in the said step 6 is where Δt i is the propagation time difference, Δτ i+1 is the communication time difference, c is the sound speed, is the distance between the position of the slave vehicle at time (i + 1) and the position of the slave vehicle at time i, is the distance between the master vehicle and the position of the slave vehicle at time (i + 1), L d is the distance between the position of the slave vehicle at time (i + 1) and the position of the master vehicle at time i.

9. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, the said Step 8 analyzes the influence of various noises on the positioning performance based on the Cramer-Rao bound principle.

10. The real-time cooperative positioning method for multiple underwater vehicles based on time difference of underwater acoustic communication according to claim 1, characterized in that, the sources of positioning error analyzed in the said Step 8 include model error, measurement noise, position error and high-order truncation error introduced by linearization.

Images