Positioning Method, Device, USBL Base Station, Storage Medium and System for Underwater Vehicle

Abstract

The present application discloses a positioning method, device, USBL base station, storage medium and system for an underwater vehicle. The method includes: 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; measuring the arrival time of each one-way acoustic signal and the corresponding first azimuth angle, and measuring the second azimuth angle corresponding to the two-way acoustic signal according to the two-way acoustic signal, and determining the distance between the surface beacon and the underwater vehicle; determining the first position and heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle and the arrival time; determining the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance and the heading error; and performing joint positioning according to the first position and the second position to obtain the positioning result of the underwater vehicle. By the above method, the positioning accuracy can be improved.

Description

Technical Field

[0001] This application relates to the technical field of autonomous underwater vehicle (AUV) positioning, and particularly to a positioning method, device, USBL base station, system, computer-readable storage medium, and system for an underwater vehicle. Background Art

[0002] Due to the particularity of the underwater environment, high-precision radio navigation cannot be used, and sound waves that can propagate over long distances in the marine environment have become the signal carriers for ranging and direction finding that underwater positioning and navigation can rely on. According to different error transmission methods, underwater positioning technologies are 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 advantages such as strong anti-interference ability, short update time, high update rate, and high position accuracy. However, the inherent drift error of 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 an underwater vehicle, in order to effectively control navigation errors, it is crucial to adopt heading calibration and external-assisted positioning means. These means not only help correct sensor drift but also improve the robustness and accuracy of the navigation system, ensuring that the underwater vehicle can reliably achieve precise control of position and direction when performing tasks in the underwater environment.

