Biomimetic underwater robot dynamic collision avoidance control method and device based on sensor fusion

Abstract

This invention provides a biomimetic underwater robot dynamic collision avoidance control method and device based on sensor fusion. The method includes: realizing the perception of the three-dimensional position, velocity, and type of obstacles through a deep collaborative sensor fusion strategy; calling the optimal strategy from a differentiated obstacle avoidance strategy library according to the obstacle type, realizing an upgrade from geometric space avoidance to intelligent decision-making combined with environmental semantics; employing a hybrid path planning that combines global planning and local obstacle avoidance, a composite motion control strategy that combines upper-level model predictive control and lower-level backstepping, driving the robot with a three-joint collaborative coupling control model, and constructing a disturbance observer for feedforward compensation. This invention can not only accurately perceive the three-dimensional position and velocity of obstacles, but also identify the type of obstacles, and effectively and intelligently execute differentiated obstacle avoidance by combining a hybrid path planning strategy of global optimum and local real-time with a high-precision, robust composite motion control strategy.

Description

Technical Field

[0001] This invention relates to a biomimetic underwater robot dynamic collision avoidance control method and device based on sensor fusion, belonging to the field of navigation, guidance and control technology of underwater autonomous vehicles. Background Technology

[0002] The ocean is the largest and least explored area on Earth, rich in biological, mineral, and energy resources. With the increasing global demand for marine resource development, environmental protection, scientific research, and national security, underwater vehicle technology has developed rapidly. Among various underwater vehicles, biomimetic underwater robots, especially robotic fish that mimic the swimming motion of fish, exhibit outstanding advantages such as high maneuverability, high propulsion efficiency, low energy consumption, low acoustic noise, and minimal environmental disturbance due to their unique propulsion mechanism. They have broad application prospects in underwater exploration, pipeline inspection, aquaculture monitoring, coral reef ecological research, underwater search and rescue, and special operations.

[0003] However, the complexity, unstructured nature, and dynamic variability of underwater environments pose significant challenges to the autonomous navigation of biomimetic underwater robots. Underwater environments typically suffer from poor lighting conditions, severe light attenuation, turbidity, unknown currents and undercurrents, and are riddled with static and dynamic obstacles of varying shapes (such as rocks, coral reefs, shipwrecks, seaweed, schools of fish, and other vehicles). Therefore, endowing biomimetic underwater robots with robust, efficient, and intelligent autonomous dynamic collision avoidance capabilities is the core key and technological bottleneck for their transition from the laboratory to practical applications and the completion of complex underwater tasks.

[0004] Currently, existing technical solutions for solving the obstacle avoidance problem of underwater robots can be mainly categorized into the following two types:

[0005] The first category is obstacle avoidance technology based on a single sensor. This type of technology typically relies on a single type of sensor to acquire environmental information. For example, some solutions use only sonar sensors (such as single-beam or multi-beam ultrasonic radar) for obstacle detection. The advantage of sonar is that its propagation is largely unaffected by water turbidity and lighting conditions, providing relatively reliable obstacle distance information. However, its inherent limitations are also quite apparent: acoustic sensors generally have low spatial resolution, making it difficult to accurately depict the geometric contours and surface details of obstacles; sound waves are susceptible to multipath effects, interface reflections, and acoustic noise when propagating underwater, potentially leading to false targets or lost targets; more importantly, sonar echo signals essentially only reflect the acoustic impedance characteristics of objects, providing almost no rich semantic information about the obstacle's material or type (whether it's hard rock or soft seaweed).

[0006] Another approach utilizes only visual sensors. Visual sensing provides high-resolution image information, containing rich color, texture, and shape details, which is crucial for obstacle recognition and classification. However, the application of vision in underwater environments is severely limited: First, light attenuation and scattering in water are far more severe than in air, resulting in underwater images generally exhibiting low contrast, color distortion, blurred details, and visual noise such as "ocean snow." Second, water turbidity directly affects the effective line-of-sight; in turbid waters, the effective range of visual sensors may drastically decrease to a few meters or even less. Third, vision-based ranging algorithms are highly sensitive to lighting conditions, texture features, and calibration accuracy, making it difficult to guarantee the stability and accuracy of ranging in underwater environments. In summary, relying on a single sensor is like "the blind men and the elephant"—the limited scope of its perception prevents it from comprehensively and accurately constructing a model of the surrounding environment, thus severely restricting the intelligence and reliability of the robot's obstacle avoidance decisions.

[0007] The second type is the traditional hierarchical, modular obstacle avoidance framework. This framework follows the classic sequential pattern of "perception-planning-control" in system design, with each module having independent functions and clear interfaces. At the path planning layer, a strategy combining global path planning and local real-time obstacle avoidance is typically employed. The global planner, such as the classic A-Star algorithm, D algorithm, or the Rapid Expanding Random Tree (RRT) algorithm, is responsible for planning a global path from the starting point to the destination that satisfies a certain optimality criterion on a known prior map. The local obstacle avoider, such as the artificial potential field method or the dynamic window method, is responsible for dynamically and locally correcting the trajectory based on unknown obstacles detected in real time by the sensors while the robot is navigating along the global path.

[0008] However, this traditional framework has the following technical problems when dealing with complex and dynamic underwater environments:

[0009] 1) The disconnect between planning and the environment leads to poor flexibility: Global planning algorithms heavily rely on the accuracy of prior maps, but obtaining accurate global maps is often impractical in the vast and ever-changing ocean. When a large number of unknown or dynamic obstacles appear in the environment, the global planner needs to frequently perform global replanning, which is computationally expensive for underwater robots with limited computing resources, making it difficult to guarantee real-time decision-making.

[0010] 2) Inherent defects of local obstacle avoidance algorithms: Although the artificial potential field method is computationally simple and reacts quickly, the gravitational and repulsive fields it designs are prone to generating local minima in complex obstacle layouts (such as narrow passages and U-shaped traps), causing the robot to get stuck and stagnate. The dynamic window method requires sampling and evaluation in the velocity space. Although it considers the robot's dynamic constraints, its available effective window may be very small or even disappear when facing suddenly appearing obstacles or when large-scale maneuvers are required, leading to obstacle avoidance failure.

[0011] 3) Decoupling between planning and control leads to poor execution: Upper-level planning modules typically plan paths based on simplified kinematic models (such as point mass models), ignoring the unique, complex nonlinear dynamics of biomimetic underwater robots caused by multi-joint coupled fluctuations. This results in planned paths (such as sharp corners) that the robot may not be able to track precisely in physics. The lower-level controller passively receives instructions from the upper level, lacking effective information feedback and collaborative optimization. This disconnect between planning and control not only generates significant trajectory tracking errors but may also lead to motion instability and even system malfunction.

[0012] 4) Lack of understanding and response to environmental semantics: Traditional methods treat all obstacles the same as geometric occupants, lacking an understanding of obstacle properties. For example, it cannot distinguish between a rigid reef that needs to be strictly avoided and a flexible clump of aquatic plants that can be carefully traversed. This "one-size-fits-all" obstacle avoidance approach greatly limits the robot's intelligence level and environmental adaptability, reducing task execution efficiency and path selection flexibility.

[0013] 5) Insufficient consideration of system robustness and energy efficiency: Most existing technologies have weak predictive capabilities for the motion intentions of dynamic obstacles and lack a mature and reliable emergency response mechanism to handle unexpected situations such as obstacle avoidance failure. Furthermore, path planning and motion control often prioritize shortest path or optimal time, while research on energy consumption models for biomimetic robots is insufficient, failing to consider energy efficiency as a key optimization indicator. This severely restricts the endurance and operating radius of underwater robots that rely on limited battery power. Summary of the Invention

[0014] To overcome the aforementioned deficiencies of existing technologies, this invention provides a biomimetic underwater robot dynamic collision avoidance control method, device, system, computer equipment, and storage medium based on sensor fusion. Targeting complex underwater environments, it acquires data on the robot itself and the environment through a deep collaborative sensor fusion strategy. This not only accurately perceives the three-dimensional position and velocity of obstacles but also identifies the types of obstacles. Furthermore, by combining a hybrid path planning strategy of global optimization and local real-time analysis with a high-precision, robust composite motion control strategy, it effectively and intelligently executes differentiated obstacle avoidance, significantly improving the robot's obstacle avoidance efficiency, robustness, and endurance in unknown dynamic environments.

[0015] The first objective of this invention is to provide a dynamic collision avoidance control method for biomimetic underwater robots based on sensor fusion.

[0016] The second objective of this invention is to provide a biomimetic underwater robot dynamic collision avoidance control device based on sensor fusion.

[0017] The third objective of this invention is to provide a biomimetic underwater robot dynamic collision avoidance control system based on sensor fusion.

[0018] The fourth object of the present invention is to provide a computer device.

[0019] The fifth object of the present invention is to provide a storage medium.

[0020] The first objective of this invention can be achieved by adopting the following technical solution:

[0021] A dynamic collision avoidance control method for a biomimetic underwater robot based on sensor fusion, the method comprising:

[0022] Through a deep collaborative sensor fusion strategy, the three-dimensional position, velocity, and type of obstacles can be perceived;

[0023] Based on the type of obstacle, the optimal strategy is called from the differentiated obstacle avoidance strategy library to upgrade from geometric space avoidance to intelligent decision-making that combines environmental semantics;

[0024] Based on the perception results of the three-dimensional position, velocity and type of obstacles, a hybrid path planning that combines global planning and local obstacle avoidance is adopted. A composite motion control strategy that combines upper-level model predictive control and lower-level backstepping is adopted. A three-joint cooperative coupling control model is used to drive the robot, and a disturbance observer is constructed for feedforward compensation.

[0025] Furthermore, the deep collaborative sensor fusion strategy enables the perception of the three-dimensional position, velocity, and type of obstacles, including:

[0026] Visual images are continuously acquired through a visual sensor, and color correction and enhancement preprocessing are performed on the acquired visual images.

[0027] The preprocessed visual image frames are fed into a lightweight deep learning object detection network to identify obstacles and output the category and visual detection box of the obstacle;

[0028] The distance and angle information of obstacles are obtained through ultrasonic sensors;

[0029] The acceleration and angular velocity of the robot body are obtained through an inertial measurement unit;

[0030] Time synchronization and coordinate system integration of data from visual and ultrasonic sensors;

[0031] Calculate the Euclidean distance between the projection point and the center of the visual detection box. If the Euclidean distance is less than the set threshold, it is determined that the two correspond to the same obstacle, and the data association is successful.

[0032] For obstacles to successful data association, extract multidimensional classification features;

