Multi-modal sensor fused masked palm robot navigation path control method and system

Abstract

The invention relates to the technical field of robot intelligent control, in particular to a multi-modal sensor fused masked palm robot navigation path control method, which comprises the following steps: acquiring underwater environment data and masked palm robot motion posture data in real time through a multi-modal sensor group; preprocessing the underwater environment data and the motion posture data of the masked palm robot; on the basis of the underwater environment dynamic model, a global navigation path is generated by adopting an improved path planning algorithm, a plurality of sensors such as sonar, a binocular vision camera, a depth sensor and an inertial measurement unit are integrated through a multi-mode sensor fusion technology, comprehensive collection of underwater environment and masked palm robot motion attitude data is achieved, and the underwater environment and masked palm robot motion attitude data can be obtained. The problem that a traditional single sensor is not comprehensive in sensing is effectively solved, the obstacle detection rate is greatly improved, the environment sensing precision is remarkably improved, and the navigation efficiency and safety of the masked palm robot in the complex underwater environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent control of beaver-like robots, and in particular to a navigation path control method and system for a beaver-like robot based on multi-modal sensor fusion. Background Art

[0002] Underwater beaver-like robots are widely used in ocean exploration, ecological monitoring, and underwater operations. Composed primarily of a control system, a displacement buoyancy system, a servo drive system, sonar, and sensors, they are primarily used to study the phenomena, processes, and patterns of life in the ocean, as well as for rescue, salvage, and military applications.

[0003] However, existing underwater beaver-like robots face numerous challenges in complex underwater environments. In terms of environmental perception, traditional underwater beaver-like robots often rely solely on single sensors, such as sonar or visual sensors. These sensors are affected by factors such as underwater turbidity, lighting variations, and water disturbances. This makes it impossible to fully and accurately acquire environmental information such as underwater obstacles and terrain, as well as the robot's own posture information. This leads to blind spots and errors in environmental perception. In path planning, traditional algorithms fail to fully account for dynamic underwater factors. In motion control, traditional controllers struggle to adapt to the complex and changing hydrodynamic environment. When the beaver-like robot is impacted by water currents or experiences sudden changes in its posture, it cannot quickly and accurately adjust, resulting in overshoot, oscillation, or prolonged deviations from the intended path, affecting the robot's navigation accuracy and stability. Summary of the Invention

[0004] The main purpose of the present invention is to provide a navigation path control method and system for a beaver-like robot based on multimodal sensor fusion, aiming to solve the technical problems raised in the above-mentioned background technology.

[0005] The present invention proposes a navigation path control method for a beaver-like robot based on multimodal sensor fusion, comprising: The underwater environment data and the motion posture data of the beaver-like robot are collected in real time by a multimodal sensor group. The underwater environment data includes the distance and contour information of underwater obstacles, underwater three-dimensional point cloud data and water pressure information. The motion posture information of the beaver-like robot includes acceleration information, angular velocity information and attitude angle information. Preprocessing the underwater environment data and the beaver-like robot motion posture data, including denoising, correction and zero bias compensation; fusing the underwater environment data and the motion posture data of the beaver-like robot to construct an underwater environment dynamic model; Based on the underwater environment dynamic model, an improved path planning algorithm is used to generate a global navigation path; Real-time adjustment of beaver robot thrust distribution via fuzzy proportional-integral-derivative controller.

[0006] Preferably, the step of collecting underwater environment data and beaver-like robot motion posture data in real time through a multimodal sensor group includes: Obtain the distance and outline information of underwater obstacles through the sonar module; Acquire underwater image information through binocular vision cameras and generate underwater 3D point cloud data; The water pressure information of the water depth where the beaver-like robot is located is obtained through a depth sensor; The acceleration information, angular velocity information and attitude angle information of the beaver-like robot are obtained through the inertial measurement unit; The distance and contour information of the underwater obstacle, the underwater three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information, and attitude angle information are time stamp-aligned.

[0007] Preferably, the step of preprocessing the underwater environment data and the beaver-like robot motion posture data comprises: Eliminating noise from the distance and contour information of the underwater obstacle by using an adaptive filtering algorithm; Performing distortion correction and feature point matching on the underwater image information, and generating underwater dense three-dimensional point cloud data; Performing mean filtering on the water pressure information to eliminate instantaneous fluctuations; Performing zero bias compensation on the acceleration information, angular velocity information, and attitude angle information; A data set is established based on the pre-processed distance and contour information of the underwater obstacle, the underwater image information and the underwater dense three-dimensional point cloud data, the water pressure information and the acceleration information, the angular velocity information and the attitude angle information.

[0008] Preferably, the step of fusing the underwater environment data and the beaver-like robot motion posture data to construct an underwater environment dynamic model comprises: Performing spatial registration on the distance and contour information of the underwater obstacle and the underwater dense three-dimensional point cloud data to obtain a three-dimensional model of the obstacle in a unified coordinate system; Based on the 3D obstacle model, the underwater environment space is divided into 3D grids. Each grid is marked to store the probability value of the obstacle. The probability value is dynamically adjusted as the sensor data is updated to obtain a 3D grid map with probability values. Combining the acceleration information, angular velocity information, and attitude angle information, the three-dimensional grid map is aligned with the coordinate system to eliminate the coordinate offset caused by the motion of the beaver-like robot, thereby obtaining an environment map that is synchronized with the real-time posture of the beaver-like robot; Obtaining a moving obstacle in the three-dimensional grid map, and obtaining moving obstacle trajectory prediction information based on the speed and acceleration of the moving obstacle; An underwater environment dynamic model is established based on the environment map of the real-time position of the synchronized beaver-like robot and the mobile obstacle trajectory prediction information.

[0009] Preferably, the step of generating a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model includes: Obtain the starting position and target position of the beaver-like robot according to the underwater environment dynamic model; A bidirectional search mechanism is used to simultaneously expand path nodes from the starting position and the target position, and the calculation is terminated when the two search domains intersect to generate a preliminary path; skipping intermediate nodes without path value in the preliminary path by a skipping point search algorithm to obtain an optimized path; Design a dynamic heuristic function, and adjust the path cost estimate of the optimization path in combination with the water flow velocity vector to obtain a secondary optimization path; The quadratic optimization path is smoothed by using a curve fitting algorithm to eliminate the jagged trajectory in the quadratic optimization path, thereby obtaining a smooth and non-jaggy global navigation path.

[0010] Preferably, the step of adjusting the thrust distribution of the propeller in real time by using a fuzzy proportional integral differential controller includes: Obtaining a lateral deviation between the beaver-like robot and the global navigation path, and calculating a rate of change of the lateral deviation; defining a fuzzy logic rule, wherein the input variables are the lateral deviation and the rate of change of the lateral deviation, and the output variables are the adjustment amounts of the proportional, integral, and differential gains; Establishing a fuzzy rule base to dynamically adjust control parameters according to the magnitude of the lateral deviation and the changing trend of the lateral deviation change rate; Defuzzification is performed using a centroid method to convert the output of the fuzzy logic rule into an actual gain adjustment value; Based on the actual gain adjustment amount, the propeller rotation speed and the hind leg movement speed of the beaver-like robot are adjusted according to the thrust distribution strategy, and an anti-saturation mechanism is introduced to prevent the propeller and the hind legs from exceeding the limit.

[0011] The present invention also discloses a multi-modal sensor fusion beaver-like robot navigation path control system, comprising: A data acquisition module is used to collect underwater environment data and beaver-like robot motion posture data in real time through a multimodal sensor group; A preprocessing module, used for preprocessing the underwater environment data and the beaver-like robot motion posture data; A fusion calculation module is used to fuse the underwater environment data and the beaver-like robot motion posture data and construct an underwater environment dynamic model; A path planning module, configured to generate a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model; The motion control module adjusts the thrust distribution of the beaver-like robot in real time through a fuzzy proportional-integral-derivative controller.

