An Unmanned Surface Vehicle Dynamic Path Planning Method and System Based on Improved D* Algorithm

Abstract

The present invention discloses a dynamic path planning method and system for an unmanned boat based on an improved D* algorithm, which is applicable to the technical field of path planning. First, the present invention conducts a rasterized modeling on the sea area map where the unmanned boat is located, clusters the obstacles according to the position coordinates, and then divides the map into regions; then, corresponding obstacle complexity quantization index vectors are established for each region to improve the cost function in the D* algorithm; and the path search behavior of the unmanned boat is combined with the improved D* path planning algorithm for dynamic path planning; finally, the Bezier curve is used for optimization to improve the smoothness of the path. The present invention can solve the problems that it is impossible to obtain all the sea area information and it is difficult to meet the requirements of global optimality and real-time obstacle avoidance when conducting path planning in a complex marine environment in the presence of unknown obstacles, and realizes the dynamic smooth optimal path planning of the unmanned boat.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and particularly to a dynamic path planning method and system for an unmanned surface vehicle based on an improved D-star (D*) algorithm. Background Art

[0002] In the process of the development of the unmanned surface vehicle cluster system towards intelligence, path decision optimization is an important link. Path planning is to plan a path from the starting point to the target point under certain constraint conditions to make the specified performance index optimal. The constraint conditions mainly refer to environmental constraints, task constraints, spatial coupling constraints, temporal coordination constraints, kinematic constraints, etc. Its performance index can include path length, path smoothness, path safety, task completion time, etc. Path decision-making is the key technology to realize the autonomous operation of the unmanned surface vehicle cluster and is an important problem that needs to be solved urgently at present.

[0003] The D* algorithm is a discrete form of dynamic path planning algorithm. It integrates the path planning idea of the Dijkstra algorithm and realizes the avoidance of dynamic obstacles through the dynamic reverse sector search algorithm and reverse backtracking, avoiding the high computational cost of backtracking. Compared with heuristic algorithms such as the genetic algorithm, since the genetic algorithm needs to initialize the population first and then perform selection, crossover, and mutation when searching for a path, and the selection of the population directly affects whether a feasible path can be searched and the optimality of the path, with strong randomness, the genetic algorithm is not suitable for path search in complex marine environments. However, the D* algorithm has high accuracy and high flexibility, can adapt to dynamic obstacles, and has wide application value. At present, the traditional D* algorithm has problems such as many path inflection points, insufficient smoothness of the feasible path, and high computational cost of the cost function, which limit the application of the traditional D* algorithm in the path planning of unmanned surface vehicles in complex marine environments. There is an urgent need to improve it to efficiently complete the dynamic path planning task of unmanned surface vehicles in complex marine environments.

[0004] Aiming at the problem that the unmanned surface vehicle cannot obtain all the sea area information and is difficult to meet the requirements of global optimality and real-time obstacle avoidance when performing path planning in an unknown complex marine environment, it is necessary to further design a safe path planning algorithm with strong real-time performance and high search efficiency in a dynamic environment. Therefore, it is of great significance to use the improved D* algorithm to implement a dynamic path planning method for the unmanned surface vehicle. There is no relevant literature on the research of the dynamic path planning method for the unmanned surface vehicle based on the improved D* algorithm. Summary of the Invention

[0005] Objective of the Invention: Aiming at the deficiencies of the prior art, the objective of the present invention is to provide a dynamic path planning method and system for unmanned boats based on an improved D* algorithm. In the case of unknown obstacles in the environment, by improving the D* path planning algorithm, it solves the problems of being unable to obtain all sea area information and difficult to meet the requirements of global optimality and real-time obstacle avoidance during path planning in a complex marine environment, and realizes dynamic optimal smooth path planning.

[0006] Technical Solution: To achieve the above-mentioned objective of the invention, the present invention adopts the following technical solutions:

[0007] A dynamic path planning method for unmanned boats based on an improved D* algorithm, comprising the following steps:

[0008] Step 1: Perform map modeling on the marine environment where the unmanned boat is located based on the grid method;

[0009] Step 2: Cluster the obstacles according to the position coordinates, and then divide the map into regions;

[0010] Step 3: Construct an obstacle complexity quantization index vector through the random obstacle density P a , the sparsity degree of obstacles P s and the fixed obstacle density P d ;

[0011] Step 4: Improve the algorithm cost function according to the obstacle complexity quantization information, adjust the weight of the heuristic function in the cost function according to the complexity degree of the regional obstacle distribution, improve the directionality of path planning, and avoid the occurrence of locally multi-twisted turning paths; the weight of the area with a low complexity degree of obstacle distribution is higher than that of the area with a high complexity degree;

[0012] Step 5: Use the improved D* algorithm to search for the global optimal path, first perform backward search, and obtain the optimal path after completion; then trace back from the starting point backward. If the state of the next node is forced to change, adjust the cost value and re-search;

[0013] Step 6: Construct a set of feature points in the planned global optimal path and perform smoothing processing to achieve dynamic optimal path planning for the unmanned boat.

[0014] Preferably, in step 2, the hierarchical clustering method is used to divide the map into regions, including:

[0015] Step 2-1: Construct an obstacle coordinate matrix according to the positions of the fixed obstacles, and regard the obstacles represented by each element in the matrix as separate clusters;

[0016] Step 2-2: Calculate the distances between the clusters, and merge the two clusters with the closest distances;

[0017] Step 2-3: Recalculate the distances between the new cluster and all the old clusters;

[0018] Step 2-4: Repeat Step 2-2 and Step 2-3 until the final total number of categories meets the clustering number;

[0019] Step 2-5: Divide the map into corresponding numbers of regions according to the clustering results of the obstacles, and classify the unit grids of the non-fixed obstacles into the nearest clusters.

[0020] Preferably, the random obstacle density P a , the obstacle sparsity degree P s and the fixed obstacle density P d in Step 3 are calculated by the following formulas:

[0021]

[0022]

[0023]

[0024] where n k is the total number of grids in the k-th region, k = 1, …, K, K is the total number of regions, P kj is the probability of random obstacles appearing in the j-th grid in the k-th region; s k is the sum of the number of rows and columns in the k-th region where the probability total value of each row or column is lower than the set threshold, L k is the number of rows in the k-th region, D k is the number of columns in the k-th region; d k is the total number of fixed obstacles in the k-th region.

[0025] Preferably, the calculation method of the cost function in the improved D* algorithm in Step 4 is as follows:

[0026] Let the current grid coordinates be x = (x1, x2), the target point coordinates be g = (g1, g2), the starting point coordinates be s = (s1, s2), and the cost function F k (x) = c k H(x) + K(x), where H(x) is the heuristic function from the current point x to the starting point, used to improve the directionality of the reverse search, and K(x) is the minimum cost estimation function from the target point g to the current point x, determined by the cost estimation function C(x, y), which is the sum of the cost estimation functions between adjacent two nodes on the optimal path formed from the target point g to the current point x;

[0027] C(x, y) = cmin(|x1 - x2|, |y1 - y2|) + ||x1 - x2| - |y1 - y2||,

[0028] Among them, y = (y1, y2) is the parent node of x, and c is a constant value;

[0029] Weight value c k is determined by the quantification index vector of the complexity of obstacles in each area. The greater the difference between the weighted value of the elements in the vector and the set threshold c0, the more complex the obstacle distribution, and the smaller the weight value c k is, and vice versa.

[0030] Preferably, the weight value c k has the following specific expression:

[0031]

[0032] where c0 satisfies Satisfy represents rounding up, represents the normalized value of the i-th element in the quantification index vector of the complexity of obstacles in the k-th area.

[0033] Preferably, the method of using the improved D* algorithm to search for the global optimal path in step 5 is as follows:

[0034] First, establish an OPEN table and a CLOSE table. The OPEN table is used to store search nodes, and the CLOSE table is used to record the key nodes forming the optimal path; then, each time the node with the smallest cost function value is taken out from the OPEN table and moved to the CLOSE table, and its adjacent nodes are added to the OPEN table for re-comparison. After repeatedly calling the PROCESS-STATE function, finally, an optimal path is represented by a back-pointer; when encountering a dynamic obstacle, call the MODIFY-COST function to change the cost between two nodes and place the affected nodes in the OPEN table.

[0035] Preferably, the method of using Bezier smoothing in step 6 is as follows:

[0036] Sort the midpoints between every two turning points on the global planning path and the starting point of the path in ascending order of the abscissa of the position and put them into the feature point set Φ. Then, select two points from the set as the starting position and the target position in turn, and select all the path nodes between the starting position and the target position from the global optimal path as control points for Bezier curve fitting. The point selected as the starting position is deleted from the feature set until Φ is an empty set. The calculation formula of the n-th Bezier curve is as follows:

[0037]

[0038] Among them, u is the internal control parameter of the curve, Denote the combination number, B(0) and B(n) are the initial position and the target position respectively, and B(i) is the coordinate of the i-th control point, where i = 1, …, n - 1.

[0039] Based on the same inventive concept, a dynamic path planning system for an unmanned boat based on an improved D* algorithm provided by the present invention includes:

[0040] A grid modeling module for map modeling of the marine environment where the unmanned boat is located based on the grid method;

[0041] A clustering area division module for clustering obstacles according to position coordinates and then dividing the map into areas;

[0042] An obstacle complexity quantification module for constructing an obstacle complexity quantification index vector through the random obstacle density P a in each area, the sparsity degree P s of obstacles, and the fixed obstacle density P d ;

[0043] A dynamic path planning module for improving the algorithm cost function according to the obstacle complexity quantification information, adjusting the weight of the heuristic function in the cost function according to the complexity of the obstacle distribution in the area to improve the directionality of path planning and avoid local multi-twisting turning paths; the weight of the area with a low obstacle distribution complexity is higher than that of the area with a high complexity;; and using the improved D* algorithm to search for the global optimal path, first performing a backward search, and obtaining the optimal path after the search ends; then tracing backward from the starting point, if the state of the next node is forced to change, adjust the cost value and re-search;

[0044] And a path output module for constructing a set of feature points in the planned global optimal path and performing smoothing processing to achieve the dynamic optimal path planning of the unmanned boat.

[0045] Based on the same inventive concept, a computer device provided by the present invention includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is loaded into the processor, the steps of the dynamic path planning method for an unmanned boat based on the improved D* algorithm are implemented.

[0046] Based on the same inventive concept, a computer-readable storage medium provided by the present invention stores a computer program, and when the computer program is executed by a processor, the steps of the dynamic path planning method for an unmanned boat based on the improved D* algorithm are implemented.

[0047] Advantageous effects: Compared with the prior art, the present invention can achieve the dynamic path planning of an unmanned boat, take into account the obstacle information, and improve the path planning effect in sea areas with different obstacle distributions. Brief Description of the Drawings

[0048] Figure 1 is the overall method flowchart of an embodiment of the present invention.

[0049] Figure 2 is the schematic diagram of the detailed steps of an embodiment of the present invention;

[0050] Figure 3 is the schematic diagram after hierarchical clustering of obstacles in the rasterized map in an embodiment of the present invention;

[0051] Figure 4 is the schematic diagram of the reverse planning path of path planning in an embodiment of the present invention;

[0052] Figure 5 is the schematic diagram of the forward backtracking path when path planning encounters dynamic obstacles in an embodiment of the present invention;

[0053] Figure 6 is the comparison diagram before and after the Bezier smoothing process of the globally optimal path in an embodiment of the present invention;

[0054] Figure 7 is the schematic diagram of the reverse planning path of path planning by the traditional D* algorithm;

