Intelligent robotic fish obstacle avoidance control method and system based on IMM-DWA algorithm

Abstract

The invention discloses an intelligent robotic fish obstacle avoidance control method and system based on an IMM-DWA algorithm, and significantly improves the obstacle avoidance capability of an intelligent robotic fish in a complex underwater environment through three-dimensional motion modeling and a dynamic obstacle prediction algorithm. An optimized trajectory evaluation function is combined with balance of path smoothness and energy consumption to ensure that an efficient and stable motion trajectory is generated. The introduction of the heterogeneous computing architecture effectively improves the real-time processing capability of the system, and the cooperative work of the multi-modal sensor enhances the accuracy of environmental perception and the response speed of dynamic obstacles. Periodic updating of a closed-loop control mechanism further guarantees continuous optimization of the obstacle avoidance process, and reliable technical support is provided for autonomous navigation of the intelligent robotic fish in the three-dimensional water area environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot fish obstacle avoidance, and in particular relates to an intelligent robot fish obstacle avoidance control method and system based on an IMM-DWA algorithm. Background Art

[0002] With the rapid development of underwater robotics, intelligent robotic fish are increasingly being used in underwater exploration, environmental monitoring, aquaculture, and other fields. However, traditional obstacle avoidance algorithms suffer from poor adaptability and low efficiency in complex underwater environments, making them unable to meet the high-precision navigation and real-time obstacle avoidance requirements of small underwater devices such as intelligent robotic fish. To enhance the autonomy and safety of small underwater robots like intelligent robotic fish in complex waters and reduce maintenance costs caused by collisions, improving their obstacle avoidance control systems is a powerful measure for efficient navigation.

[0003] Currently, small underwater robots such as intelligent robot fish mostly use intelligent obstacle avoidance methods based on the dynamic window method when avoiding obstacles. Candidate trajectories are generated through velocity space sampling, and the optimal path is evaluated based on the obstacle distance. However, this method is mostly used in two-dimensional environments and cannot achieve the effect of three-dimensional obstacle avoidance in underwater environments.

[0004] Another method for underwater robots to avoid obstacles is to use underwater cameras for visual obstacle avoidance and identify obstacle outlines through edge detection algorithms. However, this method cannot be applied to small underwater machines with limited computing resources.

[0005] The shortcomings of existing technologies include:

[0006] First, traditional dynamic window methods are mostly used in two-dimensional scenes where obstacles are static. The underwater environment is three-dimensional and complex and changeable. Traditional algorithms cannot meet the real-time response requirements of intelligent robot fish obstacle avoidance.

[0007] Second point: Using traditional single STM32H743 serial floating-point operations to process three-dimensional dynamic obstacle avoidance tasks will cause core computing load overload in the multimodal sensor data fusion scenario, and the real-time performance of the obstacle avoidance system cannot be guaranteed.

[0008] Therefore, society is in urgent need of a method to solve the above problems. Summary of the Invention

[0009] In order to solve the above technical problems, the present invention proposes an intelligent robot fish obstacle avoidance control method and system based on the IMM-DWA algorithm to solve the problems existing in the above-mentioned prior art.

[0010] In a first aspect, to achieve the above-mentioned objectives, the present invention provides an intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm, comprising the following steps:

[0011] Collect current environment information, including static obstacle locations, dynamic obstacle motion information, and the robot fish's motion status;

[0012] Predicting the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a constant velocity model and a constant turn rate model;

[0013] Generate candidate motion trajectories in both horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish;

[0014] The candidate trajectories are scored using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption, and the optimal trajectory is selected.

[0015] The robotic fish is driven to execute the optimal trajectory and the environmental information is periodically updated to achieve closed-loop control.

[0016] Optionally, the process of collecting current environmental information includes: obtaining the linear velocity, angular velocity, obstacle distance and linear velocity of the robotic fish through a combined array sensor; detecting the motion state of the robotic fish body through a tactile pressure sensor; and generating three-dimensional point cloud data through a multi-beam sonar.

[0017] Optionally, the prediction process of the future trajectory of the dynamic obstacle includes: establishing state transfer equations of a uniform speed model and a constant turning rate model for each dynamic obstacle, calculating the prediction results of the two models in parallel, and fusing and outputting them according to preset weights.