[0033] The classification model is used to analyze the multidimensional classification features extracted in real time and output the type of obstacle.

[0034] Using extended Kalman filtering to fuse correlated data from visual sensors, ultrasonic sensors, and inertial measurement units, state estimation is performed to output the three-dimensional position and velocity vectors of the obstacle.

[0035] Furthermore, the time synchronization and coordinate system unification of the data from the visual sensor and the ultrasonic sensor includes:

[0036] The ultrasound data was adjusted using linear interpolation to match the visual time.

[0037] The three-dimensional points converted from ultrasonic waves are projected onto the camera image plane to generate theoretical pixel coordinates, i.e., projection point coordinates.

[0038] Furthermore, the step of calling the optimal strategy from the differentiated obstacle avoidance strategy library based on the obstacle type includes:

[0039] When an obstacle is determined to be a regular-shaped obstacle, a strict avoidance strategy is invoked and executed, and a smooth path is planned while strictly adhering to the preset safety distance.

[0040] When an obstacle is determined to be a discrete obstacle group, a path planning method combining the kinematic constraints of the robotic fish is invoked and executed to bypass it.

[0041] When an obstacle is determined to be a soft, non-rigid obstacle, a cautious crossing strategy or an increased height crossing strategy is implemented based on the obstacle density assessment results.

[0042] When the obstacle is a linear obstacle, the parallel navigation strategy is invoked, and a parallel motion path is generated that maintains a specific angle or distance from the obstacle, in conjunction with the safe distance information provided by the ultrasonic sensor.

[0043] Furthermore, the hybrid path planning that combines global planning and local obstacle avoidance includes:

[0044] Global path planning is performed using the D*Lite incremental replanning algorithm;

[0045] When navigating along the global path, the algorithm based on vector field histogram is run at high frequency. Based on the real-time fused perception data, it performs real-time obstacle avoidance between global path points and generates a safe local heading.

[0046] When local obstacle avoidance fails, the first level of the three-level response mechanism is triggered, and adaptive parameter adjustment is performed.

[0047] When the first-level response is invalid, the second-level response is triggered, and multi-directional short-distance probing motion actions are executed;

[0048] When consecutive trial movements fail, a third-level response is triggered, executing global replanning and area avoidance actions.

[0049] Furthermore, the composite motion control strategy combining upper-level model predictive control and lower-level backstepping includes:

[0050] A model predictive controller is used at the upper layer to optimize a set of optimal cooperative waveform parameters based on the reference trajectory and robot state.

[0051] The lower layer employs a backstepping controller, which receives instructions from the model prediction controller, calculates the desired angle of each joint motor, and performs high-precision tracking.

[0052] The second objective of this invention can be achieved by adopting the following technical solution:

[0053] A biomimetic underwater robot dynamic collision avoidance control device based on sensor fusion, the device comprising:

[0054] The fusion unit is used to perceive the three-dimensional position, velocity, and type of obstacles through a deep collaborative sensor fusion strategy.

[0055] The invocation unit is used to invoke the optimal strategy from the differentiated obstacle avoidance strategy library according to the obstacle type, so as to upgrade from geometric space avoidance to intelligent decision-making combined with environmental semantics;

[0056] The planning and control unit is used to perform hybrid path planning that combines global planning and local obstacle avoidance based on the perception results of the three-dimensional position, velocity and type of obstacles. It adopts a composite motion control strategy that combines upper-level model predictive control and lower-level backstepping method, drives the robot with a three-joint cooperative coupling control model, and constructs a disturbance observer for feedforward compensation.

[0057] The third objective of this invention can be achieved by adopting the following technical solution:

[0058] A biomimetic underwater robot dynamic collision avoidance control system based on sensor fusion is disclosed to implement the aforementioned biomimetic underwater robot dynamic collision avoidance control method. The system includes a robot body, control components, a power module, an inertial measurement unit, a vision sensor, and an ultrasonic sensor array. The robot body includes a biomimetic shell. The control components, power module, and inertial measurement unit are disposed inside the biomimetic shell and are centrally arranged at the robot's center of gravity to maintain attitude stability. The vision sensor and ultrasonic sensor array are disposed at the front of the biomimetic shell, and the vision sensor and ultrasonic sensor array are installed at a 30° intersection angle.

[0059] The fourth objective of this invention can be achieved by adopting the following technical solution:

[0060] A computer device includes a processor and a memory for storing processor-executable programs, wherein when the processor executes the program stored in the memory, it implements the above-described biomimetic underwater robot dynamic collision avoidance control method.

[0061] The fifth objective of this invention can be achieved by adopting the following technical solution:

[0062] A storage medium storing a program, which, when executed by a processor, implements the aforementioned biomimetic underwater robot dynamic collision avoidance control method.

[0063] The present invention has the following advantages over the prior art:

[0064] 1. This invention, through the deep fusion and synergy of vision and ultrasound, can not only achieve high-precision three-dimensional positioning of underwater obstacles, but also identify their semantic attributes such as type and shape, providing a solid foundation for subsequent intelligent decision-making and differentiated obstacle avoidance, and overcoming the shortcomings of insufficient perception capability of a single sensor in the existing technology.

[0065] 2. This invention employs a hybrid planning strategy combining global planning (D* Lite) and local obstacle avoidance (VFH), balancing global path optimization with real-time response to dynamic environments. Its unique three-level response mechanism intelligently handles unexpected situations such as local obstacle avoidance failures, avoiding the pitfalls of traditional methods that easily get stuck in local optima or blindly retry, significantly improving the robot's robustness and success rate in complex and unknown environments.

