Unmanned surface vehicle path planning method and verification method based on improved Bi-RRT* algorithm

Abstract

The invention provides an unmanned surface vehicle path planning method and verification method based on an improved Bi-RRT * algorithm, and the method comprises the following steps: S1, improving a random point sampling mechanism of the Bi-RRT * algorithm, employing a target bias strategy, and enabling the random point probability to fall in a target region; s2, when a new node is selected, introducing an intelligent sample insertion algorithm; and S3, according to the steering performance requirement of the USV, the steering angle constraint is increased. The verification method comprises the following steps: carrying out simulation experiments in simple, complex and special water area environments; and comparing path planning effects of the PIB-RRT * algorithm, the Bi-RRT * algorithm and the RRT * algorithm, evaluating the algorithms by adopting planning time and path length as indexes, and counting an average value of each algorithm index. The directivity of node generation is defined; the speed at which the algorithm converges to the optimal solution is increased, and the requirement for high real-time performance of the USV during navigation is met; the steering angle constraint is increased, and the path is further smoothed, so that the motion constraint and the power constraint of the USV can be better met.

Description

Technical Field

[0001] The invention relates to a surface unmanned boat path planning method and a verification method based on an improved Bi-RRT* algorithm, and belongs to the technical field of unmanned boat path planning. Background Art

[0002] Although some algorithms for unmanned watercraft (USV) path planning are relatively mature, such as Dijkstra algorithm, genetic algorithm, A-star algorithm and particle swarm algorithm. However, in complex scenarios, these algorithms do not always meet the properties of optimality, feasibility or completeness. Dijkstra algorithm is a typical single-source shortest path algorithm, which is used to calculate the shortest path from one node to all other nodes. Although it has a high success rate and good robustness in obtaining the shortest path. However, in large-scale complex path topology networks, node traversal and low efficiency are its fatal flaws, so it is not suitable for complex sea environments. A* algorithm is an optimization algorithm of Dijkstra algorithm. Compared with Dijkstra algorithm, its search efficiency is improved, but the convergence speed is slow, the path has many turning points, and the distance to obstacles is too close. The planned route is not feasible and safe for unmanned watercraft when traveling on the sea. Particle swarm optimization (PSO) is an intelligent optimization algorithm. Its optimization method is very simple, easy to understand, does not require complex calculations, does not require too many parameter adjustments, and has stable robustness. However, the algorithm converges slowly in the later stage and is prone to fall into local optimality, making the planned path non-optimal. Genetic algorithms have good robustness and global optimization capabilities, but their calculation speed is slow and their local search capabilities are poor, making the planned routes unsafe and unfeasible.

[0003] The marine environment information is complex and changeable, and the above algorithms cannot guarantee the safety and real-time performance of planning. The Rapid Exploration Random Tree (RRT) algorithm is used to build a tree rooted at the starting point and explore the state space by randomly sampling in the space. Once the target position is sampled, the exploration process ends. The algorithm has strong spatial search capability, excellent obstacle avoidance performance, and strong real-time performance, which meets the requirements of USV navigation.

[0004] However, the RRT algorithm also has some limitations. For example, the solutions obtained using RRT are usually far from optimal, especially in complex environments, where these solutions may fall into local optimality. Moreover, almost all RRT-based methods are suboptimal. Since the path planned using the RRT algorithm is not optimal, the RRT* algorithm was proposed in 2011. It has asymptotic optimality (shortest distance) and adds random geometric mapping and pruning optimization theory to RRT. However, like the RRT algorithm, this algorithm uses a random search strategy during sampling, which leads to low path search efficiency, poor path smoothness, low path quality, and greatly increases its planning time in complex planning spaces.

[0005] In order to solve the above problems of the RRT* algorithm, researchers have proposed many improvement methods, which can be roughly divided into two categories: one is to improve the classic RRT algorithm in terms of expansion direction, step size selection, etc.

[0006] For example, Jordan et al. proposed a bidirectional version of the RRT* algorithm called the B-RRT* algorithm, which has a faster search speed, but the number of nodes calculated in complex environments is large and the planning efficiency is low. Jeong et al. used the principle of triangle inequality to reselect parent nodes and reconnect paths in RRT*, which greatly reduced the number of nodes, but the planned path has a large turning angle and is difficult to control the USV. Another type is the improvement of integration with other path planning algorithms, such as combining with the APF algorithm.

