An autonomous recovery-oriented underwater unmanned vehicle cooperative navigation method and system

Abstract

The application discloses an autonomous recovery-oriented underwater unmanned vehicle cooperative navigation method and system, relates to the field of underwater unmanned vehicle navigation and control, and comprises the following steps: acquiring multi-source sensor data and performing preprocessing; based on the preprocessed multi-source sensor data, adopting adaptive unscented Kalman filtering to obtain a position estimation value, a speed estimation value and an estimation error covariance matrix of the underwater unmanned vehicle at the current time; adopting a feedforward compensation artificial potential field method to generate a speed instruction for guiding the underwater unmanned vehicle; controlling the underwater unmanned vehicle based on the speed instruction, and switching the navigation mode according to the running state of the underwater unmanned vehicle; and through the multi-dimensional innovations of explicitly introducing state estimation uncertainty into path planning, establishing a current feedforward compensation mechanism and realizing mode smooth switching, the safety, environmental adaptability, docking accuracy and the like are all superior to those of the prior art, and a more reliable technical scheme is provided for UUV mother ship autonomous recovery.

Description

Technical Field

[0001] This application relates to the field of underwater unmanned vehicle navigation and control technology, and in particular to a cooperative navigation method and system for autonomous recovery of underwater unmanned vehicles. Background Technology

[0002] Unmanned Underwater Vehicles (UUVs) are widely used in marine scientific exploration, seabed resource surveys, and military surveillance due to their advantages of low cost, low risk, and high maneuverability. However, limited by battery capacity and data storage capabilities, UUVs typically cannot complete long-term continuous operations in a single mission and must rely on a mother vessel or underwater docking station for energy replenishment and data recovery. During the autonomous recovery process, UUVs face multiple challenges: GPS signals are unavailable in the underwater environment, making it impossible to obtain absolute position information; while inertial navigation systems can provide high-precision state estimates in the short term, their errors accumulate over time, resulting in a significant decrease in positioning accuracy after long-term navigation; and the near-field recovery area is compact, densely packed with obstacles, and subject to dynamic environmental disturbances such as time-varying ocean currents, placing extremely high demands on obstacle avoidance capabilities and docking accuracy.

[0003] To address the aforementioned challenges, existing technologies primarily employ two approaches. The first is a multi-source information fusion-based integrated navigation method. This method integrates the inertial measurement unit (IMU) with the acoustic positioning system through extended Kalman filtering or unscented Kalman filtering to suppress the accumulation of inertial navigation errors. However, in this approach, state estimation and path planning are generally decoupled, and the error covariance information generated during estimation is not effectively incorporated into the path planning decision, making it difficult for the planning module to perceive changes in real-time positioning uncertainty. The second approach is a path planning method based on the artificial potential field method. This method is computationally efficient and simple to implement, but it typically uses fixed obstacle safety boundaries, making it difficult to adapt to dynamic changes in positioning uncertainty. Furthermore, it is prone to getting trapped in local minima or path oscillations in narrow, obstacle-dense environments such as near-field recovery. In addition, existing technologies lack effective feedforward compensation mechanisms for environmental disturbances such as time-varying ocean currents, and also lack smooth switching strategies between far-field cruising and near-field berthing, making it difficult to simultaneously meet the requirements of safety, stability, and high-precision docking. Summary of the Invention

[0004] To address the aforementioned challenges, this application provides a cooperative navigation method and system for autonomous underwater unmanned vehicles (UUVs) undergoing autonomous recovery. By explicitly introducing state estimation uncertainties into path planning, establishing an ocean current feedforward compensation mechanism, and achieving smooth mode switching, this method enables autonomous navigation and docking in scenarios where safety, stability, and high precision are balanced in the autonomous recovery of underwater motherships under conditions of positioning uncertainty and environmental disturbances.

[0005] To achieve the above objectives, this application provides a cooperative navigation method for autonomous underwater unmanned vehicles, comprising the following steps: S1: Acquire multi-source sensor data from the underwater unmanned vehicle and preprocess the multi-source sensor data; the multi-source sensor data includes triaxial acceleration, angular velocity data, environmental magnetic field strength data, pre-set magnetic beacon positions, target point positions and obstacle information; S2: Based on the preprocessed multi-source sensor data, an adaptive unscented Kalman filter is used to obtain the position estimate, velocity estimate, and estimation error covariance matrix of the underwater unmanned vehicle at the current moment; S3: The feedforward compensated artificial potential field method is adopted to generate a speed command for guiding the underwater unmanned vehicle based on the position estimate, velocity estimate, estimation error covariance matrix and preset target point position and obstacle information. S4: Controls the underwater unmanned vehicle based on speed commands and switches navigation modes according to the operating status of the underwater unmanned vehicle.

[0006] Preferably, the preprocessing specifically includes: Timestamps were added to the acquired triaxial acceleration, angular velocity, and ambient magnetic field strength data, and time alignment was performed on the triaxial acceleration, angular velocity, and ambient magnetic field strength data based on a unified time reference to form a synchronous sensor dataset. Perform data validity checks on the synchronous sensor dataset and remove outlier measurements. The data after rejection is filtered to reduce high-frequency noise.

