A global safe path planning method for unmanned surface vessels

Abstract

This invention belongs to the field of unmanned surface vessel (USV) technology and discloses a global safe path planning method for USVs. Specifically, it involves: rasterizing the electronic nautical chart of the starting and ending points to obtain a raster map; generating a generalized Voronoi diagram (GVDM) from the raster map to obtain the boundary grids of the GVDM; constructing a risk assessment function and then using the risk assessment function to calculate the risk coefficient of each boundary grid; constructing a true cost function and employing an improved Theta* algorithm for global path planning. This invention's global path planning method not only ensures sufficient shortest distances to obstacles, significantly improving safety, but also avoids dangerous passages by introducing penalty costs, while reducing path inflection points and length, thus planning a global path that combines efficiency and safety.

Description

Technical Field

[0001] This invention belongs to the field of unmanned surface vessel technology, specifically relating to a global safe path planning method for unmanned surface vessels. Background Technology

[0002] Unmanned surface vehicles (USVs) are intelligent devices that operate autonomously without human intervention. In recent years, USVs have been widely used in both military and civilian applications. USVs perform a variety of important tasks at sea, including environmental monitoring, disaster management, deep-sea mapping, search and rescue, warfare, and patrol. Global planning, crucial for autonomous navigation, aims to find a collision-free optimal path from start to finish. However, path length and safety are often contradictory during planning, especially in complex scenarios where nautical charts include open sea areas and narrow waterways. Therefore, planning a global path that balances safety and efficiency is of paramount importance.

[0003] Theta* is an eight-neighbor search algorithm, an improved version of A*, which differs from the original algorithm in its parent node selection method. Therefore, the path planned by Theta* can be at any angle, no longer limited to fixed, finite directions, resulting in shorter path lengths, fewer turns and nodes, and thus more favorable path results for unmanned surface vessels (USVs) to track. Due to its superior path performance, the Theta* algorithm has found increasing application in the field of global path planning.

[0004] However, in practical planning, safety is just as crucial as path length. For the global path planning needs of unmanned surface vessels (USVs), they need to navigate safely through dangerous narrow passages while also quickly traversing safe open areas. Traditional Theta* algorithms treat the cost between grid cells as uniform, but for complex nautical chart environments, map costs are often non-uniform, making traditional Theta* algorithms unsuitable for complex map environments. Furthermore, path length and safety are often contradictory metrics; balancing these two aspects is particularly important for USVs to reach their destination efficiently and safely. Summary of the Invention

[0005] In view of the problems and shortcomings of the existing technology, the purpose of this invention is to provide a global safe path planning method for unmanned surface vessels.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] The first aspect of this invention provides a global safe path planning method for unmanned surface vessels, comprising the following steps:

[0008] S100, rasterize the electronic nautical chart of the starting and ending points of the site to obtain a raster map;

[0009] S200, Generate a generalized Voronoi map from the raster map, and obtain the boundary raster of the generalized Voronoi map;

[0010] S300, construct a risk assessment function, and then use the risk assessment function to calculate the risk coefficient of each boundary grid;

[0011] S400 constructs a true cost function by combining risk coefficients and uses an improved Theta* algorithm for global safe path planning.

[0012] More preferably, the starting and ending point sites are sites that include a passable starting point and a passable ending point for the unmanned surface vessel. The global safe path planning method of this invention is applicable to global safe path planning for unmanned surface vessels.

[0013] Preferably, the step S100 of rasterizing the electronic nautical chart of the starting and ending points specifically involves: performing a discrete rasterization operation on the electronic nautical chart of the starting and ending points, dividing the electronic nautical chart into grids with binary information, and defining the boundaries of the electronic nautical chart of the starting and ending points as unreachable by unmanned surface vessels (USVs); the binary information is the assignment information for each grid, specifically: if the USV can reach the grid, it is assigned a value of 0; if it cannot reach the grid, it is assigned a value of 1. More preferably, the present invention uses a scanning polygon filling algorithm to perform discrete rasterization on the electronic nautical chart of the starting and ending points.

[0014] More preferably, the electronic nautical chart of the starting and ending points is an electronic nautical chart containing actual environmental information of the starting and ending points; the boundary of the electronic nautical chart of the starting and ending points is rectangular.

[0015] Preferably, the specific steps in step S200 for generating the generalized Voronoi map from the raster map are as follows:

[0016] S201, according to the different obstacles or the boundaries of the electronic nautical chart of the starting and ending points, assign different attribute labels to each grid with a value of 1 to obtain grids with attribute labels; use each grid with attribute labels as the expansion occurrence point; then proceed to the next step;

[0017] S202, traverse each expansion point and perform expansion operations to obtain multiple expanded grids; then proceed to the next step;

[0018] S203, determine whether each extended grid has an attribute label different from the grid that is the point of extension: if so, mark the extended grid as a boundary grid and proceed to the next step; otherwise, mark the extended grid with the same attribute label as the grid that is the point of extension, and mark the extended grid as a new point of extension and return to step S202.

[0019] S204, the set of all boundary grids constitutes the generalized Voronoi diagram.

[0020] More preferably, the boundary of the starting and ending point site electronic nautical chart is a rectangle; in step S200, when generating the generalized Voronoi map, the four sides of the rectangular boundary are used as four different boundaries and four different attribute labels are applied.

