A beaver-like robot navigation path control method and system based on multi-modal sensor fusion

Abstract

The present invention relates to the field of robot intelligent control technology, and in particular to a navigation path control method for a beaver-like robot using multimodal sensor fusion. The method comprises: collecting underwater environment data and motion posture data of the beaver-like robot in real time through a multimodal sensor group; preprocessing the underwater environment data and the motion posture data of the beaver-like robot; and generating a global navigation path based on a dynamic model of the underwater environment using an improved path planning algorithm. The present invention uses multimodal sensor fusion technology to integrate multiple sensors such as sonar, binocular vision cameras, depth sensors, and inertial measurement units to achieve comprehensive collection of underwater environment and motion posture data of the beaver-like robot, effectively solving the problem of incomplete perception of traditional single sensors, greatly improving the obstacle detection rate, significantly improving the accuracy of environmental perception, and improving the navigation efficiency and safety of the beaver-like robot in complex underwater environments.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of beaver robot intelligent control, and particularly relates to a multi-modal sensor fusion beaver robot navigation path control method and system. BACKGROUND

[0002] The underwater beaver robot is widely used in the fields of ocean exploration, ecological monitoring, underwater operation, etc. The underwater beaver robot mainly consists of a control system, a drainage buoyancy system, a rudder driving system, a sonar, a sensor and the like, and is mainly used for studying the life phenomena, processes, laws in the ocean and carrying out rescue, salvage and military applications.

[0003] However, the existing underwater beaver robot faces many challenges in the complex underwater environment. In terms of environmental perception, the traditional underwater beaver robot often uses a single sensor, such as relying only on a sonar or a visual sensor. Affected by factors such as underwater turbidity, light changes, water flow disturbances, etc., it is difficult to comprehensively and accurately obtain underwater obstacle, terrain and other environmental information and the beaver robot's own attitude information, resulting in blind spots and errors in environmental perception. In terms of path planning, the traditional algorithm does not fully consider the dynamic changes of the underwater environment. In the motion control link, the traditional controller is difficult to adapt to the complex and changeable hydrodynamic environment of the underwater environment. When the beaver robot is impacted by water flow, attitude mutation and the like, it is difficult to quickly and accurately adjust, and it is easy to have problems such as overshoot, oscillation or long-term deviation from the predetermined path, which affects the navigation accuracy and stability of the beaver robot. SUMMARY

[0004] The main purpose of the present application is to provide a multi-modal sensor fusion beaver robot navigation path control method and system, which aims to solve the technical problems raised in the background.

[0005] The present application provides a multi-modal sensor fusion beaver robot navigation path control method, which comprises:

[0006] The multi-modal sensor group collects underwater environment data and beaver robot motion attitude data in real time, wherein the underwater environment data includes underwater obstacle distance and contour information, underwater three-dimensional point cloud data and water pressure information, and the beaver robot motion attitude information includes acceleration information, angular velocity information and attitude angle information;

[0007] The underwater environment data and beaver robot motion attitude data are preprocessed, including denoising, correction and zero offset compensation;

[0008] The underwater environment data and beaver robot motion attitude data are fused to construct an underwater environment dynamic model;

[0009] Based on the underwater environment dynamic model, an improved path planning algorithm is used to generate a global navigation path.

[0010] The fuzzy proportional-integral-derivative controller is used to adjust the thrust distribution of the beaver-like robot in real time.

[0011] Preferably, the step of collecting underwater environment data and beaver-like robot motion attitude data in real time through a multi-modal sensor group comprises:

[0012] The sonar module is used to obtain the distance and contour information of underwater obstacles;

[0013] The binocular vision camera is used to obtain underwater image information and generate underwater three-dimensional point cloud data;

[0014] The depth sensor is used to obtain the water pressure information of the water depth where the beaver-like robot is located;

[0015] The inertial measurement unit is used to obtain the acceleration information, angular velocity information and attitude angle information of the beaver-like robot;

[0016] The distance and contour information of the underwater obstacles, the underwater three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information and attitude angle information are time-stamped aligned.

[0017] Preferably, the step of preprocessing the underwater environment data and beaver-like robot motion attitude data comprises:

[0018] The adaptive filtering algorithm is used to eliminate noise from the distance and contour information of the underwater obstacles;

[0019] The underwater image information is corrected for distortion and feature points are matched to generate underwater dense three-dimensional point cloud data;

[0020] The mean filter is used to eliminate transient fluctuations from the water pressure information;

[0021] The acceleration information, angular velocity information and attitude angle information are compensated for zero offset;

[0022] A data set is established according to the preprocessed distance and contour information of the underwater obstacles, the underwater image information and underwater dense three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information and attitude angle information.

[0023] Preferably, the step of fusing the underwater environment data and beaver-like robot motion attitude data to construct an underwater environment dynamic model comprises:

[0024] The distance and contour information of the underwater obstacles and the underwater dense three-dimensional point cloud data are spatially registered to obtain an obstacle three-dimensional model in a unified coordinate system;

[0025] based on the three-dimensional model of the obstacle, the underwater environment space is divided by 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 with sensor data update to obtain a three-dimensional grid map with probability value;

[0026] combined with the acceleration information, angular velocity information and attitude angle information, the three-dimensional grid map is aligned in the coordinate system to eliminate the coordinate offset caused by the movement of the beaver robot, and an environment map synchronized with the real-time pose of the beaver robot is obtained;

[0027] obtain the moving obstacle in the three-dimensional grid map, and obtain the moving obstacle trajectory prediction information according to the speed and acceleration of the moving obstacle;

[0028] According to the environment map synchronized with the real-time pose of the beaver robot and the moving obstacle trajectory prediction information, an underwater environment dynamic model is established.

[0029] Preferably, the step of generating a global navigation path based on the underwater environment dynamic model by using an improved path planning algorithm comprises:

[0030] According to the underwater environment dynamic model, the starting position and target position of the beaver robot are obtained;

[0031] A bidirectional search mechanism is used to expand path nodes from the starting position and target position simultaneously, and the calculation is terminated when the two search domains intersect to generate a preliminary path;

[0032] An intermediate node without path value in the preliminary path is jumped through a jump point search algorithm to obtain an optimized path;

[0033] A dynamic heuristic function is designed to adjust the path cost estimate value of the optimized path combined with the flow velocity vector to obtain a secondary optimized path;

[0034] A curve fitting algorithm is used to smooth the secondary optimized path to eliminate the jagged trajectory in the secondary optimized path to obtain a smooth and jagged global navigation path.

[0035] Preferably, the step of adjusting the propeller thrust distribution in real time by using a fuzzy proportional-integral-derivative controller comprises:

[0036] Obtain the lateral deviation of the beaver robot from the global navigation path, and calculate the lateral deviation change rate;

[0037] Define a fuzzy logic rule, the input variables of which are the lateral deviation and the lateral deviation change rate, and the output variables are the adjustment amounts of proportional, integral and derivative gains;

[0038] A fuzzy rule base is established, and a control parameter is dynamically adjusted according to the size of the lateral deviation and the change trend of the lateral deviation change rate;

[0039] The output of the fuzzy logic rule is converted into an actual gain adjustment amount by using the gravity method to solve the ambiguity;

[0040] Based on the actual gain adjustment amount, the thruster rotation speed and the rear leg movement speed of the beaver-like robot are adjusted according to a thrust distribution strategy, and an anti-saturation mechanism is introduced to prevent the thruster and the rear leg from running out of limits.

