A method and system for local path planning of unmanned sailboats

Abstract

This invention relates to the field of ship path planning technology, specifically disclosing a method and system for local path planning of unmanned sailboats. The method includes: S1: Constructing a sea area map; S2: Constructing a random tree; S3: Obtaining a new node; if in headwind condition, proceed to S4; otherwise, proceed to S5; S4: Determining whether the new node falls outside the preset headwind constraint area; if yes, proceed to S5; otherwise, terminate the expansion and return to S3; S5: Determining whether the new node passes collision detection; if yes, add the new node to the random tree; otherwise, terminate the expansion and return to S3; S6: Reselecting a parent node for the new node; S7: Rewiring the random tree; S8: Determining whether the new node has reached the destination; if yes, determine whether the number of iterations has reached the maximum number of iterations; if so, complete the local path planning; if not, return to S3; otherwise, return to S3. This invention achieves optimized planning of local paths for unmanned sailboats.

Description

Technical Field

[0001] This invention relates to the field of ship path planning technology, and in particular to a method and system for local path planning of unmanned sailboats. Background Technology

[0002] Unmanned sailboats are autonomous marine vehicles that use wind power as their primary power source. Compared to traditional surface unmanned vehicles, they have significant advantages in terms of energy consumption and continuous operation, and are widely used in tasks such as ocean exploration and marine data collection.

[0003] Due to the uncertainty and dynamic changes in the marine environment, path planning is crucial for the navigation of unmanned sailboats. Path planning is divided into global path planning and local path planning. In complex marine environments, local path planning for unmanned sailboats involves planning their path from the starting point to the destination while avoiding obstacles. This requires combining the sailboat's own motion characteristics with various obstacle factors and the influence of power sources and wind to plan its path in real time, in order to cope with sudden environmental changes and the appearance of obstacles.

[0004] Currently, local path planning methods for unmanned sailboats include the A* algorithm, RRT algorithm, genetic algorithm, and artificial potential field method. The A* algorithm has strong randomness, is less prone to getting trapped in local optima, and has strong search performance, but it is suitable for static environments and is relatively weak in handling dynamic obstacles. The RRT algorithm uses the starting point as the initial root node and continuously searches and expands the path space through random sampling until the target point is found. It is suitable for path planning in dynamic environments and can quickly explore the space, but it does not guarantee finding the optimal path and may generate uneven paths. The principle of genetic algorithms for path planning is to use a random group of points in the space as the first generation of genetics. The next generation randomly generates new individuals through replication, inheritance, and mutation, iterating until an individual that meets the conditions is produced. Finally, the individual with the highest fitness is taken as the solution. Its advantage is that it is suitable for solving relatively complex combinatorial optimization problems, but its local search ability is weak, and it tends to have low search efficiency and premature convergence in the later stages of the algorithm. The artificial potential field method constructs an artificial virtual potential field around the object of study, consisting of a gravitational field and a repulsive field. The total potential field acting on the object is generated by the superposition of the gravitational and repulsive fields, and the movement of the object is controlled by the force generated by the total potential field. Although this method is simple to implement, it is prone to getting trapped in local optima. Summary of the Invention

[0005] To address the aforementioned technical problems, one objective of this invention is to provide a local path planning method for unmanned sailboats. Based on the RRT* algorithm, this method incorporates headwind constraints during random tree expansion to prevent new nodes from entering headwind areas, effectively improving the rationality of path planning. Furthermore, it integrates wind field factors and distance factors to construct a path cost function, which is used to optimize path selection and improve the feasibility of the unmanned sailboat's actual course.

[0006] To achieve the above-mentioned objectives, the present invention employs the following technical solution:

[0007] This application relates to a local path planning method for unmanned sailboats, including:

[0008] S1: Obtain the starting point, ending point, and marine environment of the unmanned sailboat, and construct a marine map. The marine information in the marine environment includes navigable sea areas, unnavigable sea areas, and wind field information.

[0009] S2: Set the step size and maximum number of iterations, and construct a random tree for the improved RRT* algorithm based on the starting point of the voyage;

[0010] S3: Select sampling point x from the aforementioned sea area map. rand Select from the random tree the sampling point x rand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand step size in direction to obtain new node x new ;

[0011] During the random tree expansion process, it is determined whether the unmanned sailboat is in a headwind state. If it is in a headwind state, proceed to S4; otherwise, proceed to S5.

[0012] S4: Determine the new node x new If the wind is outside the preset headwind constraint area, proceed to S5; otherwise, terminate the expansion and return to S3.

[0013] S5: Determine the new node x new Did it pass collision detection? If so, new node x new Add it to the random tree; otherwise, terminate the expansion and return to S3.

[0014] S6: For the new node x new Reselect parent node;

[0015] S7: Rewire the random tree;

[0016] S8: Determine whether the new node has reached the destination. If yes, determine whether the number of iterations has reached the maximum number of iterations. If it has, complete the local path planning. If it has not reached the destination, return to S3. If not, return to S3.

[0017] In S3, the new node x is obtained. new And in S6 and S7 for the new node x new When calculating path cost, the path cost function used is constructed based on wind field information and distance information.

[0018] In some embodiments of this application, the construction of a sea area map is specifically as follows:

[0019] Marine information in the marine environment is acquired, and a raster method is used to construct a rasterized marine map.

[0020] In this system, blank grids represent navigable sea areas, obstacle grids represent unnavigable sea areas, and wind field information is added to each grid.

[0021] The obstacle grid corresponds to static obstacles and dynamic obstacles. Dynamic obstacles are dynamically updated by modifying the grid value through matrix indexing.

[0022] In some embodiments of this application, the unmanned sailboat acquires wind field information in real time, including wind speed and wind direction;

[0023] The wind field information also includes small-amplitude normally distributed noise, used to simulate the irregularity of wind fields in real sea areas.