[0021] More preferably, the expansion operation is to expand one grid in each of the four directions (up, down, left, and right) with the grid at the expansion point as the center, as the expansion grid.

[0022] Preferably, the specific expression of the risk assessment function is:

[0023]

[0024] In the formula, λ is a scaling factor with a value of [0,1], set according to the path planning target preference; d is the nearest distance between the boundary grid and the obstacle; d m The danger distance threshold is set based on the path planning target preference; d n To customize the safe distance threshold, set it according to the path planning target preference.

[0025] More preferably, the above construction process is an offline process, that is, for the same map, only one extraction and calculation is needed, and multiple planning can be carried out.

[0026] It should be noted that the traditional Theta* algorithm is an eight-neighborhood search algorithm. When node n expands its child node n', the algorithm checks if there are obstacles between the child node and the current node's parent node n. If no obstacles exist, the child node and the current node share a parent node. Traditional Theta* uses the Euclidean distance between two points as the step size cost and selects child nodes and the shortest path based on this. However, the traditional Theta* algorithm treats the cost between each grid cell as uniform, but in complex nautical chart environments, map costs are often non-uniform. Under non-uniform cost maps, the total cost of the traditional Theta* algorithm may not be minimized. Furthermore, considering security factors, the traditional Theta* algorithm is not suitable for complex map environments. Therefore, this application improves the traditional Theta* algorithm by using path cost as one of the indicators for parent node selection: when a child node and the current node's parent node meet the visibility condition, the path cost is calculated first, and simultaneously, the cost of using the current node as the parent node is calculated. Then, the expansion is performed using the method with the smaller total cost.

[0027] Furthermore, the evaluation function expression of the traditional Theta* algorithm is: F(n) = G(n) + H(n); where F(n) is the evaluation function, calculating the total cost; G(n) is the true cost function, calculating the true cost from the starting point to the node; and H(n) is the heuristic function, calculating the heuristic cost from the node to the destination. This application uses a non-uniform cost to construct the true cost function of the evaluation function.

[0028] Preferably, the specific expression of the true cost function constructed in this application is as follows:

[0029] G(n)=G(n p )+G(n p ,n)

[0030] In the formula, n p Let S be the parent node of node n; G(n) is the total cost from the starting node S to node n; G(n) is the parent node of node n. p ) represents the path from the starting node S to the parent node n of node n. p The cumulative cost; G(n) p (n) represents the parent node of node n. p The incremental cost to node n is a non-uniform cost;

[0031] in,

[0032] In the formula, N is the parent node of node n. p The number of grid cells traversed to reach node n, d i The parent node of node n p The path length in the i-th grid cell passed through on the way to node n; D i The risk coefficient of the boundary grid cell closest to the i-th grid cell; dist i C is the shortest distance between the i-th grid cell and its nearest boundary grid cell; C(n) is the parent node n of node n. p The penalty cost to node n is a constant set according to the path planning objective preference.

[0033] Preferably, based on the parent node n of node n p Depending on the number of grid cells the path to node n crosses, the calculation of the path length in the i-th grid cell can be performed in the following four ways:

[0034] Case 1: When node n is in the parent node n of node n p When the 8-neighbor grid is adjacent, the parent node of node n is n p If the path to node n intersects with the grid edge at only one point P, and the path does not completely pass through two grids, then the path length has two segments: one segment is from the intersection point P to the parent node n of node n. p The distance is one segment; the other segment is the distance from the intersection point P to node n;

[0035] Scenario 2: When node n is not a parent node of node n p When the 8-neighbor grid is adjacent, the parent node of node n is n p The path to node n intersects with the first grid cell it passes through at two points, P1 and P2. At this point, the path length has two segments: one segment is from intersection P1 to the parent node n of node n. p The distance is one segment; the other segment is the distance between the two intersection points P1 and P2.

[0036] Scenario 3: When node n is not a parent node of node n. p When the 8-neighbor grid is adjacent, the parent node of node n is n p The path to node n intersects with the 2nd to N-1th grid cells at two points P1 and P2. In this case, the path length is the distance between the two intersection points P1 and P2.

[0037] Scenario 4: When node n is not a parent node of node n p When the 8-neighbor grid is adjacent, the parent node of node n is n p If the path to node n intersects the grid containing node n at only one point P, and the path does not completely pass through the grid containing node n, then the path length is the distance from the intersection point P to node n.

[0038] Furthermore, in this invention, the current node, child node, and parent node are all grid center points.

[0039] More preferably, the G(n) p The cost G(n) is non-uniform, and the true cost G(n) is also non-uniform. Further, based on the coordinates of the center point of the i-th grid, the nearest boundary grid to the i-th grid is obtained using a quartering method; combined with the risk coefficient contained in the nearest boundary grid, the length evaluation cost in the true cost function is calculated to obtain the total cost.