[0041] The application also discloses a multi-modal sensor fusion beaver-like robot navigation path control system, which comprises:

[0042] A data acquisition module is configured to acquire underwater environment data and beaver-like robot movement posture data in real time through a multi-modal sensor group;

[0043] A preprocessing module is configured to preprocess the underwater environment data and beaver-like robot movement posture data;

[0044] A fusion calculation module is configured to fuse the underwater environment data and beaver-like robot movement posture data and construct an underwater environment dynamic model;

[0045] A path planning module is configured to generate a global navigation path by using an improved path planning algorithm based on the underwater environment dynamic model;

[0046] A motion control module is configured to adjust beaver-like robot thrust distribution in real time through a fuzzy proportional-integral-derivative controller.

[0047] Preferably, the path planning module comprises:

[0048] An environment perception and positioning unit is configured to obtain a current position and a target position of the beaver-like robot according to the underwater environment dynamic model;

[0049] A bidirectional path expansion unit is configured to expand path nodes from the current position and the target position synchronously by using a bidirectional search mechanism, and terminate calculation when two search domains intersect, thereby generating a preliminary path;

[0050] A jump point path optimization unit is configured to jump over intermediate nodes without path value in the preliminary path by using a jump point search algorithm, thereby obtaining an optimized path;

[0051] A secondary optimization unit is configured to design a dynamic heuristic function, and adjust a path cost estimate value of the optimized path in combination with a flow velocity vector, thereby obtaining a secondary optimized path;

[0052] The path trajectory smoothing unit smoothes the secondary optimization path by using a curve fitting algorithm, eliminates jagged trajectories in the secondary optimization path, and obtains a smooth and jagged-free global navigation path.

[0053] The application further discloses a computer device comprising a memory and a processor, and the memory stores a computer program.

[0054] The application further discloses a computer readable storage medium, and the computer readable storage medium stores a computer program.

[0055] The application has the beneficial effects that: the application integrates various sensors such as a sonar, a binocular vision camera, a depth sensor and an inertial measurement unit by using a multi-modal sensor fusion technology, realizes comprehensive collection of underwater environment and motion posture data of the beaver-like robot, and improves the accuracy and synchronism of the data by means of data preprocessing and timestamp alignment, effectively solves the problem of incomplete perception of a traditional single sensor, greatly improves the obstacle detection rate, and significantly improves the environment perception accuracy.

[0056] In the path planning aspect, bidirectional search, jump point search, dynamic heuristic function and curve fitting algorithms are adopted, dynamic factors such as water flow are fully considered, a global optimal and smooth navigation path can be quickly generated, compared with a traditional algorithm, the path planning time is greatly shortened, the path length is significantly reduced, the energy consumption is effectively reduced, and the navigation efficiency and safety of the beaver-like robot in a complex underwater environment are improved.

[0057] In the motion control aspect, a fuzzy PID control technology is used, PID control parameters are dynamically adjusted in real time according to the lateral deviation and deviation change rate of the beaver-like robot and the path, the thruster thrust distribution and the motion speed of the beaver-like robot are accurately adjusted, and an anti-saturation mechanism is introduced to ensure the safety of the equipment. This enables the beaver-like robot to quickly and stably track the planned path, the path tracking error is greatly reduced, the navigation accuracy and stability of the beaver-like robot in a complex underwater environment are effectively improved, and the reliability of the system is enhanced. BRIEF DESCRIPTION OF DRAWINGS

[0058] Figure 1 It is a method flowchart of an embodiment of the application.

[0059] Figure 2 It is a system structure schematic diagram of an embodiment of the application.

[0060] 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

[0061] 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.

[0062] like Figure 1 As shown, the present application provides a navigation path control method for a beaver-like robot using multimodal sensor fusion, comprising:

[0063] 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;

[0064] S2, preprocessing the underwater environment data and the beaver-like robot motion posture data, including denoising, correction and zero bias compensation;

[0065] S3, fusing the underwater environment data and the motion posture data of the beaver-like robot to construct an underwater environment dynamic model;

[0066] S4, generating a global navigation path using an improved path planning algorithm based on the underwater environment dynamic model;

[0067] S5, real-time adjustment of the beaver-like robot thrust distribution through the fuzzy proportional-integral-derivative controller.

[0068] 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.

[0069] Because the underwater environment is complex, such as water turbidity affecting camera imaging, water flow movement interfering with sonar signals, a single sensor (such as only using sonar or camera) is easy to miss obstacles or misjudge positions; traditional methods may not see transparent or soft obstacles (such as water grass) using only sonar, and may be "blind" in mud water using only camera, unable to fully perceive the environment, and there are ocean currents and moving obstacles (such as fish) underwater, traditional path planning algorithms are difficult to adjust according to real-time conditions due to pre-set fixed rules, may select the shortest but high-risk or high-energy path, and may cause the beaver robot to "upstream" and consume too much power when following the fixed path in strong water flow, or not to avoid sudden moving obstacles, and when the beaver robot moves underwater, the water resistance and thrust are nonlinearly related (such as different directions of water flow affecting the efficiency of the propeller), and the traditional PID controller has fixed parameters and cannot quickly adapt to changes in attitude.

[0070] The present application forms a complete process of "perception-treatment-decision-control" through the following five steps: first, collecting data in all directions with multiple sensors; then removing data noise and correcting deviation; then fusing data to construct a real-time dynamic underwater environment model; then planning a flexible path according to the environment model; and finally adjusting the propeller thrust in real time based on the path deviation to achieve precise control. Thus, the effect of more comprehensive perception, more intelligent path, and more precise control is achieved, and through the above steps, the beaver robot can autonomously navigate in a complex underwater environment, ensuring the efficiency and reliability of task execution.

[0071] In an embodiment of the present application, the step of collecting underwater environment data and beaver robot motion attitude data in real time through a multi-modal sensor group comprises:

[0072] S11, acquiring distance and contour information of underwater obstacles through a sonar module;

[0073] S12, acquiring underwater image information through a binocular vision camera and generating underwater three-dimensional point cloud data;

[0074] S13, acquiring water pressure information of the water depth where the beaver robot is located through a depth sensor;

[0075] S14, acquiring acceleration information, angular velocity information and attitude angle information of the beaver robot through an inertial measurement unit;

[0076] S15, time stamping alignment of the distance and contour information of the underwater obstacles, the underwater three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information and attitude angle information.

[0077] As described in steps S11-S15 above, the present application acquires underwater environment related data (underwater obstacle distance and profile, underwater three-dimensional point cloud, water pressure) and beaver-like robot motion posture data (acceleration, angular velocity, attitude angle) respectively through a multi-modal sensor group, and timestamps the data for alignment, achieving comprehensive, accurate and synchronized collection of underwater environment and beaver-like robot motion information, providing a reliable data foundation for subsequent environment modeling, path planning and motion control.

[0078] In the underwater environment, the beaver-like robot needs to accurately perceive the surrounding environment and its own motion state to achieve autonomous, safe and efficient motion. The underwater environment is complex and variable, with obstacles, different water pressures, water flows, etc., and the beaver-like robot's own motion also involves attitude changes such as acceleration and turning. Only by comprehensively mastering these information, can we reasonably plan the path and accurately control the motion. For example, if the obstacle distance and profile are not known, the beaver-like robot may collide; without knowing its own attitude angle, it is difficult to accurately adjust the motion direction. Therefore, obtaining these data and synchronizing them is the key prerequisite for solving the reliable operation of the beaver-like robot in the underwater complex environment.