[0012] Preferably, the path planning module includes: The environmental perception and positioning unit obtains the current position and target position of the beaver-like robot based on the underwater environment dynamic model; a bidirectional path extension unit, which adopts a bidirectional search mechanism to synchronously extend path nodes from the current position and the target position, terminates the calculation when the two search domains intersect, and generates a preliminary path; a jump point path optimization unit, which skips intermediate nodes without path value in the preliminary path through a jump point search algorithm to obtain an optimized path; A secondary optimization unit designs a dynamic heuristic function and adjusts the path cost estimation value of the optimization path in combination with the water flow velocity vector to obtain a secondary optimization path; The path trajectory smoothing unit uses a curve fitting algorithm to smooth the secondary optimization path, eliminate the jagged trajectory in the secondary optimization path, and obtain a smooth and non-jaggy global navigation path.

[0013] The present invention also discloses a computer device, including a memory and a processor, wherein the memory stores a computer program, and is characterized in that when the processor executes the computer program, it implements the steps of a multi-modal sensor fusion beaver robot navigation path control method.

[0014] The present invention also discloses a computer-readable storage medium having a computer program stored thereon, characterized in that when the computer program is executed by a processor, the computer program implements the steps of a navigation path control method of a beaver-like robot with multimodal sensor fusion.

[0015] The beneficial effects of the present invention are as follows: the present invention integrates multiple sensors such as sonar, binocular vision camera, depth sensor, inertial measurement unit, etc. through multimodal sensor fusion technology to realize comprehensive collection of underwater environment and beaver-like robot motion posture data, and improves data accuracy and synchronization through data preprocessing, timestamp alignment and other means, effectively solving the problem of incomplete perception of traditional single sensor, greatly improving the obstacle detection rate, and significantly improving the accuracy of environmental perception.

[0016] In terms of path planning, algorithms such as bidirectional search, jump point search, dynamic heuristic function and curve fitting are adopted, which fully consider dynamic factors such as water flow and can quickly generate a globally optimal and smooth navigation path. Compared with traditional algorithms, the path planning time is greatly shortened and the path length is significantly reduced. At the same time, energy consumption is effectively reduced, and the navigation efficiency and safety of the beaver-like robot in complex underwater environments are improved.

[0017] In terms of motion control, fuzzy PID control technology is used. Based on the lateral deviation and rate of change of the beaver robot's path, PID control parameters are dynamically adjusted in real time to precisely regulate thrust distribution and the beaver robot's motion speed. An anti-saturation mechanism is also introduced to ensure equipment safety. This enables the beaver robot to quickly and stably track the planned path, significantly reducing path tracking error. This effectively improves the beaver robot's navigation accuracy and stability in complex underwater environments, enhancing system reliability. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a schematic diagram of a method flow chart according to an embodiment of the present application.

[0019] Figure 2 This is a schematic diagram of the system structure of an embodiment of the present application.

[0020] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0021] It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0022] like Figure 1 As shown, the present application provides a navigation path control method for a beaver-like robot using multimodal sensor fusion, comprising: S1, collecting underwater environment data and motion posture data of the beaver-like robot in real time through a multimodal sensor group, wherein the underwater environment data includes distance and contour information of underwater obstacles, underwater three-dimensional point cloud data, and water pressure information, and the motion posture information of the beaver-like robot includes acceleration information, angular velocity information, and attitude angle information; S2, preprocessing the underwater environment data and the beaver-like robot motion posture data, including denoising, correction and zero bias compensation; S3, fusing the underwater environment data and the motion posture data of the beaver-like robot to construct an underwater environment dynamic model; S4, generating a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model; S5, real-time adjustment of the beaver-like robot thrust distribution through the fuzzy proportional-integral-derivative controller.

[0023] As described in the above steps S1-S5, the present invention combines multiple sensors to collect data, process data, build environmental models, plan paths and adjust controls in real time, so that the beaver-like robot can navigate autonomously, safely and efficiently in complex underwater environments, solving the problems of traditional underwater beaver-like robots such as incomplete perception, rigid path planning and inflexible control.

[0024] Due to the complexity of the underwater environment, for example, turbid water can affect camera imaging, and water movement can interfere with sonar signals. A single sensor (such as only sonar or camera) is prone to missing obstacles or misjudging the position. Traditional methods using only sonar may not see transparent or soft obstacles (such as water plants), and using only cameras will be "blind" in muddy waters and unable to fully perceive the environment. In addition, there are ocean currents and moving obstacles (such as fish) underwater. Traditional path planning algorithms pre-set fixed rules and are difficult to adjust according to real-time conditions. They may choose the shortest but high-risk or high-energy consumption path. In addition, sailing along a fixed path in strong water currents may cause the beaver-like robot to "go upstream" and consume too much power, or encounter suddenly moving obstacles without time to avoid them. In addition, when the beaver-like robot moves underwater, the water resistance and thrust are nonlinearly related (for example, water flow in different directions during turning will affect the efficiency of the propeller). The traditional PID controller has fixed parameters and cannot quickly adapt to posture changes.

[0025] The present invention forms a complete "perception-processing-decision-control" process through the following five steps: first, comprehensive data collection using multiple sensors; second, data noise removal and deviation correction; third, data integration to construct a real-time, dynamic underwater environment model; third, flexible path planning based on the environmental model; and finally, real-time adjustment of propeller thrust based on path deviations for precise control. This results in more comprehensive perception, more intelligent routing, and more precise control. Through these steps, the beaver-like robot can autonomously navigate complex underwater environments, ensuring efficient and reliable task execution.

[0026] In one embodiment of the present invention, the step of collecting underwater environment data and beaver-like robot motion posture data in real time through a multimodal sensor group includes: S11, obtain the distance and outline information of underwater obstacles through the sonar module; S12, acquiring underwater image information through a binocular vision camera and generating underwater three-dimensional point cloud data; S13, obtaining water pressure information of the water depth where the beaver-like robot is located through a depth sensor; S14, obtaining acceleration information, angular velocity information, and attitude angle information of the beaver-like robot through an inertial measurement unit; S15, aligning the distance and contour information of the underwater obstacle, the underwater three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information, and attitude angle information timestamps.

[0027] As described in steps S11-S15 above, the present invention uses a multimodal sensor group to respectively obtain underwater environment-related data (underwater obstacle distance and outline, underwater three-dimensional point cloud, water pressure) and beaver-like robot motion posture data (acceleration, angular velocity, attitude angle), and aligns these data by time stamps, thereby achieving the purpose of comprehensive, accurate, and synchronous collection of underwater environment and beaver-like robot motion information, providing a reliable data foundation for subsequent environmental modeling, path planning, and motion control.

[0028] To achieve autonomous, safe, and efficient movement in underwater environments, a beaver-like robot must accurately perceive its surroundings and its own motion state. The underwater environment is complex and ever-changing, with obstacles, varying water pressures, and currents. The beaver-like robot's motion also involves posture changes such as acceleration and steering. Only by fully understanding this information can a beaver-like robot plan its path and precisely control its movements. For example, without knowing the distance and outline of obstacles, the beaver-like robot may collide. Without knowing its own posture angle, it will be difficult to accurately adjust its direction of movement. Therefore, acquiring and synchronizing this data is a key prerequisite for the beaver-like robot to operate reliably in complex underwater environments.

[0029] Traditional underwater data collection often relies on a single sensor or a simple combination, lacking effective data synchronization mechanisms. A single sensor (such as sonar alone) can only capture limited information, such as obstacle distance, and cannot take into account visual features of the environment or the beaver-like robot's posture. Asynchronous data collection from different sensors can lead to information distortion. For example, when determining obstacles based on visual images and sonar distance, unsynchronized data can lead to misjudgment of obstacle locations.

[0030] This solution uses a multimodal sensor group, covering various types of sensors such as sonar, binocular vision cameras, depth sensors, inertial measurement units, etc., each of which performs its function to collect data of different dimensions, and uses timestamp alignment technology to accurately synchronize various types of data to achieve comprehensive and synchronous data collection.