[0040] Preferably, the penalty cost is a safety penalty cost and / or a turning penalty cost. A larger penalty cost indicates a greater weight of the penalty cost in the total cost, resulting in fewer penalty categories in the path result. Furthermore, since the penalty cost includes two types of values—safety penalty cost and turning penalty cost—the type of penalty cost included in the true cost function needs to be determined. Specifically, the determination condition is: when the nearest boundary grid cell of the i-th grid is d ≤ di, the nearest distance d to the obstacle is di. mWhen the path turns at a visible point, the penalty cost includes a safety penalty cost, meaning the actual cost is increased by a constant safety penalty cost. Similarly, when the path turns at a visible point, the penalty cost includes a turning penalty cost, meaning the actual cost is increased by a constant turning penalty cost. That is, when the nearest boundary grid cell of the i-th grid has a minimum distance d > di to the obstacle... m When the path does not turn at a visible point, the actual cost does not include the safety penalty cost; when the path does not turn at a visible point, the actual cost does not include the turning penalty cost.

[0041] Preferably, this invention improves the visibility inspection mechanism of the traditional Theta* algorithm. The specific steps of this invention for global path planning using the improved Theta* algorithm are as follows:

[0042] S401, mark the start node S and end node G of the unmanned surface vessel on the grid map, create an OPEN list and a CLOSE list, and initialize the OPEN list and CLOSE list to be empty, and store the start node S into the OPEN list.

[0043] S402, check if the OPEN list is empty. If it is empty, the pathfinding has failed and the search is terminated. Otherwise, take the node with the minimum total cost in the OPEN list as the current node and proceed to the next step.

[0044] S403, determine if the current node is the termination node G. If it is, the path finding is successful. Backtrack the parent node of the current node until the starting node S, which is the path, and the search ends; otherwise, proceed to the next step.

[0045] S404, traverse all child nodes of the current node's neighborhood and perform the following operations:

[0046] S4041, If ​​the child node represents an obstacle or is already in the CLOSE list, no processing is performed; otherwise, proceed to step S4042.

[0047] S4042, check if the parent node of the current node exists: if it does not exist, record the current node; if it exists, further determine whether the child node and the parent node of the current node are visible. If they are not visible, record the current node; if they are visible, first take the parent node of the current node as the parent node of the child node. Since the path from the parent node of the current node to the child node does not turn, calculate the total cost 1 of the child node without the turning penalty cost. Then, take the current node as the parent node of the child node. Since the path from the parent node of the current node to the child node turns, calculate the total cost 2 of the child node with the turning penalty cost. Record the parent node of the child node corresponding to the smaller value of total cost 1 or total cost 2. Finally, name the recorded node node n. p Then proceed to step S4043;

[0048] S4043, If the child node is not in the OPEN list, add it to the OPEN list and set the parent node of the child node to node n. p Then, record the total cost of the child node (i.e., the smaller of total cost 1 or total cost 2 in step S4042); if the child node is already in the OPEN list, it is necessary to determine whether to proceed through node n. p If the required total cost is lower than the original cost, then change the parent node of the child node to node n. p And update the total cost of the child node; if not, do nothing.

[0049] S405, store the current node in the CLOSE list and delete it from the OPEN list, then return to step S402.

[0050] A second aspect of the present invention provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement any step of the global safe path planning method for unmanned surface vessels as described in the first aspect above.

[0051] A third aspect of the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements any step in the global safe path planning method for unmanned surface vessels as described in the first aspect above.

[0052] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0053] (1) This invention improves upon the traditional Theta* algorithm by using heuristic paths based on generalized Volonoi graphs. It designs risk coefficients for different levels of danger in the map, constructs a non-uniform cost calculation method, and uses cost value as one of the indicators for selecting parent nodes, thereby choosing the optimal path with the smaller total cost. In one embodiment, the path planned by the method of this invention increases the path length by 4.6% compared to the traditional Theta* algorithm, but reduces the deviation from the heuristic path by 65.94%, and has a sufficient shortest distance to obstacles, significantly improving safety.

[0054] (2) The risk assessment function constructed in this invention can be used to identify the environmental characteristics of the grid and assess the grid's safety, using safety as one of the indicators for planning the path, thereby inducing the planned path to converge towards the heuristic path. In one embodiment, the path length and number of nodes of this invention gradually increase with the increase of the set proportional coefficient λ value, but the deviation of the path from the heuristic path decreases, the shortest distance to obstacles increases, and the safety is improved accordingly.

[0055] (3) The present invention also proposes a method for calculating the path length within the grid according to the four cases of path crossing the grid, which facilitates the calculation of the non-uniform cost proposed in the present invention.

[0056] (4) This invention introduces a safety penalty cost based on the risk coefficient of the heuristic path. When a grid is in a narrow passage that is too dangerous, a safety penalty cost is added to the grid, which will guide the system to choose a longer but less expensive (safer) path to bypass the dangerous passage. At the same time, a turning penalty cost is introduced to reduce the number of turning points and the length of the path, thereby planning a global path that is both efficient and safe.

[0057] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0058] Figure 1 This is a flowchart of the path planning method of the present invention;

[0059] Figure 2 This is a generalized Volonoi map extracted from a raster map in an embodiment of the present invention; in the figure, blue grids represent boundary grids (heuristic paths);

[0060] Figure 3 This is the code for the non-uniform cost algorithm in this embodiment of the invention;

[0061] Figure 4 These are four different scenarios when the path of this invention traverses the i-th grid cell; in the figure, orange nodes represent the grid center point, red solid lines represent the path, and yellow nodes represent the intersection of the path and the grid edge.

[0062] Figure 5 This is a schematic diagram comparing the paths when selecting the parent node of a child node in the improved Theta* algorithm in this embodiment of the invention; in the figure, the blue dashed line is path 1 when the child node takes the parent node of the current node as its parent node, and the red solid line is path 2 when the child node takes the current node as its parent node.

