Submersible unmanned aerial vehicle underwater autonomous flight control method and system

Abstract

The invention relates to the technical field of underwater robots, discloses an underwater autonomous flight control method and system for a submersible type unmanned aerial vehicle, and aims to solve the technical defects of rigid route planning, high energy consumption and poor task adaptability of an existing submersible type unmanned aerial vehicle in complex dynamic underwater environment flight control. The method comprises the steps of executing multi-modal underwater environment perception, constructing an environment state fusion model under a unified space-time reference, generating a predictive hydrodynamic field four-dimensional model, executing multi-target route optimization based on energy and risk cost, and implementing self-adaptive flight control based on feedforward compensation. By adopting the above technical scheme, the method can effectively reduce the energy consumption of the unmanned aerial vehicle, remarkably prolong the operation endurance time, improve the intelligence and task adaptability of route planning, and improve the flight stability and trajectory tracking precision. Therefore, the autonomy, the economy and the task success rate in an unknown, unstructured and strong dynamic underwater environment are enhanced.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot technology, and more specifically, relates to an underwater autonomous flight control method and system for a submersible unmanned aerial vehicle. Background Technology

[0002] The field of unmanned aerial vehicle (UAV) technology encompasses multiple branches, including unmanned aerial vehicles, unmanned ground vehicles, and unmanned underwater vehicles. The core of this technology lies in achieving autonomous or remote operation of unmanned platforms through the integration of advanced sensors, control systems, and communication modules. UAVs are automated equipment capable of performing specific tasks autonomously or under remote guidance. With the development of materials science, navigation technology, and energy technology, UAV technology has been widely applied in scenarios such as military reconnaissance, environmental monitoring, resource exploration, and logistics transportation. The development of this field has not only improved the efficiency and safety of traditional operations but has also triggered profound technological changes in industries such as marine development and agricultural management.

[0003] Among them, the underwater autonomous flight control method for submersible unmanned aerial vehicles (UAVs) refers to the technology that enables UAVs to autonomously plan paths, adjust attitudes, and dynamically avoid obstacles by sensing complex underwater environmental information without human intervention, thereby achieving stable navigation and completing preset tasks. This method integrates data from multiple sensors, including sonar, depth gauges, and inertial measurement units, combined with hydrodynamic models and control algorithms, to perform real-time calculation and precise control of the UAV's motion state. In this method, the UAV autonomously decides its navigation strategy based on environmental feedback and mission objectives, thereby improving its operational capabilities and intelligence level in unknown or dynamic waters. The method also optimizes control laws to adapt to different water flow disturbances and load changes.

[0004] Existing technologies for underwater flight control typically rely on pre-established idealized hydrodynamic models, lacking the ability to adapt online to dynamic disturbances such as complex water flow and sudden density changes in real-world environments, resulting in insufficient accuracy in flight attitude control. Sensor data fusion mechanisms often employ static weighting, which struggles to effectively suppress the cumulative errors of inertial navigation systems over long endurance periods and the signal noise from acoustic sensors in turbid water, leading to inaccurate positioning and environmental perception. Insufficient delay compensation between control commands and actuator responses, especially in highly maneuverable scenarios such as emergency obstacle avoidance, fails to adequately assess the nonlinear saturation effects of the propulsion system, easily causing control overshoot or oscillations. Low coupling between the perception and control systems, and the failure to establish a feedback loop that dynamically adjusts the control strategy based on environmental perception confidence levels, results in poor robustness of path planning. This problem is particularly pronounced in long-endurance, deep-sea autonomous missions, directly impacting the mission success rate and flight safety of UAVs. Summary of the Invention

[0005] The purpose of this invention is to overcome the technical defects of existing underwater unmanned aerial vehicles (UAVs) when performing flight control in complex and dynamic underwater environments, which are caused by the inability to effectively perceive, predict and utilize the hydrodynamic environment, resulting in rigid flight path planning, excessive energy consumption and poor mission adaptability. Therefore, this invention proposes an underwater autonomous flight control method and system for underwater UAVs.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: an underwater autonomous flight control method for a submersible unmanned aerial vehicle, comprising the following steps: S1, perform multimodal underwater environmental perception. The acoustic Doppler current profiler, forward-looking sonar, turbidity sensor and pressure sensor on the UAV simultaneously collect raw hydrodynamic data of the UAV's current position, acoustic image data of obstacles in the three-dimensional space in front, water turbidity value and absolute depth value. At the same time, the airborne inertial measurement unit acquires the UAV's own three-dimensional attitude, angular velocity and linear acceleration data to generate a real-time environmental dataset containing multi-source heterogeneous sensor data.

[0007] S2. Construct an environmental state fusion model under a unified spatiotemporal reference. Based on the real-time environmental dataset, use the federated Kalman filter algorithm to fuse multi-source data. Utilize the inertial measurement unit data to perform high-frequency attitude and position trajectory estimation. Use the water flow velocity vector measured by the acoustic Doppler current profiler to correct the cumulative error of the trajectory estimation. Simultaneously, combine the depth value measured by the pressure sensor to correct the vertical position. Parse the acoustic image data of the forward-looking sonar into a three-dimensional spatial point cloud to construct a local static obstacle map. Finally, output a unified, high-precision environmental state vector.

[0008] S3. Generate a predictive four-dimensional hydrodynamic field model. Centered on the real-time position of the UAV in the environmental state vector, a pre-defined three-dimensional computational grid is defined. The local water flow velocity vector in the environmental state vector and the local static obstacle map are used as the initial and boundary conditions of the computational grid. The lattice Boltzmann method based on graphics processor acceleration is used to iteratively solve the fluid dynamics equations within the grid, thereby predicting and generating a four-dimensional (three-dimensional space plus one-dimensional time) water flow velocity vector field model that covers the computational grid and includes a pre-defined time window in the future.

[0009] S4. Perform multi-objective route optimization based on energy and risk costs. On the basis of the predictive hydrodynamic field four-dimensional model, a real-time repair A-satellite search algorithm is used for route planning. The node transfer cost function of the algorithm is defined as a weighted combination, which includes the predicted energy consumption value, predicted time cost, and predicted exposure risk value for moving from the current node to the next node. The predicted energy consumption value is calculated based on the water flow vector of the corresponding path segment in the UAV hydrodynamic model and the predictive hydrodynamic field four-dimensional model. The predicted exposure risk value is determined based on the correspondence between the water turbidity value and the route path. Finally, a four-dimensional spatiotemporal route point sequence with the lowest comprehensive cost under the premise of meeting the mission objectives is solved and output.

