AUV guidance and docking system and method based on rotating current field and electromagnetic data fusion

Abstract

The present invention discloses an AUV guidance and docking system and method based on the fusion of rotating current field and electromagnetic data, belonging to the field of underwater vehicle navigation technology. The AUV guidance and docking system includes a transmitting system arranged at a docking base station and a receiving system arranged on the AUV. The transmitting system includes a plurality of transmitting electrodes evenly distributed around the circumference, which generates a rotating current field and a magnetic field by connecting a sinusoidal voltage with a phase difference; the receiving system includes 4 groups of differential electrodes for receiving the voltage signal generated by the rotating current field and a fluxgate sensor for receiving the magnetic field signal generated by the rotating current field. The present invention uses a rotating current field to simultaneously generate an electric field and a magnetic field, and performs joint positioning by fusing the amplitude characteristics of the electric field signal with the phase characteristics of the magnetic field signal, thereby overcoming the defect that the traditional single-field guidance method is susceptible to environmental interference, and has the advantages of high precision and fast response speed.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater vehicle navigation, and in particular relates to an AUV guidance and docking system and method based on the fusion of rotating current field and electromagnetic data. Background Art

[0002] As the core equipment in the field of deep-sea exploration, the performance indicators of the navigation and positioning system of underwater unmanned vehicles (AUVs) directly determine the effectiveness of deep-sea exploration operations.

[0003] Existing navigation systems primarily include acoustic, optical, and inertial navigation systems. While acoustic navigation systems offer advantages in environmental adaptability, multipath-induced signal attenuation and positioning accuracy degradation are particularly problematic in complex hydrological environments. Optical navigation systems are limited by the turbidity characteristics of the medium and are susceptible to light scattering and absorption in turbid waters. While inertial navigation systems offer strong environmental robustness, their accumulated drift errors make them unable to maintain the accuracy required for long-term operations.

[0004] Electromagnetic guidance technology, with its insensitivity to medium turbidity and anti-multipath interference characteristics, can effectively overcome the shortcomings of the above-mentioned guidance methods. In recent years, it has become a research hotspot for breaking through the bottleneck of navigation accuracy. For example, the Chinese invention patent application with publication number CN108680170A discloses an AUV docking navigation system and method based on the principle of electromagnetic wave attenuation. The system includes an electromagnetic wave receiving module, an electromagnetic signal analysis module, an industrial computer, and an underwater wireless sensor network. It can achieve high-precision navigation in a small range and is not affected by water quality, environmental noise, etc. However, single-mode physical field guidance docking that relies solely on electromagnetic guidance is easily affected by the mixed interference of multiple physical fields and has poor robustness. It is often difficult to achieve good spatial resolution and field uniformity in dynamic underwater scenes. Summary of the Invention

[0005] The present invention proposes an AUV guidance and docking system and method based on the fusion of rotating current field and electromagnetic data, the purpose of which is to solve the problems of susceptibility to interference and low accuracy of traditional single physical field guidance and docking methods.

[0006] The technical solutions of the present invention are as follows:

[0007] An AUV guidance and docking system based on the fusion of rotating current fields and electromagnetic data includes a transmitting system installed at a docking base station and a receiving system installed on the AUV;

[0008] A fixed spatial coordinate system is established with the docking center of the docking base station as the origin, and the Z axis of the fixed spatial coordinate system is consistent with the docking centerline direction;

[0009] The transmitting system includes a plurality of axially spaced ... The transmitting electrode, is an even number greater than or equal to 6;

[0010] The The transmitting electrodes are connected to a sine wave voltage with equal amplitude, consistent frequency and equal phase difference; in the XOY plane of the fixed spatial coordinate system, along the counterclockwise or clockwise direction, among any two adjacent transmitting electrodes, the voltage phase difference between the latter transmitting electrode and the former transmitting electrode is set to , thereby generating a rotating current field, and at the same time the rotating current field generates a magnetic field;

[0011] A moving space coordinate system is established with the center of the AUV front end as the origin, and the Z axis of the moving space coordinate system coincides with the axis of the AUV;

[0012] The receiving system includes a receiving electrode and a fluxgate sensor;

[0013] The receiving electrodes include a plurality of differential electrodes uniformly distributed around the Z-axis of the mobile space coordinate system. Each set of differential electrodes includes two electrodes: a front electrode located at the front of the AUV and a rear electrode located directly behind the front electrode and at a fixed distance. The differential electrodes are used to receive the voltage signal generated by the rotating current field.