[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 extensive attention in recent years due to its convenience, high efficiency, and low cost. The single beacon positioning method includes active positioning and passive positioning. Among them, active positioning requires using a high-power clock to maintain the accuracy of ranging and is not applicable to underwater vehicles for long-term underwater navigation. Single beacon passive positioning is further divided into three methods: time difference of arrival-based method, angle of arrival-based method, and a method based on a combination of both. Installing a USBL array on an underwater vehicle platform to form an inverse ultra-short baseline positioning system can make full use of the time difference of arrival and the angle of arrival to complete positioning.

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

[0006] Looking at the development process of underwater acoustic positioning technology in domestic and foreign underwater vehicle clusters, its development trend mainly focuses on the USBL system that can be carried on underwater vehicles, with low power consumption, high precision, and concealment as the main development characteristics.

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

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

[0009] In a first aspect, this application provides a positioning method for an underwater vehicle. The positioning method includes: 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 signals contain longitude and latitude information, depth information, and communication bit identification information; measuring the arrival time and corresponding first azimuth angle of each one-way acoustic signal, and measuring the second azimuth angle corresponding to the two-way acoustic signal according to the two-way acoustic signal, and determining the distance between the surface beacon and the underwater vehicle; determining the first position and heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time; determining the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the heading error; and performing joint positioning based on the first position and the second position to obtain the positioning result of the underwater vehicle.

[0010] A further technical solution is to 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, a first azimuth angle, and arrival time, including: obtaining the 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, the first azimuth angle, and the 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 acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time, including: substituting 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; 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 that the preset equation is as follows: ;

[0013] where ;

[0014] ; , represents the weight matrix, represents the error vector, represents the heading error; represents the position of the beacon at the nth positioning, represents the th positioning, the horizontal distance between the beacon and the underwater vehicle, represents the th positioning and the th positioning, the distance difference between the horizontal distances of the beacon and the underwater vehicle, 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 represent the position of the underwater vehicle at the nth positioning.

[0015] A further technical solution is to solve the preset equation using the weighted least squares method to obtain the first position and heading error of the underwater vehicle, including: solving the preset equation using the weighted least squares method to obtain ; wherein, ; represents the residual, and represents the positioning standard deviation.

[0016] A further technical solution thereof is to determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth, the distance and the course error, including: calculating the second position of the underwater vehicle by using the following formula: ;

[0017] ;

[0018] wherein, are the latitude, longitude and depth of the beacon, are the latitude, longitude and depth of the origin of the navigation coordinate system, represents the true value of, represents the change amount from to the position change from the position, is the Euler rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the Euler rotation matrix for converting the array coordinate system to the vehicle coordinate system, is the axis of the beacon in the array coordinate system, axis and axis, is the installation error of the array coordinate system relative to the vehicle coordinate system respectively on the axis, axis and axis.

[0019] A further technical solution thereof is to perform joint positioning according to the first position and the second position to obtain the positioning result of the underwater vehicle, including: calculating the first positioning standard deviation according to the first position, and calculating the second positioning standard deviation according to the second position; obtaining the first weight and the second weight by using the first positioning standard deviation and the second positioning standard deviation; weighting the first position by using the first weight and weighting the second position by using the second weight, and further obtaining the positioning result of the underwater vehicle.

[0020] A further technical solution thereof is to obtain the first weight and the second weight by using the first positioning standard deviation and the second positioning standard deviation, including: obtaining the first weight by using the following formula:

[0021] ; wherein, represents the first positioning standard deviation, represents the second positioning standard deviation;

[0022] The second weight is obtained using the following formula: .

[0023] In a second aspect, the present application provides a positioning device for an underwater vehicle. The positioning device includes: a receiving module configured to receive a two-way acoustic signal and at least two one-way acoustic signals transmitted 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 signals include longitude and latitude information, depth information, and communication bit identification information; a measurement module configured to measure the arrival time and corresponding first azimuth angle of each one-way acoustic signal, to measure the second azimuth angle 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 based on at least two one-way acoustic signals, 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 angle, the distance, and the heading error; and a joint positioning module configured to perform joint positioning based on the first position and the second position to obtain a positioning result of the underwater vehicle.

[0024] 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.

[0025] 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.

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

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

[0028] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained according to these drawings. Among them:

[0029] Figure 1 is a schematic flowchart of an embodiment of the positioning method of the underwater vehicle provided by the present application;

[0030] Figure 2 is the MAC protocol timeline provided by the present application;

[0031] Figure 3 is Figure 1 a schematic flowchart of an embodiment of step 13 in

[0032] Figure 4 is Figure 1 a schematic flowchart of an embodiment of step 15 in

[0033] Figure 5 is a schematic diagram of an application scenario of the positioning method of the underwater vehicle provided by the present application;

[0034] Figure 6 is a schematic structural diagram of an embodiment of the positioning system of the underwater vehicle provided by the present application;

[0035] Figure 7 It is a schematic structural diagram of an embodiment of a computer-readable storage medium provided by the present application;

[0036] Figure 8 It is a schematic structural diagram of an embodiment of a positioning device for an underwater vehicle provided by the present application;

[0037] Figure 9 It is a comparison chart of positioning root mean square error provided by the present application;

[0038] Figure 10 It is a comparison chart of positioning deviation norm provided by the present application. Detailed implementation manners

[0039] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. It can be understood that the specific embodiments described herein are only used to explain the present application, rather than limiting the present application. In addition, it should be noted that for the sake of description, only parts related to the present application rather than all structures are shown in the drawings. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

[0040] Referring to "embodiment" herein means that the specific features, structures, or characteristics described in conjunction with the embodiment may be included in at least one embodiment of the present application. The phrase appears in various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein may be combined with other embodiments.

[0041] Refer to Figure 1 , Figure 1 It is a schematic flowchart of an embodiment of a positioning method for an underwater vehicle provided by the present application. The positioning method includes:

[0042] Step 11: Receive the two-way acoustic signal and at least two one-way acoustic signals sent by the surface beacon, and receive at least two displacement vectors output by the inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal include longitude and latitude information, depth information, and communication bit identification information.

[0043] In some embodiments, the surface beacon can adopt a surface mobile GPS beacon, which uses the method of single-point mooring and slack mooring buoy, and sends positioning signals according to a certain MAC protocol period. The optional modulation modes of the beacon are: PN code modulation, FM (Frequency Modulation), and SS (Spread Spectrum). The modulation mode can be adjusted according to the application scenario, 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 successful reception of the positioning signal by the positioning target.

[0044] Among them, while sending positioning signals, the surface mobile GPS beacon acts as a surface network gateway node. In a non-covert communication scenario, the beacon can broadcast positioning signals in a positioning cycle and complete data interaction with an underwater vehicle. Considering the half-duplex working mode of underwater acoustic communication, the acoustic communication machine cannot receive signals when transmitting signals. Therefore, in the process of network information interaction, time slots need to be divided for the positioning process and the communication process. As Figure 2 shown in the MAC protocol timeline of this system. In a round of communication and positioning process, the beacon node broadcasts a trigger packet (TR), and the data in the packet includes the longitude, latitude, depth, service bit flag, and distance of the beacon. After the underwater vehicle to be positioned receives the trigger packet, it judges 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, and measures the distance through the ray tracing method. When data interaction is required between the beacon and the underwater vehicle node, data is interacted during the data interaction period. After the positioning period and the data interaction period, it enters the next round of positioning and communication process, and the ranging information of the underwater vehicle ranged in the previous round is informed in the trigger packet.

[0045] Since in this system, the underwater acoustic communication machine 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, and user data can be stored in each data block. Both the TR packet and the ACK packet use one data block to interact data, and the processing delay is ms; the DATA packet occupies different data blocks according to the data size. Here, it is default to occupy 8 data blocks, and the processing delay is ms; assume that the effective distance of the positioning process is m, so the single propagation delay of the signal is ms, and the total cost of 4 propagations is ms, where is the equivalent sound speed. Therefore, the interaction time for one round is ms.

[0046] In some embodiments, a USBL base station can be carried on an underwater vehicle to communicate with a surface beacon through the USBL base station. During the navigation of the underwater vehicle, the USBL base station continuously receives the positioning signals sent by the beacon. The USBL base station includes a software system and a hardware system. The software system is equipped with a Sealinx network protocol stack, which includes four layers of networks: the application layer, the transport layer, the network layer, and the MAC layer. At the application layer, the protocol stack is responsible for establishing a connection with the main control of the underwater vehicle through UDP, receiving the inertial navigation data (output by the inertial navigation system) forwarded in real time by the underwater vehicle, and providing the positioning solution result for the underwater vehicle. At the same time, the protocol stack performs packet splitting and packet assembly at the transport layer. At the MAC layer, the underwater vehicle and the GPS beacon are equipped with a set of MAC protocols to achieve necessary communication services. The hardware system of the USBL base station includes a chip, an acoustic transducer, and a pressure-resistant housing for carrying the equipment.

[0047] Among them, a positioning solution unit is deployed on the chip of the USBL base station. The positioning solution unit calculates its own position information according to the positioning signals received by the USBL base station and the movement vector output by the inertial navigation of the underwater vehicle, and transmits the position information back to the main control system of the underwater vehicle.

[0048] In some embodiments, the surface beacon sends acoustic signals at fixed time intervals. The signals contain its longitude and latitude information, depth information, and communication bit identification information. The underwater vehicle receives the acoustic signals through the carried USBL array. The array measures the time of arrival of the one-way acoustic signal and obtains the azimuth angle through the plane wave method to determine the direction of the signal source.

[0049] In some embodiments, there is a corresponding data storage module. The data storage module is used for the underwater vehicle to continuously receive the positioning signals emitted by the beacon during navigation, and record the longitude and latitude information and depth information of the beacon, the signal arrival time and the azimuth angle information of the beacon, and record 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 data volume, the higher the accuracy of single beacon positioning, due to the accumulation of time, the movement vector introduced by the inertial navigation system gradually increases, which instead affects the final positioning accuracy. Therefore, a time window for adding data is added. The time window is the product of the sampling points and the sampling interval, where the sampling interval is the beacon transmission period , and the number of sampling points is set to .

[0050] Step 12: Measure the time of arrival of each one-way acoustic signal and the corresponding first azimuth angle, and measure the second azimuth angle 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.

[0051] The underwater vehicle continuously monitors these signals and uses the time of arrival and azimuth data of each measurement for dead reckoning.

[0052] Among them, the first azimuth is used to represent the relative position relationship between the surface beacon and the underwater vehicle. The second azimuth is also used to represent the relative position relationship between the surface beacon and the underwater vehicle. It's just that the data involved in the calculation between the two is different.

[0053] Step 13: 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 time of arrival.

[0054] Among them, the first position is used to represent the positioning result of the underwater vehicle at the current moment.

[0055] The underwater vehicle can obtain the positioning result (the first position) and heading error of the underwater vehicle at the current moment through the single beacon positioning method, combined with the sound speed curve, the first M times of time of arrival and direction finding results, and the motion vectors output by the underwater vehicle's inertial navigation for the first M times.

[0056] In some embodiments, referring to Figure 3 , step 13 may have the following process:

[0057] Step 131: Obtain the installation error corresponding to the underwater vehicle.

[0058] Step 132: 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, and the time of arrival.

[0059] In some embodiments, the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth, and the time of arrival can be substituted into a preset equation. Then, the weighted least squares method is used to solve the preset equation to obtain the first position and heading error of the underwater vehicle.

[0060] Among them, the preset equation is as follows:

[0061] ;

[0062] Among them, ; ; , represents the weight matrix, represents the error vector, represents the heading error, represents the heading error; represents the position of the beacon at the nth positioning, represents the During the second positioning, the horizontal distance between the beacon and the underwater vehicle Indicates the During the second positioning and the During the second positioning, the distance difference between the horizontal distances of the beacon and the underwater vehicle Indicates the angle between the beacon and the underwater vehicle in the x direction Indicates the angle between the beacon and the underwater vehicle in the y direction Indicates the displacement change of the surface beacon in the x direction Indicates the displacement change of the surface beacon in the y direction Indicates the displacement change of the underwater vehicle in the x direction Indicates the displacement change of the underwater vehicle in the y direction And Indicates the position of the underwater vehicle during the nth positioning

[0063] Solve the preset equation using the weighted least squares method to obtain ; where ; Indicates the residual Indicates the positioning standard deviation

[0064] In some embodiments, the above preset equation is obtained in the following manner:

[0065] The motion state of the underwater vehicle is an observed quantity in the body coordinate system, and its positioning and navigation system uses the geodetic coordinate system. Therefore, it is necessary to convert the motion state in the body coordinate system to the geodetic coordinate system through a coordinate rotation matrix. The coordinate rotation matrix is composed of attitude angles. When there is an error in the attitude angle of the vehicle That is, a rotation matrix with an error is used during the navigation process , after The navigation time, in the geodetic coordinate system, the estimated position of the underwater vehicle is:

[0066] (1)

[0067] Where Indicates the initial position of the inertial navigation position estimation, Indicates the speed of the vehicle in the body coordinate system, Indicates the speed The duration of continuous movement, the position error can be expressed as . From equation (1), we can obtain

[0068] (2)

[0069] Where Indicates from To The change in position. In common inertial navigation devices, the heading angle and the pitch angle , roll angle are defined differently, with clockwise being positive. The rotation matrix is as follows:

[0070] (3)

[0071] Due to the existence of heading angle error, the expressions of Equation (2) and Equation (3) have changed. Let the heading angle error (heading error) be , then Equation (2) is rewritten as:

[0072] (4)

[0073] Equation (3) is rewritten as:

[0074] (5)

[0075] Among them, represents the true value of the rotation matrix.

[0076] To simplify the expression, let:

[0077] (6)

[0078] The formula for USBL positioning is:

[0079] (7)

[0080] In the formula, is the Euler rotation matrix, which converts the vehicle coordinate system to the navigation coordinate system; is the Euler rotation matrix, which converts the array coordinate system to the vehicle coordinate system, where , and are the installation angle errors of the axis, axis, axis of the array coordinate system relative to the axis, axis, axis of the vehicle coordinate system; 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 axis, axis and axis of the beacon in the array coordinate system; is the relative position of the array coordinate system to the vehicle coordinate system on the axis, axis and Installation displacement error (installation error) of the shaft.

[0081] Here, the angle definition of the beacon in the navigation coordinate system is as follows:

[0082] (8)

[0083] Among them, R represents the slant range, , , , refer to The corresponding latitude, longitude, and depth of the beacon.

[0084] Substituting equations (4), (5), and (6) into equation (7), we can obtain:

[0085] (9)

[0086] Let: (10)

[0087] Equation (10) can be simplified to:

[0088] (11)

[0089] As shown in equation (11), in a straight-line track, there is a direct relationship between the course error and the positioning process, and this relationship does not apply to a circular track. Considering that the course error of an underwater vehicle is usually small, a small-angle approximation can be made, , , then:

[0090] (12)

[0091] In a short baseline system for single beacon positioning, the and between the beacon and the underwater vehicle can be obtained. 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:

[0092] (13)

[0093] Among them, and respectively represent the position information of the beacon and the underwater vehicle in the geodetic coordinate system at the th positioning. represents the horizontal distance between the beacon and the underwater vehicle at the th positioning, represents the During the second positioning and the

[0094] (14)

[0095] Substitute the data into Equation (14), and convert the absolute position to the relative position, and , we get:

[0096] (15)

[0097] wherein ,

[0098] ,

[0099] .

[0100] Solve Equation (15) using the weighted least squares method, we get:

[0101] (18)

[0102] The weight definition cited in the solution is:

[0103] (19)

[0104] wherein, is the median absolute deviation. Utilize its characteristic of being insensitive to outliers to improve the stability of the weighted least squares method:

[0105] (20)

[0106] Convert the position of the underwater vehicle using the beacon position to obtain the positioning result of the first position:

[0107] (21)

[0108] Step 14: Determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the course error.

[0109] Among them, 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 is different.

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

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

[0112] 。

[0113] ;

[0114] Wherein, are the latitude, longitude and depth of the beacon, are the latitude, longitude and depth of the origin of the navigation coordinate system, represents the true value of, represents the change from to the position change from the position, is the Euler rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the Euler rotation matrix for converting the array coordinate system to the vehicle coordinate system, is the axis, axis and axis of the beacon in the array coordinate system, is the installation error of the array coordinate system relative to the vehicle coordinate system on the axis, axis and axis respectively.

[0115] When there is a round-trip communication process between the underwater vehicle and the surface beacon, the positioning result of the underwater vehicle can be obtained directly using the USBL. After the surface beacon broadcasts the trigger packet, the underwater vehicle receives the trigger packet and records the current direction-finding information, and packs the processing delay into the acknowledgment packet and returns it to the surface beacon. After the beacon receives the signal, it calculates the round-trip delay, compensates for the influence of the sound speed using the ray final algorithm, and packs the slant range information into the data packet and sends it back to the underwater vehicle. After the underwater vehicle receives the data packet, it obtains the slant range information and the longitude and latitude of the moving beacon, and thus obtains its own USBL positioning. Therefore, the positioning result of the second position of the underwater vehicle can be expressed by the following formula:

[0116] (22)

[0117] Step 15: Perform joint positioning based on the first position and the second position to obtain the positioning result of the underwater vehicle.

[0118] In some embodiments, referring to Figure 4 , step 15 may have the following process:

[0119] Step 151: Calculate the first positioning standard deviation according to the first position, and calculate the second positioning standard deviation according to the second position.

[0120] In some embodiments, at least two measurements are taken at the first position to obtain coordinate values, and then the average value of these coordinate values is calculated. Then, the variance is calculated based on the multiple coordinate values and the average value, and further, the first positioning standard deviation is obtained based on the variance.

[0121] In some embodiments, at least two measurements are taken at the second position to obtain coordinate values, and then the average value of these coordinate values is calculated. Then, the variance is calculated based on the multiple coordinate values and the average value, and further, the second positioning standard deviation is obtained based on the variance.

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

[0123] The first weight is obtained by using the following formula:

[0124] ; where represents the first weight, represents the first positioning standard deviation, represents the second positioning standard deviation;

[0125] The second weight is obtained by using the following formula:

[0126] ; represents the second weight.

[0127] Step 153: Weight the first position by using the first weight and weight the second position by using the second weight, and further obtain the positioning result of the underwater vehicle.

[0128] Combined positioning result can be calculated by the weighted average method as:

[0129] .

[0130] Specifically, the x and y coordinates are respectively:

[0131] ;

[0132] , that is, the combined positioning result is the positioning result of the underwater vehicle.

[0133] In the present application, an estimation of the heading error can be obtained in each positioning. Let the The estimated value of the course error for the time, so the accuracy of the course error estimation can be improved by using at least two estimations. Since the estimation error of the course error is positively correlated with the sampling interval, let the sampling interval be , so weighted calculation is adopted:

[0134] ;

[0135] Among them, the accurate course error = the measured course error - the weighted average of the course error.

[0136] In an application scenario, in combination with Figure 5 for illustration:

[0137] Such as Figure 5 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 angle and arrival time according to the longitude, latitude and depth and other information of the beacon in the M groups of one-way acoustic signals, and then calculates the position 1 (the first position) and the course error according to the longitude, latitude and depth, azimuth angle, arrival time of the beacon and the 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 angle and distance according to the longitude, latitude and depth and other information in the two-way acoustic signal, and then calculates the position 2 (the second position) according to the longitude, latitude and depth, azimuth angle, distance and course error of the beacon, and then uses the position 1 and the position 2 for joint positioning to obtain the final positioning result.

[0138] Refer to Figure 6 , Figure 6 is a schematic structural diagram of an embodiment of the positioning system of the underwater vehicle provided by the present application. The positioning system 100 of the underwater vehicle includes a surface beacon 10, a USBL base station 20 and an inertial navigation system 30. Among them, 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 methods:

[0139] Receive the two-way acoustic signal and at least two one-way acoustic signals sent by the surface beacon, and receive at least two displacement vectors output by the inertial navigation system; both the two-way acoustic signal and the one-way acoustic signals contain longitude and latitude information, depth information, and communication bit identification information; measure the arrival time of each one-way acoustic signal and the corresponding first azimuth angle, and measure the second azimuth angle 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; determine the first position and heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time; determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, 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.

[0140] 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.

[0141] See Figure 7 , Figure 7 is a schematic structural diagram of an embodiment of a computer-readable storage medium provided by the present application. The computer-readable storage medium 60 is used to store a computer program 61, and when the computer program 61 is executed by a processor, it is used to implement the following method:

[0142] Receive the two-way acoustic signal and at least two one-way acoustic signals sent by the surface beacon, and receive at least two displacement vectors output by the inertial navigation system; both the two-way acoustic signal and the one-way acoustic signals contain longitude and latitude information, depth information, and communication bit identification information; measure the arrival time of each one-way acoustic signal and the corresponding first azimuth angle, and measure the second azimuth angle 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; determine the first position and heading error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time; determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, 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.

[0143] 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.

[0144] See Figure 8 , Figure 8 is a schematic structural diagram of an embodiment of a positioning device for an underwater vehicle provided by the present application. The positioning device 200 includes: a receiving module 201, a measuring module 202, a first determining module 203, a second determining module 204, and a joint positioning module 205.

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

[0146] The measuring module 202 is configured to measure the arrival time of each one-way acoustic signal and the corresponding first azimuth angle, measure the second azimuth angle 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.

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

[0148] The second determining module 204 is configured to determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the heading error.

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

[0150] In some embodiments, the first determining module 203 is further configured to obtain the installation error corresponding to the underwater vehicle; determine the first position and the heading error of the underwater vehicle according to the installation error, at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time.

[0151] In some embodiments, the first determining module 203 is further configured 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; solve the preset equation by using the weighted least squares method to obtain the first position and the heading error of the underwater vehicle.

[0152] In some embodiments, the preset equation is as follows: ;

[0153] Wherein, ;

[0154] ; 、 represents the weight matrix, represents the error vector, represents the heading error; represents the position of the beacon at the nth positioning, represents the th positioning, the horizontal distance between the beacon and the underwater vehicle, represents the th positioning and the The distance difference between the horizontal distances of the beacon and the underwater vehicle during the secondary positioning, 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 represents the position of the underwater vehicle during the nth positioning.

[0155] In some embodiments, the first determination module 203 is further configured to solve the preset equation by using the weighted least squares method to obtain ; where ; represents the residual, represents the positioning standard deviation.

[0156] In some embodiments, the second determination module 204 is further configured to calculate the second position of the underwater vehicle by using the following formula: ;

[0157] ;

[0158] where is the latitude, longitude, and depth of the beacon, is the latitude, longitude, and depth of the origin of the navigation coordinate system, represents the true value of, represents from to the change amount of the position, is the Euler rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the Euler rotation matrix for converting the array coordinate system to the vehicle coordinate system, is the axis, axis, and axis of the beacon in the array coordinate system, is the installation error of the array coordinate system relative to the vehicle coordinate system on the axis, axis, and axis.

[0159] In some embodiments, the joint positioning module 205 is further configured to calculate a first positioning standard deviation based on the first position, and calculate a second positioning standard deviation based on the second position; obtain a first weight and a second weight by using the first positioning standard deviation and the second positioning standard deviation; weight the first position by using the first weight and weight the second position by using the second weight, so as to obtain the positioning result of the underwater vehicle.

[0160] In some embodiments, the joint positioning module 205 is further configured to obtain the first weight by using the following formula:

[0161] ; where represents the first positioning standard deviation, represents the second positioning standard deviation;

[0162] obtain the second weight by using the following formula: .

[0163] 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.

[0164] In this application, to quantitatively evaluate the performance of the method of this application, the root-mean-square error (RMSE) and the 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. To ensure the reliability and statistical significance of the evaluation results, the Monte Carlo simulation method is used, and the number of simulation times is set to 3000. To demonstrate the performance advantages of the proposed algorithm, several representative algorithms are selected for comparison, and these algorithms have been widely used in the field of underwater vehicle navigation and positioning:

[0165] 1) AOA algorithm: Determine the position only based on the angle of arrival of the signal.

