Bionic fish multi-source sensing fusion and multi-mode swimming method for three-dimensional motion control

Abstract

The invention discloses a bionic fish multi-source sensing fusion and multi-mode swimming method for three-dimensional motion control, and the method comprises the steps: carrying out the adaptive robust Kalman filtering of a multi-source depth fusion result based on inertial integration, a pressure type depth gauge and underwater distance measurement, and obtaining an optimal estimation value, and taking the optimal estimation value as the input of an SAC algorithm; an observation quality index mechanism based on short-time fluctuation evaluation is introduced, adaptive measurement noise is determined according to the real-time quality of each path of depth observation, dynamic weighted fusion of multi-source depth data is realized through robust Kalman filtering, residual monitoring between a fusion result and each path of observation is performed, and the real-time quality of each path of depth observation is determined. Online identification and suppression of abnormal observation and systematic deviation are realized; a target distance error, a course angle error and a fused submerging depth error are uniformly constructed into a reinforcement learning control state, so that horizontal motion control and vertical heaving control of the bionic fish are jointly optimized under the same decision framework, and task completion efficiency and motion coordination in a complex three-dimensional water area environment are improved.

Description

Technical Field

[0001] This invention relates to underwater robot control technology, and more particularly to a method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control. Background Technology

[0002] Motion control of biomimetic fish often focuses on path tracking or heading control in a two-dimensional plane, primarily considering horizontal position and attitude adjustment, while paying less attention to vertical heave control. However, in practical underwater applications, biomimetic fish often need to switch navigation states between different depths to complete three-dimensional motion tasks such as depth-holding cruise, surfacing, or diving. Therefore, achieving stable and precise three-dimensional motion control has become an urgent problem to be solved.

[0003] Patent CN121084579A discloses a fin-propeller composite multi-mode biomimetic robotic fish. By integrating a tail-swinging propulsion system and a multi-mode pectoral fin assembly onto a streamlined body, it achieves switching between multiple motion modes such as cruising, gliding, and maneuvering. Its tail-swinging propulsion system uses a motor-cam-transmission mechanism to drive the tail's swinging motion, providing the main propulsion force. The multi-mode pectoral fin assembly integrates a propeller into an adjustable pitch-angle pectoral fin structure. An electronic control system coordinates the combination and switching of different motion modes, thereby improving the biomimetic robotic fish's maneuverability and environmental adaptability. This solution primarily increases the number of motion modes from the perspective of propulsion mechanism and structural design, focusing on solving the problem of limited motion modes. However, its control method still relies mainly on preset logic and actuator switching, lacking fine perception of three-dimensional motion states and multi-source information fusion modeling. It does not consider the impact of depth estimation errors on control stability, making it difficult to achieve stable and accurate three-dimensional autonomous motion control in complex underwater environments.

[0004] Patent CN120573245A discloses a biomimetic fish fin structure and propulsion method with wave-beat multimodal hybrid propulsion. It achieves multimodal composite propulsion by combining flexible wave propulsion with overall flapping propulsion. The flexible fin propulsion device consists of multiple unit components arranged in a wave-like pattern. Local wave motion is achieved through a flexible wave drive device, and overall up-and-down flapping motion is achieved under the action of the flapping drive device. This integrates wave and flapping propulsion on a single flexible fin structure, improving the maneuverability and propulsion continuity of the biomimetic robotic fish. The innovation of this scheme lies in the design of the flexible fin structure and propulsion mechanism, mainly addressing the issue of diversified propulsion methods. However, its research focuses on the propulsion structure itself, failing to construct a complete perception-decision-execution closed-loop control framework, and lacking multi-source fusion modeling for key states such as depth and attitude. Furthermore, the multimodal propulsion is not combined with a learning-based control strategy, resulting in limited autonomous decision-making and three-dimensional motion control capabilities under complex task conditions.

[0005] Existing bionic fish control technology faces the following problems in three-dimensional motion scenarios:

[0006] 1. Existing biomimetic fish control methods are mostly focused on position or heading control in a two-dimensional plane, with limited control capabilities for vertical heave motion. This makes it difficult to meet the requirements of three-dimensional motion such as constant depth cruise, surfacing, and diving, and its applicability in complex underwater missions is insufficient. 2. Existing biomimetic fish depth control typically relies on a single depth sensor or a single estimation method. Depth information sensing methods struggle to obtain long-term, stable, and accurate depth feedback in dynamic underwater environments. 3. Depth estimation errors are easily introduced directly into the reinforcement learning state space. The algorithm is highly sensitive to the accuracy and stability of the input state, and errors are directly transmitted to the reinforcement learning decision-making process, leading to unstable policy learning, decreased control accuracy, and affecting the learning process and control performance. 4. The integration of reinforcement learning control with the multimodal swimming mechanism of biomimetic fish is insufficient, easily resulting in discontinuous or unsmooth motion or control actions that are difficult to implement in practical engineering, thus its practical applicability needs improvement. Summary of the Invention

