Underwater vehicle positioning method and device, USBL base station, storage medium and system

Abstract

The invention discloses an underwater vehicle positioning method and device, a USBL base station, a storage medium and a system. The method comprises the following steps: receiving a two-way sound signal and at least two one-way sound signals sent by a water surface beacon, and receiving at least two displacement vectors output by an inertial navigation system; measuring the arrival time of each one-way sound signal and a corresponding first azimuth angle, measuring a second azimuth angle corresponding to the two-way sound signal according to the two-way sound signal, and determining the distance between the water surface beacon and the underwater vehicle; determining a first position and a course error of the underwater vehicle according to the at least two single-pass sound signals, the at least two displacement vectors, the first azimuth angle and the arrival time; determining a second position of the underwater vehicle according to the two-way sound signal, the second azimuth angle, the distance and the course error; and performing combined positioning according to the first position and the second position to obtain a positioning result of the underwater vehicle. In this way, the positioning accuracy can be improved.

Description

Technical Field

[0001] The present application relates to the field of autonomous underwater vehicle (AUV) positioning technology, and in particular to a positioning method, device, USBL base station, system, computer-readable storage medium and system for underwater vehicles. Background Art

[0002] Due to the particularity of the underwater environment, high-precision radio navigation cannot be used. Sound waves that can propagate over long distances in the ocean environment have become the carrier of ranging and direction-finding signals that underwater positioning and navigation can rely on. According to the different error transmission methods, underwater positioning technology is divided into two categories: navigation with unbounded errors, such as inertial navigation methods; and navigation with bounded errors, such as acoustic navigation methods.

[0003] As an autonomous method that does not rely on external information, inertial navigation has the advantages of strong anti-interference ability, short update time, high update rate and high position accuracy. However, the drift error inherent in inertial sensors cannot be avoided, and the output of a pure inertial navigation system will gradually deviate from the true value over time. Therefore, in the navigation system of underwater vehicles, in order to effectively control navigation errors, it is crucial to use heading calibration and external auxiliary positioning methods. These methods not only help correct sensor drift, but also improve the robustness and accuracy of the navigation system, ensuring that underwater vehicles can reliably achieve precise control of position and direction when performing tasks in underwater environments.

[0004] Acoustic navigation methods include long baseline (LBL), short baseline (SBL) and ultra short baseline (USBL), as well as a single beacon positioning method designed to further reduce system complexity. The single beacon positioning method has received widespread attention in recent years due to its convenience, high efficiency and low cost. Single beacon positioning methods include active positioning and passive positioning. Active positioning requires the use of high-power clocks to maintain the accuracy of ranging, which is not suitable for underwater vehicles that sail underwater for a long time. Single beacon passive positioning is divided into three types: methods based on arrival time difference, methods based on arrival angle, and methods based on a combination of the two. The USBL array is installed on the underwater vehicle platform to form an inverse ultra short baseline positioning system, which can make full use of the arrival time difference and arrival angle to complete positioning.

[0005] The single beacon passive positioning method based on the motion vector output by the underwater vehicle's inertial navigation, the single beacon periodic signal, and the arrival time difference and arrival angle information obtained by the USBL array carried by the underwater vehicle can complete the high-precision positioning of the underwater vehicle with limited basic equipment. It plays an important positioning and navigation capability in the underwater vehicle cluster communication and positioning network and when the underwater vehicle performs long-term underwater missions alone, providing a guarantee for the high-precision execution of the mission.

[0006] Judging from the development process of hydroacoustic positioning technology in underwater vehicle clusters at home and abroad, its development trend mainly revolves around the USBL system that can be carried on underwater vehicles, with low power consumption, high precision and concealment as the main characteristics of development.

[0007] Most of the existing single-beacon positioning methods assume that the underwater vehicle's short-term moving vector is relatively accurate, ignoring the error introduced in the conversion process of the navigation coordinate system positioning result to the geodetic coordinate system caused by the existence of the underwater vehicle's heading error, and ignoring the error introduced in the conversion process of the navigation coordinate system positioning result to the geodetic coordinate system, as an important data input item of the USBL system, which significantly affects the direction-finding accuracy of the USBL; currently, most single-beacon positioning systems are based on time-synchronized one-way time delay ranging (OWTT) or two-way time delay ranging (TWTT), among which OWTT requires a high-precision clock and TWTT will introduce a large ranging error; currently, most single-beacon positioning systems are based on fixed beacons, which require the beacon position to be calibrated in advance, which has a high time cost. Summary of the invention

[0008] The underwater vehicle positioning method, device, USBL base station, storage medium and system provided in the present application can effectively solve the problem of large movement vector error caused by the underwater vehicle heading error in single beacon passive positioning, and improve positioning accuracy.

[0009] In a first aspect, the present application provides a method for positioning an underwater vehicle, the positioning method comprising: receiving a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receiving at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain latitude and longitude information, depth information and communication position identification information; measuring the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measuring the second azimuth corresponding to the two-way acoustic signal based on the two-way acoustic signal, and determining the distance between the surface beacon and the underwater vehicle; determining a first position and a heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, the first azimuth and the arrival time; determining a second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error; performing joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle.