[0166] 2) MAOA algorithm: Compared with the AOA algorithm, it considers the dynamic change of the beacon position.

[0167] 3) MBAOA algorithm: Combine the azimuth angle and the time-of-arrival information, and use the least squares method to solve.

[0168] 4) MBAOA-s algorithm: The algorithm proposed in this patent, which adds heading error compensation to ensure the positioning accuracy in the presence of heading error.

[0169] Other settings in the simulation are shown in Table 1 below:

[0170] Table 1 Basic parameter settings in the simulation

[0171]

[0172] To truly simulate the course error during navigation, an initial course deviation of 0.6° was set for the underwater vehicle. In the simulation experiment, the positioning performances of various algorithms were compared. As Figure 9 、 Figure 10 shown in and Table 2, when there is a course error, the MBAOA-s algorithm of the present application demonstrates significant positioning advantages, and accurately estimates the course error to be 0.66°, with its performance being significantly better than other comparison algorithms.

[0173] Table 2 Root mean square error and deviation norm of positioning for different algorithms

[0174]

[0175] In summary, the positioning method, device, USBL base station, storage medium, and system for the underwater vehicle provided by the present application receive the two-way acoustic signal and at least two one-way acoustic signals sent by the surface beacon, and receive at least two displacement vectors output by the inertial navigation system; both the two-way acoustic signal and the one-way acoustic signal contain longitude and latitude information, depth information, and communication bit identification information; measure the arrival time of each one-way acoustic signal and the corresponding first azimuth angle, and measure the second azimuth angle 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; determine the first position and course error of the underwater vehicle according to at least two one-way acoustic signals, at least two displacement vectors, the first azimuth angle, and the arrival time; determine the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the course 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 course error of the underwater vehicle is calibrated simultaneously, two positioning results (the first position and the second position) can be obtained in one positioning, and the joint positioning result is output, which can effectively solve the problem of large moving vector error caused by the course error of the underwater vehicle in single-beacon passive positioning and improve the positioning accuracy.