[0066] 3. This invention adopts a composite motion control strategy that combines upper-level model predictive control (MPC) with lower-level backstepping, and constructs a three-joint cooperative coupling model, which fully considers the dynamic characteristics of the biomimetic robot. At the same time, it introduces a disturbance observer based on real-time feedback and feedforward compensation, which can effectively resist external disturbances such as water flow and achieve high-precision trajectory tracking capability and attitude stability.

[0067] 4. This invention deeply couples and integrates the modules of perception, fusion, decision-making, planning and control to form a complete, adaptive autonomous collision avoidance system. Its overall performance is superior to the traditional solution that simply splices together the independent modules, and the system integration is higher. Attached Figure Description

[0068] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0069] Figure 1 This is a schematic diagram of the overall functional architecture of the biomimetic underwater robot dynamic collision avoidance system of Embodiment 1 of the present invention.

[0070] Figure 2 This is a schematic diagram of the overall hardware structure of the biomimetic underwater robot dynamic collision avoidance system of Embodiment 1 of the present invention.

[0071] Figure 3 This is a flowchart of the biomimetic underwater robot dynamic collision avoidance method of Embodiment 1 of the present invention.

[0072] Figure 4 This is a detailed flowchart of the multimodal sensor data fusion and state estimation used in Embodiment 1 of the present invention.

[0073] Figure 5 This is a schematic diagram of the functional modules of the lightweight deep learning network used for underwater obstacle recognition in Embodiment 1 of the present invention.

[0074] Figure 6 This is a schematic diagram of the relevant coordinate system established to describe the pose of the biomimetic underwater robot and its sensors in Embodiment 1 of the present invention.

[0075] Figures 7-9 This is a top-level logic diagram of the obstacle avoidance strategy based on the semantic type of the obstacle in Embodiment 1 of the present invention.

[0076] Figure 10 This is a flowchart of the hybrid dynamic obstacle avoidance path planning used in Embodiment 1 of the present invention.

[0077] Figure 11 This is a schematic diagram of the three-joint wave propulsion structure of the biomimetic underwater robot, which serves as the actuator in Embodiment 1 of the present invention.

[0078] Figure 12 This is a schematic diagram of the three-motor differential cooperative coupling control principle constructed to achieve efficient biomimetic motion in Embodiment 1 of the present invention.

[0079] Figure 13 This is a detailed flowchart of the motion control execution layer in Embodiment 1 of the present invention.

[0080] Figure 14This is a flowchart of the anti-interference and compensation algorithm designed to resist external water flow disturbance in Embodiment 1 of the present invention.

[0081] Figure 15 This is a structural block diagram of the biomimetic underwater robot dynamic collision avoidance control device according to Embodiment 2 of the present invention.

[0082] Figure 16 This is a structural block diagram of the computer device according to Embodiment 3 of the present invention. Detailed Implementation

[0083] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0084] Example 1:

[0085] This embodiment provides a biomimetic underwater robot dynamic collision avoidance control method based on sensor fusion. This method is implemented through a biomimetic underwater robot dynamic collision avoidance control system, such as... Figure 1 As shown, the system is logically divided into four levels: perception, fusion, decision-making and planning, and motion control.

[0086] like Figure 2 As shown, the overall hardware structure of the biomimetic underwater robot dynamic collision avoidance control system in this embodiment includes a robot body, control components, a power module, an inertial measurement unit (IMU), a vision sensor, and an ultrasonic sensor array. The robot body includes a biomimetic shell. The control components, power module, and inertial measurement unit are housed inside the biomimetic shell and are centrally located at the robot's center of gravity to maintain posture stability. A modular design is adopted, with all cables connected via waterproof interfaces to improve system maintainability and robustness. The vision sensor and ultrasonic sensor array are located at the front of the biomimetic shell. Specifically, the vision sensor is an underwater camera housed within a transparent cover at the front of the shell, while the ultrasonic sensor array is distributed on both sides to reduce the impact of water flow disturbance and ensure the stability of data acquisition. The vision sensor and ultrasonic sensor array are installed at a 30° angle. This design eliminates overlapping blind spots and fuses their fields of view into a wider (up to 150°) effective sensing area, ensuring accurate fusion.

[0087] like Figure 3 As shown, the biomimetic underwater robot dynamic collision avoidance control method of this embodiment includes the following steps:

[0088] S301. Through a deep collaborative sensor fusion strategy, the three-dimensional position, speed and type of obstacles can be perceived.

[0089] like Figure 4 As shown, step S301 specifically includes:

[0090] S3011: Continuously acquire visual images through a visual sensor, and perform color correction and enhancement preprocessing on the acquired visual images to cope with the low contrast underwater environment.

[0091] S3012. The preprocessed visual image frames are fed into the lightweight deep learning object detection network YOLOv11, which is as follows: Figure 5 As shown, it integrates attention mechanism and multi-scale feature fusion technology to identify obstacles and output the category of the obstacle and visual detection box, which is a pixel-level bounding box.

[0092] S3013. Obtain the distance r and angle θ of the obstacle through an ultrasonic sensor.

[0093] S3014. Obtain the acceleration of the robot body through the inertial measurement unit. and angular velocity .