[0024] In some embodiments of this application, obstacle avoidance strategies are used for collision detection, including static obstacle avoidance strategies and dynamic obstacle avoidance strategies;

[0025] From neighboring point x near To the new node x new A straight line is generated between the grid points, and sampling is performed according to the grid size to obtain a series of discrete points (x, y) along the straight line. i y i ), where i is the index from 1 to the number of discrete points M;

[0026] The static obstacle avoidance strategy is as follows:

[0027] Discrete point (x) i y i The coordinates of the new node x are mapped onto a rasterized sea map and compared with the coordinates of the static obstacles mapped onto the rasterized sea map. If the two coordinates overlap, it indicates a collision, and the new node x is discarded. new Otherwise, it indicates that there is no collision, and the new node x is... new Add to the random tree;

[0028] The dynamic obstacle avoidance strategy is as follows:

[0029] Calculate each discrete point (x) i y i (x) to the current coordinates of the dynamic obstacle d y d The Euclidean distance Dis;

[0030] Determine if any Dis is less than or equal to the sum of the radius Dr of the dynamic obstacle and the distance buffer of the preset buffer. If so, a collision exists, and the new node x is discarded. new If not, it means there is no collision, and the new node x is... new Add it to the random tree.

[0031] In some embodiments of this application, the path cost function adopts the following formula:

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] ;

[0037] ;

[0038] ;

[0039] in, Represents node x near and x new The relative wind angle between them Represents node x near To x new The heading angle of the unmanned sailboat Represents node x near To x new Environmental wind angle; Represents node x i-1 and x i The relative wind angle between them Represents node x i-1 To x i The heading angle of the unmanned sailboat Represents node x i-1 To x i Environmental wind angle, V windThe value represents the ambient wind speed, and ω represents the wind influence weighting coefficient, used to adjust the magnitude of the wind field's impact on path cost. This represents the distance cost between adjacent nodes. This represents the wind field cost of adjacent nodes. For path cost function, Let i = 1, 2, 3, ..., N, where N represents the path cost from the starting point x on the current path. start To neighboring point x near The number of steps between, x start x1, x2, ..., x N Indicates x on the current path start To x near A series of nodes, x0=x start x N =x near .

[0040] In some embodiments of this application, when the ambient wind speed is lower than a first preset wind speed, ω is reduced so that the wind field cost is less than the distance cost;

[0041] When the ambient wind speed is greater than the second preset wind speed, increase ω to make the wind field cost greater than the distance cost.

[0042] In some embodiments of this application, a preset headwind constraint area is set when expanding the tree;

[0043] Based on the target heading angle of the unmanned sailboat and the preset headwind area, the extended angle range is obtained;

[0044] Calculate the new node x new relative neighboring point x near The angle between them;

[0045] If the calculated angle is within the range of the extended angle, the new node x new If the node is valid, discard the new node x; otherwise, discard it. new This expansion is now terminated.

[0046] The local path planning method for unmanned sailboats provided in this application has the following advantages and beneficial effects:

[0047] (1) Based on the RRT* algorithm, when expanding the random tree, a headwind constraint is introduced according to the influence of the wind field on the navigable area of ​​the unmanned sailboat. The expansion method of the random tree is improved to avoid new nodes from entering the headwind area, so as to adapt to the motion characteristics of the unmanned sailboat and improve the rationality of the path planning.

[0048] (2) In the pruning optimization, in the expansion of new nodes, the reselection of parent nodes and the rewiring of random trees, not only the distance factor is considered, but also the wind field factor is integrated to construct the path cost function, and the influence of the wind field is incorporated into the path planning to optimize the path selection, so that the path planning can balance between distance, energy consumption and navigation efficiency, and the feasibility of the actual navigation of unmanned sailboats is considered.

[0049] (3) The path cost function is constructed by integrating wind field factors and distance factors. It not only updates the path cost of a single node, but also updates the path cost of all child nodes under that node. This recursive update of the path cost of child nodes ensures that when the parent node of a certain node changes, all related child paths can also be optimized in time, thus realizing local path optimization planning.

[0050] Some embodiments of this application also relate to a local path planning system for unmanned sailboats, including:

[0051] The marine map construction module is used to obtain the starting point, ending point and marine environment of the unmanned sailboat, and construct a marine map. The marine information in the marine environment includes navigable sea areas, unnavigable sea areas and wind field information.

[0052] The random tree setting module is used to set the step size and maximum number of iterations, and to construct a random tree for the improved RRT* algorithm based on the starting point of the voyage;

[0053] The new node acquisition module is used to select sampling point x from the sea area map. rand Select from the random tree the sampling point x rand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand step size in direction to obtain new node x new ;

[0054] The headwind / tailwind determination module is used to determine whether the unmanned sailboat is in a headwind state during the random tree expansion process.

[0055] The wind field constraint determination module is used to determine the new node x when the unmanned sailboat is in a headwind. new Whether it falls outside the preset headwind constraint area;

[0056] The random tree extension module is used to create a new node x. new If the sailboat is outside the preset headwind constraint area or is sailing with the wind, and at the new node x new After collision detection, the new node x new To expand the random tree;

[0057] The parent node reselection module is used to select a new node x. new Reselect parent node;

[0058] The rerouting module is used to reroute the random tree;

[0059] The local path determination module is used to determine whether local path planning has been completed.

[0060] Among them, in obtaining the new node x new And for the new node x new When calculating path cost, the path cost function used is constructed based on wind field information and distance information.

[0061] In some embodiments of this application, the marine map construction module constructs a marine map specifically as follows:

[0062] Marine information in the marine environment is acquired, and a raster method is used to construct a rasterized marine map.

[0063] In this system, blank grids represent navigable sea areas, obstacle grids represent unnavigable sea areas, and wind field information is added to each grid.

[0064] The obstacle grid corresponds to static obstacles and dynamic obstacles. Dynamic obstacles are dynamically updated by modifying the grid value through matrix indexing.