[0007] Compared with the classic RRT* algorithm, the improved algorithm controls the direction and step size of node expansion, reduces the number of nodes involved in the operation, and improves the convergence speed of the algorithm to a certain extent. However, there are still problems such as slow algorithm convergence speed, large path turning angles, and unevenness, which cannot meet the real-time and feasibility requirements of USV when sailing at sea. Summary of the invention

[0008] Purpose of the invention: In view of the deficiencies in the prior art, the present invention provides a surface unmanned vehicle path planning method and a verification method based on an improved Bi-RRT* algorithm. In view of the unresolved problems in the path planning based on the RRT* algorithm, the Bi-RRT* algorithm is improved: the random point sampling mechanism of the Bi-RRT* algorithm is improved, and a target bias strategy is adopted to make random points fall in the target area with a certain probability; an intelligent sample insertion algorithm is introduced when selecting new nodes to maximize the rewiring process in each generation and improve the speed at which the algorithm converges to the optimal solution; according to the steering performance requirements of the USV, a steering angle constraint is added so that the planned algorithm can be combined with the underlying control to reduce the difficulty of USV control.

[0009] Technical solution: A surface unmanned vehicle path planning method based on an improved Bi-RRT* algorithm includes the following steps:

[0010] S1. Improve the random point sampling mechanism of the Bi-RRT* algorithm and adopt the target bias strategy to make the random points fall in the target area with probability;

[0011] S2, introduces an intelligent sample insertion algorithm when selecting new nodes, maximizing the rewiring process in each generation and improving the speed at which the algorithm converges to the optimal solution;

[0012] S3. According to the steering performance requirements of USV, steering angle constraints are added so that the planned algorithm can be combined with the underlying control to reduce the control difficulty of USV. To ensure the effectiveness of the steering angle, the steering angle is controlled within the extreme steering angle range.

[0013] The S1 is specifically as follows: introducing a sampling probability λ (0≤λ≤1), using the rand function to generate random points, if rand<λ, the sampling point sample is set to rand, that is, randomly generating samples in the global space free area; if rand≥λ, the sampling point is set to the random point multiplied by the algorithm expansion step σ, thereby ensuring the probability completeness of the improved Bi-RRT* algorithm;

[0014]

[0015] The S2 is specifically as follows: Based on the Bi-RRT* bidirectional extended RRT* algorithm, the IB-RRT* new node intelligent sample insertion algorithm is introduced, and the near vertex sets from the trees Ta and Tb are respectively represented by q a near ,q b near It means that the IB-RRT* algorithm builds a bidirectional tree step by step;

[0016] First, from the barrier-free configuration space q free Select a random sample q from rand , that is, q rand ∈q free , then, it is in q rand Fill the spherical area with a radius of r as the center with a set of near vertices q of the two trees a near ,q b near , this ball area is recorded as the neighbor point set q near ,The ball region includes the close vertices from both trees;

[0017] In the case where both sets of close vertices are empty, these sets are filled with the closest vertices in their respective trees, i.e. the vertices on their respective trees that are closest to the random sample.

[0018] The IB-RRT* algorithm mainly includes three aspects: selecting the best parent node, path reconnection, and bidirectional tree connection;

[0019] The selecting the best parent node includes:

[0020] Initially calculate the best parent vertex of the two trees; for each neighbor point in the ball area, calculate the best parent vertex from the starting point q init The path cost to the neighboring point plus the path cost to q randThe path cost is calculated and the calculation results are sorted in a list; in this process, when the starting point is q a init When calculating the path cost, the nearest vertex of the tree Ta in the ball area is calculated. The minimum value of the path cost in the tree Ta is recorded as q a min ; When the starting point is q b init When calculating the path cost, the nearest vertex of the tree Tb in the ball area is calculated. At this time, the minimum value of the path cost in the tree Tb is recorded as q b min ;

[0021] When the list is sorted in ascending order and the minimum path cost appears, the random vertex q rand Will be inserted into the best choice tree, specifically: if q a min b min That is q rand If you are close to Ta, put the random point q rand Insert into Ta, the minimum value x of the path in the two trees min =q a min ; if q a min >q b min That is q rand If the distance to Tb is close, the random point q rand Insert into Tb, the minimum value x of the path in the two trees min =q b min , the algorithm will also return the nearest vertex on the corresponding best choice tree, which is eligible to become the parent node of the random sample;

