Underwater acoustic two-way navigation method and system based on depth constraint and smoothness

Abstract

The present invention belongs to the field of underwater navigation. In order to solve the problem that the traditional underwater long-base acoustic navigation method cannot guarantee the accuracy of underwater two-way navigation, an underwater acoustic two-way navigation method and system based on depth constraint and smoothing are provided. Among them, the underwater acoustic two-way navigation method based on depth constraint and smoothing includes using the position precision factor to determine the optimal constraint information, and then combining the draft depth of the transducer to obtain the constrained vertical coordinates of the transducer in the earth-centered earth-fixed coordinate system; combining the constraint measurement equation of the measurement noise with the depth constraint, solving the underwater acoustic two-way navigation function model under the sound speed calculation constraint, and obtaining the constrained acoustic navigation result; using the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation result to obtain the final acoustic navigation result, which can improve the ranging accuracy and reduce the navigation error.

Description

Technical Field

[0001] The invention belongs to the field of underwater navigation, and in particular relates to an underwater acoustic two-way navigation method and system based on depth constraint and smoothness. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] The principle of the LBL (Long Baseline) acoustic navigation system is that the transducer transmits an interrogation acoustic signal, the transponder receives the signal and responds with an acoustic signal, and the transducer then receives the response acoustic signal. Through this process, the propagation time of the acoustic signal from the transducer to the transponder can be determined, and the straight-line observation distance from the transducer to the transponder can be calculated in combination with the sound velocity profile, and Kalman filtering is used to achieve underwater vehicle navigation. The complexity of the marine environment makes the acoustic observation data susceptible to various error factors, which leads to a decrease in the quality of the observation data, and thus significantly increases the navigation error of the underwater vehicle.

[0004] In order to improve the navigation accuracy of underwater vehicles, the existing technology uses the Sage-Husa noise estimation technology, and the algorithm can compensate for unknown system noise online, improve the adaptability of the filter in a dynamically changing environment, and use the equivalent gain matrix based on the Huber equivalent weight function to effectively resist the gross errors in acoustic observations and improve the reliability of positioning; however, in practical applications, it is necessary to consider its limitations such as computational complexity, parameter adjustment, and dependence on model accuracy. In addition, there are also studies that obtain the system error estimated by the seabed reference network data, extract the main periodic term of the system error based on Fourier transform, and then use the polynomial fitting method to construct a system error correction model, construct the underwater acoustic navigation enhanced observation equation with system error correction, and finally design an adaptive robust Kalman filter for underwater acoustic navigation to improve the accuracy of acoustic navigation observation; however, this method does not take into account the system error of underwater two-way navigation, and still cannot guarantee the accuracy of underwater two-way navigation.

[0005] In summary, the traditional underwater long baseline acoustic navigation method has limitations such as computational complexity, parameter adjustment and dependence on model accuracy. It does not take into account the systematic error of underwater two-way navigation and cannot guarantee the accuracy of underwater two-way navigation. Summary of the invention

[0006] In order to solve the technical problems existing in the above-mentioned background technology, the present invention provides an underwater acoustic two-way navigation method and system based on depth constraint and smoothing, which can improve ranging accuracy and reduce navigation errors.

[0007] In order to achieve the above object, the present invention adopts the following technical solution:

[0008] A first aspect of the present invention provides an underwater acoustic two-way navigation method based on depth constraint and smoothing.

[0009] An underwater acoustic two-way navigation method based on depth constraint and smoothness, comprising:

[0010] Based on the long baseline acoustic navigation principle, an underwater acoustic two-way navigation function model is constructed in the Earth-centered Earth-fixed coordinate system.

[0011] The optimal constraint information is determined by using the position precision factor, and then combined with the transducer draft depth, the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system are obtained;

[0012] Based on the constrained vertical coordinates and draft of the transducer in the Earth-centered Earth-fixed coordinate system, the coordinates of the seafloor transponder in the Earth-centered Earth-fixed coordinate system, and the sound velocity profile, the sound velocity calculation constraints are determined;

[0013] Combined with the constraint measurement equation of the measurement noise with depth constraint, the underwater acoustic two-way navigation function model is solved under the constraint of sound speed calculation to obtain the constrained acoustic navigation result.

[0014] The RTS smoothing method is used to perform backward smoothing on the constrained acoustic navigation result, and the result after backward smoothing is used as the final acoustic navigation result.

