A path planning method and system for unmanned boat based on improved RRT algorithm

Abstract

The present invention relates to an unmanned boat path planning method and system based on an improved RRT algorithm, belonging to the field of unmanned boat path planning. The method comprises: constructing a map model; initializing a random tree T, and setting a starting point Q start , End point Q goal Add to the random tree T; randomly obtain sampling points Q in the map rand , and find the distance Q rand The nearest node Q nearest ; Use vector #imgabs0# as the expansion direction and s as the expansion step to get the new node Q new ; For the new node Q new Perform obstacle collision detection; select the algorithm's termination detection method according to the endpoint connection probability q; goal First, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start The method of the present invention can reduce the time of path planning and eliminate the path quality problem caused by the randomness brought by the terminal connection probability.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and in particular to an unmanned boat path planning method and system based on an improved RRT algorithm. Background Art

[0002] Unmanned boats are small autonomous surface platforms. With high sensitivity and maneuverability, they play an irreplaceable role in water quality survey, emergency rescue, transportation, etc. However, the level of sensitivity and maneuverability depends largely on the quality of the autonomous path planned by the unmanned boat. A better path can enable the unmanned boat to reach the destination quickly and accurately to perform the corresponding task. Not only that, the quality of the path plays a decisive role in the energy consumption and wear degree of the unmanned boat. Therefore, a good path planning algorithm has laid a good foundation for the application of surface unmanned boats in more fields such as military and civilian.

[0003] Path planning can be divided into global path planning and local path planning according to the degree of understanding of environmental information. Global path planning mainly studies how to plan a safe and optimal path from the starting point to the end point in a static map based on obstacle information. At present, most unmanned boats commonly use global planning algorithms based on graph search, such as A*, D*, D*Lite, etc., but the efficiency of these search algorithms is overly dependent on the size of the grid map. In addition, the search characteristics of the graph search algorithm itself affect the path planning speed, resulting in the search-based global planning algorithm can only be used for unmanned boats with smaller operating scenes. In recent years, a large number of researchers have focused on using the Rapidly-Exploring RandomTree (RRT) algorithm to solve the path planning problem of surface unmanned boats in simple scenes such as open sea or sparse obstacles, as well as performing medium and long-distance tasks, and improve the motion performance of unmanned boats by continuously improving the algorithm. However, there is still no better solution to the redundant time cost caused by the single termination condition of the RRT algorithm.

[0004] In view of this, how to improve the search termination conditions of the RRT algorithm to reduce the time cost of the algorithm's search path and solve the slow planning speed of unmanned boats in simple scenarios such as open sea areas or sparse obstacles and when performing medium and long-distance tasks is a technical problem that needs to be solved urgently. Summary of the invention

[0005] The purpose of the present invention is to provide an unmanned boat path planning method and system based on an improved RRT algorithm to solve the problem of slow planning speed of unmanned boats in simple scenarios such as open sea areas or sparse obstacles and when performing medium and long-distance tasks.

[0006] To achieve the above object, the present invention provides the following solutions:

[0007] A path planning method for an unmanned boat based on an improved RRT algorithm, the path planning method comprising:

[0008] S1: Build map model;

[0009] S2: Obtaining obstacle areas and free areas in the map;

[0010] S3: Set the starting point Q of the unmanned boat start , End point Q goal , endpoint connection probability q, endpoint connection threshold a and expansion step size s;

[0011] S4: Initialize the random tree T and set the starting point Q start Add to the random tree T;

[0012] S5: Randomly obtain a sampling point Q in the map rand , and find the distance from the sampling point Q in the random tree T rand The nearest node Q nearest , the node Q nearest As the sampling point Q rand The parent node Q parent , sampling point Q rand As Q parent The child nodes of

[0013] S6: Vector is the expansion direction, s is the expansion step to get the new node Q new ;

[0014] S7: Use collision() function to deal with the new node Q new Perform obstacle collision detection;

[0015] S8: Determine the new node Q new Whether the collision detection is passed,

[0016] S9: If passed, the new node Q will pass the collision detection new And its parent node information is added to the random tree T;

[0017] S10: If the collision detection fails, return to S5;