[0065] In some embodiments of this application, the random tree expansion module employs an obstacle avoidance strategy for collision detection, the obstacle avoidance strategy including a static obstacle avoidance strategy and a dynamic obstacle avoidance strategy;

[0066] From neighboring point x near To the new node x new A straight line is generated between the grid points, and sampling is performed according to the grid size to obtain a series of discrete points (x, y) along the straight line. i y i ), where i is the index from 1 to the number of discrete points M;

[0067] The static obstacle avoidance strategy is as follows:

[0068] Discrete point (x) i y i The coordinates of the new node x are mapped onto a rasterized sea map and compared with the coordinates of the static obstacles mapped onto the rasterized sea map. If the two coordinates overlap, it indicates a collision, and the new node x is discarded. new Otherwise, it indicates that there is no collision, and the new node x is... new Add to the random tree;

[0069] The dynamic obstacle avoidance strategy is as follows:

[0070] Calculate each discrete point (x)i y i (x) to the current coordinates of the dynamic obstacle d y d The Euclidean distance Dis;

[0071] Determine if any Dis is less than or equal to the sum of the radius Dr of the dynamic obstacle and the distance buffer of the preset buffer. If so, a collision exists, and the new node x is discarded. new If not, it means there is no collision, and the new node x is... new Add it to the random tree.

[0072] Other features and advantages of the present invention will become clearer after reading the detailed embodiments of the invention in conjunction with the accompanying drawings. Attached Figure Description

[0073] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0074] Figure 1 A flowchart illustrating an embodiment of the local path planning method for unmanned sailboats proposed in this application is shown.

[0075] Figure 2 This diagram illustrates the use of a grid method for sea area modeling in an embodiment of the local path planning method for unmanned sailboats proposed in this application.

[0076] Figure 3 This diagram illustrates a simulated wind field on a sea map in an embodiment of the local path planning method for unmanned sailboats proposed in this application.

[0077] Figure 4 This diagram illustrates the expansion of the random tree in an embodiment of the local path planning method for unmanned sailboats proposed in this application when a headwind constraint is introduced.

[0078] Figure 5 This diagram illustrates a random tree after the first step of pruning optimization in an embodiment of the local path planning method for unmanned sailboats proposed in this application.

[0079] Figure 6 This diagram illustrates the random tree after the second step of pruning optimization (i.e., parent node reselection) in an embodiment of the local path planning method for unmanned sailboats proposed in this application.

[0080] Figure 7 This diagram illustrates the random tree after the third step (i.e., rewiring) of the pruning optimization in an embodiment of the local path planning method for unmanned sailboats proposed in this application. Detailed Implementation

[0081] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0082] Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without inventive effort are within the scope of protection of this invention. In the description of this invention, it should be understood that the terms "center," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings and are only for the convenience of describing this invention and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention.

[0083] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "joining" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0084] The terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0085] To avoid the problems of poor local path planning performance of existing unmanned sailboats, see [link / reference]. Figure 1 This application relates to a local path planning method for unmanned sailboats, which is based on an improved RRT* (Rapidly-exploring Random Tree Star) algorithm, adds headwind constraints, and considers both wind field cost and distance cost during the pruning optimization process of the random tree to optimize path selection.

[0086] Figure 1 A flowchart illustrating the local path planning method for unmanned sailboats is shown. See also... Figure 1 The local path planning method for unmanned sailboats is described in detail below.

[0087] S1: Obtain the starting point, ending point, and marine environment of the unmanned sailboat, and construct a marine map.

[0088] Given the existing environmental conditions of the unmanned sailboat, determine the starting point x of the unmanned sailboat's navigation. start End point x end And the marine environment.

[0089] Before creating a sea area map, it is necessary to clarify the sea area information, which includes navigable sea areas, non-navigable sea areas, and wind field information. Non-navigable sea areas are sea areas with obstacles (including static and dynamic obstacles), and the obstacles include their size and location.

[0090] In some embodiments of this application, a raster method is used to construct a sea area map based on sea area information, forming a rasterized sea area map.

[0091] The raster method is a discretization method that divides the marine environment into regular grids. Each grid cell (or pixel) represents a small area in the map, and a value represents the state of that area (e.g., accessibility, cost, etc.). This method transforms the marine environment from a continuous space into a discrete space, making it easier for computers to process.

[0092] The marine environment is divided into equal-sized grids using a raster method, with each grid having a unique identifier. See also Figure 2 A blank grid can be represented by "1", which represents a navigable sea area that allows passage, while a black grid (i.e., an obstacle grid) can be represented by "0", which represents an unnavigable sea area that obstructs passage.

[0093] For dynamic obstacles, the attributes of the obstacles in front of the unmanned sailboat (e.g., size, distance) can be obtained by using the sensors on the unmanned sailboat, and its direction of movement can be predicted to obtain the fuzzy coordinate information of the dynamic obstacles.

[0094] For dynamic obstacles, the values ​​of the raster can be dynamically updated by modifying the matrix index. After the obstacle moves, the raster corresponding to the old position is restored to 1, and the new position is set to 0. The raster method for modeling marine maps provides a clear search space for the RRT* algorithm.

[0095] Wind fields are a significant factor influencing the path planning of unmanned sailboats. Therefore, modeling the marine environment also includes modeling the wind field to simulate the actual wind speed and direction distribution.

[0096] Real-time wind field information is known and can be obtained from data provided by global forecasting systems, mainly including wind speed and wind direction information, which provides raw and reliable wind field data for wind field modeling.

[0097] Linear interpolation is used to map wind field information data onto nodes of each grid. Each grid node represents a specific point in the wind field. See the wind field diagram for details. Figure 3 The wind field information of each grid node is drawn as an arrow. The direction and length of the arrow represent the wind direction and wind speed, respectively. A longer arrow indicates a higher wind speed, and a shorter arrow indicates a lower wind speed.