[0015] As an implementation method, the process of determining the optimal constraint information using the location precision factor is as follows:

[0016] The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

[0017] As an implementation method, the underwater acoustic two-way navigation function model is:

[0018]

[0019] In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received.

[0020] As an implementation, the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system are:

[0021]

[0022] In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; For the The draft of the transducer at the point of signal transmission or reception at the epoch; For the The constrained vertical coordinate of the transducer in the Earth-centered, Earth-fixed coordinate system at the point of signal transmission or reception at the epoch.

[0023] As an implementation method, the speed of sound calculation constraint is determined by a weighted average speed of sound method:

[0024]

[0025] In the formula, represents the weighted average speed of sound, represents the depth difference between the transducer and the transponder, represents the number of layers in the sound velocity profile, Indicates the speed of sound; Indicates the depth value, Subscript and They represent the first Layer and layer.

[0026] As an implementation method, the constraint measurement equation with added depth constraint measurement noise is:

[0027]

[0028] In the formula, is the predicted vertical coordinate error of the transducer in the Earth-centered Earth-fixed coordinate system; , For epoch to The vertical coordinates in the Earth-centered Earth-fixed coordinate system predicted by the transducer at the signal emission point during the epoch process; For the The constrained vertical coordinate of the transducer at the point of signal transmission or reception at the epoch in the Earth-centered Earth-fixed coordinate system; ; is the measurement noise of the depth constraint; It is The state estimate vector for each epoch; To adjust the state prediction vector of the transducer when the signal is transmitted; is a constant term; is the design matrix; It's noise.

[0029] A second aspect of the present invention provides an underwater acoustic two-way navigation system based on depth constraints and smoothing.

[0030] An underwater acoustic two-way navigation system based on depth constraint and smoothness, comprising:

[0031] The two-way navigation model construction module is based on the long baseline acoustic navigation principle and constructs an underwater acoustic two-way navigation function model in the Earth-centered Earth-fixed coordinate system;

[0032] The constrained vertical coordinate calculation module uses the position precision factor to determine the optimal constraint information, and then combines the transducer draft to obtain the constrained vertical coordinate of the transducer in the Earth-centered Earth-fixed coordinate system;

[0033] A sound speed calculation constraint determination module, which is used to determine the sound speed calculation constraints based on the constrained vertical coordinates and draft of the transducer in the earth-centered earth-fixed coordinate system, the coordinates of the seabed transponder in the earth-centered earth-fixed coordinate system, and the sound speed profile;

[0034] The acoustic navigation result constraint module is used to solve the underwater acoustic two-way navigation function model under the constraint of sound speed calculation by combining the constraint measurement equation of the measurement noise with the depth constraint, and obtain the constrained acoustic navigation result;

[0035] The RTS smoothing module uses the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation results, and uses the backward smoothed results as the final acoustic navigation results.

[0036] As an implementation mode, in the constrained vertical coordinate calculation module, the process of determining the optimal constraint information using the position precision factor is as follows:

[0037] The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

[0038] As an implementation manner, in the two-way navigation model building module, the underwater acoustic two-way navigation function model is: ;

[0039] In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received.

[0040] As an implementation mode, in the constrained vertical coordinate calculation module, the constrained vertical coordinate of the transducer in the Earth-centered Earth-fixed coordinate system is:

[0041]

[0042] In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; For the The draft of the transducer at the point of signal transmission or reception at the epoch; For the The constrained vertical coordinate of the transducer in the Earth-centered, Earth-fixed coordinate system at the point of signal transmission or reception at the epoch.

[0043] Compared with the prior art, the present invention has the following beneficial effects:

[0044] The present invention is based on the long baseline acoustic navigation principle and constructs an underwater acoustic two-way navigation function model, so that the model is consistent with the "inquiry-response-reception" mode of underwater acoustic navigation, reducing the navigation system error caused by the model; the present invention also uses the position precision factor to search for optimal constraint information, constrains the sound speed calculation and the vertical coordinates of the constraint transducer in the earth-centered earth-fixed coordinate system, expands the constraint information, improves the accuracy and reliability of the constraint information, reduces navigation errors, and realizes more precise navigation.

[0045] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0047] Figure 1 is a flow chart of an underwater acoustic two-way navigation method based on depth constraint and smoothing according to an embodiment of the present invention;