[0007] Purpose of the invention: The present invention aims to provide a method for achieving precise positioning and autonomous navigation of biomimetic fish in underwater environments through multi-source perception fusion and multimodal swimming.

[0008] Technical solution: The method for multi-source perception fusion and multimodal swimming of biomimetic fish for three-dimensional motion control described in this invention includes the following steps:

[0009] (1) Using the actual depth of the bionic fish as the state variable, and the inertial navigation depth, pressure depth and laser depth of the bionic fish as the three depth observations. Within the sliding time window, the short-term fluctuation intensity of each depth observation is calculated to obtain the quality index corresponding to each depth observation. Determine adaptive measurement noise ;

[0010] (2) The three depth observations are fused using robust Kalman filtering. An innovative consistency gating mechanism is used to automatically reduce the weight of abnormal observations or remove them. After obtaining the fused depth, a statistical monitoring mechanism based on the residuals of each path is introduced to perform self-checking and feedback adjustment on the fusion results to obtain the optimal fused depth estimate. Depth error determined by target depth ;

[0011] (3) Based on the planar position coordinates of the bionic fish output by the Beidou system and the IMU inertial navigation system and the optimal fusion depth estimate obtained in step (2), construct the three-dimensional position coordinates of the bionic fish. The speed of the bionic fish is calculated based on its position changes at adjacent moments. ; Calculate the distance error between the bionic fish and the target point based on the current position of the bionic fish and the coordinates of the preset target point. Using the heading angle information of the bionic fish obtained through the inertial navigation system, the heading angle error between the current heading angle and the target heading angle of the bionic fish is calculated. and obtain angular velocity ;

[0012] (4) The soft actor-critic SAC algorithm is adopted to reduce the depth error of the bionic fish. Distance error Heading angle error sailing speed and angular velocity As a state input, the output CPG control parameters are used. ;

[0013] (5) Based on the control parameter u output by SAC, select or adjust the corresponding swimming mode from the preset multi-modal swimming library, and generate servo control signal to realize the three-dimensional autonomous navigation and stable control of the bionic fish.

[0014] Furthermore, the method for calculating the inertial navigation depth of the bionic fish is as follows:

[0015] Based on the original Z-axis acceleration, the component of gravitational acceleration in the Z-axis direction, the accelerometer bias, and random noise output by the biomimetic fish IMU inertial navigation system, the true Z-axis acceleration after gravity compensation is determined. The Z-axis heave velocity is obtained by time integration of the gravity-compensated Z-axis acceleration. The Z-axis heave displacement, i.e., the inertial navigation depth of the biomimetic fish, is obtained by integrating the Z-axis velocity.

[0016] Furthermore, the inertial navigation depth of the bionic fish for

[0017] =

[0018]

[0019]

[0020] in, The Z-axis acceleration is the original output of the IMU inertial navigation system. Let Z be the component of gravitational acceleration along the Z-axis. To achieve zero bias in the accelerometer, This is a random noise term.

[0021] Furthermore, quality indicators for

[0022]

[0023]

[0024]

[0025]

[0026] in, It is a monotonically decreasing mapping function. For noise intensity estimation, For variance operators, Indicates from the first From the sampling time to the... A sequence of consecutive sampling points at each sampling time point, with the sliding time window length determined by the sampling frequency and system response requirements. For the i-th depth observation, , The depth is obtained by the second integral of the Z-axis acceleration fed back by the IMU inertial navigation system. To output depth for pressure-type depth gauges, The depth is calculated from the output depth of the underwater laser ranging module. To control the maximum depth allowed for the task.

[0027] Furthermore, adaptive measurement noise for

[0028]

[0029]

[0030] in, For diagonal matrix functions, Let be the variance of the noise in the i-th path. , Let be the reference noise variance of the i-th channel under normal and stable operating conditions. and Let represent the minimum and maximum allowable values ​​of the noise variance of the i-th path, respectively. This is the clipping function.

[0031] Furthermore, the fused depth estimate is obtained by fusing the three depth observations using robust Kalman filtering. for

[0032]

[0033]

[0034]

[0035]

[0036]

[0037] in, for The depth estimate after time-mapping For Kalman gain, To estimate the error covariance, For residuals, To innovate covariance, For the observation matrix, To achieve the actual diving depth, This is the observed noise vector.

[0038] Furthermore, the innovation consistency index of the innovation consistency gating mechanism. for

[0039] .

[0040] Furthermore, in the statistical monitoring mechanism based on the residuals of each path, the fusion depth and the residuals between each observation are considered. for

[0041]

[0042]

[0043] Calculate its mean within the sliding window. for

[0044]

[0045] Calculate its variance within the sliding window. for

[0046] .

[0047] Furthermore, the pre-defined multimodal motion library is as follows:

[0048]

[0049] .

[0050] Furthermore, the CPG model of the CPG controller is as follows:

[0051]