[0055] Figure 8 is the schematic diagram of the forward backtracking path when path planning by the traditional D* algorithm encounters dynamic obstacles. Detailed Embodiment

[0056] The invention purpose, technical solution and invention advantages of the present invention will be further described in detail below in conjunction with the drawings and specific embodiments.

[0057] During the path planning process of the unmanned boat, it is necessary to quickly search for the globally optimal path. When the sea area environment is complex and there are unknown obstacles, it is very difficult for traditional path search algorithms to achieve path planning, and even lead to deadlock phenomena. Such as Figure 1As shown in the figure, an unmanned boat dynamic path planning method based on an improved D* algorithm is disclosed in an embodiment of the present invention. First, map modeling is performed on the ocean environment where the unmanned boat is located based on the grid method. Then, obstacles are clustered according to the position coordinates, and then the map is divided into regions. An obstacle complexity quantification index vector is constructed through the random obstacle density, obstacle sparsity, and fixed obstacle density in each region. Next, the cost function of the algorithm is improved according to the obstacle complexity quantification information, and the weight of the heuristic function in the cost function is adjusted according to the complexity of the regional obstacle distribution, improving the directionality of path planning and avoiding local multi-twisting turning paths. Then, the improved D* algorithm is used to search for the global optimal path. First, a backward search is performed, and the optimal path is obtained after the search ends. Then, starting from the starting point, trace backward. If the state of the next node is forced to change, adjust the cost value and search again. Finally, a set of feature points in the planned global optimal path is constructed and smoothed to achieve the dynamic optimal path planning of the unmanned boat. The present invention integrates environmental obstacle information into the evaluation function, improves the D* algorithm, realizes different search methods in different map regions, improves the flexibility of the algorithm, and path smoothing can reduce large turning of the unmanned boat, improving the execution efficiency.

[0058] The following combines Figure 2 to detail the specific steps of an unmanned boat dynamic path planning method based on an improved D* algorithm disclosed in an embodiment of the present invention:

[0059] Step 1: Perform map modeling on the ocean environment where the unmanned boat is located based on the grid method.

[0060] Step 1-1: Grid the map, establish a coordinate system with the path starting point as the origin, and assign a value to each unit grid according to the probability of obstacles appearing in the unit grid. The value range is [0, 1].

[0061] Step 2: Use the hierarchical clustering method to classify obstacles according to the position coordinates, and then realize the regional division of the map. As Figure 3 shown, the obstacles are divided into four categories according to the position coordinates, and the obstacles in the same category are represented by the same shape.

[0062] Step 2-1: Construct an obstacle coordinate matrix P according to the positions of each fixed obstacle (an obstacle with an obstacle appearance probability value of 1 is regarded as a fixed obstacle) x,y , and regard the obstacles represented by each element in P x,y as separate clusters, and set the number of clusters.

[0063] P x,y The specific expression of is:

[0064]

[0065] Among them, (x n , y n ) is the position of the nth fixed obstacle, and n is the total number of fixed obstacles.

[0066] Step 2-2: Define the inter-cluster distance as the infimum of the distances between all obstacles within two clusters. Then calculate the distance between each pair of clusters, and merge the two clusters with the closest distance, reducing the total number of classes by one.

[0067] Step 2-3: Recalculate the distances between the new cluster and all the old clusters.

[0068] Step 2-4: Repeat Steps 2-2 and 2-3 until the final total number of classes meets the clustering number, and output the classification result.

[0069] Step 2-5: According to the clustering result of the obstacles, divide the map into the corresponding number of regions, and assign the unit grids of non-fixed obstacles to the closest cluster.

[0070] Step 3: Construct a quantization index vector of obstacle complexity through the random obstacle density P a , the obstacle sparsity degree P s , and the fixed obstacle density P d , and perform normalization processing.