[0079] Traditional underwater data collection often uses a single sensor or simple combination, and lacks effective data synchronization mechanism. A single sensor (such as using sonar only) can only obtain part of the information such as obstacle distance, and cannot take into account the environmental visual features, beaver-like robot attitude, etc. When different sensors collect data asynchronously, it will lead to information disorder, for example, when judging obstacles according to visual images and sonar distance, if the data is not synchronized, it may cause misjudgment of the obstacle position.

[0080] The present scheme uses a multi-modal sensor group, covering sonar, binocular vision camera, depth sensor, inertial measurement unit and other types of sensors, each collecting different dimension data, and through timestamp alignment technology, accurately synchronizing various data, achieving comprehensive and synchronized data collection.

[0081] Specifically, the sonar module obtains the distance and contour information of underwater obstacles. The sonar module emits sound waves to the surroundings, and the sound waves are reflected back after encountering underwater obstacles. The sonar receives the reflected waves. By measuring the time difference between the emitted waves and the reflected waves, combined with the propagation speed of sound waves in water, the distance of the obstacle can be calculated. According to the time and intensity of the reflected waves in different directions, the contour of the obstacle can be further outlined. For example, when encountering a rock, the sound waves emitted by the sonar reflect back after hitting different parts of the rock. After processing, the distance of the rock from the beaver robot and the approximate shape of the rock, whether it is square, circular or irregular, can be obtained. By allowing the beaver robot to perceive the spatial position and shape characteristics of the surrounding obstacles, it is an important basis for the beaver robot to avoid obstacles and plan paths. With this information, the beaver robot can judge in advance whether there is a risk of collision and how to choose the right direction to detour. It can effectively detect obstacles within a certain range, and the distance measurement accuracy can meet the needs of the beaver robot to avoid obstacles, and the contour description can provide basic shape information to assist in judgment.

[0082] The binocular vision camera obtains underwater image information and generates underwater three-dimensional point cloud data. The binocular vision camera has two lenses that capture underwater scenes from different angles to obtain two images. By using the parallax of objects in the two lenses, based on the principle of triangulation, the position of each point on the object in three-dimensional space is calculated, thereby generating three-dimensional point cloud data. For example, by comparing the images of the two lenses, the spatial position of each part of the plant can be determined, and a three-dimensional point cloud model can be constructed. Providing visual information of the underwater environment and three-dimensional spatial distribution of objects can be used to identify obstacle types (such as distinguishing between rocks and corals), detect underwater targets (such as finding specific organisms), etc., to supplement the details and visual feature information that sonar cannot provide. The generated three-dimensional point cloud can present the three-dimensional shape of underwater objects, which helps the beaver robot to better understand the surrounding environment and improve the richness and accuracy of environmental perception.

[0083] The depth sensor obtains the water pressure information of the water depth where the beaver robot is located. The depth sensor usually works based on the principle of water pressure. As the beaver robot changes in depth underwater, the water pressure will change. The sensitive elements inside the depth sensor can sense the change in water pressure and convert it into an electrical signal or other measurable signal, which is processed to calculate the water pressure value of the current water depth. Water depth and water pressure information are crucial for the buoyancy control, depth limitation, etc. of the beaver robot. For example, when the beaver robot needs to maintain a certain depth for stable operation, the water pressure information can be used to determine whether it exceeds the pressure limit of the beaver robot. By accurately measuring the water pressure, the depth control and safe operation of the beaver robot are ensured, and the beaver robot works in the appropriate water pressure environment.

[0084] The acceleration information, angular velocity information and attitude angle information of the beaver-like robot are acquired by an inertial measurement unit (IMU) including an accelerometer, a gyroscope and the like. The accelerometer measures the acceleration of the beaver-like robot by detecting the inertial force received by the internal mass block during accelerated motion. The gyroscope measures the angular velocity of the beaver-like robot during rotation by using the principle of angular momentum conservation. By integrating the acceleration and angular velocity data and combining the initial attitude information, the attitude angle (such as the pitch angle, roll angle and yaw angle) of the beaver-like robot can be calculated. For example, when the beaver-like robot starts to accelerate or turn, the accelerometer and gyroscope can detect these changes in real time and output corresponding data. These information reflect the motion state and attitude change of the beaver-like robot itself, and are the key basis for the motion control and path tracking of the beaver-like robot. For example, when the beaver-like robot turns, the steering angle and speed can be accurately controlled according to the angular velocity and attitude angle information, to ensure that the beaver-like robot travels along the planned path. By providing the motion attitude parameters of the beaver-like robot in real time and accurately, the beaver-like robot can adjust its motion in time, to ensure the stability and accuracy of the motion.

[0085] The distance and profile 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 stamped. When each sensor collects data, a time stamp is marked on each data to record the accurate time of data collection. Then, through a data processing system, the data of different sensors are matched and calibrated according to the time stamp, so that they are consistent in the time dimension. For example, if the sonar detects obstacle information at a certain time, the binocular vision camera also captures related images at a similar time, and the two sets of data are accurately matched through the time stamp. Ensuring that the various types of data collected by different sensors are synchronized in time, so that the subsequent environmental modeling, path planning and other operations based on these data can be based on accurate information at the same time, avoiding information deviation and wrong decisions due to asynchronous data. Precise synchronization of multi-source data provides a reliable foundation for subsequent data fusion and analysis, and improves the accuracy and reliability of the entire system for sensing the underwater environment and the state of the beaver-like robot.

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

[0087] In an embodiment of the present application, the step of preprocessing the underwater environment data and the motion attitude data of the beaver-like robot includes:

[0088] S21, removing noise from the distance and profile information of the underwater obstacle by an adaptive filtering algorithm;

[0089] S22, distortion correction and feature point matching are performed on the underwater image information, and underwater dense three-dimensional point cloud data is generated;

[0090] S23, mean filtering is performed on the water pressure information to eliminate transient fluctuations;

[0091] S24, zero offset compensation is performed on the acceleration information, angular velocity information and attitude angle information;

[0092] S25, a data set is established according to the pre-processed distance and contour information of the underwater obstacle, the underwater image information and underwater dense three-dimensional point cloud data, the water pressure information, and the acceleration information, angular velocity information and attitude angle information.

[0093] As described in steps S21-S25, the present application uses different preprocessing algorithms for underwater environmental data (underwater obstacle distance and contour information, underwater image information, water pressure information) and simulated beaver robot motion attitude data (acceleration, angular velocity, attitude angle information) collected by multi-modal sensors, respectively, to eliminate noise, correct distortion, compensate for deviation, etc. Finally, an accurate, reliable and unified data set is established, providing a high-quality data basis for subsequent environment modeling, path planning and motion control.

[0094] In the underwater environment, the data collected by the sensors will be disturbed by various factors. For example, the sonar signal will be affected by water flow, water impurities, etc., resulting in noise, which will cause the obstacle distance and contour information to be inaccurate; the underwater image will be distorted due to the characteristics of the camera lens, affecting feature recognition; the water pressure will fluctuate due to water flow; the acceleration, angular velocity and attitude angle information of the inertial measurement unit will have zero offset errors. These disturbances will cause the data to be biased and inaccurate, and if not handled, it will cause errors in subsequent environment modeling based on these data, unreasonable path planning, unstable motion control, and affect the normal operation of the simulated beaver robot in the water. Therefore, data preprocessing is a necessary link to ensure the reliable operation of the simulated beaver robot.