[0052] in, For the first The frequency of each joint; For the first The joint and the first Phase difference between joints; For the first The corresponding angles of each joint; k ranges from 0 to 7, with each value representing a corresponding motion mode. =1,2,3,4 represents the joint number. , and They represent the first The amplitude, offset, and phase of each joint; and The first The expected amplitude and expected offset of each joint; It is a state variable for amplitude; This is the offset state variable; This refers to the phase state variable.

[0053] Beneficial Effects: Compared with existing technologies, the significant advantages of this invention are: 1. This invention introduces multi-source depth fusion results based on inertial integration, pressure depth measurement, and underwater ranging. The optimal estimate is obtained through adaptive robust Kalman filtering and used as one of the state inputs of the SAC algorithm. This effectively avoids the problem of single-source depth observation noise or drift error being directly introduced into the decision-making process, thus ensuring the stability and long-term accuracy of depth control when the bionic fish performs three-dimensional motion tasks such as surfacing, diving, and constant-depth navigation. 2. This invention introduces an observation quality index mechanism based on short-term fluctuation assessment during multi-source depth fusion and determines adaptive measurement noise based on the real-time quality status of each depth observation. Dynamic weighted fusion of multi-source depth data is achieved through robust Kalman filtering. Simultaneously, through statistical monitoring of the residuals between the fusion results and each observation, online identification and suppression of abnormal observations and systematic deviations are achieved, thus avoiding the problems of traditional fixed-parameter Kalman filtering in complex underwater environments. 1. The problem of gradual distortion of fusion results due to environmental disturbances and sensor drift; 2. This invention does not directly use reinforcement learning algorithms to output the low-level control commands of the actuator. Instead, it maps the reinforcement learning decision results to the adjustment of the control parameters of the central pattern generator, so that the control output is always limited to the pre-built multimodal swimming pattern library. This ensures the adaptive capability of the strategy while avoiding the discontinuous or unrealizable control actions that may be generated by reinforcement learning directly driving the actuator, which significantly improves the feasibility and safety of the bionic fish control method; 3. This invention constructs the target distance error, heading angle error and fused diving depth error into a unified reinforcement learning control state, so that the horizontal motion control and vertical heave control of the bionic fish are jointly optimized under the same decision framework. This avoids the problem of independent and mutual interference between planar control and depth control in the prior art, thereby improving the task completion efficiency and motion coordination of the bionic fish in complex three-dimensional water environment. Attached Figure Description

[0054] Figure 1 This is a top view of a biomimetic fish.

[0055] Figure 2 A bottom view of a biomimetic fish;

[0056] Figure 3 This is a diagram showing the distribution of components inside the core control cabin.

[0057] Figure 4 This is a schematic diagram of a biomimetic fish control system.

[0058] Figure 5 This is a hardware connection framework diagram for a biomimetic fish control system.

[0059] Figure 6 This is a schematic diagram of data conversion in the laser ranging module;

[0060] Figure 7 Flowchart for sensor data preprocessing and fusion;

[0061] Figure 8 A schematic diagram illustrating the task of position control for a biomimetic fish.

[0062] Figure 9 Here is the SAC algorithm decision control flowchart;

[0063] Figure 10 This is a schematic diagram of the CPG controller model.

[0064] Figure 11 Diagram of CPG multimodal mobile library and output signal;

[0065] Figure 12 This is a flowchart of the control strategy of the present invention. Detailed Implementation

[0066] The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control described in this invention includes the following steps:

[0067] S1: Configure the hardware of the bionic fish. In the experimental pool environment, the bionic fish integrates a Beidou positioning system, an IMU inertial navigation system, a pressure depth gauge and an underwater laser ranging module, and completes data acquisition and control calculation through the main control unit.

[0068] S1.1: As Figure 1As shown, the biomimetic fish structure consists of a core control cabin 1, a biomimetic outer shell 2, flexible drive pectoral fins 3 and 4, a servo motor mounting bracket 5, a power battery compartment 6, an actuator servo motor 7, and a rudder arm 8. The core control cabin contains various sensors and components, serving as an integrated module for data acquisition and processing. The battery compartment provides functions to each part, and the servo motor executes the final motion, enabling the flexible pectoral fins to combine flexible wave propulsion with overall flapping motion for buoyancy and descent, achieving multimodal composite motion.

[0069] S1.2: As Figure 2 As shown, the bionic fish's abdomen carries an underwater laser ranging module, consisting of a mounting bracket 21 and a laser sensor 22. The mounting bracket 21 is connected to the bionic shell 2 via bolts and threaded sleeves. The mounting bracket 21 and the laser sensor 22 are bonded together with waterproof adhesive. The wiring extends to the sealing flange end cap at the rear of the core control cabin 1. The bionic fish communicates wirelessly with the host computer via an antenna receiver 23 to transmit data. The flexible actuation pectoral fin consists of a pectoral fin membrane 32 and clamping fin rays 31.

[0070] S1.3: As Figure 3 The diagram shows the internal component distribution of the core control cabin. The front end is an acrylic hemispherical cover 11 and an aluminum alloy sealing flange 12. The overall structure is stabilized by hexagonal copper pillars 13 and support rings 14.