[0014] The fluxgate sensor is installed at the origin of the moving space coordinate system and is used to receive the magnetic field signal generated by the rotating current field.

[0015] As a further improvement of the AUV guidance and docking system based on the fusion of rotating current field and electromagnetic data: there are 4 groups of differential electrodes;

[0016] Observe along the Z-axis direction of the mobile space coordinate system, starting from the positive half axis of X, number the four front electrodes in a counterclockwise direction as a, b, c, d, and the corresponding four rear electrodes in a counterclockwise direction as e, f, g, h ... The front electrodes and numbered The amplitude of the voltage signal of the differential electrode composed of the rear electrode is recorded as , the phase is recorded as ;

[0017] The amplitudes of the components of the magnetic field signal received by the fluxgate sensor along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are respectively recorded as 、 and The phases of the components of the magnetic field signal along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are recorded as 、 and .

[0018] The present invention also discloses an AUV guidance and docking method based on the above-mentioned AUV guidance and docking system based on the fusion of rotating current field and electromagnetic data: the three-dimensional coordinates of the AUV in the fixed space coordinate system are estimated in real time through the voltage signal obtained by the differential electrode and the magnetic field signal obtained by the fluxgate sensor; after each three-dimensional coordinate is estimated, the movement direction of the AUV is adjusted according to the current three-dimensional coordinate and the preset guidance path, so that the AUV is further approached to the docking center along the guidance path.

[0019] As a further improvement to the AUV guidance and docking method, the current three-dimensional coordinates of the AUV in a fixed space coordinate system are estimated based on the voltage signal obtained by the differential electrode and the magnetic field signal obtained by the fluxgate sensor as follows:

[0020] Step 1: Calculate the angle estimate of the AUV relative to the fixed space coordinate system based on the amplitude of the voltage signal of the differential electrode and the phase of the magnetic field signal of the fluxgate sensor. ; Assume that the projection point of the current AUV front center on the XOY plane of the fixed space coordinate system is A, then the angle estimation value is It refers to the vector on the XOY plane of the fixed space coordinate system The angle relative to the positive X-axis of the fixed space coordinate system;

[0021] Step 2: Calculate the estimated distance between the projection point A of the AUV and the origin O in the XOY plane of the fixed spatial coordinate system based on the phase of the voltage signal of the differential electrode. ;

[0022] Step 3: Estimated value based on angle and distance estimates Calculate the X-axis coordinate of the AUV in the fixed space coordinate system and Y-axis coordinate , the amplitude and distance estimation of the magnetic field signal based on the fluxgate sensor Calculate the Z-axis coordinate of the AUV in the fixed space coordinate system .

[0023] As a further improvement of the AUV guided docking method: in step 1, the angle estimation value The calculation method is as follows: the first angle estimation value is calculated based on the amplitude of the voltage signal of the differential electrode , the second angle estimation value is calculated based on the phase of the magnetic field signal of the fluxgate sensor , the first angle estimate and the second angle estimate The mean of the angle is taken as the estimated value .

[0024] As a further improvement of the AUV guided docking method, the first angle estimation value The calculation method is:

[0025] ;

[0026] in, 、 、 and are the amplitudes of the voltage signals of the four groups of differential electrodes.

[0027] As a further improvement of the AUV guided docking method, the second angle estimation value The calculation method is:

[0028] .

[0029] As a further improvement to the AUV guidance docking method, the distance estimation value in step 2 The calculation method is:

[0030] Step 2-1: Calculate the proportional relationship based on the phase of the voltage signal of the differential electrode :

[0031] ;

[0032] in, 、 、 and are the phases of the voltage signals of the four groups of differential electrodes;

[0033] Step 2-2: The distance estimate determined by the last calculation of the three-dimensional coordinates and the cross-sectional radius of the AUV The size relationship determines the estimated value of this distance How to calculate ?

[0034] Define the first calculation method: ;

[0035] Define the second calculation method: ;

[0036] If this is the first time the distance estimate is calculated , then directly select the pre-set default calculation method;

[0037] if , then the first calculation method is adopted;

[0038] if , then the second calculation method is adopted;

[0039] if , then switch to a calculation method different from the last time;

[0040] is the preset threshold;

[0041] Step 2-3: Calculate the estimated distance value according to the determined calculation method .

[0042] As a further improvement of the AUV guidance and docking method: in step 2-2, the second calculation method is used as the default calculation method.

[0043] As a further improvement to the AUV guided docking method:

[0044] In step 3, the X-axis coordinate The calculation method is:

[0045] ;

[0046] In step 3, the Y-axis coordinate The calculation method is:

[0047] ;

[0048] In step 3, the Z-axis coordinate The calculation method is:

[0049] .

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

[0051] 1. This invention uses a rotating current field to simultaneously generate electric and magnetic fields. By integrating the amplitude characteristics of the electric field signal with the phase characteristics of the magnetic field signal, it performs joint positioning, overcoming the susceptibility of traditional single-field guidance methods to environmental interference. Experiments have shown that using different guidance paths and varying signal-to-noise ratios, the root mean square error (RMSE) gradually decreases and stabilizes with decreasing guidance distance, meeting the requirements for high-precision docking.

[0052] 2. The present invention calculates the angle estimation values ​​based on the electric field amplitude and magnetic field phase respectively and then takes the average, which effectively suppresses the positioning deviation caused by the distortion of a single physical field signal and significantly improves the positioning accuracy of the AUV in complex underwater environments.

[0053] 3. The present invention uses the last distance estimation value and AUV cross-sectional radius The geometric relationship between the two, dynamically select the appropriate calculation method to quickly solve the distance estimate ; At the same time, the vertical distance is directly estimated by combining the spatial attenuation characteristics of the magnetic field amplitude This method only requires simple algebraic operations, does not require iterative optimization, has a small amount of calculation, can meet the millisecond-level real-time positioning requirements, and ensures the dynamic response speed of the guidance process.

[0054] 4. The transmitting system only requires an electrode array evenly distributed around the circumference, while the receiving system adopts a compact layout of differential electrodes and fluxgate sensors, which does not require complex hardware and has the advantages of simple structure and low cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 Schematic diagram of the structure of the guided docking system of the present invention;

[0056] Figure 2 This is a spatial schematic diagram of three different docking guide paths: a straight line, a parabola, and a spiral line in a specific embodiment;

[0057] Figure 3 : This is a mean square error diagram of the AUV moving along the straight docking guidance path when the signal-to-noise ratio is 30dB, 25dB, and 20dB respectively in a specific embodiment. The horizontal axis is the guidance distance, that is, the Euclidean distance between the center of the AUV front end and the docking center, and the vertical axis is the mean square error RMSE of the Euclidean distance;

[0058] Figure 4 : This is a mean square error diagram of the AUV moving along the parabolic docking guidance path when the signal-to-noise ratio is 30 dB, 25 dB, and 20 dB respectively in a specific embodiment. The horizontal axis is the guidance distance, that is, the Euclidean distance between the front center of the AUV and the docking center, and the vertical axis is the mean square error RMSE of the Euclidean distance.

[0059] Figure 5 Figure 3 is a mean square error diagram of the AUV moving along the helical docking guidance path when the signal-to-noise ratio is 30 dB, 25 dB, and 20 dB, respectively. The horizontal axis is the guidance distance, that is, the Euclidean distance between the front center of the AUV and the docking center, and the vertical axis is the mean square error (RMSE) of the Euclidean distance.

[0060] Reference numerals include:

[0061] 1. Rear electrode; 2. Front electrode; 3. Fluxgate sensor; 4. Emitting electrode. DETAILED DESCRIPTION

[0062] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the embodiments described are only a part of the embodiments of the present invention, rather than all the embodiments.

[0063] Example 1

[0064] like Figure 1 , an AUV guidance and docking system based on the fusion of rotating current field and electromagnetic data, including a transmitting system arranged at a docking base station and a receiving system arranged on the AUV.

[0065] A fixed space coordinate system is established with the docking center of the docking base station as the origin, and the Z axis of the fixed space coordinate system is consistent with the docking centerline direction.

[0066] The transmitting system includes a plurality of axially spaced ... There are 4 emitting electrodes. is an even number greater than or equal to 6.

[0067] The Each transmitting electrode 4 is connected to a sine wave voltage with equal amplitude, consistent frequency and equal phase difference. In the XOY plane of the fixed spatial coordinate system, along the counterclockwise or clockwise direction, the voltage phase difference between the latter transmitting electrode 4 and the former transmitting electrode 4 is set to , thereby generating a rotating current field, and at the same time the rotating current field generates a magnetic field.

[0068] In this embodiment, , the emitting electrode 4 located on the positive half axis of the X-axis of the fixed space coordinate system is recorded as the first emitting electrode 4, and the emitting electrodes 4 are numbered in sequence in the counterclockwise direction, and the voltages of the eight emitting electrodes 4 are recorded as , then:

[0069] ;

[0070] ;

[0071] ;

[0072] …

[0073] ;

[0074] .

[0075] A moving space coordinate system is established with the front center of the AUV as the origin, and the Z axis of the moving space coordinate system coincides with the axis of the AUV.

[0076] The receiving system includes a receiving electrode and a fluxgate sensor 3 .

[0077] The receiving electrodes consist of four sets of differential electrodes evenly distributed around the Z-axis of the moving space coordinate system. Each set consists of two electrodes: a front electrode 2 located at the front of the AUV and a rear electrode 1 located directly behind the front electrode 2 and at a fixed distance. These differential electrodes are used to receive the voltage signal generated by the rotating current field. The receiving electrodes are mounted on a cylindrical frame on the outer surface of the AUV.

[0078] Observe along the Z-axis direction of the moving space coordinate system, starting from the positive half axis of X, number the four front electrodes 2 in a counterclockwise direction as a, b, c, d, and the corresponding four rear electrodes 1 in a counterclockwise direction as e, f, g, h, and the numbered The front electrodes 2 and numbered The amplitude of the voltage signal of the differential electrode composed of the rear electrode 1 is recorded as , the phase is recorded as .

[0079] The fluxgate sensor 3 is installed at the origin of the moving space coordinate system and is used to receive the magnetic field signal generated by the rotating current field. The amplitudes of the components of the magnetic field signal received by the fluxgate sensor 3 along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are respectively recorded as 、 and The phases of the components of the magnetic field signal along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are recorded as 、 and .

[0080] The amplitude and phase of the above voltage signal and magnetic field signal can be obtained through Fourier transform and other methods, which will not be described in detail here.

[0081] Example 2

[0082] This embodiment discloses an AUV guidance and docking method, which is based on the AUV guidance and docking system of the first embodiment.

[0083] This method uses the voltage signals obtained from the differential electrodes and the magnetic field signals from the fluxgate sensor 3 to estimate the AUV's current three-dimensional coordinates in a fixed spatial coordinate system in real time. After each three-dimensional coordinate is estimated, the AUV's movement direction is adjusted based on the current three-dimensional coordinates and the preset guidance path, so that the AUV moves closer to the docking center along the guidance path.

[0084] Specifically, the current three-dimensional coordinates of the AUV in the fixed space coordinate system are estimated according to the voltage signal obtained by the differential electrode and the magnetic field signal obtained by the fluxgate sensor 3 as follows:

[0085] Step 1: Calculate the estimated angle of the AUV relative to the fixed space coordinate system based on the amplitude of the voltage signal of the differential electrode and the phase of the magnetic field signal of the fluxgate sensor 3. Assume that the projection point of the current AUV front center on the XOY plane of the fixed space coordinate system is A, then the angle estimation value is It refers to the vector on the XOY plane of the fixed space coordinate system The angle relative to the positive X-axis of the fixed space coordinate system.

[0086] Angle estimate The calculation method is as follows: the first angle estimation value is calculated based on the amplitude of the voltage signal of the differential electrode , the second angle estimation value is calculated based on the phase of the magnetic field signal of the fluxgate sensor 3 , the first angle estimate and the second angle estimate The mean of the angle is taken as the estimated value .

[0087] Specifically, the first angle estimate The calculation method is:

[0088] ;

[0089] in, 、 、 and are the amplitudes of the voltage signals of the four groups of differential electrodes.

[0090] Second angle estimate The calculation method is:

[0091] .

[0092] in, 、 and They are respectively the phases of the components of the magnetic field signal of the fluxgate sensor 3 along the X-axis, Y-axis, and Z-axis of the moving space coordinate system.

[0093] Step 2: Calculate the estimated distance between the projection point A of the AUV and the origin O in the XOY plane of the fixed spatial coordinate system based on the phase of the voltage signal of the differential electrode. .

[0094] The calculation method is:

[0095] Step 2-1: Calculate the proportional relationship based on the phase of the voltage signal of the differential electrode :

[0096] ;

[0097] in, 、 、 and They are the phases of the voltage signals of the four groups of differential electrodes.

[0098] Step 2-2: The distance estimate determined by the last calculation of the three-dimensional coordinates and the cross-sectional radius of the AUV The size relationship determines the estimated value of this distance Calculation method.

[0099] Define the first calculation method: ;

[0100] Define the second calculation method: .

