Underwater environment mixed local path planning method

Abstract

The invention discloses an underwater environment mixed local path planning method, which belongs to the field of underwater path planning, and comprises the following steps: improving a dynamic window method, a vector diagram straight method and an artificial potential field method; wherein the dynamic window method is improved through speed constraint, multi-target trajectory evaluation function reconstruction and dynamic obstacle motion intention prediction; a vector diagram straight method is improved through an adaptive sector resolution method and constraint course angle changes; the influence of water flow movement on a potential field is considered, and a distance attenuation factor and a vortex potential field are introduced to improve an artificial potential field method; fusing the improved dynamic window method, the improved vector diagram straight method and the improved artificial potential field method, and distributing weights for the improved dynamic window method, the improved vector diagram straight method and the improved artificial potential field method to obtain a dynamic arbiter; and inputting the environment data into the dynamic arbiter to obtain the course angle and the speed of the underwater robot so as to complete path planning. According to the method, the optimal path can be generated by fusing the three algorithms.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater path planning, and particularly relates to a hybrid local path planning method for underwater environments. Background Art

[0002] With the continuous progress of technology, underwater robots are increasingly widely used in fields such as ocean exploration, resource development, ocean scientific research, underwater facility operation and maintenance, military reconnaissance, and ecological protection. These robots need to perform tasks in complex underwater environments, which poses higher requirements for their autonomous operation capabilities. Among them, path planning technology is the core link of the intelligence and autonomy of underwater robots, directly determining their execution efficiency and safety.

[0003] Currently, for robots used in underwater environments, the existing path planning methods are mainly divided into three types: the Dynamic Window Approach (DWA), the Vector Field Histogram (VFH), and the Artificial Potential Field (APF). These methods have their own advantages and disadvantages, but all have certain limitations, affecting the operation efficiency and safety of underwater robots.

[0004] The disadvantages of the Dynamic Window Approach (DWA) include large computational complexity, poor environmental adaptability, and insufficient real-time performance. DWA needs to simulate all possible speed combinations in the speed space, which leads to a high computational complexity. In addition, when the surrounding environment changes, DWA needs to re-plan the path, affecting the real-time performance of the algorithm.

[0005] The disadvantages of the Vector Field Histogram (VFH) include that the generated path is not smooth enough, it is difficult to find the global optimal path, and the parameters are strongly correlated with the environment. The path generated by the VFH algorithm is often the result of a greedy search based on the local vector field, and sometimes there are many inflection points and discontinuities. In a complex environment, the VFH algorithm is prone to falling into local optimal solutions and difficult to find the true global optimal path.

[0006] The disadvantages of the Artificial Potential Field (APF) include being prone to falling into local optima, the problem of the target being unreachable, and the existence of path oscillation problems. In a complex environment, once the robot enters a local minimum point, it will be trapped and unable to reach the target. In addition, due to the excessive influence of the repulsive force, or when the gravitational force and the repulsive force are balanced, the robot may not be able to reach the target point.

[0007] In order to overcome the limitations of these methods and improve the path planning efficiency and safety of underwater robots, the present invention proposes a hybrid local path planning method for underwater robots. Summary of the Invention

[0008] The present invention proposes a hybrid local path planning method for underwater environments to solve the problems existing in the above-mentioned prior art.

[0009] To achieve the above object, the present invention provides an underwater environment hybrid local path planning method, including the following steps:

[0010] Improve the dynamic window method, the vector graph straight method, and the artificial potential field method; among them, improve the dynamic window method by speed constraint, multi-objective trajectory evaluation function reconstruction, and prediction of the motion intention of dynamic obstacles; improve the vector graph straight method by the adaptive sector resolution method and constraint of the course angle change; improve the artificial potential field method by considering the influence of water flow movement on the potential field, introducing a distance attenuation factor, and a vortex potential field;

[0011] Fuse the improved dynamic window method, the vector graph straight method, and the artificial potential field method, and assign weights to the improved dynamic window method, the vector graph straight method, and the artificial potential field method to obtain a dynamic arbiter;