[0071] S1.4: The bionic fish's perception of its environment is primarily achieved through depth information fed back by a depth sensor 111 and its processing board 116, pose information fed back by an IMU inertial navigation system 115, and position feedback by a BeiDou system 113. The sensor data is processed via a Raspberry Pi 112 and its expansion board 114. The component circuitry is externally connected to a waterproof aviation connector 16 via a watertight connector 110. The aluminum alloy sealing flange end cap is equipped with an exhaust port 15 and a waterproof switch 17 to meet overall operational requirements. The overall design and hardware connection framework of the bionic fish control system are as follows: Figure 4 and Figure 5 As shown.

[0072] S2: After configuring the bionic fish hardware, the sensor data feedback needs to be collected and preprocessed. In this invention, after the IMU inertial navigation system acquires the Z-axis acceleration information of the bionic fish, it first compensates for the gravity component based on the attitude calculation results to obtain the pure Z-axis motion acceleration of the bionic fish; then, it performs a first integration on this acceleration to obtain the Z-axis velocity, and a second integration to obtain the Z-axis heave displacement of the bionic fish. Since the inertial integration process is easily affected by zero bias and noise, resulting in accumulated errors, the obtained heave displacement is used as one input for depth estimation. It is fused with the depth information from the pressure depth gauge and the underwater laser ranging module through Kalman filtering, thereby improving the accuracy and stability of depth estimation.

[0073] S2.1: The IMU inertial navigation system includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, forming a ten-axis inertial navigation system. In the bionic fish coordinate system, the Z-axis direction is defined with vertically upward as positive. The displacement change of the bionic fish along the Z-axis direction fed back by the IMU inertial navigation system is denoted as... .

[0074] S2.2: The Z-axis acceleration measured by the accelerometer in the IMU inertial navigation system is denoted as...

[0075]

[0076] in, The Z-axis acceleration is the original output of the IMU inertial navigation system. To simulate the realistic Z-axis acceleration of a biomimetic fish, This represents the acceleration due to gravity in the bionic fish coordinate system, specifically the component along the Z-axis. To achieve zero bias in the accelerometer, This is a random noise term.

[0077] S2.3: Obtain the current attitude angle, i.e., the roll angle, through attitude calculation of the IMU inertial navigation system (fusion of gyroscope, magnetometer, and accelerometer). Pitch angle Heading angle The projection of gravity along the Z-axis is:

[0078]

[0079] Where g is the gravitational acceleration constant, approximately

[0080] S2.4: The pure acceleration after removing the gravitational acceleration component and other variables is:

[0081]

[0082] If the gravity component is not removed, the final result will contain a constant value of g for a long time. Performing first and second integrations on the acceleration with constant bias will cause the velocity and displacement to diverge twice, resulting in a rapid drift of the depth result obtained by the IMU inertial navigation system integration.

[0083] S2.5: Integrate the gravity-compensated Z-axis acceleration over time to obtain the Z-axis heave velocity:

[0084]

[0085] S2.6: Integrating the Z-axis velocity yields the Z-axis heave displacement, i.e., the inertial navigation depth:

[0086] =

[0087] If there is a small constant deviation in acceleration, the displacement error will increase quadratically with time. Therefore, a pressure depth gauge and a laser depth gauge are introduced and fused by Kalman filtering.

[0088] S3: Submersible depth is fed back via a pressure depth gauge. .

[0089] S4: Data feedback and preprocessing of underwater laser ranging module, as follows Figure 6 As shown, the laser from the ranging module shines directly onto the bottom of the water, and the angle between the laser and the Z-axis is the pitch angle fed back by the IMU inertial navigation system. .

[0090] S4.1: The ranging module reports the laser distance L from the bionic fish to the bottom of the water, and the IMU inertial navigation system reports the pitch angle of the bionic fish. Therefore, the distance from the bionic fish to the bottom of the water is in the Z direction. .

[0091] S4.2: Let the water depth in the experimental environment be H, and the diving depth of the biomimetic fish fed back by the underwater laser ranging module be:

[0092]

[0093] S5: This invention improves upon conventional fusion methods by proposing a multi-source depth fusion method based on online quality assessment, adaptive robust Kalman filtering, and statistical self-checking of residuals from each source. It uses three depth observation data streams as input: inertial navigation depth... Pressure depth Laser depth By unifying input quality assurance, adaptive weight adjustment during the fusion process, and a closed-loop self-checking mechanism for the fusion results into a single framework, stable and reliable fusion of multi-source depth information is achieved, outputting a fused depth estimate. The flowchart for sensor data preprocessing and fusion is as follows: Figure 7 As shown;

[0094] Furthermore, at the beginning of the fusion process, physical boundary constraints are first applied to each depth data stream, and short-term fluctuations are assessed to construct a quality score reflecting the reliability of the observations, thereby ensuring the quality of the input data. In the fusion processing stage, adaptive measurement noise is determined based on the quality score, and robust Kalman filtering is used to fuse the multi-source depth data at each time step. An innovative consistency gating mechanism is used to automatically reduce the weight of abnormal observations or remove them, achieving a balance between short-term time-by-time fusion and long-term continuous monitoring. At the end of the fusion process, the residuals between the fused depth output and each observation stream are statistically monitored, and the observation quality is adjusted based on the changes in the mean and variance of the residuals, thus forming a self-checking and self-correcting mechanism for the fusion results.