[0063] Figure 6 The figure shows the path comparison results under different algorithms in the embodiments of the present invention; in the figure, the blue dashed line represents the path result of the traditional Theta* algorithm, the blue solid line represents the global path planned by the improved Theta* algorithm of the present invention, and the red solid line represents the trajectory of the unmanned surface vessel following the improved path of the present invention.

[0064] Figure 7The following are path comparison results under different scaling factors in the embodiments of the present invention; where a represents scaling factor λ of 0 (yellow solid line) and 0.2 (blue dashed line), b represents scaling factor λ of 0.4 (yellow solid line) and 0.6 (blue dashed line), and c represents scaling factor λ of 0.8 (yellow solid line) and 1 (blue dashed line). Detailed Implementation

[0065] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0066] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0067] Example 1

[0068] This embodiment provides a global safe path planning method for unmanned surface vessels, the process of which is as follows: Figure 1 As shown, the specific steps include the following:

[0069] S100, rasterize the starting and ending point site electronic nautical chart to obtain a raster map.

[0070] The specific steps for creating the rasterized start and end point site electronic nautical chart are as follows:

[0071] (1) Add rectangular boundaries to electronic nautical charts containing actual environmental information of the starting and ending points, use image recognition technology to convert electronic nautical charts with rectangular boundaries into grayscale images, and perform binarization to obtain raster with binary information.

[0072] (2) Add the classification information of the electronic map to the grid. The specific information is: if the unmanned surface vessel can reach the grid, it is assigned a value of 0, and if it cannot reach the grid, it is assigned a value of 1. At the same time, the rectangular boundary is defined as the inaccessible area, that is, the rectangular boundary is assigned a value of 1.

[0073] (3) The final form is as follows Figure 2 The grid map shown has black representing obstacles and white representing feasible areas.

[0074] S200: Generate a generalized Voronoi map from the raster map, and obtain the boundary raster of the generalized Voronoi map.

[0075] During actual operation, the level of danger varies across different areas within the feasible domain of an unmanned surface vessel (USV). It is crucial to effectively identify and avoid hazardous areas. The Generalized Volonoi Map (GVD) method employed in this invention is an environmental segmentation method that divides the entire space into sub-regions, thereby enabling the planning of required paths based on the different characteristics of each sub-region.

[0076] The specific steps for generating a generalized Voronoi map from a raster map are as follows:

[0077] (1) Treat each obstacle and the four rectangular boundaries as independent control subsets. Each control subset has a unique identifier. Store each control subset in set O.

[0078] (2) Determine if set O is empty. If it is empty, the expansion ends and proceed to step (4). If it is not empty, pop the first control subset in set O and expand the elements in the control subset in Manhattan form. If the expanded node was originally in an unexpanded state, store the expanded node information in the new control subset. If the expanded node was originally in an expanded state and belongs to another control subset, mark the expanded node and store it in the boundary set.

[0079] (3) If the new control subset is not empty, add it to the end of set O and return to step (2) for the next iteration.

[0080] (4) The boundary set where the marked elements are located is the generalized Volonoi map under the raster map, and each discretized continuous boundary raster forms the heuristic path.

[0081] The intersection lines of each sub-region are the edges of the generalized Volonoi graph. Extracting the grid points occupied by these edges and marking them in blue yields the following result. Figure 2 As shown in the blue grid, although the edges (heuristic paths) of the generalized Volonoi graph shown in the figure have the property of being farthest from adjacent obstacles, they are not suitable for actual navigation due to the irregularity of the curve and the excessive length of the path.

[0082] S300, construct a risk assessment function, and then use the risk assessment function to calculate the risk coefficient of each boundary grid.

[0083] First, construct the risk assessment function, the specific expression of which is:

[0084]

[0085] In the formula, λ is a proportionality coefficient taking values ​​of [0,1], set according to the path planning target preference; d is the nearest distance between the boundary grid n and the obstacle in the heuristic path; d m The danger distance threshold is set based on the path planning target preference; d nTo customize the safe distance threshold, set it according to the path planning target preference.

[0086] It should be noted that when the boundary grid is in a very narrow channel, i.e., d≤d m At this point, the planned path should closely follow the heuristic path to avoid danger; when the boundary grid is in a relatively narrow passage, i.e., d m <d<d n The closer a node is to the heuristic path, the smaller the additional cost, thus causing the planned path to align with the heuristic path; when a node is in an open area, i.e., d≥d n The planned path no longer needs to closely follow the heuristic path. In addition, the path length varies greatly in this type of area, so the influence of the heuristic path on the planning results should be weakened.

[0087] The risk assessment function can be used to identify the environmental characteristics of the grid and assess its safety, using safety as one of the indicators for planning the path; at the same time, the value of the scaling factor λ determines the distance between the planned path and the heuristic path, that is, the degree of convergence of the paths.

[0088] Then, based on the risk assessment function, the risk coefficient of each boundary grid is calculated and stored.

[0089] Finally, each discrete point is stored in a risk matrix, and this matrix is ​​passed to the pathfinding algorithm.

[0090] The construction process mentioned above is an offline process, meaning that for the same map, only one extraction and calculation are needed before multiple planning operations can be performed, thus not increasing the time complexity of the planning algorithm.