[0176] Furthermore, the present application constructs a single-beacon passive positioning model considering the course error of the underwater vehicle, and completes the model solution through the weighted least squares method, and calibrates the course error of the underwater vehicle simultaneously during the positioning process. And fully considers the fusion result of the single-beacon positioning result and the USBL positioning result, improving the final positioning stability.

[0177] Furthermore, the present application can directly provide underwater acoustic positioning and course calibration without using the complex iterative process of the Kalman filter, significantly improving the positioning accuracy and reducing the calibration time.

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

[0179] If the integrated unit in the above-mentioned other embodiments is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, or all or part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processing circuit component (processor) to execute all or part of the steps of the methods described in various embodiments of the present application. The foregoing storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs.

[0180] The above is only the embodiments of the present application, and thus does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made by using the content of the specification and drawings of the present application, or directly or indirectly applied in other related technical fields, shall be similarly included in the patent protection scope of the present application.

Claims

1. A positioning method for an underwater vehicle, characterized in that, The positioning method includes: 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 signals include longitude and latitude information, depth information, and communication bit identification information; Measuring the arrival time of each of the one-way acoustic signals and the corresponding first azimuth angle, measuring the second azimuth angle corresponding to the two-way acoustic signal according to the two-way acoustic signal, and determining the distance between the surface beacon and the underwater vehicle; 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; Determining the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the heading error; Performing joint positioning according to the first position and the second position to obtain the positioning result of the underwater vehicle; Wherein, the second position of the underwater vehicle is calculated using the following formula: ； ； Wherein, are the latitude, longitude and depth of the beacon, are the latitude, longitude and depth of the origin of the navigation coordinate system, represents the true value of, represents the change amount from to the position change amount from the position, is the Euler rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the Euler rotation matrix for converting the array coordinate system to the vehicle coordinate system, is the axis, axis and axis of the beacon in the array coordinate system, are the installation errors of the array coordinate system relative to the vehicle coordinate system on the axis, axis and axis respectively, represents the heading error.

