A path planning method based on a rapidly-exploring random tree algorithm

Abstract

This invention belongs to the field of path planning and discloses a path planning method based on a fast expanding random tree algorithm. The method acquires an environmental image and calculates the target bias probability based on the image. Then, at each sampling stage, a random value is obtained, and the final sampling point is derived. Next, the actual expansion step size T in each direction and the expansion direction angle of the generated new nodes are calculated. Then, the target point connection probability is calculated, and several new nodes obtained in each expansion are connected to the target point according to their probability. If a connection is safe and collision-free, this feasible node is added to the random tree as the parent node of the target point, and the algorithm terminates. This method effectively escapes local optima through parameter resetting and direction offset; avoids invalid computation by limiting the maximum number of backtracking attempts; and introduces a backtracking mechanism to avoid local traps.

Description

Technical Field

[0001] This invention belongs to the field of path planning, specifically a path planning method based on the fast expanding random tree algorithm. Background Technology

[0002] In recent years, amphibious vehicles have played an irreplaceable role in transportation, geological exploration, and emergency rescue. To enable more convenient and flexible continuous cross-domain observation of the air-sea boundary layer, a new type of amphibious vehicle (HAUV) has emerged. HAUVs are amphibious vehicles capable of operating simultaneously in the air and underwater. The advent of HAUVs breaks the limitations of unmanned aerial vehicles (UAVs) and unmanned underwater vehicles (UUVs), which can only operate in a single, specific environment, enabling a single vehicle to simultaneously conduct aerial, surface, and underwater surveys of a specific sea area.

[0003] As an emerging amphibious vehicle, the HAUV's cross-domain operation characteristics place higher demands on path planning algorithms. In 1998, LaValle proposed the Rapidly-Exploring Random Tree (RRT) algorithm, which searches for a path by continuously sampling random points in the map space to expand and obtain new nodes until a safe and effective path connecting the starting point and the destination point is found. Although the RRT algorithm is widely used due to its probabilistic completeness, simple algorithm structure, and strong search capability in complex environments, in special application scenarios like HAUVs that need to consider both the complex air and underwater environments, the RRT algorithm still suffers from problems such as large blind expansion and tortuous planned paths.

[0004] Therefore, it is necessary to propose a path planning method based on the fast expanding random tree algorithm to solve the above-mentioned technical problems in the existing technology. Summary of the Invention

[0005] The purpose of this invention is to provide a path planning method based on the Fast Extended Random Tree (RRT) algorithm, which adopts different improvement strategies for the RRT algorithm according to different planning scenarios, so as to quickly search for the asymptotically optimal path.

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

[0007] A path planning method based on a fast expanding random tree algorithm includes the following steps:

[0008] Step 1: Acquire an environmental image, which includes the starting point and the target point;

[0009] Step 2: Calculate the target offset probability based on the environmental image obtained in Step 1. ;

[0010] Step 3: For each sample, a random value is obtained using a random function. ,according to and Obtain the final sampling points ;

[0011] Step 4: Based on the final sampling points Determine the expansion direction of the new node. , Then, a maximum expansion step size S is set, and the actual expansion step size T in each direction is calculated to obtain... A new node ;

[0012] Step 5: Calculate the connection probability of the target point. For each new node obtained from the expansion, according to probability... If a connection to the target point is safe and collision-free, this feasible node is added to the random tree as the parent node of the target point, and the algorithm terminates.

[0013] Step 6: Trigger the backtracking mechanism when consecutive failures occur during path expansion.

[0014] Preferably, in step 2, the target bias probability The calculation formula is:

[0015] ;

[0016] In the formula, , It is a constant. The distance between the random tree and the target point is the shortest distance after each new node expansion. The straight-line distance between the starting point and the target point;

[0017] When the number of random tree expansions exceeds 60% of the total number of iterations or the current path length reaches 1.5 times the straight-line distance between the starting point and the target point, the probability of using the target point as a random point is increased to accelerate the convergence of the algorithm.