[0101] If this is the first time the distance estimate is calculated , then directly select the pre-set default calculation method. In this embodiment, the second calculation method is used as the default calculation method.

[0102] if , then the first calculation method is adopted;

[0103] if , then the second calculation method is adopted;

[0104] if , it switches to a calculation method different from the last time.

[0105] is the preset threshold.

[0106] Step 2-3: Calculate the estimated distance value according to the determined calculation method .

[0107] Step 3: Estimated value based on angle and distance estimates Calculate the X-axis coordinate of the AUV in the fixed space coordinate system and Y-axis coordinate , based on the amplitude and distance estimation of the magnetic field signal of the fluxgate sensor 3 Calculate the Z-axis coordinate of the AUV in the fixed space coordinate system .

[0108] Specifically, the X-axis coordinate The calculation method is:

[0109] ;

[0110] Y-axis coordinate The calculation method is:

[0111] ;

[0112] Z-axis coordinate The calculation method is:

[0113] ;

[0114] in, 、 and are the amplitudes of the components of the magnetic field signal of the fluxgate sensor 3 along the X-axis, Y-axis, and Z-axis of the moving space coordinate system, respectively.

[0115] Example 3

[0116] This embodiment verifies the effectiveness and superiority of the system and method through simulation and comparative experiments.

[0117] Assume that the docking scenario is dominated by a conductivity of S / m of seawater surrounding the base station 16 emitting electrodes are arranged equidistantly on the docking plane of m ( ), the loading current amplitude of each transmitting electrode is 5A, and the transmitting frequency is set to 200Hz. The cross-sectional radius of the AUV to be guided and docked is m, the distance between the front electrode and the back electrode m, the signal sampling frequency is set to kHz, the number of sampling snapshots is set to 200. Without loss of generality, we select Figure 2 The three different types of curves (straight line, parabola, and spiral) that asymptotically approach the docking center are shown as the guided docking paths for the three experimental groups. Gaussian white noise was added to simulate environmental interference during the simulations, with signal-to-noise ratios set to 30dB, 25dB, and 20dB. The Monte Carlo method was used for 10,000 repeated experiments, with the root mean square error (RMSE) between the Euclidean distance between the AUV front center and the docking center and the theoretical distance corresponding to the ideal guidance path used as the positioning accuracy evaluation metric.

[0118] Figures 3 to 5 The following plots show the mean squared error for three different guidance paths, each containing three signal-to-noise ratio (SNR) scenarios. As the guidance progresses, the distance between the AUV's front center and the docking center gradually decreases. Therefore, as time passes, the root mean square error (RMSE) decreases as viewed from right to left along the horizontal axis. Specific positioning error results are shown in Table 1 below, where the values ​​are in meters.

[0119] Table 1 – Positioning error results.

[0120]

[0121] According to the above results, it can be seen that the method can obtain a relatively accurate AUV spatial position under different signal-to-noise ratios and different guidance and docking paths, and the positioning error meets the requirements.

[0122] It should be noted that it is obvious to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention. The scope of the present invention is defined by the claims rather than the foregoing description.

Claims