[0095] Traditional data preprocessing methods are often single and fixed. For example, the filter for sonar data may use fixed parameters, which cannot dynamically adjust the filtering effect according to the complex and changing underwater environment; the image distortion correction may lack targeted processing for special underwater lighting and medium effects; the zero offset compensation for inertial measurement unit data may not be accurate enough. These traditional methods are difficult to adapt to the complex and dynamic underwater environment, resulting in more errors after data processing.

[0096] The scheme is aimed at the characteristics and interference factors of different types of data, and adopts adaptive filtering, distortion correction, feature point matching, mean filtering, zero offset compensation and other targeted algorithms for fine processing of data, and finally integrates and establishes a data set to improve data quality.

[0097] Specifically, the distance and contour information of the underwater obstacle is de-noised by an adaptive filtering algorithm. The adaptive filtering algorithm can automatically adjust the filtering parameters according to the statistical characteristics (such as signal strength, noise level, etc.) of the input sonar signal. For example, when the underwater current is turbulent, causing the sonar noise to increase, the algorithm will increase the filtering strength to suppress more noise. In a relatively stable underwater environment, the filtering strength is reduced to retain more original signal details. It adjusts the coefficients of the filter by continuously monitoring the error between the signal and the expected output (such as the relatively smooth signal after preliminary processing) to achieve dynamic noise suppression. In the underwater environment, the sonar signal is easily disturbed by water flow, bubbles, suspended particles, etc., producing noise, which will cause the measured obstacle distance and the depicted contour to deviate, affecting the judgment of the beaver robot on the position and shape of the obstacle. De-noising can make the sonar data more accurate, providing a reliable basis for the beaver robot to avoid obstacles and plan the path. Therefore, effectively reducing the noise interference in the sonar data can reduce the measurement error of the obstacle distance and make the contour depiction closer to the true shape.

[0098] The underwater image information is corrected for distortion and feature point matching, and underwater dense three-dimensional point cloud data is generated. By calibrating the underwater camera, its internal parameters (such as focal length, principal point coordinates, etc.) and distortion coefficients are obtained. Using these parameters, a specific mathematical model (such as the radial distortion model, the tangential distortion model) is used to remap the pixel points in the image, restoring the pixel points that have been displaced due to lens distortion to their correct positions, thereby correcting the image distortion. First, feature points are extracted from the corrected underwater image (such as using the Scale-Invariant Feature Transform (SIFT), Speeded-Up Robust Features (SURF), etc.), which have unique local features. Then, between different images (such as the left and right images of a binocular vision camera), the similarity (such as the Euclidean distance, Hamming distance, etc.) between the descriptors of the feature points (such as the 128-dimensional descriptors of SIFT feature points) is calculated, and similar feature points are matched to determine their corresponding positions in different images. Based on the matched feature points, the principle of triangulation is used, combined with the baseline distance of the binocular vision camera and the calibration parameters, to calculate the position of each feature point in three-dimensional space, and then generate three-dimensional point cloud data containing a large number of points, more accurately depicting the three-dimensional shape of underwater objects. Since the underwater camera lens has distortion, it will cause the image to deform, affecting the judgment of the shape and position of the object; and feature point matching is a key step in obtaining three-dimensional information of underwater objects. By correcting distortion and matching feature points to generate dense three-dimensional point cloud data, more accurate underwater environment visual information can be provided for the beaver robot, which can be used to identify obstacle types, detect target objects, etc. Therefore, the corrected image is more consistent with the actual scene, the accuracy of feature point matching is improved, and the generated three-dimensional point cloud can more accurately present the three-dimensional structure of underwater objects. For example, when detecting underwater coral reefs, the shape and range can be more accurately identified, providing more reliable information for the beaver robot to avoid obstacles and explore.

[0099] The water pressure information is subjected to mean filtering to eliminate transient fluctuations. Mean filtering is a process of averaging the collected water pressure data within a certain time window. For example, a time window of 100 milliseconds is set, and multiple water pressure values are continuously collected within this time. Then, the average of these values is calculated, and this average is used as the output water pressure data for this time window. By continuously sliding the time window, the continuous water pressure data is processed, and the water pressure fluctuations caused by transient changes in water flow, etc. are smoothed out. Since the water flow under water, the movement of the beaver robot itself, etc. can cause the water pressure measured by the water pressure sensor to fluctuate transiently, these fluctuations are not real changes in water depth and can interfere with the beaver robot's judgment of its depth and buoyancy state. Eliminating transient fluctuations can make the water pressure data more stable, reflecting the real water depth, and facilitating the beaver robot's depth control and buoyancy adjustment. Therefore, effectively smoothing the water pressure data can reduce misjudgments caused by transient fluctuations.

[0100] The acceleration information, angular velocity information and attitude angle information are zero offset compensated. First, the inertial measurement unit collects data for a period of time in a stationary state, and analyzes the output deviation (i.e. zero offset) of the accelerometer and gyroscope in the data. For the accelerometer, the theoretical output should be the acceleration of gravity when stationary, but there may be a deviation in practice; for the gyroscope, the output should be zero when stationary, but there will also be a zero offset. Then, according to the zero offset values obtained by analysis, the subsequent real-time collected data is compensated and calculated. For example, the acceleration data collected by the accelerometer is subtracted by the corresponding zero offset value, and the angular velocity data collected by the gyroscope is also subtracted by the corresponding zero offset value, so as to obtain more accurate acceleration and angular velocity information, and then more accurate attitude angle information is obtained through integration operation. Since the inertial measurement unit has zero offset error, the measured acceleration, angular velocity and attitude angle information are inaccurate, which affects the judgment of the motion state of the beaver robot. Zero offset compensation can improve the accuracy of these data, so that the beaver robot can more accurately control its motion attitude and ensure stable operation according to the planned path. The accuracy of the inertial measurement unit data can be significantly improved, and the attitude angle error is reduced.

[0101] According to the distance and profile information of the underwater obstacle after preprocessing, the underwater image information and underwater dense three-dimensional point cloud data, the water pressure information and the acceleration information, angular velocity information and attitude angle information, a data set is established. After the above preprocessing step, each type of data is integrated and stored according to certain format and rules. For example, the sonar data after processing, image and three-dimensional point cloud data, water pressure data, inertial measurement unit data after processing at the same time are associated to form a complete data record. Repeat this process to record the data at different times in turn to build a data set covering multi-modal sensor information. After preprocessing, each type of data needs to be integrated uniformly for subsequent training and operation of environment modeling, path planning and motion control algorithms. The establishment of the data set can orderly organize multi-source data, facilitate system calling and analysis, improve data utilization efficiency and algorithm accuracy. A multi-modal data set with clear structure and accurate data can be formed to provide high-quality data support for subsequent intelligent decision-making and control of the beaver robot, such as more accurately constructing an underwater scene model in environment modeling and selecting an optimal path based on more reliable data in path planning.

[0102] Through the preprocessing step, the data quality can be significantly improved, laying a solid data foundation for the reliable operation of the beaver robot in the underwater environment.

[0103] In an embodiment of the present application, the step of fusing the underwater environment data and the beaver robot motion attitude data to construct a dynamic model of the underwater environment comprises:

[0104] S31, spatially register the distance and profile information of the underwater obstacle and the underwater dense three-dimensional point cloud data to obtain an obstacle three-dimensional model in a unified coordinate system;

[0105] S32, based on the obstacle three-dimensional model, divide the underwater environment space using a three-dimensional grid method, mark each grid to store the probability value of the obstacle, and dynamically adjust the probability value with sensor data update to obtain a three-dimensional grid map with probability value;

[0106] S33, combine the acceleration information, angular velocity information and attitude angle information to perform coordinate system alignment on the three-dimensional grid map to eliminate the coordinate offset caused by the motion of the beaver robot to obtain an environment map synchronized with the real-time pose of the beaver robot;

[0107] S34, obtain the moving obstacle in the three-dimensional grid map, and obtain the moving obstacle trajectory prediction information according to the speed and acceleration of the moving obstacle;

[0108] S35, establish an underwater environment dynamic model according to the environment map synchronized with the real-time pose of the beaver robot and the moving obstacle trajectory prediction information.

[0109] As described in steps S31-S35 above, the underwater environment dynamic model reflecting the real-time state of the underwater environment, including obstacle distribution and moving obstacle trajectory prediction, is constructed by fusing the underwater environment data (underwater obstacle distance and profile information, underwater dense three-dimensional point cloud data, etc.) collected and preprocessed by multi-modal sensors and the motion attitude data (acceleration, angular velocity, attitude angle information, etc.) of the beaver robot, which provides accurate environmental information basis for subsequent path planning and motion control of the beaver robot.

[0110] In the underwater environment, the beaver robot needs to accurately understand the surrounding environment to achieve autonomous navigation and safe operation. The underwater environment is complex and dynamically changing, with various shapes and locations of obstacles, as well as moving objects (such as fish and other underwater equipment). Traditional static environment models cannot meet the needs because the beaver robot's position and attitude change when moving will cause environmental perception deviation, and cannot respond to moving obstacles in time. Building a dynamic model can reflect environmental changes in real time, allowing the beaver robot to plan a path in advance to 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 sufficient fusion of multi-source data. For example, a model constructed only with sonar data lacks visual details and cannot accurately identify obstacle types; and it cannot update the model in real time according to the beaver robot's motion, making it difficult to predict the trajectory of moving obstacles. During the motion of the beaver robot, the coordinate offset problem cannot be effectively solved, resulting in an increase in the deviation between the model and the actual environment.

[0111] The scheme adopts multi-source data fusion technology, combines spatial registration, three-dimensional grid division, coordinate system alignment, trajectory prediction and other methods, and constructs a dynamic model that is updated in real time with the environment and the movement of the beaver-like robot. By integrating the advantages of various sensor data, the model parameters are dynamically adjusted to achieve accurate and real-time modeling of the underwater environment.

[0112] Specifically, the distance and contour information of the underwater obstacle and the underwater dense three-dimensional point cloud data are spatially registered to obtain an obstacle three-dimensional model in a unified coordinate system. Spatial registration is to determine the coordinate system parameters of the underwater obstacle distance and contour information obtained by the sonar and the underwater dense three-dimensional point cloud data generated by the binocular vision camera. Then, by finding corresponding feature points (such as the vertices and edge features of the obstacle) in the two sets of data, geometric transformation (such as translation, rotation, scaling) algorithms are used to convert them to the same coordinate system. For example, by calculating the coordinate difference of a certain obstacle feature point in the sonar data and the vision point cloud data, the translation and rotation parameters are determined, and the two sets of data are superimposed to form a complete obstacle three-dimensional model in a unified coordinate system. Since the data collected by the sonar and the binocular vision camera come from different sensors and have different coordinate systems, if spatial registration is not performed, there will be a positional deviation when the data is fused, which will lead to incorrect judgment of the position and shape of the obstacle. The unified coordinate system can integrate the advantages of the two types of data and more accurately present the three-dimensional shape of the obstacle, providing an accurate basis for subsequent environment modeling. Therefore, establishing an accurate obstacle three-dimensional model can greatly reduce the error in the beaver-like robot's judgment of the position and shape of the obstacle.

[0113] Based on the three-dimensional model of the obstacle, the underwater environment space is divided by using the three-dimensional grid method, each grid is marked to store the probability value of the obstacle, and the probability value is dynamically adjusted with the sensor data update to obtain the three-dimensional grid map with probability value, which is to divide the underwater environment space into a plurality of small three-dimensional grids. According to the three-dimensional model of the obstacle, it is judged whether there is an obstacle in each grid or the possibility of existence of the obstacle. If a grid is located inside or close to the obstacle, a higher obstacle probability value is given; if it is far away from the obstacle, a lower probability value is given. With the continuous collection of new data by the sensor, such as the detection of a new obstacle or the change of the position of the obstacle by the sonar, the probability value of the corresponding grid is updated in real time. For example, when the sonar detects a new obstacle entering a certain area, the obstacle probability value of the grid in the area is adjusted. Due to the complexity of the underwater environment and the irregular distribution of the obstacle, the three-dimensional grid method can discretize the environment, which is convenient for computer processing and storage. The marked probability value can reflect the possibility of existence of the obstacle, and the dynamic adjustment can adapt to the environmental change, which provides intuitive environmental information for the path planning of the beaver robot, so that it can avoid the area where the obstacle exists with high probability. The generated three-dimensional grid map with probability value can clearly present the probability of the distribution of the underwater obstacle, and the beaver robot can choose a safer path according to the probability value during path planning, thereby reducing the risk of collision. For example, in the area with more fuzzy obstacles, the map can accurately indicate the high-risk area to guide the beaver robot to bypass.

[0114] In combination with the acceleration information, the angular velocity information and the attitude angle information, the three-dimensional grid map is aligned in the coordinate system to eliminate the coordinate offset caused by the movement of the beaver robot, and an environment map synchronized with the real-time pose of the beaver robot is obtained, which is to use the acceleration, angular velocity and attitude angle information of the beaver robot obtained by the inertial measurement unit to calculate the position and attitude change of the beaver robot in real time. According to these changes, the three-dimensional grid map is subjected to corresponding coordinate transformation. For example, when the beaver robot moves forward by a distance and rotates, the actual movement and rotation amount of the beaver robot are obtained by integrating the acceleration and angular velocity data, and then the three-dimensional grid map is subjected to the same translation and rotation transformation in space, so that the map coordinate system is synchronized with the current pose of the beaver robot. Since the position and attitude of the beaver robot change constantly when it moves underwater, if the coordinate system is not aligned, the three-dimensional grid map based on the fixed coordinate system will deviate from the actual environment, which will lead to the failure of the beaver robot to judge the environment. Through real-time alignment, it can be ensured that the map always accurately reflects the actual environment around the beaver robot, and provides a reliable basis for path planning and motion control. Through the scheme, the coordinate offset caused by the movement of the beaver robot is effectively eliminated, and the matching degree of the environment map and the actual environment is greatly improved.