[0012] Input the environmental data into the dynamic arbiter to obtain the course angle and speed of the underwater robot, and complete the path planning.

[0013] Preferably, the improvement of the dynamic window method includes:

[0014] Calculate the maximum speed according to the maximum power of the underwater robot's thruster, water density, drag coefficient, and cross-sectional area facing the flow, and perform speed constraint according to the maximum speed;

[0015] Construct a multi-objective trajectory evaluation function according to the distance between the underwater robot and the target point, the distance between the underwater robot and the obstacle, the energy consumption of the robot, and the path curvature;

[0016] Predict the motion intention of dynamic obstacles based on game theory.

[0017] Preferably, the calculation expression of the maximum speed is:

[0018] ;

[0019] In the formula, is the maximum power of the underwater robot's thruster, ρ is the water density, is the drag coefficient, and A is the cross-sectional area facing the flow.

[0020] Preferably, the calculation expression of the multi-objective trajectory evaluation function is:

[0021] ;

[0022] In the formula, is the distance between the underwater robot and the target point, is the distance between the underwater robot and the obstacle, is the energy consumption of the robot, is the path curvature, is the weight coefficient of the distance between the underwater robot and the target point, is the weight coefficient of the distance between the underwater robot and the obstacle, is the weight coefficient of the robot's energy consumption, is the weight coefficient of the path curvature.

[0023] Preferably, the improvement of the vector graph straightening method includes: dynamically adjusting the sector angle according to the obstacle density; constraining the change of the heading angle according to the angle threshold.

[0024] Preferably, the improvement of the artificial potential field method includes:

[0025] Constructing a water flow potential field gradient to guide the robot to sail downstream;

[0026] Introducing a distance attenuation factor into the repulsive force function to rapidly attenuate the repulsive force of distant obstacles;

[0027] Introducing a vortex potential field to generate a small vortex field at the local minimum point and guiding the robot to break away from the local minimum through the rotational force.

[0028] Preferably, the expression of the repulsive force function after introducing the distance attenuation factor is:

[0029] ;

[0030] In the formula, represents the repulsive force function, represents the repulsive force coefficient, d is the distance between the ROV and the obstacle, is the repulsive force range, and σ represents the distance attenuation factor.

[0031] Preferably, obtaining the dynamic arbiter includes:

[0032] Calculating the membership degrees of the improved dynamic window method, vector graph straightening method, and artificial potential field method;

[0033] Performing weight allocation according to the membership degrees. If it is a dynamic obstacle environment, increase the weight of the dynamic window method; if it is a dense obstacle environment, increase the weight of the vector graph straightening method; if it is a sparse obstacle environment, increase the weight of the artificial potential field method.

[0034] The present invention also proposes a computer device, including a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the steps of the method.

[0035] The present invention also proposes a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the method are implemented.

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

[0037] The underwater robot hybrid local path planning method proposed by the present invention achieves remarkable technical effects by comprehensively improving three algorithms: the Dynamic Window Approach (DWA), the Vector Field Histogram (VFH), and the Artificial Potential Field (APF), and designing a dynamic arbiter for algorithm fusion. This algorithm not only combines the advantages of each algorithm to generate a smoother and globally optimal path, but also significantly improves environmental adaptability and real-time performance, enabling the underwater robot to respond more quickly to environmental changes and adjust the path in a timely manner, thereby improving the operation efficiency. In addition, by improving the algorithm, the uncertainty and risk in path planning are reduced, and the safety of the robot during task execution is enhanced. The optimized path planning also helps to reduce energy consumption, extend the working time of the robot, and improve energy utilization efficiency. By introducing measures such as the adaptive sector resolution method and restricting the change of the heading angle, the present invention improves the stability of path planning, reduces the path oscillation phenomenon, and makes the robot move more smoothly. At the same time, the present invention effectively solves the problem that the traditional artificial potential field method is prone to falling into local optima by introducing methods such as the vortex potential field, and improves the ability of the algorithm to find the global optimal solution. In summary, the underwater robot hybrid local path planning method of the present invention has significant technical advantages in improving the efficiency, safety, and adaptability of path planning, and has important theoretical and practical significance for promoting the development and application of underwater robot technology. Brief Description of the Drawings