[0018] Optionally, the process of generating the candidate motion trajectory includes: discrete sampling within a feasible range of speed and angular velocity, simulating the motion path of the robotic fish in three-dimensional space in the future time, and the path includes horizontal displacement and vertical depth adjustment.

[0019] Optionally, the comprehensive evaluation function includes: calculating the minimum distance between the robot fish trajectory and the obstacle, the three-dimensional distance between the trajectory end point and the global target, the vertical height error, the collision risk cost of the dynamic obstacle predicted trajectory, the smoothness of the heading angle change and the weighted sum of the total motion energy consumption.

[0020] Optionally, the process of periodically updating environmental information includes: scheduling multimodal sensor data through the main control core, triggering the hardware acceleration unit to parallelly calculate the trajectory prediction for the next period, and re-executing the obstacle avoidance process at fixed time intervals.

[0021] In a second aspect, the present invention further provides an intelligent robotic fish obstacle avoidance control system based on the IMM-DWA algorithm, which is used to implement an intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm. The system includes:

[0022] Environmental information acquisition module, used to collect current environmental information, including static obstacle positions, dynamic obstacle motion information, and the robot fish's motion status;

[0023] A dynamic obstacle trajectory prediction module is used to predict the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a uniform velocity model and a constant turn rate model;

[0024] The candidate trajectory generation module is used to generate candidate motion trajectories in both horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish;

[0025] The trajectory evaluation and selection module is used to score candidate trajectories and select the optimal trajectory using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption;

[0026] The control execution module is used to drive the robotic fish to execute the optimal trajectory and periodically update environmental information to achieve closed-loop control.

[0027] Optionally, the environmental information collection module includes:

[0028] Combined array sensor unit, used to obtain the linear velocity, angular velocity, obstacle distance and linear velocity of dynamic obstacles of the robotic fish;

[0029] Tactile sensing unit, used to detect the motion state of the robotic fish;

[0030] Multibeam sonar unit for generating 3D point cloud data.

[0031] In a third aspect, the present invention further provides a computer terminal device, comprising:

[0032] one or more processors;

[0033] a memory, coupled to the processor, for storing one or more programs;

[0034] When the one or more programs are executed by the one or more processors, the one or more processors implement, for example, an intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm.

[0035] In a fourth aspect, the present invention further provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, it implements, for example, an intelligent robot fish obstacle avoidance control method based on the IMM-DWA algorithm.

[0036] Compared with the prior art, the present invention has the following advantages and technical effects:

[0037] The present invention provides an obstacle avoidance control method and system for an intelligent robotic fish based on the IMM-DWA algorithm. The present invention significantly improves the obstacle avoidance capability of the intelligent robotic fish in complex underwater environments through three-dimensional motion modeling and dynamic obstacle prediction algorithms. The optimized trajectory evaluation function combines the balance between path smoothness and energy consumption to ensure the generation of efficient and stable motion trajectories. The introduction of heterogeneous computing architecture effectively improves the real-time processing capability of the system, and the collaborative work of multimodal sensors enhances the accuracy of environmental perception and the response speed to dynamic obstacles. The periodic update of the closed-loop control mechanism further ensures the continuous optimization of the obstacle avoidance process, providing reliable technical support for the autonomous navigation of the intelligent robotic fish in a three-dimensional water environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The accompanying drawings, which constitute part of the present invention, are provided to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are provided to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0039] Figure 1 Flowchart of the IMM-DWA algorithm according to an embodiment of the present invention;

[0040] Figure 2 This is a hardware block diagram of the underwater intelligent robotic fish obstacle avoidance system according to an embodiment of the present invention;

[0041] Figure 3 Three views of a traditional DWA algorithm simulation experiment according to an embodiment of the present invention, wherein (a) is a main view, (b) is a top view, and (c) is a side view;

[0042] Figure 4 These are three views of the simulation experiment of the improved DWA algorithm according to an embodiment of the present invention, where (a) is the main view, (b) is the top view, and (c) is the side view. DETAILED DESCRIPTION