[0018] S11: selecting a termination detection method of an algorithm according to the endpoint connection probability q; the termination detection method includes: a first termination detection algorithm and a second termination detection algorithm;

[0019] S12: If the first termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, the algorithm is terminated, otherwise it returns to S5;

[0020] S13: If the second termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, the terminal parent node reselection mechanism is executed, and the terminal Q is selected in the candidate set of the terminal parent node. goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to S5;

[0021] S14: In the random tree T, from the end point Q goal Towards the starting point Q start Backtracking path: From Q goal First, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start until;

[0022] S15: Connect the nodes in path() in sequence to obtain a preliminary path;

[0023] S16: Control the unmanned boat to travel along the preliminary path.

[0024] Optionally, the termination detection method is selected according to the following formula:

[0025]

[0026] Where A is the algorithm termination detection method finally selected, q is the endpoint connection probability, and p1 is a random number between (0, 1) generated by the random number generation function rand().

[0027] Optionally, the first termination detection algorithm is:

[0028] Calculate the new node Q each time through the collision detection new To the end Q goal The distance z, if Q new , Q goal The line segments formed satisfy no collision and z≤a, then Q new As Q goal The parent node of Q goal And its corresponding parent node information is added to the random tree T, and the algorithm terminates.

[0029] Optionally, the second termination detection algorithm is:

[0030] Connect the current new node Q obtained by expansion new and the end point Q goal , determine whether the line segment collides with the obstacle. If no collision occurs, the current new node Q new Temporarily as Q goal The parent node of Q goal And its parent node information is added to the random tree.

[0031] Optionally, the endpoint parent node reselection mechanism includes: a first endpoint parent node reselection mechanism, a second endpoint parent node reselection mechanism, a third endpoint parent node reselection mechanism, and a fourth endpoint parent node reselection mechanism:

[0032] The first terminal parent node reselection mechanism is: randomly selecting a preset percentage of candidate nodes from the random tree T and adding them to the terminal parent node candidate set;

[0033] The second terminal parent node reselection mechanism is: considering the update method of the random tree, taking the last expanded part of the nodes as candidate nodes and adding them to the terminal parent node candidate set;

[0034] The third terminal parent node reselection mechanism is: in the manner of reselecting the parent node according to the RRT algorithm, a circular closed area is constructed with the parent node of the current terminal as the center and the preset distance as the radius, and the nodes falling within the circle are added to the terminal parent node candidate set;

[0035] The fourth end point parent node reselection mechanism is: the parent node straight line Q of the current end point start , Q goal The distance between the nodes in the random tree and the straight line Q is l. start , Q goal The distance L i , will satisfy L i The nodes corresponding to <L are added to the parent node candidate set.

[0036] Based on the above method in the present invention, the present invention further provides an unmanned boat path planning system based on an improved RRT algorithm, the system comprising:

[0037] Map construction module, used to build map models;

[0038] An obstacle area and free area acquisition module, used to acquire the obstacle area and free area in the map;

[0039] Parameter setting module, used to set the starting point Q of the unmanned boat start , End point Q goal , endpoint connection probability q, endpoint connection threshold a and expansion step size s;

[0040] Initialization module, used to initialize the random tree T and set the starting point Q start Add to the random tree T;

[0041] A sampling point acquisition module is used to randomly acquire a sampling point Q in the map. rand , and find the distance from the sampling point Q in the random tree T rand The nearest node Q nearest , the node Qnearest As the sampling point Q rand The parent node Q parent , sampling point Q rand As Q parent The child nodes of

[0042] New node determination module for vector is the expansion direction, s is the expansion step to get the new node Q new ;

[0043] Collision detection module, used to use collision() function to detect new node Q new Perform obstacle collision detection;

[0044] A judgment module is used to judge the new node Q new Whether the collision detection is passed,

[0045] Random tree update module, used to update the new node Q that passes the collision test when it passes the collision test new And its parent node information is added to the random tree T;

[0046] The first loop module is used to return to the "sampling point acquisition module" when the collision detection fails;

[0047] A termination detection mode selection module is used to select a termination detection mode of the algorithm according to the endpoint connection probability q; the termination detection mode includes: a first termination detection algorithm and a second termination detection algorithm;