[0098] In some embodiments of this application, in order to make the wind field more realistic, some random perturbation is added to the wind field. The perturbation can be a small amount of normally distributed noise to simulate the irregularity of the wind field in real sea areas.

[0099] In this way, a rasterized sea map is constructed that includes information on navigable sea areas, unnavigable sea areas, and wind fields.

[0100] S2: Set the step size L and the maximum number of iterations M, and construct a random tree for the improved RRT* algorithm based on the starting point of the voyage.

[0101] After the map is built, a random tree will be constructed based on the RRT* algorithm.

[0102] Before constructing the random tree, it should be initialized. At this point, the random tree only contains the starting point x. start Based on starting point x start The random tree is expanded through an iterative process.

[0103] Before constructing the random tree, set the step size, maximum number of iterations, and target threshold.

[0104] The step size is the length of the node movement during the expansion process. An initial step size can be set, and the size of the step size can be adjusted according to the known environmental information. If the map is large, the step size can be increased accordingly. Otherwise, too short a step size can slow down the search speed. Similarly, when the map area is small, too large a step size is prone to collision with obstacles.

[0105] The following steps will begin the iteration to expand the random tree.

[0106] Alternatively, a target threshold can be set, assuming that during the tree expansion process, the new node x... new With the endpoint x end When the distance between them is less than the target threshold, the unmanned sailboat is considered to have reached the destination, and the local path planning is completed.

[0107] S3: Get the new node x new .

[0108] Get new node x new The purpose is to expand the tree.

[0109] Select sampling point x from the sea area map. rand Select sampling point x from the random treerand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand the direction by step size L to form a new node x new .

[0110] Compared to the path cost calculation method of the traditional RRT* algorithm, the path cost calculated here takes into account not only the distance cost between nodes but also the wind field cost between nodes, which enables the path planning to balance distance, energy consumption and navigation efficiency, and takes into account the feasibility of actual navigation of unmanned sailboats.

[0111] In the standard RRT* algorithm, the tree expansion process is randomized to improve the coverage of the search space. However, for unmanned sailboats, their navigation is greatly affected by wind fields. If random expansion is used directly, the unmanned sailboat may enter a headwind area where it cannot move forward or deviate from the optimal navigation path, affecting the efficiency of path planning.

[0112] Therefore, this application improves the tree expansion method by incorporating wind field information and introducing headwind constraints during random expansion to adapt to the motion characteristics of unmanned sailboats and improve the rationality of path planning.

[0113] Unmanned sailboats have high navigability in downwind and crosswind directions. When planning a route, random expansion can be performed to achieve a wider sampling and more comprehensive acquisition of wind speed and direction data at various locations. This allows for a reasonable evaluation of the path cost of each route in the subsequent path cost function calculation.

[0114] In headwind conditions, to prevent unmanned sailboats from entering unfavorable navigation areas, angular constraints are imposed on random expansion to limit the expansion direction, enabling unmanned sailboats to reasonably detour around headwind areas, thereby improving the feasibility of the route and navigation efficiency.

[0115] Therefore, the new node x is obtained. new Next, whether it can be added to the random tree depends on whether it is in a headwind state, and if so, the new node x. new Whether to enter the preset headwind constraint area (this area is defined by the applied headwind angle constraint).

[0116] During the random tree expansion process, it is determined whether the unmanned sailboat is in a headwind state. If it is in a headwind state, proceed to S4; otherwise, proceed to S5.

[0117] S4: Determine the new node x new If the target area falls outside the preset headwind constraint area, proceed to S5; otherwise, terminate the expansion and return to S3.

[0118] The preset headwind constraint area can be set based on experience and the dynamic characteristics of unmanned sailboats.

[0119] See Figure 4 In this application, the preset headwind constraint region is set as the headwind angle constraint [-γ, γ] (i.e., Figure 4 (The area indicated by the green dashed arrow) γ can be 45°. Unmanned sailboats are uncontrollable and cannot obtain effective power within this headwind angle constraint range, so this area should be avoided.

[0120] In some embodiments of this application, a preset headwind constraint region is set when expanding the tree. This is based on the target heading angle θ of the unmanned sailboat (i.e., the starting point x). start With the endpoint x start The angle between the line connecting the x-axis and the x-axis (and the preset headwind constraint area) is used to obtain the extended angle range.

[0121] When the headwind angle constraint is [-γ,γ], the extended angle η can be obtained, which is located in the extended angle range [θ-π,θ-γ] or [θ+γ,θ+π].

[0122] See Figure 4 Top node x new The expansion method involves calculating the new node x. new relative neighboring point x near If the calculated angle α between the two nodes is within the range of the extended angle, then the new node x is considered to be... new If the new node x falls outside the pre-defined headwind constraint area, then... new This is the valid point; if the calculated angle β is outside the extended angle range, see [reference needed]. Figure 4 The node below x new The expansion method assumes that the new node x new If the unmanned sailboat falls within the pre-defined headwind constraint area, it will become uncontrollable and unable to obtain effective power. In this case, abandon this new node x. new Terminate this expansion and return to S3, then randomly select sampling point x. rand .

[0123] S5: Determine the new node x new Did it pass collision detection? If so, move the new node x. new Add it to the random tree; otherwise, terminate the expansion and return to S3.

[0124] In local path planning for unmanned sailboats, in addition to considering the impact of wind on navigation, it is also necessary to comprehensively consider the obstacle avoidance requirements of both static and dynamic obstacles to ensure the safety of the planned path. Therefore, each time a new node x is obtained... newCollision detection is performed before and after adding the node to the random tree to ensure that the planned path will not collide with obstacles. If a collision is detected, the new node x is discarded. new To avoid invalid expansion.

[0125] As described above, the representations of static and dynamic obstacles in the grid are given.

[0126] Static obstacles, when displayed on a sea map, are represented as a binary image or matrix. During collision detection, a static obstacle avoidance strategy is employed for static obstacles.

[0127] From neighboring point x near To the new node x new A straight line (i.e., a sub-path) is generated between the grid points, and samples are taken according to the grid size to obtain a series of discrete points (x, y) along this line. i y i ), where i is the index from 1 to the number of discrete points M.