[0043] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0044] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0045] Example 1

[0046] like Figure 1 As shown, this embodiment provides an intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm, including:

[0047] Collect current environment information, including static obstacle locations, dynamic obstacle motion information, and the robot fish's motion status;

[0048] Predicting the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a constant velocity model and a constant turn rate model;

[0049] Generate candidate motion trajectories in both horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish;

[0050] The candidate trajectories are scored using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption, and the optimal trajectory is selected.

[0051] The robotic fish is driven to execute the optimal trajectory and the environmental information is periodically updated to achieve closed-loop control.

[0052] Specifically, in order to solve the shortcomings of traditional technologies in the obstacle avoidance ability of intelligent robotic fish in complex underwater environments, the present invention discloses an intelligent robotic fish obstacle avoidance control system based on the IMM-DWA algorithm.

[0053] The present invention designs an obstacle avoidance system for an intelligent robotic fish and proposes an obstacle avoidance algorithm and an obstacle avoidance device based on IMM-DWA. Figure 1 As shown, the system includes the following steps: information sampling, which collects environmental information at the current moment, including the position of static obstacles, dynamic obstacle motion information, and robotic fish motion information; obstacle position prediction, which uses a simplified interactive multi-model (IMM) algorithm to predict the motion of dynamic obstacles. Based on the parallel filtering of the dual motion model, the position, speed, and future motion trend of the dynamic obstacles are predicted, and the predicted trajectory of the obstacles in the future is output; robotic fish trajectory prediction, which generates a candidate set within the feasible range of speed and angular velocity based on the current motion state and physical constraints of the robotic fish. For each candidate speed combination, the motion path of the robotic fish in the future, including horizontal and vertical directions, is simulated; trajectory evaluation, which calculates the minimum distance between the robotic fish trajectory and the obstacle and assesses the collision risk. Each candidate trajectory is scored based on four dimensions: the proximity of the trajectory endpoint to the global target, the minimum distance between the trajectory and the static obstacle, the minimum distance between the trajectory and the predicted trajectory of the dynamic obstacle, and the smoothness of the speed and steering changes, and the optimal trajectory is selected; trajectory execution, which issues the optimal command to drive the robotic fish to execute the next cycle of motion. Sensor data is re-collected at fixed intervals to update the position information of static obstacles, dynamic obstacles, and the target. The above steps are repeated to form a closed-loop feedback control.

[0054] As an implementation method in this embodiment, the process of collecting current environmental information includes: obtaining the linear velocity, angular velocity, obstacle distance and linear velocity of the robotic fish through a combined array sensor; detecting the motion state of the robotic fish body through a tactile pressure sensor; and generating three-dimensional point cloud data through a multi-beam sonar.

[0055] Underwater intelligent robot fish obstacle avoidance system hardware Figure 2 As shown, specifically including:

[0056] The combined array sensor is connected to the main control core STM32H743 to obtain the linear velocity and angular velocity of the robotic fish, and the linear velocity and distance of obstacles; the main control unit, the main control core STM32H743, is connected to all other modules and is responsible for system scheduling; the hardware acceleration unit is connected to the main control core STM32H743, solidifies the trajectory prediction algorithm, and realizes hardware acceleration; the power module is connected to each device and is responsible for power supply; the motor drive module is connected to the main control core STM32H743 to control the operation of the motor; the propulsion module is connected to the motor drive module and is responsible for the movement of the robotic fish.

[0057] As an implementation method in this embodiment, the prediction process of the future trajectory of the dynamic obstacle includes: establishing state transfer equations of a uniform speed model and a constant turning rate model for each dynamic obstacle, calculating the prediction results of the two models in parallel, and fusing and outputting them according to preset weights.

[0058] Specifically, the obstacle position prediction process includes:

[0059] Kinematic modeling is performed by expanding the state vector into three-dimensional position, heading angle, three-axis velocity and angular velocity, a total of six dimensions, which can more accurately describe the robot's full-degree-of-freedom motion capability in three-dimensional space. Its state vector is expressed as:

[0060]