[0048] The second loop module is used for selecting the first termination detection algorithm to perform algorithm termination judgment, and when the judgment result is true, the algorithm terminates, otherwise it returns to the "sampling point acquisition module";

[0049] The third loop module is used to execute the terminal parent node reselection mechanism when the second termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, and the terminal parent node candidate set is constructed as the terminal Q goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to the "sampling point acquisition module";

[0050] A path backtracking module is used to start from the end point Q in the random tree T. goal Towards the starting point Q start Backtracking path: From Q goal First, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start until;

[0051] The path determination module is used to sequentially connect the nodes in path() to obtain a preliminary path;

[0052] The unmanned boat control module is used to control the unmanned boat to travel along the preliminary path.

[0053] Optionally, the termination detection mode in the termination detection mode selection module is selected according to the following formula:

[0054]

[0055] Where A is the algorithm termination detection method finally selected, q is the endpoint connection probability, and p1 is a random number between (0, 1) generated by the random number generation function rand().

[0056] Optionally, the first termination detection algorithm is:

[0057] Calculate the new node Q each time through the collision detection new To the end Q goal The distance z, if Q new , Q goal The line segments formed satisfy no collision and z≤a, then Q new As Q goal The parent node of Q goal And its corresponding parent node information is added to the random tree T, and the algorithm terminates.

[0058] Optionally, the second termination detection algorithm is:

[0059] Connect the current new node Q obtained by expansion new and the end point Q goal , determine whether the line segment collides with the obstacle. If no collision occurs, the current new node Q new Temporarily as Q goal The parent node of Q goal And its parent node information is added to the random tree.

[0060] Optionally, the endpoint parent node reselection mechanism includes: a first endpoint parent node reselection mechanism, a second endpoint parent node reselection mechanism, a third endpoint parent node reselection mechanism, and a fourth endpoint parent node reselection mechanism:

[0061] The first terminal parent node reselection mechanism is: randomly selecting a preset percentage of candidate nodes from the random tree T and adding them to the terminal parent node candidate set;

[0062] The second terminal parent node reselection mechanism is: considering the update method of the random tree, taking the last expanded part of the nodes as candidate nodes and adding them to the terminal parent node candidate set;

[0063] The third terminal parent node reselection mechanism is: in the manner of reselecting the parent node according to the RRT algorithm, a circular closed area is constructed with the parent node of the current terminal as the center and the preset distance as the radius, and the nodes falling within the circle are added to the terminal parent node candidate set;

[0064] The fourth end point parent node reselection mechanism is: the parent node straight line Q of the current end point start , Q goal The distance between the nodes in the random tree and the straight line Q is l. start , Q goal The distance L i , will satisfy L i The node corresponding to <l is added to the parent node candidate set.

[0065] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0066] The above method in the present invention introduces the endpoint connection probability q to connect the new node obtained by each expansion with the endpoint with a certain probability. If a certain connection is safe and collision-free, this feasible node is added to the random tree as the parent node of the endpoint, thereby realizing early termination of the algorithm, reducing the time cost of the algorithm, and reducing the path planning time. Then, by establishing a terminal parent node reselection mechanism, the path quality problem caused by the randomness brought by the terminal connection probability is eliminated, thereby accelerating the path planning speed of the unmanned boat in a simple scenario. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] 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 will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0068] Figure 1 This is a flowchart of the unmanned boat path planning method using the improved RRT algorithm of the present invention;

[0069] Figure 2 It is a schematic diagram of the expansion of the original RRT algorithm in the barrier-free environment of the present invention;

[0070] Figure 3 It is a schematic diagram of the expansion of the improved RRT algorithm in the barrier-free environment of the present invention;

[0071] Figure 4 It is a schematic diagram of the expansion of the original RRT algorithm in a single obstacle environment of the present invention;

[0072] Figure 5 It is a schematic diagram of the expansion of the improved RRT algorithm in a single obstacle environment of the present invention;

[0073] Figure 6 It is a schematic diagram of the expansion of the original RRT algorithm in a multi-obstacle environment of the present invention;

[0074] Figure 7 This is a schematic diagram of the expansion of the improved RRT algorithm in a multi-obstacle environment of the present invention. DETAILED DESCRIPTION

[0075] 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.