[0031] Specifically, the sonar module acquires information about the distance and outline of underwater obstacles. The sonar module transmits sound waves, which are reflected by underwater obstacles and then received by the sonar. By measuring the time difference between the transmitted and reflected waves and combining this with the speed of sound waves in water, the distance to the obstacle can be calculated. The obstacle's outline is further delineated based on information such as the timing and intensity of the reflected waves from different directions. For example, when encountering a reef, the sonar-emitted sound waves reflect off different parts of the reef. This processing reveals the reef's distance from the beaver robot and its approximate shape, whether it is square, circular, or irregular. This allows the beaver robot to perceive the spatial position and shape of surrounding obstacles, providing crucial data for obstacle avoidance and path planning. With this information, the beaver robot can predict collision risks and choose appropriate detours. The system effectively detects obstacles within a certain range, with distance measurement accuracy sufficient for obstacle avoidance. Contour delineation provides basic shape information to aid in this process.

[0032] A binocular camera captures underwater image information and generates underwater 3D point cloud data. The camera has two lenses, each capturing the underwater scene from different angles, producing two images. Using the parallax between the two lenses, the position of each point on the object in 3D space is calculated based on triangulation principles, generating 3D point cloud data. For example, by comparing the images of an underwater plant, the spatial positions of its parts can be determined and a 3D point cloud model constructed. This provides visual information about the underwater environment and the 3D spatial distribution of objects, which can be used to identify obstacle types (e.g., rocks or corals) and detect underwater targets (e.g., finding specific organisms). This supplements the details and visual features that sonar cannot provide. The generated 3D point cloud can visualize underwater objects in three dimensions, helping the beaver-like robot to more accurately understand its surroundings and enhance the richness and accuracy of its environmental perception.

[0033] The depth sensor obtains water pressure information at the beaver robot's depth. Depth sensors typically operate based on the principle of water pressure. As the beaver robot's underwater depth changes, the water pressure changes. Sensitive components within the depth sensor detect these changes in water pressure and convert them into electrical or other measurable signals. After processing, the water pressure value at the current depth is calculated. Water depth and pressure information are crucial for the beaver robot's buoyancy control and diving depth limit. For example, when the beaver robot needs to maintain stable operation at a certain depth, water pressure information can be used to determine whether the pressure limit of the beaver robot has been exceeded. Accurate water pressure measurement ensures the beaver robot's depth control and safe operation, ensuring that the beaver robot operates within an appropriate water pressure environment.

[0034] The beaver robot's acceleration, angular velocity, and attitude angle information are acquired through an inertial measurement unit (IMU). The IMU consists of components such as accelerometers and gyroscopes. The accelerometer measures the beaver robot's acceleration by detecting the inertial force exerted on the internal mass during acceleration. The gyroscope uses the principle of conservation of angular momentum to measure the beaver robot's angular velocity during rotation. By integrating the acceleration and angular velocity data and combining them with the initial attitude information, the beaver robot's attitude angles (such as pitch, roll, and yaw) can be calculated. For example, when the beaver robot accelerates or turns, the accelerometer and gyroscope detect these changes in real time and output the corresponding data. This information reflects the beaver robot's motion state and attitude changes and is key to its motion control and path tracking. For example, when the beaver robot turns, the angular velocity and attitude angle information can be used to precisely control the steering angle and speed to ensure it follows the planned path. By providing the motion posture parameters of the beaver-like robot in real time and accurately, the beaver-like robot can adjust its own motion in time to ensure the stability and accuracy of the motion.

[0035] The distance and outline information of the underwater obstacle, the underwater 3D point cloud data, the water pressure information, and the acceleration, angular velocity, and attitude angle information are time-stamped. As each sensor collects data, a timestamp is added to each data point to record the precise moment of data acquisition. The data processing system then matches and calibrates the data from different sensors based on the timestamps, ensuring temporal consistency. For example, if the sonar detects an obstacle at a certain moment and the binocular vision camera captures a related image at a similar time, the timestamps are used to precisely match these two sets of data. This ensures that the various data collected by different sensors are synchronized in time, ensuring that subsequent operations such as environmental modeling and path planning based on these data are based on accurate information at the same moment, avoiding information bias and erroneous decisions caused by data asynchrony. This precise synchronization of multi-source data provides a reliable foundation for subsequent data fusion and analysis, improving the accuracy and reliability of the entire system's perception of the underwater environment and the beaver-like robot's state.

[0036] In summary, this provides high-quality data support for subsequent key links of environmental modeling, path planning, and motion control, effectively enhancing the autonomous operation capability of the beaver-like robot in complex underwater environments.

[0037] In one embodiment of the present invention, the step of pre-processing the underwater environment data and the beaver-like robot motion posture data comprises: S21, eliminating noise from the distance and contour information of the underwater obstacle using an adaptive filtering algorithm; S22, performing distortion correction and feature point matching on the underwater image information, and generating underwater dense three-dimensional point cloud data; S23, performing mean filtering on the water pressure information to eliminate instantaneous fluctuations; S24, performing zero bias compensation on the acceleration information, angular velocity information, and attitude angle information; S25, establishing a data set based on the pre-processed distance and contour information of the underwater obstacle, the underwater image information and the underwater dense three-dimensional point cloud data, the water pressure information and the acceleration information, the angular velocity information and the attitude angle information.

[0038] As described in the above steps S21-S25, the present invention uses different preprocessing algorithms to eliminate noise, correct distortion, compensate for deviation, etc. on the underwater environmental data (underwater obstacle distance and contour information, underwater image information, water pressure information) collected by the multimodal sensor and the beaver-like robot motion posture data (acceleration, angular velocity, posture angle information), and ultimately establishes an accurate, reliable, and unified data set, providing a high-quality data foundation for subsequent environmental modeling, path planning, and motion control.

[0039] In underwater environments, sensor data is subject to interference from a variety of factors. For example, sonar signals are affected by water currents and impurities, generating noise that can lead to inaccurate obstacle distance and outline information. Underwater images can be distorted by camera lens characteristics, affecting feature recognition. Water pressure can fluctuate instantaneously due to current fluctuations. And the acceleration, angular velocity, and attitude angle information of the inertial measurement unit can contain zero-bias errors. These interferences can cause data deviations and inaccuracies. If left unaddressed, these can lead to errors in subsequent environmental modeling based on this data, irrational path planning, unstable motion control, and the normal operation of the beaver-like robot underwater. Therefore, data preprocessing is essential to ensure the reliable operation of the beaver-like robot.

[0040] Traditional data preprocessing methods are often relatively simplistic and fixed. For example, sonar data may be filtered using fixed-parameter filters, which cannot dynamically adjust the filtering effect to the complex and changing underwater environment. Image distortion correction may lack specific treatment for the effects of unique underwater lighting and media. Bias compensation for inertial measurement unit data may be inaccurate. These traditional methods are difficult to adapt to the complex and dynamically changing underwater environment, resulting in significant errors after data processing.

[0041] This solution uses a variety of targeted algorithms such as adaptive filtering, distortion correction, feature point matching, mean filtering, and zero bias compensation to process the data in a refined manner according to the characteristics and interference factors of different types of data, and finally integrates and establishes a data set to improve data quality.

[0042] Specifically, an adaptive filtering algorithm is used to eliminate noise from the distance and contour information of underwater obstacles. This adaptive filtering algorithm automatically adjusts filtering parameters based on the statistical characteristics of the input sonar signal (such as signal strength and noise level). For example, when turbulent underwater currents increase sonar noise, the algorithm increases the filtering intensity to further suppress the noise; whereas, in relatively calm underwater environments, the algorithm reduces the filtering intensity to preserve more original signal details. The algorithm dynamically suppresses noise by continuously monitoring the error between the signal and the desired output (e.g., a relatively smooth signal after preliminary processing). Underwater, sonar signals are susceptible to noise generated by interference from currents, bubbles, and suspended particles. This noise can cause deviations in the measured obstacle distance and contour depiction, affecting the beaver robot's ability to determine the obstacle's position and shape. Eliminating noise improves the accuracy of sonar data, providing a reliable basis for the beaver robot's obstacle avoidance and path planning. This effectively reduces noise interference in sonar data, minimizing obstacle distance measurement errors and ensuring that the contour depiction is closer to the true shape.