[0061] Where (x, y, z) is the three-dimensional space coordinate, θ is the rotation angle of the robot fish around the global z axis, v x is the x-axis component of linear velocity, v y It is the y-axis component of the linear velocity. Different from the heading of two-dimensional DWA, the separation design of decoupling horizontal heading and vertical direction can avoid the complexity of quaternion and meet the actual needs of most underwater robots that only need yaw control.

[0062] For obstacle position prediction, the interactive multi-model algorithm runs multiple models in parallel, each model performs state estimation separately, and finally performs weighted fusion based on model probability. This is more effective for obstacle prediction in multiple motion modes, but the computational complexity is high, especially when there are many models or the state dimension is large. The present invention only retains two models, the constant velocity (CV) and constant turning rate (CTRV), to cover the motion modes of underwater obstacles. The state of each obstacle is defined as:

[0063] s o =[x o ,y o ,z o ,v o ,ψ o ] T

[0064] Among them, x o is the x-axis coordinate value of the obstacle, y o is the y-axis coordinate value of the obstacle, z o is the z-axis coordinate value of the obstacle, v o is the y-axis coordinate value of the obstacle, ψ o is the obstacle heading angle.

[0065] The state transition expression of the uniform velocity (CV) model is:

[0066]

[0067] Where Δt is the time step, is the state of the obstacle at time k.

[0068] The state transition expression of the constant turning rate (CTRV) model is:

[0069]

[0070] Where ω is the obstacle turning rate, which is set to a constant value.

[0071] The expression for generating the final result of obstacle trajectory prediction is:

[0072]

[0073] Wherein the weight λ1=λ2=0.5.

[0074] As an implementation method in this embodiment, the process of generating the candidate motion trajectory includes: discrete sampling within a feasible range of speed and angular velocity, simulating the motion path of the robotic fish in three-dimensional space in the future time, and the path includes horizontal displacement and vertical depth adjustment.

[0075] Specifically, the robot fish trajectory prediction process includes:

[0076] To predict the trajectory of the robotic fish, the improved DWA algorithm defines the robot's position at time k+1 as:

[0077]

[0078] Among them, x k is the x-axis coordinate value of the robot fish at time k, y k is the y-axis coordinate value of the robot fish at time k, z k is the z-axis coordinate value of the robot fish at time k, x k+1 is the x-axis coordinate value of the robot fish at time k+1, y k+1 is the y-axis coordinate value of the robot fish at time k+1, z k+1 is the z-axis coordinate value of the robot fish at time k+1, K p is the proportional coefficient of the PID controller, K i is the integral coefficient of the PID controller, K d is the differential coefficient of the PID controller, e z is the height difference between the current coordinate point of the robot fish and the target point, e z =z goal -z current .

[0079] As an implementation method in this embodiment, the comprehensive evaluation function includes: calculating the minimum distance between the mechanical fish trajectory and the obstacle, the three-dimensional distance between the trajectory end point and the global target, the vertical height error, the collision risk cost of the dynamic obstacle predicted trajectory, the smoothness of the heading angle change and the weighted sum of the total motion energy consumption.

[0080] Specifically, the comprehensive evaluation includes:

[0081] Trajectory evaluation, based on the distance evaluation function of the traditional DWA algorithm, uses two-dimensional Euclidean distance. goal Expanded to three-dimensional space distance, the improved DWA algorithm distance evaluation function is:

[0082]

[0083] In view of the three-dimensional characteristics of the underwater environment, the improved DWA algorithm of the present invention adds a height error term in the evaluation function:

[0084] G height =|z final -z goal |

[0085] G height The focus of the intelligent robot fish's depth control has been enhanced to prevent it from focusing too much on horizontal movement and ignoring vertical positioning.

[0086] In order to deal with the uncertainty of dynamic obstacles in the underwater environment, the dynamic risk term is introduced as follows:

[0087]

[0088] Where N obs is the total number of dynamic obstacles array length, T pred is the number of discrete steps in the prediction time window, ||p t -p goal || is the distance between the predicted point and the target point in the tth step, d i (t) is the distance between the i-th obstacle and the predicted position of the robot fish at time t. By integrating the obstacle motion prediction and the spatiotemporal attenuation factor λ, the collision risk cost within the future window is calculated, solving the problem of delayed obstacle avoidance caused by ignoring obstacle trajectory prediction in traditional algorithms. This algorithm strengthens risk perception when the distance to the target point is close and reduces redundant avoidance when it is farther away, ensuring both obstacle avoidance safety and path approach efficiency.