[0094] S3015. Time synchronization and coordinate system integration of data from the vision sensor and the ultrasonic sensor.

[0095] In order to achieve accurate fusion, this embodiment performs time synchronization and coordinate system unification of the data from the visual sensor and the ultrasonic sensor through hardware triggering or high-precision software timestamps, thereby solving the fusion error caused by the spatiotemporal asynchrony of the different sensor sources.

[0096] Furthermore, the ultrasound data were adjusted using linear interpolation to align with visual time. Matching, ultrasound coordinates The linear interpolation adjustment formula is as follows: The three-dimensional points converted from ultrasonic waves are then projected onto the camera image plane to generate theoretical pixel coordinates. That is, the coordinates of the projection point, such as Figure 6 As shown.

[0097] S3016, Calculate the projection point Center of the visual inspection box The Euclidean distance d, the Euclidean distance formula is: If the Euclidean distance d is less than the set threshold δ, then the two are determined to correspond to the same obstacle, and the data association is successful.

[0098] S3017. Extract multidimensional classification features for obstacles to successful data association.

[0099] In this embodiment, the multidimensional classification features include: shape factor and contour continuity based on visual contour; surface properties based on visual texture and ultrasonic echo intensity; and distribution density based on ultrasonic point cloud space.

[0100] S3018. Use a classification model to analyze the multidimensional classification features extracted in real time, output the type of obstacle, and realize the upgrade from "perception" to "cognition".

[0101] S3019. Using extended Kalman filtering (EKF) to fuse the correlated data from the visual sensor, ultrasonic sensor, and inertial measurement unit, state estimation is performed to output the three-dimensional position and velocity vector of the obstacle.

[0102] In this embodiment, a state vector is maintained for each tracked obstacle. , representing its three-dimensional position and velocity.

[0103] Furthermore, the prediction step of the extended Kalman filter is based on the acceleration provided by the inertial measurement unit. State estimation at the previous moment The prior state estimate at the current moment is calculated using a constant acceleration model. Among them, the next moment position The prediction formula is The speed of the next moment The prediction formula is Simultaneously, the covariance matrix is ​​updated to reflect the uncertainty in the prediction: ,in It is the Jacobian matrix of the state transition model. It is the process noise covariance.

[0104] Furthermore, the update step of the extended Kalman filter uses fused observation data. Correcting the prior estimate. Specifically: First, calculate the observation residual, which is the difference between the actual observed value and the predicted observed value. Secondly, the Kalman gain is calculated based on the residual and covariance. ,in It is the Jacobian matrix of the observation model. It is the observation noise covariance; finally, the state estimate is updated using Kalman gain. Covariance This allows for more accurate state output.

[0105] S302. Based on the obstacle type, the optimal strategy is called from the differentiated obstacle avoidance strategy library to upgrade from geometric space avoidance to intelligent decision-making that combines environmental semantics.

[0106] In this embodiment, the differentiated obstacle avoidance strategy library is a strategy template library pre-built for the semantic categories of common underwater obstacles. This differentiated obstacle avoidance strategy library is formed based on (i) the kinematic / dynamic constraints and safety distance requirements of the biomimetic underwater robot, (ii) the classification of obstacle categories in typical underwater scenarios (regularly shaped obstacles, discrete obstacle groups, soft non-rigid obstacles, linear obstacles, etc.), and (iii) the offline calibration of safety thresholds, cost weights, and maneuver parameters using simulation or experimental data. During operation, based on the obstacle type and status information output in step S301, the strategy that satisfies the constraints and has the lowest cost is retrieved from the differentiated obstacle avoidance strategy library and executed. At the same time, the differentiated obstacle avoidance strategy library can be extended or updated in a parameterized manner to adapt to different aquatic environments and task requirements.

[0107] like Figures 7-9 As shown, the specific explanation of the strategy invoked based on the obstacle type is as follows:

[0108] 1) When an obstacle is determined to be a regular-shaped obstacle (such as a reef or wall), a strict avoidance strategy is invoked and executed. Under the premise of strictly adhering to the preset safe distance, a smooth path is planned, specifically, navigation along the wall.

[0109] 2) When an obstacle is determined to be a discrete obstacle group (such as a group of reefs), a path planning method combining the kinematic constraints of the robotic fish is invoked and executed to bypass it.

[0110] 3) When an obstacle is determined to be a soft, non-rigid obstacle (such as aquatic plants), a cautious crossing strategy or an increased height crossing strategy shall be implemented based on the obstacle density assessment results.

[0111] 4) When the obstacle is a linear obstacle (such as a pipe or fishing net), the parallel navigation strategy is invoked, and the safe distance information provided by the ultrasonic sensor is combined to generate a parallel motion path that maintains a specific angle or distance from the obstacle.

[0112] S303. Based on the perception results of the three-dimensional position, velocity and type of obstacles, a hybrid path planning that combines global planning and local obstacle avoidance is adopted. A composite motion control strategy that combines upper-level model predictive control and lower-level backstepping is adopted. A three-joint cooperative coupling control model is used to drive the robot, and a disturbance observer is constructed for feedforward compensation.