[0048] Figure 2 It is a schematic diagram of underwater acoustic two-way navigation based on depth constraint and smoothing according to an embodiment of the present invention. DETAILED DESCRIPTION

[0049] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0050] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0051] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0052] Terminology explanation:

[0053] Underwater acoustic navigation: A key technology for ocean exploration, it promotes a wide range of ocean development activities such as seabed earthquake risk assessment, ocean crust deformation, ocean measurement and offshore exploration.

[0054] LBL (Long Baseline) acoustic navigation system: It is a commonly used method in underwater acoustic navigation, with the characteristics of high navigation accuracy and wide operating range. The navigation system consists of a transducer installed on the bottom of the underwater vehicle and a transponder fixed to the seabed. Usually four or more transponders are used and they are precisely calibrated to ensure accurate positioning.

[0055] RTS, fixed interval smoothing algorithm, Rauch-Tung-Striebel;

[0056] LBL, long baseline;

[0057] PDOP, Position Dilution of Precision;

[0058] 3D-RMS, Three-Dimensional Root Mean Square, 3D-RMS.

[0059] Epoch: refers to the time when observation data is collected. The epoch is The data collection moment.

[0060] Figure 1 A flow chart of an underwater acoustic two-way navigation method based on depth constraint and smoothing according to an embodiment of the present invention is provided; Figure 2 The principle diagram of underwater acoustic two-way navigation based on depth constraint and smoothing according to an embodiment of the present invention is given. Figure 1 and Figure 2 , this embodiment provides an underwater acoustic two-way navigation method based on depth constraint and smoothness, including:

[0061] Step S101: Based on the long baseline acoustic navigation principle, an underwater acoustic two-way navigation function model is constructed in an Earth-centered Earth-fixed coordinate system;

[0062] Step S102: using the position precision factor to determine the optimal constraint information, and then combining the transducer draft to obtain the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system;

[0063] Step S103: determining the speed of sound calculation constraints based on the constrained vertical coordinates and draft of the transducer in the Earth-centered Earth-fixed coordinate system, the coordinates of the seabed transponder in the Earth-centered Earth-fixed coordinate system, and the speed of sound profile;

[0064] Step S104: combining the constrained measurement equation of the measurement noise with the depth constraint, solving the underwater acoustic two-way navigation function model under the sound speed calculation constraint, and obtaining the constrained acoustic navigation result;

[0065] Step S105: using the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation result, and using the backward smoothed result as the final acoustic navigation result.

[0066] This embodiment is based on the LBL acoustic navigation principle and constructs an underwater acoustic two-way navigation function model, so that the model is consistent with the "inquiry-response-reception" mode of underwater acoustic navigation, reducing the navigation system error caused by the model; it also uses the position precision factor to search for optimal constraint information, constrains the sound speed calculation and the vertical coordinates of the constraint transducer in the earth-centered earth-fixed coordinate system, expands the constraint information, improves the accuracy and reliability of the constraint information, reduces navigation errors, and achieves more accurate navigation.

[0067] In step S101, based on LBL acoustic navigation, the position of the underwater vehicle is determined by the distance intersection. In the "inquiry-response-receive" mode of underwater acoustic signal propagation, the position of the underwater vehicle is different when the acoustic signal is transmitted and received. The underwater acoustic two-way navigation function model is:

[0068] (1)

[0069] In the formula, the subscript For the The number of the epoch, subscript For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, It is the theoretical geometric distance between the transducer and the transponder when receiving the signal.

[0070] The theoretical geometric distance between the transducer and the transponder during signal transmission can be expressed as:

[0071] (2)

[0072] In the formula, are the coordinates of the transducer where the signal is emitted, is the coordinate of the transponder. In addition, the geometric distance at which the signal is received can be expressed by the same formula.

[0073] In the constant speed prediction model of the Kalman filter, the predicted position of the underwater vehicle when the signal is transmitted can be predicted as:

[0074] (3)

[0075] (4)

[0076] (5)

[0077] (6)

[0078] (7)

[0079] (8)

[0080] In the formula, It is from epoch to epoch process; is the state prediction vector, It is The state estimate vector for each epoch; is the predicted coordinate of the transducer when the signal is emitted, The estimated coordinates of the transducer at the time of signal emission; is the predicted velocity of the transducer when the signal is emitted, is the estimated velocity of the transducer at the time of signal emission; is the state transition matrix; is the process noise allocation matrix; is the time difference between adjacent epochs when the signal is transmitted; is the process noise of velocity.