[0010] A further technical solution is to determine a first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, a first azimuth angle and an arrival time, including: obtaining an installation error corresponding to the underwater vehicle; determining the first position and heading error of the underwater vehicle based on the installation error, at least two one-way acoustic signals, at least two displacement vectors, a first azimuth angle and an arrival time.

[0011] A further technical solution is to determine the first position and heading error of the underwater vehicle based on the installation error, at least two one-way sound signals, at least two displacement vectors, the first azimuth angle and the arrival time, including: substituting the installation error, at least two one-way sound signals, at least two displacement vectors, the first azimuth angle and the arrival time into a preset equation; solving the preset equation using the weighted least squares method to obtain the first position and heading error of the underwater vehicle.

[0012] A further technical solution is to preset the equation as follows: ; in, ; ; , represents the weight matrix, represents the error vector, Indicates heading error; Indicates the location of the beacon during the nth positioning. Indicates During the first positioning, the horizontal distance between the beacon and the underwater vehicle is Indicates The first positioning and The difference in the horizontal distance between the beacon and the underwater vehicle during the second positioning is: represents the angle between the beacon and the underwater vehicle in the x direction, represents the angle between the beacon and the underwater vehicle in the y direction, represents the displacement change of the surface beacon in the x direction, represents the displacement change of the surface beacon in the y direction, represents the displacement change of the underwater vehicle in the x direction, represents the displacement change of the underwater vehicle in the y direction, and Indicates the position of the underwater vehicle at the nth positioning.

[0013] A further technical solution is to solve the preset equation using the weighted least square method to obtain the first position and heading error of the underwater vehicle, including: solving the preset equation using the weighted least square method to obtain ;in, ; Residual, Represents the positioning standard deviation.

[0014] A further technical solution is to determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth, the distance and the heading error, including: using the following formula to calculate the second position of the underwater vehicle: ; ; in, are the latitude, longitude and depth of the beacon, is the latitude, longitude and depth of the origin of the navigation coordinate system, express The true value of Indicates from arrive From the change in position, is the Euler rotation matrix, which is used to transform the carrier coordinate system into the navigation coordinate system. is the Euler rotation matrix, which is used to transform the base matrix coordinate system to the carrier coordinate system. is the beacon in the base array coordinate system axis, Axis and axis, The matrix coordinate system is relative to the carrier coordinate system. axis, Axis and Shaft installation error.

[0015] A further technical solution is to perform joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle, including: calculating a first positioning standard deviation based on the first position, and calculating a second positioning standard deviation based on the second position; using the first positioning standard deviation and the second positioning standard deviation to obtain a first weight and a second weight; weighting the first position using the first weight and weighting the second position using the second weight, thereby obtaining a positioning result of the underwater vehicle.

[0016] A further technical solution is to obtain the first weight and the second weight using the first positioning standard deviation and the second positioning standard deviation, including: obtaining the first weight using the following formula: ;in, represents the first positioning standard deviation, represents the second positioning standard deviation; The second weight is obtained using the following formula: .

[0017] In a second aspect, the present application provides a positioning device for an underwater vehicle, the positioning device comprising: a receiving module, used to receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and to receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal include latitude and longitude information, depth information and communication position identification information; a measuring module, used to measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and to measure the second azimuth corresponding to the two-way acoustic signal according to the two-way acoustic signal, and to determine the distance between the surface beacon and the underwater vehicle; a first determination module, used to determine a first position and a heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, a first azimuth and an arrival time; a second determination module, used to determine a second position of the underwater vehicle according to the two-way acoustic signal, a second azimuth, a distance and a heading error; a joint positioning module, used to perform joint positioning according to the first position and the second position to obtain a positioning result of the underwater vehicle.

[0018] In a third aspect, the present application provides a USBL base station, which is used to implement the positioning method provided in the first aspect.

[0019] In a fourth aspect, the present application provides a computer-readable storage medium, which is used to store a computer program. When the computer program is executed by a processor, it is used to implement the positioning method provided in the first aspect.

[0020] In a fifth aspect, the present application provides a positioning system for an underwater vehicle, which includes a surface beacon, a USBL base station and an inertial navigation system, and the surface beacon and the USBL base station communicate through a water medium; wherein the inertial navigation system and the USBL base station are arranged on the underwater vehicle to be positioned, and the USBL base station is used to implement the positioning method provided in the first aspect.