[0076] The purpose of the present invention is to provide an unmanned boat path planning method and system based on an improved RRT algorithm to solve the problem of slow planning speed of unmanned boats in simple scenarios such as open sea areas or sparse obstacles and when performing medium and long-distance tasks.

[0077] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0078] like Figure 1 As shown, Figure 1 The flowchart of the unmanned boat path planning method of the present invention for improving the RRT algorithm is as follows. The method reduces the redundant time cost of the RRT algorithm due to the single termination condition by diversifying the termination detection mode of the RRT algorithm and introducing the target connection probability, so as to achieve the purpose of terminating the algorithm in advance and reducing the path planning time; based on the established terminal parent node reselection mechanism, the path quality problem caused by the randomness brought by the terminal connection probability is eliminated. The method mainly includes the following five steps:

[0079] Step 1: Establish a map model that is the same as the unmanned boat operating environment, obtain the obstacle area and free area in the map, and obtain the robot's starting point Q start and the end point Q goal , set the endpoint connection probability to q, the endpoint connection threshold to a, the expansion step size to s, initialize the random tree T, and set the starting point Q start Add to the random tree T.

[0080] Specifically, three simple map environments, obstacle-free, single-obstacle, and multi-obstacle, were established to simulate the operating environment of the unmanned boat. The map size was 1000m×1000m, the starting point of the unmanned boat was set to (0,0), the end point was set to (999,999), the end point connection probability q=0.1, the end point connection threshold a=25m, and the algorithm expansion step s=25m;

[0081] Step 2: Randomly obtain a sampling point Q in the above map space rand , find the distance Q in the random tree T rand The nearest node Q nearest As Q rand The parent node Q parent , at this time Q rand Q parent The child nodes of

[0082] Step 3: Vector is the expansion direction, s is the expansion step to get the new node Q new ; Use collision() function to deal with the new node Q new Perform obstacle collision detection and pass the collision detection Q new And its parent node information is added to the random tree.

[0083] Step 4: Select the termination detection method of the algorithm according to the endpoint connection probability q. If the first termination detection algorithm is selected for algorithm termination judgment and the result is true, the algorithm terminates, otherwise returns to step 2; if the second termination detection algorithm is selected for algorithm termination judgment and the result is true, the endpoint parent node reselection mechanism is executed, by constructing the endpoint parent node candidate set for the endpoint Q goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to step 2.

[0084] Specifically: the termination detection method is selected according to the following formula:

[0085]

[0086] Where A is the algorithm termination detection method finally selected, q is the endpoint connection probability, and p1 is a random number between (0, 1) generated by the random number generation function rand().

[0087] The first termination detection algorithm is:

[0088] Calculate the new node Q each time through the collision detection new To the end Q goal The distance z, if Q new , Q goal The line segments formed satisfy no collision and z≤a, then Q new As Qgoal The parent node of Q goal And its corresponding parent node information is added to the random tree T, and the algorithm terminates.

[0089] The second termination detection algorithm is:

[0090] Connect the current new node Q obtained by expansion new and the end point Q goal , determine whether the line segment collides with the obstacle. If no collision occurs, the current new node Q new Temporarily as Q goal The parent node of Q goal And its parent node information is added to the random tree.

[0091] The specific description of the endpoint parent node reselection mechanism is as follows:

[0092] To avoid the terminal connection probability causing the terminal Q goal The selection of the parent node of the terminal is somewhat random, which indirectly leads to the situation that the path distance is too long or even unnecessary turns. By establishing a terminal parent node reselection mechanism, a better parent node is reselected for the terminal in the constructed terminal parent node candidate set according to the shortest path constraint. There are four ways to construct the terminal parent node candidate set: 1. Select a certain percentage of candidate nodes by randomly selecting points in the random tree and add them to the terminal parent node candidate set;

[0093] 2. Consider the update method of the random tree, take some nodes that are finally expanded by the algorithm as candidate nodes and add them to the candidate set of the terminal parent node;

[0094] 3. According to the RRT* algorithm, in the way of reselecting the parent node, a circular closed area is constructed with the parent node of the current end point as the center and a certain distance as the radius, and the nodes falling within the circle are added to the candidate set of the end point parent node;