[0007] Preferably, S2 specifically includes: S201: Initialize filter parameters and define the system state vector containing the position and velocity components of the underwater unmanned vehicle; S202: Based on the preprocessed triaxial acceleration and angular velocity data, the measured velocity is obtained through integration. The measured velocity is corrected using the velocity bias estimate updated at the previous moment. Based on the corrected velocity, state prediction is performed to obtain the predicted state and the predicted state covariance matrix. S203: Establish the magnetic field strength observation equation based on the magnetic dipole theory, and calculate the predicted value of the magnetic field strength according to the position component in the predicted state; S204: Based on the predicted magnetic field strength and the environmental magnetic field strength data, the observation residuals are obtained; S205: Adaptively adjust the observation noise covariance matrix based on environmental magnetic field strength data; S206: Generate a Sigma point set and its corresponding weights through unscented transformation, and substitute the Sigma points into the magnetic field strength observation equation to calculate the predicted observation value; S207: Based on the predicted observations, obtain the observation mean, observation covariance, and cross-covariance between state and observation. Based on the observation covariance and cross-covariance between state and observation, obtain the Kalman gain. S208: Based on the observation residuals, environmental magnetic field strength data and Kalman gain, update the state estimate, obtain the updated state estimate, and update the predicted state covariance matrix to obtain the estimation error covariance matrix; S209: Based on the difference between the velocity component in the updated state estimate and the measured velocity, update the velocity offset estimate at the current moment in real time, which will be used to correct the measured velocity in the next moment S202. S210: Based on the estimation error covariance matrix, obtain the position sub-covariance matrix, and output the position estimate, velocity estimate, estimation error covariance matrix, and position sub-covariance matrix at the current time.

[0008] Preferably, the state prediction equation is expressed as: ; in, For the state estimation of the previous time step, The measured velocity after bias correction. The sampling period is This is process noise; The equation for observing magnetic field strength is expressed as: ; Where B(r) is the predicted magnetic field strength, and r is the position of the underwater unmanned vehicle. Here, m represents the position of the magnetic beacon, and m represents the magnetic dipole moment. The vacuum permeability; The observation residuals are expressed as: ; in, To observe the residual vector, This is the vector of measured magnetic field strength values. This is the vector of predicted magnetic field strength values.

[0009] The preferred adaptive adjustment strategy is expressed as follows: ; in, This is the adaptively adjusted observation noise covariance matrix; The preset minimum observation noise covariance matrix threshold; η is the basic observation noise covariance matrix; η is the adaptive gain constant; Measured value of magnetic induction intensity The Euclidean norm; ε is the threshold for determining weak magnetic field; The fixed observation noise covariance matrix used in the weak magnetic region; Kalman gain is expressed as: ; in, The Kalman gain matrix; Let be the cross-covariance matrix between the state vector and the observation vector; Let be the covariance matrix of the observation vectors.

[0010] Preferably, S3 specifically includes: S301: Based on the 3σ probability envelope principle, the dynamic safety margin is calculated according to the position sub-covariance matrix; S302: Construct a gravitational field around the target point and generate a gravitational vector pointing towards the target point; S303: Construct a repulsive field centered on the location of the obstacle in the environment, and superimpose the dynamic safety margin onto the basic radius of the obstacle to generate a repulsive vector; S304: Superimpose a tangential auxiliary force component on a tangential plane perpendicular to the repulsive force direction to guide the underwater unmanned vehicle to smoothly circumnavigate along the edge of the obstacle; S305: Compare the measured velocity after offset correction with the velocity command at the previous moment, calculate the velocity deviation, and smooth it with a low-pass filter to obtain the estimated value of the ocean current disturbance velocity; S306: Combine the gravitational vector, repulsive vector, and tangential auxiliary force vector into a resultant force vector, combine it with the ocean current disturbance velocity estimate, generate a velocity command including a feedforward compensation term, and perform amplitude constraint processing on the velocity command to obtain the final velocity command.

[0011] Preferably, the dynamic safety margin is expressed as: ; in, κ represents the basic safety margin; κ is the risk modulation coefficient. Let be the positional subcovariance matrix; trace(∙) denotes the trace of the matrix; The gravitational vector is represented as: ; in, For gravitational gain; The target point's position vector; δ is the current position estimation vector of the underwater unmanned vehicle; δ is a small constant to prevent division by zero.

[0012] The repulsive force vector is represented as: ; in, Let i be the repulsive force vector generated by the i-th obstacle; For repulsive force gain; Let be the distance between the underwater unmanned vehicle and the expanded surface of the i-th obstacle; Let be the center position vector of the i-th obstacle; Let be the base radius of the i-th obstacle; The distance is affected by the repulsive force; The auxiliary force component is represented as: ; Where γ is the tangential coefficient; Let be the norm of the total repulsive force vector; This is a rotation matrix for a 90-degree rotation; This is the estimated vector of the current position of the underwater unmanned vehicle. Preferably, the speed command is expressed as: ; ; in, Let the force vector be the force vector in the total potential field. The gravitational vector, It is the repulsive force vector. The auxiliary force vector is the tangential force vector. This is the vector of estimated ocean current disturbance velocity values.

[0013] Preferably, S4 specifically includes: S401: Converts speed commands into thrust or speed commands for each of the underwater unmanned vehicle's thrusters, driving the underwater unmanned vehicle to move along the planned path; S402: Collect new sensor data and return to step S1. Repeat steps S1 to S4 to form closed-loop navigation control. S403: Real-time monitoring of the distance between the underwater unmanned vehicle and the target point, as well as the trace of the position covariance matrix; S404: When the distance between the underwater unmanned vehicle and the target point is less than a preset distance threshold and the trace of the position covariance matrix is ​​less than a preset positioning accuracy threshold, the underwater unmanned vehicle is determined to have entered the near-field docking phase, and the artificial potential field parameters are automatically adjusted, including reducing the repulsive gain and increasing the gravitational gain.