[0010] S5. Implement adaptive flight control based on feedforward compensation, constructing a cascaded proportional-integral-derivative controller. Its outer loop controller calculates the desired flight velocity vector based on the four-dimensional spatiotemporal track point sequence and the real-time position of the UAV in the environmental state vector. Its inner loop controller calculates the control commands required to drive the UAV's thrusters and control surfaces based on the deviation between the desired velocity vector and the real-time velocity vector. At the same time, the controller introduces a feedforward compensation loop, which directly extracts the predicted hydrodynamic disturbance data of the UAV's upcoming position from the four-dimensional predictive hydrodynamic field model and converts it into a compensation control quantity to offset the predicted water flow impact in advance, and finally generates and outputs a precise control signal to the actuator.

[0011] In a preferred embodiment of the present invention, the real-time environmental dataset includes the following data fields: velocity components of the water body in the east, north, and sky directions generated by a four-beam phased array acoustic Doppler current profiler with a center frequency of 300 kHz; a two-dimensional fan-shaped acoustic grayscale image generated by a forward-looking phased array sonar with a center frequency of 750 kHz; turbidity values ​​of the water body generated by a turbidity sensor based on the near-infrared light scattering principle, in turbidity units (NTU); hydrostatic pressure values ​​generated by a piezoresistive absolute pressure sensor; and angular velocity and linear acceleration values ​​along the three axes of the body coordinate system generated by a six-axis microelectromechanical system inertial measurement unit.

[0012] In a preferred embodiment of the present invention, the environmental state vector is a set of structured data, which specifically includes: the three-dimensional position coordinates (longitude, latitude, and depth) of the UAV in the global geographic coordinate system, the three-dimensional velocity vector, the three-dimensional attitude angles (roll angle, pitch angle, and heading angle), and a local environmental map with a radius of 200 meters centered on the UAV. The map contains two data layers: a static obstacle point cloud layer and a discretized water flow vector field layer, wherein each vector represents the direction and velocity of the water flow at its spatial location.

[0013] As a further aspect of the present invention, the construction steps of the federated Kalman filter algorithm are as follows: First, independent sub-filters are established for the inertial measurement unit, the acoustic Doppler current profiler, and the pressure sensor. Each sub-filter performs local optimal estimation of some components of the UAV state based on the physical model and noise characteristics of its corresponding sensor. Second, a master filter is established. This master filter does not directly process the original sensor data, but periodically collects the local state estimates and their covariance matrices output by all sub-filters. Finally, the master filter performs weighted fusion of the information from each sub-filter according to the information allocation principle to generate a globally optimal environmental state vector, and feeds back the fused global state to each sub-filter for resetting and correction, thereby ensuring the stability and accuracy of the entire fusion system.

[0014] As a preferred embodiment of the present invention, the generation steps of the predictive hydrodynamic field four-dimensional model are as follows: First, initialize a Cartesian coordinate system computational grid centered on the UAV with dimensions of 200 meters long, 200 meters wide, and 100 meters high, with a grid resolution of one meter; second, map the static obstacle point cloud data in the environmental state vector to the computational grid, defining it as a fixed no-slip boundary; third, use the discretized water flow vector field data in the environmental state vector as the initial flow field distribution; then, on a dedicated embedded graphics processor, perform parallel computation using the lattice Boltzmann method of the D3Q19 model (three-dimensional space, nineteen discrete velocity directions) to simulate the collision and migration process of fluid particles between grids, with a time step set to 0.1 seconds, predicting the flow field evolution for the next 60 seconds; finally, store the full grid velocity vector field data calculated at each time step to form a four-dimensional array, i.e., the predictive hydrodynamic field four-dimensional model.

[0015] In a preferred embodiment of the present invention, the specific mathematical expression of the node transfer cost function is as follows: ,in To move to node Total cost To predict energy consumption values, To predict time costs, To predict exposure risk values, , , r is a preset weighting coefficient, and the sum of the three is one. The calculation formula is: ,in For the drone propulsion system at relative speed The power consumption function under the condition of relative speed, while the relative speed It is the vector difference between the UAV's ground velocity and the water flow velocity at the corresponding spatiotemporal location in the four-dimensional model of the predictive hydrodynamic field. The calculation is based on the average turbidity value of the area traversed by the path segment; the higher the turbidity, the higher the risk value. The lower.

[0016] As a preferred embodiment of the present invention, the implementation steps of the feedforward compensation link in the cascaded proportional-integral-derivative controller are as follows: First, extract the path points within a short time period (e.g., five seconds) from the planned four-dimensional spatiotemporal trajectory point sequence; second, query the predictive hydrodynamic field four-dimensional model to obtain the predicted water flow velocity vectors corresponding to these future path points; third, based on the hydrodynamic model of the UAV, calculate the additional thrust and rudder angle required to counteract the forces and torques generated by these predicted water flow vectors; finally, use these calculated additional thrust and rudder angles as feedforward signals and linearly superimpose them with the feedback control signals calculated by the inner loop controller to form the final control command output.