[0071] Step 3-1: Establish a corresponding quantization index vector P k of obstacle complexity according to the obstacle complexity of each region. The specific expression is: P k = (P a , P s , P d ), and the calculation formula is as follows:

[0072]

[0073]

[0074]

[0075] Among them, n k is the total number of grid cells in the kth region, k = 1,..., K, where K is the total number of regions, P kj is the occurrence probability of random obstacles (0 < P kj < 1) in the jth grid cell of the kth region; s k is the sum of the number of rows and columns in the kth region where the total probability of each row or column is lower than the set threshold, L k is the number of rows in the kth region, D k is the number of columns in the kth region; d k is the total number of fixed obstacles in the kth region.

[0076] Step 3-2: Quantify the obstacle complexity metric vector P k After normalization, we get The specific expression is as follows:

[0077]

[0078] Where is the i-th element of P k in, min{P ki} is the minimum value among the i-th elements corresponding to all the obstacle complexity metric vectors of the regions, and max{P ki} is the maximum value among the i-th elements corresponding to all the obstacle complexity metric vectors of the regions. is the i-th element of.

[0079] Step 4: Improve the design of the algorithm cost function according to the obstacle complexity quantization information. If the obstacle distribution in the region is simple (low complexity), increase the weight of the heuristic function to improve the directionality of path planning and avoid local multi-winding turning paths; on the contrary, if the obstacle distribution in the region is complex (high complexity), reduce the corresponding weight to avoid problems such as excessive planning time.

[0080] Denote the current grid coordinate as x = (x1, x2), the target point coordinate as g = (g1, g2), the starting point coordinate as s = (s1, s2), and the cost function F k (x) = c k H(x) + K(x), where H(x) is the heuristic function from the current point x to the starting point, used to improve the directionality of backward search, and K(x) is the minimum cost estimation function from the target point g to the current point x, determined by the cost estimation function C(x, y), which is the sum of the cost estimation functions between adjacent two nodes on the optimal path formed from the target point g to the current point x;

[0081] C(x, y) = cmin(|x1 - x2|, |y1 - y2|) + ||x1 - x2| - |y1 - y2||,

[0082] where y = (y1, y2) is the parent node of x, c is a constant value, which can take the value of 2.5 in this example; the weight c k is determined by the obstacle complexity metric vector of each region. The greater the difference between the weighted value of the element in the vector and the set threshold c0, the more complex the obstacle distribution, and the smaller the weight c k is, and vice versa; a decreasing function can be used. In this example, the weight c k can adopt the following specific expression:

[0083]

[0084] where c0 satisfies satisfies

[0085] Step 5: Use the improved D* algorithm to search for the global optimal path. First, perform a backward search. After completion, obtain the optimal path, as Figure 4 shown; then trace backward from the starting point. If the state of the next node is forced to change, adjust the stored key function value and re-search, finally achieving dynamic path planning, as Figure 5 shown.

[0086] Step 5-1: Initialize the tag value of all nodes as New. Let K(x) of the target point g be 0, and put the target point into the OPEN list.

[0087] Step 5-2: Repeatedly call PROCESS-STATE, and put the point with the minimum cost function into the CLOSE list until the starting point is removed from the OPEN list, generating the shortest path sequence.

[0088] Step 5-3: Travel along the back-pointer of the path sequence. When encountering a suddenly emerging obstacle, call MODIFY-COST to update the F(x) value of the corresponding node, and put the affected nodes into the OPEN list.

[0089] Step 5-4: Call PROCESS-STATE, and put the point with the minimum cost value into the CLOSE list, generating the local optimal path sequence.

[0090] Step 5-5: Continue to backtrack the path along the back-pointer of the remaining optimal path sequence, and repeat Step 3 to Step 4 until reaching the target point.

[0091] Step 5-6: Output the optimal path according to the nodes recorded in the CLOSE list.

[0092] Step 6: Construct the set of feature points in the planned global optimal path and perform Bezier smoothing to achieve the dynamic optimal path planning of the unmanned boat, as Figure 6 shown, where the solid line represents the globally optimal path after smoothing.