[0115] Obtaining the moving obstacle in the three-dimensional grid map, and obtaining the moving obstacle trajectory prediction information according to the speed and acceleration of the moving obstacle, is to determine which is the moving obstacle from the three-dimensional grid map by analyzing the change of the obstacle probability value and combining the continuous detection data of the sensor (such as the continuous detection data of the sonar). After determining the moving obstacle, the speed and acceleration information of the moving obstacle obtained by the sensor is used to adopt a kinematic prediction algorithm (such as Kalman filtering or a prediction method based on a physical model). Taking Kalman filtering as an example, the current position is predicted according to the position, speed and acceleration at the last moment, and then the current sensor measurement value is corrected to iteratively predict the future trajectory of the moving obstacle. Since there are many moving obstacles (such as fish and other moving devices) under water, if the beaver robot cannot predict the trajectory in advance, a collision may occur. Obtaining the moving obstacle trajectory prediction information can enable the beaver robot to plan a path in advance, avoid the moving obstacle, and ensure the safety of the beaver robot and the smooth operation. The moving obstacle trajectory can be accurately predicted to provide sufficient reaction time for the beaver robot to avoid obstacles.

[0116] Establishing an underwater environment dynamic model according to the environment map of the real-time pose of the synchronous beaver robot and the moving obstacle trajectory prediction information is to integrate the environment map (containing obstacle distribution probability and the like) of the real-time pose of the synchronous beaver robot and the moving obstacle trajectory prediction information. For example, the position area where the moving obstacle may appear in the future is marked on the map to form a model that comprehensively reflects the static obstacle distribution and the dynamic trajectory of the moving obstacle of the current underwater environment. The model can be updated in real time according to the sensor data to keep synchronization with the actual environment. Since a single environment map or moving obstacle trajectory information cannot comprehensively reflect the dynamic underwater environment, the dynamic model established by integrating the two pieces of information can provide complete and real-time environment information for the beaver robot, so that the beaver robot can fully consider the environmental changes when planning a path and controlling motion, and realize more intelligent and safe underwater operation. The underwater environment dynamic model can accurately present the overall situation of the underwater environment in real time, provide high-quality input for the path planning algorithm of the beaver robot, and significantly improve the success rate of obstacle avoidance of the beaver robot in a complex underwater environment and the operation efficiency.

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

[0118] In an embodiment of the present application, the step of generating a global navigation path based on the underwater environment dynamic model includes:

[0119] S41, obtaining the starting position and target position of the beaver robot according to the underwater environment dynamic model;

[0120] S42, a bidirectional search mechanism is used to expand path nodes from the starting position and the target position simultaneously, and the calculation is terminated when the two search domains intersect, and a preliminary path is generated;

[0121] S43, an intermediate node without path value in the preliminary path is skipped through a jump point search algorithm to obtain an optimized path;

[0122] S44, a dynamic heuristic function is designed to adjust the path cost estimate value of the optimized path in combination with a water flow velocity vector to obtain a second optimized path, and the dynamic heuristic function is:

[0123] ;

[0124] In the formula, the dynamic heuristic function is denoted by, the current node position vector (denoting a path node to be evaluated in path planning) is denoted by, the target position vector (denoting the end point of the current task) is denoted by, the beaver robot velocity vector is denoted by, the water flow velocity vector (obtained through historical hydrological data) is denoted by, the water flow influence coefficient (with a value range of 0.1-0.5, and the greater the value, the more significant the influence of water flow on path cost) is denoted by;

[0125] S45, a curve fitting algorithm is used to smooth the second optimized path to eliminate jagged trajectories in the second optimized path, and a smooth and jagged-free global navigation path is obtained.

[0126] As described in the steps S41-S45, the present application gradually optimizes the path by using the underwater environment dynamic model, bidirectional search, jump point search, dynamic heuristic function, and curve fitting algorithm, to generate a smooth, efficient, and adaptive global navigation path to the underwater environment (such as water flow influence) from the starting point to the target point of the beaver robot, providing planning guidance for the autonomous navigation of the beaver robot underwater.

[0127] In the underwater environment, the beaver robot needs to plan a reasonable path from the starting point to the target position. There are complex factors such as obstacles and water flow underwater, and traditional path planning methods often cannot adapt well to these dynamic changes. For example, if the water flow is not considered, the beaver robot may choose a path with high energy consumption or long time consumption; if the path is not smooth, it will increase the difficulty of motion control and energy consumption of the beaver robot. Therefore, a path planning method that can consider multiple factors is needed to ensure that the beaver robot safely and efficiently reaches the target position.

[0128] Traditional path planning algorithms usually search from the start point to the target point in one direction, which is low in search efficiency and high in time complexity. Moreover, a fixed heuristic function is often used without considering dynamic environmental factors such as water flow, and the generated path may not be optimal. In addition, the generated path may have many zigzag inflection points, which is not conducive to the actual motion control of the beaver robot. The present scheme improves the search efficiency by improving the search mechanism (bidirectional search, jump point search), introduces a dynamic heuristic function to consider the influence of water flow, and uses a curve fitting algorithm to smooth the path, thereby generating a more optimal global navigation path.

[0129] Specifically, the start point position and the target position of the beaver robot are obtained according to the dynamic model of the underwater environment: the dynamic model of the underwater environment records various information of the underwater environment, including the relevant position information of the beaver robot. By reading and analyzing the model data, the start point position where the beaver robot is currently located and the target position coordinates set by the task are directly obtained. Since the start point and the target position are the basis for path planning, only when these two key positions are determined, subsequent path search and planning can be carried out around them. By accurately obtaining the start point and target position information, a clear starting and ending point is provided for the subsequent path planning steps, which is the premise of the entire path planning.

[0130] A bidirectional search mechanism is used to expand path nodes from the start point position and the target position simultaneously, and the calculation is terminated when the two search domains intersect, generating a preliminary path: the bidirectional search mechanism starts searching path nodes from the start point and the target point simultaneously. In the search process, new path nodes are expanded around the start point and the target point as the center according to certain rules (such as distance measurement). This expansion process is repeated until the search areas from the start point and the target point intersect. At this time, connecting the intersection point with the start point and the target point generates a preliminary path. For example, in a simple two-dimensional underwater plane scene, exploring the passable nodes around from the start point A and the target point B, when the search areas of the two meet at node C, A-C-B constitutes a preliminary path. Since traditional one-way search needs to traverse a large number of nodes from the start point to the target point, the search efficiency is low. Bidirectional search is performed from both ends, greatly reducing the search range and the number of node expansions, and can find the path connecting the start point and the target point faster, improving the efficiency of path planning. Compared with traditional one-way search, the search time is greatly shortened, and the preliminary path can be generated in a shorter time, improving the response speed of the beaver robot. For example, in a larger underwater environment, the path search time can be reduced by more than 50%.

[0131] The intermediate nodes without path value in the preliminary path are skipped through a jump point search algorithm to obtain an optimized path: the jump point search algorithm is to find the "jump point" in the path on the basis of the preliminary path. The jump point refers to a key point that can be directly connected without traversing the intermediate nodes in the path search. These jump points are identified through certain rules (such as judgment based on geometric relationship and passability), and then the jump points are directly connected to skip the intermediate nodes without value, thereby simplifying the path. For example, in a preliminary path with multiple turns, some nodes can be directly connected through the algorithm to remove the intermediate redundant turning nodes, so that the path is more concise. Since the preliminary path can contain many unnecessary intermediate nodes, the path length and the calculation complexity are increased. The jump point search algorithm can remove these redundant nodes, optimize the path, and make the river otter robot navigate along a shorter and more efficient path. The path length is effectively shortened, the number of path nodes is reduced, the calculation complexity of path planning is reduced, and at the same time, the navigation distance of the river otter robot is shorter and the energy consumption is saved.