[0089] The motion smoothing cost function is:

[0090]

[0091] Where N step is the total number of discrete steps in trajectory prediction, Δθ i =θ i+1 -θ i The heading angle difference between adjacent steps is penalized by square, which controls the generation of a smooth steering path and avoids path oscillation.

[0092] The energy consumption cost function introduced in the total evaluation function can constrain the total motion energy consumption and balance safety and endurance. The energy consumption cost function is:

[0093]

[0094] In summary, the improved evaluation function of the present invention is:

[0095] G total =γ g ·G goal +γ h ·G height +γ r ·G risk +γ s ·G smooth +γ e ·G energy

[0096] Among them, γ g is the distance evaluation function weight, γ his the height evaluation function weight, γ r is the weight of the obstacle dynamic risk assessment function, γ s is the weight of motion smoothness evaluation function, γ e is the weight of the energy consumption evaluation function.

[0097] As an implementation method of this embodiment, the process of periodically updating environmental information includes: scheduling multimodal sensor data through the main control core, triggering the hardware acceleration unit to parallel calculate the trajectory prediction for the next period, and re-executing the obstacle avoidance process at fixed time intervals.

[0098] Specifically, the intelligent robotic fish obstacle avoidance device proposed in the present invention includes:

[0099] The combined array sensor is connected to the main control core STM32H743 and the hardware acceleration unit FPGA. The multi-beam sonar module measures the linear velocity and distance of obstacles, and collaborates with the tactile array to scan the contact area, generate underwater terrain point clouds and detect moving targets, and obtain the relative speed of objects through FPGA hardware acceleration calculation; the MEMS inertial measurement unit calculates the yaw angle of the robotic fish; the tactile pressure sensor detects the linear velocity and angular velocity signals of the robotic fish itself, and estimates its own motion state through the deformation of distributed tactile units.

[0100] The STM32H743 main control core calculates motion states, receives data from the tactile array and IMU, estimates the body's six degrees of freedom (DOF) state through Kalman filtering, and constructs velocity constraint equations. It also manages multimodal operations and dynamically assigns interrupt priorities, such as triggering obstacle avoidance mode switching with a tactile collision interrupt or point cloud updates with a sensor data transmission completion interrupt. It also manages the protocol layer, controlling thruster parameters and communicating with the surface base station. The main control core serves as the global resource scheduling and decision-making center, enabling multi-source data fusion and real-time control command issuance.

[0101] The hardware acceleration unit, an FPGA, is connected to the main control core and the combined array sensor. It is dedicated to algorithm acceleration, offloading the main control's computational burden and ensuring real-time path performance. A fixed DSP48 unit executes the CV-CTRV hybrid model in parallel, compressing the matrix operations required for trajectory prediction. While the FPGA processes the current frame, the STM32 writes the next frame's data.

[0102] The power module, the energy center, is connected to each device and is responsible for power supply and wide voltage input management.

[0103] The motor driver module converts the control signal into precise motor movement.

[0104] The propulsion module, serving as an energy conversion terminal, realizes multi-degree-of-freedom vector propulsion. The three tail fin servos control the forward power of the robot fish. Two DC motors are fixed to the wings of the robot fish at a 45° downward angle, and the forward and reverse rotation of the motors controls the ascent and descent of the robot fish. The slider controls the servo, and the connecting rod drives the slider to control the center of gravity of the robot fish.

[0105] To verify the effectiveness of the improved algorithm in this project, a simulation experiment comparing the traditional DWA algorithm with the proposed algorithm was conducted using a 3D version. The experimental environment was an AMD Ryzen 75700G with Radeon Graphics eight-core CPU, an AMD Radeon RX 6600 GPU, 32GB of RAM, Windows 10 Professional (64-bit) operating system, and Matlab. To verify the obstacle avoidance performance of the improved DWA algorithm and the traditional DWA algorithm in a mixed static and dynamic environment, the comparison algorithms were simulated under the same experimental conditions on a 10*10*10 grid map. The robot fish had a size of 0.5m*0.3m*0.2m, a starting point coordinate of (0, 0, 0), and a target coordinate of (10, 10, 10). Five static obstacles measuring 3m*2m*1m and five dynamic obstacles measuring 2m*1m*1m were distributed in the space. In the simulation experiment, the robot fish is considered to have arrived if it is within 0.5m of the target point.