[0043] The underwater image information is subjected to distortion correction and feature point matching to generate dense underwater 3D point cloud data. The underwater camera is calibrated to obtain its internal parameters (such as focal length, principal point coordinates, etc.) and distortion coefficients. Using these parameters, specific mathematical models (such as radial distortion models and tangential distortion models) are employed to remap image pixels, restoring pixels shifted due to lens distortion to their correct positions, thereby correcting image distortion. Feature points are first extracted from the corrected underwater image (using algorithms such as Scale-Invariant Feature Transform (SIFT) and Speeded Up Robust Features (SURF)). These feature points possess unique local characteristics. Similar feature points are then matched between different images (such as the left and right images of a binocular camera) by calculating the similarity (e.g., Euclidean distance or Hamming distance) between the feature point descriptors (e.g., 128-dimensional SIFT feature point descriptors). The corresponding positions in the different images are determined. Based on the matched feature points, the position of each feature point in 3D space is calculated using triangulation principles, combined with the baseline distance and calibration parameters of the binocular vision camera. This generates a large number of 3D point cloud data, more accurately depicting the 3D shape of underwater objects. Distortion in underwater camera lenses can distort captured images, affecting the interpretation of object shape and position. Feature point matching is a key step in obtaining 3D information about underwater objects. Generating dense 3D point cloud data through distortion correction and feature point matching provides beaver-like robots with more accurate visual information about the underwater environment, enabling them to identify obstacle types and detect target objects. The corrected image better matches the actual scene, improving feature point matching accuracy. The generated 3D point cloud can more accurately depict the 3D structure of underwater objects. For example, when detecting underwater coral reefs, the shape and extent can be more accurately identified, providing more reliable information for the beaver-like robot's obstacle avoidance and exploration.

[0044] The water pressure information is subjected to mean filtering to eliminate instantaneous fluctuations. Mean filtering involves averaging the collected water pressure data within a specific time window. For example, a time window of 100 milliseconds is set. Multiple water pressure values ​​are continuously collected within this time window, and the average of these values ​​is calculated. This average is then used as the output water pressure data for that time window. By continuously sliding the time window, the continuous water pressure data is processed, smoothing out water pressure fluctuations caused by transient changes in water flow. Underwater fluctuations and the beaver robot's own motion can cause transient fluctuations in the water pressure values ​​measured by the water pressure sensor. These fluctuations are not true changes in water depth and can interfere with the beaver robot's ability to determine its depth and buoyancy. Eliminating transient fluctuations makes the water pressure data more stable, reflecting the actual water depth, and facilitating the beaver robot's depth control and buoyancy adjustment. This effectively smooths the water pressure data and reduces misjudgments caused by transient fluctuations.

[0045] Bias compensation is performed on the acceleration, angular velocity, and attitude angle information. First, the inertial measurement unit (IMU) is allowed to collect data for a period of time while stationary. The output deviations (i.e., biases) of the accelerometer and gyroscope within this data are analyzed. While the accelerometer's output should theoretically be the acceleration due to gravity when stationary, deviations may exist in practice. While the gyroscope's output should be zero when stationary, it may also have bias. Based on the bias values ​​obtained from this analysis, compensation calculations are then performed on subsequent real-time data. For example, the corresponding bias values ​​are subtracted from the acceleration data collected by the accelerometer, and from the angular velocity data collected by the gyroscope. This yields more accurate acceleration and angular velocity information, which is then further refined through operations such as integration. Bias errors in the IMU can lead to inaccurate measured acceleration, angular velocity, and attitude angle information, affecting the beaver robot's ability to determine its own motion state. Bias compensation improves the accuracy of this data, enabling the beaver robot to more precisely control its motion and ensure stable operation along the planned path. It can significantly improve the accuracy of inertial measurement unit data and reduce attitude angle errors.

[0046] A dataset is established based on the preprocessed underwater obstacle distance and contour information, underwater image information, underwater dense 3D point cloud data, water pressure information, acceleration information, angular velocity information, and attitude angle information. All of the preprocessed data is integrated and stored according to specific formats and rules. For example, using time series as an index, sonar-processed data, image and 3D point cloud data, water pressure data, and inertial measurement unit-processed data at the same moment are linked to form a complete data record. This process is repeated, sequentially recording data at different moments to construct a dataset encompassing multimodal sensor information. All preprocessed data must be integrated for subsequent use in environmental modeling, path planning, and motion control algorithm training and execution. Establishing a dataset organizes multi-source data in an orderly manner, facilitating system access and analysis, and improving data utilization efficiency and algorithm accuracy. It can form a multimodal dataset with a clear structure and accurate data, providing high-quality data support for the subsequent intelligent decision-making and control of the beaver-like robot. For example, it can more accurately construct underwater scene models when modeling the environment, and select the optimal path based on more reliable data when planning the path.

[0047] The preprocessing steps can significantly improve the data quality, laying a solid data foundation for the reliable operation of the beaver-like robot in underwater environments.

[0048] In one embodiment of the present invention, the step of fusing the underwater environment data and the beaver-like robot motion posture data to construct an underwater environment dynamic model includes: S31, performing spatial registration on the distance and contour information of the underwater obstacle and the underwater dense three-dimensional point cloud data to obtain a three-dimensional model of the obstacle in a unified coordinate system; S32, based on the three-dimensional obstacle model, the underwater environment space is divided using a three-dimensional grid method, each grid is marked to store the probability value of the obstacle, and the probability value is dynamically adjusted as the sensor data is updated to obtain a three-dimensional grid map with probability values; S33, combining the acceleration information, angular velocity information, and attitude angle information, aligning the coordinate system of the three-dimensional grid map to eliminate the coordinate offset caused by the motion of the beaver-like robot, and obtaining an environment map synchronized with the real-time posture of the beaver-like robot; S34, obtaining a moving obstacle in the three-dimensional grid map, and obtaining moving obstacle trajectory prediction information based on the speed and acceleration of the moving obstacle; S35 , establishing an underwater environment dynamic model based on the environment map of the real-time position of the synchronized beaver-like robot and the moving obstacle trajectory prediction information.

[0049] As described in the above steps S31-S35, the present invention fuses the underwater environmental data (underwater obstacle distance and contour information, underwater dense three-dimensional point cloud data, etc.) collected and pre-processed by the multimodal sensor and the beaver-like robot's motion posture data (acceleration, angular velocity, attitude angle information, etc.) to construct an underwater environmental dynamic model that can reflect the real-time status of the underwater environment, including obstacle distribution and moving obstacle trajectory prediction, thereby providing an accurate environmental information basis for subsequent path planning and beaver-like robot motion control.

[0050] To achieve autonomous navigation and safe underwater operations, beaver-like robots require an accurate understanding of their surroundings. The underwater environment is complex and dynamically changing, with obstacles of various shapes and positions, as well as moving objects (such as fish and other underwater equipment). Traditional static environmental models cannot meet these requirements. The changes in the beaver-like robot's position and posture during movement can lead to erroneous environmental perception and prevent it from responding to moving obstacles. Building a dynamic model can reflect environmental changes in real time, allowing the beaver-like robot to plan its path in advance, avoid collisions, and achieve efficient and safe underwater operations. Traditional underwater environment modeling methods often use single-sensor data to construct static models or lack the ability to integrate multi-source data. For example, models constructed solely from sonar data lack visual detail and cannot accurately identify obstacle types. Furthermore, the model cannot be updated in real time based on the beaver-like robot's movements, making it difficult to predict the trajectory of moving obstacles. During the beaver-like robot's movement, coordinate offset cannot be effectively addressed, resulting in a significant deviation between the model and the actual environment.

[0051] This solution uses multi-source data fusion technology, combined with spatial registration, 3D gridding, coordinate system alignment, and trajectory prediction, to construct a dynamic model that updates in real time with the environment and the beaver-like robot's movements. By integrating the advantages of multiple sensor data and dynamically adjusting model parameters, accurate, real-time modeling of the underwater environment is achieved.