[0018] Preferably, in step 3, according to and Obtain the final sampling points The formula is:

[0019] ;

[0020] In the formula, For the final sampling point; These are randomly generated values ​​used to determine whether to use the target point or other sampling points; a Voronoi diagram-based spatial segmentation method is used to generate two candidate points with complementary directions in free space. and Select the point closest to the target point as ; The target point.

[0021] Preferably, in step 4, the expansion direction of the new node is determined. The steps are as follows:

[0022] At the final sampling point central axis Based on the angle interval generate The expansion direction angle of the new node , .

[0023] Preferably, in step 4, the formula for calculating the actual expansion step size T is:

[0024] T = B•S;

[0025] In the formula, B is the step size random factor, which is a random number between (0.2, 0.8) generated by a random function to ensure that the actual expansion step size T fluctuates significantly; S is the preset maximum expansion step size.

[0026] Preferably, in step 5, the target point connection probability The calculation formula is:

[0027] ;

[0028] In the formula, The normalized distance from the current node to the target node. The sensitivity coefficient is denoted as .

[0029] Preferably, in step 6, during the path expansion process, a path expansion failure is defined as one of the following conditions being met:

[0030] The newly expanded node collides with an obstacle;

[0031] The node concealment score Γ < 0.6 or the energy cost Cswitch > 1.2Cavg, where Cavg is the historical average energy consumption;

[0032] After five consecutive expansions, the path length did not decrease and the target point was not approached.

[0033] Preferably, in step 6, the backtracking mechanism processes the following steps:

[0034] Step 6.1: When consecutive failures occur during path expansion, backtrack to the parent node of the current node or the node with higher concealment among the nearest high-scoring nodes;

[0035] Step 6.2: Reduce the current target bias probability. To increase the global exploration weight; reduce the current effective step size L to avoid repeated collisions; and generate a new sampling sector with the most recent effective expansion direction angle θ±30° centered on the backtrack node in step 6.1.

[0036] Step 6.3: Starting from the fallback node, generate 3 candidate directions to replan the path:

[0037] Extend the vector from the back node to the target point;

[0038] Extending tangentially along the surface of the obstacle;

[0039] Random sampling within free space;

[0040] Step 6.4: If the target point is successfully connected, the backtracking will terminate; if the total number of backtracking attempts exceeds the preset number, the path will be determined to be unreachable.

[0041] If the new node is successfully expanded after backtracking, the original step size L and target bias probability are restored. Continue the global search;

[0042] Step 6.5: Dynamically adjust the step size L based on the environmental complexity:

[0043] ;

[0044] In the formula, , To equip the maximum maneuvering stride, The maximum obstacle density threshold;

[0045] If five consecutive expansion attempts fail, the newly expanded node collides with an obstacle, or the node's concealment score falls below a threshold, the node reverts to its parent node and regenerates sampling points. Simultaneously, the target bias probability is reduced to avoid deadlock in the target direction.

[0046] Compared with the prior art, the present invention has the following advantages:

[0047] As described above, the path planning method based on the fast expanding random tree algorithm of this invention is particularly effective in handling the complex environment of amphibious vehicles (HAUVs). It accelerates the algorithm's convergence process through a dynamic target bias probability mechanism and generates candidate points with complementary directions using a Voronoi diagram-based spatial segmentation method, significantly enhancing its adaptability to complex obstacles. After determining the expansion direction of a new node, a backtracking mechanism is triggered when the algorithm fails to expand consecutively: it backtracks to the nearest high-scoring node, reducing the target bias probability and shortening the effective step size to reduce time costs. This invention achieves safe and efficient cross-domain path planning by resetting parameters and shifting directions to escape local optima, limiting the maximum number of backtracking attempts to avoid invalid computations, and coordinating concealment scoring with an energy consumption model. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly described below.

[0049] Figure 1 This is a flowchart of a path planning method based on a fast expanding random tree algorithm in an embodiment.