[0021] The beneficial effects of the present application are as follows: Different from the prior art, the underwater vehicle positioning method, device, USBL base station, storage medium and system provided by the present application receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain latitude and longitude information, depth information and communication position identification information; measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measure the second azimuth corresponding to the two-way acoustic signal based on the two-way acoustic signal, and determine the distance between the surface beacon and the underwater vehicle; determine the first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, the first azimuth and the arrival time; determine the second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error; perform joint positioning based on the first position and the second position to obtain the positioning result of the underwater vehicle. That is, during the positioning process, the underwater vehicle heading error is calibrated at the same time, so that two positioning results (the first position and the second position) can be obtained in one positioning, and the joint positioning result can be output. This can effectively solve the problem of large moving vector error caused by the underwater vehicle heading error in single beacon passive positioning and improve positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. Among them: Figure 1 It is a flow chart of an embodiment of a method for positioning an underwater vehicle provided by the present application; Figure 2 It is the MAC protocol timeline provided by this application; Figure 3 yes Figure 1 A schematic diagram of a flow chart of an embodiment of step 13; Figure 4 yes Figure 1 A schematic diagram of a flow chart of an embodiment of step 15; Figure 5 This is a schematic diagram of an application scenario of a positioning method for an underwater vehicle provided by the present application; Figure 6 It is a structural schematic diagram of an embodiment of a positioning system for an underwater vehicle provided by the present application; Figure 7 It is a structural schematic diagram of an embodiment of a computer-readable storage medium provided by the present application; Figure 8It is a structural schematic diagram of an embodiment of a positioning device for an underwater vehicle provided by the present application; Fig. 9 This is a comparison chart of the positioning root mean square error provided by this application; Fig.10 This is a comparison chart of positioning deviation norms provided by this application. DETAILED DESCRIPTION

[0023] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. It will be understood that the specific embodiments described herein are only used to explain the present application, rather than to limit the present application. It should also be noted that, for ease of description, only some but not all structures related to the present application are shown in the drawings. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0024] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0025] See also Figure 1 , Figure 1 1 is a flow chart of an embodiment of a method for positioning an underwater vehicle provided by the present application. The positioning method comprises: Step 11: Receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal include latitude and longitude information, depth information, and communication bit identification information.

[0026] In some embodiments, the surface beacon can be a surface mobile GPS beacon, which uses a single-point mooring loose mooring buoy method to send a positioning signal according to a certain MAC protocol cycle. The optional modulation modes of the beacon are: PN code modulation, FM (Frequency modulation, frequency modulation signal), SS (Spread spectrum, spread spectrum signal). The modulation mode can be adjusted according to the application scenario and the required positioning accuracy and anti-interference ability. The beacon can select different transmission powers according to the communication distance and environmental noise to increase the probability of the positioning signal being successfully received by the positioning target.

[0027] Among them, the surface mobile GPS beacon acts as a surface gateway node while sending positioning signals. In a non-covert communication scenario, the beacon can broadcast positioning signals in one positioning cycle and complete data exchange with an underwater vehicle. Considering the half-duplex working mode of underwater acoustic communication, the acoustic communication machine cannot receive signals while transmitting signals. Therefore, in the process of network information interaction, time slots need to be divided for the positioning process and the communication process. Figure 2 The figure shows the MAC protocol timeline of this system. In one round of communication positioning process, the beacon node broadcasts a trigger packet (TR), and the data in the packet includes the beacon's longitude and latitude, depth, service bit flag, and distance. After receiving the trigger packet, the underwater vehicle to be positioned determines whether the service bit flag is 0. If it is 0, it immediately returns an acknowledgment packet (ACK) to occupy the communication time slot. After receiving the acknowledgment packet, the beacon uses the round-trip data packet (DATA), as well as the depth information, incident angle information, and processing time information in the packet to measure the distance through ray tracing. When the beacon and the underwater vehicle node need to exchange data, they exchange data during the data exchange period. After the positioning period and the data exchange period, the next round of positioning communication process begins, and the ranging information of the underwater vehicle being measured in the previous round is notified in the trigger packet.

[0028] In this system, the underwater acoustic communication device is integrated with communication and positioning. The data frame includes a trigger header (HFM), a preamble, a PN positioning sequence and multiple data blocks. Different protection intervals are added between different blocks. Each data block can store user data. TR packets and ACK packets use one data block to exchange data, and the processing delay is ms; DATA packets occupy different data blocks according to the data size. Here, 8 data blocks are occupied by default, and the processing delay is ms; set the effective distance of the positioning process to m, so the single propagation delay of the signal is ms, the total cost of 4 transmissions ms, where is the equivalent speed of sound. Therefore, the time for one round of interaction is ms.

[0029] In some embodiments, the underwater vehicle may be equipped with a USBL base station, through which the USBL base station communicates with the surface beacon. The USBL base station continuously receives the positioning signal sent by the beacon during the navigation of the underwater vehicle. The USBL base station includes a software system and a hardware system. The software system is equipped with the Sealinx network protocol stack, which includes four layers of network: application layer, transport layer, network layer and MAC layer. At the application layer, the protocol stack is responsible for establishing a connection with the underwater vehicle main control through UDP, receiving the inertial navigation data (output by the inertial navigation system) forwarded by the underwater vehicle in real time, and providing the underwater vehicle with positioning solution results. At the same time, the protocol stack performs packetization and grouping of data packets at the transport layer. At the MAC layer, the underwater vehicle and the GPS beacon are equipped with a set of MAC protocols to realize the necessary communication services. The USBL base station hardware system includes a chip, an acoustic transducer and a pressure-resistant shell of the carrying equipment.

[0030] Among them, a positioning unit is deployed on the chip of the USBL base station. The positioning unit will calculate its own position information based on the positioning signal received by the USBL base station and the movement vector output by the underwater vehicle inertial navigation, and transmit the position information back to the main control system of the underwater vehicle.