[0095] 4. The parent node straight line Q of the current end point start , Q goal The distance between the nodes in the random tree and the straight line Q is l. start , Q goal The distance L i , will satisfy L i The nodes corresponding to <L are added to the parent node candidate set.

[0096] In the embodiment of the present invention, method 2 is selected to construct the terminal parent node candidate set.

[0097] Step 5: In the random tree T, start from the end point Q goal Towards the starting point Q start Backtracking path, that is, from Q goalFirst, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start Connect the nodes in path() in sequence, and the resulting path is used as the preliminary path of this improved algorithm. The array path() is used as the control array of the preliminary path, and the nodes stored in it are the path control points.

[0098] After the above steps, we can get Figure 3 , Figure 5 as well as Figure 7 Expanded schematic. Figure 3 In the figure, path 1 is the path that does not pass through the parent node of the terminal and is reselected, and path 2 is the path that passes through the parent node of the terminal and is reselected; Figure 5 and Figure 7 Path 1 plus path 2 is the path that is not reselected through the end parent node, and path 1 plus path 3 is the path that is reselected through the end parent node (path 1 is the common part of the two).

[0099] Compare Figure 2 and Figure 3 , Figure 4 and Figure 5 , Figure 6 and Figure 7 It can be seen that the original RRT algorithm has the disadvantages of too many useless nodes expanded and slow convergence speed in the three environments due to its own blindness. The improved RRT algorithm takes into account the environmental characteristics and simplifies the search process of the original algorithm by diversifying the termination methods of the algorithm, reducing its blindness in simple environments and effectively improving the convergence speed of the algorithm. It provides a very effective solution to the path planning problem of unmanned boats in simple scenarios such as open sea areas or sparse obstacles, as well as for performing medium and long-distance tasks.

[0100] Based on the above method in the present invention, the present invention also provides an unmanned boat path planning system based on an improved RRT algorithm, the system comprising:

[0101] Map construction module, used to build map models;

[0102] An obstacle area and free area acquisition module, used to acquire the obstacle area and free area in the map;

[0103] Parameter setting module, used to set the starting point Q of the unmanned boat start , End point Q goal , endpoint connection probability q, endpoint connection threshold a and expansion step size s;

[0104] Initialization module, used to initialize the random tree T and set the starting point Q start Add to the random tree T;

[0105] A sampling point acquisition module is used to randomly acquire a sampling point Q in the map. rand , and find the distance from the sampling point Q in the random tree T rand The nearest node Q nearest , the node Q nearest As the sampling point Q rand The parent node Q parent , sampling point Q rand As Q parent The child nodes of

[0106] New node determination module for vector is the expansion direction, s is the expansion step to get the new node Q new ;

[0107] Collision detection module, used to use collision() function to detect new node Q new Perform obstacle collision detection;

[0108] A judgment module is used to judge the new node Q new Whether the collision detection is passed,

[0109] Random tree update module, used to update the new node Q that passes the collision test when it passes the collision test new And its parent node information is added to the random tree T;

[0110] The first loop module is used to return to the "sampling point acquisition module" when the collision detection fails;

[0111] A termination detection mode selection module is used to select a termination detection mode of the algorithm according to the endpoint connection probability q; the termination detection mode includes: a first termination detection algorithm and a second termination detection algorithm;

[0112] The second loop module is used for selecting the first termination detection algorithm to perform algorithm termination judgment, and when the judgment result is true, the algorithm terminates, otherwise it returns to the "sampling point acquisition module";

[0113] The third loop module is used to execute the terminal parent node reselection mechanism when the second termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, and the terminal parent node candidate set is constructed as the terminal Q goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to the "sampling point acquisition module";

[0114] A path backtracking module is used to start from the end point Q in the random tree T. goal Towards the starting point Q start Backtracking path: From Q goalFirst, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start until;

[0115] The path determination module is used to sequentially connect the nodes in path() to obtain a preliminary path;

[0116] The unmanned boat control module is used to control the unmanned boat to travel along the preliminary path.

[0117] The above method and system in the present invention have the following advantages:

[0118] The invention relates to the field of path planning for unmanned boats, and discloses an unmanned boat path planning method based on an improved RRT algorithm. The invention mainly studies how to use the RRT algorithm to solve the path planning problem of the unmanned boat in simple scenes such as open sea areas or sparse obstacles and in performing medium and long-distance tasks. By diversifying the termination detection methods of the RRT algorithm and introducing the target connection probability, the redundant time cost of the RRT algorithm caused by the single termination condition is reduced. Then, by establishing a terminal parent node reselection mechanism, the path quality problem caused by the randomness brought by the terminal connection probability is eliminated, and the speed of the path planning of the unmanned boat in simple scenes is accelerated.

[0119] 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 system 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.

[0120] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A path planning method for unmanned boat based on improved RRT algorithm. It is characterized in that The path planning method comprises: S1: Build map model; S2: Obtaining obstacle areas and free areas in the map; S3: Set the starting point Q of the unmanned boat start , End point Q goal , endpoint connection probability q, endpoint connection threshold a and expansion step size s; S4: Initialize the random tree T and set the starting point Q start Add to the random tree T; S5: Randomly obtain a sampling point Q in the map rand , and find the distance from the sampling point Q in the random tree T rand The nearest node Q nearest , the node Q nearest As the sampling point Q rand The parent node Q parent , sampling point Q rand As Q parent The child nodes of S6: Vector is the expansion direction, s is the expansion step to get the new node Q new ; S7: Use collision() function to deal with the new node Q new Perform obstacle collision detection; S8: Determine the new node Q new Whether the collision detection is passed; S9: If passed, the new node Q will pass the collision detection new And its parent node information is added to the random tree T; S10: If the collision detection fails, return to S5; S11: selecting a termination detection method of an algorithm according to the endpoint connection probability q; the termination detection method includes: a first termination detection algorithm and a second termination detection algorithm; S12: If the first termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, the algorithm is terminated, otherwise it returns to S5; S13: If the second termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, the terminal parent node reselection mechanism is executed, and the terminal Q is selected in the candidate set of the terminal parent node. goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to S5; S14: In the random tree T, from the end point Q goal Towards the starting point Q start Backtracking path: From Q goal First, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you trace back to Q start until; S15: Connect the nodes in path() in sequence to obtain a preliminary path; S16: Controlling the unmanned boat to travel along the preliminary path; The first termination detection algorithm is: Calculate the new node Q each time through the collision detection new To the end Q goal The distance z, if Q new , Q goal The line segments formed satisfy no collision and z≤a, then Q new As Q goal The parent node of Q goal And its corresponding parent node information is added to the random tree T, and the algorithm terminates; The second termination detection algorithm is: Connect the current new node Q obtained by expansion new and the end point Q goal , determine whether the line segment collides with the obstacle. If there is no collision, the current new node Q new Temporarily as Q goal The parent node of Q goal And its parent node information is added to the random tree.

2. The unmanned boat path planning method based on the improved RRT algorithm according to claim 1, It is characterized in that The termination detection method is selected according to the following formula: Where A is the final algorithm termination detection method, q is the endpoint connection probability, and p 1 Generate a random number between (0, 1) for the random number generation function rand().

3. The unmanned boat path planning method based on the improved RRT algorithm according to claim 1, It is characterized in that The endpoint parent node reselection mechanism includes: the first endpoint parent node reselection mechanism, the second endpoint parent node reselection mechanism, the third endpoint parent node reselection mechanism, and the fourth endpoint parent node reselection mechanism: The first terminal parent node reselection mechanism is: randomly selecting a preset percentage of candidate nodes from the random tree T and adding them to the terminal parent node candidate set; The second terminal parent node reselection mechanism is: considering the update method of the random tree, taking the last expanded part of the nodes as candidate nodes and adding them to the terminal parent node candidate set; The third terminal parent node reselection mechanism is: in the manner of reselecting the parent node according to the RRT algorithm, a circular closed area is constructed with the parent node of the current terminal as the center and the preset distance as the radius, and the nodes falling within the circle are added to the terminal parent node candidate set; The fourth end point parent node reselection mechanism is: the parent node straight line Q of the current end point start , Q goal The distance between the nodes in the random tree and the straight line Q is l. start , Q goal The distance L i , will satisfy L i The node corresponding to <l is added to the parent node candidate set.