[0050] Figure 2 This is a flowchart illustrating the backtracking mechanism in the embodiment;

[0051] Figure 3 This is a schematic diagram of two-point sampling optimization in the embodiment;

[0052] Figure 4 This is a schematic diagram illustrating multi-directional expansion in the embodiment;

[0053] Figure 5 This is a schematic diagram of the RRT algorithm search in a simple environment as shown in the example. Detailed Implementation

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

[0055] Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] Example 1

[0057] like Figures 1 to 5As shown in the figure, this embodiment describes a path planning method based on the Fast Extended Random Tree (RRT) algorithm. This method modifies the original RRT algorithm's random point generation method, integrating the conventional goal bias strategy and multi-sampling strategy in the RRT field. It proposes dynamic parameter adjustment and spatially structured sampling rules: adaptive adjustment of the sampling direction is achieved through a dynamic goal bias probability model, and candidate sampling points with complementary directions are generated based on Voronoi diagram segmentation, forming a random point generation mechanism suitable for amphibious cross-domain environments.

[0058] The path planning method based on the fast expanding random tree algorithm described in this embodiment includes the following steps:

[0059] Step 1: Obtain an environmental image, which includes the starting point and the target point.

[0060] Step 2: Calculate the target bias probability (dynamic target bias probability) based on the environmental image obtained in Step 1. The calculation formula is as follows:

[0061] ;

[0062] In the formula, , It is a constant. The distance between the random tree and the target point is the shortest distance after each new node expansion. This represents the straight-line distance between the starting point and the target point.

[0063] When one of the following conditions is met, the number of random tree expansions exceeds 60% of the total number of iterations or the current path length reaches 1.5 times the straight-line distance between the starting point and the target point, the probability of using the target point as a random point is increased to accelerate the convergence of the algorithm.

[0064] This embodiment calculates the dynamic target bias probability based on real-time distance feedback, changing the fixed sampling mode of the traditional RRT algorithm, and can better adapt to the dynamic characteristics of amphibious unmanned equipment in cross-domain environments.

[0065] Step 3, as follows Figure 3 As shown, for each sample, a random value between (0, 1) is obtained through a random function. ,according to Bias probability with target Obtain the final sampling points The formula is as follows:

[0066] ;

[0067] In the formula, For the final sampling point; These are randomly generated values ​​used to determine whether to use the target point or other sampling points, increasing the algorithm's flexibility and adaptability; a Voronoi diagram-based spatial segmentation method is used to generate two candidate points with complementary directions in free space. and Select the point closest to the target point as ; The target point.

[0068] This embodiment reduces the probability of invalid sampling caused by collisions between nodes expanded in one direction and obstacles in the original RRT algorithm by using a strategy of expanding multiple new nodes in multiple directions with random step sizes.

[0069] Step 4, as follows Figure 4 As shown, based on the final sampling points Determine the expansion direction of the new node, i.e., at the final sampling point. central axis (i.e., final sampling point) Based on the extended direction baseline, at angular intervals generate The expansion direction angle of the new node , .