[0022] The path reconnection includes:

[0023] After selecting the best parent node, the random vertex q rand Will be inserted into the best choice tree, i.e. random vertex q rand It becomes one of the expanded nodes in the tree;

[0024] Then the improved Bi-RRT* algorithm starts to check the random sample q rand As a set q near The feasibility of the parent vertex of each vertex in;

[0025] Let vertex X∈q near , if through random sample q rand Arrival of neighboring set q near The cost of a particular vertex X in is lower than the existing cost, then q​rand Become the specific vertex X∈q near The parent vertex of , retains the shortest path;

[0026] The bidirectional tree connection includes:

[0027] When q rand When the region ball with centered contains close vertices from two trees Ta and Tb, find the minimum path cost x between the two trees a min , the improved Bi-RRT* algorithm will connect the two trees, otherwise the connection will fail; finally, the trees are connected to generate an end-to-end global path.

[0028] Specifically, S3 is as follows: when the USV is sailing at sea, due to its own dynamic and kinematic constraints, the steering angle is limited to 0° to 40°; let the new node inserted in this iteration be q new , calculate the vector q of the last iteration process a min q rand With vector q rand q new Angle θ: When θ>40°, it indicates that the new node q new The steering angle constraint is not met, so q is discarded. new ; When θ≦40°, the expansion tree is successfully extended and enters the next iteration, and q new Insert into the tree.

[0029] The verification method of the surface unmanned vehicle path planning method based on the improved Bi-RRT* algorithm includes the following steps:

[0030] Using MATLAB, simulation experiments were conducted in three water environments: simple, complex, and special.

[0031] Before the improved Bi-RRT* algorithm is implemented, the environment is modeled using the grid method. Each grid represents its own pixel information, and the color information of each grid is represented by an 8-bit binary number. Different colors are used to mark obstacle areas and non-obstacle areas. Different colors represent different RGB values, which is convenient for identifying obstacles.

[0032] The path planning effects of the PIB-RRT* algorithm, Bi-RRT* algorithm and RRT* algorithm were compared. The planning time and path length were used as indicators to evaluate the algorithms. 50 optimization experiments were conducted in each group, and the average value of each algorithm indicator was calculated.

[0033] The simple water environment experiment specifically includes: setting up 8 obstacles, represented by black circles of different radii;

[0034] The environment map is divided into 630*840 grids using the grid method. The starting point is set to (15,614), the target point is set to (767,39), and a total of 8 obstacles of different sizes and positions are set on the path from the starting point to the end point. The positions of the obstacles are arranged symmetrically along the straight line from the starting point to the end point, and the sizes of the obstacles are arranged from small to large from the starting point to the end point. After the final route is formed, the planning time, path length and the degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

[0035] The complex water environment experiment specifically includes: setting up 14 obstacles; using the grid method to divide the environment map into 630*840 grids, setting the starting point to (15,614), the target point to (767,39), and setting a total of 14 circular obstacles of different sizes on the path from the starting point to the end point; adding 6 obstacles of different sizes and randomly arranged on the basis of the simple water environment. After the final route is formed, the planning time, path length and degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

[0036] The special water environment experiment specifically includes: setting up a real sea environment; using the grid method to divide the environment map into 720*960 grids, setting the starting point to (55,493), and the target point to (905,199). The selected sea environment waters are tortuous, and the width of the water at the corner is lower than the width of the water from the starting point to the corner and from the corner to the end point. Obstacles such as reefs are set in the water. After the final route is formed, the planning time, path length, and degree of curvature of the path at the corner used by the three algorithms in the same environment are compared.

[0037] Beneficial effects: The present invention improves the random point generation mechanism of the bidirectional rapidly expanding random tree (Bi-RRT*) algorithm. In the process of generating random points, a target bias strategy is added to clarify the directionality of node generation; intelligent insertion of nodes (samples) is added to further improve the speed at which the algorithm converges to the optimal solution, meeting the strong real-time requirements of USV during navigation; steering angle constraints are added to further smooth the path, so that it better meets the motion constraints and power constraints of USV. In order to verify the superiority of the improved algorithm (PIB-RRT* algorithm) in terms of convergence speed and path smoothness, simulation experiments were carried out on the PIB-RRT* algorithm, the Bi-RRT* algorithm, and the RRT* algorithm in three water environments: simple, complex, and special. The results show that compared with the algorithms before the improvement, the target bias, intelligent node insertion, and steering angle constraints of the present invention can better improve the convergence speed and path quality, such as improving path smoothness and reducing path length. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0039] Figure 1 Schematic diagram of the process of calculating a group of near nodes from two trees in the IB-RRT* algorithm of the present invention, where q parent Represents the neighbor point q near The parent node of .