[0052] Specifically, spatial registration is performed on the underwater obstacle distance and outline information and the underwater dense 3D point cloud data to obtain a 3D obstacle model in a unified coordinate system. Spatial registration involves first determining the coordinate system parameters for the underwater obstacle distance and outline information acquired by sonar and the underwater dense 3D point cloud data generated by the binocular vision camera. Corresponding feature points (such as vertices and edge features of the obstacle) are then found in the two data sets and converted to the same coordinate system using geometric transformation algorithms (such as translation, rotation, and scaling). For example, the coordinate difference between a feature point in the sonar data and the visual point cloud data is calculated to determine the translation and rotation parameters. The two data sets are then superimposed to form a complete 3D obstacle model in the unified coordinate system. Because the data collected by the sonar and binocular vision cameras come from different sensors and have different coordinate systems, without spatial registration, positional deviations will occur during data fusion, leading to erroneous judgments of the obstacle's position and shape. A unified coordinate system can integrate the advantages of both data types, more accurately presenting the obstacle's 3D form and providing a precise foundation for subsequent environmental modeling. Therefore, establishing an accurate three-dimensional model of the obstacle can significantly reduce the error in the beaver-like robot's judgment of the obstacle's position and shape.

[0053] Based on a 3D obstacle model, a 3D grid method is used to divide the underwater environment. Each grid is labeled to store an obstacle probability value, and the probability value is dynamically adjusted as sensor data is updated. This results in a 3D grid map with probability values. This divides the underwater environment into small 3D grids. Based on the 3D obstacle model, the presence or probability of an obstacle within each grid is determined. If a grid is located inside or close to an obstacle, a higher obstacle probability value is assigned; if it is far from an obstacle, a lower probability value is assigned. As sensors continuously collect new data, such as when sonar detects a new obstacle or an obstacle changes its position, the probability value for the corresponding grid is updated in real time. For example, if the sonar detects a new obstacle entering a certain area, the obstacle probability value for the grid in that area is increased. Because the underwater environment is complex and obstacles are irregularly distributed, the 3D grid method discretizes the environment for easier computer processing and storage. The labeled probability values ​​reflect the likelihood of obstacle presence and dynamically adjust to environmental changes, providing intuitive environmental information for the beaver-like robot's path planning, enabling it to avoid areas with a high probability of obstacles. The generated 3D grid map with probability values ​​clearly displays the distribution probability of underwater obstacles. This allows the beaver-like robot to choose safer paths based on these probabilities during path planning, reducing collision risks. For example, in areas with numerous ambiguous obstacles, the map can accurately indicate high-risk areas, guiding the beaver-like robot around them.

[0054] The three-dimensional grid map is aligned with the acceleration, angular velocity, and attitude angle information to eliminate coordinate offsets caused by the beaver robot's motion and generate an environmental map synchronized with the robot's real-time position and posture. The system utilizes the beaver robot's acceleration, angular velocity, and attitude angle information acquired by the inertial measurement unit (IMU) to calculate the robot's position and attitude changes in real time. Based on these changes, the three-dimensional grid map undergoes a corresponding coordinate transformation. For example, when the beaver robot moves forward a certain distance and rotates, the actual movement and rotation of the beaver robot are determined by integrating the acceleration and angular velocity data. The three-dimensional grid map is then subjected to the same translation and rotation transformations in space to synchronize the map's coordinate system with the robot's current position and posture. Because the beaver robot's position and attitude constantly change while moving underwater, without coordinate alignment, the three-dimensional grid map based on a fixed coordinate system would deviate from the actual environment, leading to misjudgment of the robot's environment. Real-time alignment ensures that the map always accurately reflects the robot's surroundings, providing a reliable basis for path planning and motion control. Through this solution, the coordinate offset caused by the beaver-like robot's movement is effectively eliminated, and the matching degree between the environmental map and the actual environment is greatly improved.

[0055] Obtaining moving obstacles from the three-dimensional grid map and obtaining predicted obstacle trajectories based on their speed and acceleration involves analyzing changes in obstacle probability values ​​within the three-dimensional grid map and combining this with sensor data (such as continuous sonar detection data) to determine which moving obstacles are present. After identifying the moving obstacles, the system uses the speed and acceleration information acquired by the sensors to employ a kinematic prediction algorithm (such as a Kalman filter or a prediction method based on a physical model). Taking the Kalman filter as an example, the current position is predicted based on the previous position, speed, and acceleration. This is then corrected using current sensor measurements to iteratively predict the future trajectory of the moving obstacle. Because many moving obstacles exist underwater (such as fish and other mobile devices), collisions may occur if the beaver robot fails to predict their trajectories in advance. Obtaining predicted obstacle trajectories allows the beaver robot to plan its path in advance, avoid moving obstacles, and ensure its own safety and smooth operation. This relatively accurate prediction of the obstacle's trajectory provides the beaver robot with ample reaction time to avoid obstacles.

[0056] A dynamic underwater environment model is established based on the environment map synchronized with the real-time position of the beaver-like robot and the predicted obstacle trajectories. This model integrates the environment map (including information such as obstacle distribution probabilities) with the predicted obstacle trajectories. For example, the map marks the likely future locations of obstacles, forming a model that comprehensively reflects the static obstacle distribution and dynamic trajectories of the current underwater environment. This model can be updated in real time based on sensor data, maintaining synchronization with the actual environment. Since a single environment map or obstacle trajectory information alone cannot fully reflect the dynamic underwater environment, the dynamic model established by integrating these two information provides the beaver-like robot with complete, real-time environmental information, enabling it to fully account for environmental changes during path planning and motion control, achieving more intelligent and safe underwater operations. The established dynamic underwater environment model accurately and in real time presents the entire underwater environment, providing high-quality input for the beaver-like robot's path planning algorithm. This significantly improves the beaver-like robot's obstacle avoidance success rate in complex underwater environments and enhances operational efficiency.

[0057] Through a series of steps, these data are fused and processed to construct an accurate and real-time dynamic model of the underwater environment, which can provide key support for the autonomous operation of the beaver-like robot in complex underwater environments.

[0058] In one embodiment of the present invention, the step of generating a global navigation path based on the underwater environment dynamic model using an improved path planning algorithm includes: S41, obtaining a starting position and a target position of the beaver-like robot according to the underwater environment dynamic model; S42, using a bidirectional search mechanism to synchronously expand path nodes from the starting position and the target position, terminating the calculation when the two search domains intersect, and generating a preliminary path; S43, skipping intermediate nodes without path value in the preliminary path by a jump point search algorithm to obtain an optimized path; S44, design a dynamic heuristic function, and adjust the path cost estimate of the optimization path in combination with the water velocity vector to obtain a secondary optimization path. The dynamic heuristic function is: ; Where, represents the dynamic heuristic function, represents the current node position vector (representing the path node to be evaluated in path planning), represents the target position vector (indicates the end point of the current task), represents the velocity vector of the beaver-like robot, represents the water velocity vector (obtained from historical hydrological data), Indicates the water flow influence coefficient (the value range is 0.1~0.5, and the larger the value, the more significant the impact of water flow on path cost); S45 , smoothing the secondary optimization path using a curve fitting algorithm to eliminate jagged tracks in the secondary optimization path, thereby obtaining a smooth and non-jaggy global navigation path.

[0059] As described in steps S41-S45 above, the present invention utilizes a dynamic model of the underwater environment and adopts algorithms such as bidirectional search, jump point search, dynamic heuristic function, and curve fitting to gradually optimize the path and generate a global navigation path from the starting point of the beaver-like robot to the target point that is smooth, efficient, and adaptable to the underwater environment (such as the influence of water flow), thereby providing planning guidance for the beaver-like robot's autonomous navigation underwater.