[0113] This embodiment uses the fusion perception results of step S301 (i.e., the perception results of the three-dimensional position, velocity, and type of the obstacle) as the key input. Among them, the obstacle's three-dimensional position, velocity vector, and semantic type output by step S301 through data association, classification, and EKF state estimation are used for: (i) updating the grid / cost map of global planning D*Lite and assigning "obstacle type cost"; (ii) constructing the local polar coordinate histogram and generating the safe heading based on the vector field histogram (VFH) for local obstacle avoidance; (iii) real-time correction of the feasible region and safety constraints of the reference trajectory by the upper-level model predictive controller in rolling optimization. At the same time, the robot body state-related observations output by step S301 (such as acceleration and angular velocity measured by the inertial measurement unit) are used as the state feedback input of the control layer to participate in the state update of the model predictive controller and the estimation of the disturbance observer, thereby realizing the closed-loop coupling of "perception-planning-control".

[0114] like Figure 10 As shown, a hybrid path planning approach combining global planning and local obstacle avoidance is employed, including:

[0115] 1) Global path planning is performed using the D*Lite incremental replanning algorithm.

[0116] In this embodiment, the D* Lite incremental replanning algorithm is used in the comprehensive cost function. Searching on the above, the At least include path length Safety , smoothness Exercise energy consumption and obstacle type cost Among them, the obstacle type cost is assigned different values ​​according to the type of obstacle (such as hard, soft, dynamic, linear, etc.), so that the planned path can actively avoid high-risk obstacles.

[0117] 2) When navigating along the global path, the algorithm based on the vector field histogram is run at high frequency. Based on the real-time fused perception data, obstacle avoidance is performed between global path points in real time, and a safe local heading is generated.

[0118] 3) When local obstacle avoidance fails, the first level of the three-level response mechanism is triggered, and adaptive parameter adjustment actions are performed.

[0119] The adaptive parameter adjustment in this embodiment specifically involves: increasing the obstacle expansion radius, adjusting the heading cost function weight, and narrowing the velocity search space to generate a local path in a more conservative manner.

[0120] 4) When the first-level response is invalid, the second-level response is triggered to perform multi-directional short-distance probing motions.

[0121] The multi-directional short-distance probing in this embodiment specifically involves controlling the robot to perform probing movements of 1-2 fuselage lengths in predefined candidate directions (including the current heading, a 30° left turn, a 30° right turn, a 10° ascent, and a 10° descent).

[0122] 5) When consecutive trial movements fail, a third-level response is triggered, executing global replanning and area avoidance actions.

[0123] In this embodiment, if the number of consecutive failures is greater than or equal to 3, global replanning and area avoidance are performed. Specifically, an area with a radius of 2m centered on the robot's current position is marked as a "temporary obstacle" on the map, and the D* Lite global replanning algorithm is immediately triggered. A high-cost term for the temporary obstacle is added to the cost function.

[0124] like Figure 11 and Figure 12 As shown, a three-joint cooperative coupling control model based on biomimetic principles is used to drive the robot. The core of this model is that the three propulsion joints oscillate at the same frequency, have a fixed phase delay, and the swing amplitude is controlled by the same set of cooperative parameters to mimic the efficient wave propulsion of fish. Among them, the motor power of the propulsion joints is differentiated and the power distribution ratio is 4:3:3. The first joint (main yaw joint) is allocated 40% of the power and is responsible for starting action and main steering; the second joint (auxiliary steering joint) is allocated 30% of the power to enhance wave transmission; and the third joint (tail fin main propulsion joint) is allocated 30% of the power to provide the main thrust and fine adjustment.

[0125] like Figure 13 As shown, a composite motion control strategy combining upper-level model predictive control and lower-level backstepping is adopted, including:

[0126] 1) A model predictive controller (MPC) is used at the upper layer to continuously optimize a set of optimal cooperative waveform parameters based on the reference trajectory and robot state.

[0127] In this embodiment, the path instructions planned by the upper layer are input into the model prediction controller, which performs optimization calculations within a rolling time window and outputs cooperative waveform parameters, including the tail fin oscillation frequency, amplitude, and phase difference.

[0128] 2) A backstepping controller is used in the lower layer to receive instructions from the model prediction controller, calculate the desired angle of each joint motor and perform high-precision tracking.

[0129] In this embodiment, the waveform parameter command output by the model prediction controller is input into the underlying backstepping controller to calculate the desired angle of the three joint motors in each control cycle, and drive the motors to perform high-precision tracking to achieve biomimetic wave propulsion.

[0130] like Figure 14 As shown, a disturbance observer is constructed. This observer estimates the external disturbance value d, such as water flow, in real time by reading the actual joint rotation angle fed back by the encoder and the body acceleration / angular velocity measured by the inertial measurement unit. The disturbance estimate d is then added to the control command through feedforward compensation. The control command formula is as follows: ,in It is the basic control variable based on the backstepping method. It is a disturbance compensation gain, used to dynamically adjust the motor output.

[0131] When a collision risk is determined to be extremely high, a reactive safety strategy is triggered, immediately calling the highest priority reaction strategy from the predefined maneuver command library, such as emergency stop maneuver or emergency left turn maneuver.

[0132] When an emergency stop maneuver command is invoked, the motion controller generates a braking waveform that is in the opposite frequency to the forward wave and maximizes the tail fin amplitude to increase fluid resistance, thereby achieving the fastest deceleration effect.

[0133] It should be noted that although the method operations of the above embodiments are described in a specific order, this does not require or imply that these operations must be performed in that specific order, or that all the operations shown must be performed to achieve the desired result. On the contrary, the described steps may be performed in a different order. Additionally or alternatively, certain steps may be omitted, multiple steps may be combined into one step, and / or one step may be broken down into multiple steps.