[0031] In some embodiments, the surface beacon is sent at fixed time intervals. The underwater vehicle sends an acoustic signal, which contains its latitude and longitude information, depth information and communication position identification information. The underwater vehicle receives the acoustic signal through the USBL array it carries. The array measures the arrival time of the one-way acoustic signal and obtains the azimuth through the plane wave method to determine the direction of the signal source.

[0032] In some embodiments, a corresponding data storage module is provided. The data storage module is used for the underwater vehicle to continuously receive the positioning signal transmitted by the beacon during navigation, and record the latitude and longitude information and depth information of the beacon, the signal arrival time and the azimuth information of the beacon, and the position information and attitude information given by the inertial navigation system of the underwater vehicle at the time of receiving the positioning signal. Although the larger the amount of data, the higher the accuracy of single beacon positioning, the movement vector introduced by the inertial navigation system gradually increases due to the accumulation of time, which affects the final positioning accuracy. Therefore, a time window of the data is added. The time window is the product of the sampling point and the sampling interval, where the sampling interval is the beacon sending period. , the number of sampling points is set to .

[0033] Step 12: Measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measure the second azimuth corresponding to the two-way acoustic signal according to the two-way acoustic signal, and determine the distance between the surface beacon and the underwater vehicle.

[0034] The underwater vehicle continuously monitors these signals and uses the arrival time and azimuth data from each measurement to perform a dead reckoning.

[0035] The first azimuth is used to indicate the relative position relationship between the surface beacon and the underwater vehicle. The second azimuth is also used to indicate the relative position relationship between the surface beacon and the underwater vehicle. The only difference is that the data involved in the calculation between the two are different.

[0036] Step 13: Determine a first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, a first azimuth angle, and an arrival time.

[0037] The first position is used to represent the positioning result of the underwater vehicle at the current moment.

[0038] The underwater vehicle uses a single beacon positioning method, combined with the sound speed curve, the previous M arrival times and direction finding results, and the motion vector output by the previous M underwater vehicle inertial navigation, to obtain the underwater vehicle's current positioning result (first position) and heading error.

[0039] In some embodiments, see Figure 3 , step 13 may have the following process: Step 131: Obtain the installation error corresponding to the underwater vehicle.

[0040] Step 132: Determine a first position and heading error of the underwater vehicle based on the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time.

[0041] In some embodiments, the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle and the arrival time may be substituted into a preset equation, and then the preset equation may be solved using a weighted least square method to obtain the first position and heading error of the underwater vehicle.

[0042] The preset equation is as follows: ; in, ; ; , represents the weight matrix, represents the error vector, represents the heading error, Indicates heading error; Indicates the location of the beacon during the nth positioning. Indicates During the first positioning, the horizontal distance between the beacon and the underwater vehicle is Indicates The first positioning and The difference in the horizontal distance between the beacon and the underwater vehicle during the second positioning is: represents the angle between the beacon and the underwater vehicle in the x direction, represents the angle between the beacon and the underwater vehicle in the y direction, represents the displacement change of the surface beacon in the x direction, represents the displacement change of the surface beacon in the y direction, represents the displacement change of the underwater vehicle in the x direction, represents the displacement change of the underwater vehicle in the y direction, and Indicates the position of the underwater vehicle at the nth positioning.

[0043] The weighted least squares method is used to solve the preset equation and obtain ;in, ; Residual, Represents the positioning standard deviation.

[0044] In some embodiments, the above preset equation is obtained by: The motion state of the underwater vehicle is the observation quantity in the carrier coordinate system. Its positioning and navigation system uses the earth coordinate system. Therefore, it is necessary to transform the motion state in the carrier coordinate system to the earth coordinate system through the coordinate rotation matrix. The coordinate rotation matrix is ​​composed of attitude angles. When there is an error in the attitude angle of the carrier, , that is, the rotation matrix with error is used during navigation , sailing After time, the estimated position of the underwater vehicle in the geodetic coordinate system is: (1) in, represents the initial position of the inertial navigation position calculation, represents the speed of the vehicle in the carrier coordinate system, Indicates speed The duration of the position error can be expressed as From formula (1), we can get (2) in, Indicates from arrive From the change in position. In common inertial navigation equipment, the heading angle and pitch angle , Roll Angle The definition is different, clockwise is positive, and the rotation matrix is ​​as follows: (3) Due to the existence of heading angle error, the expressions of equations (2) and (3) have changed. Let the heading angle error (heading error) be , then formula (2) can be rewritten as: (4) Formula (3) can be rewritten as: (5) in, Represents the true value of the rotation matrix.

[0045] To simplify the expression, assume: (6) The formula for USBL positioning is: (7) In the formula, is the Euler rotation matrix, which transforms the carrier coordinate system into the navigation coordinate system; is the Euler rotation matrix, which transforms the matrix coordinate system to the carrier coordinate system, where , and are the matrix coordinate systems axis, axis, Axis relative to the carrier coordinate system axis, axis, The installation angle error of the shaft; is the latitude, longitude and depth of the beacon; is the latitude, longitude and depth of the origin of the navigation coordinate system; is the beacon in the base array coordinate system axis, Axis and axis; The matrix coordinate system is relative to the carrier coordinate system. axis, Axis and Installation displacement error of the shaft (installation error).