[0095] After outputting the optimal depth estimate of the bionic fish, a depth error is generated between it and the target depth. During implementation, the Kalman filter adopts a single-state model, using the actual depth of the biomimetic fish as the state variable, and the inertial navigation depth, pressure depth, and laser depth as the observations.

[0096] S5.1: Define the state variable, denoted by the time step of the bionic fish at discrete time points. The actual diving depth is:

[0097]

[0098] in, This refers to the filter state, i.e., the true depth that needs to be estimated. For bionic fish at all times The true depth of the submarine.

[0099] S5.2: Define the observation vector as a column vector composed of three depth observations:

[0100]

[0101] in, The depth is obtained by the second integral of the Z-axis acceleration fed back by the IMU inertial navigation system. To output depth for pressure-type depth gauges, The depth is calculated from the output depth of the underwater laser ranging module.

[0102] S5.3: Apply real-time constraints to each observation path; directly identify anomaly candidates if the observation exceeds the physical boundary.

[0103] ,in, To control the maximum allowed depth of the task, if a certain path If the constraints are not met, the path is marked as abnormal. This constraint is used to eliminate abnormal observation data that clearly does not conform to physical meaning.

[0104] S5.4: In a length of Within the sliding time window, the short-term fluctuation intensity of each data point, i.e., the median absolute deviation (MAD), is calculated and used as a noise intensity estimate. , It is a variance operator used to characterize short-term stability. Indicates from the first From the sampling time to the... A sequence of continuous sampling points at each sampling time point, with the sliding time window length determined by the sampling frequency and system response requirements. Furthermore, based on the fluctuation intensity and physical constraints, the quality index is obtained:

[0105]

[0106] For a monotonically decreasing mapping function, when the jitter of a certain path increases within the short window, If the value increases, the quality index decreases; if it exceeds the limit, it is directly set to zero.

[0107] S5.5: Based on the quality indicators of each observation path Adaptive construction of observation noise First, set upper and lower bound constraints on the noise to avoid extreme values ​​causing filtering instability:

[0108]

[0109] in, =1,2,3 This represents the baseline noise variance of the road under normal and stable operating conditions. These are the minimum and maximum allowable values ​​for the noise variance of the i-th path, to prevent filter divergence or over-reliance on any particular path. This is the clipping function;

[0110] Furthermore, the observation noise covariance matrix Weighting coefficient Equivalent to observation noise The inverse relationship between quality indicators The smaller the value, the lower the corresponding noise variance. The larger the value, the smaller the contribution of that observation to the state update during the Kalman gain calculation; the higher the weight. The smaller it is.

[0111] S5.6: Establish a system prediction model: ,in, To address process noise, the prediction model is used to describe the continuous evolution of the biomimetic fish's diving depth between adjacent sampling times. By using the fused depth of the previous time as the prior prediction value of the current time, it provides a smooth constraint for subsequent observation updates, thus providing a reasonable prior estimate even when the observation information is temporarily unstable or contains noise.

[0112] S5.7: Establish a system observation model: The three-way depth observation is associated with the state variables;

[0113] in, Let be the observation matrix, indicating that the sensitivity of the three observations to the state is 1. The three observations are consistent in terms of dimensions and physical meaning, and can be fused in the same state space. The noise vector is used to establish the mapping relationship between each depth observation and the actual underwater depth state.

[0114] Furthermore, based on the aforementioned state variables and observation model, at each discrete time step, the system first obtains a priori depth estimate according to the prediction model, and then, combining the three depth observations and their corresponding adaptive noise weights, calculates the fused depth estimate for the current time step using the Kalman filter update formula.

[0115] S5.8: Provide prior estimates through the above prediction model, assuming... For a moment The prior estimate, i.e. the predicted value, This is a posterior estimate, i.e., the fused result. To estimate the error covariance, the prediction update formula is:

[0116]

[0117] in, This represents the process noise variance; a larger value indicates a faster change in depth. The state transition matrix is ​​F=1. This is the depth estimate after fusion at the previous time step.

[0118] S5.9: The state update is completed using the above observation model and adaptive noise weights, and the residuals are updated accordingly. Update the innovation covariance: Furthermore, the Kalman gain is updated: Update the measurement status: ;

[0119] Furthermore, by performing statistical analysis on each group of observations using innovation consistency gating, the normalized squared innovation is calculated as follows: ,like If an anomaly is found in the observation set at this moment, the observation path that contributes the most (i.e., the one with the largest absolute residual value) is identified, and its variance is amplified before updating. This update will be recalculated later. To innovate consistency indicators, This is the gating threshold.