[0132] A dynamic heuristic function is designed to adjust the path cost estimate value of the optimized path in combination with the water flow velocity vector to obtain a second optimized path: through the dynamic heuristic function, the path cost estimate value is adjusted in real time according to the water flow velocity vector. For example, when the water flow velocity is large and opposite to the motion direction of the river otter robot, the path cost calculated by the function is increased, which guides the algorithm to select a more favorable path. The water flow velocity vector is obtained through historical hydrological data. Since there is water flow under water, the path planning algorithm with a traditional fixed heuristic function does not consider the influence of water flow, and the generated path can be high in energy consumption or difficult to pass. The dynamic heuristic function takes the water flow factor into account in the path cost calculation, so that the path planning is more in line with the actual underwater environment, and can guide the river otter robot to select a more energy-saving and efficient path. The generated path can better adapt to the water flow environment, reduce the navigation energy consumption of the river otter robot in the water flow, and improve the navigation efficiency.

[0133] A curve fitting algorithm is used to smooth the second optimized path to eliminate the jagged trajectory in the second optimized path, and a smooth and jagged-free global navigation path is obtained: the curve fitting algorithm is to process the nodes on the second optimized path. Through a mathematical method (such as least squares method, etc.), a smooth curve is fitted according to the coordinate information of the path nodes. For example, for a series of discrete path nodes, a curve that best approximates these nodes is found, so that the curve passes through or approaches these nodes, thereby eliminating the jagged inflection points in the original path. Since the path obtained through the previous steps can have jagged trajectories, this will make the river otter robot frequently adjust the direction when navigating, increase the difficulty of motion control and energy consumption, and also can affect the navigation stability. The smoothed path is more in line with the motion characteristics of the river otter robot, which is convenient for stable and efficient navigation. Since the generated global navigation path is smooth and continuous, the motion control of the river otter robot is simpler, the navigation is more stable, and the energy consumption is further reduced.

[0134] The whole path planning process is carried out around the dynamic model of the underwater environment, starting from obtaining the starting point and target position, through searching, jump point optimization, adjustment combined with the flow factor, and path smoothing, finally generating a global path suitable for the river otter robot to navigate underwater, effectively improving the efficiency, quality and adaptability to the complex underwater environment of path planning.

[0135] In an embodiment of the present application, the step of adjusting the propeller thrust distribution in real time by the fuzzy proportional-integral-derivative controller comprises:

[0136] S51, obtaining the lateral deviation of the river otter robot from the global navigation path, and calculating the lateral deviation change rate;

[0137] S52, defining a fuzzy logic rule, the input variable of which is the lateral deviation and the lateral deviation change rate, and the output variable is the adjustment amount of proportional, integral and derivative gain;

[0138] S53, establishing a fuzzy rule base, and dynamically adjusting the control parameters according to the size of the lateral deviation and the change trend of the lateral deviation change rate;

[0139] S54, using the center of gravity method to solve the fuzziness, and converting the output of the fuzzy logic rule into the actual gain adjustment amount, the formula is:

[0140] ;

[0141] In the formula, represents the actual gain adjustment amount, represents the proportional gain basic value, represents the adjustment amount of proportional, integral and derivative gain (i.e. the output of the fuzzy logic rule);

[0142] S55, based on the actual gain adjustment amount, adjusting the propeller speed and the rear leg movement speed of the river otter robot according to the thrust distribution strategy, and introducing an anti-saturation mechanism to prevent the propeller and the rear leg from running out of limits.

[0143] As described in the steps S51-S55 above, the present application uses a fuzzy proportional-integral-derivative (PID) controller to adjust the propeller thrust distribution in real time based on the lateral deviation and the deviation change rate of the river otter robot from the global navigation path, accurately controls the propeller speed and the rear leg movement speed of the river otter robot, and prevents the equipment from running out of limits, so as to realize high-precision tracking of the river otter robot on the planned path and ensure its stable and safe navigation underwater.

[0144] When navigating underwater, the beaver-like robot will be disturbed by water flow, obstacles and other factors, causing deviation from the preset global navigation path. If the deviation is not corrected in time, the beaver-like robot will not be able to reach the target location as planned, and may even encounter collisions and other dangers. The parameters of the traditional PID controller are fixed and cannot be adjusted flexibly according to the actual deviation in the complex and variable underwater environment. Therefore, there is an urgent need for a method that can dynamically adjust the control parameters in real time according to the deviation and the rate of change of the deviation to ensure the navigation accuracy and stability of the beaver-like robot.

[0145] The proportional, integral and derivative gain parameters of the traditional PID controller remain unchanged once they are set. In the underwater environment, when encountering sudden changes in water flow, obstacles approaching and other conditions, fixed parameters cannot quickly and effectively correct the path deviation of the beaver-like robot, which may cause the beaver-like robot to adjust excessively or take a long time to return to the correct path, seriously affecting the navigation efficiency and safety. The present scheme uses fuzzy logic rules, taking the lateral deviation of the beaver-like robot and the path and the rate of change of the deviation as input quantities to adjust the proportional, integral and derivative gain parameters of the PID controller in real time. By constructing a fuzzy rule base, the parameter adjustment amount is dynamically determined according to the deviation, and then the actual applicable parameter adjustment value is converted through defuzzification operation, and then precise control of the thruster thrust distribution is realized.

[0146] Specifically, the lateral deviation of 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 by using sensors such as inertial measurement units, vision sensors and the like equipped on the beaver-like robot. The collected position information is compared with the pre-planned global navigation path to calculate the lateral distance difference between the current position of the beaver-like robot and the path, and 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 between the lateral deviations at adjacent time points to the time interval. Since the lateral deviation directly reflects the degree of deviation of the beaver-like robot from the predetermined path, and the rate of change of the lateral deviation reflects the trend of the deviation. These two key data are important basis for adjusting the control parameters, and only by accurately obtaining them can the motion state of the beaver-like robot be accurately judged and adjustment decisions be made in time. Real-time and accurate deviation and deviation trend information of the beaver-like robot and the path is obtained, providing reliable input data for subsequent fuzzy logic control,

[0147] Define fuzzy logic rules, the input variable is the lateral deviation and the lateral deviation rate, the output variable is the adjustment amount of proportional, integral, and differential gain: divide the two input variables, lateral deviation and lateral deviation rate, into multiple fuzzy sets according to size range, such as "negative large", "negative medium", "negative small", "zero", "positive small", "positive medium", "positive large", etc. At the same time, the adjustment amount of proportional, integral, and differential gain is also divided into corresponding fuzzy sets, such as "negative large adjustment", "negative medium adjustment", "negative small adjustment", "no adjustment", "positive small adjustment", "positive medium adjustment", "positive large adjustment", etc. Then, a series of fuzzy logic rules are formulated based on experience and a large amount of experimental data. For example, "if the lateral deviation is positive large and the lateral deviation rate is positive small, then the proportional gain adjustment amount is positive medium adjustment". These rules explicitly describe the correspondence between different fuzzy set combinations of input variables and fuzzy sets of output variables. Since the underwater environment is complex and full of variables, the bias of the beaver robot is various. By defining fuzzy logic rules, the precise input data is fuzzified, and appropriate PID gain adjustment strategies are determined according to different bias conditions, so that the controller can more flexibly cope with various actual scenarios. A fuzzy logic decision mechanism for adjusting the path deviation of the underwater beaver robot is constructed, which provides clear rules for real-time adjustment of control parameters and greatly improves the adaptability of the controller to complex environments.