[0134] Example 2:

[0135] like Figure 15 As shown, this embodiment provides a biomimetic underwater robot dynamic collision avoidance control device based on sensor fusion. The device includes a fusion unit 1501, a calling unit 1502, and a planning and control unit 1503. The specific functions of each unit are as follows:

[0136] The fusion unit 1501 is used to perceive the three-dimensional position, velocity and type of obstacles through a deep collaborative sensor fusion strategy.

[0137] Calling unit 1502 is used to call the optimal strategy from the differentiated obstacle avoidance strategy library according to the obstacle type, so as to upgrade from geometric space avoidance to intelligent decision-making combined with environmental semantics;

[0138] The planning and control unit 1503 is used for hybrid path planning that combines global planning and local obstacle avoidance, a composite motion control strategy that combines upper-level model predictive control and lower-level backstepping, a three-joint cooperative coupling control model to drive the robot, and a disturbance observer to perform feedforward compensation.

[0139] It should be noted that the device provided in this embodiment is only an example of the above-described division of functional modules. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure can be divided into different functional modules to complete all or part of the functions described above.

[0140] Example 3:

[0141] like Figure 16 As shown, this embodiment provides a computer device, which includes a processor 1602, a memory, an input device 1603, a display device 1604, and a network interface 1605 connected via a system bus 1601. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium 1606 and an internal memory 1607. The non-volatile storage medium 1606 stores an operating system, computer programs, and a database. The internal memory 1607 provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. When the processor 1602 executes the computer programs stored in the memory, it implements the biomimetic underwater robot dynamic collision avoidance control method of Embodiment 1 described above, as follows:

[0142] Through a deep collaborative sensor fusion strategy, the robot can perceive the three-dimensional position, velocity, and type of obstacles. Based on the obstacle type, the optimal strategy is called from a differentiated obstacle avoidance strategy library to upgrade from geometric space avoidance to intelligent decision-making that combines environmental semantics. Based on the perception results of the three-dimensional position, velocity, and type of obstacles, a hybrid path planning that combines global planning and local obstacle avoidance is adopted. A composite motion control strategy that combines upper-level model predictive control and lower-level backstepping is adopted. A three-joint collaborative coupling control model is used to drive the robot, and a disturbance observer is constructed for feedforward compensation.

[0143] Example 4:

[0144] This embodiment provides a storage medium, which is a computer-readable storage medium, storing a computer program. When the computer program is executed by a processor, it implements the biomimetic underwater robot dynamic collision avoidance control method of Embodiment 1 above, as follows:

[0145] Through a deep collaborative sensor fusion strategy, the robot can perceive the three-dimensional position, velocity, and type of obstacles. Based on the obstacle type, the optimal strategy is called from a differentiated obstacle avoidance strategy library to upgrade from geometric space avoidance to intelligent decision-making that combines environmental semantics. Based on the perception results of the three-dimensional position, velocity, and type of obstacles, a hybrid path planning that combines global planning and local obstacle avoidance is adopted. A composite motion control strategy that combines upper-level model predictive control and lower-level backstepping is adopted. A three-joint collaborative coupling control model is used to drive the robot, and a disturbance observer is constructed for feedforward compensation.

[0146] It should be noted that the computer-readable storage medium in this embodiment can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof.

[0147] In this embodiment, the computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in connection with an instruction execution system, apparatus, or device. In this embodiment, the computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying a computer-readable program. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium can also be any computer-readable storage medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The computer program contained on the computer-readable storage medium can be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination thereof.

[0148] The computer-readable storage medium described above can be used to write computer programs for executing this embodiment in one or more programming languages ​​or combinations thereof. These programming languages ​​include object-oriented programming languages—such as Java, Python, and C++—and conventional procedural programming languages—such as C or similar programming languages. The program can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0149] In summary, this invention addresses complex underwater environments by employing a deep collaborative sensor fusion strategy to acquire data on the robot itself and its environment. This not only enables precise perception of the three-dimensional position and velocity of obstacles but also identifies the types of obstacles. Furthermore, by combining a hybrid path planning strategy that integrates global optimization and local real-time analysis with a high-precision, robust composite motion control strategy, it effectively and intelligently executes differentiated obstacle avoidance, significantly improving the robot's obstacle avoidance efficiency, robustness, and endurance in unknown dynamic environments.

[0150] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope disclosed in the present invention, based on the technical solution and inventive concept of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A dynamic collision avoidance control method for a biomimetic underwater robot based on sensor fusion, characterized in that, The method includes: Through a deep collaborative sensor fusion strategy, the three-dimensional position, velocity, and type of obstacles can be perceived; Based on the type of obstacle, the optimal strategy is called from the differentiated obstacle avoidance strategy library to upgrade from geometric space avoidance to intelligent decision-making that combines environmental semantics; Based on the perception results of the three-dimensional position, velocity and type of obstacles, a hybrid path planning that combines global planning and local obstacle avoidance is adopted. A composite motion control strategy that combines upper-level model predictive control and lower-level backstepping is adopted. A three-joint cooperative coupling control model is used to drive the robot, and a disturbance observer is constructed for feedforward compensation.