[0040] Figure 2 This is a schematic diagram of the process of selecting a parent node in the IB-RRT* algorithm process of the present invention.

[0041] Figure 3 Schematic diagram of the process of reselecting a parent node in the IB-RRT* algorithm of the present invention.

[0042] Figure 4 It is a schematic diagram of the path reconnection process in the IB-RRT* algorithm process of the present invention.

[0043] Figure 5 This is a schematic diagram of the process of forming the final path during the IB-RRT* algorithm of the present invention.

[0044] Figure 6 The steering angle constraint process of the present invention takes the extended book Ta as an example, FIG.

[0045] Figure 7 This is a simulation of the present invention in a simple environment.

[0046] Figure 8 This is a simulation of the present invention in a complex environment.

[0047] Fig. 9 This is a simulation of the present invention in a special water environment. DETAILED DESCRIPTION

[0048] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0049] In the description of the present invention, it is necessary to understand that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0050] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0051] A path planning method for a surface unmanned vehicle based on an improved Bi-RRT* algorithm comprises the following steps:

[0052] S1. Improve the random point sampling mechanism of the Bi-RRT* algorithm and adopt the target bias strategy to make the random points fall in the target area with probability;

[0053] S2, introduces an intelligent sample insertion algorithm when selecting new nodes, maximizing the rewiring process in each generation and improving the speed at which the algorithm converges to the optimal solution;

[0054] S3. According to the steering performance requirements of USV, steering angle constraints are added so that the planned algorithm can be combined with the underlying control to reduce the control difficulty of USV. To ensure the effectiveness of the steering angle, the steering angle is controlled within the extreme steering angle range.

[0055] The S1 is specifically as follows: introducing a sampling probability λ (0≤λ≤1), using the rand function to generate random points, if rand<λ, the sampling point sample is set to rand, that is, randomly generating samples in the global space free area; if rand≥λ, the sampling point is set to the random point multiplied by the algorithm expansion step σ, thereby ensuring the probability completeness of the improved Bi-RRT* algorithm;

[0056]

[0057] The S2 is specifically as follows: Based on the Bi-RRT* bidirectional extended RRT* algorithm, the IB-RRT* new node intelligent sample insertion algorithm is introduced, and the near vertex sets from the trees Ta and Tb are respectively represented by q a near ,q b near It means that the IB-RRT* algorithm builds a bidirectional tree step by step;

[0058] The improved Bi-RRT* algorithm is used to calculate the obstacle-free configuration space q free By starting from the starting point q start and the end point q goal Start to build two search trees Ta and Tb respectively, and set the starting point of Ta to q start Start the extended search, Tb from the end point q goal Start to expand the search until the two trees are connected and finally achieve path planning. The specific steps are as follows: select any random sample q in the search space rand As the sampling point, q rand ∈q free , then, q rand Fill the spherical area with a radius of r as the center with a set of near vertices q of the two trees a near ,q b near , this ball area is recorded as the neighbor point set q near , the ball region includes close vertices from both trees, such as Figure 1 As shown, when there are two near vertex sets q a near ,q b near If both are empty, the sets will be filled with the nearest vertex in their respective trees, that is, the vertex in their respective trees that is closest to the random sample.

[0059] Then, start calculating Ta from the starting point to q a near The distances of all neighboring points in the sum of the distances from the neighboring points to q rand The sum of the distances from the starting point to q; similarly calculate Tb from the starting point to q b near The distances of all neighboring points in the sum of the distances from the neighboring points to q rand The sum of the distances. Sort all the calculated distances by size. When the distance is the smallest, the random vertex q rand It will be inserted into the tree with the smallest corresponding distance, which is the new node intelligent sample insertion method.

[0060] The original Bi-RRT* algorithm first searches and expands one of the search trees Ta, and then selects any random sample q in the search space. randAs the sampling point, q rand ∈q free ,