[0060] In underwater environments, a beaver-like robot needs to plan a reasonable path from its starting point to its target location. Underwater, complex factors such as obstacles and water currents exist, and traditional path planning methods often cannot adapt well to these dynamic changes. For example, if water currents are not taken into account, the beaver-like robot may choose a path that consumes a lot of energy or takes a long time. A non-smooth path increases the difficulty of motion control and energy consumption. Therefore, a path planning method that comprehensively considers multiple factors is needed to ensure that the beaver-like robot reaches its target location safely and efficiently.

[0061] Traditional path planning algorithms typically perform a one-way search from the starting point to the target, resulting in low search efficiency and high time complexity. Furthermore, they often use fixed heuristic functions and fail to consider dynamic environmental factors such as water flow, resulting in suboptimal paths. Furthermore, the resulting paths may contain numerous jagged inflection points, hindering the practical motion control of beaver-like robots. This solution improves search efficiency by improving the search mechanism (bidirectional search and point-hopping search), introducing a dynamic heuristic function to account for the influence of water flow, and using a curve-fitting algorithm to smooth the path, thereby generating a more optimal global navigation path.

[0062] Specifically, the starting and target positions of the beaver-like robot are obtained based on the dynamic model of the underwater environment: the dynamic model of the underwater environment records various information about the underwater environment, including the relevant position information of the beaver-like robot. By reading and parsing the model data, the beaver-like robot's current starting position and the coordinates of the target position set for the task are directly obtained. Because clearly defining the starting and target positions is the basis of path planning, only after determining these two key locations can subsequent path search and planning be carried out around them. By accurately obtaining the starting and target position information, clear starting and ending points are provided for subsequent path planning steps, which is the prerequisite for the entire path planning.

[0063] A bidirectional search mechanism simultaneously expands path nodes from the starting point and target location. The calculation terminates when the two search areas intersect, generating a preliminary path. The bidirectional search mechanism simultaneously searches for path nodes from both the starting point and target point. During the search, new path nodes are expanded outwards from the starting point and target point, following certain rules (such as a distance metric). This expansion process is repeated until the search areas from the starting point and target point intersect. At this point, the intersection point is connected with the starting point and target point to generate a preliminary path. For example, in a simple underwater two-dimensional scene, starting point A and target point B simultaneously explore the surrounding areas for traversable nodes. When their search areas intersect at node C, A and B form a preliminary path. Because traditional one-way search requires traversing a large number of nodes from the starting point to the target point, search efficiency is low. Bidirectional search, performed simultaneously from both ends, significantly reduces the search range and the number of nodes expanded, enabling faster discovery of the path connecting the starting point and target point, improving path planning efficiency. Compared to traditional one-way search, search time is significantly reduced, enabling the generation of preliminary paths in a shorter time, thus increasing the responsiveness of the beaver-like robot. For example, in larger underwater environments, path search time can be reduced by more than 50%.

[0064] A skipping point search algorithm is used to skip intermediate nodes in the preliminary path that have no path value, resulting in an optimized path. Based on the preliminary path, the skipping point search algorithm searches for "jumping points" in the path. Jumping points are key points in the path search that can be directly connected without traversing intermediate nodes. These jumping points are identified using specific rules (such as those based on geometric relationships and traversability). These jumping points are then directly connected, skipping intermediate nodes that have no value, thereby simplifying the path. For example, in a preliminary path with multiple turns, the algorithm can identify certain nodes that can be directly connected and remove unnecessary intermediate turning nodes, making the path simpler. Because the preliminary path may contain many unnecessary intermediate nodes, which increase the path length and computational complexity, the skipping point search algorithm removes these redundant nodes, optimizing the path and enabling the beaver-like robot to navigate a shorter, more efficient path. This effectively shortens the path length, reduces the number of path nodes, and reduces the computational complexity of path planning. It also enables the beaver-like robot to travel shorter distances, saving energy.

[0065] A dynamic heuristic function is designed to adjust the estimated path cost of the optimized path in conjunction with the water flow velocity vector, resulting in a secondary optimized path. The dynamic heuristic function adjusts the estimated path cost in real time based on the water flow velocity vector. For example, when the water flow velocity is high and opposite to the direction of the beaver-like robot's motion, the path cost calculated by the function increases, guiding the algorithm to select a more favorable path. The water flow velocity vector is obtained from historical hydrological data. Due to the presence of underwater currents, traditional path planning algorithms using fixed heuristic functions do not consider the impact of water flow, and the resulting paths may be energy-intensive or difficult to navigate. The dynamic heuristic function incorporates water flow into the path cost calculation, making path planning more consistent with the actual underwater environment and guiding the beaver-like robot to select a more energy-efficient and efficient path. The resulting path can better adapt to the water flow environment, reducing the energy consumption of the beaver-like robot during navigation in the water flow and improving navigation efficiency.

[0066] A curve fitting algorithm is used to smooth the quadratically optimized path, eliminating any jagged edges in the path and producing a smooth, non-jaggy global navigation path. The curve fitting algorithm processes the nodes on the quadratically optimized path. Using mathematical methods (such as the least squares method), a smooth curve is fitted based on the coordinate information of the path nodes. For example, for a series of discrete path nodes, a curve is found that best approximates these nodes, passing through or approaching these nodes, thereby eliminating any jagged inflection points in the original path. Since the path obtained in the previous steps may contain jagged edges, this can cause the beaver robot to frequently adjust its direction during navigation, increasing the difficulty of motion control and energy consumption, and potentially affecting navigation stability. The smoothed path better conforms to the beaver robot's motion characteristics, facilitating stable and efficient navigation. Because the resulting global navigation path is smooth and continuous, the beaver robot's motion control is simpler, navigation is more stable, and energy consumption is further reduced.

[0067] The entire path planning process revolves around a dynamic model of the underwater environment. It begins with obtaining the starting and target positions, and through operations such as search, jump point optimization, adjustment of water flow factors, and path smoothing, it ultimately generates a global path suitable for the beaver-like robot to navigate underwater, effectively improving the efficiency and quality of path planning and its adaptability to complex underwater environments.

[0068] In one embodiment of the present invention, the step of adjusting the thrust distribution of the propeller in real time by using a fuzzy proportional integral differential controller includes: S51, obtaining a lateral deviation between the beaver-like robot and the global navigation path, and calculating a change rate of the lateral deviation; S52, defining a fuzzy logic rule, wherein the input variables are the lateral deviation and the lateral deviation change rate, and the output variables are the adjustment amounts of the proportional, integral, and differential gains; S53, establishing a fuzzy rule base, and dynamically adjusting control parameters according to the magnitude of the lateral deviation and the changing trend of the lateral deviation change rate; S54, using the center of gravity method to defuzzify, converting the output of the fuzzy logic rule into an actual gain adjustment value, the formula is: ; Where, Indicates the actual gain adjustment amount, Indicates the basic value of proportional gain, represents the adjustment amount of proportional, integral and differential gains (i.e. the output of the fuzzy logic rule); S55, based on the actual gain adjustment amount, adjusting the propeller speed and hind leg movement speed of the beaver-like robot according to the thrust distribution strategy, and introducing an anti-saturation mechanism to prevent the propeller and the hind legs from exceeding the limit operation.

[0069] As described in the above steps S51-S55, the present invention uses a fuzzy proportional integral differential (PID) controller to adjust the thrust distribution of the propellers in real time based on the lateral deviation and deviation change rate between the beaver robot and the global navigation path, accurately control the propeller speed and hind leg movement speed of the beaver robot, and prevent the equipment from running beyond the limit, thereby achieving high-precision tracking of the planned path by the beaver robot and ensuring its stable and safe navigation underwater.

[0070] When navigating underwater, beaver-like robots are subject to interference from various factors, such as currents and obstacles, which can cause them to deviate from their pre-set global navigation path. If these deviations are not corrected promptly, the beaver-like robot will not be able to reach its target location as planned and may even encounter dangers such as collisions. Traditional PID controllers, with their fixed parameters, struggle to cope with the complex and changing underwater environment and lack the flexibility to adjust control parameters based on actual deviations. Therefore, a method is urgently needed to dynamically adjust control parameters in real time based on deviations and their rate of change to ensure the accuracy and stability of the beaver-like robot's navigation.