[0081] In addition, according to the above formula, it can be inferred that the predicted position of the transducer when the signal is received is:

[0082] (9)

[0083] In the formula, is the predicted coordinate of the transducer when the signal is received, It is the time difference from signal transmission to reception.

[0084] The Taylor series expansion method is used to linearize formula (1) into:

[0085] (10)

[0086] (11)

[0087] (12)

[0088] (13)

[0089] (14)

[0090] (15)

[0091] (16)

[0092] In the formula, the subscript For the The number of the epoch, subscript For the transponder number; For the transducer The number of transponder signals received per epoch; is the innovation vector, which represents the difference between the predicted value and the measured value; To adjust the state prediction vector of the transducer when the signal is transmitted; is the Jacobian matrix, is the Jacobian matrix related to the transducer position when the signal is emitted, is the Jacobian matrix related to the transducer velocity during signal emission; is a comprehensive error term, including random errors and systematic errors caused by the speed of sound and propagation time of the acoustic signal; , … is the measured value of the round-trip distance between the transducer and the corresponding numbered transponder, and the corresponding expression is represented by formula (1); , … is the theoretical geometric distance between the transducer and the corresponding numbered transponder when the signal is transmitted; , … is the theoretical geometric distance between the transducer and the corresponding numbered transponder when receiving the signal; , … For the At the epoch, the ranging error caused by the sound velocity error of the corresponding numbered transponder; , … For the The ranging error caused by the delay error of the corresponding numbered transponder at the epoch; , … For the The measurement noise of the corresponding numbered transponder ranging at epoch.

[0093] In step S102, the process of determining the optimal constraint information using the location precision factor is as follows:

[0094] The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

[0095] The calculation formula of position precision dilution (PDOP) is expressed as:

[0096] (17)

[0097] (18)

[0098] (19)

[0099] In the formula, is the cofactor matrix, is the geometric matrix, For the transducer The number of transponder signals received per epoch, is the coordinate of the transducer in the Earth-centered Earth-fixed coordinate system when the signal is transmitted, are the coordinates of the transponder in the Earth-centered, Earth-fixed coordinate system.

[0100] In step 2, the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system are:

[0101]

[0102] In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; The draft depth of the transducer at the signal transmitting or receiving location; Constrains the vertical coordinate of the transducer where the signal is transmitted or received.

[0103] In step S103, the speed of sound calculation constraint is determined by the weighted average speed of sound method:

[0104] (20)

[0105] In the formula, represents the weighted average speed of sound, represents the depth difference between the transducer and the transponder, represents the number of layers in the sound velocity profile, Indicates the speed of sound; Indicates the depth value, Subscript and They represent the first Layer and layer.

[0106] In other embodiments, the constrained sound speed can also be determined by a ray tracing method, which depends on the constrained vertical coordinates and draft of the transducer, the coordinates of the seafloor transponder in the station center coordinate system, and the sound speed profile.

[0107] In step 4, the constraint measurement equation with added depth constraint measurement noise is:

[0108] (twenty one)

[0109] In the formula, is the predicted vertical coordinate error of the transducer in the Earth-centered Earth-fixed coordinate system; , For epoch to The vertical coordinates in the Earth-centered Earth-fixed coordinate system predicted by the transducer at the signal emission point during the epoch process; For the The constrained vertical coordinate of the transducer at the point of signal transmission or reception at the epoch in the Earth-centered Earth-fixed coordinate system; ; is the measurement noise of the depth constraint; It is The state estimate vector for each epoch; To adjust the state prediction vector of the transducer when the signal is transmitted; is a constant term; is the design matrix; It's noise.

[0110] In step S105, the extended Kalman filter is used to forward process the acoustic navigation data, calculate and store the state transfer matrix, state estimation vector, state estimation covariance matrix, state prediction vector and state prediction covariance matrix, and then the forward filtering output is backward smoothed using RTS, and the smoothed output is used as the result.