[0014] A cooperative navigation system for autonomously recoverable underwater unmanned vehicles (UUVs) employs a cooperative navigation method for autonomously recoverable UUVs, comprising: The data acquisition module is used to acquire multi-source sensor data from the underwater unmanned vehicle and to preprocess the multi-source sensor data. The adaptive state estimation module, connected to the data acquisition module, is used to obtain the position estimate, velocity estimate, and estimation error covariance matrix of the underwater unmanned vehicle at the current moment based on the preprocessed multi-source sensor data and by using adaptive unscented Kalman filtering. The dynamic potential field path planning module, connected to the adaptive state estimation module, is used to generate speed commands to guide the underwater unmanned vehicle by adopting the feedforward compensated artificial potential field method, based on the position estimate, velocity estimate, estimation error covariance matrix, and preset target point position and obstacle information. The closed-loop control module, connected to the dynamic potential field path planning module, is used to convert speed commands into control commands and drive the underwater unmanned vehicle to move. The mode switching module is connected to the adaptive state estimation module and the dynamic potential field path planning module, respectively, and is used to switch the navigation mode according to the operating state of the underwater unmanned vehicle.

[0015] Therefore, the above-mentioned cooperative navigation method and system for autonomous underwater unmanned vehicles adopted in this application has the following beneficial effects: (1) This application explicitly introduces the position sub-covariance matrix of the adaptive unscented Kalman filter output into the path planning module, and constructs a dynamic safety margin based on the 3σ probability envelope principle, so that the obstacle avoidance boundary can be adaptively adjusted with the change of positioning accuracy: when the positioning is uncertain, the safety area is automatically expanded to ensure safety, and when the positioning is accurate, the safety area is automatically shrunken to release feasible space. This mechanism fundamentally solves the dilemma between safety and feasibility of fixed safety boundaries.

[0016] (2) This application estimates the ocean current velocity in real time by comparing the offset-corrected IMU measured velocity with the velocity command at the previous moment, and then uses this estimate as a feedforward term to add to the velocity command, thereby achieving active compensation for disturbances. At the same time, the measured velocity, rather than the ideal command velocity, is used to drive the state prediction, so that the state estimation can more realistically reflect the impact of environmental disturbances on the movement of the UUV. The synergistic effect of the two enables the UUV to maintain high-precision path tracking capability in time-varying ocean current environments.

[0017] (3) This application establishes a dual threshold switching strategy based on distance and positioning accuracy. The potential field parameters are automatically adjusted only when the UUV is close enough to the target point and the positioning accuracy meets the requirements, so as to achieve a smooth transition from global obstacle avoidance mode to high-precision docking mode. This mechanism effectively avoids the oscillation phenomenon that is easy to be generated near the target point by the traditional artificial potential field method, and provides a reliable guarantee for centimeter-level docking at the end.

[0018] (4) This application, through the comprehensive application of multi-dimensional adaptive mechanisms, forms a cooperative navigation solution that combines safety, robustness, and high precision in complex underwater environments. Specifically, it includes: adaptive adjustment of observation noise based on magnetic induction intensity, which solves the problems of misleading observations in weak magnetic areas and insufficient utilization of information in strong magnetic areas; online estimation and feedback correction of IMU velocity offset, which suppresses the long-term accumulation of inertial navigation errors; and the introduction of tangential auxiliary force, which reduces the risk of traditional APF getting trapped in local minima in areas with dense obstacles. These mechanisms work together to enable this invention to significantly improve the berthing success rate and docking accuracy of UUVs in autonomous recovery scenarios with significant positioning uncertainty and complex environmental disturbances.

[0019] The technical solution of this application will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of a cooperative navigation method for autonomous recovery of underwater unmanned vehicles according to this application. Figure 2 This is a block diagram illustrating the working principle of AUKF in the embodiments of this application; Figure 3 This is a schematic diagram illustrating the path planning principle of the feedforward compensated artificial potential field method in the embodiments of this application; Figure 4 This is a flowchart of the mode switching control logic in the embodiments of this application; Figure 5 This is a schematic diagram showing the comparison results of time-position error in the embodiments of this application; Figure 6 This is a schematic diagram of the time-lateral displacement comparison results in the embodiments of this application; Figure 7 This is a schematic diagram showing the comparison results of the X-axis position and Y-axis position in an embodiment of this application; Figure 8 This is a schematic diagram showing the comparison results of time and safety margin in the embodiments of this application; Figure 9 This is a schematic diagram of the time-error comparison results in the embodiments of this application. Detailed Implementation

[0021] The following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0022] Unless otherwise defined, the technical or scientific terms used in this application shall have the ordinary meaning as understood by a person of ordinary skill in the art to which this application pertains.

[0023] The terms "comprising" or "including," as used in this application, mean that the element preceding the term encompasses the element listed after the term, and do not exclude the possibility of encompassing other elements as well. The terms "inner," "outer," "upper," and "lower," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application. When the absolute position of the described object changes, the relative positional relationship may also change accordingly. In this application, unless otherwise expressly specified and limited, the term "attached," etc., should be interpreted broadly. For example, it can refer to a fixed connection, a detachable connection, or an integral part; it can refer to a direct connection or an indirect connection through an intermediate medium; it can refer to the internal communication of two elements or the interaction relationship between two elements. Those skilled in the art can understand the specific meaning of the above terms in this application according to the specific circumstances.

