Underwater vehicle cooperative navigation positioning method and device based on multi-hop relay and dynamic error compensation

Abstract

The invention discloses an underwater vehicle cooperative navigation and positioning method and device based on multi-hop relay and dynamic error compensation, and relates to the technical field of underwater vehicle cooperative navigation and positioning, and the method comprises the following steps: providing a double-hop cooperative architecture: a hierarchical positioning mode of a main platform-relay AUV-remote AUV (operation AUV or reconnaissance AUV); relative positioning is carried out between the main platform and the relay AUV through the ultra-short baseline underwater acoustic positioning system; cooperative positioning is carried out between the relay AUV and the long-distance AUV through an underwater acoustic communication pure distance measurement mode; by establishing an underwater acoustic positioning full-error model (covering time, lever arm, sound velocity disturbance and the like) and utilizing EKF to dynamically correct a positioning result, meter-scale positioning precision in a 100km range is realized. According to the invention, high-precision cooperative positioning of the main platform on the long-distance AUV can be realized.

Description

Technical Field

[0001] The present invention relates to the technical field of cooperative navigation and positioning of underwater vehicles, and particularly to a cooperative navigation and positioning method and device for autonomous underwater vehicles (AUVs) based on a two-hop relay architecture, multi-source error modeling, and extended Kalman filter (EKF) dynamic compensation. The present invention is applicable to scenarios such as deep-sea exploration, large-scale ocean mapping, and covert military reconnaissance, and solves the problems of limited communication distance, uncontrollable error accumulation, and poor environmental adaptability in traditional single-hop cooperative positioning. Background Art

[0002] Autonomous Underwater Vehicles (AUVs) are widely used in fields such as ocean mapping, resource exploration, and military reconnaissance due to their strong concealment and wide operation range. However, a single AUV is limited by sensor accuracy, energy supply, and communication distance, and faces problems such as cumulative positioning errors and the need for frequent surfacing for GPS calibration in long-distance tasks, which seriously restrict task efficiency and concealment.

[0003] Existing master-slave cooperative navigation technologies assist slave AUVs in positioning through a high-precision master platform (such as a scientific research ship or a mother ship) equipped with high-precision navigation equipment, but still have the following defects: First, single-hop communication limitation: The attenuation rate of common medium-frequency (1–10 kHz) underwater acoustic signals in seawater is about 0.1 dB / km, and the effective single-hop communication radius is usually less than 30 km, which cannot meet the requirements of ultra-long-distance (>100 km) tasks; Second, error accumulation effect: Factors such as time reference deviation, lever arm effect, and hydrological environment disturbance (such as changes in sound speed profile) cause the positioning error between the master platform and the slave AUV to increase exponentially with distance, and cannot meet the requirements of accurate long-distance operation of the slave AUV.

[0004] In response to the above problems, although some studies have used inertial navigation and filtering algorithms to suppress errors, the challenges of long-distance relay and multi-source error coupling have not been solved. Summary of the Invention

[0005] Embodiments of the present invention provide a cooperative navigation and positioning method and device for underwater vehicles based on multi-hop relay and dynamic error compensation, which realize cooperative positioning of long-distance AUVs.

[0006] In a first aspect, the present invention provides a cooperative navigation and positioning method for underwater vehicles based on multi-hop relay and dynamic error compensation, including:

[0007] One is to introduce a "two-hop cooperation" architecture: a hierarchical positioning mode of master platform → relay AUV → long-distance AUV (operation AUV or reconnaissance AUV);

[0008] Relative positioning is performed between the main platform and the relay AUV through an ultra-short baseline underwater acoustic positioning system; cooperative positioning is performed between the relay AUV and the long-distance AUV through the pure distance measurement method of underwater acoustic communication;

[0009] Second, by establishing an underwater acoustic positioning full error model (covering time, lever arm, sound speed perturbation, etc.), the EKF is used to dynamically correct the positioning result to achieve a positioning accuracy of meter level within 100 km.

[0010] In some instances, the positioning influence error is obtained, where