[0093] Step 6-1: Sort the midpoints between every two turning points on the global planning path and the starting point of the path in ascending order of the abscissa of the position, and put them into the feature point set Φ. Then, select two points from the set as the starting position and the target position in turn. Select all the path nodes between the starting position and the target position from the CLOSE table as control points for Bezier curve fitting. The point selected as the starting position is deleted from the feature set until Φ is an empty set. The formula for the nth Bezier curve is: u is the internal control parameter of the curve, represents the combination number, B(0) and B(n) are the initial position and the target position respectively, and B(i) is the coordinate of the ith control point, where i = 1,…,n-1.

[0094] In addition, in the embodiment of the present invention, the search paths of the improved algorithm and the traditional D* algorithm on the same map are compared. The coordinate (0,0) is the starting point, represented by a triangle, the coordinate (20,20) is the end point, represented by a circle, and the coordinates (3,7) and (4,7) are the positions where dynamic obstacles appear. The path searched backward by the traditional D* algorithm, as Figure 7 shown, and the path traced forward by the traditional D* algorithm, as Figure 8 shown. By comparing Figure 4 and Figure 7 it can be seen that the improved D* algorithm can eliminate unnecessary inflection points in the original backward search path. By comparing Figure 5 and Figure 8 it can be seen that both the improved D* algorithm and the traditional algorithm can cope with the appearance of unknown obstacles, and the improved algorithm can also eliminate unnecessary inflection points in the forward trace-back path. As Figure 6 shown, the dotted line represents the global path after dynamic search, and the solid line is the result after Bezier smoothing of the original path. It can be seen that all right-angled inflection points are smoothed, reducing the difficulty of the unmanned boat traveling along the planned path, and having practical value.

[0095] Based on the same inventive concept, a dynamic path planning system for an unmanned boat based on an improved D* algorithm disclosed in the embodiment of the present invention includes: a grid modeling module for performing map modeling on the marine environment where the unmanned boat is located based on the grid method; a clustering area division module for clustering obstacles according to position coordinates and then dividing the map into areas; an obstacle complexity quantification module for quantifying through the random obstacle density P a in each area, the sparsity degree P s of obstacles, and the fixed obstacle density P dConstruct an obstacle complexity quantification index vector; a dynamic path planning module, which is used to improve the algorithm cost function according to the obstacle complexity quantification information, adjust the weight of the heuristic function in the cost function according to the complexity of the regional obstacle distribution, improve the directionality of path planning, and avoid local multi-twisting turning paths; the weight of the area with a low obstacle distribution complexity is higher than that of the area with a high complexity;; and use the improved D* algorithm to search for the global optimal path, first perform a reverse search, and obtain the optimal path after completion; then trace back from the starting point backward, if the state of the next node is forced to change, adjust the cost value and re-search; and a path output module, which is used to construct a set of feature points in the planned global optimal path and perform smoothing processing to achieve the dynamic optimal path planning of the unmanned boat.

[0096] For the specific working processes of the above-described modules, reference can be made to the corresponding processes in the foregoing method embodiments, and details are not described herein again. The division of the modules is only a logical function division, and there may be other division methods in actual implementation. For example, multiple modules can be combined or integrated into another system.

[0097] Based on the same inventive concept, a computer device disclosed in an embodiment of the present invention includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is loaded into the processor, the steps of the method for dynamic path planning of an unmanned boat based on the improved D* algorithm are implemented.

[0098] Based on the same inventive concept, a computer-readable storage medium disclosed in an embodiment of the present invention stores a computer program, and when the computer program is executed by a processor, the steps of the method for dynamic path planning of an unmanned boat based on the improved D* algorithm are implemented.