1. A guided docking method for an AUV, characterized by: The AUV guidance and docking method is implemented by an AUV guidance and docking system based on the fusion of rotating current field and electromagnetic data; The AUV guidance and docking system based on the fusion of rotating current field and electromagnetic data includes a transmitting system arranged at a docking base station and a receiving system arranged on the AUV; A fixed spatial coordinate system is established with the docking center of the docking base station as the origin, and the Z axis of the fixed spatial coordinate system is consistent with the docking centerline direction; The transmitting system includes a plurality of axially spaced ... emitting electrodes (4), is an even number greater than or equal to 6; The The transmitting electrodes (4) are connected to a sine wave voltage with equal amplitude, consistent frequency and equal phase difference; in the XOY plane of the fixed spatial coordinate system, along the counterclockwise or clockwise direction, among any two adjacent transmitting electrodes (4), the voltage phase difference between the latter transmitting electrode (4) and the former transmitting electrode (4) is set to , thereby generating a rotating current field, and at the same time the rotating current field generates a magnetic field; A moving space coordinate system is established with the center of the AUV front end as the origin, and the Z axis of the moving space coordinate system coincides with the axis of the AUV; The receiving system includes a receiving electrode and a fluxgate sensor (3); The receiving electrodes include a plurality of differential electrodes uniformly distributed around the Z-axis circumference of the mobile space coordinate system, and each group of differential electrodes includes two electrodes: a front electrode (2) located in front of the AUV, and a rear electrode (1) located directly behind the front electrode (2) and at a fixed distance from the front electrode (1); the differential electrodes are used to receive the voltage signal generated by the rotating current field; There are 4 groups of differential electrodes; observing along the Z-axis direction of the moving space coordinate system, starting from the positive X-axis, the 4 front electrodes (2) are numbered as a, b, c, d in the counterclockwise direction, and the corresponding 4 rear electrodes (1) are numbered as e, f, g, h in the counterclockwise direction, and the numbers are The front electrode (2) and the number The amplitude of the voltage signal of the differential electrode composed of the rear electrode (1) is recorded as , the phase is recorded as ; The fluxgate sensor (3) is installed at the origin of the moving space coordinate system and is used to receive the magnetic field signal generated by the rotating current field; The amplitudes of the components of the magnetic field signal received by the fluxgate sensor (3) along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are respectively recorded as 、 and The phases of the components of the magnetic field signal along the X-axis, Y-axis, and Z-axis of the moving space coordinate system are recorded as 、 and ; The AUV guided docking method estimates the current three-dimensional coordinates of the AUV in a fixed space coordinate system in real time through the voltage signal obtained by the differential electrode and the magnetic field signal obtained by the fluxgate sensor (3); after each three-dimensional coordinate is estimated, the moving direction of the AUV is adjusted according to the current three-dimensional coordinate and the preset guidance path, so that the AUV is further approached to the docking center along the guidance path; The way to estimate the current three-dimensional coordinates of the AUV in the fixed space coordinate system based on the voltage signal obtained by the differential electrode and the magnetic field signal obtained by the fluxgate sensor (3) is: Step 1: Calculate the estimated angle of the AUV relative to the fixed space coordinate system based on the amplitude of the voltage signal of the differential electrode and the phase of the magnetic field signal of the fluxgate sensor (3). ; Assume that the projection point of the current AUV front center on the XOY plane of the fixed space coordinate system is A, then the angle estimation value is It refers to the vector on the XOY plane of the fixed space coordinate system The angle relative to the positive X-axis of the fixed space coordinate system; In step 1, the angle estimate The calculation method is as follows: the first angle estimation value is calculated based on the amplitude of the voltage signal of the differential electrode , a second angle estimation value is calculated based on the phase of the magnetic field signal of the fluxgate sensor (3) , the first angle estimate and the second angle estimate The mean of the angle is taken as the estimated value ; Step 2: Calculate the estimated distance between the projection point A of the AUV and the origin O in the XOY plane of the fixed spatial coordinate system based on the phase of the voltage signal of the differential electrode. ; Step 3: Estimated value based on angle and distance estimates Calculate the X-axis coordinate of the AUV in the fixed space coordinate system and Y-axis coordinate , the amplitude and distance estimation of the magnetic field signal based on the fluxgate sensor (3) Calculate the Z-axis coordinate of the AUV in the fixed space coordinate system .

2. The AUV guided docking method according to claim 1, wherein: First angle estimate The calculation method is: ； in, 、 、 and are the amplitudes of the voltage signals of the four groups of differential electrodes.

3. The AUV guidance and docking method according to claim 1, characterized in that: Second angle estimate The calculation method is: 。 4. The AUV guidance and docking method according to claim 1, wherein: The distance estimate in step 2 The calculation method is: Step 2-1: Calculate the proportional relationship based on the phase of the voltage signal of the differential electrode : ； in, 、 、 and are the phases of the voltage signals of the four groups of differential electrodes; Step 2-2: The distance estimate determined by the last calculation of the three-dimensional coordinates and the cross-sectional radius of the AUV The size relationship determines the estimated value of this distance How to calculate ? Define the first calculation method: ; Define the second calculation method: ; If this is the first time the distance estimate is calculated , then directly select the pre-set default calculation method; if , then the first calculation method is adopted; if , then the second calculation method is adopted; if , then switch to a calculation method different from the last time; is the preset threshold; Step 2-3: Calculate the estimated distance value according to the determined calculation method .

5. The AUV guidance and docking method according to claim 4, characterized in that: In step 2-2, the second calculation method is used as the default calculation method.

6. The AUV guidance and docking method according to claim 1, wherein: In step 3, the X-axis coordinate The calculation method is: ； In step 3, the Y-axis coordinate The calculation method is: ； In step 3, the Z-axis coordinate The calculation method is: 。

Images