[0128] Discrete point (x) i y i The coordinates of the new node x are mapped onto a rasterized sea map and compared with the coordinates of the static obstacles mapped onto the rasterized sea map. If the two coordinates overlap, a collision occurs, and the collision detection fails. In this case, the new node x is discarded. new If the two nodes do not overlap, it means there is no collision and the collision detection has passed. In this case, the new node x is... new Add it to the random tree.

[0129] For dynamic obstacles, as mentioned above, their size (which can be represented by the radius Dr) and coordinates (x, y) can be obtained using sensors carried by the unmanned sailboat. d y d ).

[0130] For each discrete point (x) on the subpath i y i ), calculate each discrete point (x) i y i (x) to the current coordinates of the dynamic obstacle d y d The Euclidean distance Dis.

[0131] Determine if any Dis is less than or equal to the sum of the radius Dr of the dynamic obstacle and the distance buffer of the preset buffer. If so, it indicates that the subpath has collided with the dynamic obstacle, and the new node x is discarded. new If no collision occurs, the expansion is terminated; otherwise, it indicates that no collision exists, and the new node x is added. new Add to the random tree.

[0132] In some embodiments of this application, the purpose of introducing a preset buffer distance is that dynamic obstacles are not stationary; they typically move in a certain direction.

[0133] In real-world environments, it is difficult to accurately predict the position of dynamic obstacles at every instant. Therefore, if only the current exact position of the dynamic obstacle is considered (i.e., only the radius Dr of the dynamic obstacle is considered), the dynamic obstacle may move into the sub-path in the next few steps, which may cause the sub-path to collide with it. Therefore, the preset buffer distance buffer provides an additional buffer space for each dynamic obstacle. Even if the dynamic obstacle has not yet reached its current position, it reserves enough distance to cope with its future movement and can effectively avoid obstacles.

[0134] After terminating this expansion, it will return to S3 and continue selecting sampling point x. rand This cycle continues until the unmanned sailboat reaches the destination x. end .

[0135] S6: For the new node x new Reselect the parent node.

[0136] In the RRT* algorithm, to further optimize the path, RRT* checks whether replacing the node x can solve the problem. new To obtain a better path length, we need to define the parent node of the new node x. new By reselecting the parent node, the path quality is optimized and the algorithm is able to converge to the optimal solution.

[0137] It can be used with a new node x new Construct a circular closed region with a center and a certain distance as the radius, and add nodes that fall inside the circle to the parent node candidate set.

[0138] See Figure 5 and Figure 6 New node x new Let node ⑤ be the parent node of node ④. Draw a circle with radius R. The candidate set of parent nodes that fall into the circle includes nodes ③, ④, ⑥ and ⑧.

[0139] Whether to change the parent node of node ⑤ from node ④ to node ③, ⑥, or ⑧, or to keep node ④, depends on comparing path costs.

[0140] In some embodiments of this application, the path cost considers wind field factors and distance factors, for example... Figures 5 to 7 The numbers on the lines connecting adjacent nodes represent the path cost of forming a sub-path between adjacent nodes.

[0141] When the parent node is node ④, the first path to node ⑤ is ②->⑦->③->④->⑤, with a path cost of 18. When the parent node is node ③, the second path to node ⑤ is ②->⑦->③->⑤, with a path cost of 17. When the parent node is node ⑥, the third path to node ⑤ is ②->⑦->⑥->⑤, with a path cost of 14. When the parent node is node ⑧, the fourth path to node ⑤ is ②->⑦->⑧->⑤, with a path cost of 18.

[0142] It can be seen that the third path to node ⑤ has the shortest path cost. Therefore, the parent node of node ⑤ is switched from node ④ to node ⑥ (i.e., the new node x is changed). new (Parent node) See Figure 6 Connect node ⑤ and node ⑥.

[0143] S7: Rewire the random tree.

[0144] For the new node x new After reselecting the parent node, to further minimize the path cost between nodes in the random tree, the random tree is rewired. If the parent node of a nearest neighbor node is changed to the new node x... new If the path cost can be reduced, then make the change; otherwise, keep the original parent node.

[0145] See Figure 7 New node x new Let ⑤ be a node, and its nearest neighbors include nodes ③, ④ and ⑧.

[0146] Whether to change the parent node of nodes ③, ④, and ⑧ to node ⑤, or keep the original parent node, depends on comparing path costs.

[0147] When node ③'s parent node is node ⑦, the path to node ③ is ②->⑦->③, with a path cost of 13. If node ③'s parent node is changed to node ⑤, the path to node ③ is ②->⑦->⑥->⑤->③, with a path cost of 19. Since 19 > 13, node ③'s parent node remains unchanged.

[0148] When node ④'s parent node is node ③, the path to node ④ is ②->⑦->③->④, with a path cost of 17. If node ④'s parent node is changed to node ⑤, the path to node ③ is ②->⑦->⑥->⑤->④, with a path cost of 15. Since 15 < 17, node ④'s parent node is changed to ⑤, node ④ and node ⑤ are connected, and the connection between node ④ and node ③ is canceled.

[0149] When node ⑧'s parent node is node ⑦, the path to node ⑧ is ②->⑦->⑧, with a path cost of 18. If node ⑧'s parent node is changed to node ⑤, the path to node ⑧ is ②->⑦->⑥->⑤->⑧, with a path cost of 17. Since 17 < 18, node ⑧'s parent node is changed to node ⑤, node ⑧ and node ⑤ are connected, and the connection between node ⑦ and node ⑧ is canceled.