[0046] Here the angle of the beacon in the navigation coordinate system is defined as: (8) Where R is the slope distance, , , , refers to The latitude, longitude, and depth of the corresponding beacon.

[0047] Substituting equations (4), (5) and (6) into equation (7), we can obtain: (9) set up: (10) Formula (10) can be simplified as: (11) As shown in equation (11), in a straight track, there is a direct relationship between the heading error and the positioning process, but this relationship does not apply in a circular track. Considering that the heading error of an underwater vehicle is usually small, a small angle approximation can be made, , ,but: (12) In the ultra-short baseline system with single beacon positioning, the distance between the beacon and the underwater vehicle can be obtained. and , represents the angle between the beacon and the underwater vehicle in the x direction, represents the angle between the beacon and the underwater vehicle in the y direction, as shown in equation (8). The relative position relationship between the beacon and the underwater vehicle can be expressed by the following equation: (13) in, and Respectively represent During the secondary positioning, the position information of the beacon and the underwater vehicle in the geodetic coordinate system. Indicates During the first positioning, the horizontal distance between the beacon and the underwater vehicle is Indicates The first positioning and The distance difference between the horizontal distance between the beacon and the underwater vehicle during the second positioning. Substituting equations (8), (11) and (12) into (13), we get: (14) Substitute the data into equation (14) and replace the absolute position with the relative position. and ,have to: (15) In the formula , , .

[0048] Using the weighted least squares method to solve equation (15), we get: (18) The weight of a reference in the solution is defined as: (19) In the formula, The median absolute deviation is used to improve the stability of the weighted least squares method by taking advantage of its insensitivity to outliers: (20) The position of the underwater vehicle is converted using the beacon position to obtain the positioning result of the first position: (twenty one) Step 14: Determine a second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error.

[0049] The second position is used to represent the positioning result of the underwater vehicle at the current moment. Compared with the first position, the data involved in the calculation are different.

[0050] When the underwater vehicle can communicate directly with the surface beacon as a communication node, the propagation delay is measured through two-way signal to obtain the distance between the beacon and the USBL array. The USBL positioning system is used to complete a single underwater vehicle positioning and obtain the positioning result (second position).

[0051] In some embodiments, the second position of the underwater vehicle is calculated using the following formula: .

[0052] ; in, are the latitude, longitude and depth of the beacon, is the latitude, longitude and depth of the origin of the navigation coordinate system, express The true value of Indicates from arrive From the change in position, is the Euler rotation matrix, which is used to transform the carrier coordinate system into the navigation coordinate system. is the Euler rotation matrix, which is used to transform the base matrix coordinate system to the carrier coordinate system. is the beacon in the base array coordinate system axis, Axis and axis, The matrix coordinate system is relative to the carrier coordinate system. axis, Axis and Shaft installation error.

[0053] When there is a round-trip communication process between the underwater vehicle and the surface beacon, the USBL can be used to obtain the positioning result of the underwater vehicle. After the surface beacon broadcasts the trigger packet, the underwater vehicle receives the trigger packet and records the current direction-finding information, and packages the processing delay into a confirmation packet and returns it to the surface beacon. The beacon receives the signal well, calculates the round-trip delay, uses the sound line final algorithm to compensate for the influence of the speed of sound, and packages the slant distance information into a data packet and sends it back to the underwater vehicle. After receiving the data packet, the underwater vehicle obtains the slant distance information and the longitude and latitude of the mobile beacon to obtain its own USBL positioning. Therefore, the positioning result of the second position of the underwater vehicle can be expressed by the following formula: (twenty two) Step 15: Perform joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle.

[0054] In some embodiments, see Figure 4 , step 15 may have the following process: Step 151: Calculate a first positioning standard deviation according to the first position, and calculate a second positioning standard deviation according to the second position.

[0055] In some embodiments, coordinate values ​​are obtained by measuring at least twice at the first position, and then the average value of the coordinate values ​​is calculated. Then, the variance is calculated based on the multiple coordinate values ​​and the average value, and then the first positioning standard deviation is obtained based on the variance.

[0056] In some embodiments, coordinate values ​​are obtained by measuring at least twice at the second position, and then the average value of the coordinate values ​​is calculated. Then, the variance is calculated based on the multiple coordinate values ​​and the average value, and then the second positioning standard deviation is obtained based on the variance.

[0057] Step 152: Obtain a first weight and a second weight using the first positioning standard deviation and the second positioning standard deviation.

[0058] The first weight is obtained using the following formula: ;in, represents the first weight, represents the first positioning standard deviation, represents the second positioning standard deviation; The second weight is obtained using the following formula: ; Represents the second weight.

[0059] Step 153: weighting the first position with the first weight and weighting the second position with the second weight, thereby obtaining a positioning result of the underwater vehicle.

[0060] Joint localization results It can be calculated using the weighted average method as: .