[0061] Find the distance q on the search tree Ta rand The nearest node q near and move to q with a certain step length of 20 rand Expand in the direction to get a new node q new Next, check q near and q new Is there an obstacle between them? If there is no obstacle, the new node q new Add to the search tree, otherwise abandon q new , which is the sample insertion method of the Bi-RRT* algorithm. The expansion steps of the second tree are the same as those of the first tree. The two trees are continuously expanded alternately until the distance between them is less than the set threshold. At this time, the q new Nodes start to move towards their respective q near The nodes are backtracked to generate the nodes of the path. Finally, the path is generated by connecting the nodes with line segments.

[0062] The IB-RRT* algorithm mainly includes three aspects: selecting the best parent node, path reconnection, and bidirectional tree connection;

[0063] The selecting the best parent node includes:

[0064] Initially calculate the best parent vertex of the two trees; for each neighbor point in the ball area, calculate the best parent vertex from the starting point q init The path cost to the neighboring point plus the path cost to q rand The path cost is calculated and the calculation results are sorted in a list; in this process, when the starting point is q a init When calculating the path cost, the nearest vertex of the tree Ta in the ball area is calculated. The minimum value of the path cost in the tree Ta is recorded as q a min ; When the starting point is q b init When calculating the path cost, the nearest vertex of the tree Tb in the ball area is calculated. At this time, the minimum value of the path cost in the tree Tb is recorded as q b min ;

[0065] When the list is sorted in ascending order and the minimum path cost appears, the random vertex q rand Will be inserted into the best choice tree, specifically: if q a min b min That is q rand If you are close to Ta, put the random point q​rand Insert into Ta, the minimum value x of the path in the two trees min =q a min ; if q a min >q b min That is q rand If the distance to Tb is close, the random point q rand Insert into Tb, the minimum value x of the path in the two trees min =q b min , the algorithm will also return the nearest vertex on the corresponding best choice tree, which is eligible to become the parent node of the random sample; Figure 2 As shown, Ta is the best choice tree, and the minimum value x of the path in the two trees min =q a min ;

[0066] The path reconnection includes:

[0067] After selecting the best parent node, the random vertex q rand Will be inserted into the best choice tree, i.e. random vertex q rand It becomes one of the expanded nodes in the tree;

[0068] Then the improved Bi-RRT* algorithm starts to check the random sample q rand As a set q near The feasibility of the parent vertex of each vertex in;

[0069] Let vertex X∈q near , if through random sample q rand Arrival of neighboring set q near The cost of a particular vertex X in is lower than the existing cost, then q rand Become the specific vertex X∈q near The parent vertex of , retains the shortest path; such as Figure 3 As shown, from q a init to q rand Then to x min The path cost is the same as the existing cost, so keep the path from q rand Then to x min The path, q rand Paths to other neighboring points are discarded.

[0070] The bidirectional tree connection includes:

[0071] When q rand When the region ball with centered contains close vertices from two trees Ta and Tb, find the minimum path cost x between the two treesa min , the improved Bi-RRT* algorithm will connect the two trees, otherwise the connection will fail; finally, the trees are connected to generate an end-to-end global path. Therefore, unlike the Bi-RRT* heuristic connection, the IB-RRT* algorithm is not greedy because the connection is only performed inside the ball area, such as Figure 4 Finally, the tree connection generates an end-to-end global path, as shown in Figure 5 shown.

[0072] Specifically, S3 is: when the USV is sailing at sea, due to its own dynamic and kinematic constraints, the steering angle is limited to 0° to 40°; in order to better meet the actual operation conditions at sea, this paper sets the maximum steering angle to 38°. Figure 6 , taking the extended tree Ta as an example, let the new node inserted in this iteration be q new , calculate the vector q of the last iteration process a min q rand With vector q rand q new Angle θ: When θ>38°, it indicates that the new node q new The steering angle constraint is not met, so q is discarded new ; When θ≦38°, the expansion tree is successfully extended and enters the next iteration, and q new Insert into the tree.

[0073] The performance of the improved PIB-RRT* algorithm will be analyzed from three aspects: sampling method, intelligent insertion of new nodes, and steering angle constraints.

[0074] ⑴ Sampling method