[0111] The present invention proposes an underwater acoustic two-way navigation algorithm that combines depth constraints and fixed interval (Rauch-Tung-Striebel, RTS) smoothing. Specifically, the method establishes a detailed function model of underwater acoustic two-way navigation, uses the Position Dilution of Precision (PDOP) to find the optimal constraint information, integrates the constraint information into the sound speed calculation, constrains the vertical coordinates of the transducer in the measurement equation, and finally uses the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation results. High-quality experimental data show that the proposed method is superior to traditional methods, and the three-dimensional root mean square (3D-RMS) value of the navigation error is reduced by an average of 4.44%. For low-quality data, the 3D-RMS value of the proposed method is reduced by an average of 85.47% compared with the traditional method. These results show that compared with the traditional method, the proposed method exhibits higher accuracy, precision and consistency, especially in the vertical direction. The improvement is most significant.

[0112] In one or more embodiments, an underwater acoustic two-way navigation system based on depth constraint and smoothing includes:

[0113] The two-way navigation model construction module is based on the long baseline acoustic navigation principle and constructs an underwater acoustic two-way navigation function model in the Earth-centered Earth-fixed coordinate system;

[0114] The constrained vertical coordinate calculation module uses the position precision factor to determine the optimal constraint information, and then combines the transducer draft to obtain the constrained vertical coordinate of the transducer in the Earth-centered Earth-fixed coordinate system;

[0115] A sound speed calculation constraint determination module, which is used to determine the sound speed calculation constraints based on the constrained vertical coordinates and draft of the transducer in the earth-centered earth-fixed coordinate system, the coordinates of the seabed transponder in the earth-centered earth-fixed coordinate system, and the sound speed profile;

[0116] The acoustic navigation result constraint module is used to solve the underwater acoustic two-way navigation function model under the constraint of sound speed calculation by combining the constraint measurement equation of the measurement noise with the depth constraint, and obtain the constrained acoustic navigation result;

[0117] The RTS smoothing module uses the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation results, and uses the backward smoothed results as the final acoustic navigation results.

[0118] In a specific implementation process, in the constraint vertical coordinate calculation module, the process of determining the optimal constraint information using the position precision factor is as follows:

[0119] The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

[0120] In the specific implementation process, in the two-way navigation model construction module, the underwater acoustic two-way navigation function model is:

[0121]

[0122] In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received.

[0123] In the specific implementation process, in the constrained vertical coordinate calculation module, the constrained vertical coordinate of the transducer in the Earth-centered Earth-fixed coordinate system is: ;

[0124] In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; For the The draft of the transducer at the point of signal transmission or reception at the epoch; For the The constrained vertical coordinate of the transducer in the Earth-centered, Earth-fixed coordinate system at the point of signal transmission or reception at the epoch.

[0125] It should be noted here that the specific implementation process of each module in the underwater acoustic two-way navigation system based on depth constraint and smoothness is the same as the specific implementation process of each step in the above-mentioned underwater acoustic two-way navigation method based on depth constraint and smoothness, and will not be described in detail here.

[0126] In one or more embodiments, a computer-readable storage medium is provided, on which a computer program is stored. When the program is executed by a processor, the steps in the underwater acoustic two-way navigation method based on depth constraint and smoothing are implemented as described above.

[0127] In other embodiments, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the underwater acoustic two-way navigation method based on depth constraints and smoothing are implemented as described above.