[0071] The proportional, integral, and differential gain parameters of traditional PID controllers remain unchanged once set. In underwater environments, when encountering sudden changes in water flow or approaching obstacles, fixed parameters cannot quickly and effectively correct the path deviation of the beaver robot. This may cause the beaver robot to over-adjust or be unable to return to the correct path for a long time, seriously affecting navigation efficiency and safety. This solution uses fuzzy logic rules, taking the lateral deviation of the beaver robot from the path and the rate of change of the deviation as input, and adjusts the proportional, integral, and differential gain parameters of the PID controller in real time. By constructing a fuzzy rule library, the parameter adjustment amount is dynamically determined according to the deviation situation, and then converted into a practical and applicable parameter adjustment value through a defuzzification operation, thereby achieving precise control of the thrust distribution of the propeller.

[0072] Specifically, the lateral deviation between the beaver-like robot and the global navigation path is obtained, and the rate of change of the lateral deviation is calculated: the position information of the beaver-like robot is collected in real time using sensors equipped by the beaver-like robot, such as inertial measurement units, visual sensors, etc. The collected position information is compared with the pre-planned global navigation path, and the lateral distance difference between the current position of the beaver-like robot and the path is calculated. This difference is the lateral deviation. At the same time, the lateral deviation data within a certain time interval is analyzed and processed, and the rate of change of the lateral deviation is obtained by calculating the ratio of the difference in lateral deviations at adjacent moments to the time interval. Since the lateral deviation directly reflects the degree to which the beaver-like robot currently deviates from the predetermined path, the rate of change of the lateral deviation reflects the trend of change of the deviation. These two key data are important bases for the subsequent adjustment of control parameters. Only by accurately obtaining them can the motion state of the beaver-like robot be accurately judged and timely adjustment decisions be made. It is possible to obtain the deviation of the beaver-like robot from the path and the deviation change trend information in real time and accurately, providing reliable input data for subsequent fuzzy logic control. Fuzzy logic rules are defined, with the input variables being the lateral deviation and the lateral deviation change rate, and the output variables being the adjustments to the proportional, integral, and differential gains. The two input variables, lateral deviation and lateral deviation change rate, are divided into multiple fuzzy sets based on their magnitude ranges, such as "negative large," "negative medium," "negative small," "zero," "positive small," "positive medium," and "positive large." Furthermore, the adjustments to the proportional, integral, and differential gains are divided into corresponding fuzzy sets, such as "negative large adjustment," "negative medium adjustment," "negative small adjustment," "no adjustment," "positive small adjustment," "positive medium adjustment," and "positive large adjustment." Then, based on experience and extensive experimental data, a series of fuzzy logic rules are developed. For example, "If the lateral deviation is positive large and the lateral deviation change rate is positive small, then the proportional gain adjustment is positive medium." These rules clearly describe the corresponding relationships between different fuzzy set combinations of the input variables and the fuzzy sets of the output variables. Due to the complex and variable underwater environment, the deviations of the beaver-like robot vary widely. By defining fuzzy logic rules, precise input data is fuzzified and appropriate PID gain adjustment strategies are determined based on different deviations, enabling the controller to more flexibly respond to various practical scenarios. A fuzzy logic decision-making mechanism for adjusting path deviations for underwater beaver-like robots has been established, providing clear rules for adjusting control parameters in real time and significantly improving the controller's adaptability to complex environments.

[0073] A fuzzy rule base is established to dynamically adjust control parameters based on the magnitude of the lateral deviation and the trend of the lateral deviation rate of change. The fuzzy logic rules defined in step S52 are systematically summarized and organized to form a complete fuzzy rule base. During actual operation, after obtaining the current lateral deviation and deviation rate of change of the beaver-like robot, the corresponding rules are searched in the fuzzy rule base based on the fuzzy sets to which they belong, thereby determining the adjustments for the proportional, integral, and differential gains. For example, if the current lateral deviation belongs to the "positive small" fuzzy set and the lateral deviation rate of change belongs to the "negative medium" fuzzy set, the proportional gain adjustment is determined to be "negative small adjustment" by searching the rule base. Because the fuzzy rule base is a core component of fuzzy logic control, it integrates various possible deviation situations and corresponding control parameter adjustment strategies. By querying the rule base in real time, appropriate control parameter adjustment strategies can be quickly and accurately determined based on the actual deviation state of the beaver-like robot, effectively controlling the beaver-like robot's motion. This achieves dynamic adjustment of control parameters, enabling the PID controller to automatically optimize the control strategy based on different deviation situations.

[0074] The center of gravity method is used to defuzzify the output of the fuzzy logic rules, converting them into actual gain adjustments. The fuzzy logic rules output a fuzzy set of proportional, integral, and differential gain adjustments, which cannot be directly applied to a PID controller and must be converted into precise numerical values. The center of gravity method is a defuzzification method that determines the precise value by calculating the center of gravity of the fuzzy set. Specifically, for each fuzzy set of output variables, a weighted average is calculated based on its membership function (a function that describes the degree to which an element belongs to the fuzzy set) and the corresponding quantization level. This weighted average is the actual gain adjustment. For example, for the fuzzy set of proportional gain adjustments, the adjustment values ​​corresponding to each fuzzy subset are multiplied by their membership, summed, and then divided by the total membership to obtain the final actual proportional gain adjustment. Since the fuzzy results output by the fuzzy logic rules cannot be directly used for controller parameter adjustment, the defuzzification process converts them into specific, actionable numerical values, allowing the controller to accurately adjust the proportional, integral, and differential gains based on these precise values, thereby achieving precise control of thrust distribution. By converting the fuzzy decision results into precise control parameter adjustment values, the feasibility of the control algorithm is ensured, the control precision is effectively improved, and the motion control of the beaver-like robot is made more accurate and stable.

[0075] Based on the actual gain adjustment, the propeller speed and hind leg movement speed of the beaver-like robot are adjusted according to the thrust distribution strategy. An anti-saturation mechanism is introduced to prevent the propellers and hind legs from exceeding their limits. Based on the actual gain adjustment obtained in step S54 and combined with the pre-set thrust distribution strategy, the required thrust for each propeller is calculated and converted into a propeller speed control signal and a hind leg movement speed control signal. The adjusted actual gain adjustment is obtained according to the proportional gain adjustment formula and substituted into the PID control algorithm to calculate the thrust value. This thrust value is then converted into the propeller speed via the motor drive circuit. Furthermore, to prevent the propellers and hind legs from exceeding their limits (e.g., exceeding the maximum speed or maximum thrust limit) due to excessive control signals during the adjustment process, an anti-saturation mechanism is introduced. When the control signal reaches a set upper limit, it is capped at that upper limit, ensuring safe and stable operation of the device. By adjusting the propeller speed and hind leg movement speed, the beaver-like robot's movement direction and speed can be changed, returning it to its predetermined global navigation path. The anti-saturation mechanism protects the propellers and hind legs, preventing damage from overdrive and ensuring system reliability and safety. This precise control of the beaver robot's propellers and hind legs enables it to quickly and stably track its global navigation path. It also effectively prevents damage to the equipment due to overload, extending its service life.

[0076] In summary, the fuzzy PID control method significantly improves the accuracy and stability of path tracking of the beaver-like robot in complex underwater environments and enhances the reliability of the system.

[0077] like Figure 2 As shown, the present invention also discloses a multi-modal sensor fusion beaver robot navigation path control system, comprising: A data acquisition module is used to collect underwater environment data and beaver-like robot motion posture data in real time through a multimodal sensor group; A preprocessing module, used for preprocessing the underwater environment data and the beaver-like robot motion posture data; A fusion calculation module is used to fuse the underwater environment data and the beaver-like robot motion posture data and construct an underwater environment dynamic model; A path planning module, configured to generate a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model; A motion control module is used to adjust the thrust distribution of the thrusters in real time through a fuzzy proportional-integral-derivative controller.