[0091] This invention constructs a risk assessment function, which heuristically stores a corresponding risk coefficient for each node in the path. Combining this assessment function with a planning algorithm can improve path safety.

[0092] S400 optimizes the Theta* algorithm structure by adding a parent node selection method and a penalty factor, and combines the non-uniform cost of the grid for global safe path planning.

[0093] First, the true cost function is constructed by combining the risk coefficient, and its specific expression is as follows:

[0094] G(n)=G(n p )+G(n p ,n)

[0095] In the formula, G(n) is the total cost from the starting node S to node n; G(n p ) represents the distance from the starting node S to the parent node n of the node. p The cumulative cost; G(n) p(n) represents the parent node of the node. p The incremental cost to node n is a non-uniform cost;

[0096] in,

[0097] In the formula, The parent node n of the node p The cost of the path to node n is evaluated, consisting of the path length and the evaluated value; C(n) represents the parent node n of node n. p The penalty cost to node n is a constant set according to the path planning objective preference; N is the parent node of node n. p The number of grid cells traversed to reach node n, d i The parent node n of the node p The path length in the i-th grid cell passed through on the way to node n; D i The risk coefficient of the boundary grid cell closest to the i-th grid cell; dist i It represents the shortest distance between the i-th grid cell and its nearest boundary grid cell.

[0098] Wherein, G(n) p The non-uniform cost G(n) represents the true cost, and the non-uniform cost G(n) is also non-uniform. For complex nautical chart environments, non-uniform cost maps can better represent environmental information. When calculating non-uniform costs, the nearest heuristic path point is obtained using the quartering method based on the position coordinates of each grid point. The path points within the grid are then calculated and accumulated based on the risk coefficient contained in these path points.

[0099] This invention incorporates an evaluation value into the path cost to assess the safety of the path. Thus, while considering path length, safety is also taken as one of the planning indicators, allowing the path to converge towards the heuristic path to varying degrees based on environmental characteristics.

[0100] Furthermore, when calculating path costs, corresponding calculation methods need to be designed for different path types. The pseudocode for the cost calculation method is as follows: Figure 3 The algorithm is shown in the diagram. Therefore, for d in the above formula... i Because of its arbitrariness, after checking that the grid cells traversed by the path are not obstacles, the path should be determined based on the parent node n of the node. p Depending on the number of grid cells the path to node n crosses, the calculation of the path length in the i-th grid cell can be performed in the following four ways:

[0101] Case 1: When node n is the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects the grid edge at only one point P, and the path does not completely pass through both grids. See details. Figure 4In (a), the path length has two segments: one segment is from the intersection point P to the parent node n of the node. p The distance n1P is the distance from the intersection point P to node n; the distance Pn3 is the distance from the intersection point P to node n.

[0102] Scenario 2, when node n is the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects with the first grid cell it passes through at two points, P1 and P2, as detailed below. Figure 4 In (b), the path length has two segments: one segment is from the intersection point P1 to the parent node n of the node. p The distance n1P1 is the distance between the two intersection points P1 and P2; the other segment is the distance P1P2 between the two intersection points P1 and P2.

[0103] Scenario 3: When node n is the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects the grid containing node n at only one point P, and the path does not completely pass through the grid containing node n. See details. Figure 4 In (c), at this point, the path length is the distance Pn2 from the intersection point P to the node n;

[0104] Scenario 4: When node n is the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects with the 2nd to N-1th traversed grid cells at two points, P1 and P2, as detailed below. Figure 4 In the middle (d), the path length is the distance P1P2 between the two intersection points P1 and P2.

[0105] By combining the path length within the grid obtained by this method with the non-uniform cost calculation method in the above formula, the cost of a single grid can be obtained.

[0106] Furthermore, when calculating the non-uniform cost of the raster, an additional penalty is applied to the raster cost based on the risk coefficient of the nearest heuristic path point, using a safety penalty cost.

[0107] The penalty cost C(n) represents the safety penalty cost and / or the turning penalty cost. A higher penalty cost indicates a greater weight of the penalty item in the planning process, resulting in a higher cost within narrow passages. Consequently, the system will choose a longer but lower-cost (safer) path as the optimal path, leading to fewer penalty categories in the resulting path. Specifically, the safety penalty cost is an additional constant penalty cost added when the risk coefficient value in the heuristic path corresponds to an excessively narrow passage; the turning penalty cost is an additional constant penalty cost added when the path turns at a visible point. By adding penalty factors for inflection points and dangerous areas, the number of path inflection points is reduced, and the path is made to avoid overly dangerous narrow passages, thus planning a global path that combines efficiency and safety.

[0108] By combining the path length within the grid obtained using this method with the non-uniform cost calculation method in the above formula, and summing the costs of all grids, the non-uniform cost of the path can be obtained. Under this calculation method, since the cost of the path in a narrow channel will be higher, the system will choose a longer but lower-cost (safer) path as the optimal path, thereby bypassing overly dangerous areas.

[0109] Then, a modified Theta* algorithm is used for global path planning, specifically:

[0110] S401, mark the starting node S and ending node G of the unmanned surface vessel on the grid map, create an OPEN list and a CLOSE list, and initialize the OPEN list and CLOSE list to be empty. The OPEN list is sorted in ascending order of path cost, and the starting node S is stored in the OPEN list.