[0024] Example 1: A cooperative navigation method and system for autonomous recovery of underwater unmanned vehicles, such as Figures 1-4 As shown, it includes the following steps: S1: Acquire multi-source sensor data from the underwater unmanned vehicle and preprocess the multi-source sensor data; the multi-source sensor data includes triaxial acceleration, angular velocity data, environmental magnetic field strength data, pre-set magnetic beacon positions, target point positions and obstacle information; Preprocessing specifically includes: Timestamps were added to the acquired triaxial acceleration, angular velocity, and ambient magnetic field strength data, and time alignment was performed on the triaxial acceleration, angular velocity, and ambient magnetic field strength data based on a unified time reference to form a synchronous sensor dataset. Perform data validity checks on the synchronous sensor dataset and remove outlier measurements. The data after rejection is filtered to reduce high-frequency noise.

[0025] Specifically, the system collects triaxial acceleration and angular velocity data from the inertial measurement unit in real time, and obtains velocity information in the carrier coordinate system through integration. Simultaneously, it collects ambient magnetic field strength data from the triaxial magnetometer. This sensor data serves as the foundational input for subsequent state estimation. The system also acquires pre-set spatial coordinates of the magnetic beacon and its magnetic dipole moment, as well as the location of the mothership recovery target point and obstacle information in the environment. The obstacle information includes the center position and basic radius of each obstacle, which can be obtained through pre-mapped surveys or detection by the sonar onboard the vessel.

[0026] The acquired multi-source sensor data undergoes time synchronization processing: timestamps are added to each set of IMU and magnetometer data, and they are aligned based on a unified time reference to form a synchronized sensor dataset for the current moment. This step is crucial for subsequent multi-source information fusion, as only time-aligned data can accurately reflect the spacecraft's state at the same instant.

[0027] The data is then validated, discarding measurements that exceed the preset range or exhibit obvious anomalies. Valid data undergoes necessary filtering to reduce the impact of high-frequency noise on subsequent calculations. The preprocessed sensor data sequence is then used for subsequent state estimation.

[0028] S2: Based on the preprocessed multi-source sensor data, the position estimate, velocity estimate and estimation error covariance matrix of the underwater unmanned vehicle at the current moment are obtained by using the Adaptive Unscented Kalman Filter (AUKF). S2 specifically includes: S201: Initialize filter parameters and define the system state vector containing the position and velocity components of the underwater unmanned vehicle; ; Where x, y, and z are the northward, eastward, and vertical position coordinates of the UUV in the local N-E-T-Earth coordinate system, respectively; vx, vy, and vz are the corresponding northward, eastward, and vertical velocity components, respectively. These initial parameters can be calibrated based on the UUV's dynamic characteristics and sensor performance.

[0029] S202: Based on the preprocessed triaxial acceleration and angular velocity data, the measured velocity is obtained through integration. The measured velocity is corrected using the velocity bias estimate updated at the previous moment. Based on the corrected velocity, the state is predicted to obtain the predicted state and the predicted state covariance matrix. Unlike traditional methods that use ideal command velocity, this method uses measured velocity to more realistically reflect the impact of ocean current disturbances on the UUV's trajectory.

[0030] The state prediction equation is expressed as: ; in, For the state estimation of the previous time step, The measured velocity after bias correction. The sampling period is This is process noise; S203: Establish the magnetic field strength observation equation based on the magnetic dipole theory, and calculate the predicted value of the magnetic field strength according to the position component in the predicted state; When the UUV approaches the preset magnetic beacon, the magnetic field strength generated by the beacon has a non-linear relationship with the relative position of the UUV. The observation model expression is as follows: ; Where B(r) is the predicted magnetic field strength, and r is the position of the underwater unmanned vehicle. Here, m represents the position of the magnetic beacon, and m represents the magnetic dipole moment. The magnetic permeability of vacuum is used to characterize the characteristic that the magnetic field amplitude decays nonlinearly with distance: the magnetic field gradient is significant in the near field region, which is beneficial for accurate positioning; the signal weakens rapidly in the far field region, and the observability of positioning decreases accordingly.

[0031] S204: Based on the predicted magnetic field strength and the environmental magnetic field strength data, the observation residuals are obtained; The observation residuals are expressed as: ; in, To observe the residual vector, This is the vector of measured magnetic field strength values. This is the vector of predicted magnetic field strength values.

[0032] S205: Adaptively adjust the observation noise covariance matrix based on environmental magnetic field strength data; The adaptive adjustment strategy is expressed as: ; in, This is the adaptively adjusted observation noise covariance matrix; The preset minimum observation noise covariance matrix threshold; η is the basic observation noise covariance matrix; η is the adaptive gain constant; Measured value of magnetic induction intensity The Euclidean norm; this mechanism enables the system to make full use of magnetic gradient information to quickly correct accumulated errors when the magnetic signal is strong, and automatically degenerates into an estimation mode based on dead reckoning when the magnetic signal is weak, thus avoiding divergence caused by weak magnetic signals dominating the filtering process.

[0033] S206: Generate a Sigma point set and its corresponding weights through unscented transformation, and substitute the Sigma points into the magnetic field strength observation equation to calculate the predicted observation value; S207: Based on the predicted observations, obtain the observation mean, observation covariance, and cross-covariance between state and observation. Based on the observation covariance and cross-covariance between state and observation, obtain the Kalman gain. Kalman gain is expressed as: ; in, The Kalman gain matrix; Let be the cross-covariance matrix between the state vector and the observation vector; Let be the covariance matrix of the observation vectors.