[0150] Nodes ⑧ and ⑤ are connected, making it possible for node ⑧ to have a shorter path through node ⑤. Therefore, the paths between nodes ⑥ and ⑧, and between nodes ⑦ and ⑧, are cancelled.

[0151] Nodes 4 and 5 are connected, and nodes 8 and 5 are also connected, making it possible for node 4 to have a shorter path to node 8 via node 5. Therefore, the path between nodes 4 and 8 is cancelled.

[0152] Thus, during the random tree expansion process, the new node x will be... new Reselect the parent node and rewire.

[0153] It should be noted that collision detection is required during the connection process involved in reselecting the parent node and rewiring. Only when two points pass the collision detection will they be connected. The collision detection method is as described above and will not be repeated here.

[0154] In some embodiments of this application, regardless of whether a new node x is obtained in S3... new and the new node x new Reselecting a parent node and rerouting both require calculating path costs.

[0155] In some embodiments of this application, the path cost function is designed as the sum of wind field cost and distance cost.

[0156] For the new node x new Starting from point x start To the new node x new The path cost is equal to the starting point x. start To neighboring point x near Cumulative path cost, neighboring point x near To the new node x new Wind farm cost and neighboring point x near To the new node x new The sum of distance costs.

[0157] First, define This represents the distance cost between adjacent nodes. This represents the wind field cost of adjacent nodes. Let i be the path cost function, i = 1, 2, 3, ..., N, where N represents the path length from the starting point x. startTo neighboring point x near The number of steps between, x start x1, x2, ... x N Indicates x on the current path start To x near A series of nodes, x0=x start x N =x near .

[0158] New node x new The path cost function is:

[0159] .

[0160] Indicates starting from the point x start To the new node x new Path cost, Indicates the starting point x start To neighboring point x near Path cost, Indicates adjacent node x near and x new The cost of distance between them Indicates adjacent node x near and x new The cost of wind farms between them.

[0161] in, ;

[0162] .

[0163] Let x represent the node i-1 to node x i Path cost, The new node x represents i-2 to node x i-1 Path cost, Indicates adjacent node x i-1 and x i The cost of distance between them Indicates adjacent node x i-1 and x i The cost of wind farms between them.

[0164] For example, the path L has N=3 steps and a series of nodes x. start x1, x2, x near .

[0165] .

[0166] = ( ).

[0167] = ( ).

[0168] = ( ).

[0169] The distance cost between the two nodes as described above is the Euclidean distance. The wind field cost is described below.

[0170] Definition: Node x near and x new The relative wind angle θ' between rel and node x i-1 and x i The relative wind direction angle θ between rel .

[0171] .

[0172] .

[0173] in, Represents node x near To x new The heading angle of the unmanned sailboat Represents node x near To x new Environmental wind angle, Represents node x i-1 To x i The heading angle of the unmanned sailboat Represents node x i-1 To x i The ambient wind direction angle.

[0174] Wind field weighting of path cost Combining relative wind direction angle and wind speed V wind Joint calculations were performed to avoid wind speed V. wind The numerical singularity problem caused by a value of 0, where the denominator is taken as wind speed V. wind The maximum value of 0.1 is max.

[0175] but, .

[0176] .

[0177] Wherein, ω represents the wind influence weighting coefficient, which is used to adjust the magnitude of the wind field's influence on path cost.

[0178] The addend 1 in the equation represents environmental impedance. Under favorable wind conditions, the wind field cost approaches 1, thus avoiding the situation where the wind field's influencing factors are zero when the wind speed is infinitely high.

[0179] Wind field cost function The construction of the wind field cost function is closely related to the navigable area of ​​the unmanned sailboat, which includes the relative wind direction and wind speed. Here, the numerator represents the heading. The closer to a headwind, the more difficult the sailboat's navigation becomes, and the larger the wind cost function becomes. Conversely, the closer to a tailwind, the smaller the wind cost function becomes, consistent with the principles of unmanned sailboat navigation. Therefore, the algorithm is encouraged to avoid headwinds and instead favor headings with the wind or crosswinds, allowing the unmanned sailboat to navigate to areas with higher wind speeds.

[0180] In some embodiments of this application, ω is introduced to adjust the impact of wind speed and direction on the total path cost. This varies depending on the specific case:

[0181] When the wind speed is low (a first preset wind speed can be set, the wind speed is related to the size of the unmanned sailboat, the first preset wind speed can be set to 1.5~3m / s), the unmanned sailboat gains less energy, whether it is with the wind or against the wind, and the unmanned sailboat is constrained by insufficient power. At this time, ω should be appropriately reduced and the weight of distance cost in the total path cost should be increased (i.e., wind cost is less than distance cost) to avoid excessive wind cost causing excessive path search in local areas, and to prioritize ensuring that the path can be traversed.

[0182] When the wind speed is relatively high (a second preset wind speed can be set, which can be set to 6~10m / s), the unmanned sailboat gains more energy. The wind field cost is given priority. At this time, ω is appropriately increased to make the value of the wind field cost slightly larger, increasing the weight of the wind field cost in the total path cost. The algorithm will select the path with more favorable wind force to maximize the energy gain of the unmanned sailboat, achieve the fastest speed, and the shortest distance.

[0183] S8: Determine the new node x new Has the destination been reached? If so, check if the number of iterations has reached the maximum number of iterations. If it has, complete the local path planning. If not, return to S3.

[0184] A target threshold can be set at the new node x. new When the distance between the node and the endpoint reaches the target threshold, it indicates that a new node x is formed. new Arrived at the destination.

[0185] During the iteration process, consider whether the terminal has been reached and whether the maximum number of iterations has been reached.

[0186] Continue iterating until the endpoint is reached and the maximum number of iterations has been reached.