4. An unmanned boat path planning system based on improved RRT algorithm, It is characterized in that The system comprises: Map construction module, used to build map models; An obstacle area and free area acquisition module, used to acquire the obstacle area and free area in the map; Parameter setting module, used to set the starting point Q of the unmanned boat start , End point Q goal , endpoint connection probability q, endpoint connection threshold a and expansion step size s; Initialization module, used to initialize the random tree T and set the starting point Q start Add to the random tree T; A sampling point acquisition module is used to randomly acquire a sampling point Q in the map. rand , and find the distance from the sampling point Q in the random tree T rand The nearest node Q nearest , the node Q nearest As the sampling point Q rand The parent node Q parent , sampling point Q rand As Q parent The child nodes of New node determination module for vector is the expansion direction, s is the expansion step to get the new node Q new ; Collision detection module, used to use collision() function to detect new node Q new Perform obstacle collision detection; A judgment module is used to judge the new node Q new Whether the collision detection is passed, Random tree update module, used to update the new node Q that passes the collision test when it passes the collision test new And its parent node information is added to the random tree T; The first loop module is used to return to the "sampling point acquisition module" when the collision detection fails; A termination detection mode selection module is used to select a termination detection mode of the algorithm according to the endpoint connection probability q; the termination detection mode includes: a first termination detection algorithm and a second termination detection algorithm; The second loop module is used for terminating the algorithm when the first termination detection algorithm is selected to perform algorithm termination judgment, and when the judgment result is true, the algorithm terminates, otherwise it returns to the "sampling point acquisition module"; The third loop module is used to execute the terminal parent node reselection mechanism when the second termination detection algorithm is selected to perform algorithm termination judgment, and the judgment result is true, and the terminal parent node candidate set is constructed as the terminal Q goal Select the best parent node, update the random tree, and the algorithm terminates, otherwise return to the "sampling point acquisition module"; A path backtracking module is used to start from the end point Q in the random tree T. goal Towards the starting point Q start Backtracking path: From Q goal First, find the parent node corresponding to each node level by level, and add these parent nodes to the array path() one by one until you backtrack to Q start until; The path determination module is used to sequentially connect the nodes in path() to obtain a preliminary path; An unmanned boat control module, used to control the unmanned boat to travel along the preliminary path; The first termination detection algorithm is: Calculate the new node Q each time through the collision detection new To the end Q goal The distance z, if Q new , Q goal The line segments formed satisfy no collision and z≤a, then Q new As Q goal The parent node of Q goal And its corresponding parent node information is added to the random tree T, and the algorithm terminates; The second termination detection algorithm is: Connect the current new node Q obtained by expansion new and the end point Q goal , determine whether the line segment collides with the obstacle. If there is no collision, the current new node Q new Temporarily as Q goal The parent node of Q goal And its parent node information is added to the random tree.

5. The unmanned boat path planning system based on the improved RRT algorithm according to claim 4, It is characterized in that The termination detection mode in the termination detection mode selection module is selected according to the following formula: Where A is the final algorithm termination detection method, q is the endpoint connection probability, and p 1 Generate a random number between (0, 1) for the random number generation function rand().

6. The unmanned boat path planning system based on the improved RRT algorithm according to claim 4, It is characterized in that The endpoint parent node reselection mechanism includes: the first endpoint parent node reselection mechanism, the second endpoint parent node reselection mechanism, the third endpoint parent node reselection mechanism, and the fourth endpoint parent node reselection mechanism: The first terminal parent node reselection mechanism is: randomly selecting a preset percentage of candidate nodes from the random tree T and adding them to the terminal parent node candidate set; The second terminal parent node reselection mechanism is: considering the update method of the random tree, taking the last expanded part of the nodes as candidate nodes and adding them to the terminal parent node candidate set; The third terminal parent node reselection mechanism is: in the manner of reselecting the parent node according to the RRT algorithm, a circular closed area is constructed with the parent node of the current terminal as the center and the preset distance as the radius, and the nodes falling within the circle are added to the terminal parent node candidate set; The fourth end point parent node reselection mechanism is: the parent node straight line Q of the current end point start , Q goal The distance between the nodes in the random tree and the straight line Q is l. start , Q goal The distance L i , will satisfy L i The node corresponding to <l is added to the parent node candidate set.

Images