[0078] The path planning module includes: The environmental perception and positioning unit obtains the current position and target position of the beaver-like robot based on the underwater environment dynamic model; a bidirectional path extension unit, which adopts a bidirectional search mechanism to synchronously extend path nodes from the current position and the target position, terminates the calculation when the two search domains intersect, and generates a preliminary path; a jump point path optimization unit, which skips intermediate nodes without path value in the preliminary path through a jump point search algorithm to obtain an optimized path; A secondary optimization unit designs a dynamic heuristic function and adjusts the path cost estimation value of the optimization path in combination with the water flow velocity vector to obtain a secondary optimization path; The path trajectory smoothing unit uses a curve fitting algorithm to smooth the secondary optimization path, eliminate the jagged trajectory in the secondary optimization path, and obtain a smooth and non-jaggy global navigation path.

[0079] A computer device includes a memory and a processor, wherein the memory stores a computer program, and is characterized in that when the processor executes the computer program, it implements the steps of a navigation path control method of a beaver-like robot using multimodal sensor fusion.

[0080] A computer-readable storage medium stores a computer program thereon, wherein when the computer program is executed by a processor, the computer program implements the steps of a navigation path control method of a beaver-like robot using multimodal sensor fusion.

[0081] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, apparatus, article, or method comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, apparatus, article, or method. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, apparatus, article, or method comprising the element.

[0082] The above description is only a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made by using the contents of the present invention description and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A multimodal sensor fusion beaver robot navigation path control method, characterized in that: include: The underwater environment data and the motion posture data of the beaver-like robot are collected in real time by a multimodal sensor group. The underwater environment data includes the distance and contour information of underwater obstacles, underwater three-dimensional point cloud data and water pressure information. The motion posture information of the beaver-like robot includes acceleration information, angular velocity information and attitude angle information. Preprocessing the underwater environment data and the beaver-like robot motion posture data, including denoising, correction and zero bias compensation; fusing the underwater environment data and the motion posture data of the beaver-like robot to construct an underwater environment dynamic model; Based on the underwater environment dynamic model, an improved path planning algorithm is used to generate a global navigation path; Real-time adjustment of beaver robot thrust distribution via fuzzy proportional-integral-derivative controller.

2. The navigation path control method of a beaver-like robot based on multimodal sensor fusion according to claim 1 is characterized in that: The step of collecting underwater environment data and beaver-like robot motion posture data in real time through the multimodal sensor group includes: Obtain the distance and outline information of underwater obstacles through the sonar module; Acquire underwater image information through binocular vision cameras and generate underwater 3D point cloud data; The water pressure information of the water depth where the beaver-like robot is located is obtained through a depth sensor; The acceleration information, angular velocity information and attitude angle information of the beaver-like robot are obtained through the inertial measurement unit; The distance and contour information of the underwater obstacle, the underwater three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information, and attitude angle information are time stamp-aligned.

3. The navigation path control method of a beaver-like robot based on multimodal sensor fusion according to claim 2 is characterized in that: The step of pre-processing the underwater environment data and the beaver-like robot motion posture data comprises: Eliminating noise from the distance and contour information of the underwater obstacle by using an adaptive filtering algorithm; Performing distortion correction and feature point matching on the underwater image information, and generating underwater dense three-dimensional point cloud data; Performing mean filtering on the water pressure information to eliminate instantaneous fluctuations; Performing zero bias compensation on the acceleration information, angular velocity information, and attitude angle information; A data set is established based on the pre-processed distance and contour information of the underwater obstacle, the underwater image information and the underwater dense three-dimensional point cloud data, the water pressure information and the acceleration information, the angular velocity information and the attitude angle information.

4. The navigation path control method of a beaver-like robot based on multimodal sensor fusion according to claim 3 is characterized in that: The step of fusing the underwater environment data and the beaver-like robot motion posture data to construct an underwater environment dynamic model comprises: Performing spatial registration on the distance and contour information of the underwater obstacle and the underwater dense three-dimensional point cloud data to obtain a three-dimensional model of the obstacle in a unified coordinate system; Based on the 3D obstacle model, the underwater environment space is divided into 3D grids. Each grid is marked to store the probability value of the obstacle. The probability value is dynamically adjusted as the sensor data is updated to obtain a 3D grid map with probability values. Combining the acceleration information, angular velocity information, and attitude angle information, the three-dimensional grid map is aligned with the coordinate system to eliminate the coordinate offset caused by the motion of the beaver-like robot, thereby obtaining an environment map that is synchronized with the real-time posture of the beaver-like robot; Obtaining a moving obstacle in the three-dimensional grid map, and obtaining moving obstacle trajectory prediction information based on the speed and acceleration of the moving obstacle; An underwater environment dynamic model is established based on the environment map of the real-time position of the synchronized beaver-like robot and the mobile obstacle trajectory prediction information.

5. The navigation path control method of a beaver-like robot based on multimodal sensor fusion according to claim 4 is characterized in that: The step of generating a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model includes: Obtain the starting position and target position of the beaver-like robot according to the underwater environment dynamic model; A bidirectional search mechanism is used to simultaneously expand path nodes from the starting position and the target position, and the calculation is terminated when the two search domains intersect to generate a preliminary path; skipping intermediate nodes without path value in the preliminary path by a skipping point search algorithm to obtain an optimized path; Design a dynamic heuristic function, and adjust the path cost estimate of the optimization path in combination with the water flow velocity vector to obtain a secondary optimization path; The quadratic optimization path is smoothed by using a curve fitting algorithm to eliminate the jagged trajectory in the quadratic optimization path, thereby obtaining a smooth and non-jaggy global navigation path.

6. The navigation path control method of a beaver-like robot based on multimodal sensor fusion according to claim 1, characterized in that: The steps of adjusting the thrust distribution of the beaver-like robot in real time through the fuzzy proportional-integral-derivative controller include: Obtaining a lateral deviation between the beaver-like robot and the global navigation path, and calculating a rate of change of the lateral deviation; defining a fuzzy logic rule, wherein the input variables are the lateral deviation and the rate of change of the lateral deviation, and the output variables are the adjustment amounts of the proportional, integral, and differential gains; Establishing a fuzzy rule base to dynamically adjust control parameters according to the magnitude of the lateral deviation and the changing trend of the lateral deviation change rate; Defuzzification is performed using a centroid method to convert the output of the fuzzy logic rule into an actual gain adjustment value; Based on the actual gain adjustment amount, the propeller rotation speed and the hind leg movement speed of the beaver-like robot are adjusted according to the thrust distribution strategy, and an anti-saturation mechanism is introduced to prevent the propeller and the hind legs from exceeding the limit.

7. A multimodal sensor fusion beaver robot navigation path control system, characterized in that: include: A data acquisition module is used to collect underwater environment data and beaver-like robot motion posture data in real time through a multimodal sensor group; A preprocessing module, used for preprocessing the underwater environment data and the beaver-like robot motion posture data; A fusion calculation module is used to fuse the underwater environment data and the beaver-like robot motion posture data and construct an underwater environment dynamic model; A path planning module, configured to generate a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model; A motion control module is used to adjust the thrust distribution of the thrusters in real time through a fuzzy proportional-integral-derivative controller.

8. The multimodal sensor fusion beaver robot navigation path control system according to claim 7, characterized in that: The path planning module includes: The environmental perception and positioning unit obtains the current position and target position of the beaver-like robot based on the underwater environment dynamic model; a bidirectional path extension unit, which adopts a bidirectional search mechanism to synchronously extend path nodes from the current position and the target position, terminates the calculation when the two search domains intersect, and generates a preliminary path; a jump point path optimization unit, which skips intermediate nodes without path value in the preliminary path through a jump point search algorithm to obtain an optimized path; A secondary optimization unit designs a dynamic heuristic function and adjusts the path cost estimation value of the optimization path in combination with the water flow velocity vector to obtain a secondary optimization path; The path trajectory smoothing unit uses a curve fitting algorithm to smooth the secondary optimization path, eliminate the jagged trajectory in the secondary optimization path, and obtain a smooth and non-jaggy global navigation path.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images