[0187] If the destination has not been reached and the maximum number of iterations has been reached, the current path is searched again.

[0188] Local path planning is completed when the destination is reached and the maximum number of iterations is reached.

[0189] If the destination is reached and the maximum number of iterations has not been reached, continue iterating to find the optimal path.

[0190] The local path planning method for unmanned sailboats involved in this application can take into account the influence of dynamic obstacles and dynamic wind fields, update obstacle positions in real time, dynamically avoid obstacles, and adapt to changes in the dynamic marine environment in real time. Furthermore, it incorporates the influence of wind fields into the path planning, taking into account the impact of wind speed and wind direction on the course path. In the case of headwinds, it changes the tree expansion method to achieve zigzag path planning. In addition, it introduces wind field costs, so that the path planning can balance distance, energy consumption, and navigation efficiency.

[0191] In the process of pruning and optimizing a random tree, the mechanism of recursively updating the path cost of child nodes ensures that when the parent node of a certain node changes, all related child paths can also be optimized in a timely manner. This design can effectively prevent the problem of local optima and make the overall path more optimized.

[0192] In some embodiments of this application, a local path planning system for unmanned sailboats (not shown) is also disclosed, which is used to implement the local path planning method for unmanned sailboats as described above.

[0193] The local path planning system for unmanned sailboats includes a sea area map construction module, a random tree setting module, a new node acquisition module, a headwind / tailwind determination module, a wind field constraint determination module, a random tree expansion module, a parent node reselection module, a rewiring module, and a local path determination module. This system is applied to unmanned sailboats to realize local path planning for unmanned sailboats.

[0194] The marine map building module is used to obtain the starting point, ending point, and marine environment of the unmanned sailboat, and to build a marine map.

[0195] The random tree setting module is used to set the step size and maximum number of iterations, and to construct a random tree for the improved RRT* algorithm based on the starting point of the voyage.

[0196] The new node acquisition module is used to select sampling point x from the sea area map. rand Select sampling point x from the random tree rand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand the step size in the direction to obtain the new node x. new .

[0197] The headwind / tailwind determination module is used to determine whether the unmanned sailboat is in a headwind state during the random tree expansion process.

[0198] The wind field constraint determination module is used to determine the new node x of an unmanned sailboat in headwind conditions. new Whether it falls outside the preset headwind constraint area.

[0199] The random tree extension module is used to create a new node x new When the sailboat falls outside the preset headwind constraint area or is in a downwind state, at the new node x new After collision detection, the new node x will be... new Add to the random tree.

[0200] The parent node reselection module is used to select a new node x. new Select a new parent node.

[0201] The rerouting module is used to reroute the random tree; the local path determination module is used to determine whether local path planning has been completed.

[0202] The specific implementation process of each of the above modules can be combined with Figures 1 to 7 As mentioned above, further details will not be repeated here.

[0203] The local path planning system for unmanned sailboats involved in this application ensures that the unmanned sailboats obtain an optimized overall path while avoiding obstacles.

[0204] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions claimed by the present invention.

Claims

1. A local path planning method for an unmanned sailboat, characterized in that, include: S1: Obtain the starting point, ending point, and marine environment of the unmanned sailboat, and construct a marine map. The marine information in the marine environment includes navigable sea areas, unnavigable sea areas, and wind field information. S2: Set the step size and maximum number of iterations, and construct a random tree for the improved RRT* algorithm based on the starting point of the voyage; S3: Select sampling point x from the aforementioned sea area map. rand Select from the random tree the sampling point x rand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand step size in direction to obtain new node x new ; During the random tree expansion process, it is determined whether the unmanned sailboat is in a headwind state. If it is in a headwind state, proceed to S4; otherwise, proceed to S5. S4: Determine the new node x new If the wind is outside the preset headwind constraint area, proceed to S5; otherwise, terminate the expansion and return to S3. S5: Determine the new node x new Did it pass collision detection? If so, new node x new Add it to the random tree; otherwise, terminate the expansion and return to S3. S6: For the new node x new Reselect parent node; S7: Rewire the random tree; S8: Determine the new node x new Has the destination been reached? If yes, determine if the number of iterations has reached the maximum number of iterations. If it has, complete the local path planning. If not, return to S3. In S3, the new node x is obtained. new And in S6 and S7 for the new node x new The path cost function used for path cost calculation is as follows: ； ； ； ； ； ； ； in, Represents node x near and x new The relative wind angle between them Represents node x near To x new The heading angle of the unmanned sailboat Represents node x near To x new Environmental wind angle; Represents node x i-1 and x i The relative wind angle between them Represents node x i-1 To x i The heading angle of the unmanned sailboat Represents node x i-1 To x i Environmental wind angle, V wind The value represents the ambient wind speed, and ω represents the wind influence weighting coefficient, used to adjust the magnitude of the wind field's impact on path cost. This represents the distance cost between adjacent nodes. This represents the wind field cost of adjacent nodes. For path cost function, Let i = 1, 2, 3, ..., N, where N represents the path cost from the starting point x on the current path. start To neighboring point x near The number of steps between, x start x1, x2, ..., x N Indicates x on the current path start To x near A series of nodes, x0=x start x N =x near .

2. The local path planning method for unmanned sailboats according to claim 1, characterized in that, Constructing a sea area map, specifically: Marine information in the marine environment is acquired, and a raster method is used to construct a rasterized marine map. In this system, blank grids represent navigable sea areas, obstacle grids represent unnavigable sea areas, and wind field information is added to each grid. The obstacle grid corresponds to static obstacles and dynamic obstacles. Dynamic obstacles are dynamically updated by modifying the grid value through matrix indexing.

3. The local path planning method for unmanned sailboats according to claim 2, characterized in that, The unmanned sailboat acquires wind field information in real time, including wind speed and wind direction. The wind field information also includes small-amplitude normally distributed noise, used to simulate the irregularity of wind fields in real sea areas.