[0038] The drawings constituting a part of this application are used to provide a further understanding of this application. The schematic embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation of this application. In the drawings:

[0039] Figure 1 It is the flowchart of the method of the embodiment of the present invention. Detailed Embodiments

[0040] It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The following will refer to the drawings and combine the embodiments to detail this application.

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

[0042] Embodiment 1

[0043] As Figure 1As shown in the figure, in this embodiment, a method for underwater environment hybrid local path planning is provided, including the following steps:

[0044] Improve the dynamic window method, the vector graph straight method, and the artificial potential field method; among them, improve the dynamic window method by velocity constraint, multi-objective trajectory evaluation function reconstruction, and prediction of the motion intention of dynamic obstacles; improve the vector graph straight method by the adaptive sector resolution method and constraint of the heading angle change; improve the artificial potential field method by considering the influence of water flow motion on the potential field, introducing a distance attenuation factor, and a vortex potential field.

[0045] Fuse the improved dynamic window method, the vector graph straight method, and the artificial potential field method, and assign weights to the improved dynamic window method, the vector graph straight method, and the artificial potential field method to obtain a dynamic arbiter.

[0046] Input the environmental data into the dynamic arbiter to obtain the heading angle and speed of the underwater robot, and complete the path planning; where the environmental data includes the distance from the underwater robot to the obstacle, and the distance from the underwater robot to the obstacle is collected by a side-scan sonar; it also includes the angle information of the obstacle with respect to the underwater robot, including the azimuth angle and pitch angle of the obstacle, as well as the acceleration and angular velocity information of the robot itself.

[0047] Furthermore, the improvement of the DWA algorithm is as follows:

[0048] Since the traditional DWA algorithm does not consider the motion of obstacles, the computational complexity is large, which affects the real-time performance of the algorithm. To solve this problem, in order to optimize the motion energy consumption and improve the obstacle avoidance efficiency, the present invention constrains the speed range according to the thruster power model, reducing the traditional energy consumption perception speed, the maximum speed It can be obtained from the formula:

[0049] ;

[0050] In the formula, is the maximum power of the underwater robot thruster, ρ is the water density, is the drag coefficient, and A is the cross-sectional area facing the flow. Thus, it is ensured that the ROV moves within the energy consumption constraint.

[0051] After that, according to the particularity of underwater operations, considering energy consumption, safety, and task urgency comprehensively, a multi-objective trajectory evaluation function is redesigned. The calculation formula of the multi-objective trajectory evaluation function is as follows:

[0052] ;

[0053] In the formula, , , , is the weight coefficient, , is the remaining task time, T is the total task time, is the distance to the target point, is the distance to the obstacle, is the energy consumption of the robot, is the path curvature. The weight of energy consumption is dynamically adjusted according to the remaining task time. If the remaining time is long, then increase , making the path planning pay more attention to energy consumption, thus controlling the robot speed and reducing the collision risk; if the remaining time is short, it means the task is urgent, so reduce , making the algorithm pay more attention to other evaluation criteria.

[0054] Finally, considering that there are often dynamic obstacles such as fish schools in water, and the movement of fish schools is relatively regular, so the game theory is used to predict the movement intention of dynamic obstacles. The calculation formula for the predicted position of the obstacle is as follows:

[0055] ;

[0056] ;

[0057] ;

[0058] In the formula, is the position vector of the obstacle, is the position vector of the robot, is the acceleration of the obstacle, is the Nash equilibrium solution obtained according to the game between the robot and the obstacle, representing the predicted acceleration of the obstacle in the next step, is the current speed of the obstacle, is the predicted speed of the obstacle after a time step, is the position information updated according to the predicted speed of the obstacle. Thus, the candidate trajectory set of the robot is generated according to the predicted position of the obstacle.

[0059] Furthermore, the improvement of the VFH algorithm is as follows:

[0060] One defect of the VFH algorithm is that the division of the fan-shaped area will affect the algorithm performance. To address this problem, an adaptive sector resolution method is designed to dynamically adjust the sector angle according to the obstacle density, improving the flexibility of path planning. The sector angle adjustment formula is as follows:

[0061] ;

[0062] In the formula, is the adjusted sector angle, is the basic sector angle, and α is the adjustment coefficient. , , respectively represent the maximum, minimum, and average densities. It realizes reducing the sector angle in the obstacle-dense area to improve the resolution; increasing the sector angle in the sparse area to reduce the computational complexity.

[0063] Sometimes, there will be a large angle between the optimal sector and the current sector, which will cause the robot to have directions such as sharp turn candidates, resulting in path oscillation. Therefore, in the present invention, by constraining the change of the heading angle, an optimized set of safe directions is obtained to improve the smoothness of the path. The calculation formula for the optimized set of safe directions is as follows:

[0064] ;

[0065] In the formula, represents the optimized set of safe directions, is the heading angle at the current moment, is the angle change threshold. By means of the angle threshold, violent jitters during path planning are prevented, making the path smoother and enhancing the stability of the robot's movement.

[0066] Furthermore, the improvement of APF is as follows:

[0067] In the underwater environment, the artificial potential field method not only has the repulsive force of obstacles on the robot and the gravitational force of the target point on the robot, but also the influence of water flow movement on the ROV should be considered. Therefore, in the present invention, in order to optimize the path energy consumption and reduce the countercurrent navigation, the water flow movement is also regarded as a kind of potential field. The construction formula for the water flow movement potential field gradient is as follows:

[0068] ;

[0069] In the formula represents the water flow potential field gradient, is the water flow potential field coefficient, is the speed of the ROV, is the water flow speed. By converting the kinetic energy of the water flow into the potential field gradient to guide the robot to sail downstream, the energy consumption is reduced.

[0070] In addition, in order to avoid the interference of long-distance obstacles, the present invention introduces a distance attenuation factor β into the repulsive force function. After introducing the distance attenuation factor, the repulsive force function is as follows:

[0071] ;

[0072] In the formula, represents the repulsive force function, represents the repulsive force coefficient, d is the distance between the ROV and the obstacle, is the repulsive force range. By introducing σ, the repulsive force of distant obstacles decays rapidly, reducing the interference to path planning.

[0073] Finally, to solve the problem that APF is prone to falling into local minima, the present invention introduces a vortex potential field , generates a small vortex field at the local minimum point, and guides the robot to break away through the rotational force. The vortex potential field is calculated as follows:

[0074] ;

[0075] In the formula, is the vortex field strength coefficient, r is the action radius of the vortex field, ( , ) is the local minimum point coordinate.

[0076] Furthermore, the dynamic arbiter fuses the three algorithms as follows:

[0077] 1. Input data: including the local optimal trajectory direction and speed output by DWA, the safe direction output by VFH, and the potential field gradient direction output by APF.

[0078] 2. Calculate the confidence of each algorithm: The formula for calculating the confidence of the DWA algorithm is as follows:

[0079] ;

[0080] In the formula, is the DWA response time, λ is the attenuation coefficient, and this formula represents that the shorter the response time, the higher the confidence of the DWA algorithm.

[0081] The formula for calculating the confidence of the VFH algorithm is as follows:

[0082] ;

[0083] In the formula, is the current maximum obstacle density, is the density threshold, and when it is exceeded, the current environment is considered complex. This formula represents that the lower the obstacle density, the higher the confidence of the VFH algorithm.

[0084] The formula for calculating the confidence of the APF algorithm is as follows:

[0085] ;

[0086] In the formula, represents the total potential field gradient of gravity plus repulsion, Represents the gradient of the gravitational potential field. This equation represents that the lower the proportion of the repulsive force, the higher the confidence level of the AFH algorithm.

[0087] 3. Weight dynamic allocation: According to the confidence evaluation results, dynamically allocate the weights of each algorithm. The weight calculation formula is as follows:

[0088] ;