2. The positioning method according to claim 1, wherein The 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 includes: Obtaining the installation error corresponding to the underwater vehicle; Determining the first position and 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.

3. The positioning method according to claim 2, characterized in that, The determining the first position and 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 includes: 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; Solving the preset equation using the weighted least squares method to obtain the first position and heading error of the underwater vehicle.

4. The positioning method according to any one of claims 1-3, characterized in that The performing joint positioning according to the first position and the second position to obtain the 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; Weighting the first position using the first weight and weighting the second position using the second weight, thereby obtaining the positioning result of the underwater vehicle.

5. The positioning method according to claim 4, wherein The obtaining the first weight and the second weight using the first positioning standard deviation and the second positioning standard deviation includes: Obtaining the first weight using the following formula: ; wherein, represents the first positioning standard deviation, represents the second positioning standard deviation; Obtaining the second weight using the following formula: 。 6. A positioning device for an underwater vehicle, characterized in that, The positioning device includes: A receiving module, configured to 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 signals include longitude and latitude information, depth information, and communication bit identification information; A measurement module, configured to measure the arrival time and the corresponding first azimuth angle of each of the one-way acoustic signals, and measure the second azimuth angle 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; A first determination module, configured to determine the first position and the 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 the second position of the underwater vehicle according to the two-way acoustic signal, the second azimuth angle, the distance, and the heading error; wherein, the second position of the underwater vehicle is calculated by using the following formula: ； ； Wherein, are the latitude, longitude and depth of the beacon, are the latitude, longitude and depth of the origin of the navigation coordinate system, denotes the true value of, denotes the change in position from to the position, is the Euler rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the Euler rotation matrix for converting the array coordinate system to the vehicle coordinate system, are the axis, axis and axis of the beacon in the array coordinate system, are the installation errors of the array coordinate system relative to the vehicle coordinate system on the axis, axis and axis, denotes the heading error; A joint positioning module, configured to perform joint positioning according to the first position and the second position to obtain the positioning result of the underwater vehicle.

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

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

9. 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. The surface beacon and the USBL base station communicate through an aqueous medium; wherein, the inertial navigation system and the USBL base station are disposed on the underwater vehicle to be positioned, and the USBL base station is used to implement the method according to any one of claims 1-5.

Images