[0017] Compared with existing technologies, the advantages and positive effects of this invention are as follows: By constructing a complete closed-loop system from environmental perception, state fusion, dynamic prediction, route optimization to adaptive control, this invention enables submersible UAVs to perceive and utilize the dynamic underwater environment. First, through information fusion from multimodal sensors, it ensures that the UAV has accurate and reliable perception of its own state and surrounding environment, providing a high-quality data foundation for subsequent decision-making. Second, the core lies in the introduction of a predictive four-dimensional hydrodynamic field model, enabling the UAV to shift from passively adapting to the environment to actively predicting and utilizing it. It no longer blindly fights against water currents but, like aquatic organisms, calculates and utilizes energy-efficient water flow channels, thereby significantly reducing energy consumption and extending operational endurance. Furthermore, the multi-objective route optimization mechanism incorporates multiple dimensions such as energy, time, and risk, making route planning more intelligent and task-adaptive, enabling efficient task completion while ensuring safety and stealth. Finally, the adaptive control system with feedforward compensation can proactively respond to upcoming water flow disturbances, making the UAV's flight attitude more stable and trajectory tracking more accurate, improving operational performance and survivability in harsh sea conditions. Overall, the method proposed in this invention elevates the flight control of unmanned aerial vehicles (UAVs) to a level of cognitive intelligence, significantly enhancing their autonomy, economy, and mission success rate in unknown, unstructured, and highly dynamic underwater environments. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the overall process of the underwater autonomous flight control method for a submersible unmanned aerial vehicle provided in an embodiment of the present invention; Figure 2This is a schematic diagram of the functional modules of the underwater unmanned aerial vehicle system provided in an embodiment of the present invention; Figure 3 This is a detailed flowchart illustrating the environmental state fusion processing procedure in an embodiment of the present invention. Figure 4 This is a schematic diagram of the adaptive flight control module in an embodiment of the present invention; Figure 5 This is a schematic diagram illustrating an application scenario for route optimization provided by an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. In the description of this invention, it should be understood that terms such as "center," "upper," "lower," "left," "right," "longitudinal," "lateral," "inner," and "outer," indicating orientation or positional relationships, are based on the orientation or positional relationships shown in the accompanying drawings and are only for the convenience of describing the invention and simplifying the description. They 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, and therefore should not be construed as a limitation of the invention. Furthermore, in the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0020] Reference Figure 1 This invention provides a method for autonomous underwater flight control of a submersible unmanned aerial vehicle (UAV) according to a specific embodiment. The overall flowchart illustrates a complete closed loop from environmental perception to flight control. Specifically, the method may include the following steps: Step S1: Perform multimodal underwater environment perception, and acquire raw data on the UAV's own motion state and the surrounding hydrodynamic environment through the onboard sensor suite.

[0021] Step S2: Construct an environmental state fusion model under a unified spatiotemporal reference, process multi-source heterogeneous sensor data, and generate a high-precision, unified environmental state vector.

[0022] Step S3: Generate a predictive four-dimensional hydrodynamic field model and predict the evolution of the water flow velocity field in a local water area over a future period through fluid dynamics simulation.

[0023] Step S4: Perform multi-objective route optimization based on energy and risk costs, and plan the four-dimensional spatiotemporal trajectory with the lowest overall cost based on the predicted hydrodynamic field model.

[0024] Step S5: Implement adaptive flight control based on feedforward compensation to accurately track the planned trajectory and counteract foreseeable water flow disturbances in advance.

[0025] Reference Figure 2 The diagram illustrates a functional module of a submersible unmanned aerial vehicle (UAV) system that implements the aforementioned method. The system includes an environmental perception module, a state fusion module, a hydrodynamic field prediction module, a route optimization module, and a flight control module. The steps are described in detail below.

[0026] Step S1: Perform multimodal underwater environment sensing. In a specific embodiment, this step is broken down into multiple sub-steps to ensure the synchronization and integrity of data acquisition.

[0027] Step S101: Sensor Suite Initialization and Synchronization. During the task startup phase, the central processing unit (CPU) sends initialization commands to each sensor within the environmental perception module. These commands include the configuration of each sensor's operating parameters, such as the sampling frequency, binsize, and blind zone range of the acoustic Doppler current profiler; the scanning sector angle and maximum detection range of the forward-looking sonar; and the data output rate of the inertial measurement unit. Simultaneously, the system precisely synchronizes the data acquisition timestamps of all sensors using an internal clock source based on Network Time Protocol (NTP) or Precise Time Protocol (PTP), ensuring the consistency of the reference for subsequent data fusion. Time synchronization accuracy must be at the millisecond level.

[0028] Step S102: Parallel acquisition of multi-source heterogeneous raw data. After initialization, each sensor operates in parallel according to its configuration parameters.

[0029] Specifically, in sub-step S1021, an acoustic Doppler current profiler (ADCP) measures the water flow velocity profile. This ADCP is preferably a four-beam phased array system with a center frequency of 300 kHz, using a Janus configuration (the four beams are orthogonal to each other, forming an angle of 20 to 30 degrees with the vertical). It transmits acoustic pulses to the water body at a frequency of 1 Hz and receives the echoes. By analyzing the Doppler frequency shift of the echo signals from different depths, the velocity vectors of the water body relative to the UAV within a series of depth bins below the UAV are calculated. After coordinate transformation, the three velocity components of the water body in the East, North, and Up geographic coordinate systems are finally output.

[0030] As a data instance, at time T0, ADCP might output a data stream in the following format: {timestamp:1672531200000,status:0x00,data:[{bin_depth:1.0m,Ve:0.12m / s,Vn:-0.04m / s,Vu:0.01m / s,correlation:95%},{bin_depth:2.0m,Ve:0.15m / s,Vn:-0.06m / s,Vu:0.01m / s,correlation:92%},...]}, where correlation is the correlation coefficient, representing the confidence level of the velocity measurement value of this depth cell.

[0031] In sub-step S1022, a forward-looking sonar (FLS) detects obstacles ahead. This FLS is preferably a forward-looking phased array sonar with a center frequency of 750 kHz, capable of generating a fan-shaped imaging region of 130 degrees horizontally and 20 degrees vertically. It generates a two-dimensional fan-shaped acoustic grayscale image at a frequency of 2 Hz. The brightness of each pixel in the image represents the acoustic reflection intensity of an object at the corresponding spatial direction and distance.

[0032] As a data example, at time T0.5, FLS outputs a two-dimensional unsigned 8-bit integer array of size 512x1024, where the array index corresponds to the azimuth and range of the sonar, and the array value range is 0 to 255, representing the mapping of acoustic reflection intensity from zero to strong.

[0033] In sub-step S1023, auxiliary environmental sensors perform scalar physical quantity measurements. Among these, a turbidity sensor, based on the near-infrared light (e.g., wavelength 850 nm) scattering principle, measures the turbidity value of the water body in turbidity units (NTUs) at a sampling frequency of 1 Hz. A piezoresistive absolute pressure sensor measures the hydrostatic pressure at the depth where the UAV is located in Pascals (Pa) at a sampling frequency of 10 Hz.

[0034] As a data example, at time T0, the turbidity sensor outputs {turbidity: 21.3 NTU}, and the pressure sensor outputs {pressure: 607950 Pa}, the latter corresponding to a water depth of approximately 50 meters.