[0034] S208: Based on the observation residuals, environmental magnetic field strength data and Kalman gain, update the state estimate, obtain the updated state estimate, and update the predicted state covariance matrix to obtain the estimation error covariance matrix; Update the state estimate using actual magnetic observations: ; in, This is the updated state estimation vector.

[0035] Covariance update: ; in, This is the updated state covariance matrix; This is the predicted state covariance matrix.

[0036] S209: Based on the difference between the velocity component in the updated state estimate and the measured velocity, update the velocity offset estimate at the current moment in real time, which will be used to correct the measured velocity in the next moment S202. The velocity bias estimate is expressed as: ; in, This is the velocity offset estimate. This is the vector of raw velocity measurements output by the inertial measurement unit. The velocity component vector is the state estimation vector.

[0037] This bias is then fed back to the prediction step at the next time step to compensate for the measured speed of the IMU: ; in, This is the velocity vector of the inertial measurement unit after bias correction; This is the vector of raw velocity measurements output by the inertial measurement unit; This is the estimated velocity offset vector of the inertial measurement unit. This closed-loop calibration mechanism can gradually suppress the long-term accumulation of inertial navigation errors, ensuring the accuracy of dead reckoning in non-magnetic signal segments.

[0038] S210: Based on the estimation error covariance matrix, obtain the position sub-covariance matrix, and output the position estimate, velocity estimate, estimation error covariance matrix, and position sub-covariance matrix at the current time.

[0039] S3: The feedforward compensated artificial potential field method (FC-APF) is adopted to generate speed commands for guiding the underwater unmanned vehicle based on the position estimate, velocity estimate, estimation error covariance matrix and preset target point position and obstacle information. S3 specifically includes: S301: Based on the 3σ probability envelope principle, the dynamic safety margin is calculated according to the position sub-covariance matrix; Based on the 3σ probability envelope principle, the positioning uncertainty is mapped to the increment of the obstacle safety boundary, and the dynamic safety margin is calculated: ; in, κ represents the basic safety margin; κ is the risk modulation coefficient. Here, represents the position sub-covariance matrix; `trace(∙)` denotes the trace of the matrix; this margin is added to the base radius of each obstacle to adjust the obstacle avoidance boundary in path planning. When positioning uncertainty increases, the safety margin automatically expands, reserving sufficient buffer space for the UUV; when positioning uncertainty decreases, the safety margin shrinks accordingly, allowing the UUV to navigate closer to obstacles while ensuring safety. This mechanism enables obstacle avoidance behavior to adaptively adjust with changes in positioning accuracy, effectively solving the problem of balancing safety and feasibility in complex environments with fixed safety boundaries.

[0040] S302: Construct a gravitational field around the target point and generate a gravitational vector pointing towards the target point; The gravitational vector is represented as: ; in, For gravitational gain; The target point's position vector; δ is the estimated current position vector of the underwater unmanned vehicle; δ is a small constant to prevent division by zero. The magnitude of gravity is related to the distance between the UUV and the target point; the closer the distance, the weaker the gravity, thus avoiding oscillations near the target point.

[0041] S303: Construct a repulsive field centered on the location of the obstacle in the environment, and superimpose the dynamic safety margin onto the basic radius of the obstacle to generate a repulsive vector; For each obstacle i, calculate the repulsive force it generates: ; in, Let i be the repulsive force vector generated by the i-th obstacle; For repulsive force gain; Let be the distance between the underwater unmanned vehicle and the expanded surface of the i-th obstacle; Let be the center position vector of the i-th obstacle; Let be the base radius of the i-th obstacle; The distance is affected by the repulsive force; This safety margin is added to the base radius of each obstacle, enabling adaptive adjustment of the obstacle avoidance boundary: the safety margin is automatically expanded when the positioning uncertainty increases and automatically contracted when the uncertainty decreases, solving the problem that fixed safety boundaries cannot balance safety and feasibility.

[0042] S304: Superimpose a tangential auxiliary force component on a tangential plane perpendicular to the repulsive force direction to guide the underwater unmanned vehicle to smoothly circumnavigate along the edge of the obstacle; The auxiliary force component is represented as: ; Where γ is the tangential coefficient; Let be the norm of the total repulsive force vector; This is a rotation matrix for a 90-degree rotation; This is the current position estimation vector for the underwater unmanned vehicle; this tangential auxiliary force helps guide the vehicle to smoothly navigate around the edge of obstacles, reducing the risk of traditional artificial potential field methods getting stuck in local minima in areas with dense obstacles.

[0043] S305: Compare the measured velocity after offset correction with the velocity command at the previous moment, calculate the velocity deviation, and smooth it with a low-pass filter to obtain the estimated value of the ocean current disturbance velocity; The estimated velocity of ocean current disturbance is expressed as follows: ; in, The estimated ocean current disturbance velocity vector is denoted as ; LowPassFilter(∙) is the low-pass filter function.

[0044] S306: Combine the gravitational vector, repulsive vector, and tangential auxiliary force vector into a resultant force vector, combine it with the ocean current disturbance velocity estimate, generate a velocity command including a feedforward compensation term, and perform amplitude constraint processing on the velocity command to obtain the final velocity command.

[0045] The speed command is expressed as: ; ; in, Let the force vector be the force vector in the total potential field. The gravitational vector, It is the repulsive force vector. The auxiliary force vector is the tangential force vector. This is the vector of estimated ocean current disturbance velocity values.