[0061] Specifically, the x and y coordinates are: ; , that is, the joint positioning result This is the positioning result of the underwater vehicle.

[0062] In this application, an estimate of the heading error can be obtained in each positioning. The heading error estimate is , so the accuracy of heading error estimation can be improved by using at least two estimates. Since the heading error estimation error is positively correlated with the sampling interval, let the sampling interval be , so the weighted calculation is adopted: ; Among them, the accurate heading error = measured heading error - weighted average of heading errors.

[0063] In one application scenario, combined with Figure 5 To explain: like Figure 5 As shown, the surface beacon sends a one-way acoustic signal to interact with the USBL array on the underwater vehicle. The USBL array measures the azimuth and arrival time based on the beacon longitude, latitude and depth in the M groups of one-way acoustic signals, and then calculates the position 1 (first position) and the heading error based on the beacon longitude, latitude and depth, azimuth, arrival time and displacement vector sent by the inertial navigation system. And the surface beacon sends a two-way acoustic signal to interact with the USBL array on the underwater vehicle. The USBL array measures the azimuth and distance based on the beacon longitude, latitude and depth in the two-way acoustic signal, and then calculates the position 2 (second position) based on the beacon longitude, latitude and depth, azimuth, distance and heading error, and then uses the position 1 and position 2 for joint positioning to obtain the final positioning result.

[0064] See also Figure 6 , Figure 6 1 is a schematic diagram of the structure of an embodiment of a positioning system for an underwater vehicle provided by the present application. The positioning system 100 for an underwater vehicle includes a surface beacon 10, a USBL base station 20 and an inertial navigation system 30. The inertial navigation system 30 and the USBL base station 20 are arranged on the underwater vehicle, and the USBL base station 20 is used to interact with the surface beacon 10 to implement the following method: Receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain latitude and longitude information, depth information and communication position identification information; measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measure the second azimuth corresponding to the two-way acoustic signal based on the two-way acoustic signal, and determine the distance between the surface beacon and the underwater vehicle; determine the first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, the first azimuth and the arrival time; determine the second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error; perform joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle.

[0065] In some embodiments, the USBL base station 20 is used to interact with the surface beacon 10 and is also used to implement the method of any of the above embodiments.

[0066] See also Figure 7 , Figure 7 60 is a schematic diagram of a computer readable storage medium according to an embodiment of the present application. The computer readable storage medium 60 is used to store a computer program 61. When the computer program 61 is executed by a processor, it is used to implement the following method: Receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain latitude and longitude information, depth information and communication position identification information; measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measure the second azimuth corresponding to the two-way acoustic signal based on the two-way acoustic signal, and determine the distance between the surface beacon and the underwater vehicle; determine the first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, the first azimuth and the arrival time; determine the second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error; perform joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle.

[0067] In some embodiments, when the computer program 61 is executed by a processor, it is also used to implement the method of any of the above embodiments.

[0068] See also Figure 8 , Figure 8 The schematic diagram of the structure of an embodiment of the positioning device for underwater vehicles provided by the present application is as follows: The positioning device 200 comprises: a receiving module 201 , a measuring module 202 , a first determining module 203 , a second determining module 204 and a joint positioning module 205 .

[0069] The receiving module 201 is used to receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and to receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal include latitude and longitude information, depth information and communication position identification information.

[0070] The measurement module 202 is used to measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and to measure the second azimuth corresponding to the two-way acoustic signal according to the two-way acoustic signal, and to determine the distance between the surface beacon and the underwater vehicle.

[0071] The first determination module 203 is used to determine a first position and a heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, a first azimuth angle and an arrival time.

[0072] The second determination module 204 is used to determine a second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth, the distance and the heading error.

[0073] The joint positioning module 205 is used to perform joint positioning according to the first position and the second position to obtain a positioning result of the underwater vehicle.

[0074] In some embodiments, the first determination module 203 is also used to obtain the installation error corresponding to the underwater vehicle; determine the first position and heading error of the underwater vehicle based on the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle and the arrival time.

[0075] In some embodiments, the first determination module 203 is also used to substitute the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle and the arrival time into a preset equation; and solve the preset equation using the weighted least squares method to obtain the first position and heading error of the underwater vehicle.

[0076] In some embodiments, the preset equation is as follows: ; in, ; ; , represents the weight matrix, represents the error vector, Indicates heading error; Indicates the location of the beacon during the nth positioning. Indicates During the first positioning, the horizontal distance between the beacon and the underwater vehicle is Indicates The first positioning and The difference in the horizontal distance between the beacon and the underwater vehicle during the second positioning is: represents the angle between the beacon and the underwater vehicle in the x direction, represents the angle between the beacon and the underwater vehicle in the y direction, represents the displacement change of the surface beacon in the x direction, represents the displacement change of the surface beacon in the y direction, represents the displacement change of the underwater vehicle in the x direction, represents the displacement change of the underwater vehicle in the y direction, and Indicates the position of the underwater vehicle at the nth positioning.

[0077] In some embodiments, the first determination module 203 is further configured to solve the preset equation using the weighted least squares method to obtain ;in, ; Residual, Represents the positioning standard deviation.

