A path planning method for unmanned surface boat

Abstract

The present invention discloses a method for planning the path of a surface unmanned boat, and relates to the field of marine engineering technology, including determining the starting point and end point information, dynamic obstacles and static obstacles of the unmanned boat in a rasterized map; improving the heuristic function of the A* algorithm; establishing OPEN and CLOSE tables for the starting point, using the improved A* algorithm search strategy to search from the starting point, and deleting the child nodes that are too close to the obstacles; judging the global evaluation function of the child nodes of the OPEN table, taking the point with the lowest global evaluation function as the new parent node, and adding the newly generated parent node to the CLOSE table, improving the node array to reduce the path nodes, and forming a global path; smoothing the global path; performing dynamic obstacle avoidance, and after being away from the dynamic obstacles, the unmanned boat takes the points of the original global path as sub-target points and continues to return to the original path. The present invention reduces the path planning length and effectively improves the safety of the unmanned boat.

Description

Technical Field

[0001] The invention relates to the technical field of marine engineering, and in particular to a path planning method for an unmanned surface boat. Background Art

[0002] As a key equipment for marine exploration and resource development, the importance of surface unmanned boats to marine research is self-evident. Path planning is one of the key technologies for autonomous navigation and control of surface unmanned boats. Due to the particularity of the working environment of surface unmanned boats, ensuring the safety and real-time performance of surface unmanned boat path planning is a hot spot and difficulty in the current research of surface unmanned boats. Many path planning algorithms have been proposed, but their scope of application is different. In addition, the current path planning algorithms ignore the primary premise of surface unmanned boat safety, so further improvement is needed.

[0003] The Chinese invention patent with publication number CN118569462A proposed a path planning method based on reinforcement learning, which solved the problem of slow convergence and easy to fall into local optimality in path planning. The Chinese invention patent with publication number CN116931569A proposed an unmanned boat path planning algorithm based on fast search tree, including the following steps: determining the starting point X-start and the target point X-goal, and Figure 2 The value processing is performed, the non-navigable area is represented as black, the navigable water area is represented as white, and the obstacles are expanded; it is determined whether there are obstacles on the line between X-start and the target point X-goal. If there are no obstacles, the path search is successful, otherwise proceed to the next step. By combining the idea of ​​the extremely greedy algorithm, the bidirectional fast search random tree can quickly find the path in an environment with fewer obstacles.

[0004] The two patents use different algorithms for unmanned boat path planning, but both methods do not consider the safety standard of unmanned boat path planning. During the navigation of unmanned boats, complex and changeable and sudden dynamic obstacles undoubtedly threaten the safety of unmanned boats. Therefore, it is necessary to further study a path planning method to improve the safety of surface unmanned boats and real-time obstacle avoidance of dynamic obstacles, so as to ensure the safety of unmanned boat path planning navigation. Summary of the invention

[0005] In order to overcome the above problems existing in the prior art, the present invention proposes a path planning method for an unmanned surface boat.

[0006] The technical solution adopted by the present invention to solve the technical problem is: a path planning method for an unmanned surface boat, comprising the following steps:

[0007] Step 1: rasterize the map and divide it into feasible domain and infeasible domain, determine the starting point and end point information of the unmanned boat in the map, and determine the location of dynamic obstacles and the information of static obstacles;

[0008] Step 2, improve the heuristic function of the A* algorithm and replace the Manhattan distance with the triangular distance;

[0009] Step 3, establish OPEN and CLOSE tables for the starting point, add the starting point to the OPEN and CLOSE tables, use the improved A* algorithm search strategy to start searching from the starting point, delete the child nodes that are too close to the obstacle, and add the processed child nodes to the OPEN table;

[0010] Step 4, determine the global evaluation function of the child nodes of the OPEN table, take the point with the lowest global evaluation function as the new parent node, and add the newly generated parent node to the CLOSE table, improve the node array to reduce the path nodes, and form a global path;

[0011] Step 5, using a Bezier curve to smooth the global path obtained in step 4;

[0012] Step 6, introduce the improved artificial potential field function to dynamically avoid dynamic obstacles. After moving away from the dynamic obstacles, the unmanned boat will take the point on the path generated in step 5 as the sub-target point and continue to return to the original path.

[0013] In the above-mentioned method for path planning of an unmanned surface vehicle, the triangular distance expression in step 2 is:

[0014] ;