[0011] ΔX a 、ΔY a 、ΔZ a represent the comprehensive error without considering the lever arm information, ΔX e 、ΔY e 、ΔZ e represent the cooperative navigation positioning error caused by the lever arm measurement error, θ my 、Δθ my 、θ mz 、Δθ mz 、R, c, ΔT, ΔR, respectively represent the y-axis incident angle of the master-slave AUVs, the y-axis incident angle error of the master-slave AUVs, the z-axis incident angle of the master-slave AUVs, the z-axis incident angle error of the master-slave AUVs, the slant range measurement result of the master-slave AUVs, the sound speed in water, the time reference error, the slant range error affected by the water temperature environment, the lever arm information measurement error of the ultra-short baseline hydrophone in the unmanned underwater vehicle body coordinate system, and the lever arm information measurement error of the ultra-short baseline acoustic beacon in the unmanned underwater vehicle body coordinate system, represents the rotation matrix from the AUV body coordinate system to the navigation coordinate system, represents the rotation matrix from the main platform body coordinate system to the navigation coordinate system.

[0012] In some instances, relative positioning is performed between the main platform and the relay AUV, where X a 、Y a 、Z a represent the position of the ultra-short baseline acoustic beacon in the array coordinate system.

[0013] In some instances, cooperative positioning is performed between the relay AUV and the long-distance AUV, where Z k represents the observable of the cooperative navigation system based on distance measurement, X A 、Y A are the position information of the relay AUV solved by the ultra-short baseline positioning system at time k, v k-1denotes the velocity of the long - range AUV measured by the sensor at time k - 1, (x k , y k ) denotes the position information of the long - range AUV at time k.

[0014] In some instances, the dynamic correction of the positioning result using the EKF includes:

[0015] Regarding the positioning influence error as the system noise and observation noise of the EKF equation, and incorporating it into the noise covariance matrix of the state equation and the observation noise covariance matrix during the solution process;

[0016] The state prediction equation uses the motion model of the long - range AUV, combines the collaborative positioning result of the relay AUV and the long - range AUV, and the position information and velocity - heading information of the long - range AUV are used as X k-1 and u k-1 of the state prediction equation respectively to solve the equation;

[0017] The state update equation uses the ultra - short baseline positioning result of the main platform and the relay AUV, and combines the positioning influence error to correct the predicted value.

[0018] In a second aspect, the present invention provides an underwater vehicle cooperative navigation and positioning device based on multi - hop relay and dynamic error compensation, which is implemented based on a two - hop cooperative architecture. The two - hop cooperative architecture includes a hierarchical positioning mode of a main platform, a relay AUV, and a long - range AUV. The device is characterized in that it includes:

[0019] A positioning module for performing relative positioning between the main platform and the relay AUV through an ultra - short baseline underwater acoustic positioning system, and performing cooperative positioning between the relay AUV and the long - range AUV through an underwater acoustic communication pure distance measurement method;

[0020] A position estimation module for establishing an underwater acoustic positioning full - error model covering time, lever arm, and sound - speed perturbation to analyze the positioning influence error, and dynamically correcting the positioning result using the EKF to achieve centimeter - level positioning accuracy at a distance of kilometers.

[0021] In some instances, obtain the positioning influence error, where

[0022] ΔX a , ΔY a , ΔZ a represent the comprehensive error without considering the lever - arm information, and ΔX e , ΔY e , ΔZ e represent the cooperative navigation and positioning error caused by the lever - arm measurement error, and θ my , Δθ my , θ mz, Δθ mz , R, c, ΔT, ΔR, respectively represent the y-axis incident angle of the master and slave AUVs, the y-axis incident angle error of the master and slave AUVs, the z-axis incident angle of the master and slave AUVs, the z-axis incident angle error of the master and slave AUVs, the slant range measurement result of the master and slave AUVs, the sound speed in water, the time reference error, the slant range error affected by the water temperature environment, the measurement error of the lever arm information of the ultra-short baseline sound head in the AUV vehicle coordinate system, and the measurement error of the lever arm information of the ultra-short baseline sound beacon in the AUV vehicle coordinate system, represents the rotation matrix from the AUV vehicle coordinate system to the navigation coordinate system, represents the rotation matrix from the main platform vehicle coordinate system to the navigation coordinate system.

[0023] In some instances, by relative positioning is performed between the main platform and the relay AUV, where X a , Y a , Z a represent the position of the ultra-short baseline sound beacon in the array coordinate system.