[0078] In some embodiments, the second determination module 204 is further configured to calculate the second position of the underwater vehicle using the following formula: ; ; in, are the latitude, longitude and depth of the beacon, is the latitude, longitude and depth of the origin of the navigation coordinate system, express The true value of Indicates from arrive From the change in position, is the Euler rotation matrix, which is used to transform the carrier coordinate system into the navigation coordinate system. is the Euler rotation matrix, which is used to transform the base matrix coordinate system to the carrier coordinate system. is the beacon in the base array coordinate system axis, Axis and axis, The matrix coordinate system is relative to the carrier coordinate system. axis, Axis and Shaft installation error.

[0079] In some embodiments, the joint positioning module 205 is also used to calculate a first positioning standard deviation based on the first position, and to calculate a second positioning standard deviation based on the second position; to obtain a first weight and a second weight using the first positioning standard deviation and the second positioning standard deviation; to weight the first position using the first weight and to weight the second position using the second weight, thereby obtaining a positioning result of the underwater vehicle.

[0080] In some embodiments, the joint positioning module 205 is further configured to obtain the first weight using the following formula: ;in, represents the first positioning standard deviation, represents the second positioning standard deviation; The second weight is obtained using the following formula: .

[0081] In some embodiments, the receiving module 201, the measuring module 202, the first determining module 203, the second determining module 204 and the joint positioning module 205 cooperate with each other to implement the method of any of the above embodiments.

[0082] In this application, in order to quantitatively evaluate the performance of the method of this application, the positioning root mean square error (RMSE) and bias norm (Bias Norm) are used as the main evaluation indicators. This indicator system can comprehensively reflect the performance of the algorithm in terms of positioning accuracy and bias control. In order to ensure the reliability and statistical significance of the evaluation results, the Monte Carlo simulation method is used, and the number of simulations is set to 3000. In order to demonstrate the performance advantages of the proposed algorithm, several representative algorithms are selected for comparison, which have been widely used in the field of underwater vehicle navigation and positioning: 1) AOA algorithm: Determines the position based only on the signal arrival angle.

[0083] 2) MAOA algorithm: Compared with the AOA algorithm, it takes into account the dynamic changes of the beacon position.

[0084] 3) MBAOA algorithm: Combines azimuth and arrival time information and uses the least squares method to solve.

[0085] 4) MBAOA-s algorithm: The algorithm proposed in this patent adds heading error compensation to ensure positioning accuracy when heading error exists.

[0086] Other settings in the simulation are shown in Table 1 below: Table 1 Basic parameter settings in simulation

[0087] In order to truly simulate the heading error during navigation, an initial heading deviation of 0.6° was set for the underwater vehicle. In the simulation experiment, the positioning performance of various algorithms was compared. Fig. 9 , Fig.10 As shown in Table 2, when the heading error exists, the MBAOA-s algorithm of the present application shows a significant positioning advantage, and accurately estimates the heading error to be 0.66°, with performance significantly better than other comparison algorithms.

[0088] Table 2 Positioning root mean square error and deviation norm of different algorithms

[0089] In summary, the underwater vehicle positioning method, device, USBL base station, storage medium and system provided in the present application receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receive at least two displacement vectors output by an inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain latitude and longitude information, depth information and communication position identification information; measure the arrival time and the corresponding first azimuth of each one-way acoustic signal, and measure the second azimuth corresponding to the two-way acoustic signal based on the two-way acoustic signal, and determine the distance between the surface beacon and the underwater vehicle; determine the first position and heading error of the underwater vehicle based on at least two one-way acoustic signals, at least two displacement vectors, the first azimuth and the arrival time; determine the second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance and the heading error; perform joint positioning based on the first position and the second position to obtain the positioning result of the underwater vehicle. That is, during the positioning process, the underwater vehicle heading error is calibrated at the same time, so that two positioning results (the first position and the second position) can be obtained in one positioning, and the joint positioning result can be output. This can effectively solve the problem of large moving vector error caused by the underwater vehicle heading error in single beacon passive positioning and improve positioning accuracy.

[0090] Furthermore, the present application constructs a single beacon passive positioning model that takes into account the underwater vehicle heading error, and completes the model solution through the weighted least squares method. During the positioning process, the underwater vehicle heading error is calibrated at the same time. And the fusion result of the single beacon positioning result and the USBL positioning result is fully considered to improve the final positioning stability.

[0091] Furthermore, the present application can directly provide hydroacoustic positioning and heading calibration without the need to use Kalman filtering for a complex iterative process, thereby significantly improving positioning accuracy and reducing calibration time.

[0092] In the several embodiments provided in this application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only illustrative, for example, the division of the modules or units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0093] If the integrated units in the above other embodiments are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art, or all or part of the technical solution. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processing circuit component (processor) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0094] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A method for positioning an underwater vehicle, characterized in that: The positioning method comprises: Receiving a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and receiving at least two displacement vectors output by an inertial navigation system; the two-way acoustic signal and the one-way acoustic signal both include latitude and longitude information, depth information and communication position identification information; measuring the arrival time and the corresponding first azimuth of each of the one-way acoustic signals, and measuring the corresponding second azimuth of the two-way acoustic signals according to the two-way acoustic signals, and determining the distance between the surface beacon and the underwater vehicle; determining a first position and a heading error of the underwater vehicle based on the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle, and the arrival time; determining a second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance, and the heading error; Joint positioning is performed based on the first position and the second position to obtain a positioning result of the underwater vehicle.