[0089] In the formula, is the confidence level of the i-th algorithm, is the historical error change rate of the i-th algorithm, is the average error, represents the confidence level of the j-th algorithm above. Then the weight allocation logic is: If it is a dynamic obstacle environment, increase the weight of DWA to ensure real-time obstacle avoidance first; if it is a dense obstacle environment, increase the weight of VFH to ensure path safety first; if it is a sparse obstacle environment, increase the weight of APF to ensure path smoothness first.

[0090] 4. Final output: The dynamic arbiter fuses the outputs of each algorithm according to the weights to generate the heading angle and speed of the ROV in the path planning task. The heading angle update formula is as follows:

[0091] ;

[0092] The speed update formula is as follows:

[0093] .

[0094] This embodiment also proposes a computer device, including a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the steps of the method.

[0095] This embodiment also proposes a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the method are implemented.

[0096] The above is only a preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. An underwater environment hybrid local path planning method, characterized in that, It includes the following steps: Improve the dynamic window method, the vector graph straight-line method, and the artificial potential field method. Among them, improve the dynamic window method by velocity constraint, multi-objective trajectory evaluation function reconstruction, and predicting the motion intention of dynamic obstacles; improve the vector graph straight-line method by the adaptive sector resolution method and restricting the change of the heading angle; improve the artificial potential field method by considering the influence of water flow motion on the potential field, introducing a distance attenuation factor, and a vortex potential field. Fuse the improved dynamic window method, vector graph straight-line method, and artificial potential field method, and assign weights to the improved dynamic window method, vector graph straight-line method, and artificial potential field method to obtain a dynamic arbiter. Input the environmental data into the dynamic arbiter to obtain the heading angle and velocity of the underwater robot, and complete the path planning.

2. The method according to claim 1, wherein The improvement of the dynamic window method includes: Calculate the maximum velocity according to the maximum power of the underwater robot's thruster, water density, drag coefficient, and the flow-facing area, and perform velocity constraint according to the maximum velocity. Construct a multi-objective trajectory evaluation function based on the distance between the underwater robot and the target point, the distance between the underwater robot and the obstacle, the robot's energy consumption, and the path curvature. Predict the motion intention of dynamic obstacles based on game theory.

3. The method according to claim 2, wherein The calculation expression of the maximum velocity is: ； In the formula, is the maximum power of the underwater robot thruster, ρ is the water density, is the drag coefficient, and A is the flow-facing area.

4. The method according to claim 2, characterized in that The calculation expression of the multi-objective trajectory evaluation function is: ； Wherein, is the distance between the underwater robot and the target point, is the distance between the underwater robot and the obstacle, is the energy consumption of the robot, is the path curvature, is the weight coefficient of the distance between the underwater robot and the target point, is the weight coefficient of the distance between the underwater robot and the obstacle, is the weight coefficient of the energy consumption of the robot, is the weight coefficient of the path curvature.

5. The method according to claim 1, wherein The improvement of the vector graph straight-line method includes: Dynamically adjust the sector angle according to the obstacle density; Restrict the change of the heading angle according to the angle threshold.

6. The method according to claim 1, wherein The improvement of the artificial potential field method includes: Construct a water flow potential field gradient to guide the robot to sail along the current. Introduce a distance attenuation factor into the repulsive force function to make the repulsive force of distant obstacles decay rapidly. Introduce a vortex potential field to generate a small vortex field at the local minimum point, and guide the robot to break away from the local minimum through the rotational force.

7. The method according to claim 6, wherein The expression of the repulsive force function after introducing the distance attenuation factor is: ； In the formula, represents the repulsive force function, represents the repulsive force coefficient, d is the distance between the ROV and the obstacle, is the repulsive force range, represents the distance attenuation factor.

8. The method according to claim 1, wherein Obtaining the dynamic arbiter includes: Calculate the membership degrees of the improved dynamic window method, vector graph straight-line method, and artificial potential field method. Perform weight assignment according to the membership degrees. If it is a dynamic obstacle environment, increase the weight of the dynamic window method; if it is a dense obstacle environment, increase the weight of the vector graph straight-line method; if it is a sparse obstacle environment, increase the weight of the artificial potential field method.

9. A computer device, comprising a memory, a processor, and a computer program stored on the memory, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1-8.

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

Images