[0024] In some instances, by cooperative positioning is performed between the relay AUV and the long-range AUV, where Z k represents the observable quantity of the cooperative navigation system based on distance measurement, X A , Y A is the position information of the relay AUV solved by the ultra-short baseline positioning system at time k, v k-1 represents the velocity of the long-range AUV measured by the sensor at time k - 1, (x k , y k ) represents the position information of the long-range AUV at time k.

[0025] In some instances, the position estimation module is specifically configured to use the positioning influence error as the system noise and observation noise of the EKF equation, and incorporate them into the noise covariance matrix of the state equation and the observation noise covariance matrix during the solution process; the state prediction equation uses the motion model of the long-range AUV, combines the cooperative positioning result of the relay AUV and the long-range AUV, and the position information and velocity heading information of the long-range AUV are used as X k-1 and u k-1 of the state prediction equation to solve the equation; the state update equation uses the ultra-short baseline positioning results of the main platform and the relay AUV, and combines the positioning influence error to correct the predicted value.

[0026] Generally speaking, compared with the prior art by the above technical solutions conceived by the present invention, the following beneficial effects can be achieved:

[0027] The AUV cooperative navigation and positioning of the present invention combines multi-field technologies, which can not only analyze the influence of the main errors in the positioning process, but also correct the positioning errors by using the extended Kalman filter algorithm. The ultra-short baseline positioning and pure distance positioning technologies are combined through a relay AUV to achieve combined navigation and positioning, and a long-distance AUV positioning estimation result with high accuracy and reliability is obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, 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 invention. For those skilled in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0029] Figure 1 It is a logic block diagram of the method execution provided by the embodiment of the present invention;

[0030] Figure 2 It is a two-hop cooperative navigation and positioning model provided by the embodiment of the present invention;

[0031] Figure 3 It is a schematic diagram of the lever arm in the cooperative navigation system provided by the embodiment of the present invention;

[0032] Figure 4 It is the positioning principle of the ultra-short baseline system provided by the embodiment of the present invention;

[0033] Figure 5 It is a simulated ocean gradient sound speed profile provided by the embodiment of the present invention;

[0034] Figure 6 It is an AUV path diagram of Scenario 1 provided by the embodiment of the present invention;

[0035] Figure 7 It is an AUV path diagram of Scenario 2 provided by the embodiment of the present invention;

[0036] Figure 8 It is an experimental result diagram of Scenario 1 provided by the embodiment of the present invention;

[0037] Figure 9 It is an experimental result diagram of Scenario 2 provided by the embodiment of the present invention;

[0038] Figure 10 It is a schematic diagram of the device provided by the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0039] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative efforts shall fall within the protection scope of the present invention.

[0040] In the following description, specific embodiments of the present invention will be described with reference to steps and symbols executed by one or more computers, unless otherwise specified. Therefore, these steps and operations will be referred to as being executed by a computer several times. The computer execution referred to herein includes the operation of a computer processing unit that represents data in a structured form by electronic signals. This operation transforms the data or maintains it at a position in the computer's memory system, which can be reconfigured or otherwise changed in a manner well known to those skilled in the art. The data structure maintained by the data is the physical location of the memory, which has specific characteristics defined by the data format. However, the principles of the present invention are described in the above text, which does not represent a limitation. Those skilled in the art will understand that the following various steps and operations can also be implemented in hardware.

[0041] The term "module" or "unit" used herein can be regarded as a software object executed on the computing system. Different components, modules, engines, and services herein can be regarded as implementation objects on the computing system. The devices and methods herein are preferably implemented in software, and of course, they can also be implemented in hardware, all within the protection scope of the present invention.

[0042] Those skilled in the art of this technology can understand that, unless specifically stated otherwise, the singular forms "a", "an", and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of the present invention means the presence of features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or their groups. It should be understood that when we say an element is "connected" or "coupled" to another element, it can be directly connected or coupled to other elements, or there may also be intermediate elements. In addition, the "connection" or "coupling" used herein may include wireless connection or wireless coupling. The phrase "and / or" used herein includes all or any unit and all combinations of one or more related listed items.

[0043] The present invention innovatively introduces a "double-hop cooperation" architecture: a hierarchical positioning mode of the master platform → relay AUV → long-distance slave AUV (operation AUV or reconnaissance AUV), combines the ultra-short baseline (USBL) high-precision relative positioning and pure distance measurement technology, and by establishing an underwater acoustic positioning full error model (covering time, lever arm, sound speed perturbation, etc.), uses the EKF to dynamically correct the positioning result, realizes centimeter-level positioning accuracy at a distance of kilometers, and significantly improves the operation ability and survivability of the long-distance slave AUV in a complex marine environment.