2. The biomimetic underwater robot dynamic collision avoidance control method according to claim 1, characterized in that, The sensor fusion strategy, which utilizes deep collaboration, enables the perception of the three-dimensional position, velocity, and type of obstacles, including: Visual images are continuously acquired through a visual sensor, and color correction and enhancement preprocessing are performed on the acquired visual images. The preprocessed visual image frames are fed into a lightweight deep learning object detection network to identify obstacles and output the category and visual detection box of the obstacle; The distance and angle information of obstacles are obtained through ultrasonic sensors; The acceleration and angular velocity of the robot body are obtained through an inertial measurement unit; Time synchronization and coordinate system integration of data from visual and ultrasonic sensors; Calculate the Euclidean distance between the projection point and the center of the visual detection box. If the Euclidean distance is less than the set threshold, it is determined that the two correspond to the same obstacle, and the data association is successful. For obstacles to successful data association, extract multidimensional classification features; The classification model is used to analyze the multidimensional classification features extracted in real time and output the type of obstacle. Using extended Kalman filtering to fuse correlated data from visual sensors, ultrasonic sensors, and inertial measurement units, state estimation is performed to output the three-dimensional position and velocity vectors of the obstacle.

3. The biomimetic underwater robot dynamic collision avoidance control method according to claim 2, characterized in that, The time synchronization and coordinate system integration of data from the visual sensor and the ultrasonic sensor includes: The ultrasound data was adjusted using linear interpolation to match the visual time. The three-dimensional points converted from ultrasonic waves are projected onto the camera image plane to generate theoretical pixel coordinates, i.e., projection point coordinates.

4. The biomimetic underwater robot dynamic collision avoidance control method according to claim 1, characterized in that, The step of calling the optimal strategy from the differentiated obstacle avoidance strategy library based on obstacle type includes: When an obstacle is determined to be a regular-shaped obstacle, a strict avoidance strategy is invoked and executed, and a smooth path is planned while strictly adhering to the preset safety distance. When an obstacle is determined to be a discrete obstacle group, a path planning method combining the kinematic constraints of the robotic fish is invoked and executed to bypass it. When an obstacle is determined to be a soft, non-rigid obstacle, a cautious crossing strategy or an increased height crossing strategy is implemented based on the obstacle density assessment results. When the obstacle is a linear obstacle, the parallel navigation strategy is invoked, and a parallel motion path is generated that maintains a specific angle or distance from the obstacle, in conjunction with the safe distance information provided by the ultrasonic sensor.

5. The biomimetic underwater robot dynamic collision avoidance control method according to claim 1, characterized in that, The hybrid path planning method, which combines global planning with local obstacle avoidance, includes: Global path planning is performed using the D*Lite incremental replanning algorithm; When navigating along the global path, the algorithm based on vector field histogram is run at high frequency. Based on the real-time fused perception data, it performs real-time obstacle avoidance between global path points and generates a safe local heading. When local obstacle avoidance fails, the first level of the three-level response mechanism is triggered, and adaptive parameter adjustment is performed. When the first-level response is invalid, the second-level response is triggered, and multi-directional short-distance probing motion actions are executed; When consecutive trial movements fail, a third-level response is triggered, executing global replanning and area avoidance actions.

6. The biomimetic underwater robot dynamic collision avoidance control method according to claim 1, characterized in that, The composite motion control strategy, which combines upper-level model predictive control with lower-level backstepping, includes: A model predictive controller is used at the upper layer to optimize a set of optimal cooperative waveform parameters based on the reference trajectory and robot state. The lower layer employs a backstepping controller, which receives instructions from the model prediction controller, calculates the desired angle of each joint motor, and performs high-precision tracking.

7. A biomimetic underwater robot dynamic collision avoidance control device based on sensor fusion, characterized in that, The device includes: The fusion unit is used to perceive the three-dimensional position, velocity, and type of obstacles through a deep collaborative sensor fusion strategy. The invocation unit is used to invoke the optimal strategy from the differentiated obstacle avoidance strategy library according to the obstacle type, so as to upgrade from geometric space avoidance to intelligent decision-making combined with environmental semantics; The planning and control unit is used to perform hybrid path planning that combines global planning and local obstacle avoidance based on the perception results of the three-dimensional position, velocity and type of obstacles. It adopts a composite motion control strategy that combines upper-level model predictive control and lower-level backstepping method, drives the robot with a three-joint cooperative coupling control model, and constructs a disturbance observer for feedforward compensation.

8. A biomimetic underwater robot dynamic collision avoidance control system based on sensor fusion, used to implement the biomimetic underwater robot dynamic collision avoidance control method according to any one of claims 1-6, characterized in that, The system includes a robot body, control components, a power module, an inertial measurement unit, a vision sensor, and an ultrasonic sensor array. The robot body includes a bionic shell. The control components, power module, and inertial measurement unit are disposed inside the bionic shell and are centrally arranged at the center of gravity of the robot body to maintain posture stability. The vision sensor and ultrasonic sensor array are disposed at the front of the bionic shell, and the vision sensor and ultrasonic sensor array are installed at a 30° intersection angle.

9. A computer device comprising a processor and a memory for storing a processor-executable program, characterized in that, When the processor executes the program stored in the memory, it implements the biomimetic underwater robot dynamic collision avoidance control method according to any one of claims 1-6.

10. A storage medium storing a program, characterized in that, When the program is executed by the processor, it implements the biomimetic underwater robot dynamic collision avoidance control method according to any one of claims 1-6.

Images