[0120] S5.10: After obtaining the fusion depth output, to further ensure the reliability of the fusion results, a statistical monitoring mechanism based on the residuals of each path is introduced to perform self-checks on the fusion results. The residuals between the fusion depth and each path observation are calculated:

[0121] Calculate its mean and variance within the sliding window:

[0122] ,when When the value is consistently too high, it is determined that there is a systematic deviation in the path. When a sudden increase occurs, it is determined that the path is experiencing a short-term anomaly or enhanced environmental disturbance. In this case, the variance of that path is automatically increased, and the weighting coefficient is decreased. This results in fusion. optimal depth estimation after The depth error is obtained by comparing it with the target depth. .

[0123] S6: The bionic fish position control task is described as follows: Input data for other algorithms is jointly acquired through the BeiDou system and the IMU inertial navigation system. Figure 8 As shown. The bionic fish position control problem studied in this invention aims to design a control strategy to enable the bionic fish to reach a specified target position from its current position without a predefined path. The control task can be described as: adjusting the swimming mode input signal... The swimming mode of the bionic fish is switched to make the relative distance between its current position and the target position approach 0. When the bionic fish reaches the red circular area around the target position and the angle between its current position and the target heading is less than 30°, it is considered to have reached the target position and the control task is completed.

[0124] S6.1: Navigation is achieved through a combination of the BeiDou system and the IMU inertial navigation system, outputting the position coordinates and pose information of the bionic fish in the navigation coordinate system. The difference between the original observations from the BeiDou system and the predicted values ​​from the IMU inertial navigation system is used as the filtered observation input. Even when the number of visible satellites is low or there are short periods without satellite signals, the IMU inertial navigation system can still complete state propagation and maintain continuous output, thereby reducing the error offset impact of relying solely on BeiDou positioning in underwater environments. Combined with the optimal depth estimate, the three-dimensional position coordinates of the bionic fish are obtained. The sailing speed is then calculated from the position difference between adjacent sampling times. .

[0125] S6.2: At each sampling time k, the IMU inertial navigation system provides angular velocity and acceleration information to complete the continuous propagation of the biomimetic fish's state. Even if the BeiDou signal is unavailable, the IMU inertial navigation system can still output continuous position and velocity prediction results through state propagation, ensuring continuous system operation. The state propagation of the IMU inertial navigation system is defined as follows:

[0126]

[0127] in, This includes the integrated navigation state, position, angular velocity, attitude, and errors of the IMU (Inertial Measurement Unit) system. The inputs to the IMU inertial navigation system include angular velocity, acceleration, etc. For process noise, This is an inertial navigation state propagation model.

[0128] S6.3: Based on the difference between the original observations from the BeiDou system and the INS prediction results, observation information is constructed, and the state propagation error is corrected. The observations are then updated as follows:

[0129]

[0130] in, The observational information is formed by combining raw BeiDou observations and INS predictions. For the BeiDou system observation model, To observe noise.

[0131] Based on the above observation information, a Kalman filter is used to update the state, resulting in a corrected state estimate. ;

[0132] When the BeiDou signal quality is poor or temporarily unavailable, the system only performs state propagation in S6.2; when the BeiDou signal is restored, the system corrects the accumulated error through observation updates, thereby reducing the error offset caused by relying solely on BeiDou positioning in the underwater environment.

[0133] S6.4: The position coordinates of the bionic fish are output from the integrated navigation system, and the optimal fusion depth estimate obtained in step S5 is introduced to construct the three-dimensional position coordinates of the bionic fish. The bionic fish's speed is calculated based on its position changes at adjacent moments. .

[0134] S6.5: Calculate the distance error between the bionic fish and the target point based on the current position of the bionic fish and the coordinates of the preset target point. .

[0135] S6.6: Obtain the heading angle information of the bionic fish through the inertial navigation system, and calculate the heading angle error between the current heading angle and the target heading angle of the bionic fish. Simultaneously obtain angular velocity .

[0136] S7: Employs the Soft Actor-Critic (SAC) algorithm to minimize the distance error of the bionic fish. Heading angle error Depth error sailing speed and angular velocity As a state input, the output CPG control parameters are used. The SAC algorithm decision control flowchart is as follows: Figure 9 As shown.

[0137] S7.1: Initialize the total training cycle Maximum number of interactions between the agent and the environment per round Experience pool capacity Batch size Rewards and discounts .

[0138] S7.2: Initialization Strategy In terms of weight Initialize the Q function , Its weight , Initialize target , Its weight , Initialize the return cache Its capacity .

[0139] S7.3: Initialize the simulation environment, reset the initial pose of the bionic fish, and randomly generate the desired position.

[0140] S7.4: The agent determines the current state based on the current state. and strategy Generate Actions The action The parameters are mapped to internal control parameters of the CPG, which are used to adjust the frequency, amplitude, phase, and bias of the oscillation signal, thereby indirectly controlling the movement and swimming modes of the biomimetic fish. Swimming mode control signal .

[0141] S7.5: Will and The input is fed into the intelligent agent in the simulation environment to obtain the state change of the bionic fish at the next time step. The state changes are obtained based on noise interference:

[0142] Furthermore, we obtain and reward function .

[0143] S7.6: Sample Store in temporary experience pool The perturbation observer evaluates samples in the temporary experience pool. When sample perturbation is too large, the temporary buffer is cleared; when the sample learning rate increases or the number of steps reaches a certain threshold, the perturbation is cleared. At that time, the sample information in the temporary cache is stored in the experience pool. middle.

[0144] S7.7: From the experience pool Extracting small batches of data samples .

[0145] S7.8: Perform gradient descent to update the target Gradient ascent is performed to update the control policy. Finally, update the target weight. .

[0146] S7.9: Repeat S7.4-S7.8 until the task is completed or the maximum number of steps is reached. The round ends.

[0147] S7.10: Repeat S7.4-S7.9 until the maximum number of rounds E is reached to obtain the optimal strategy. .

[0148] S8: The strategy outputs the swimming mode control parameters u to the CPG controller. Based on the control parameters output by the reinforcement learning algorithm, the corresponding swimming mode is selected or adjusted from the pre-built multimodal swimming library, and servo control signals are generated to realize the three-dimensional autonomous navigation and stable control of the biomimetic fish.

[0149] S8.1: After observing the current state of the bionic fish, the position control algorithm adjusts the input parameters of the CPG controller based on the phase oscillator to achieve modal control of the bionic fish and guide it to swim towards the target position. The modal selection is as follows:

[0150]

[0151] S8.2: The dynamic characteristics of the CPG model based on the phase oscillator are expressed by mathematical equations, which mainly include the amplitude, bias, phase, and output equations:

[0152]

[0153] Where k ranges from 0 to 7, with each value representing a corresponding floating mode. =1,2,3,4 represents the joint number. , and For the first The amplitude, offset, and phase of each joint; and The first The expected amplitude and expected offset of each joint; For the first The frequency of each joint; For the first The joint and the first Phase difference between joints; For the first The corresponding angles of each joint. A schematic diagram of the CPG controller model is shown below. Figure 10 As shown.

[0154] S8.3: CPG multimodal mobile library and output signal diagram as shown below Figure 11 As shown, the pectoral fin rays on each side of the biomimetic fish exhibit a phase difference, thus the flexible pectoral fins generate forward or backward traveling waves and corresponding thrust. The CPG outputs the servo angle and executes it via PWM. The overall control strategy principle flowchart is shown below. Figure 12 As shown.

[0155] To address the problems of insufficient depth perception accuracy, unstable state information, and insufficient coupling between reinforcement learning control and actual swimming mechanisms in existing biomimetic underwater robotic fish during 3D motion control, this invention aims to provide a biomimetic fish depth reinforcement learning control method and system for 3D motion control. The main objective of this invention is to construct a unified state perception framework suitable for 3D motion control of biomimetic fish, thereby improving the accuracy and stability of depth perception during underwater navigation, enhancing the stability and robustness of reinforcement learning control strategies in actual underwater environments, and improving the engineering feasibility and safety of the biomimetic fish reinforcement learning control method.

[0156] This invention acquires the position coordinates of a biomimetic fish in a pool or confined aquatic environment through a combined positioning method using the BeiDou Navigation Satellite System (BDS) and an Inertial Measurement Unit (IMU). The BDS is used for position correction when the fish is at the surface, near the surface, or periodically surfacing, forming a combined navigation system with the IMU. During underwater missions without satellite signals, the IMU performs state propagation and acquires motion parameters such as attitude, angular velocity, and acceleration of the fish. Based on this, multiple depth data sources are acquired using inertial measurement, pressure-based depth sensing, and underwater ranging information. These multi-source depth data are then fused to obtain the optimal estimate of the fish's current depth. Subsequently, the distance error between the fish's current position and the target point, the heading angle error, the fused depth error, and key state parameters such as the fish's speed and angular velocity are constructed into a unified state vector and input into a deep reinforcement learning control algorithm. The reinforcement learning algorithm outputs control parameters to adjust the fish's swimming mode based on the current state, rather than directly outputting low-level control commands for the actuators. The control parameters are further input into a multimodal swimming controller, which generates continuous, smooth, and implementable servo control signals based on a pre-built swimming modality library, thereby driving the bionic fish to complete three-dimensional movements such as forward movement, turning, surfacing, or diving. Through this approach, the present invention, while ensuring the control strategy learning capability, introduces multi-source perception fusion and multimodal constraint mechanisms, effectively improving the stability, accuracy, and engineering feasibility of the bionic fish's three-dimensional motion control.

[0157] In the multi-source deep information fusion and reinforcement learning control method described in this invention, the setting of noise parameters of each sensor, process model parameters and control parameters are related to the structure, propulsion method and sensor characteristics of the bionic fish. They need to be calibrated and configured according to the actual situation of the specific bionic fish. The dynamic disturbance characteristics and swimming mode characteristics of the bionic fish are implicitly encoded into the state estimation and decision-making process, so that the control method and the bionic fish body form a close coupling relationship.