[0044] In the first embodiment of the present invention, a cooperative navigation and positioning method for underwater vehicles based on multi-hop relay and dynamic error compensation is provided, as Figure 1 shown, and includes the following steps:

[0045] S1. Acquisition and generation of AUV positioning data information: Acquire and organize the data information for AUV cooperative navigation and positioning;

[0046] S2. Analysis of cooperative navigation and positioning errors: An underwater acoustic positioning error model is established for the time reference error, lever arm information, hydrological environment, and the relative position between the master platform and the AUV respectively, and the influence on the underwater acoustic positioning accuracy is analyzed respectively;

[0047] S3. Ultra-short baseline positioning between the master platform and the relay AUV: Relative positioning is performed between the master platform and the relay AUV through an ultra-short baseline underwater acoustic positioning system;

[0048] S4. Cooperative positioning between the relay AUV and the long-distance AUV: Cooperative positioning is performed between the relay AUV and the long-distance AUV based on the pure distance measurement direction;

[0049] S5. Double-hop integrated navigation and positioning: According to the positioning influence error and the Extended Kalman Filter (EKF) algorithm, realize distance-assisted integrated navigation and positioning.

[0050] The method of the present invention is described in detail below. In the embodiment of the present invention, the AUV cooperative navigation and positioning method invented is used to realize the positioning estimation of the long-distance AUV.

[0051] In step S1, a scene model of the cooperative navigation system is established, and the data information of each AUV is acquired.

[0052] The main platform carries a high-precision inertial navigation system and assumes that its position is accurately known. The main platform regularly sends its position information to the relay AUV through the carried underwater acoustic communication device. The relative positioning between the relay AUV and the main platform is carried out through an ultra-short baseline underwater acoustic positioning system, which will be affected by factors such as the hydrographic environment and time reference error, resulting in certain errors in the positioning result of the relay AUV; a pure distance cooperation method is adopted between the relay AUV and the long-distance AUV to estimate the position of the long-distance AUV. The two-hop cooperative navigation is as Figure 2 shown.

[0053] In step S2, the distance measurement error modeling of the cooperative navigation system. In the master-slave cooperative navigation system based on distance measurement, the main platform carries a high-precision inertial navigation system and assumes that its position is accurately known. The main platform regularly sends its position information to the slave AUV through the carried underwater acoustic communication device. The main platform and the AUV carry synchronous clocks. Then, when the AUV obtains the position information of the leader, it can obtain the distance information between the AUV and the main platform through the acoustic wave transmission time. However, the underwater environment is complex, and the distance information obtained from underwater acoustic communication and synchronous clocks is affected by various factors. The embodiments of the present invention consider factors such as clock drift and ocean environment influence to model the distance measurement error.

[0054] The time reference error ΔT is the time deviation caused by the relative deviation between the clocks carried by the AUVs, which affects the measured distance. The influence of the hydrographic environment is mainly in two aspects. One is that the actual sound speed is related to the temperature, salinity, and pressure (depth) of the seawater environment. If there is a deviation between the sound speed used in the slant range measurement and the actual sound speed in the hydrographic environment, it will cause slant range measurement error. The other is that the sound speed in the depth profile of the ocean has a changing characteristic, and sound ray bending will occur, resulting in an offset between the incident angle and the distance. When there is no error in the observation quantity of the positioning system, the position of the relay AUV relative to the main platform will not affect the positioning result. Considering the phase difference and the existence of errors in the slant range measurement, the change of the positioning error under different relative positions is considered.

[0055] For the error analysis of the positioning system, since the time reference error, hydrographic environment, and phase difference measurement error directly lead to slant range error and incident angle error, therefore, first, take the incident angle and slant range as variables. Taking the positioning error of Y as an example, for Y a for θ my and R, taking the differential gives:

[0056] ΔY a =-Rsinθ my Δθ my +ΔRcosθ my

[0057] Similarly, it can be obtained:

[0058] ΔZ a = -Rsinθ mz Δθ mz +ΔRcosθ mz

[0059] According to Differentiating it gives:

[0060]

[0061] Substituting ΔY a and ΔZ a yields:

[0062]

[0063] Since R = cT, where the measurement error of the slant range is mainly caused by the time reference error and the hydrographic environment, we can obtain:

[0064] ΔR = cΔT + ΔR(hydrographic environment influence)

[0065] Substituting it into ΔX a , ΔY a , and ΔZ a , the comprehensive error model without considering the lever arm information can be obtained as:

[0066]

[0067] The lever arm information is the position calculation error caused by the inability to install the ultra-short baseline positioning system at the centroid of the manned platform or AUV. The attitude of the main platform is (θ m , γ m , ψ m ). The coordinate representation of the ultra-short baseline array in the main platform carrier coordinate system is The attitude of the AUV is (θ s , γ s , ψ s ). The coordinate representation of the acoustic beacon in the AUV carrier coordinate system is The measurement errors of the lever arm information are respectively denoted as The schematic diagram of the lever arm is as Figure 3 shown.

[0068] The rotation matrix from the main platform carrier coordinate system to the navigation coordinate system can be expressed as:

[0069]

[0070] The rotation matrix from the AUV carrier coordinate system to the navigation coordinate system can be expressed as:

[0071]

[0072] The positioning error caused by ignoring the information of the ultra-short baseline installation lever arm is:

[0073]

[0074] In the case of considering the lever arm information, the cooperative navigation positioning error caused by the lever arm measurement error is:

[0075]

[0076] Adding the above error model to the positioning error caused by the lever arm information gives the comprehensive positioning error model:

[0077]

[0078] The positioning error is:

[0079]

[0080] In step S3, relative positioning is performed between the relay AUV and the main platform through an ultra-short baseline underwater acoustic positioning system. The schematic diagram of the ultra-short baseline system positioning is as Figure 4 shown.

[0081] The formula for solving the position of the transponder in the array coordinate system can be written as:

[0082]

[0083] The formula for solving the position in the ultra-short baseline positioning system can be written as:

[0084]