[0035] In sub-step S1024, the airborne inertial measurement unit (IMU) performs high-frequency measurements of its own motion state. This IMU is preferably a six-axis microelectromechanical system (MEMS) inertial measurement unit integrating a three-axis gyroscope and a three-axis accelerometer. It continuously outputs the three-axis angular velocities (p, q, r) and three-axis linear accelerations (ax, ay, az) in the UAV's body coordinate system at a frequency of not less than 200 Hz.

[0036] As a data example, at time T0+0.005s, the IMU outputs... {omega_body:{p:0.02rad / s,q:-0.01rad / s,r:0.08rad / s},accel_body:{ax:0.05m / s^2,ay:0.03m / s^2,az:-9.81m / s^2}}.

[0037] Step S103: Generate a real-time environment dataset. This step aggregates and encapsulates all the parallel-acquired raw data streams with precise timestamps into a structured data frame. This dataset is a collection of multi-source heterogeneous sensor data, serving as the sole input to the subsequent state fusion module. The structure of this dataset can be defined as a record (struct) containing multiple fields, each field corresponding to the output of a sensor, with attached state flag bits.

[0038] It should be noted that this step includes a preliminary data verification mechanism. For example, a Cyclic Redundancy Check (CRC) is performed on each received data packet. If the verification fails, or the sensor data value exceeds a preset reasonable physical range (e.g., the pressure value suddenly drops to zero, or the ADCP correlation coefficient is below 50%), the corresponding status flag in the dataset is marked, but the original data is retained and processed by subsequent fusion algorithms. For example, if the ADCP data is abnormal, the corresponding bit in the sensor_status field is set to 1.

[0039] Step S2: Construct an environmental state fusion model under a unified spatiotemporal benchmark. (Refer to...) Figure 3 The purpose of this step is to fuse the discrete, noisy, and heterogeneous data obtained in step S1 into a continuous, accurate, and unified description of the UAV's own state and local environment. In a specific embodiment, the Federated Kalman Filter (FKF) algorithm is used to achieve this purpose.

[0040] Step S201: Construct and initialize the federated Kalman filter. This filter system consists of a main filter and multiple parallel sub-filters.

[0041] Specifically, independent sub-filters are established for the inertial measurement unit, the acoustic Doppler velocity profiler, and the pressure sensor.

[0042] Sub-filter 1 (IMU filter): Its state vector x_imu contains the UAV's position, velocity, and attitude (represented as quaternions), totaling 10 state variables. Its process model is based on the strapdown inertial navigation equations derived from the integrals of angular velocity and linear acceleration in the body coordinate system. Due to the high frequency of IMU data (200 Hz), this sub-filter is primarily responsible for high-frequency state prediction, providing continuous trajectory estimation. Its process noise covariance matrix Q_imu is set according to the angle random walk (ARW) and velocity random walk (VRW) parameters calibrated in the IMU device datasheet. For example, if the gyroscope noise density is 0.005° / s / √Hz and the accelerometer noise density is 50µg / √Hz, the diagonal elements of Q_imu can be calculated accordingly.

[0043] Sub-filter two (ADCP filter): Its state vector x_adcp contains only the three-dimensional velocity components of the UAV in the geographic coordinate system. Its measurement model is z_adcp = H_adcp * x_adcp + v_adcp, where the measured value z_adcp is the vector sum of the UAV's velocity over water and the water flow velocity measured by the ADCP. This sub-filter operates at a low frequency (1 Hz), and its main function is to use the ground / water velocity information provided by the ADCP to correct the velocity accumulation error caused by the IMU trajectory extrapolation. Its measurement noise covariance matrix R_adcp is set according to the accuracy specifications provided by the ADCP manufacturer (such as a velocity measurement uncertainty of ±1%).

[0044] Sub-filter three (pressure sensor filter): Its state vector x_press contains only the UAV's vertical depth z. Its measurement model directly converts the pressure sensor reading (in Pa) into depth using the hydrostatic formula z=P / (ρ*g), where ρ is the density of water and g is the acceleration due to gravity. This filter also operates at a low frequency (10 Hz) specifically to correct for vertical position drift. Its measurement noise covariance R_press is determined based on the accuracy and resolution of the pressure sensor.

[0045] The main filter has the same state vector structure as the IMU sub-filter's state vector x_imu, representing the global optimal estimate. The main filter does not directly process any raw sensor data.

[0046] Step S202: Perform periodic data fusion. The fusion process is divided into two stages.

[0047] Phase 1, Local Filtering. Each sub-filter operates independently. At each time step, the IMU sub-filter performs state prediction based on the latest IMU data. When new measurement data from the ADCP or pressure sensor arrives, the corresponding sub-filter performs a measurement update, generating its local state estimate x_i and the corresponding covariance matrix P_i.

[0048] Phase Two, Information Fusion. The main filter operates at a preset fusion period (e.g., 1 Hz). At this period, the main filter collects the latest local state estimates x_i and covariance P_i from all sub-filters. Subsequently, the main filter calculates the global state estimate x_g and global covariance P_g according to the optimal criterion for information fusion. The core calculation formula is: P_g^-1=P_master_pred^-1+Σ(P_i^-1-P_common_i^-1) x_g=P_g*(P_master_pred^-1*x_master_pred+Σ(P_i^-1*x_i-P_common_i^-1*x_common_i)) In a simplified example of two-dimensional position estimation, assume the position predicted by the IMU sub-filter is x_imu=[10.2,20.5]^T, with a covariance P_imu=diag([0.2,0.2]). Meanwhile, an auxiliary positioning system (equivalent to the combined effect of ADCP and pressure sensors) provides a position estimate of x_aux=[10.0,20.9]^T, with a covariance P_aux=diag([0.1,0.1]). The inverse covariance matrix after fusion by the main filters is... P_g^-1=P_imu^-1+P_aux^-1=diag([1 / 0.2,1 / 0.2])+diag([1 / 0.1,1 / 0.1])=diag([15,15]), therefore P_g=diag([1 / 15,1 / 15]). The estimated position after fusion is... x_g = P_g * (P_imu^-1 * x_imu + P_aux^-1 * x_aux), which calculates to x_g ≈ [10.067, 20.767]^T. It is evident that the fused estimation result is closer to the assisted positioning system with a smaller covariance (i.e., more accurate), and its covariance P_g is smaller than the covariance of any single sub-filter, indicating that fusion improves the estimation accuracy.