[0046] The speed command is subjected to amplitude constraints to ensure that it does not exceed the maximum speed of the UUV: ; in, is the preset maximum permissible speed of the UUV, and sat(∙) is the saturation function.

[0047] By combining the potential field force vector with the estimated ocean current disturbance velocity, a final velocity command including a feedforward compensation term is generated. This feedforward compensation mechanism enables UUVs to actively resist ocean current disturbances, improving path tracking accuracy in strongly disturbed environments.

[0048] S4: Controls the underwater unmanned vehicle based on speed commands and switches navigation modes according to the operating status of the underwater unmanned vehicle.

[0049] S4 specifically includes: S401: Converts speed commands into thrust or speed commands for each of the underwater unmanned vehicle's thrusters, driving the underwater unmanned vehicle to move along the planned path; S402: Collect new sensor data and return to step S1. Repeat steps S1 to S4 to form closed-loop navigation control. S403: Real-time monitoring of the distance between the underwater unmanned vehicle and the target point, as well as the trace of the position covariance matrix; S404: When the distance between the underwater unmanned vehicle and the target point is less than a preset distance threshold and the trace of the position covariance matrix is ​​less than a preset positioning accuracy threshold, the underwater unmanned vehicle is determined to have entered the near-field docking phase, and the artificial potential field parameters are automatically adjusted, including reducing the repulsive gain and increasing the gravitational gain.

[0050] The spacecraft is considered to have entered the near-field docking phase when the following conditions are met: and ; in, The distance between the UUV and the target point; This is a preset distance threshold; This is a preset positioning accuracy threshold. This criterion ensures that the UUV will only switch to docking mode when it is sufficiently close to the target and the positioning is accurate enough, avoiding instability caused by premature switching.

[0051] Upon entering the near-field docking phase, a switching command is sent, automatically adjusting the potential field parameters: reducing the repulsive gain and increasing the attractive gain. This adjustment allows the UUV to smoothly switch from global obstacle avoidance mode to high-precision docking mode, effectively avoiding the target unreachability or oscillation problems that easily occur near the target point in traditional Artificial Potential Field (APF) methods, and achieving centimeter-level docking at the end point by utilizing the characteristics of high-frequency magnetic fields. The switched parameters continue to apply until docking is complete.

[0052] Example 2: A cooperative navigation system for autonomously recoverable underwater unmanned vehicles (UUVs) employs a cooperative navigation method for autonomously recoverable UUVs, comprising: The data acquisition module is used to acquire multi-source sensor data from the underwater unmanned vehicle and to preprocess the multi-source sensor data. The adaptive state estimation module, connected to the data acquisition module, is used to obtain the position estimate, velocity estimate, and estimation error covariance matrix of the underwater unmanned vehicle at the current moment based on the preprocessed multi-source sensor data and by using adaptive unscented Kalman filtering. The dynamic potential field path planning module, connected to the adaptive state estimation module, is used to generate speed commands to guide the underwater unmanned vehicle by adopting the feedforward compensated artificial potential field method, based on the position estimate, velocity estimate, estimation error covariance matrix, and preset target point position and obstacle information. The closed-loop control module, connected to the dynamic potential field path planning module, is used to convert speed commands into control commands and drive the underwater unmanned vehicle to move. The mode switching module is connected to the adaptive state estimation module and the dynamic potential field path planning module, respectively, and is used to switch the navigation mode according to the operating state of the underwater unmanned vehicle.

[0053] In the system, the modules are connected in sequence to form a closed-loop information flow. The uncertainty information of the state estimation is explicitly transmitted to the path planning module. The speed command generated by the path planning is applied to the UUV through the control module. The motion state of the UUV is then fed back to the state estimation module through new observation data.

[0054] Example 3: To further demonstrate the effectiveness of the method provided in this application, verification is conducted from three aspects: state estimation performance, path planning performance, and the impact of positioning uncertainty on obstacle avoidance planning performance, as detailed below: First, the performance of state estimation is analyzed. Under the same sensor input conditions, the performance is compared with that of the Extended Kalman Filter (EKF), the Standard Unscented Kalman Filter (UKF), and the AUKF proposed in this application. The quantization results are shown in Table 1 and... Figure 5 As shown in the figure. The results indicate that the EKF exhibits significant performance degradation in regions with strong nonlinearity in the magnetic field model, especially producing large positioning fluctuations near magnetic beacons. Although the standard UKF theoretically improves nonlinearity handling capabilities, its fixed observation noise covariance matrix R leads to over-reliance on noisy observations in weak magnetic signal regions, resulting in unstable estimation results. In contrast, the AUKF demonstrates a significant advantage in positioning accuracy, with a root mean square error (RMSE) of only 1.3249 m, representing a 60.77% improvement over the EKF and an 88.11% improvement over the standard UKF.

[0055] Table 1. Quantitative Comparison of Positioning Accuracy (Unit: m)

[0056] Secondly, regarding path planning performance, the trajectory performance of the conventional artificial potential field method and the FC-APF proposed in this paper were compared in complex ocean current environments. The comparison results are shown in Table 2. Figure 6 and Figure 7 As shown in the figure. Experiments show that the maximum drift is reduced by approximately 25.40%. Even under strong disturbances, it can maintain the desired course, with a smoother trajectory that is closer to the shortest feasible path, and lateral drift is significantly suppressed. Figure 6 and Figure 7 The advantages of FC-APF in terms of smoothness and stability were confirmed from both spatial trajectory and time series dimensions.