[0075] The PIB-RRT* algorithm uses the target bias sampling method to replace the global uniform sampling strategy in RRT*, which can improve the sampling efficiency of the algorithm. Compared with the expansion of one tree in RRT*, the PIB-RRT* algorithm expands two trees at the same time, which can speed up the algorithm's calculation speed.

[0076] ⑵ Intelligent insertion of new nodes

[0077] Since RRT* samples points more densely near the generation point during the search process. Therefore, if the sampling point is closer to the generation point of the tree, the density of neighboring points contained in its neighborhood is greater. When the algorithm performs path reconnection, it detects whether the cost of the random sample qrand to reach a specific vertex x' in the neighboring set qnear is lower than the existing cost. If it is lower, qrand becomes the parent vertex of the specific vertex x'∈qnear. Therefore, PIB-RRT* inserts random points into areas with a higher density of neighboring points, allowing more nodes to participate in path reconnection and maximizing the rewiring process for each iteration. This step allows fast convergence to the optimal solution.

[0078] ⑶ Steering angle constraint

[0079] In order to make the planned algorithm easier to control the USV, the steering angle constraint was added to the improved algorithm, which reduced the number of nodes involved in turning and avoided frequent large-angle steering left and right when detouring, making the planned path smoother and the algorithm convergence faster.

[0080] The verification method of the surface unmanned vehicle path planning method based on the improved Bi-RRT* algorithm includes the following steps:

[0081] Using MATLAB, simulation experiments were conducted in three water environments: simple, complex, and special.

[0082] Before the improved Bi-RRT* algorithm is implemented, the environment is modeled using the grid method. Each grid represents its own pixel information, and the color information of each grid is represented by an 8-bit binary number. Different colors are used to mark obstacle areas and non-obstacle areas. Different colors represent different RGB values, which is convenient for identifying obstacles.

[0083] The path planning effects of the PIB-RRT* algorithm, Bi-RRT* algorithm and RRT* algorithm were compared. The planning time and path length were used as indicators to evaluate the algorithms. 50 optimization experiments were conducted in each group, and the average value of each algorithm indicator was calculated.

[0084] The simple water environment experiment specifically includes: setting up 8 obstacles, represented by black circles of different radii;

[0085] The environment map is divided into 630*840 grids using the grid method. The starting point is set to (15,614), the target point is set to (767,39), and a total of 8 obstacles of different sizes and positions are set on the path from the starting point to the end point. The positions of the obstacles are arranged symmetrically along the straight line from the starting point to the end point, and the sizes of the obstacles are arranged from small to large from the starting point to the end point. After the final route is formed, the planning time, path length and the degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

[0086] The final path planning results of the three algorithms are shown in Figure 7 , the statistical results of the indicators are shown in Table 1. Compared with the paths planned by the other two algorithms: the PIB-RRT* algorithm has greatly improved the path length, path smoothness and algorithm convergence speed because it introduces target bias and steering angle constraints; the Bi-RRT* algorithm searches for expansion in the global space, so the planning efficiency is not high; compared with the Bi-RRT* algorithm, the planning time of the PIB-RRT* algorithm is reduced by 78%; the RRT* algorithm is a one-way search of a tree, and the optimal path is not considered during the expansion process, so the planned path is more tortuous. Compared with the RRT* algorithm, the path length planned by the PIB-RRT* algorithm is reduced by 27%.

[0087] Table 1 Simulation of the algorithm in a simple environment

[0088]

[0089] The complex water environment experiment specifically includes: setting up 14 obstacles; using the grid method to divide the environment map into 630*840 grids, setting the starting point to (15,614), the target point to (767,39), and setting a total of 14 circular obstacles of different sizes on the path from the starting point to the end point; adding 6 obstacles of different sizes and randomly arranged on the basis of the simple water environment. After the final route is formed, the planning time, path length and degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

[0090] The final path planning results of the three algorithms are shown in Figure 8 The statistical results of the indicators are shown in Table 2. Compared with the Bi-RRT* algorithm, the PIB-RRT* algorithm reduces the time by 43%, and compared with the RRT* algorithm, the time reduced by 87%. Figure 8 It can be seen that although the path length of the PIB-RRT* algorithm is not much different from that of the Bi-RRT* algorithm, the path planned by the PIB-RRT* algorithm is smoother and has a smaller angle at the turning point, while the path of the RRT* algorithm is more tortuous.