[0128] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0129] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0130] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the embodiments of the above-mentioned methods. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0131] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An underwater acoustic two-way navigation method based on depth constraint and smoothness, characterized in that: include: Based on the long baseline acoustic navigation principle, an underwater acoustic two-way navigation function model is constructed in the Earth-centered Earth-fixed coordinate system. The underwater acoustic two-way navigation function model is: （1） In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received; The theoretical geometric distance between the transducer and the transponder when the signal is transmitted is expressed as: （2） In the formula, are the coordinates of the transducer where the signal is emitted, are the coordinates of the transponder; the geometric distance at which the signal is received is expressed by the same formula; In the constant speed prediction model of the Kalman filter, the predicted position of the underwater vehicle when the signal is transmitted is predicted as: （3） （4） （5） （6） （7） （8） In the formula, It is from epoch to epoch process; is the state prediction vector, It is The state estimate vector for each epoch; is the predicted coordinate of the transducer when the signal is emitted, The estimated coordinates of the transducer at the time of signal emission; is the predicted velocity of the transducer when the signal is emitted, is the estimated velocity of the transducer at the time of signal emission; is the state transition matrix; is the process noise allocation matrix; is the time difference between adjacent epochs when the signal is transmitted; is the process noise of velocity; According to the above formula, the predicted position of the transducer when receiving the signal is inferred as: （9） In the formula, is the predicted coordinate of the transducer when the signal is received, is the time difference from signal transmission to reception; The Taylor series expansion method is used to linearize formula (1) into: （10） （11） （12） （13） （14） （15） （16） In the formula, the subscript For the The number of the epoch, subscript For the transponder number; For the transducer The number of transponder signals received per epoch; is the innovation vector, which represents the difference between the predicted value and the measured value; To adjust the state prediction vector of the transducer when the signal is transmitted; is the Jacobian matrix, is the Jacobian matrix related to the transducer position when the signal is emitted, is the Jacobian matrix related to the transducer velocity during signal emission; is a comprehensive error term, including random errors and systematic errors caused by the speed of sound and propagation time of the acoustic signal; The optimal constraint information is determined by using the position precision factor, and then combined with the transducer draft, the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system are obtained; based on the constrained vertical coordinates and draft of the transducer in the Earth-centered Earth-fixed coordinate system, the coordinates of the seabed transponder in the Earth-centered Earth-fixed coordinate system, and the sound speed profile, the sound speed calculation constraints are determined; Combined with the constraint measurement equation of the measurement noise with depth constraint, the underwater acoustic two-way navigation function model is solved under the constraint of sound speed calculation to obtain the constrained acoustic navigation result. The constraint measurement equation with added depth constraint measurement noise is: In the formula, is the predicted vertical coordinate error of the transducer in the Earth-centered Earth-fixed coordinate system; , For epoch to The vertical coordinates in the Earth-centered Earth-fixed coordinate system predicted by the transducer at the signal emission point during the epoch process; For the The constrained vertical coordinate of the transducer at the point of signal transmission or reception at the epoch in the Earth-centered Earth-fixed coordinate system; ; is the measurement noise of the depth constraint; It is The state estimate vector for each epoch; To adjust the state prediction vector of the transducer when the signal is transmitted; is a constant term; is the design matrix; It is noise; The RTS smoothing method is used to perform backward smoothing on the constrained acoustic navigation result, and the result after backward smoothing is used as the final acoustic navigation result.

2. The underwater acoustic two-way navigation method based on depth constraint and smoothness as claimed in claim 1, characterized in that: The process of determining the optimal constraint information using the location precision factor is as follows: The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

3. The underwater acoustic two-way navigation method based on depth constraint and smoothness as claimed in claim 1, characterized in that: The constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system are: In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; For the The draft of the transducer at the point of signal transmission or reception at the epoch; For the The constrained vertical coordinate of the transducer in the Earth-centered, Earth-fixed coordinate system at the point of signal transmission or reception at the epoch.

4. The underwater acoustic two-way navigation method based on depth constraint and smoothness as claimed in claim 1, characterized in that: The sound speed calculation constraints are determined by the weighted average sound speed method: In the formula, represents the weighted average speed of sound, represents the depth difference between the transducer and the transponder, represents the number of layers in the sound velocity profile, Indicates the speed of sound; Indicates the depth value, Subscript and They represent the first Layer and layer.