[0148] Establish a fuzzy rule base to dynamically adjust the control parameters according to the size of the lateral deviation and the change trend of the lateral deviation rate: systemically summarize the fuzzy logic rules defined in step S52 to form a complete fuzzy rule base. During actual operation, when the lateral deviation and the deviation rate of the current beaver robot are obtained, the corresponding rules are found in the fuzzy rule base according to the fuzzy sets they belong to, so as to determine the adjustment amount of proportional, integral, and differential gain. For example, if the current lateral deviation belongs to the "positive small" fuzzy set and the lateral deviation rate belongs to the "negative medium" fuzzy set, the proportional gain adjustment amount is determined to be "negative small adjustment" by searching the rule base. Since the fuzzy rule base is the core component of fuzzy logic control, it integrates various possible bias conditions and corresponding control parameter adjustment strategies. By querying the rule base in real time, appropriate control parameter adjustment schemes can be quickly and accurately determined according to the actual bias state of the beaver robot, realizing effective control of the beaver robot motion. The dynamic adjustment of control parameters enables the PID controller to automatically optimize the control strategy according to different bias conditions.

[0149] The output of the fuzzy logic rule is converted into an actual gain adjustment amount by using the barycenter method to resolve the ambiguity: The fuzzy logic rule outputs a fuzzy set of proportional, integral, and derivative gain adjustment amounts, which cannot be directly applied to the PID controller and needs to be converted into an accurate value. The barycenter method is a method for resolving ambiguity, which determines the accurate value by calculating the barycenter position of the fuzzy set. Specifically, for the fuzzy set of each output variable, a weighted average value is calculated according to its membership function (a function for describing the degree to which an element belongs to the fuzzy set) and the corresponding quantization level, and this weighted average value is the actual gain adjustment amount. For example, for the fuzzy set of the proportional gain adjustment amount, the adjustment values of each fuzzy subset are multiplied by their membership degrees, summed, and then divided by the sum of the membership degrees to obtain the final proportional gain actual adjustment amount. Since the fuzzy results output by the fuzzy logic rule cannot be directly used for controller parameter adjustment, the de-fuzzification process converts them into specific and operable values, enabling the controller to accurately adjust the proportional, integral, and derivative gains based on these accurate values, and thus achieving precise control of the thruster thrust distribution. By converting the fuzzy decision results into accurate control parameter adjustment values, the implementability of the control algorithm is ensured, the control accuracy is effectively improved, and the motion control of the beaver robot is more accurate and stable.

[0150] Based on the actual gain adjustment amount, the thruster speed of the beaver robot and the rear leg movement speed are adjusted according to the thrust distribution strategy, and an anti-saturation mechanism is introduced to prevent the thrusters and the rear legs from running out of limits: According to the actual gain adjustment amount obtained in step S54, the required thrust size of each thruster is calculated in combination with the pre-set thrust distribution strategy, and then the thruster speed control signal and the rear leg movement speed control signal are converted. According to the proportional gain adjustment formula, the adjusted actual gain adjustment amount is obtained, which is substituted into the PID control algorithm to calculate the thrust value, and the thrust value is converted into the thruster speed through the motor drive circuit. At the same time, in order to avoid the thrusters and the rear legs running out of limits (such as exceeding the maximum speed or maximum thrust limit) during adjustment due to excessive control signals, an anti-saturation mechanism is introduced. When the control signal reaches the set upper limit value, it is limited to the upper limit value to ensure safe and stable operation of the equipment. Since the thruster speed and the rear leg movement speed are adjusted, the motion direction and speed of the beaver robot can be changed to return to the predetermined global navigation path. The anti-saturation mechanism protects the thrusters and the rear legs and other equipment from being damaged due to excessive driving, ensuring the reliability and safety of the system. The precise control of the thrusters and the rear leg movement of the beaver robot is achieved, enabling the beaver robot to quickly and stably track the global navigation path, while effectively preventing the equipment from being damaged due to overload and prolonging the service life of the equipment.

[0151] In summary, the method based on fuzzy PID control 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.

[0152] As Figure 2 shown, the application also discloses a multi-modal sensor fusion beaver-like robot navigation path control system, comprising:

[0153] A data acquisition module is configured to acquire underwater environment data and beaver-like robot motion attitude data in real time through a multi-modal sensor group.

[0154] A preprocessing module is configured to preprocess the underwater environment data and beaver-like robot motion attitude data.

[0155] A fusion calculation module is configured to fuse the underwater environment data and beaver-like robot motion attitude data, and construct an underwater environment dynamic model.

[0156] A path planning module is configured to generate a global navigation path by using an improved path planning algorithm based on the underwater environment dynamic model.

[0157] A motion control module is configured to adjust the propeller thrust distribution in real time through a fuzzy proportional-integral-derivative controller.

[0158] The path planning module comprises:

[0159] An environment perception and positioning unit is configured to obtain the current position and target position of the beaver-like robot according to the underwater environment dynamic model.

[0160] A bidirectional path expansion unit is configured to expand path nodes from the current position and target position synchronously by using a bidirectional search mechanism, and terminate the calculation when the two search domains intersect, thereby generating a preliminary path.

[0161] A jump point path optimization unit is configured to obtain an optimized path by skipping intermediate nodes with no path value in the preliminary path through a jump point search algorithm.

[0162] A secondary optimization unit is configured to design a dynamic heuristic function, and adjust the path cost estimate value of the optimized path in combination with a flow velocity vector, thereby obtaining a secondary optimized path.

[0163] A path trajectory smoothing unit is configured to perform smoothing processing on the secondary optimized path by using a curve fitting algorithm, eliminate jagged trajectories in the secondary optimized path, and obtain a smooth and jagged-free global navigation path.

[0164] A computer device comprises a memory and a processor, and the memory stores a computer program. When the processor executes the computer program, the steps of a multi-modal sensor fusion beaver-like robot navigation path control method are implemented.

[0165] A computer readable storage medium, having stored thereon a computer program, wherein the computer program is executed by a processor to implement steps of a multi-modal sensor fusion beaver-like robot navigation path control method.

[0166] It should be noted that, in this text, the terms "comprising", "including" or any other variant thereof are intended to cover non-exclusive inclusion, so that processes, devices, articles or methods including a series of elements not only include those elements, but also include other elements not explicitly listed, or include elements inherent to such processes, devices, articles or methods. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, device, article or method including the element.

[0167] The above description is only preferred embodiments of the present application, and does not limit the patent scope of the present application, and any equivalent structure or equivalent process transformation using the content of the present application specification and drawings, or direct or indirect application in other related technical fields, are also included in the patent protection scope of the present application.

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; 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; A dynamic heuristic function is designed, and the path cost estimate of the optimization path is adjusted 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, represents the target position vector, represents the velocity vector of the beaver-like robot, represents the water velocity vector, represents the water flow influence coefficient; A curve fitting algorithm is used to smooth the quadratic optimization path, thereby eliminating jagged tracks in the quadratic optimization path and obtaining a smooth and non-jaggy 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 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.

6. A beaver-like robot navigation path control system using multimodal sensor fusion, applying the beaver-like robot navigation path control method using multimodal sensor fusion according to any one of claims 1 to 5, 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.

7. The multimodal sensor fusion beaver robot navigation path control system according to claim 6, 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.

8. 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 5 are implemented.

9. 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 5 are implemented.

Images