[0049] Step S203: Perform information feedback and reset. After calculating the global optimal estimates x_g and P_g, the main filter feeds this information back to all sub-filters. Each sub-filter uses the global estimate to reset or correct its local state based on the allocated information. This feedback mechanism ensures the stability of the entire federated filtering system and avoids system divergence caused by the continuous drift of a certain sub-filter.

[0050] As an alternative, state fusion can also employ a single, centralized Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF).

[0051] In an EKF-based alternative, the system maintains a single high-dimensional state vector. Measurement models for all sensors are linearized (by calculating the Jacobian matrix) and used to update the state within a unified framework. This approach is structurally simple, but its performance is highly dependent on the system's nonlinearity; under strong nonlinearity, linearization errors can lead to degraded or even divergent filter performance.

[0052] In another UKF-based alternative, the system approximates the probability distribution of the state using a carefully chosen set of "sigma points," thus avoiding explicit linearization of the nonlinear model. UKF typically offers higher accuracy than EKF in strongly nonlinear systems. However, its computational complexity is higher than EKF, and it requires tuning additional hyperparameters.

[0053] This invention preferably uses a federated Kalman filter, whose main technical advantage lies in its modular structure. This structure enhances the robustness of the system; the failure of a single sub-filter or sensor will not immediately cause the entire fusion system to collapse. Simultaneously, its distributed computing characteristics reduce the requirements for the instantaneous computing power of the main processor, resulting in better scalability.

[0054] Step S204: Construct a local static obstacle map. This step is processed in parallel with state fusion, utilizing forward-looking sonar data and the fused UAV pose information.

[0055] Sub-step S2041: Acoustic image to 3D point cloud conversion. For each frame of the 2D fan-shaped acoustic grayscale image generated by the forward-looking sonar, the system first applies an intensity threshold to filter out low-intensity background noise pixels. For pixels (u,v) with intensity higher than the threshold, their polar coordinates (r,θ) in the sonar coordinate system can be calculated from the pixel index and sonar parameters: r=v*range_resolution, θ=(u-u_center)*angle_resolution. Subsequently, these are converted to Cartesian coordinates (x_s,y_s,z_s) in the sonar coordinate system. Finally, using the real-time global position and attitude (quaternions) of the UAV obtained in step S202, the points (x_s,y_s,z_s) in the sonar coordinate system are transformed to 3D points (x_g,y_g,z_g) in the global geographic coordinate system through a rigid body transformation matrix.

[0056] Sub-step S2042: Point cloud registration and accumulation. As the UAV moves, continuously generated point cloud frames are registered to a unified local map. The Iterative Closest Point (ICP) algorithm or its variants can be used to finely adjust the UAV's relative pose by minimizing the distance between overlapping points in consecutive frames, further improving the consistency of map construction. To control the amount of data, the newly generated point cloud can be processed by a voxel downsampling filter (VoxelGridFilter) to sparsify the point cloud at a preset resolution (e.g., 0.2 meters).

[0057] Sub-step S2043: Map data structuring. The final generated local static obstacle map is stored in the form of a 3D point cloud and serves as a data layer for the environment state vector.

[0058] Step S205: Output a unified environmental state vector. This step integrates the UAV state output from the federated Kalman filter and the local obstacle map into a structured data packet. This environmental state vector serves as the data foundation for all subsequent planning and control modules. Its specific composition can be defined by the following table:

[0059] Step S3: Generate a predictive four-dimensional hydrodynamic field model. The core of this step is to use the known current environmental conditions to perform physical simulation and prediction of the water flow field over a future period.

[0060] Step S301: Initialize the computational domain grid. A computational grid in a Cartesian coordinate system is defined in three-dimensional space, centered on the real-time position of the UAV in the environmental state vector. In a specific embodiment, the grid size is set to 200 meters long, 200 meters wide, and 100 meters high. The grid resolution, i.e., the distance between adjacent grid nodes, is set to one meter. This setting means that the computational domain is discretized into 200*200*100=4,000,000 computational units (or voxels). This grid moves with the UAV as it moves, always keeping the UAV in the central region.

[0061] Step S302: Set initial conditions and boundary conditions.

[0062] For the initial conditions, the discretized water flow vector field data (local_flow_field) in the environmental state vector is used as the initial flow field distribution for the simulation calculation. Since the water flow data obtained by ADCP measurement is a sparse profile along the UAV trajectory, it needs to be extended to the entire three-dimensional computational grid using spatial interpolation methods (such as Kriging interpolation or inverse distance weighting), and each grid node is assigned an initial velocity vector u(x,y,z,t=0).

[0063] For boundary conditions, the static obstacle point cloud data (local_obstacle_map) in the environment state vector is mapped onto the computational grid. All grid cells containing obstacle points are marked as solid boundaries. On these solid boundaries, no-slip boundary conditions are applied, meaning the fluid velocity on the solid surface is zero. For the outer boundary of the computational domain, open boundary conditions (such as the Zou-He pressure boundary condition) can be set according to the task scenario, allowing fluid to flow freely in and out to simulate an open aquatic environment.

[0064] Step S303 involves performing an iterative solution for the fluid dynamics. This step is executed on a dedicated embedded graphics processing unit (GPU) to leverage its massively parallel computing capabilities. In a specific embodiment, the Lattice Boltzmann Method (LBM) is used for the calculations. Specifically, the D3Q19 model is selected, which defines nineteen discrete particle velocity directions at each node of a three-dimensional cubic lattice, enabling accurate simulation of the macroscopic behavior of a three-dimensional incompressible fluid. The calculation process primarily consists of two alternating microscopic steps: collisions and migrations.

[0065] In the collision step, each node in the computational grid is treated as an independent computational unit and assigned to a processing core of the GPU. At that node, the particle distribution function representing different velocity directions is redistributed according to a collision operator to simulate the interaction between particles. This collision operator is preferably a Bhatnagar-Gross-Krook (BGK) model with a single-relaxation-time (SRT), which relaxes the distribution function to a local equilibrium distribution function. This equilibrium distribution function is a function of macroscopic fluid velocity and density, thus linking microscopic particle dynamics with macroscopic fluid properties.