[0111] S402, check if the OPEN list is empty. If it is empty, the pathfinding has failed and the search is terminated. Otherwise, take the node with the minimum total cost in the OPEN list as the current node and proceed to the next step.

[0112] S403, determine if the current node is the termination node G. If it is, the path finding is successful. Backtrack the parent node of the current node until the starting node S, which is the path, and the search ends; otherwise, proceed to the next step.

[0113] S404, traverse the child nodes in the 8-neighborhood of the current node and perform the following operations:

[0114] S4041, If ​​the child node represents an obstacle or is already in the CLOSE list, no processing is performed; otherwise, proceed to step S4042.

[0115] S4042, check if the parent node of the current node exists: if it does not exist, record the current node; if it exists, further determine whether the child node and the parent node of the current node are visible. If they are not visible, record the current node; if they are visible, first take the parent node of the current node as the parent node of the child node. Since the path from the parent node of the current node to the child node does not turn, calculate the total cost 1 of the child node without the turning penalty cost. Then, take the current node as the parent node of the child node. Since the path from the parent node of the current node to the child node turns, calculate the total cost 2 of the child node with the turning penalty cost. Record the parent node of the child node corresponding to the smaller value of total cost 1 or total cost 2. Finally, name the recorded node node n. p Then proceed to step S4043;

[0116] S4043, If the child node is not in the OPEN list, add it to the OPEN list and set the parent node of the child node to node n. p Then, record the total cost of the child node (i.e., the smaller of total cost 1 or total cost 2 in step S4042); if the child node is already in the OPEN list, it is necessary to determine whether to proceed through node n. p If the required total cost is lower than the original cost, then change the parent node of the child node to node n. p The total cost and length evaluation cost of the child nodes are updated and placed into the OPEN list for reordering; otherwise, no action is taken.

[0117] S405, store the current node in the CLOSE list and delete it from the OPEN list, then return to step S402.

[0118] Because the traditional Theta* algorithm focuses on constructing the shortest path, child nodes only choose a parent node when the route encounters an obstacle. While this method maximizes the shortest path, it may not minimize the total cost in a non-uniform cost map.

[0119] To this end, the present invention improves the visibility determination process: in order to reduce path cost, the path cost should be used as one of the indicators for selecting the parent node. When the child node and the parent node of the current node meet the visibility condition, the path cost is calculated first, and the cost of using the current node as the parent node is assumed. Then, the method with the smaller total cost is selected for expansion.

[0120] Figure 5The blue dashed path 1 represents the path when the adjacent nodes take the current node's parent node as their parent node, while the red solid path 2 represents the path when the current node takes the current node as its parent node. Although the improved Theta* algorithm produces a longer path, under a non-uniform cost map, the total cost value of this path is smaller than that of the traditional Theta* path.

[0121] The improved Theta* algorithm assumes the current node as the parent node and calculates the route cost when selecting a parent node. However, using the current node as the parent node implies that the path may have corners, so an additional penalty cost should be added to reduce this. This cost is compared with the path cost of the original Theta* algorithm, and the route with the lower cost is selected as the optimal path. For example, when planning path 2, the non-uniform cost of the path will be supplemented with a constant turning penalty cost. The larger the penalty value, the greater the weight of the penalty term in the planning process, and the fewer penalty categories will be included in the path result.

[0122] A smaller turning penalty cost results in more path corners and a closer alignment with the heuristic path, while minimizing obstacles to ensure a safe arrival at the destination. Conversely, a larger turning penalty factor reduces the number of path corners, but some parts of the path deviate from the heuristic path and get closer to obstacles, making the path more dangerous. Therefore, an appropriate turning penalty value should be selected based on the planned task requirements, striking a balance between the conflicting factors of path length and safety.

[0123] Effect test:

[0124] Map scenes such as Figure 6 As shown, the scene is generated as a raster map with a granularity of 5m, containing a total of 164×78 grids. The latitude and longitude of the selected starting point S are (120.912411, 31.061112), and the latitude and longitude of the ending point G are (120.920830, 31.059108). The figure shows the planning results of different algorithms under the same background. The path result of the traditional Theta* algorithm is shown as the blue dashed line, while the global improved path planned by this invention is shown as the blue solid line. The red solid line is the trajectory of the unmanned surface vessel (USV) following the improved path. It should be noted that because the traditional Theta* algorithm focuses on constructing the shortest path, the planned path length is the shortest. This path is too close to obstacles, resulting in a high degree of danger, making it unsuitable for USV path tracking. The USV trajectory under the method of this invention is as follows: Figure 6 As shown by the solid red line, the unmanned surface vessel's trajectory is relatively smooth due to the moderate number and length of global path nodes. Furthermore, the unmanned surface vessel maintains a large distance from adjacent obstacles, enabling it to safely navigate narrow waterways and meeting the safety and efficiency requirements of real-world missions.

[0125] Table 1 Comparison of paths under different methods

[0126] algorithm Number of nodes Path length / m Shortest distance to obstacle / m Deviation from the heuristic path / m Theta* 19 835.53 2.83 15.78 A* under non-uniform cost 167 945.98 10.00 5.37 Improved Theta* under non-uniform cost 12 873.89 6.11 2.00