5. An underwater acoustic two-way navigation system based on depth constraint and smoothness, characterized in that: include: The two-way navigation model construction module is based on the long baseline acoustic navigation principle and constructs an underwater acoustic two-way navigation function model in the Earth-centered Earth-fixed coordinate system; The underwater acoustic two-way navigation function model is: （1） In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received; The theoretical geometric distance between the transducer and the transponder when the signal is transmitted is expressed as: （2） In the formula, are the coordinates of the transducer where the signal is emitted, are the coordinates of the transponder; the geometric distance at which the signal is received is expressed by the same formula; In the constant speed prediction model of the Kalman filter, the predicted position of the underwater vehicle when the signal is transmitted is predicted as: （3） （4） （5） （6） （7） （8） In the formula, It is from epoch to epoch process; is the state prediction vector, It is The state estimate vector for each epoch; is the predicted coordinate of the transducer when the signal is emitted, The estimated coordinates of the transducer at the time of signal emission; is the predicted velocity of the transducer when the signal is emitted, is the estimated velocity of the transducer at the time of signal emission; is the state transition matrix; is the process noise allocation matrix; is the time difference between adjacent epochs when the signal is transmitted; is the process noise of velocity; According to the above formula, the predicted position of the transducer when receiving the signal is inferred as: （9） In the formula, is the predicted coordinate of the transducer when the signal is received, is the time difference from signal transmission to reception; The Taylor series expansion method is used to linearize formula (1) into: （10） （11） （12） （13） （14） （15） （16） In the formula, the subscript For the The number of the epoch, subscript For the transponder number; For the transducer The number of transponder signals received per epoch; is the innovation vector, which represents the difference between the predicted value and the measured value; To adjust the state prediction vector of the transducer when the signal is transmitted; is the Jacobian matrix, is the Jacobian matrix related to the transducer position when the signal is emitted, is the Jacobian matrix related to the transducer velocity during signal emission; is a comprehensive error term, including random errors and systematic errors caused by the speed of sound and propagation time of the acoustic signal; a constrained vertical coordinate calculation module, which uses the position precision factor to determine the optimal constraint information, and then combines the transducer draft to obtain the constrained vertical coordinates of the transducer in the Earth-centered Earth-fixed coordinate system; A sound speed calculation constraint determination module, which is used to determine the sound speed calculation constraints based on the constrained vertical coordinates and draft of the transducer in the earth-centered earth-fixed coordinate system, the coordinates of the seabed transponder in the earth-centered earth-fixed coordinate system, and the sound speed profile; The acoustic navigation result constraint module is used to solve the underwater acoustic two-way navigation function model under the constraint of sound speed calculation by combining the constraint measurement equation of the measurement noise with the depth constraint, and obtain the constrained acoustic navigation result; The constraint measurement equation with added depth constraint measurement noise is: In the formula, is the predicted vertical coordinate error of the transducer in the Earth-centered Earth-fixed coordinate system; , For epoch to The vertical coordinates in the Earth-centered Earth-fixed coordinate system predicted by the transducer at the signal emission point during the epoch process; For the The constrained vertical coordinate of the transducer at the point of signal transmission or reception at the epoch in the Earth-centered Earth-fixed coordinate system; ; is the measurement noise of the depth constraint; It is The state estimate vector for each epoch; To adjust the state prediction vector of the transducer when the signal is transmitted; is a constant term; is the design matrix; It is noise; The RTS smoothing module uses the RTS smoothing method to perform backward smoothing on the constrained acoustic navigation results, and uses the backward smoothed results as the final acoustic navigation results.

6. The underwater acoustic two-way navigation system based on depth constraint and smoothness as claimed in claim 5, characterized in that: In the constrained vertical coordinate calculation module, the process of determining the optimal constraint information using the position precision factor is as follows: The position precision factor of the underwater vehicle at the current epoch is calculated and compared with the previous optimal position precision factor. If the current position precision factor is lower than the previous optimal position precision factor, the optimal constraint information is replaced by the current constraint information including the vertical coordinates of the signal transmission point transducer and the draft depth; otherwise, the optimal constraint information is determined.

7. The underwater acoustic two-way navigation system based on depth constraint and smoothness as claimed in claim 5, characterized in that: In the two-way navigation model building module, the underwater acoustic two-way navigation function model is: In the formula, For the The number of the epoch, For the transponder number; is the coordinate of the transducer installed at the bottom of the vehicle when the signal is emitted, is the coordinate position of the transducer when receiving the signal; are the coordinates of the transponder fixed on the seabed, which are known values; is the ranging error caused by the sound speed error, is the ranging error caused by the delay error, is the measurement noise of ranging; is the measurement of the round-trip distance between the transducer and the transponder; is the theoretical geometric distance between the transducer and the transponder when the signal is transmitted, is the theoretical geometric distance when the signal is received.

8. The underwater acoustic two-way navigation system based on depth constraint and smoothness as claimed in claim 5, characterized in that: In the constrained vertical coordinate calculation module, the constrained vertical coordinate of the transducer in the Earth-centered Earth-fixed coordinate system is: In the formula, and are the ordinate and draft of the transducer in the optimal constraint information respectively; For the The draft of the transducer at the point of signal transmission or reception at the epoch; For the The constrained vertical coordinate of the transducer in the Earth-centered, Earth-fixed coordinate system at the point of signal transmission or reception at the epoch.

Images