[0057] Table 2 Quantitative Comparison of Anti-interference Performance

[0058] Furthermore, the impact of positioning uncertainty on obstacle avoidance planning performance is analyzed. A comparative experiment constructs a reference system with decoupled perception and planning, whose path planner uses a fixed safety boundary and does not utilize state estimation covariance information. The experimental results are shown in Table 3. Figure 8 As shown. Figure 9 This further demonstrates that variations in positioning error directly impact the effectiveness of obstacle avoidance strategies. In contrast, the perception-planning coupled architecture proposed in this application can dynamically adjust safety boundaries based on real-time positioning uncertainties.

[0059] Table 3 Adaptive obstacle avoidance parameter table under the perception-planning coupled framework

[0060] In summary, the proposed solution demonstrates significant advantages across all indicators.

[0061] Therefore, this application adopts the aforementioned cooperative navigation method and system for autonomous underwater unmanned vehicles (UUVs) for autonomous recovery. By coupling adaptive unscented Kalman filter state estimation with feedforward compensated artificial potential field path planning in a closed loop, an integrated cooperative navigation architecture of "perception-planning-control" is formed. This achieves explicit transmission of state estimation uncertainty to path planning, adaptive adjustment of obstacle avoidance safety boundaries, active feedforward compensation for environmental disturbances, and smooth switching from far-field cruising to near-field docking. It effectively solves the technical problems of decoupling state estimation and path planning, the inability of fixed safety boundaries to adapt to changes in uncertainty, insufficient compensation for environmental disturbances, and unsmooth mode switching in the prior art. This ensures the safety, stability, and docking accuracy of the UUV in the autonomous recovery scenario of the mother vessel with significant positioning uncertainty and complex environmental disturbances.

[0062] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application and not to limit them. Although this application has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of this application, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of this application.

Claims

1. A cooperative navigation method for autonomous underwater unmanned vehicles, characterized in that, Includes the following steps: S1: Acquire multi-source sensor data from the underwater unmanned vehicle and preprocess the multi-source sensor data; the multi-source sensor data includes triaxial acceleration, angular velocity data, environmental magnetic field strength data, pre-set magnetic beacon positions, target point positions and obstacle information; S2: Based on the preprocessed multi-source sensor data, an adaptive unscented Kalman filter is used to obtain the position estimate, velocity estimate, and estimation error covariance matrix of the underwater unmanned vehicle at the current moment; S3: The feedforward compensated artificial potential field method is adopted to generate a speed command for guiding the underwater unmanned vehicle based on the position estimate, velocity estimate, estimation error covariance matrix and preset target point position and obstacle information. S4: Controls the underwater unmanned vehicle based on speed commands and switches navigation modes according to the operating status of the underwater unmanned vehicle.

2. The cooperative navigation method for autonomous underwater unmanned vehicles according to claim 1, characterized in that, Preprocessing specifically includes: Timestamps were added to the acquired triaxial acceleration, angular velocity, and ambient magnetic field strength data, and time alignment was performed on the triaxial acceleration, angular velocity, and ambient magnetic field strength data based on a unified time reference to form a synchronous sensor dataset. Perform data validity checks on the synchronous sensor dataset and remove outlier measurements. The data after rejection is filtered to reduce high-frequency noise.

3. The cooperative navigation method for autonomous underwater unmanned vehicles according to claim 2, characterized in that, S2 specifically includes: S201: Initialize filter parameters and define the system state vector containing the position and velocity components of the underwater unmanned vehicle; S202: Based on the preprocessed triaxial acceleration and angular velocity data, the measured velocity is obtained through integration. The measured velocity is corrected using the velocity bias estimate updated at the previous moment. Based on the corrected velocity, state prediction is performed to obtain the predicted state and the predicted state covariance matrix. S203: Establish the magnetic field strength observation equation based on the magnetic dipole theory, and calculate the predicted value of the magnetic field strength according to the position component in the predicted state; S204: Based on the predicted magnetic field strength and the environmental magnetic field strength data, the observation residuals are obtained; S205: Adaptively adjust the observation noise covariance matrix based on environmental magnetic field strength data; S206: Generate a Sigma point set and its corresponding weights through unscented transformation, and substitute the Sigma points into the magnetic field strength observation equation to calculate the predicted observation value; S207: Based on the predicted observations, obtain the observation mean, observation covariance, and cross-covariance between state and observation. Based on the observation covariance and cross-covariance between state and observation, obtain the Kalman gain. S208: Based on the observation residuals, environmental magnetic field strength data and Kalman gain, update the state estimate, obtain the updated state estimate, and update the predicted state covariance matrix to obtain the estimation error covariance matrix; S209: Based on the difference between the velocity component in the updated state estimate and the measured velocity, update the velocity offset estimate at the current moment in real time, which will be used to correct the measured velocity in the next moment S202. S210: Based on the estimation error covariance matrix, obtain the position sub-covariance matrix, and output the position estimate, velocity estimate, estimation error covariance matrix, and position sub-covariance matrix at the current time.

4. The cooperative navigation method for autonomous underwater unmanned vehicles according to claim 3, characterized in that, The state prediction equation is expressed as: ； in, For the state estimation of the previous time step, The measured velocity after bias correction. The sampling period is This is process noise; The equation for observing magnetic field strength is expressed as: ； Where B(r) is the predicted magnetic field strength, and r is the position of the underwater unmanned vehicle. Here, m represents the position of the magnetic beacon, and m represents the magnetic dipole moment. The vacuum permeability; The observation residuals are expressed as: ； in, To observe the residual vector, This is a vector of measured magnetic field strength values. This is the vector of predicted magnetic field strength values.