[0015] in, and They represent the distance between the current position of the unmanned boat and the horizontal and vertical coordinates of the target point respectively, and h(n) represents the heuristic function.

[0016] In the above-mentioned surface unmanned boat path planning method, the improved A* algorithm search strategy in step 3 is specifically as follows: taking the starting point as the parent node, searching the information of the eight surrounding child nodes, judging whether there is an obstacle node among the eight child nodes, if there is an obstacle node, marking the obstacle node, marking the upper, lower, left and right grids of the marked child node as infeasible areas, and removing the child nodes that are too close to the obstacles in the eight child nodes in turn.

[0017] In the above-mentioned surface unmanned boat path planning method, the node array is improved to reduce the path nodes in step 4 as follows: in the node array that has been planned, three nodes are selected, node one and node three are connected, and it is determined whether the connection line intersects with the obstacle area. If there is an intersection, node two is retained. If not, the shortest distance between the connection line and the obstacle is continued to be determined. If the minimum distance is greater than n times the size of the unmanned boat, node two is removed. If it is less than n times the size of the unmanned boat, node two is retained.

[0018] Among them, n is not less than 4.

[0019] The above-mentioned surface unmanned boat path planning method determines the shortest distance between the connecting line and the obstacle by: saving all the nodes passed by the connecting line and putting them into an array, determining whether the array has an intersection with the obstacle array, marking the obstacles, and determining the distance between the four vertices of each obstacle and the connecting line between nodes one and three.

[0020] In the above-mentioned unmanned surface boat path planning method, the improved artificial potential field function in step 6 is specifically:

[0021] Gravitational function The expression is:

[0022]

[0023] in, is the gravitational magnitude coefficient, is the gravitational variation range adjustment factor, is the gravitational upper and lower limit adjustment factor, is the Euclidean distance between the current position of the unmanned boat and the target point;

[0024] Repulsion function The expression is:

[0025]

[0026] Among them, K represents the repulsion coefficient, represents the repulsion range adjustment factor, and c represents the Euclidean distance between the dynamic obstacle and the unmanned boat.

[0027] In the above-mentioned surface unmanned boat path planning method, the specific method of dynamically avoiding dynamic obstacles in step 6 is: if the angle difference between the unmanned boat and the obstacle is between [0°, 45°] or [315°, 360°] during navigation, the unmanned boat sails to the left; if the angle between the unmanned boat and the dynamic obstacle is between [165°, 195°], the unmanned boat sails to the right; if the angle between the unmanned boat and the obstacle is between [45°, 165°], the unmanned boat sails to the right; if the angle between the unmanned boat and the obstacle is between [195°, 315°], the unmanned boat sails to the left.

[0028] In the above-mentioned path planning method for a surface unmanned boat, the corner maneuvering of the unmanned boat during turning is less than 60°.

[0029] The beneficial effects of the present invention are that, by making multiple improvements to the A* algorithm, the present invention can effectively improve the safety of unmanned boat path planning, reduce the length of path planning, and effectively reduce energy consumption; an improved artificial potential field function is introduced, so that dynamic obstacles can be avoided during path planning, thereby improving the safety of path planning in the face of dynamic obstacles, and improving the practicability of unmanned boat path planning, which is more in line with actual conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is a schematic diagram of the process of the present invention;

[0031] Figure 2 Schematic diagram of the A* algorithm searching eight byte nodes in an embodiment of the present invention;

[0032] Figure 3 This is the path planning result of the traditional A* algorithm in an obstacle map;

[0033] Figure 4 It is a path planning result diagram of the improved A* algorithm of the present invention in a certain obstacle map;

[0034] Figure 5 It is the path planning result graph obtained from step 1 to step 4 of the present invention;

[0035] Figure 6 It is the path planning result graph obtained from step 1 to step 5 of the present invention;

[0036] Figure 7 Schematic diagram of a method for avoiding dynamic obstacles according to the present invention, wherein USV represents an unmanned surface vehicle and Obstacle represents an obstacle;

[0037] Figure 8 It is a partial simulation result diagram of the unmanned boat of the present invention performing dynamic obstacle avoidance under the premise of having global path planning;

[0038] Fig. 9 It is a detailed process diagram of the unmanned boat of the present invention avoiding dynamic obstacles. DETAILED DESCRIPTION

[0039] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0040] This embodiment discloses a method for planning a path for an unmanned surface vehicle. Figure 1 It is a detailed flow chart of the method, and its specific implementation steps are as follows:

[0041] (1) Rasterize the map and divide it into feasible and infeasible domains, determine the starting and ending points of the unmanned boat in the map, and determine the location of dynamic obstacles and the information of static obstacles. Mark this information in the image.

[0042] (2) Improvements to the A* algorithm. First, the present invention does not use the heuristic function of the traditional A* algorithm, but replaces the Manhattan distance with the triangular distance, the expression of which is as follows:

[0043] ;

[0044] in, and They represent the distance between the current position of the unmanned boat and the horizontal and vertical coordinates of the target point. The use of triangular distance can effectively reduce the length of the unmanned boat path planning.

[0045] (3) Create OPEN and CLOSE tables for the starting point and add the starting point to the OPEN and CLOSE tables.

[0046] (4) Improve the search strategy of the A* algorithm. In the search process of the traditional A* algorithm, there are situations where the node crosses the obstacle vertex and is too close to the obstacle. This is not in line with the actual situation and poses a great threat to the safety of the unmanned boat path planning process. Improve the search strategy of the A* algorithm. In the process of searching from the starting point, according to the search strategy of the traditional A* algorithm, first search the information of the eight surrounding sub-nodes, and then determine whether there is an obstacle node among the eight sub-nodes. If there is an obstacle node, mark the obstacle node, and mark the upper, lower, left and right grids of the marked sub-node as infeasible areas. Remove the sub-nodes that are too close to the obstacle in the eight sub-nodes one by one. The improved A* algorithm will not search the sub-nodes close to the marked point. Figure 2This is a schematic diagram of the A* algorithm searching eight child nodes. In the figure, the position of each child node is marked, represented by 1-8. According to the search strategy, if child node 1 is an obstacle node, nodes 2 and 4 will be removed from the child node array. If child node 2 is an obstacle point, points 1 and 3 will be removed from the child node array. This method will ensure that the unmanned boat will not be too close to the obstacle or even cross the obstacle vertex obliquely. Finally, the improved child node is added to the OPEN child node list. Figure 3 This is the path planning result of the traditional A* algorithm in an obstacle map. Figure 4 This is the result diagram of the improved A* algorithm in this embodiment, where the black part is the obstacle, the red square is the starting point, the green square is the end point, the circle represents the child node, and the red line represents the planned path. According to the comparison, at the bend of the obstacle, the traditional A* algorithm crosses the vertices of the obstacle, which is extremely unfavorable for the navigation of the unmanned boat, while the improved algorithm chooses to bypass the vertices without crossing them, which improves safety.

[0047] (5) Determine the global evaluation function of the child nodes in the OPEN table, find the point with the lowest global evaluation function as the new parent node, and add the newly generated parent node to the CLOSE table. This process forms a global path.

[0048] (6) Improve the nodes of the A* algorithm. Due to the nature of the heuristic function of the traditional A* algorithm, there are many turning points in the path planning process, which is fatal to the unmanned boat. The unmanned boat is an under-actuated system. Too many turning points will lead to the performance degradation and energy consumption increase of the unmanned boat, and even threaten the navigation safety of the unmanned boat. Therefore, we need to reduce its path nodes. The main process is: in the node array that has been planned, select three nodes, connect one node and the third node, and determine whether the connection line intersects with the obstacle area. If so, do not process it and save the second node. If not, continue to determine the shortest distance between the connection line and the obstacle. The method adopted is: save all the nodes passed by the connection line, put them into an array, and determine whether the array intersects with the obstacle array, that is, determine whether there are obstacles in the grid blocks passed, mark these obstacles, and determine the distance between the four vertices of each obstacle and the connection line between the first node and the third node. If the smallest distance is greater than 0.4 squares (Note: This value can be changed according to the specific navigation situation. It is best to be four times the size of the unmanned boat. It can effectively ensure the maneuverability of the unmanned boat. It is also acceptable to be greater than this distance, but it may sacrifice the path length or planning time.). It is considered that the second node has no effect on the path planning and can be removed. If it is less than 0.4, the second node will not be removed. The second node is an inevitable turning point at this time. Figure 5This is the path planning result diagram after the improvement based on (2) and this step. The black part is the obstacle, the red square is the starting point, the green square is the end point, the circle represents the child node, and the red line represents the result obtained according to (1)-(6). Figure 5 It can be seen that compared with the traditional A* algorithm, this improvement can greatly reduce the number of turning points in the path planning of the unmanned boat and reduce the length of the path planning.

