An underwater autonomous vehicle tracking method based on WLS-UKF
By using the WLS-UKF method, combined with TDOA measurement and ray tracing to correct acoustic ray bending and optimize UKF initialization, the problems of acoustic ray bending and clock asynchrony in AUV tracking are solved, achieving high-precision and stable AUV tracking.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- HUBEI UNIV OF TECH
- Filing Date
- 2026-01-19
- Publication Date
- 2026-05-29
AI Technical Summary
Existing AUV tracking technologies suffer from acoustic ray bending modeling errors, measurement errors caused by clock asynchrony, and insufficient UKF initial state estimation accuracy, all of which affect tracking accuracy and stability.
The WLS algorithm is used to obtain the initial state of the AUV, and the UKF is used for continuous tracking. The TDOA measurement model and ray tracing method are used to correct the sound ray bending, eliminate the influence of clock asynchrony, and optimize the UKF initialization.
It significantly improves the accuracy and stability of AUV tracking, solves the measurement errors caused by acoustic ray bending and clock asynchrony, and ensures the stability and accuracy of the filtering process.
Smart Images
Figure CN122109998A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of underwater positioning technology, and in particular to a tracking method for underwater autonomous vehicles based on WLS-UKF. Background Technology
[0002] Continuous tracking of Autonomous Underwater Vehicles (AUVs) is a prerequisite for the successful execution of critical underwater missions such as marine resource exploration, underwater environmental monitoring, and underwater rescue. Currently, AUV tracking tasks mainly rely on underwater acoustic sensor networks. Sensor nodes deployed within these networks interact with the AUV in real time to achieve continuous tracking. However, the complexity of the underwater environment, such as the sound ray bending effect, introduces significant errors into traditional measurement models based on the straight-line propagation assumption. Clock asynchrony issues disrupt the signal synchronization between underwater nodes and the AUV, affecting the validity of measurement data. Both factors severely restrict the improvement of tracking accuracy. Furthermore, the widely used unscented Kalman filter (UKF) in existing tracking methods relies on prior information about the initial state. If errors exist in parameters such as position and velocity during the initialization phase, it can easily lead to filter divergence, further limiting tracking accuracy and stability.
[0003] In the prior art, application publication number CN12043664A, entitled "A Three-Dimensional Underwater Target Tracking Method Based on Improved Particle Filtering," describes a method that constructs a tracking and measurement model for underwater three-dimensional targets. It employs an improved unscented Kalman filter and particle filter fusion algorithm to build an importance density function and introduces a dynamic adaptive hierarchical weighting factor to improve tracking accuracy and stability. However, this method fails to consider the asynchronous clocking between nodes during information interaction and the acoustic ray bending problem during signal transmission when constructing the measurement equations, thus limiting the underwater target tracking accuracy.
[0004] Furthermore, publication number CN114494340A, entitled "A KL Interactive Multiple Model Underwater Target Tracking Method," considers the physical characteristics of seawater and the interference in measurement information. It interacts with sensor nodes within the target area by waking them up to exchange Time of Arrival (TOA) information, and combines relative entropy KL divergence with the Interacting Multiple Model (IMM) algorithm to optimize the model and improve tracking accuracy. However, this method also lacks a compensation mechanism for the inherent asynchronous nature of underwater clocks and fails to address the propagation path modeling errors caused by acoustic ray bending, thus still falling short of tracking requirements.
[0005] Furthermore, existing tracking methods using UKF generally suffer from the problem of initialization accuracy depending on prior information. If the estimation of the AUV's position, velocity, and other state parameters is significantly flawed in the initial stage, it can lead to slow convergence or even divergence in the filtering process, further reducing tracking reliability.
[0006] Therefore, there is an urgent need to propose an AUV tracking method that can simultaneously solve the problems of acoustic ray bending modeling error, clock asynchronous compensation, and insufficient UKF initialization accuracy, so as to improve the stability and accuracy of AUV tracking in complex underwater environments. Summary of the Invention
[0007] This invention provides a WLS-UKF-based underwater autonomous vehicle tracking method to address the shortcomings of existing AUV tracking technologies, such as measurement errors caused by acoustic ray bending, timestamp distortion due to clock asynchrony, and insufficient accuracy of UKF initial state estimation. By obtaining the initial state of the AUV through the WLS algorithm and combining it with UKF, continuous, stable, and high-precision tracking of the AUV is achieved, providing technical support for AUV operations in complex underwater scenarios.
[0008] In a first aspect, the present invention provides a WLS-UKF-based underwater autonomous vehicle tracking method, comprising: Several surface buoys and underwater sensor nodes are randomly deployed within the monitoring area to interact with the autonomous vehicle (AUV) and continuously track the AUV's position. Determine the AUV state vector and dynamic equations at any given time; Once the AUV sends a positioning request to the underwater sensor node, a TDOA measurement model is constructed to obtain the time difference of arrival. Based on the time difference of arrival, WLS is used to estimate the initial position of the AUV; By combining clock asynchrony and acoustic ray bending, a measurement equation for the AUV is constructed. The UKF is used to continuously track the AUV to obtain the predicted value of the system state at the current moment. The system iterates again to send positioning requests from the AUV to the underwater sensor nodes, continuously obtaining the system state prediction value for the next moment, thus achieving continuous tracking of the AUV.
[0009] According to the present invention, a WLS-UKF-based underwater autonomous vehicle tracking method is provided, which randomly deploys several surface buoys and underwater sensor nodes within a monitoring area to interact with the AUV and continuously track the AUV's position, including: After acquiring GNSS signals, the surface buoy determines its position coordinates, and the underwater sensor node determines its sensor position coordinates by receiving the position coordinates sent by the surface buoy. Once the AUV enters the monitoring area, the underwater sensor network is activated, enabling information exchange between the AUV and the underwater sensor nodes.
[0010] According to the present invention, a WLS-UKF-based underwater autonomous vehicle tracking method is provided to determine the AUV state vector and dynamic equations at any given time, including: Sure k AUV state vector at time step for:
[0011] in, They are respectively k The position of the AUV along the X, Y, and Z axes in the inertial reference frame (IRF) at any given moment. for k The heading angle of the AUV at any given moment. They are respectively k The linear velocities of the AUV in the longitudinal, transverse, and helical directions in the body reference frame BRF at any given time. for k Yaw rate of the AUV at any given moment; Establish a four-degree-of-freedom dynamic model and construct the dynamic equations:
[0012] in, This is the state transition function. This is process noise.
[0013] According to the present invention, an underwater autonomous vehicle tracking method based on WLS-UKF is provided, wherein the AUV to be tracked sends a positioning request to an underwater sensor node, a TDOA measurement model is constructed, and the time difference of arrival is obtained, including: Acquiring the time synchronization relationship between AUV and underwater sensor nodes ,in For AUV measurement time, For the actual time of AUV, s For clock drift, o Clock offset; Once the AUV enters the monitoring area, Constantly sending wake-up underwater sensor nodes i Sending a location request, underwater sensor node i exist It continuously receives AUV positioning requests and records the time the information is received. Return to AUV; Constructing a TDOA measurement model :
[0014] in , Each represents an arbitrary underwater sensor node. i , j Timestamp of receiving the AUV positioning request; Determine the relationship between underwater sound speed and underwater depth. :
[0015] in The rate of change of sound velocity at depth. The speed of sound at sea surface. For depth; The AUV and underwater sensor nodes were calculated using the ray tracing method. i Propagation delay :
[0016] in, Represents the acoustic trajectory and the node from the AUV to the underwater sensor. i The angle between the lines, Indicates AUV to underwater sensor node i The angle with the horizontal axis.
[0017] According to the WLS-UKF-based underwater autonomous vehicle tracking method provided by the present invention, after the AUV sends a positioning request to the underwater sensor node, constructs a Time Difference of Arrival (TDOA) measurement model, and obtains the time difference of arrival, the method further includes: The measurement noise is determined to follow a zero-mean Gaussian distribution. ,in The standard deviation of the basic noise; The measurement model is represented as a matrix. ,in For the measurement value vector, For an ideal measurement vector, To measure the noise vector, N This represents the total number of underwater sensor nodes.
[0018] The present invention provides a WLS-UKF-based underwater autonomous vehicle tracking method, which estimates the initial position of the AUV based on the time difference of arrival (TLO). The method includes: when k When =0, the measurement model is transformed into ,in , ; The initial position of the AUV is obtained by WLS estimation. and the initial error covariance matrix .
[0019] The present invention provides a WLS-UKF-based underwater autonomous vehicle tracking method, which integrates clock asynchrony and acoustic ray bending to construct measurement equations for the AUV, uses UKF to continuously track the AUV, and obtains the predicted system state value at the current moment, including: Initialize UKF parameters to determine the initial state of the UKF. Initial error covariance And UKF core parameters; Based on the current state Covariance 2n+1 sigma points are generated, and the state of these sigma points is predicted using the dynamic equations of the AUV. ( i =0,1,…,2n), the expression for calculating the prediction error covariance is:
[0020] in, Covariance weights; Perform the measurement update process.
[0021] According to the present invention, an underwater autonomous vehicle tracking method based on WLS-UKF performs a measurement update process, including: The predicted sigma point is passed through the measurement model. Mapping to the measurement space yields ; Calculate the predicted value ; Calculate measurement covariance ; Calculate state-measured cross covariance ; Calculate Kalman gain ; Update state estimation ; Update error covariance .
[0022] In a second aspect, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the underwater autonomous vehicle tracking method based on any of the above-described methods.
[0023] Thirdly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the underwater autonomous vehicle tracking method based on any of the above-described methods.
[0024] The underwater autonomous vehicle tracking method based on WLS-UKF provided by this invention has the following beneficial effects: (1) Accurate correction of sound ray bending: Based on the equal gradient sound velocity profile, a sound ray propagation model is constructed, and the propagation time delay between the AUV and the sensor node is calculated by the ray tracing method, which effectively eliminates the measurement error caused by sound ray bending and significantly improves the measurement accuracy of TDOA; (2) Clock Asynchronous Interference Cancellation: The TDOA measurement method effectively cancels the clock offset and clock drift between the AUV and the sensor node, avoiding measurement distortion caused by clock asynchrony; (3) UKF initialization accuracy optimization: The WLS method is used to iteratively estimate the initial position of the AUV, and the UKF algorithm is combined to achieve long-term stable tracking of the AUV position and velocity parameters, which solves the problem of filter divergence or slow convergence caused by initial value deviation. Attached Figure Description
[0025] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.
[0026] Figure 1 This is one of the flowcharts illustrating the underwater autonomous vehicle tracking method based on WLS-UKF provided by the present invention; Figure 2 This is the second flowchart of the underwater autonomous vehicle tracking method based on WLS-UKF provided by the present invention; Figure 3 This invention provides an asynchronous clock model and a diagram illustrating the effect of sound ray bending. Figure 4 This is a schematic diagram of the AUV continuous tracking method combining WLS-UKF provided by the present invention; Figure 5 This is one of the schematic diagrams of the simulation results provided by the present invention; Figure 6 This is the second schematic diagram of the simulation results provided by the present invention; Figure 7 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation
[0027] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.
[0028] Figure 1 This is one of the flowcharts illustrating the underwater autonomous vehicle tracking method based on WLS-UKF provided in this embodiment of the invention, such as... Figure 1 As shown, it includes: Step 100: Randomly deploy several surface buoys and underwater sensor nodes within the monitoring area to interact with the autonomous vehicle (AUV) and continuously track the AUV's position. Step 200: Determine the AUV state vector and dynamic equations at any given time; Step 300: The AUV sends a positioning request to the underwater sensor node, constructs a TDOA measurement model, and obtains the time difference of arrival; Step 400: Based on the time difference of arrival, estimate the initial position of the AUV using WLS; Step 500: Construct a measurement equation for the AUV by combining clock asynchrony and acoustic ray bending, and continuously track the AUV using UKF to obtain the predicted system state value at the current moment; Step 600: Iterate again to send a positioning request from the AUV to the underwater sensor node, continuously obtain the system state prediction value at the next moment, and realize continuous tracking of the AUV.
[0029] Specifically, the technical route logic of the embodiments of the present invention is as follows: Figure 2 As shown, it includes: Step 1: Randomly deploy several surface buoys and underwater sensor nodes within the monitoring area. The surface buoys acquire their positions using GNSS signals. The underwater sensor nodes obtain their position information through the surface buoys. When the AUV enters the monitoring area, the underwater sensor network within communication range is activated and interacts with the AUV to continuously track its position.
[0030] Step 2: Considering the inherent stability of AUV underwater motion, neglecting roll and pitch motion, define... k The state vector of the AUV at time t is:
[0031] in, They are respectively kThe position of the AUV along the X, Y, and Z axes in the inertial reference frame (IRF) at any given moment. for k The heading angle of the AUV at any given moment. They are respectively k The linear velocities of the AUV in the sway, roll, and heave directions at any given moment in the body reference frame (BRF). for k The yaw rate of the AUV at any given time.
[0032] Step 3: To accurately describe the changes in the position and velocity of the AUV over time, a four-degree-of-freedom dynamic model is established, and the dynamic equations are constructed:
[0033] in, The state transition function is expressed as follows:
[0034] In the formula, The local sampling period for the AUV. , , These represent the mass of the AUV in the longitudinal, transverse, and helical directions, respectively. For the yaw moment of inertia, , , These are the external forces in the longitudinal, transverse, and vertical directions, respectively. For yaw moment, Buoyancy in the direction of heave The damping coefficient is... For Gaussian process noise, Let be the process noise covariance matrix.
[0035] Step 4: The clocks of the AUV and the underwater sensor nodes are out of sync, satisfying the relationship... ,in For AUV measurement time, For the actual time of AUV, s For clock drift, o This is due to clock offset. After the AUV enters the monitoring area, Constantly sending wake-up underwater sensor nodes i Sending a location request, underwater sensor node i exist It continuously receives AUV positioning requests and records the time the information is received. The data is then transmitted back to the AUV. The Time Difference of Arrival (TDOA) is used to eliminate the effects of clock skew and drift.
[0036] in , underwater sensor nodes i , j Timestamp of receiving the AUV positioning request.
[0037] Considering that underwater transmission speed is related to underwater depth, the speed of sound underwater satisfies the following with depth:
[0038] in The rate of change of sound velocity at depth. The speed of sound at sea surface. For depth, the AUV and sensor can be calculated using ray tracing. i Propagation delay The calculation is as follows:
[0039] In the formula, Represents the acoustic trajectory and AUV-underwater sensor node i The angle between the lines, Indicates AUV - Underwater Sensor Node i The angle with the horizontal axis. The transmission delay between the AUV and underwater sensor nodes at different locations under acoustic ray bending constraints, obtained through the above calculations.
[0040] Figure 3 This demonstrates the effects of an asynchronous clock model and ray bending, in which Figure 3 (a) in the diagram represents the asynchronous clock model, and it compares the actual time with the measured time under both synchronous and asynchronous clock scenarios. Figure 3 (b) in the diagram shows the effect of sound ray bending, with the AUV and underwater sensor node. i The sound lines between them bend underwater.
[0041] The process of eliminating clock skew by exchanging timestamp information between the AUV and different underwater sensor nodes is as follows: Suppose there are four active underwater sensor nodes numbered A, B, C, and D, whose locations are known prior to each other with the aid of surface buoys.
[0042] The AUV sends a tracking request signal to four underwater sensor nodes, A, B, C, and D, with its own transmission timestamp being [timestamp value missing]. Each sensor records its own reception time after receiving the request signal: A: B: C: D: Then each at a preset time Send response signals to the AUV. Each response signal should include its own coordinates, reception time, and transmission time. Wait for the AUV to receive response signals from four underwater sensor nodes (A, B, C, and D) respectively, and record the reception time of each underwater sensor node: A: B: C: D: Then, the following measurement equation can be constructed through information exchange between the underwater sensor node and the AUV: , , , Then, the difference between the TDOA of the two underwater sensor nodes and the AUV is calculated. ,in For AUV and underwater sensor nodes i The propagation delay is reduced, thereby canceling out clock asynchronous errors.
[0043] Step 5: Using the time difference obtained in Step 4, consider that the measurement noise follows a zero mean. ,in Based on the standard deviation of the basic noise, the measurement model is represented in matrix form as follows: ,in For the measurement value vector, For an ideal measurement vector, To measure the noise vector, N This represents the total number of underwater sensor nodes.
[0044] Step 6: To address the insufficient accuracy of UKF initial state estimation, the Weighted Least Squares (WLS) method is used to estimate the initial position of the AUV. The process is as follows: Figure 4 As shown: when k When =0, the measurement model is transformed into ,in , The initial position of the AUV is achieved using a WLS-based optimization algorithm. and the initial error covariance matrix The estimate.
[0045] Step 7: To achieve continuous AUV tracking based on UKF, first use the estimated position obtained in Step 6. Covariance Matrix Complete UKF initialization. (e.g.) Figure 4 As shown, continuous tracking of the AUV is then achieved using an unscented Kalman filter (UKF). The main steps are as follows: First, the UKF parameters are initialized, setting the initial state of the UKF. Initial error covariance And UKF core parameters. Based on the current state. Covariance Generate 2n+1 sigma points, and use the dynamic model of the AUV in step 3 to predict the state of the sigma points, and obtain... ( i =0,1,…,2n), the expression for calculating the prediction error covariance is:
[0046] in To implement measurement updates using covariance weights, the following process can be used: (1) The predicted sigma points are measured using the model. Mapping to the measurement space yields ; (2) Calculate the predicted measurement value ; (3) Calculate the measurement covariance ; (4) Calculate the state-measurement cross covariance ; (5) Calculate the Kalman gain ; (6) Update state estimation ; (7) Update error covariance ; Step 8: Calculate the one-step prediction and covariance matrix of the system state variables based on Step 7. To further achieve iterative tracking, return to Step 4 and use the measured values at the next time step. This enables continuous, real-time tracking of AUVs.
[0047] The embodiments of this invention also provide simulation comparisons of the WLS-UKF method and the UKF method, such as... Figure 5 The tracking error shown and as Figure 6 As shown in the speed error diagram, it can be seen that the WLS-UKF method proposed in this invention has smaller errors than the UKF method, demonstrating superior performance.
[0048] Figure 7 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 7As shown, the electronic device may include: a processor 710, a communication interface 720, a memory 730, and a communication bus 740, wherein the processor 710, the communication interface 720, and the memory 730 communicate with each other through the communication bus 740. The processor 710 can call logic instructions in the memory 730 to execute a WLS-UKF-based underwater autonomous vehicle tracking method. This method includes: randomly deploying several surface buoys and underwater sensor nodes within the monitoring area to interact with the autonomous vehicle (AUV) and continuously tracking its position; determining the AUV's state vector and dynamic equations at any given time; constructing a TDOA measurement model to obtain the time difference of arrival (TDOA) before the AUV sends a positioning request to the underwater sensor nodes; estimating the AUV's initial position using WLS based on the TDOA; constructing measurement equations for the AUV by integrating clock asynchrony and acoustic ray bending, continuously tracking the AUV using UKF, and obtaining a predicted system state value for the current time; iteratively re-initiating the AUV's sending of positioning requests to the underwater sensor nodes to continuously obtain the predicted system state value for the next time moment, thus achieving continuous tracking of the AUV.
[0049] Furthermore, the logical instructions in the aforementioned memory 730 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the 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 to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.
[0050] On the other hand, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements the WLS-UKF-based underwater autonomous vehicle tracking method provided by the above methods. The method includes: randomly deploying a number of surface buoys and underwater sensor nodes within a monitoring area to interact with the autonomous vehicle (AUV) and continuously tracking the AUV's position; determining the AUV's state vector and dynamic equations at any given time; constructing a TDOA measurement model to obtain the time difference of arrival (TDOA) before the AUV sends a positioning request to the underwater sensor nodes; estimating the AUV's initial position using WLS based on the TDOA; constructing a measurement equation for the AUV by combining clock asynchrony and acoustic ray bending; continuously tracking the AUV using UKF to obtain the predicted system state value at the current time; and iteratively re-issuing the AUV's positioning request to the underwater sensor nodes to continuously obtain the predicted system state value at the next time moment, thereby achieving continuous tracking of the AUV.
[0051] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.
[0052] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.
[0053] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims
1. A tracking method for underwater autonomous vehicles based on WLS-UKF, characterized in that, include: Several surface buoys and underwater sensor nodes are randomly deployed within the monitoring area to interact with the autonomous vehicle (AUV) and continuously track the AUV's position. Determine the AUV state vector and dynamic equations at any given time; Once the AUV sends a positioning request to the underwater sensor node, a Time Difference of Arrival (TDOA) measurement model is constructed to obtain the time difference of arrival. Based on the time difference of arrival, the weighted least squares (WLS) method is used to estimate the initial position of the AUV; By combining clock asynchrony and acoustic ray bending, a measurement equation for the AUV is constructed. An unscented Kalman filter (UKF) is used to continuously track the AUV and obtain the predicted value of the system state at the current moment. The system iterates again to send positioning requests from the AUV to the underwater sensor nodes, continuously obtaining the system state prediction value for the next moment, thus achieving continuous tracking of the AUV.
2. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 1, characterized in that, Several surface buoys and underwater sensor nodes are randomly deployed within the monitoring area to interact with the AUV and continuously track its location, including: After acquiring GNSS signals, the surface buoy determines its position coordinates, and the underwater sensor node determines its sensor position coordinates by receiving the position coordinates sent by the surface buoy. Once the AUV enters the monitoring area, the underwater sensor network is activated, enabling information exchange between the AUV and the underwater sensor nodes.
3. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 1, characterized in that, Determine the AUV state vector and dynamic equations at any given time, including: Sure k AUV state vector at time step for: in, They are respectively k The position of the AUV along the X, Y, and Z axes in the inertial reference frame (IRF) at any given moment. for k The heading angle of the AUV at any given moment. They are respectively k The linear velocities of the AUV in the longitudinal, transverse, and helical directions in the body reference frame BRF at any given time. for k Yaw rate of the AUV at any given moment; Establish a four-degree-of-freedom dynamic model and construct the dynamic equations: in, This is the state transition function. This is process noise.
4. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 1, characterized in that, Once the AUV sends a positioning request to the underwater sensor node, a TDOA measurement model is constructed to obtain the time difference of arrival, including: Acquiring the time synchronization relationship between AUV and underwater sensor nodes ,in For AUV measurement time, For the actual time of AUV, s For clock drift, o Clock offset; Once the AUV enters the monitoring area, Constantly sending wake-up underwater sensor nodes i Sending a location request, underwater sensor node i exist It continuously receives AUV positioning requests and records the time the information is received. Return to AUV; Constructing a TDOA measurement model : in , Each is an arbitrary underwater sensor node. i , j Timestamp of receiving the AUV positioning request; Determine the relationship between underwater sound speed and underwater depth. : in The rate of change of sound velocity at depth. The speed of sound at sea surface. For depth; The AUV and underwater sensor nodes were calculated using the ray tracing method. i Propagation delay : in, Represents the acoustic trajectory and the node from the AUV to the underwater sensor. i The angle between the lines, Indicates AUV to underwater sensor node i The angle with the horizontal axis.
5. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 4, characterized in that, After the AUV sends a positioning request to the underwater sensor node, constructs the Time Difference of Arrival (TDOA) measurement model, and obtains the TDOA, the following steps are also included: The measurement noise is determined to follow a zero-mean Gaussian distribution. ,in The standard deviation of the basic noise; The measurement model is represented as a matrix. ,in For the measurement value vector, For an ideal measurement vector, To measure the noise vector, N This represents the total number of underwater sensor nodes.
6. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 1, characterized in that, Based on the time difference of arrival, WLS is used to estimate the initial position of the AUV, including: when k When =0, the measurement model is transformed into ,in , ; The initial position of the AUV is obtained by WLS estimation. and the initial error covariance matrix .
7. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 1, characterized in that, By combining clock asynchrony and acoustic ray bending, a measurement equation for the AUV is constructed. The AUV is continuously tracked using UKF to obtain the predicted system state at the current moment, including: Initialize UKF parameters to determine the initial state of the UKF. Initial error covariance And UKF core parameters; Based on the current state Covariance 2n+1 sigma points are generated, and the state of these sigma points is predicted using the dynamic equations of the AUV. ( i =0,1,…,2n), the expression for calculating the prediction error covariance is: in, Covariance weights; Perform the measurement update process.
8. The underwater autonomous vehicle tracking method based on WLS-UKF according to claim 7, characterized in that, Performing the measurement update process includes: The predicted sigma point is passed through the measurement model. Mapping to the measurement space yields ; Calculate the predicted value ; Calculate measurement covariance ; Calculate state-measured cross covariance ; Calculate Kalman gain ; Update state estimation ; Update error covariance .
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the underwater autonomous vehicle tracking method based on WLS-UKF as described in any one of claims 1 to 8.
10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the underwater autonomous vehicle tracking method based on any one of claims 1 to 8.