[0070] Then, a maximum expansion step size S is set (S ranges from 20% to 50% of the equipment's maximum maneuver radius), and the actual expansion step size in each direction is calculated as T = B•S, where B is a step size random factor. To ensure that the actual expansion step size T fluctuates significantly, a random number B (i.e., the step size random factor) between (0.2, 0.8) is generated using a random function to ensure the expansion is successful. A new node .

[0071] Step 5: Since the amphibious unmanned equipment operates in a relatively simple water environment with few obstacles, simply relying on judging the distance between nodes on the random tree and the target point and the set target point connection threshold to terminate the algorithm will increase the time cost of path planning.

[0072] by Figure 5 For example, when searching for a path using the RRT algorithm in an environment with sparse obstacles, there will always be one or more feasible nodes connected to the target point without collision at some point before the algorithm ends according to the original termination condition.

[0073] Therefore, to terminate the algorithm early, a normalized distance-sensitive logistic function is introduced to calculate the connection probability of the target point. For each new node obtained from the expansion, according to probability... If a connection to the target point is safe and collision-free, this feasible node is added to the random tree as the parent node of the target point, and the algorithm terminates.

[0074] Where the target point connection probability The calculation formula is:

[0075] ;

[0076] In the formula, The normalized distance from the current node to the target node. The sensitivity coefficient is denoted as .

[0077] The above steps complete the process of generating multiple candidate sampling points and selecting the optimal direction for expansion under non-target bias conditions.

[0078] Step 6: After generating random sampling points by fusing the RRT Goal Bias and multi-sampling point strategies from Steps 2 to 5, the step size is dynamically adjusted when expanding to new nodes, and a backtracking mechanism is introduced to avoid local traps. The backtracking mechanism is triggered when the algorithm fails consecutively during path expansion.

[0079] The condition for the algorithm to fail consecutively during path expansion is defined as follows:

[0080] Collision detection failed: The newly expanded node collided with an obstacle;

[0081] Insufficient scoring: Node concealment score Γ < 0.6 or energy cost Cswitch > 1.2Cavg, where Cavg is the historical average energy consumption;

[0082] Local convergence stagnation: After 5 consecutive expansions, the path length does not decrease and the target point is not approached;

[0083] Only one of the above conditions needs to be met.

[0084] like Figure 2 As shown, the backtracking mechanism processes the following steps:

[0085] Step 6.1: Select the backtracking node after determining the completion of the backtracking conditions.

[0086] When the algorithm fails consecutively during path expansion, it backtracks to the parent node of the current node or to the nearest high-scoring node with higher concealment.

[0087] Step 6.2, Parameter Reset

[0088] Adjust target bias probability Reduce the current target bias probability ←0.7 Increase the global exploration weight;

[0089] Dynamic step size reduction: The step size L is temporarily adjusted to L′=0.5L to avoid repeated collisions; L is the current effective step size, whose initial value is equal to T, and it will be dynamically adjusted during the backtracking process;

[0090] Sampling area offset: A new sampling sector with a direction angle of θ ± 30° is generated, centered on the back-off node, based on the most recent effective expansion.

[0091] Step 6.3, Path Replanning

[0092] Starting from the fallback node, the following three candidate directions are generated:

[0093] Target-oriented direction: Extend the vector from the backtrack node to the target point;

[0094] Obstacle avoidance direction: extends tangentially along the obstacle surface;

[0095] Random exploration direction: Random sampling within free space.

[0096] Step 6.4, Termination and Resumption

[0097] Termination: If the target point is successfully connected, the backtracking will terminate; if the total number of backtracking attempts exceeds the preset number (10 times), the path will be determined to be unreachable.

[0098] State recovery: If the new node is successfully expanded after backtracking, the original step size L and target bias probability are restored. Continue the global search;

[0099] Step 6.5: Dynamically adjust the step size L based on the environmental complexity:

[0100] ;

[0101] In the formula, , To equip the maximum maneuvering stride, The maximum obstacle density threshold;

[0102] If five consecutive expansion attempts fail (collision or score below the threshold), the system reverts to the parent node and regenerates sampling points, while simultaneously reducing the target bias probability to avoid deadlock in the target direction.

[0103] Step 6.2 is a temporary adjustment for the collision emergency response. When there are consecutive failures (collision / insufficient scoring / convergence stagnation), the current effective step size L is immediately reduced by 50% (L'=0.5L). This only takes effect during the backtracking period and will be restored to the original value after success. Step 6.5 is a persistent adjustment for environmental adaptation.

[0104] This embodiment describes a path planning method based on a fast expanding random tree algorithm, which can effectively complete path planning. This method effectively escapes local optima by resetting parameters and shifting directions; avoids invalid computation by limiting the maximum number of backtracking attempts; and improves path safety through collaborative optimization with concealment scoring and energy consumption models.

[0105] The embodiments of the present invention are only used to illustrate the technical solutions of the present invention and are not intended to limit it. For those skilled in the art, it will be understood that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method based on a fast expanding random tree algorithm, characterized in that, Includes the following steps: Step 1: Acquire an environmental image, which includes the starting point and the target point; Step 2: Calculate the target offset probability based on the environmental image obtained in Step 1. ; Step 3: For each sample, a random value is obtained using a random function. ,according to and Obtain the final sampling points ; Step 4: Based on the final sampling points Determine the expansion direction of the new node. , Then, a maximum expansion step size S is set, and the actual expansion step size T in each direction is calculated to obtain... A new node ; Step 5: Calculate the connection probability of the target point. For each new node obtained from the expansion, according to probability... If a connection to the target point is safe and collision-free, this feasible node is added to the random tree as the parent node of the target point, and the algorithm terminates. Step 6: Trigger the backtracking mechanism when consecutive failures occur during path expansion; The backtracking mechanism's processing steps are as follows: Step 6.1: When consecutive failures occur during path expansion, backtrack to the parent node of the current node or the node with higher concealment among the nearest high-scoring nodes; Step 6.2: Reduce the current target bias probability. To increase the global exploration weight; reduce the current effective step size L to avoid repeated collisions; and generate a new sampling sector with the most recent effective expansion direction angle θ±30° centered on the backtrack node in step 6.

1. Step 6.3: Starting from the fallback node, generate 3 candidate directions to replan the path: Extend the vector from the back node to the target point; Extending tangentially along the surface of the obstacle; Random sampling within free space; Step 6.4: If the target point is successfully connected, the backtracking will terminate; if the total number of backtracking attempts exceeds the preset number, the path will be determined to be unreachable. If the new node is successfully expanded after backtracking, the original step size L and target bias probability are restored. Continue the global search; Step 6.5: Dynamically adjust the step size L based on the environmental complexity: ； In the formula, , To equip the maximum maneuvering stride, The maximum obstacle density threshold; If five consecutive expansion attempts fail, the newly expanded node collides with an obstacle, or the node's concealment score falls below a threshold, the node reverts to its parent node and regenerates sampling points. Simultaneously, the target bias probability is reduced to avoid deadlock in the target direction.

2. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 2, the target bias probability The calculation formula is: ； In the formula, , It is a constant. The distance between the random tree and the target point is the shortest distance after each new node expansion. The straight-line distance between the starting point and the target point; When the number of random tree expansions exceeds 60% of the total number of iterations or the current path length reaches 1.5 times the straight-line distance between the starting point and the target point, the probability of using the target point as a random point is increased to accelerate the convergence of the algorithm.

3. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 3, according to and Obtain the final sampling points The formula is: ； In the formula, For the final sampling point; These are randomly generated values ​​used to determine whether to use the target point or other sampling points; a Voronoi diagram-based spatial segmentation method is used to generate two candidate points with complementary directions in free space. and Select the point closest to the target point as ; The target point.

4. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 4, the expansion direction of the new node is determined. The steps are as follows: At the final sampling point central axis Based on the angle interval generate The expansion direction angle of the new node , .

5. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 4, the formula for calculating the actual expansion step size T is: T = B•S; In the formula, B is the step size random factor. To ensure that the actual expansion step size T fluctuates significantly, B is a random number between (0.2, 0.8) generated by a random function. S is the preset maximum step size for expansion.

6. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 5, the target point connection probability The calculation formula is: ； In the formula, The normalized distance from the current node to the target node. The sensitivity coefficient is denoted as .

7. The path planning method based on the fast expanding random tree algorithm according to claim 1, characterized in that, In step 6, during the path extension process, a path extension is considered to have failed continuously if one of the following conditions is met: The newly expanded node collides with an obstacle; The node concealment score Γ < 0.6 or the energy cost Cswitch > 1.2Cavg, where Cavg is the historical average energy consumption; After five consecutive expansions, the path length did not decrease and the target point was not approached.

Images