Claims

1. A method for bionic fish multi-source perception fusion and multi-modal swimming oriented to three-dimensional motion control, characterized in that, Includes the following steps: (1) Take the real depth of the bionic fish as the state variable, and take the inertial navigation depth, pressure depth and laser depth of the bionic fish as three depth observations In the sliding time window, the short-term fluctuation intensity of each depth observation is calculated to obtain the quality index corresponding to each depth observation , determine the adaptive measurement noise ; (2) The three depth observations are fused using robust Kalman filtering. An innovative consistency gating mechanism is used to automatically reduce the weight of abnormal observations or remove them. After obtaining the fused depth, a statistical monitoring mechanism based on the residuals of each path is introduced to perform self-checking and feedback adjustment on the fusion results to obtain the optimal fused depth estimate. Depth error determined by target depth ; (3) Based on the planar position coordinates of the bionic fish output by the Beidou system and the IMU inertial navigation system and the optimal fusion depth estimate obtained in step (2), construct the three-dimensional position coordinates of the bionic fish. The speed of the bionic fish is calculated based on its position changes at adjacent moments. ; Calculate the distance error between the bionic fish and the target point based on the current position of the bionic fish and the coordinates of the preset target point. Using the heading angle information of the bionic fish obtained through the inertial navigation system, the heading angle error between the current heading angle and the target heading angle of the bionic fish is calculated. and obtain angular velocity ; (4) The soft actor-critic SAC algorithm is adopted to reduce the depth error of the bionic fish. Distance error Heading angle error sailing speed and angular velocity As a state input, the output CPG control parameters are used. ; (5) Based on the control parameter u output by SAC, select or adjust the corresponding motion mode from the preset multi-mode motion library, and generate the angle value of the servo motor motion. This enables three-dimensional autonomous navigation and stable control of biomimetic fish.

2. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 1, characterized in that, The method for calculating the inertial navigation depth of the bionic fish is as follows: Based on the original Z-axis acceleration, the component of gravitational acceleration in the Z-axis direction, the accelerometer bias, and random noise output by the biomimetic fish IMU inertial navigation system, the true Z-axis acceleration after gravity compensation is determined. The Z-axis heave velocity is obtained by time integration of the gravity-compensated Z-axis acceleration. The Z-axis heave displacement, i.e., the inertial navigation depth of the biomimetic fish, is obtained by integrating the Z-axis velocity.

3. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 2, characterized in that, Inertial navigation depth of bionic fish for = in, The Z-axis acceleration is the original output of the IMU inertial navigation system. Let Z be the component of gravitational acceleration along the Z-axis. To achieve zero bias in the accelerometer, This is a random noise term.

4. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 1, characterized in that, Quality Indicators for in, It is a monotonically decreasing mapping function. For noise intensity estimation, For variance operators, Indicates from the first From the sampling time to the... A sequence of consecutive sampling points at each sampling time point, with the sliding time window length determined by the sampling frequency and system response requirements. For the i-th depth observation, , The depth is obtained by the second integral of the Z-axis acceleration fed back by the IMU inertial navigation system. To output depth for pressure-type depth gauges, The depth is calculated from the output depth of the underwater laser ranging module. To control the maximum depth allowed for the task.

5. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 4, characterized in that, Adaptive measurement noise for in, For diagonal matrix functions, Let be the variance of the i-th path noise. , Let be the reference noise variance of the i-th channel under normal and stable operating conditions. and Let represent the minimum and maximum allowable values ​​of the noise variance of the i-th path, respectively. This is the clipping function.

6. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 5, characterized in that, The fused depth estimate is obtained by fusing the three depth observations using robust Kalman filtering. for in, for The depth estimate after time-mapping For Kalman gain, To estimate the error covariance, For residuals, To innovate covariance, For the observation matrix, To achieve the actual diving depth, This is the observed noise vector.

7. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 6, characterized in that, Innovation Consistency Indicators of Innovation Consistency Gating Mechanism for 。 8. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 7, characterized in that, In a statistical monitoring mechanism based on residuals from each path, the fusion depth and the residuals between each path's observations are considered. for Calculate its mean within the sliding window. for Calculate its variance within the sliding window. for 。 9. The method for biomimetic fish multi-source perception fusion and multimodal swimming for three-dimensional motion control according to claim 8, characterized in that, The pre-defined multimodal motion library is as follows: 。 10. The method for multi-source sensor fusion and multimodal swimming of biomimetic fish for three-dimensional motion control according to claim 9, characterized in that, The CPG model of the CPG controller is in, For the first The frequency of each joint; For the first The joint and the first Phase difference between joints; For the first The corresponding angles of each joint; k ranges from 0 to 7, with each value representing a corresponding motion mode. =1,2,3,4 represents the joint number. , and They represent the first The amplitude, offset, and phase of each joint; and The first The expected amplitude and expected offset of each joint; It is a state variable for amplitude; This is the offset state variable; This refers to the phase state variable.

Images