[0085] In step S4, in the cooperative navigation system, the state variable X k = [x k , y k T is the position information of the long-range AUV at time k. The system input u k = [v k , a k T is the speed and heading information of the long-range AUV measured by the sensor at time k. The state equation of the cooperative navigation system is:

[0086]

[0087] It can be abbreviated as:

[0088] X k = f(X k-1 , u k-1)+w k-1

[0089] The input quantity noise variance matrix is:

[0090]

[0091] The observed quantity of the cooperative navigation system based on distance measurement is the distance between AUVs, and the observation equation is:

[0092]

[0093] where, X A , Y A is the position information of the relay AUV solved by the ultra-short baseline positioning system at time k.

[0094] In step S5, the extended Kalman filter expands the nonlinear system based on the Taylor series, takes the first-order truncation as the approximation of the original state equation and measurement equation, and then applies the Kalman filter.

[0095] Among them, the positioning influence error obtained in step S2 (including errors caused by time reference error, hydrological environment, phase difference measurement, etc.) is the system noise and observation noise of the EKF equation, and is incorporated into the noise covariance matrix Σ k-1 and the observation noise covariance matrix R during the solution process.

[0096] Measurement noise covariance matrix R:

[0097] Linearize the state equation to obtain:

[0098]

[0099] where, F is the Jacobian matrix of f with respect to X k-1 , and Fu is the Jacobian matrix of f with respect to u k-1 . Linearize the measurement equation to obtain the measurement matrix as:

[0100]

[0101] The specific process of the cooperative navigation extended Kalman filter algorithm is as follows:

[0102] The state one-step prediction equation is:

[0103]

[0104] The state prediction equation uses the motion model of the long-distance AUV, combines the long-distance cooperative positioning result in step S4, and the position information (x k , y k) and the speed and heading information (v k , a k ) are used as X of the state prediction equation k-1 and u k-1 respectively to solve the equation.

[0105] The predicted covariance matrix equation is:

[0106]

[0107] When there is no measurement information fusion, the dead reckoning error covariance matrix is transmitted through the above formula. When the AUV obtains distance information, the distance information can be fused through the EKF algorithm.

[0108] The equation for calculating the filter gain is:

[0109]

[0110] The equation for state update calculation is:

[0111]

[0112] The state update equation uses the ultra-short baseline positioning result in step S3 and combines the error model in step S2 to correct the predicted value. Specifically, the positioning result (x A , y A ) of the relay AUV solved in step S3 is the input of the EKF measurement matrix, and the observed value z in the state update equation is incorporated during the solution process k .

[0113] The equation for the estimated covariance matrix is:

[0114]

[0115] The specific simulation and result analysis using the above method are as follows:

[0116] The simulation experiment is carried out in a simulated real ocean environment, and the simulated ocean hydrological condition sound speed profile is as Figure 5 shown.

[0117] Design scenario 1: The main platform is stationary at a depth of 120 meters, with coordinates (0, 0, -120). The relay AUV starts moving from the position (500, -800, -300) at a speed of 2 m / s for 1000 seconds. The heading angle starts at -50° and increases by 0.2° per second to 50°, and then decreases from 50° to -50° at a rate of 0.2° per second. The long-range AUV starts moving from the position (800, 0, -300) and moves in a straight line along the y-axis at a speed of 2 m / s for 1000 seconds. The movement path is as Figure 6 shown.

[0118] Design scenario 2: The main platform is stationary at a depth of 120 meters, with coordinates (0, 0, -120). The relay AUV starts from a point (0, 0, -300) below the main platform and moves along the x-axis at a constant speed of 1.5 m / s for 1000 seconds. The long-distance AUV starts moving from the position (800, 0, -300) and moves along the y-axis at a constant speed of 2 m / s for 1000 seconds. The movement paths are as Figure 7 shown.

[0119] In the above simulation scenario, the time reference error, lever arm information, etc. are the same as those in the comprehensive error analysis section. The standard deviation of the speed measurement error of the long-distance AUV is 0.5 m / s, the standard deviation of the heading angle measurement error is 5°, and the standard deviation of the Gaussian white noise of the ranging error is 0.1 meter.

[0120] In the designed scenario, the relay AUV and the long-distance AUV move in a plane at the same depth. And in practical applications, the depth of the AUV is usually measured by a depth gauge. Therefore, only the plane where the AUV is located is calculated and analyzed, and the results of simulation scenario 1 are as Figure 8 shown, and the result graph of simulation scenario 2 is as Figure 9 shown.

[0121] Analyzing the result graph, Figure 8 (a), Figure 9 In (a), the red line is the actual position of the long-distance AUV, the purple line is the positioning result of the dead reckoning of the long-distance AUV through its own measurement, and the green line is the result obtained by the cooperative navigation system positioning through the distance measurement between AUVs. It can be clearly seen that the position calculated by the cooperative navigation system is closer to the real position. Figure 8 (b), Figure 9 (b) shows the error values of the two position calculation methods. The red line is the positioning error of the cooperative navigation system, which is lower than the error of the dead reckoning of the AUV itself.

[0122] In the second embodiment of the present invention, to facilitate better implementation of the method provided in the embodiment of the present invention, the embodiment of the present invention also provides a device based on the above method. The meanings of the terms are the same as those in the above method, and the specific implementation details can be referred to the description in the method embodiment.

[0123] Please refer to Figure 10 , Figure 10 which is the structural schematic diagram of the device provided in the embodiment of the present invention. The device 1000 may include an error analysis module 1001, a first positioning module 1002, a second positioning module 1003, and a position estimation module 1004, where:

[0124] The device is implemented based on a double-hop cooperative architecture, and the double-hop cooperative architecture includes a hierarchical positioning mode of a main platform, a relay AUV, and a long-range AUV, specifically as follows:

[0125] An error analysis module 1001 is used to establish an underwater acoustic positioning error model for the time reference error, lever arm information, hydrographic environment, and relative position between the main platform and the AUV in the AUV cooperative navigation and positioning data information, so as to analyze the positioning influence error;

[0126] A first positioning module 1002 is used to perform relative positioning between the main platform and the relay AUV through an ultra-short baseline underwater acoustic positioning system;

[0127] A second positioning module 1003 is used to perform cooperative positioning between the relay AUV and the long-range AUV based on a pure distance measurement method;

[0128] A position estimation module 1004 is used to estimate the position of the long-range AUV according to the positioning influence error and the EKF algorithm.

[0129] In some instances, from the positioning influence error is obtained, where

[0130] ΔX a 、ΔY a 、ΔZ a represent the comprehensive error without considering the lever arm information, and ΔX e 、ΔY e 、ΔZ e represent the cooperative navigation and positioning error caused by the lever arm measurement error, and θ my 、Δθ my 、θ mz 、Δθ mz 、R, c, ΔT, ΔR, respectively represent the y-axis incident angle of the master-slave AUVs, the y-axis incident angle error of the master-slave AUVs, the z-axis incident angle of the master-slave AUVs, the z-axis incident angle error of the master-slave AUVs, the slant range measurement result of the master-slave AUVs, the sound speed in water, the time reference error, the slant range error affected by the water temperature environment, the lever arm information measurement error of the ultra-short baseline sound head in the AUV body coordinate system, and the lever arm information measurement error of the ultra-short baseline sound beacon in the AUV body coordinate system. represents the rotation matrix from the AUV body coordinate system to the navigation coordinate system, represents the rotation matrix from the main platform body coordinate system to the navigation coordinate system.

[0131] In some instances, from relative positioning is performed between the main platform and the relay AUV, where X a 、Y a 、Za Indicates the position of the ultra-short baseline acoustic beacon in the array coordinate system.

[0132] In some instances, Cooperative positioning is performed between the relay AUV and the long-range AUV. Among them, Z k Indicates the observation quantity of the cooperative navigation system based on distance measurement, and X A , Y A Is the position information of the relay AUV solved by the ultra-short baseline positioning system at time k, and v k-1 Indicates the velocity of the long-range AUV measured by the sensor at time k-1. (x k , y k ) Indicates the position information of the long-range AUV at time k.

[0133] In some instances, the above-mentioned position estimation module 1004 is specifically configured to use the positioning influence error as the system noise and observation noise of the EKF equation, and incorporate it into the noise covariance matrix of the state equation and the observation noise covariance matrix during the solution process; the state prediction equation uses the motion model of the long-range AUV, combines the cooperative positioning results of the relay AUV and the long-range AUV, and the position information and velocity and heading information of the long-range AUV are used as X k-1 and u k-1 To solve the equation; the state update equation uses the ultra-short baseline positioning results of the main platform and the relay AUV, and combines the positioning influence error to correct the predicted value.

[0134] The above has introduced in detail a cooperative navigation and positioning method and device for an underwater vehicle based on multi-hop relay and dynamic error compensation. In this article, specific examples are used to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A method for cooperative navigation and positioning of underwater vehicles based on multi-hop relay and dynamic error compensation, characterized in that: include: Constructing a dual-hop collaborative architecture, wherein the dual-hop collaborative architecture includes a hierarchical positioning mode of a main platform, a relay AUV, and a long-range AUV; The main platform and the relay AUV are relatively positioned by the ultra-short baseline hydroacoustic positioning system, and the relay AUV and the long-distance AUV are collaboratively positioned by pure distance measurement through hydroacoustic communication. A full error model of underwater acoustic positioning covering time, arm and sound speed disturbance is established to analyze the positioning error, and the EKF is used to dynamically correct the positioning result to achieve meter-level positioning accuracy at kilometer-level distances.

2. The method according to claim 1, characterized in that: Depend on The positioning influence error is obtained, where ΔX a , ΔY a , ΔZ a represents the comprehensive error without considering the lever arm information, ΔX e , ΔY e , ΔZ e represents the collaborative navigation positioning error caused by the arm measurement error, θ my , Δθ my ,θ mz , Δθ mz ,R,c,ΔT,ΔR, They respectively represent the y-axis incident angle of the master and slave AUV, the y-axis incident angle error of the master and slave AUV, the z-axis incident angle of the master and slave AUV, the z-axis incident angle error of the master and slave AUV, the slant range measurement results of the master and slave AUV, the water sound speed, the time reference error, the slant range error affected by the water temperature environment, the arm information measurement error of the ultra-short baseline acoustic head in the unmanned underwater vehicle carrier coordinate system, and the arm information measurement error of the ultra-short baseline acoustic beacon in the unmanned underwater vehicle carrier coordinate system. Represents the rotation matrix from the AUV carrier coordinate system to the navigation coordinate system, Represents the rotation matrix from the main platform carrier coordinate system to the navigation coordinate system.

3. The method according to claim 2, characterized in that Depend on Relative positioning between the main platform and the relay AUV, where X a , Y a , Z a Represents the position information of the ultra-short baseline acoustic beacon in the array coordinate system.

4. The method according to claim 3, characterized in that: Depend on Coordinated positioning between the relay AUV and the long-distance AUV, where Z k represents the observation quantity of the cooperative navigation system based on distance measurement, X A , Y A is the position information of the relay AUV calculated by the ultra-short baseline positioning system at time k, v k-1 represents the speed of the long-distance AUV measured by the sensor at time k-1, (x k ,y k ) represents the position information of the long-distance AUV at time k.

5. The method according to claim 4, characterized in that The method of dynamically correcting the positioning result using EKF includes: The positioning error is regarded as the system noise and observation noise of the EKF equation, and is included in the noise covariance matrix and observation noise covariance matrix of the state equation during the solution process. The state prediction equation uses the motion model of the long-range AUV, combined with the collaborative positioning results of the relay AUV and the long-range AUV, and the long-range AUV position information and speed and heading information are used as the X k-1 and u k-1 To solve equations; The state update equation uses the ultra-short baseline positioning results of the main platform and the relay AUV, and combines the positioning effect error to correct the predicted value.

6. A master-slave AUV dual-hop collaborative navigation and positioning device based on EKF, implemented based on a dual-hop collaborative architecture, wherein the dual-hop collaborative architecture includes a hierarchical positioning mode of a master platform, a relay AUV, and a long-distance AUV, characterized in that: The device comprises: Positioning module, used to perform relative positioning between the main platform and the relay AUV through the ultra-short baseline hydroacoustic positioning system, and to perform collaborative positioning between the relay AUV and the long-range AUV through pure distance measurement of hydroacoustic communication; The position estimation module is used to establish a full error model of underwater acoustic positioning covering time, arm and sound speed disturbance to analyze the positioning error, and use EKF to dynamically correct the positioning results to achieve meter-level positioning accuracy at kilometer-level distances.

7. The device according to claim 6, characterized in that Depend on The positioning influence error is obtained, where ΔX a , ΔY a , ΔZ a represents the comprehensive error without considering the lever arm information, ΔX e , ΔY e , ΔZ e represents the collaborative navigation positioning error caused by the arm measurement error, θ my , Δθ my ,θ mz , Δθ mz ,R,c,ΔT,ΔR, They respectively represent the y-axis incident angle of the master and slave AUV, the y-axis incident angle error of the master and slave AUV, the z-axis incident angle of the master and slave AUV, the z-axis incident angle error of the master and slave AUV, the slant range measurement results of the master and slave AUV, the water sound speed, the time reference error, the slant range error affected by the water temperature environment, the arm information measurement error of the ultra-short baseline acoustic head in the unmanned underwater vehicle carrier coordinate system, and the arm information measurement error of the ultra-short baseline acoustic beacon in the unmanned underwater vehicle carrier coordinate system. Represents the rotation matrix from the AUV carrier coordinate system to the navigation coordinate system, Represents the rotation matrix from the main platform carrier coordinate system to the navigation coordinate system.

8. The device according to claim 7, characterized in that Depend on Relative positioning between the main platform and the relay AUV, where X a , Y a , Z a Indicates the position of the ultra-short baseline acoustic beacon in the array coordinate system.

9. The device according to claim 8, characterized in that Depend on The relay AUV and the long-distance AUV are coordinated to locate each other. k represents the observation quantity of the cooperative navigation system based on distance measurement, X A , Y A is the position information of the relay AUV calculated by the ultra-short baseline positioning system at time k, v k-1 represents the speed of the long-distance AUV measured by the sensor at time k-1, (x k ,y k ) represents the position information of the long-distance AUV at time k.

10. The device according to claim 9, characterized in that The position estimation module is specifically used to treat the positioning influence error as the system noise and observation noise of the EKF equation, and incorporate it into the noise covariance matrix and the observation noise covariance matrix of the state equation during the solution process; the state prediction equation uses the motion model of the long-range AUV, combines the collaborative positioning results of the relay AUV and the long-range AUV, and the long-range AUV position information and speed and heading information are respectively used as the X k-1 and u k-1 Used to solve the equation; the state update equation uses the ultra-short baseline positioning results of the main platform and the relay AUV, and combines the positioning influence error to correct the predicted value.

Images