[0066] In the migration step, the particle distribution function at each node moves to the adjacent grid node along its corresponding discrete velocity direction. This process is implemented on the GPU through efficient memory read / write operations, that is, reading the distribution function value from one memory address and writing it to another memory address representing the adjacent node.

[0067] These two steps (collision and migration) constitute a time step. The system repeats these two steps in simulation time steps of 0.1 seconds. After each time step calculation is completed, the macroscopic velocity field of the entire three-dimensional computational grid at the next moment is obtained. By iterating continuously for 600 times, the system can predict and generate the flow field evolution process for the next 60 seconds.

[0068] Step S304: Generate and store four-dimensional model data. After each simulation time step, the calculated macroscopic velocity vector field data covering the entire grid is stored. This data structure is a four-dimensional array, with its dimensions being the X, Y, and Z coordinate indices of the grid and the time step index, respectively. For example, Model[i][j][k][t] stores the predicted water flow velocity vector {vx,vy,vz} at grid coordinates (i,j,k) and time step t. This collection of velocity field data, which includes spatial and temporal evolution, constitutes the aforementioned predictive hydrodynamic field four-dimensional model. This model is stored in a cache for quick retrieval by the subsequent route optimization module.

[0069] Step S4 involves performing multi-objective path optimization based on energy and risk costs. The goal of this step is to plan an optimal path from the UAV's current location to the target point, based on the predictive hydrodynamic field four-dimensional model generated in step S3. (Refer to...) Figure 5 The diagram illustrates a planned course that cleverly avoids areas of strong current and utilizes downstream flow, while navigating through turbid waters when concealment is required. In one specific embodiment, the Anytime Repairing A* (ARA*) search algorithm is employed to achieve this multi-objective optimization.

[0070] Step S401: Define a four-dimensional spatiotemporal search graph. The search algorithm is performed in a four-dimensional discrete space. A node (x, y, z, t) in this space represents the event that the UAV reaches a spatial grid point (x, y, z) at discrete time step t. The edges of the graph connect spatiotemporally adjacent and reachable nodes. For example, a UAV can move from a node (x_i, y_i, z_i, t_i) to a neighboring spatial node (x_j, y_j, z_j, t_j), where t_j = t_i + Δt, and the spatial distance ||(x_j, y_j, z_j) - (x_i, y_i, z_i)|| must be less than or equal to the maximum distance the UAV can fly within that time step Δt.

[0071] Step S402: Implement the ARA* search with ongoing repair. The ARA* algorithm is a heuristic search algorithm characterized by its ability to quickly provide an initial feasible solution, then continuously optimize the solution using the remaining computation time until the optimal solution is reached or computation time is exhausted. The algorithm maintains an open list to store nodes to be expanded, and sorts them according to the total cost of the nodes f(n) = g(n) + h(n), where g(n) is the cost from the starting point to the node. The actual cost, h(n), is the cost from node The heuristic cost estimation to the target point is used. The heuristic function h(n) can be calculated by multiplying the Euclidean distance by an average unit cost estimated in terms of energy and risk. The algorithm balances solution quality and search speed through an adjustable expansion factor ε. Initially, ε is large, and the algorithm behaves close to a greedy best-first search, quickly finding a suboptimal solution; subsequently, ε is gradually decreased, and the algorithm re-expands the nodes in the previously open list, repairing and improving the quality of the solution.

[0072] Step S403: Construct the multi-target node transfer cost function. This function represents the transfer cost of moving from one node to its neighboring nodes. The increment of g(n) in the A* algorithm is defined as a weighted combination function. Its specific mathematical expression is: .

[0073] in, , , These are preset weighting coefficients, the sum of which is one, and their values ​​are configured according to the priority of the current task. For example, for long-endurance reconnaissance missions, {...} can be set. 0.6, 0.2, :0.2}, prioritizing energy consumption. For rapid assault missions, { can be set 0.1, 0.8, :0.1}, with time as the priority.

[0074] To move from the current node to the next node The predicted energy consumption value is calculated based on the UAV's own hydrodynamic model. Specifically, the predicted water velocity vector v_water at the corresponding spatiotemporal location of the path segment is first retrieved from the four-dimensional predictive hydrodynamic field model. To maintain a preset ground velocity v_ground, the UAV's velocity v_relative relative to the water body is the vector difference between v_ground and v_water. The thrust required by the UAV's propulsion system is... It is proportional to the square of the relative velocity, that is =0.5*ρ*A_eff*C_d*|v_relative|^2, where ρ is the water density, A_eff is the effective cross-sectional area, and C_d is the drag coefficient. Power consumption of the propulsion system. It is again a function of thrust and relative velocity, and can be approximated as... , where k is the propulsion system efficiency coefficient. Ultimately, energy consumption... The power is obtained by integrating over time along the path segment: .

[0075] To predict time cost, i.e. the time required to complete this path segment. In a discrete search graph, this value is typically a fixed time step.

[0076] To predict exposure risk values, this value is related to the turbidity of the water body and is used to implement strategies for covert flight using turbid water. Specifically, the system retrieves the average turbidity value τ (in NTU) of the area traversed by the UAV's path segment from a real-time turbidity map generated by interpolation of historical turbidity sensor data. Exposure Risk Value It is defined as a decreasing function of turbidity. In one embodiment, =R_max*exp(-k_τ*τ), where R_max is the maximum risk value (corresponding to pure water), and k_τ is a positive attenuation coefficient. This definition ensures that when the drone's planned path enters a high-turbidity area, the corresponding risk cost is minimized. This will significantly reduce, thus lowering the total cost. Gain an advantage through optimization.

[0077] Step S404: Output a four-dimensional spatiotemporal waypoint sequence. After the ARA* algorithm completes its operation within a given computation time (e.g., 500 milliseconds), it outputs a waypoint sequence from the UAV's current spatiotemporal position to the target point, with the lowest overall cost. The path is a series of time-ordered four-dimensional waypoints, formatted as {(x_1,y_1,z_1,t_1),(x_2,y_2,z_2,t_2),...,(x_goal,y_goal,z_goal,t_goal)}. This sequence not only defines the spatial trajectory the UAV should follow but also specifies the time to reach each spatial point, constituting the precise instructions for subsequent flight control modules.