5. A cooperative navigation method for autonomous underwater unmanned vehicles according to claim 4, characterized in that, The adaptive adjustment strategy is expressed as: ； in, This is the adaptively adjusted observation noise covariance matrix; The preset minimum observation noise covariance matrix threshold; η is the basic observation noise covariance matrix; η is the adaptive gain constant; Measured value of magnetic induction intensity The Euclidean norm; ε is the threshold for determining weak magnetic field; The fixed observation noise covariance matrix used in the weak magnetic region; Kalman gain is expressed as: ； in, The Kalman gain matrix; Let be the cross-covariance matrix between the state vector and the observation vector; Let be the covariance matrix of the observation vectors.

6. A cooperative navigation method for autonomous underwater unmanned vehicles according to claim 5, characterized in that, S3 specifically includes: S301: Based on the 3σ probability envelope principle, the dynamic safety margin is calculated according to the position sub-covariance matrix; S302: Construct a gravitational field around the target point and generate a gravitational vector pointing towards the target point; S303: Construct a repulsive field centered on the location of the obstacle in the environment, and superimpose the dynamic safety margin onto the basic radius of the obstacle to generate a repulsive vector; S304: Superimpose a tangential auxiliary force component on a tangential plane perpendicular to the repulsive force direction to guide the underwater unmanned vehicle to smoothly circumnavigate along the edge of the obstacle; S305: Compare the measured velocity after offset correction with the velocity command at the previous moment, calculate the velocity deviation, and smooth it with a low-pass filter to obtain the estimated value of the ocean current disturbance velocity; S306: Combine the gravitational vector, repulsive vector, and tangential auxiliary force vector into a resultant force vector, combine it with the ocean current disturbance velocity estimate, generate a velocity command including a feedforward compensation term, and perform amplitude constraint processing on the velocity command to obtain the final velocity command.

7. A cooperative navigation method for autonomous underwater unmanned vehicles according to claim 6, characterized in that, The dynamic safety margin is expressed as: ； in, κ represents the basic safety margin; κ is the risk modulation coefficient. Let be the positional subcovariance matrix; trace(∙) denotes the trace of the matrix; The gravitational vector is represented as: ； in, For gravitational gain; The target point's position vector; δ is the current position estimation vector of the underwater unmanned vehicle; δ is a small constant to prevent division by zero. The repulsive force vector is represented as: ； in, Let i be the repulsive force vector generated by the i-th obstacle; For repulsive force gain; Let be the distance between the underwater unmanned vehicle and the expanded surface of the i-th obstacle; Let be the center position vector of the i-th obstacle; Let be the base radius of the i-th obstacle; The distance is affected by the repulsive force; The auxiliary force component is represented as: ； Where γ is the tangential coefficient; Let be the norm of the total repulsive force vector; This is a rotation matrix for a 90-degree rotation; This is the estimated vector of the current position of the underwater unmanned vehicle.

8. A cooperative navigation method for autonomous underwater unmanned vehicles according to claim 7, characterized in that, The speed command is expressed as: ； ； in, Let the force vector be the force vector in the total potential field. The gravitational vector, It is the repulsive force vector. The tangential auxiliary force vector, This is the vector of estimated ocean current disturbance velocity values.

9. A cooperative navigation method for autonomous underwater unmanned vehicles according to claim 8, characterized in that, S4 specifically includes: S401: Converts speed commands into thrust or speed commands for each of the underwater unmanned vehicle's thrusters, driving the underwater unmanned vehicle to move along the planned path; S402: Collect new sensor data and return to step S1. Repeat steps S1 to S4 to form closed-loop navigation control. S403: Real-time monitoring of the distance between the underwater unmanned vehicle and the target point, as well as the trace of the position covariance matrix; S404: When the distance between the underwater unmanned vehicle and the target point is less than a preset distance threshold and the trace of the position covariance matrix is ​​less than a preset positioning accuracy threshold, the underwater unmanned vehicle is determined to have entered the near-field docking phase, and the artificial potential field parameters are automatically adjusted, including reducing the repulsive gain and increasing the gravitational gain.

10. A cooperative navigation system for autonomously recoverable underwater unmanned vehicles, employing a cooperative navigation method for autonomously recoverable underwater unmanned vehicles as described in any one of claims 1-9, characterized in that, include: The data acquisition module is used to acquire multi-source sensor data from the underwater unmanned vehicle and to preprocess the multi-source sensor data. The adaptive state estimation module, connected to the data acquisition module, is used to obtain the position estimate, velocity estimate, and estimation error covariance matrix of the underwater unmanned vehicle at the current moment based on the preprocessed multi-source sensor data and by using adaptive unscented Kalman filtering. The dynamic potential field path planning module, connected to the adaptive state estimation module, is used to generate speed commands to guide the underwater unmanned vehicle by adopting the feedforward compensated artificial potential field method, based on the position estimate, velocity estimate, estimation error covariance matrix, and preset target point position and obstacle information. The closed-loop control module, connected to the dynamic potential field path planning module, is used to convert speed commands into control commands and drive the underwater unmanned vehicle to move. The mode switching module is connected to the adaptive state estimation module and the dynamic potential field path planning module, respectively, and is used to switch the navigation mode according to the operating state of the underwater unmanned vehicle.

Images