[0127] The table above shows the path comparison results under different methods. As shown in Table 1, when A* plans a path under non-uniform cost, the path length increases by 13.22% compared to Theta*, but the deviation from the heuristic path decreases by 87.34%, and the shortest distance to obstacles is also significantly improved, resulting in a significant improvement in path safety. However, this safe path contains many nodes and turns, and is relatively long, making it difficult to plan in practice. Therefore, compared to the A* algorithm under non-uniform cost, the method of this invention plans a path with fewer nodes and turns, a moderate level of danger and path length, and a path that is easy to track. The path planned by the method of this invention increases the path length by 4.6% compared to the traditional Theta*, but reduces the deviation from the heuristic path by 65.94%, and has a sufficient shortest distance to obstacles, resulting in a significant improvement in safety.

[0128] Traditional Theta* algorithms focus on constructing the shortest path, resulting in the shortest planned path length. However, this path is too close to obstacles, posing a high risk and making it unsuitable for unmanned surface vessels (USVs) to track. The proposed method, however, is influenced by non-uniform costs and penalty factors during path planning. Therefore, the path tends to converge towards a heuristic path to varying degrees depending on the surrounding environment, thus moving away from obstacles. Compared to traditional algorithms, the final global path has fewer nodes and turns, a moderate level of risk and path length, and is easier to track. Thus, when USVs track the global path of this invention, the trajectory is smoother and the distance from adjacent obstacles is greater, enabling safe passage through narrow waterways and meeting the safety and efficiency requirements of practical tasks.

[0129] In summary, to balance path efficiency and safety requirements in the global path planning of unmanned surface vessels (USVs), this invention applies Global Value Decomposition (GVD) to a grid map and designs risk coefficients based on the degree of danger in different scenarios within the map to construct a risk assessment function. By combining the constructed non-uniform cost map with the improved Theta* algorithm, the method of this invention can effectively avoid dangerous areas and construct a superior path with fewer nodes and moderate safety. Furthermore, in practical tasks, a scaling factor can be selected according to specific needs to control the trade-off between safety and the shortest path. To verify the effectiveness of the proposed method, simulations were conducted on a real-world map environment. In the experiments, as the danger coefficient increases, the path gradually converges towards the heuristic path, improving safety. Moreover, comparing the original algorithm with the method of this invention in the experiments shows that the method of this invention plans a path with fewer nodes, a shorter path length, and meets safety requirements. In complex real-world scenarios, the global path planned by the method of this invention can ensure that the USV reaches its destination safely and efficiently.

[0130] Example 2

[0131] To investigate the impact of the scaling factor λ on the planned path, the inventors conducted the following experiments, namely Examples 2-1 to 2-6, with scaling factors λ of 0, 0.2, 0.4, 0.6, 0.8, and 1, respectively. The length of the planned path, the shortest distance to the obstacle, and the degree of deviation from the heuristic path were observed. The results are shown in Table 2 and... Figure 7 As shown.

[0132] Example 2-1

[0133] The content of the global safe path planning method for unmanned surface vessels is basically the same as that of Embodiment 1, except that: when facing an excessively narrow passage, the safety penalty cost C1 represents the cost of paying up to C1 to bypass the danger grid. Here, the safety penalty cost is 10, allowing the path to bypass the danger grid within a certain range; the turning penalty cost C0 is 3 to avoid an overly tortuous path; the scale factor λ determines the distance between the planned path and the heuristic path, i.e., the degree of path convergence. In this embodiment, the scale factor λ is 0.

[0134] The global safe path planning method for unmanned surface vessels in Examples 2-2 to 2-6 is basically the same as that in Example 2-1, except that the scaling factor λ in Examples 2-2 to 2-6 is 0.2, 0.4, 0.6, 0.8, and 1, respectively. The specific parameter values ​​are shown in Table 2.

[0135] Table 2 Path Indicators under Different Proportion Coefficients λ

[0136]

[0137]

[0138] Table 2 shows the indicators of the planned paths under different scales. The average distance between the grid cells occupied by the planned path and the heuristic path (composed of boundary grid cells) is used as the deviation between the planned path and the heuristic path. It can be seen that as the coefficient increases, the path length and safety change. Specific planning results are as follows... Figure 7 As shown, the red nodes represent the starting point, and the green pentagrams represent the ending point.

[0139] Combining the two sets of data above, it can be seen that when the coefficient is 0, the path is not affected by the evaluation value and the safety penalty cost C0, and the planning result is the shortest path. Compared with other cases, this path has the fewest nodes and the shortest length, but it is closest to the obstacle and deviates significantly from the heuristic path. Overall, it is close to the obstacle and has low safety. Figure 7As shown by the yellow solid line in (a). When the coefficient is 0.2, the heuristic path and safety penalty cost C0 come into play. The evaluated value in the cost guides the path toward the heuristic path, and the safety penalty cost C0, within acceptable limits, will cause it to bypass dangerous narrow passages. The planning result is as follows. Figure 7 As shown by the blue dashed line in (a). The results show that although the number of nodes and the path length increased, the distance to obstacles and the deviation from the heuristic path improved, and the dangerous area was avoided, thus enhancing safety. With further increases in the coefficients, the planning results are as follows... Figure 7 As shown in (b) and (c), and in conjunction with Table 2, it can be concluded that while the path length and number of nodes gradually increase with the coefficients, the deviation between the path and the heuristic path decreases, the shortest distance to the obstacle increases, and the safety improves accordingly. When the coefficients are 0.8 and 1.0, the reason why the shortest distance to the obstacle is the same for both paths is because a necessary narrow passage has reached the maximum safety value.