[0078] Step S5: Implement adaptive flight control based on feedforward compensation. The purpose of this step is to accurately execute the four-dimensional spatiotemporal trajectory planned in step S4, while actively suppressing disturbances caused by water flow. (Refer to...) Figure 4 This step is achieved by using a cascaded proportional-integral-derivative (PID) controller combined with a feedforward compensation circuit.

[0079] Step S501: Construct a cascaded PID controller. This controller consists of an outer loop controller and an inner loop controller.

[0080] The outer loop controller, also known as the position-velocity controller, is primarily responsible for tracking the trajectory. In one specific embodiment, this controller operates at a frequency of 10 Hz. Its inputs are: 1) the current target waypoint from the four-dimensional spatiotemporal trajectory point sequence obtained in step S404 ( , , ); 2) The real-time location of the UAV from the environmental state vector in step S205 ( , , ) and real-time speed ( , , The controller first calculates the position error. Then, a PID controller is applied to this position error to calculate a desired UAV ground velocity vector. Its control law can be expressed as: ,in , , These are the outer loop PID parameters.

[0081] The inner-loop controller, also known as the attitude-angular velocity controller, is responsible for calculating the specific commands needed to drive the actuators based on the desired velocity given by the outer loop controller. This controller operates at a higher frequency, such as 100 Hz. Its inputs are: 1) the desired velocity vector output by the outer loop controller. 2) The UAV's real-time velocity and attitude (represented by Euler angles or quaternions) in the environmental state vector. The controller calculates the velocity error e_vel = -( , , Another PID controller (or a set of independent PID controllers controlling different degrees of freedom) is applied to the velocity error, and in conjunction with the attitude control requirements, the desired force F_d and desired torque M_d that need to be applied in the UAV body coordinate system are calculated.

[0082] For example, to achieve the desired forward speed, the thrust of the main propulsion unit needs to be calculated; to achieve the desired yaw or pitch, the rudder angle of the vertical tail rudder or horizontal rudder needs to be calculated; and to achieve roll control, differential operation of the horizontal rudder or aileron may be required.

[0083] Step S502: Implement the feedforward compensation mechanism. This mechanism is the core of the adaptive control of this invention, aiming to preemptively offset known environmental disturbances, rather than waiting for them to generate errors before making corrections. The implementation of this mechanism includes the following sub-steps.

[0084] Sub-step S5021: Extract disturbance information for future path segments. The control system looks ahead at path points within a short time window (e.g., the next five seconds) from the planned four-dimensional spatiotemporal trajectory point sequence.

[0085] Sub-step S5022: Query the predicted hydrodynamic field model. For each future path point (x_f, y_f, z_f, t_f), the system queries the four-dimensional predicted hydrodynamic field model generated in step S3 to obtain the predicted water flow velocity vector v_water_predicted(x_f, y_f, z_f, t_f) at that spatiotemporal point.

[0086] Sub-step S5023: Calculate the feedforward compensation control variable. Based on the UAV's hydrodynamic model, the system calculates the additional compensation force required to counteract the hydrodynamic forces and torques generated by v_water_predicted. and compensation torque For example, if a lateral current from the right is predicted, the system will calculate a compensating thrust to the right to maintain the drone's straight-line flight, while possibly requiring a yaw torque in the opposite direction to counteract the deflection effect of the current on the nose. This calculation process is the inverse operation of the hydrodynamic model, that is, based on the external force / torque to be counteracted, the required control inputs (thrust, rudder angle) are solved.

[0087] Sub-step S5024: Superimpose the control signal. Calculate the feedforward compensation force. and torque The feedback force calculated by the inner-loop PID controller based on error feedback and torque Linear superposition is performed to form the final resultant force and resultant moment commands: .

[0088] Step S503: Generate and output the actuator control signal. Final resultant force. With resultant torque Commands are converted into specific physical signals that drive the various actuators of the UAV (such as the main thruster, multiple tunnel thrusters, and control surface servo motors) through a control distribution module. For example, thrust commands are converted into the duty cycle of the corresponding motor's pulse width modulation (PWM) signal, and control angle commands are converted into the target angle value of the corresponding servo motor. These signals are ultimately sent to the UAV's hardware driver layer to achieve precise control of the UAV's flight maneuvers.

[0089] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments that can be applied to other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A method for autonomous underwater flight control of a submersible unmanned aerial vehicle (UAV), characterized in that, Includes the following steps: S1, perform multimodal underwater environment perception: synchronously collect raw hydrodynamic data of the current position of the UAV, acoustic image data of obstacles in the three-dimensional space in front, water turbidity value and absolute depth value, and obtain the UAV's own motion state data to generate a real-time environmental dataset; S2, Construct an environmental state fusion model under a unified spatiotemporal reference: Based on the real-time environmental dataset, the federated Kalman filter algorithm is used to fuse the real-time environmental dataset, correct the cumulative error of trajectory estimation, and parse the acoustic image data into a local static obstacle map to output the environmental state vector. S3, Generate a predictive four-dimensional hydrodynamic field model: Delineate a computational grid centered on the real-time position of the UAV in the environmental state vector, use the local water flow velocity vector and the local static obstacle map as initial and boundary conditions, and use the lattice Boltzmann method to iteratively solve the problem to generate a predictive four-dimensional hydrodynamic field model containing a future preset time window. S4, Perform multi-objective route optimization based on energy and risk costs: On the basis of the four-dimensional predictive hydrodynamic field model, the A-satellite search algorithm with real-time repair is adopted to perform trajectory planning based on the node transfer cost function containing the predicted energy consumption value, time cost and exposure risk value, and solve and output the four-dimensional spatiotemporal trajectory point sequence with the lowest comprehensive cost. S5, Implement adaptive flight control based on feedforward compensation: Construct a cascaded proportional-integral-derivative controller. Its outer loop controller calculates the desired flight velocity vector based on the four-dimensional spatiotemporal trajectory point sequence and the real-time position of the UAV in the environmental state vector. Its inner loop controller calculates the control commands required to drive the UAV's thrusters and control surfaces based on the deviation between the desired velocity vector and the real-time velocity vector. At the same time, the cascaded proportional-integral-derivative controller introduces a feedforward compensation loop. The feedforward compensation loop directly extracts the predicted hydrodynamic disturbance data of the UAV's upcoming position from the four-dimensional predictive hydrodynamic field model and converts the predicted hydrodynamic disturbance data into a compensation control quantity to offset the predicted water flow impact in advance. Finally, it generates and outputs a control signal to the actuator.

2. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 1, characterized in that, The real-time environment dataset includes water velocity components generated by an acoustic Doppler current profiler, acoustic grayscale images generated by forward-looking sonar, water turbidity values ​​generated by a turbidity sensor, hydrostatic pressure values ​​generated by a pressure sensor, and angular velocity and linear acceleration values ​​generated by an inertial measurement unit. The environmental state vector includes the UAV's global three-dimensional position coordinates, three-dimensional velocity vector, three-dimensional attitude angle, and a local environment map containing a static obstacle point cloud layer and a water flow vector field layer. The predictive hydrodynamic field four-dimensional model is a four-dimensional array representing the velocity vectors of each node in the computational grid at multiple future time steps. The four-dimensional spatiotemporal track point sequence is a set of track points containing three-dimensional spatial coordinates and timestamps.

3. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 1, characterized in that, The specific steps for executing the multimodal underwater environment perception are as follows: The real-time environment dataset with precise timestamps is verified and encapsulated, and a structured real-time environment dataset is generated.

4. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 3, characterized in that, The specific steps for obtaining the environment state vector are as follows: S211: Establish independent sub-filters for the inertial measurement unit, acoustic Doppler current profiler, and pressure sensor, and set up a main filter to jointly form a federated Kalman filter system; S212: The main filter periodically collects the local state estimates and their covariance matrices from the outputs of each sub-filter, and performs weighted fusion according to the information allocation principle to generate the global optimal state estimate. S213: Feed back the global optimal state to each sub-filter for correction, and convert the acoustic image data of the forward-looking sonar into a three-dimensional point cloud in the global coordinate system in parallel, and integrate them into an environmental state vector containing the UAV pose and local map.

5. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 4, characterized in that, The specific steps for generating the predictive four-dimensional hydrodynamic field model are as follows: S311: Initialize a Cartesian coordinate system computation grid with a preset size and resolution centered on the real-time position of the UAV, and map the static obstacle point cloud data in the environmental state vector to the grid and define it as a fixed non-slip boundary; S312: Extend the discretized water flow vector field data in the environmental state vector to the entire computational grid using a spatial interpolation method, as the initial flow field distribution; S313: Parallel computation is performed on the D3Q19 model using the lattice Boltzmann method on an embedded graphics processor to simulate the collision and migration process of fluid particles between grids, predict the flow field evolution in a future preset time window, and generate the four-dimensional predictive hydrodynamic field model.

6. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 5, characterized in that, The specific steps for multi-objective route optimization are as follows: S411: Define a weighted combination form of the node transfer cost function. The weights of the node transfer cost function include the predicted energy consumption value calculated based on the UAV hydrodynamic model and the predicted four-dimensional hydrodynamic field model, and the exposure risk value determined based on the relationship between water turbidity value and flight path. S412: On the spatiotemporal environment defined by the four-dimensional predictive hydrodynamic field model, the A-satellite search algorithm is used to minimize the node transfer cost function. The algorithm seeks the optimal value among multiple sets of predicted energy consumption values ​​and exposure risk values ​​determined based on the relationship between water turbidity and flight path, and solves and outputs the four-dimensional spatiotemporal track point sequence.

7. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 1, characterized in that, The method also includes step S5: S5: Implement adaptive flight control based on feedforward compensation, call the four-dimensional spatiotemporal track point sequence and the environmental state vector, construct a cascaded proportional-integral-derivative controller, and introduce a feedforward compensation link that directly extracts the predicted hydrodynamic disturbance data from the four-dimensional model of the predictive hydrodynamic field to generate the final flight control command set. The final flight control command set includes the desired flight speed vector calculated by the outer loop controller, the feedback control quantity calculated by the inner loop controller, and the compensation control quantity calculated by the feedforward compensation loop.

8. The underwater autonomous flight control method for a submersible unmanned aerial vehicle according to claim 7, characterized in that, The specific steps for generating the final flight control command set are as follows: S511: Construct a cascaded proportional-integral-derivative controller. Its outer loop controller calculates the desired flight speed vector based on the four-dimensional spatiotemporal track point sequence and the real-time position of the UAV. Its inner loop controller calculates the feedback control signal based on the deviation between the desired speed and the real-time speed. S512: Extract future path points from the four-dimensional spatiotemporal track point sequence, query the four-dimensional predictive hydrodynamic field model to obtain the corresponding predicted water flow vector, and calculate the additional thrust and rudder angle required to counteract the predicted water flow based on the UAV hydrodynamic model to form a feedforward compensation signal. S513: The feedforward compensation signal and the feedback control signal are linearly superimposed to form a composite control command, which is then integrated into the final flight control command set.

9. A submersible unmanned aerial vehicle (UAV) underwater autonomous flight control system, characterized in that, The system is used to implement the underwater autonomous flight control method for a submersible unmanned aerial vehicle as described in any one of claims 1-8, and the system includes: The environmental perception module is used to synchronously collect raw data from the multimodal sensor suite carried by the UAV, including hydrodynamic data, acoustic image data and its own motion state data, to generate a real-time environmental dataset. The state fusion module is used to fuse multi-source data based on the real-time environment dataset using the federated Kalman filter algorithm, and to parse the acoustic image to construct a local static obstacle map, outputting a unified high-precision environmental state vector. The hydrodynamic field prediction module is used to construct a computational domain centered on the real-time position of the UAV based on the environmental state vector, and to iteratively solve the fluid dynamics equations using the lattice Boltzmann method to generate a predictive four-dimensional hydrodynamic field model. The route optimization module is used to plan and output the four-dimensional spatiotemporal track point sequence with the lowest overall cost based on the four-dimensional predictive hydrodynamic field model, using the real-time repair A-satellite search algorithm based on the weighted cost function. The flight control module is used to generate a final flight control command set based on the four-dimensional spatiotemporal track point sequence and the environmental state vector, through a cascaded controller that incorporates a feedforward compensation loop, for accurately tracking the track and offsetting water flow disturbances in advance.

Images