[0099] Those skilled in the art can understand that the technical solution of the present invention, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer system (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in the embodiments of the present invention. The storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memory ROM, random access memory RAM, magnetic disks, or optical discs that can store computer programs.

[0100] The above are only the preferred embodiments of the present invention. It should be noted that the above embodiments do not limit the present invention. Various changes and modifications made by relevant workers within the scope not departing from the technical idea of the present invention all fall within the protection scope of the present invention.

Claims

1. A dynamic path planning method for unmanned boats based on an improved D* algorithm, characterized in that: It includes the following steps: Step 1: Perform map modeling on the ocean environment where the unmanned boat is located based on the grid method; Step 2: Cluster the obstacles according to the position coordinates, and then divide the map into regions; Among them, the hierarchical clustering method is used to divide the map into regions, including: Step 2-1: Construct an obstacle coordinate matrix according to the positions of the fixed obstacles, and regard the obstacles represented by each element in the matrix as separate clusters; Step 2-2: Calculate the distances between the clusters, and merge the two closest clusters; Step 2-3: Recalculate the distances between the new cluster and all the old clusters; Step 2-4: Repeat Step 2-2 and Step 2-3 until the final total number of classes meets the clustering number; Step 2-5: Divide the map into the corresponding number of regions according to the clustering results of the obstacles, and classify the unit grids of the non-fixed obstacles into the closest cluster; Step 3: Construct a quantization index vector of obstacle complexity through the random obstacle density P in each area a , the sparsity degree P of obstacles s , and the fixed obstacle density P d ; Step 4: Improve the algorithm cost function according to the obstacle complexity quantization information, adjust the weight of the heuristic function in the cost function according to the complexity of the regional obstacle distribution, improve the path planning directionality, and avoid the appearance of local multi-twisting turning paths; the weight of the region with low obstacle distribution complexity is higher than that of the region with high complexity; Step 5: Use the improved D* algorithm to search for the global optimal path. First, perform a backward search, and obtain the optimal path after completion; then trace back from the starting point. If the state of the next node is forced to change, adjust the cost value and re-search; Step 6: Construct a set of feature points in the planned global optimal path and perform smoothing processing to achieve the dynamic optimal path planning of the unmanned boat.

2. The dynamic path planning method for an unmanned boat based on the improved D* algorithm according to claim 1, characterized in that: The random obstacle density P in each area in Step 3 a , the obstacle sparsity degree P s and the fixed obstacle density P d The calculation formulas are as follows: where n k is the total number of grids in the k-th area, k = 1, …, K, where K is the total number of areas, and P kj is the probability of a random obstacle appearing in the j-th grid in the k-th area; s k is the sum of the number of rows and columns in the k-th area where the total probability value of each row or column is lower than a set threshold, and L k is the number of rows in the k-th area, and D k is the number of columns in the k-th area; d k is the total number of fixed obstacles in the k-th area.

3. The dynamic path planning method for an unmanned boat based on the improved D* algorithm according to claim 1, characterized in that: The calculation method of the cost function in the improved D* algorithm in Step 4 is: Let the current grid coordinates be \(x=(x_1,x_2)\), the target point coordinates be \(g=(g_1,g_2)\), the starting point coordinates be \(s=(s_1,s_2)\), and the cost function \(F\) k (x)=c k \(H(x)+K(x)\), where \(H(x)\) is the heuristic function from the current point \(x\) to the starting point, used to improve the directionality of the reverse search, and \(K(x)\) is the minimum cost estimation function from the target point \(g\) to the current point \(x\), determined by the cost estimation function \(C(x,y)\), which is the sum of the cost estimation functions between adjacent two nodes on the optimal path formed from the target point \(g\) to the current point \(x\); C(x,y) = cmin(|x1 - x2|,|y1 - y2|)+||x1 - x2|-|y1 - y2||, where y = (y1,y2) is the parent node of x, and c is a constant value; Weight value c k It is determined by the quantization index vector of the obstacle complexity in each area. The greater the difference between the weighted value of the elements in the vector and the set threshold c0, the more complex the obstacle distribution, and the weight value c k is smaller, and vice versa.

4. The unmanned boat dynamic path planning method based on the improved D* algorithm according to claim 3, characterized in that: Weight value c k The specific expression is as follows: where c0 satisfies satisfies denotes rounding up, denotes the normalized value of the i-th element in the k-th regional obstacle complexity quantization index vector.

5. The method for dynamic path planning of an unmanned boat based on the improved D* algorithm according to claim 1, wherein: The method of using the improved D* algorithm to search for the global optimal path in Step 5 is: First, establish an OPEN table and a CLOSE table. The OPEN table is used to store the search nodes, and the CLOSE table is used to record the key nodes forming the optimal path; then, each time the node with the smallest cost function value is taken out from the OPEN table and moved to the CLOSE table, and its adjacent nodes are added to the OPEN table for re-comparison. After repeatedly calling the PROCESS-STATE function, finally, a back-pointer is used to represent an optimal path; when encountering a dynamic obstacle, call the MODIFY-COST function to change the cost between two nodes and place the affected nodes in the OPEN table.

6. The method for dynamic path planning of an unmanned boat based on the improved D* algorithm according to claim 1, characterized in that: In Step 6, Bezier smoothing processing is used, and the specific method is: Sort the midpoints between every two turning points on the global planned path and the starting point of the path in ascending order of the abscissa of the positions, and put them into the feature point set Φ. Select two points from the set as the starting position and the target position in turn, select all the path nodes between the starting position and the target position from the global optimal path as control points for Bezier curve fitting, and delete the point selected as the starting position from the feature set until Φ is an empty set. The calculation formula of the nth-degree Bezier curve is as follows: where u is the internal control parameter of the curve, represents the combination number, B(0) and B(n) are the initial position and the target position respectively, and B(i) is the coordinate of the i-th control point, where i = 1, …, n - 1.

7. An unmanned surface vehicle dynamic path planning system based on an improved D* algorithm, characterized in that: Including: A grid modeling module for map modeling of the ocean environment where the unmanned boat is located based on the grid method; A clustering area division module for clustering obstacles according to the position coordinates and then dividing the map into areas; Among them, the map is divided into areas by using the hierarchical clustering method, including: Step 2-1: Construct an obstacle coordinate matrix according to the positions of each fixed obstacle, and regard the obstacles represented by each element in the matrix as separate clusters; Step 2-2: Calculate the distances between each cluster, and merge the two clusters with the closest distance; Step 2-3: Recalculate the distances between the new cluster and all the old clusters; Step 2-4: Repeat Step 2-2 and Step 2-3 until the final total number of classes meets the clustering number; Step 2-5: According to the clustering results of the obstacles, divide the map into the corresponding number of areas, and incorporate the unit grids of non-fixed obstacles into the closest cluster; An obstacle complexity quantification module for constructing an obstacle complexity quantification index vector through the random obstacle density P of each region a , the sparsity degree P of obstacles s , and the fixed obstacle density P d ; A dynamic path planning module for improving the algorithm cost function according to the obstacle complexity quantization information, adjusting the weight of the heuristic function in the cost function according to the complexity of the regional obstacle distribution, improving the directionality of path planning, and avoiding the occurrence of local multi-twisted turning paths; the weight of the area with a low obstacle distribution complexity is higher than that of the area with a high complexity; and use the improved D* algorithm to search for the global optimal path, first perform a backward search, and obtain the optimal path after completion; then trace back from the starting point backward, and if the state of the next node is forced to change, adjust the cost value and search again; And a path output module for constructing a feature point set in the planned global optimal path and performing smoothing processing to achieve the dynamic optimal path planning of the unmanned boat.

8. A computer device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the computer program is loaded into the processor, it realizes the steps of the unmanned boat dynamic path planning method based on the improved D* algorithm according to any one of claims 1-6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it realizes the steps of the unmanned boat dynamic path planning method based on the improved D* algorithm according to any one of claims 1-6.

Images