[0049] (7) Bezier curve is used for smoothing. Since the path information generated in the previous steps has the characteristic of large corners, it is not conducive to the actual navigation of the unmanned boat. Too large corners may cause the unmanned boat to turn in time and cause over-control problems, which greatly threatens the navigation safety of the unmanned boat. Therefore, path smoothing is required. The path planning generated in step 4 is segmented and control points are added to each segment to ensure that each segment can be autonomously smoothed according to the obstacle information in the current area. Each smoothed path is connected to ensure the continuity of the path. Figure 6 This is the path planning result diagram based on this step and the previous improvements. The black part is the obstacle, the red square is the starting point, the green square is the end point, the circle represents the child node, the red line represents the planned path obtained according to (1)-(6), and the black line represents the planned path after the Bezier curve is smoothed. Figure 6 It can be seen that the path planning after path smoothing can be safer and can effectively ensure the safety of the unmanned boat path planning. After the above steps and the improvement of the A* algorithm, the global path planning information of the unmanned boat can be effectively formed, which can effectively deal with the threat of static obstacles in the real environment. On the premise of ensuring its safety first, it also reduces energy consumption and reduces the path planning length.

[0050] (8) An improved artificial potential field function is introduced to deal with the threat of dynamic obstacles in the path planning process. The traditional artificial potential field algorithm has the problems of local optimality and unreachable target. The artificial potential field function is redefined as follows:

[0051] Gravitational function definition:

[0052]

[0053] in is the gravitational magnitude coefficient, is the gravitational range adjustment factor, It is the upper and lower limit adjustment factor of gravity. It represents the Euclidean distance between the current position of the unmanned boat and the position of the target point. The factor that determines the gravity of the target point in the traditional artificial potential field method is the distance between the unmanned boat and the target point. The gravity increases with the increase of distance. When the unmanned boat is farthest from the target point, the gravity is the largest. If the gravity is too large, the unmanned boat will ignore the repulsive force and expand, which is a great threat to the safety of the unmanned boat. Therefore, it is necessary to redefine the gravity function.

[0054] Repulsion function definition:

[0055]

[0056] in represents the repulsion coefficient, Represents the repulsive force range adjustment factor, Represents the Euclidean distance between the dynamic obstacle and the unmanned boat.

[0057] Combined with international rules for avoiding obstacles at sea, such as Figure 7 As shown in the figure, the specific method of avoiding dynamic obstacles is: (1) If the angle difference between the unmanned boat and the obstacle is between [0°, 45°] or [315°, 360°], the unmanned boat is close to the dangerous distance, and the unmanned boat sails to the left. (2) If the angle between the unmanned boat and the dynamic obstacle is between [165°, 195°]. At this angle, it means that the unmanned boat and the obstacle are facing each other, so the unmanned boat should sail to the right. If the angle between the unmanned boat and the obstacle is at other angles, it is a cross-encounter situation. If the angle between the two is between [45°, 165°], the unmanned boat sails to the right. When the angle is between [195°, 315°] and close to the dangerous distance, the unmanned boat should sail to the left. Each obstacle avoidance action must ensure the distance between the unmanned boat and the dynamic obstacle, so as to ensure the safety of the unmanned boat during operation. After the unmanned boat completes the obstacle avoidance of the dynamic obstacle, the path planned by the global path will be used as the target path to guide the unmanned boat toward the global planned path. For the safety of the unmanned boat, the present invention sets a corner limit to limit the corner maneuvering of the unmanned boat to less than 60°.

[0058] Figure 8 This is a partial simulation result diagram of the unmanned boat performing dynamic obstacle avoidance under the premise of global path planning. The black part is the obstacle, the red square is the starting point, the green square is the end point, the circle represents the child node, the red line represents the result obtained according to (1)-(6), the black line represents the planned path after curve smoothing, the red dots represent dynamic obstacles, and the blue line represents the process of dynamic obstacle avoidance. Fig. 9 This is the detailed process of avoiding dynamic obstacles. The green line is the direction of the originally planned path, and the blue line is the path to avoid dynamic obstacles. Fig. 9It can be seen that the unmanned boat can make evasive actions against dynamic obstacles in advance and always maintain a safe distance from dynamic obstacles. Figure 8-Figure 9 It can be seen that the unmanned boat can reasonably avoid obstacles according to the international maritime obstacle avoidance rules, and ensure the safe distance between the unmanned boat and dynamic obstacles, ensuring the safety of the unmanned boat's navigation. It effectively solves the obstacle avoidance problem of dynamic and static obstacles in the path planning of the unmanned boat, and has great feasibility and practicality.