2. The positioning method according to claim 1, characterized in that: Determining the first position and heading error of the underwater vehicle according to the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle and the arrival time comprises: Obtaining an installation error corresponding to the underwater vehicle; A first position and heading error of the underwater vehicle are determined based on the installation error, the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle, and the arrival time.

3. The positioning method according to claim 2, characterized in that: Determining a first position and a heading error of the underwater vehicle according to the installation error, the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle, and the arrival time comprises: Substituting the installation error, the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle and the arrival time into a preset equation; The preset equation is solved by using a weighted least square method to obtain a first position and a heading error of the underwater vehicle.

4. The positioning method according to claim 3, characterized in that: The preset equation is as follows: ； in, ; ; , represents the weight matrix, represents the error vector, Indicates heading error; Indicates the location of the beacon during the nth positioning. Indicates During the first positioning, the horizontal distance between the beacon and the underwater vehicle is Indicates The first positioning and The difference in the horizontal distance between the beacon and the underwater vehicle during the second positioning is: represents the angle between the beacon and the underwater vehicle in the x direction, represents the angle between the beacon and the underwater vehicle in the y direction, represents the displacement change of the surface beacon in the x direction, represents the displacement change of the surface beacon in the y direction, represents the displacement change of the underwater vehicle in the x direction, represents the displacement change of the underwater vehicle in the y direction, and Indicates the position of the underwater vehicle at the nth positioning.

5. The positioning method according to claim 4, characterized in that: The method of solving the preset equation by weighted least square method to obtain the first position and heading error of the underwater vehicle includes: The preset equation is solved by weighted least square method to obtain ; in, ; Residual, Represents the positioning standard deviation.

6. The positioning method according to claim 1, characterized in that: Determining a second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth, the distance and the heading error comprises: The second position of the underwater vehicle is calculated using the following formula: ； ； in, are the latitude, longitude and depth of the beacon, is the latitude, longitude and depth of the origin of the navigation coordinate system, express The true value of Indicates from arrive From the change in position, is the Euler rotation matrix, which is used to transform the carrier coordinate system into the navigation coordinate system. is the Euler rotation matrix, which is used to transform the base matrix coordinate system to the carrier coordinate system. is the beacon in the base array coordinate system axis, Axis and axis, The matrix coordinate system is relative to the carrier coordinate system. axis, Axis and Shaft installation error.

7. The positioning method according to any one of claims 1 to 6, characterized in that: The performing joint positioning according to the first position and the second position to obtain a positioning result of the underwater vehicle includes: Calculating a first positioning standard deviation according to the first position, and calculating a second positioning standard deviation according to the second position; Obtaining a first weight and a second weight using the first positioning standard deviation and the second positioning standard deviation; The first position is weighted using the first weight and the second position is weighted using the second weight, thereby obtaining a positioning result of the underwater vehicle.

8. The positioning method according to claim 7, characterized in that: The obtaining a first weight and a second weight by using the first positioning standard deviation and the second positioning standard deviation includes: The first weight is obtained by using the following formula: ;in, represents the first positioning standard deviation, represents the second positioning standard deviation; The second weight is obtained by the following formula: 。 9. A positioning device for an underwater vehicle, characterized in that: The positioning device comprises: A receiving module, used to receive a two-way acoustic signal and at least two one-way acoustic signals sent by a surface beacon, and to receive at least two displacement vectors output by an inertial navigation system; the two-way acoustic signal and the one-way acoustic signal both include latitude and longitude information, depth information and communication position identification information; a measuring module, configured to measure the arrival time and the corresponding first azimuth of each of the one-way acoustic signals, and to measure the second azimuth corresponding to the two-way acoustic signal according to the two-way acoustic signal, and to determine the distance between the surface beacon and the underwater vehicle; A first determination module, configured to determine a first position and a heading error of the underwater vehicle according to the at least two one-way acoustic signals, the at least two displacement vectors, the first azimuth angle, and the arrival time; A second determination module, configured to determine a second position of the underwater vehicle based on the two-way acoustic signal, the second azimuth, the distance, and the heading error; A joint positioning module is used to perform joint positioning according to the first position and the second position to obtain a positioning result of the underwater vehicle.

10. A USBL base station, characterized in that: The USBL base station is used to implement the method according to any one of claims 1-8.

11. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store a computer program, and when the computer program is executed by a processor, it is used to implement the method according to any one of claims 1 to 8.

12. A positioning system for an underwater vehicle, characterized in that: The positioning system of the underwater vehicle includes a surface beacon, a USBL base station and an inertial navigation system, and the surface beacon and the USBL base station communicate through a water medium; wherein the inertial navigation system and the USBL base station are arranged on the underwater vehicle to be positioned, and the USBL base station is used to implement the method described in any one of claims 1-8.

Images