4. The local path planning method for unmanned sailboats according to claim 2, characterized in that, Collision detection is performed using obstacle avoidance strategies, which include static obstacle avoidance strategies and dynamic obstacle avoidance strategies. From neighboring point x near To the new node x new A straight line is generated between the grid points, and sampling is performed according to the grid size to obtain a series of discrete points (x, y) along the straight line. i y i ), where i is the index from 1 to the number of discrete points M; The static obstacle avoidance strategy is as follows: Discrete point (x) i y i The coordinates of the new node x are mapped onto a rasterized sea map and compared with the coordinates of the static obstacles mapped onto the rasterized sea map. If the two coordinates overlap, it indicates a collision, and the new node x is discarded. new Otherwise, it indicates that there is no collision, and the new node x is... new Add to the random tree; The dynamic obstacle avoidance strategy is as follows: Calculate each discrete point (x) i y i (x) to the current coordinates of the dynamic obstacle d y d The Euclidean distance Dis; Determine if any Dis is less than or equal to the sum of the radius Dr of the dynamic obstacle and the distance buffer of the preset buffer. If so, a collision exists, and the new node x is discarded. new If not, it means there is no collision, and the new node x is... new Add it to the random tree.

5. The local path planning method for unmanned sailboats according to claim 1, characterized in that, When the ambient wind speed is lower than the first preset wind speed, decrease ω to make the wind field cost less than the distance cost; When the ambient wind speed is greater than the second preset wind speed, increase ω to make the wind field cost greater than the distance cost.

6. The local path planning method for unmanned sailboats according to claim 1, characterized in that, When expanding the tree, set a preset backwind constraint area; Based on the target heading angle of the unmanned sailboat and the preset headwind area, the extended angle range is obtained; Calculate the new node x new relative neighboring point x near The angle between them; If the calculated angle is within the range of the extended angle, the new node x new If the node is valid, discard the new node x. new This expansion is now terminated.

7. A local path planning system for an unmanned sailboat, characterized in that, include: The marine map construction module is used to obtain the starting point, ending point and marine environment of the unmanned sailboat, and construct a marine map. The marine information in the marine environment includes navigable sea areas, unnavigable sea areas and wind field information. The random tree setting module is used to set the step size and maximum number of iterations, and to construct a random tree for the improved RRT* algorithm based on the starting point of the voyage; The new node acquisition module is used to select sampling point x from the sea area map. rand Select from the random tree the sampling point x rand The nearest neighbor x with the minimum path cost near From neighboring point x near Starting from sampling point x rand Expand step size in direction to obtain new node x new ; The headwind / headwind determination module is used to determine whether the unmanned sailboat is in a headwind state during the random tree expansion process. The wind field constraint determination module is used to determine the new node x when the unmanned sailboat is in a headwind. new Whether it falls outside the preset headwind constraint area; The random tree extension module is used to create a new node x. new If the sailboat falls outside the preset headwind constraint area or is in a downwind state, and at the new node x new After collision detection, the new node x new Add to the random tree; The parent node reselection module is used to select a new node x. new Reselect parent node; The rerouting module is used to reroute the random tree; The local path determination module is used to determine whether local path planning has been completed. Among them, in obtaining the new node x new And for the new node x new The path cost function used for path cost calculation is as follows: ； ； ； ； ； ； ； in, Represents node x near and x new The relative wind angle between them Represents node x near To x new The heading angle of the unmanned sailboat Represents node x near To x new Environmental wind angle; Represents node x i-1 and x i The relative wind angle between them Represents node x i-1 To x i The heading angle of the unmanned sailboat Represents node x i-1 To x i Environmental wind angle, V wind The value represents the ambient wind speed, and ω represents the wind influence weighting coefficient, used to adjust the magnitude of the wind field's impact on path cost. This represents the distance cost between adjacent nodes. This represents the wind field cost of adjacent nodes. For path cost function, Let i = 1, 2, 3, ..., N, where N represents the path cost from the starting point x on the current path. start To neighboring point x near The number of steps between, x start x1, x2, ..., x N Indicates x on the current path start To x near A series of nodes, x0=x start x N =x near .

8. The unmanned sailboat local path planning system according to claim 7, characterized in that, The marine map construction module constructs a marine map, specifically as follows: Marine information in the marine environment is acquired, and a raster method is used to construct a rasterized marine map. In this system, blank grids represent navigable sea areas, obstacle grids represent unnavigable sea areas, and wind field information is added to each grid. The obstacle grid corresponds to static obstacles and dynamic obstacles. Dynamic obstacles are dynamically updated by modifying the grid value through matrix indexing.

9. The unmanned sailboat local path planning system according to claim 8, characterized in that, The random tree expansion module uses an obstacle avoidance strategy for collision detection, which includes a static obstacle avoidance strategy and a dynamic obstacle avoidance strategy. From neighboring point x near To the new node x new A straight line is generated between the grid points, and sampling is performed according to the grid size to obtain a series of discrete points (x, y) along the straight line. i y i ), where i is the index from 1 to the number of discrete points M; The static obstacle avoidance strategy is as follows: Discrete point (x) i y i The coordinates of the new node x are mapped onto a rasterized sea map and compared with the coordinates of the static obstacles mapped onto the rasterized sea map. If the two coordinates overlap, it indicates a collision, and the new node x is discarded. new Otherwise, it indicates that there is no collision, and the new node x is... new Add to the random tree; The dynamic obstacle avoidance strategy is as follows: Calculate each discrete point (x) i y i (x) to the current coordinates of the dynamic obstacle d y d The Euclidean distance Dis; Determine if any Dis is less than or equal to the sum of the radius Dr of the dynamic obstacle and the distance buffer of the preset buffer. If so, a collision exists, and the new node x is discarded. new If not, it means there is no collision, and the new node x is... new Add it to the random tree.

Images