[0091] Table 2 Algorithm simulation in complex environment

[0092]

[0093] The special water environment experiment specifically includes: setting up a real sea surface environment; using the grid method to divide the environment map into 720*960 grids, setting the starting point to (55,493), and the target point to (905,199). The selected sea surface environment waters are tortuous, and the width of the water at the corner is lower than the width of the water from the starting point to the corner and from the corner to the end point. Reef obstacles are set in the waters. After the final route is formed, the planning time, path length, and degree of curvature of the path at the corner used by the three algorithms in the same environment are compared.

[0094] The final path planning results of the three algorithms are shown in Fig. 9 The statistical results of the indicators are shown in Table 3. In the real sea environment, the PIB-RRT* algorithm takes less time than the other two algorithms, which is about 3% of the RRT* algorithm and 10.7% of the Bi-RRT* algorithm, and the planned path is the shortest. In addition, since the corner constraint is added to the algorithm, the path at the corner is also smoother, which is more in line with the real-time and feasibility requirements of USV navigation at sea.

[0095] Table 3 Algorithm simulation in special water environment

[0096]

[0097]

[0098] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0099] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. The path planning method for unmanned surface vehicle based on the improved Bi-RRT* algorithm is characterized by: The following steps are involved: S1. Improve the random point sampling mechanism of the Bi-RRT* algorithm and adopt the target bias strategy to make the random points fall in the target area with probability; S2, introduces an intelligent sample insertion algorithm when selecting new nodes, maximizing the rewiring process in each generation and improving the speed at which the algorithm converges to the optimal solution; S3. According to the steering performance requirements of USV, steering angle constraints are added so that the planned algorithm can be combined with the underlying control to reduce the control difficulty of USV. To ensure the effectiveness of the steering angle, the steering angle is controlled within the extreme steering angle range.

2. The method for unmanned surface vehicle path planning based on the improved Bi-RRT* algorithm according to claim 1 is characterized in that: The S1 is specifically: The sampling probability λ (0 ≤ λ≤1) is introduced, and the rand function is used to generate random points. If rand<λ, the sampling point sample is set to rand, that is, random sampling is generated in the global space free area; if rand≥λ, the sampling point is set to the random point multiplied by the algorithm expansion step σ, which ensures the probability completeness of the improved Bi-RRT* algorithm; (1).

3. The surface unmanned vehicle path planning method based on the improved Bi-RRT* algorithm according to claim 2 is characterized in that: The S2 is specifically: Based on the Bi-RRT* bidirectional extended RRT* algorithm, the IB-RRT* new node intelligent sample insertion algorithm is introduced. Suppose the near vertex sets from trees Ta and Tb are respectively represented by q a near ,q b near It means that the IB-RRT* algorithm builds a bidirectional tree step by step; First, from the barrier-free configuration space q free Select a random sample q from rand , that is, q rand ∈q free , then, it is in q rand Fill the spherical area with a radius of r as the center with a set of near vertices q of the two trees a near ,q b near , this ball area is recorded as the neighbor point set q near ,The ball region includes the close vertices from both trees; In the case where both sets of close vertices are empty, these sets are filled with the closest vertices in their respective trees, i.e. the vertices on their respective trees that are closest to the random sample.

4. The method for path planning of an unmanned surface vehicle based on the improved Bi-RRT* algorithm according to claim 3 is characterized in that: The IB-RRT* algorithm mainly includes three aspects: selecting the best parent node, path reconnection, and bidirectional tree connection; The selecting the best parent node includes: Initially calculate the best parent vertex of the two trees; for each neighbor point in the ball area, calculate the best parent vertex from the starting point q init The path cost to the neighboring point plus the path cost to q rand The path cost is calculated and the calculation results are sorted in a list; in this process, when the starting point is q a init When calculating the path cost, the nearest vertex of the tree Ta in the ball area is calculated. The minimum value of the path cost in the tree Ta is recorded as q a min ; When the starting point is q b init When calculating the path cost, the nearest vertex of the tree Tb in the ball area is calculated. The minimum value of the path cost in the tree Tb is recorded as q b min ; When the list is sorted in ascending order and the minimum path cost appears, the random vertex q rand Will be inserted into the best choice tree, specifically: if q a min b min That is q rand If you are close to Ta, put the random point q rand Insert into Ta, the minimum value x of the path in the two trees min =q a min ; if q a min >q b min That is q rand If the distance to Tb is close, the random point q rand Insert into Tb, the minimum value x of the path in the two trees min =q b min , the algorithm will also return the nearest vertex on the corresponding best choice tree, which is eligible to become the parent node of the random sample;​ The path reconnection includes: After selecting the best parent node, the random vertex q rand Will be inserted into the best choice tree, i.e. random vertex q rand It becomes one of the expanded nodes in the tree; Then the improved Bi-RRT* algorithm starts to check the random sample q rand As a set q near The feasibility of the parent vertex of each vertex in; Let vertex X∈q near , if through random sample q rand Arrival of neighboring set q near The cost of a particular vertex X in is lower than the existing cost, then q rand Become the specific vertex X∈ q near The parent vertex of , retain the shortest path; The bidirectional tree connection includes: When q rand When the region ball with centered contains close vertices from two trees Ta and Tb, find the minimum path cost x between the two trees a min , the improved Bi-RRT* algorithm will connect the two trees, otherwise the connection will fail; finally, the trees are connected to generate an end-to-end global path.