[0106] In this simulation environment, the local path planning of the traditional DWA algorithm generates the trajectory three-view drawing as follows Figure 3 shown. Figure 3 The blue sphere represents the robot fish, the red five-pointed star represents the target point, the red rectangle represents the static obstacle, and the pink rectangle represents the dynamic obstacle. Figure 3 As can be seen, while the traditional DWA algorithm can achieve basic obstacle avoidance in a 3D dynamic obstacle environment, the overall path it generates is relatively tortuous. When encountering obstacles, it is prone to emergency steering due to insufficient prediction of the collision point. Global suboptimality and speed oscillation also expose the limitations of the algorithm's coordination capabilities in complex 3D scenes.

[0107] The improved DWA algorithm was simulated in the same environment, and its local path planning generated trajectory three-view images as shown below: Figure 4 As shown. Figure 4 It can be seen that compared with the traditional method, the improved DWA algorithm has reduced the amplitude of sawtooth fluctuations in the XY plane, achieved continuous and smooth climbing of the Z axis in three-dimensional motion, reduced the frequency of emergency turns, and the global path presents a continuous and smooth characteristic, with higher reliability.

[0108] The local path planning performance of the traditional 3D DWA algorithm and the improved 3D DWA algorithm is analyzed, and the simulation results are shown in Table 1. Figure 3 、 Figure 4 As shown in Table 1, the improved DWA algorithm achieved significant improvements in runtime, average speed, and smoothness compared to the basic DWA algorithm in a simulation environment. The improved DWA algorithm achieved a 54.68% runtime improvement and a 127% increase in average cruising speed compared to the original algorithm. By streamlining the traditional IMM model to a CV+CTRV dual-model combination, the prediction computational overhead was reduced by 50%. Furthermore, a Euclidean distance pre-screening strategy with a dynamic safety radius reduced invalid collision detections, resulting in an overall algorithm runtime reduction of approximately 40%. The hybrid motion model enhances the ability to predict obstacle maneuvers, the three-dimensional trajectory cost function suppresses vertical fluctuations, and the dynamic safety radius reduces sensitivity to local perturbations, addressing path smoothness without increasing trajectory length. Trajectory caching and an adaptive safety radius reduce the frequency of emergency braking, while curvature smoothing constraints shorten turning deceleration intervals. The improved prediction accuracy reduces the frequency of path replanning, resulting in an increase in average cruising speed. Simulation experimental results show that the improved DWA algorithm proposed in this patent can effectively improve the smoothness and operating efficiency of the trajectory, obtain a better planned trajectory, and provide an efficient and robust solution for autonomous navigation in complex water environments.

[0109] Table 1 Comparison of simulation performance indicators

[0110]

[0111] Advantage 1: The present invention improves upon the traditional dynamic window method, expands the algorithm to three-dimensional space, and optimizes the evaluation function, making the algorithm more suitable for obstacle avoidance of robotic fish moving underwater.

[0112] Advantage 2: By introducing a simplified IMM algorithm and improving the trajectory evaluation function, the present invention can predict the position change trend of dynamic obstacles, improve the obstacle avoidance performance of the robot fish and avoid unnecessary detours.

[0113] Advantage 3: The present invention designs a heterogeneous real-time computing architecture of STM32H743 and FPGA, calculates trajectory scores in parallel, improves the computational efficiency of the algorithm, and ensures the real-time performance of the obstacle avoidance system.