[0140] This invention incorporates heuristic paths and evaluation values, making security one of the planning metrics for paths. The planned paths converge towards the heuristic paths to varying degrees based on environmental characteristics, and as the proportion coefficient gradually increases, the path closely resembles the heuristic path, thus improving security. Combining this security metric with the improved Theta* algorithm results in planned paths that are not only shorter and have fewer nodes, but also significantly more secure.

[0141] In summary, this invention effectively overcomes the shortcomings of the prior art and has high industrial applicability. The above embodiments are intended to illustrate the substantive content of this invention, but are not intended to limit the scope of protection of this invention. Those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of this invention without departing from the essence and scope of protection of this invention.

Claims

1. A global safe path planning method for unmanned surface vessels, characterized in that, Includes the following steps: S100, rasterize the starting and ending point site electronic nautical chart to obtain a raster map; S200: Generate a generalized Voronoi map from the grid map to obtain the boundary grids of the generalized Voronoi map; S300: Construct a risk assessment function and then use the risk assessment function to calculate the risk coefficient of each boundary grid; S400: Construct a true cost function based on the risk coefficients and use an improved Theta* algorithm for global safe path planning. The specific expression for the risk assessment function is as follows: In the formula, λ is a scaling factor with a value of [0,1], set according to the path planning target preference; d is the nearest distance between the boundary grid and the obstacle; d m The danger distance threshold is set based on the target preference in path planning; d n The safe distance threshold is set based on the path planning target preference. The specific expression for the true cost function is: G(n)=G(n p )+G(n p ,n) In the formula, G(n) p ) represents the distance from the starting node S to the parent node n of the node. p The cumulative cost; G(n) p (n) represents the parent node of the node. p The incremental cost to node n is a non-uniform cost; in, In the formula, N is the parent node n of the node. p The number of grid cells traversed to reach node n, d i The parent node n of the node p The path length in the i-th grid cell passed through on the way to node n; D i The risk coefficient of the boundary grid cell closest to the i-th grid cell; dist i C is the shortest distance between the i-th grid cell and its nearest boundary grid cell; C(n) is the parent node n of the node. p The penalty cost to node n is a constant set according to the path planning objective preference. The improved Theta* algorithm modifies the visibility check mechanism of the traditional Theta* algorithm. Specifically, it determines whether the child node and the parent node of the current node are visible. If they are not visible, the current node is recorded. If they are visible, the parent node of the current node is first taken as the parent node of the child node. Since the path from the parent node of the current node to the child node does not turn, the total cost 1 of the child node without the turning penalty cost is calculated. Then, the current node is taken as the parent node of the child node. Since the path from the parent node of the current node to the child node turns, the total cost 2 of the child node with the turning penalty cost is calculated. The parent node of the child node corresponding to the smaller value of total cost 1 or total cost 2 is recorded. Finally, the recorded node is taken as the parent node of the child node.

2. The global safe path planning method according to claim 1, characterized in that, The calculation of the path length in the i-th grid has the following four cases: Scenario 1: When node n is the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p If the path to node n intersects the grid edge at only one point P, and the path does not completely pass through both grids, then the path length has two segments: one segment is from the intersection point P to the parent node n. p The distance is one segment; the other segment is the distance from the intersection point P to node n; Scenario 2: When node n is not the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects with the first grid cell it passes through at two points, P1 and P2. At this point, the path length has two segments: one segment is from intersection P1 to the parent node n. p The distance is one segment; the other segment is the distance between the two intersection points P1 and P2. Scenario 3: When node n is not the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p The path to node n intersects with the 2nd to N-1th grid cells at two points P1 and P2. In this case, the path length is the distance between the two intersection points P1 and P2. Case 4: When node n is not the parent node of node n p When the 8-neighbor grid is adjacent, the parent node n of the node is... p If the path to node n intersects the grid containing node n at only one point P, and the path does not completely pass through the grid containing node n, then the path length is the distance from the intersection point P to node n.

3. The global safe path planning method according to claim 2, characterized in that, The penalty cost is a safety penalty cost or / and a turning penalty cost.

4. The global safe path planning method according to claim 3, characterized in that, The type of penalty cost included in the true cost function needs to be determined. The specific determination condition is: when the nearest distance d between the nearest boundary grid cell and the obstacle of the i-th grid cell is ≤ d... m When the path turns at a visible point, the penalty cost includes a safety penalty cost; when the path turns at a visible point, the penalty cost includes a turning penalty cost.

5. The global safe path planning method according to claim 1, characterized in that, The boundaries of the electronic nautical chart of the starting and ending points are rectangles; in step S200, when generating the generalized Voronoi diagram, the four sides of the rectangular boundary are used as four different boundaries.

6. An electronic device comprising a memory and a processor, characterized in that, The memory stores a computer program, and when the processor executes the computer program, it implements the global safe path planning method for unmanned surface vessels as described in any one of claims 1-5.

7. A computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is executed by the processor, it implements the global safe path planning method for unmanned surface vessels as described in any one of claims 1-5.

Images