[0059] The above embodiments are only exemplary embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the essence and protection scope of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall within the protection scope of the present invention.

Claims

1. A method for path planning of an unmanned surface vehicle, characterized in that: The steps include: Step 1: rasterize the map and divide it into feasible domain and infeasible domain, determine the starting point and end point information of the unmanned boat in the map, and determine the location of dynamic obstacles and the information of static obstacles; Step 2, improve the heuristic function of the A* algorithm and replace the Manhattan distance with the triangular distance; Step 3, establish OPEN and CLOSE tables for the starting point, add the starting point to the OPEN and CLOSE tables, use the improved A* algorithm search strategy to start searching from the starting point, delete the child nodes that are too close to the obstacle, and add the processed child nodes to the OPEN table; Step 4, determine the global evaluation function of the child nodes of the OPEN table, take the point with the lowest global evaluation function as the new parent node, and add the newly generated parent node to the CLOSE table, improve the node array to reduce the path nodes, and form a global path; Step 5: Use Bezier curve to smooth the global path obtained in step 4; divide the path planning generated in step 4 into segments, add control points to each segment, so as to ensure that each segment can be autonomously smoothed according to the obstacle information in the current area, and connect each smoothed path to ensure the continuity of the path; Step 6: Introduce the improved artificial potential field function to dynamically avoid dynamic obstacles. After moving away from the dynamic obstacles, the unmanned boat will take the point on the path generated in step 5 as the sub-target point and continue to return to the original path. The triangular distance expression in step 2 is: in, and They represent the distance between the current position of the unmanned boat and the horizontal and vertical coordinates of the target point, respectively, and h(n) represents the heuristic function; The improved A* algorithm search strategy in step 3 is specifically as follows: take the starting point as the parent node, search the information of the eight surrounding child nodes, determine whether there is an obstacle node among the eight child nodes, if there is an obstacle node, mark the obstacle node, mark the upper, lower, left and right grids of the marked child node as infeasible areas, and remove the child nodes that are too close to the obstacles in turn; The specific method of improving the node array to reduce the path nodes in step 4 is as follows: in the planned node array, select any three nodes, connect node one and node three, determine whether the connection line intersects with the obstacle area, if there is an intersection, retain node two, if not, continue to determine the shortest distance between the connection line and the obstacle, if the shortest distance is greater than n times the size of the unmanned boat, remove node two, if less than n times the size of the unmanned boat, retain node two; Among them, n is not less than 4.

2. A method for path planning of an unmanned surface vehicle according to claim 1, characterized in that: The specific method for determining the shortest distance between a line and an obstacle is as follows: save all the nodes that the line passes through, put them into an array, determine whether the array intersects with the obstacle array, mark the obstacles, and determine the distance between the four vertices of each obstacle and the line connecting nodes one and three.

3. A method for path planning of an unmanned surface vehicle according to claim 1, characterized in that: The improved artificial potential field function in step 6 is specifically: Gravitational function The expression is: in, is the gravitational magnitude coefficient, is the gravitational variation range adjustment factor, is the gravitational upper and lower limit adjustment factor, is the Euclidean distance between the current position of the unmanned boat and the target point; Repulsion function The expression is: Among them, K represents the repulsion coefficient, represents the repulsion range adjustment factor, and c represents the Euclidean distance between the dynamic obstacle and the unmanned boat.

4. A method for path planning of an unmanned surface vehicle according to claim 1, characterized in that: The specific method for dynamically avoiding dynamic obstacles in step 6 is: if the angle difference between the unmanned boat and the obstacle is between [0°, 45°] or [315°, 360°), the unmanned boat sails to the left; if the angle between the unmanned boat and the dynamic obstacle is [165°, 195°], the unmanned boat sails to the right; if the angle between the unmanned boat and the obstacle is (45°, 165°), the unmanned boat sails to the right; if the angle between the unmanned boat and the obstacle is (195°, 315°), the unmanned boat sails to the left.

5. A method for path planning of an unmanned surface vehicle according to claim 4, characterized in that: The turning angle of the unmanned boat is less than 60°.

Images