5. The method for path planning of an unmanned surface vehicle based on the improved Bi-RRT* algorithm according to claim 4 is characterized in that: The S3 is specifically: When the USV is sailing at sea, due to its own dynamic and kinematic constraints, the steering angle is limited to 0°~40°; let the new node inserted in this iteration be q new , calculate the vector q of the last iteration process a min q rand With vector q rand q new Angle θ: When θ>40°, it indicates that the new node q new The steering angle constraint is not met, so q is discarded new ; When θ≦40°, the expansion tree is successfully extended and enters the next iteration, and q new Insert into the tree.

6. The verification method of the surface unmanned vehicle path planning method based on the improved Bi-RRT* algorithm according to claim 1 is characterized in that: The following steps are involved: Using MATLAB, simulation experiments were conducted in three water environments: simple, complex, and special. Before the improved Bi-RRT* algorithm is implemented, the environment is modeled using the grid method. Each grid represents its own pixel information, and the color information of each grid is represented by an 8-bit binary number. Different colors are used to mark obstacle areas and non-obstacle areas. Different colors represent different RGB values, which is convenient for identifying obstacles. The path planning effects of the PIB-RRT* algorithm, Bi-RRT* algorithm and RRT* algorithm were compared. The planning time and path length were used as indicators to evaluate the algorithms. 50 optimization experiments were conducted in each group, and the average value of each algorithm indicator was calculated.

7. The verification method of the unmanned surface vehicle path planning method based on the improved Bi-RRT* algorithm according to claim 6 is characterized by: The simple water environment experiment specifically includes: setting up 8 obstacles, represented by black circles of different radii; The environment map is divided into 630*840 grids using the grid method. The starting point is set to (15,614), the target point is set to (767,39), and a total of 8 obstacles of different sizes and positions are set on the path from the starting point to the end point. The positions of the obstacles are arranged symmetrically along the straight line from the starting point to the end point, and the sizes of the obstacles are arranged from small to large from the starting point to the end point. After the final route is formed, the planning time, path length and degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

8. The verification method of the unmanned surface vehicle path planning method based on the improved Bi-RRT* algorithm according to claim 7 is characterized in that: The complex water environment experiment specifically includes: setting up 14 obstacles; using the grid method to divide the environment map into 630*840 grids, setting the starting point to (15,614), the target point to (767,39), and setting a total of 14 circular obstacles of different sizes on the path from the starting point to the end point; adding 6 randomly arranged obstacles of different sizes on the basis of the simple water environment. After the final route is formed, the planning time, path length and degree of curvature of the path at the corners used by the three algorithms in the same environment are compared.

9. The verification method of the unmanned surface vehicle path planning method based on the improved Bi-RRT* algorithm according to claim 8 is characterized by: The special water environment experiment specifically includes: setting up a real sea surface environment; using the grid method to divide the environment map into 720*960 grids, setting the starting point to (55,493) and the target point to (905,199); the selected sea surface environment has tortuous waters, the width of the water at the corner is lower than the width of the water from the starting point to the corner and from the corner to the end point, and setting up obstacles such as reefs in the water. After the final route is formed, the planning time, path length and degree of curvature of the path at the corner used by the three algorithms in the same environment are compared.

Images