[0114] Based on this, an embodiment of the present invention provides an intelligent robot fish obstacle avoidance control method based on the IMM-DWA algorithm, which significantly improves the obstacle avoidance ability of the intelligent robot fish in complex underwater environments through three-dimensional motion modeling and dynamic obstacle prediction algorithm. The optimized trajectory evaluation function combines the balance of path smoothness and energy consumption to ensure the generation of efficient and stable motion trajectories. The introduction of heterogeneous computing architecture effectively improves the real-time processing capability of the system, and the collaborative work of multimodal sensors enhances the accuracy of environmental perception and the response speed of dynamic obstacles. The periodic update of the closed-loop control mechanism further ensures the continuous optimization of the obstacle avoidance process, providing reliable technical support for the autonomous navigation of intelligent robot fish in three-dimensional water environments.

[0115] Example 2

[0116] In this embodiment, a computer terminal device is provided, including:

[0117] one or more processors;

[0118] a memory, coupled to the processor, for storing one or more programs;

[0119] When the one or more programs are executed by the one or more processors, the one or more processors implement the methods in the above embodiments.

[0120] In this embodiment, a computer-readable storage medium is further provided, on which a computer program is stored. When the computer program is executed by a processor, the method in the above embodiment is implemented.

[0121] In this embodiment, an electronic device is further provided, including a memory and a processor. The memory stores a computer program, and the processor is configured to run the computer program to execute the method in the above embodiment.

[0122] The above program can be run in the processor, or it can be stored in the memory (or computer-readable medium), which includes permanent and non-permanent, removable and non-removable media and can be implemented by any method or technology to store information. The information can be computer-readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, read-only compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, tape disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device.

[0123] These computer programs can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for executing on the computer or other programmable device to implement the process. Figure 1 a process or multiple processes and / or boxes Figure 1 The steps of the functions specified in one or more blocks can be implemented by different modules corresponding to different steps.

[0124] This embodiment provides such a device or system. The system is called an intelligent robot fish obstacle avoidance control system based on the IMM-DWA algorithm, including:

[0125] Environmental information acquisition module, used to collect current environmental information, including static obstacle positions, dynamic obstacle motion information, and the robot fish's motion status;

[0126] A dynamic obstacle trajectory prediction module is used to predict the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a uniform velocity model and a constant turn rate model;

[0127] The candidate trajectory generation module is used to generate candidate motion trajectories in the horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish;

[0128] The trajectory evaluation and selection module is used to score candidate trajectories and select the optimal trajectory using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption;

[0129] The control execution module is used to drive the robotic fish to execute the optimal trajectory and periodically update environmental information to achieve closed-loop control.

[0130] As an implementation method of this embodiment, the environmental information collection module includes:

[0131] Combined array sensor unit, used to obtain the linear velocity, angular velocity, obstacle distance and linear velocity of dynamic obstacles of the robotic fish;

[0132] Tactile sensing unit, used to detect the motion state of the robotic fish;

[0133] Multibeam sonar unit for generating 3D point cloud data.

[0134] As an implementation in this embodiment, the dynamic obstacle trajectory prediction module includes:

[0135] A dual-model parallel computing unit is used to establish the state transfer equations of the uniform speed model and the constant turning rate model for each dynamic obstacle, and to calculate the prediction results of the two models in parallel;

[0136] The weighted fusion unit is used to fuse and output the prediction results of the two models according to preset weights.

[0137] As an implementation method of this embodiment, the candidate trajectory generation module includes:

[0138] A motion sampling unit for discrete sampling within a feasible range of velocity and angular velocity;

[0139] The three-dimensional trajectory simulation unit is used to simulate the movement path of the robotic fish in the three-dimensional space in the future time, and the path includes horizontal displacement and vertical depth adjustment.

[0140] As an implementation method in this embodiment, the trajectory evaluation and selection module includes:

[0141] A multi-dimensional evaluation unit is used to calculate the minimum distance between the robot fish's trajectory and obstacles, the three-dimensional distance between the trajectory endpoint and the global target, the vertical height error, the collision risk cost of the predicted trajectory with dynamic obstacles, the smoothness of the heading angle change, and the weighted sum of the total motion energy consumption;

[0142] The optimal trajectory decision unit is used to select the optimal trajectory according to the weighted summation result.

[0143] As an implementation method of this embodiment, the control execution module includes:

[0144] The master control scheduling unit is used to schedule multimodal sensor data and trigger closed-loop updates at fixed time intervals;

[0145] Hardware accelerated computing unit, used for parallel calculation of trajectory prediction for the next cycle.

[0146] The system or device is used to implement the functions of the method in the above-mentioned embodiment. Each module in the system or device corresponds to each step in the method, which has been explained in the method and will not be repeated here.

[0147] Through the above implementation, the problem of obstacle avoidance control of intelligent robotic fish based on the IMM-DWA algorithm in the related art is solved, thereby ensuring that the problems existing in the existing technology are solved.

[0148] The above are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. An intelligent robot fish obstacle avoidance control method based on the IMM-DWA algorithm, characterized in that: The following steps are involved: Collect current environment information, including static obstacle locations, dynamic obstacle motion information, and the robot fish's motion status; Predicting the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a constant velocity model and a constant turn rate model; Generate candidate motion trajectories in both horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish; The candidate trajectories are scored using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption, and the optimal trajectory is selected. The robotic fish is driven to execute the optimal trajectory and the environmental information is periodically updated to achieve closed-loop control.

2. The method according to claim 1, characterized in that The process of collecting current environmental information includes: obtaining the linear velocity, angular velocity, obstacle distance and linear velocity of the robotic fish through a combined array sensor; detecting the motion state of the robotic fish through a tactile pressure sensor; and generating three-dimensional point cloud data through a multi-beam sonar.

3. The method according to claim 1, characterized in that The process of predicting the future trajectory of the dynamic obstacle includes: establishing state transfer equations of a uniform speed model and a constant turning rate model for each dynamic obstacle, calculating the prediction results of the two models in parallel, and fusing and outputting them according to preset weights.

4. The method according to claim 1, wherein The process of generating the candidate motion trajectory includes: discrete sampling within a feasible range of speed and angular velocity, simulating the motion path of the robotic fish in three-dimensional space in the future time, and the path includes horizontal displacement and vertical depth adjustment.

5. The method according to claim 1, wherein The comprehensive evaluation function includes: calculating the minimum distance between the robot fish trajectory and obstacles, the three-dimensional distance between the trajectory endpoint and the global target, the vertical height error, the collision risk cost of the dynamic obstacle predicted trajectory, the smoothness of the heading angle change and the weighted sum of the total motion energy consumption.

6. The method according to claim 1, wherein The process of periodically updating environmental information includes: scheduling multimodal sensor data through the main control core, triggering the hardware acceleration unit to parallelly calculate the trajectory prediction for the next period, and re-executing the obstacle avoidance process at fixed time intervals.

7. An intelligent robot fish obstacle avoidance control system based on the IMM-DWA algorithm, characterized in that: The system comprises: Environmental information acquisition module, used to collect current environmental information, including static obstacle positions, dynamic obstacle motion information, and the robot fish's motion status; A dynamic obstacle trajectory prediction module is used to predict the future trajectory of dynamic obstacles based on an interactive multi-model algorithm, which includes parallel filtering and weighted fusion of a uniform velocity model and a constant turn rate model; The candidate trajectory generation module is used to generate candidate motion trajectories in both horizontal and vertical directions based on the current motion state and physical constraints of the robotic fish; The trajectory evaluation and selection module is used to score candidate trajectories and select the optimal trajectory using a comprehensive evaluation function of 3D target distance, height error, dynamic collision risk, path smoothness, and energy consumption; The control execution module is used to drive the robotic fish to execute the optimal trajectory and periodically update environmental information to achieve closed-loop control.

8. The system according to claim 7, characterized in that The environmental information collection module includes: Combined array sensor unit, used to obtain the linear velocity, angular velocity, obstacle distance and linear velocity of dynamic obstacles of the robotic fish; Tactile sensing unit, used to detect the motion state of the robotic fish; Multibeam sonar unit for generating 3D point cloud data.

9. A computer terminal device, characterized in that: include: one or more processors; a memory, coupled to the processor, for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm as described in any one of claims 1 to 6.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the intelligent robotic fish obstacle avoidance control method based on the IMM-DWA algorithm as described in any one of claims 1 to 6 is implemented.

Images