Mechanical arm path planning method and system based on self-adaptive expansion sampling

Abstract

The invention discloses a mechanical arm path planning method and system based on self-adaptive expansion sampling, belongs to the technical field of path planning, and solves the problems of low sampling rate, low convergence rate and poor initial path quality caused by too random sampling area and improper selection of expansion step length during path planning of a mechanical arm in the prior art. According to the self-adaptive expansion sampling strategy provided by the invention, on the basis of the F-RRT * optimization algorithm, the growth direction of the random tree is corrected by introducing the target bias force, so that the random tree deviates to the target point, the inflection point is optimized, the generated path is smoother, and by introducing the concept of target point gravity deviation, the optimal path is obtained. A strategy is adaptively expanded according to the obstacle complexity in the environment, and expansion is performed by dynamic step length, so that the stability and precision of the trajectory are improved, and the length of the generated path, the sampling frequency and the convergence time are reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and relates to a robot arm path planning method and system based on adaptive extended sampling. Background Art

[0002] With the continuous advancement and widespread application of robotic arm technology, the working environment and tasks faced by robotic arms are becoming increasingly diverse and complex. Especially in the context of an aging population, robotic arms have become an important component of replacing the labor force and play a key role in economic production. In this context, path planning and trajectory optimization have become a research hotspot in the field of robotic arms. By optimizing the path and trajectory of the robotic arm, it can operate efficiently and stably in an unknown and complex working environment, thereby improving work efficiency, reducing costs, and promoting economic production and development.

[0003] The goal of path planning is to find a safe, efficient, and collision-free path for the robot arm from the starting point to the target point in a given workspace. Existing path planning algorithms can be divided into three categories: (1) Grid search-based A *and Dijkstra algorithm, which find the optimal path by dividing the workspace into grids and searching for neighboring nodes. As the grid resolution increases, these algorithms can find the optimal path, but the number of nodes will increase and the computing power requirements will also increase. (2) Genetic algorithms and ant colony algorithms based on intelligence use heuristic methods to find the global optimal path. Compared with grid search-based algorithms, they can find better paths, but their local search capabilities are weaker and their convergence speed is slower. At the same time, although the ant colony algorithm is robust, its parameters are difficult to set and the solution time is longer. (3) The RRT algorithm based on random sampling generates a path tree through random sampling and greedy search. Compared with other algorithms, the RRT algorithm has a faster search speed and is suitable for high-dimensional path planning, but the path it generates may not be smooth enough and has low efficiency. RRT* is an improved algorithm based on RRT, which reduces the path length by optimizing the path. Although the RRT* algorithm can provide a better path, it has a high computational complexity and a slow convergence speed. The F-RRT* algorithm further improves RRT* by introducing more efficient optimization strategies to speed up path planning. For example, the paper "F-RRT*: An improved path planning algorithm with improved initial solution and convergence rate" (Liao Bin, Wan Fangyi, Hua Yi, Ma Ruirui, Zhu Shenrui, Expert Systems With Applications. Volume 184, Issue. 2021) discloses a path planning algorithm based on F-RRT* improvement, which can produce better initial solutions and faster convergence speed than RRT*.

[0004] However, the F-RRT* algorithm relies on random sampling when generating paths, so the sampling areas are too dispersed, which makes it impossible to effectively cover all important areas in complex environments. At the same time, the high-dimensional characteristics of the path planning problem and the complexity of obstacles make it difficult for even the optimized F-RRT* algorithm to quickly converge to the global optimal solution. The choice of expansion step size also has an important impact on the expansion efficiency of the tree. Therefore, it is urgent to propose a robot path planning method that can solve the above problems. Summary of the invention

[0005] The technical solution of the present invention is used to solve the problems of too random sampling areas and inappropriate expansion step selection in robot arm path planning under the prior art, resulting in slow sampling rate, slow convergence speed and poor initial path quality.

[0006] The present invention solves the above technical problems through the following technical solutions:

[0007] A robot arm path planning method based on adaptive extended sampling includes the following steps:

[0008] S1. Obstacle modeling, remember the starting point is X init , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest ;

[0009] S2, based on the adaptive expansion sampling strategy, determine the expansion method of the new node;

[0010] S3, generate a new node, and expand from the nearest point to the new node using the step length calculated by the dynamic step length formula;

[0011] S4, reconnect the random tree through the parent node reselection strategy of F-RRT*;

[0012] S5. Determine whether the target point has been reached. If so, end the path planning; otherwise, return to step S2.

[0013] Furthermore, the expansion method of the new node is determined based on the adaptive expansion sampling strategy described in step S2 as follows:

[0014] S21. First generate a random probability P through a random function r , and then calculate the probability of collision detection failure P cfd , by comparing P r and P cfd The size relationship determines the new node expansion strategy; if P r ≥P cfd , then execute step S22 to implement the target gravity bias sampling strategy; if P r <P cfd , then a random sampling strategy is used to expand new nodes;

[0015] S22. Based on the target gravity bias sampling strategy, the growth direction of the random tree is corrected to make it biased towards the target point.

[0016] Furthermore, the collision detection failure probability P in step S21 is cfd The expression is as follows:

[0017]

[0018] Where P min P max The random probability P r The minimum and maximum values ​​that can be obtained, T failed The number of collision detection failures.

[0019] Furthermore, the random sampling strategy may adopt one or more sampling methods of uniform sampling, Gaussian sampling, and adaptive sampling.

[0020] Furthermore, the target gravity bias sampling strategy is used in step S22 to correct the growth direction of the random tree so that it is biased toward the target point. Specifically,

[0021] S221, introduce a resultant force F, which is the gravity component F of the target point g and the gravity component F at the random sampling point r The expression of the resultant force F is as follows:

[0022] F=F g (α)+F r

[0023] In the formula, F r is a random sampling point X rand The corresponding nearest point X nearest The gravity, F g The target point X goal The corresponding nearest point X nearest The gravity of the target point, α is the angle between the gravity of the target point and the gravity of the random sampling point;

[0024] S222, calculate the gravity component F of the random sampling point r ; The gravity component F of the random sampling point r The expression is as follows:

[0025]

[0026] In the formula, is the gravity component F at the random sampling point r Unit vector in direction;

[0027] S223, calculate the gravity component F of the target point g ; The target point gravity component F g The expression is as follows:

[0028]

[0029] Where f(α) is the target gravity bias function;

[0030] S224, according to the change characteristics of the target gravity bias function f(α), calculate the transformed target point gravity component F g ; The target gravity bias function f(α) is expressed as follows:

[0031]

[0032] Where G is the gravitational coefficient;

[0033] S225. Calculate the transformed resultant force F according to steps S221 to S224.

[0034] Furthermore, the transformed target point gravity component F in step S224 g The expression is as follows:

[0035]

[0036] Furthermore, the expression of the transformed resultant force F in step S225 is as follows:

[0037]

[0038] Furthermore, the dynamic step length formula in step S3 is as follows:

[0039]

[0040] In the formula, S l , S are the selected dynamic step size and original step size respectively, d nearest is the closest point after expansion nearest Distance to the nearest obstacle, d rand is the shortest distance allowed from the obstacle, and d is the distance threshold, which is used to determine whether the random tree is close to the obstacle.

[0041] Furthermore, in step S4, the random tree is reconnected by the parent node reselection strategy of F-RRT* as follows: searching for the nearest point x through the FindReachest process nearest ancestor of , and find a point x that can be connected to the random sample without conflict rand The vertex x reachest ; Create a node x through the CreatNode process create , while connecting the random sampling points x rand and vertex x reachest The parent node of .

[0042] The present invention also provides a robot arm path planning system based on adaptive extended sampling, including an obstacle modeling module, a new node judgment module, a dynamic step extension module, a parent node reselection module and a target point judgment module;

[0043] The obstacle modeling module is used to record the starting point as X init , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest ;

[0044] The new node judgment module is used to judge the expansion mode of the new node based on the adaptive expansion sampling strategy;

[0045] The dynamic step extension module is used to generate a new node, and expand from the nearest point to the new node using the step length calculated by the dynamic step length formula;

[0046] The parent node reselection module is used to reconnect the random tree through the parent node reselection strategy of F-RRT*;

[0047] The target point judgment module is used to judge whether the target point has been reached. If the target point has been reached, the path planning is terminated; otherwise, the path planning is returned to the new node judgment module.

[0048] The advantages of the present invention are:

[0049] (1) The present invention provides a robot path planning method based on an adaptive expansion sampling strategy. On the basis of the F-RRT* optimization algorithm, it can adaptively expand the dynamic step size. By introducing the concept of target point gravity deviation, the expansion strategy is adaptively expanded according to the complexity of obstacles in the environment, and the expansion is performed with a dynamic step size, thereby improving the stability and accuracy of the trajectory.

[0050] (2) In view of the problems of strong randomness, large number of searches, and long convergence time in the RRT optimization algorithm, after obtaining the random sampling points, the growth direction of the random tree is corrected by introducing the target bias force to make it biased towards the target point. After the final path is generated, its turning point is optimized to make the generated path smoother and more in line with the motion characteristics of the actual mobile robot, thereby reducing the generated path length, sampling times, and convergence time. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flow chart of a method for robot arm path planning based on adaptive extended sampling according to an embodiment of the present invention;

[0052] Figure 2 is a schematic diagram of expanding new nodes based on an adaptive expansion sampling strategy according to an embodiment of the present invention;

[0053] Figure 3 is a curve diagram of a target gravity bias function image in a target gravity bias sampling strategy according to an embodiment of the present invention;

[0054] Figure 4 is a curve diagram of the relationship between the magnitude and angle of the target point gravity component of an embodiment of the present invention;

[0055] Figure 5 It is a curve chart of the changing trend of the dynamic step length function according to an embodiment of the present invention. DETAILED DESCRIPTION

[0056] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are 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.

[0057] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and specific embodiments:

[0058] Embodiment 1

[0059] The search process of the path planning algorithm based on RRT is similar to that of a tree that continues to spread and grow in all directions. The starting point is the root node of the random tree. The nodes in the tree are represented in the form of a set, and the connections between the nodes are also stored in the form of a set. The growth direction of the tree is guided by randomly sampling nodes in the free space. Find the node closest to the random point in the tree, and generate a new node with a step size on the line connecting the nearest point and the random point. If the path between the nearest neighbor node and the new node does not collide with obstacles, the new node is added to the tree, otherwise it is resampled. Repeat the above process. When the new node spreads to the target point, the entire search is completed, and the line connecting the node to the root node is returned as the path. The random scalability of RRT makes the algorithm unstable and unable to converge to the global optimum.

[0060] During the tree expansion process, RRT* determines the position of the random point and adds the reselection of parent nodes and rewiring operations for the new nodes added to the search tree. Although the RRT* algorithm can be asymptotically optimal, it has shortcomings such as excessive randomness, high path cost, and redundant nodes. In addition, the amount of calculation of the algorithm increases sharply with the increase in the number of nodes, and it takes a long time to approach the optimal solution.

[0061] F-RRT* optimizes the path by creating a parent node for a random point instead of selecting the parent node from the existing vertices. Its creation process requires very little calculation and the triangle inequality is reused throughout the process, resulting in a path with higher performance than the path of RRT*. Since the F-RRT* algorithm is essentially a tree expansion algorithm, its performance can be further improved when combined with other sampling strategies. Therefore, the present invention discloses a robot path planning method based on adaptive expansion sampling.

[0062] like Figure 1 Specifically, a method for robot arm path planning based on adaptive extended sampling is disclosed, comprising the following steps:

[0063] S1. Obstacle modeling, specifically, Figure 2 As shown, the starting point is Xinit , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest .

[0064] S2, based on the adaptive expansion sampling strategy, determine the expansion method of the new node;

[0065] The adaptive extended sampling strategy provided by the present invention can select a sampling strategy according to the complexity of obstacles in the environment, specifically the following steps:

[0066] S21. First generate a random probability P through a random function r , and then calculate the probability of collision detection failure P cfd , by comparing P r and P cfd The size relationship is used to determine the new node expansion strategy.

[0067] If P r ≥P cfd , then execute step S22 to implement the target gravity bias sampling strategy; if P r <P cfd , then a random sampling strategy is used to expand new nodes; specifically, P cfd The probability calculated based on the number of collision detection failures is expressed as follows:

[0068]

[0069] Where P min , P max The random probability P r The minimum and maximum values ​​that can be obtained, T failed is the number of collision detection failures. In this embodiment, P min =0.1, P max is 1, we get

[0070] In the present invention, the random probability generated by the random function is compared with the probability of collision detection failure. If the probability value calculated based on the collision detection failure is smaller and the random probability is greater than the probability of collision detection failure, it means that there are fewer obstacles around and the target gravity bias sampling can be performed directly; otherwise, there are more obstacles and random sampling is required to avoid collision.

[0071] The random sampling strategy is to randomly generate sampling points in the search space according to a random function. In this embodiment, one or more sampling methods of uniform sampling, Gaussian sampling, and adaptive sampling can be used. Using the uniform sampling method, random points can be uniformly generated in the entire search space, which is simple and easy to implement; using the Gaussian sampling method, random points are generated using Gaussian distribution, which can generate more sample points in certain areas and improve the sampling density; using the adaptive sampling method, the sampling strategy can be dynamically adjusted according to the existing path planning results or environmental characteristics.

[0072] S22, based on the target gravity bias sampling strategy, correct the growth direction of the random tree to make it biased towards the target point; the target gravity bias sampling strategy includes the following steps:

[0073] S221, introduce the resultant force F, which is the gravity component F of the target point g and the gravity component F of the random sampling point r The expression of the resultant force F is as follows:

[0074] F=F g (α)+F r

[0075] In the formula, F r is a random sampling point X rand The corresponding nearest point X nearest The gravity, F g The target point X goal The corresponding nearest point X nearest The gravity of the target point is α, and α is the angle between the gravity of the target point and the gravity of the random sampling point.

[0076] S222, calculate the gravity component F of the random sampling point r ;

[0077] The gravity component F of the random sampling point r Set to the unit vector in that direction That is, the search tree points to the random sampling point X rand The closest point X nearest The unit vector of the random sampling point gravity component F r The expression is as follows:

[0078]

[0079] S223, calculate the gravity component F of the target point g ;

[0080] Target point gravity component F g The size of F is related to the angle α. When α increases, g Also increases accordingly. It is the point on the search tree that points to the target point X.goal Closest point X nearest The target point gravity component F g The expression is as follows:

[0081]

[0082] Where f(α) is the target gravity bias function.

[0083] S224, according to the change characteristics of the target gravity bias function f(α), calculate the transformed target point gravity component F g ;

[0084] like Figure 3 As shown, when x∈[-2,-1]∪[1,2], the trend of the target gravity bias function f(α) is relatively gentle and the growth rate is slow; when x∈[-1,1], the function changes faster. The present invention utilizes the characteristic that the target gravity bias function changes at different rates in different intervals to automatically adjust the gravity of the target. When the angle α is large, it means that the sampling direction is different from the target direction, and a larger target gravity is required; when the angle α is small, a smaller target gravity is required.

[0085] according to Figure 3 It can be seen that when x∈[-2,0], the function can change quickly or slowly, which can meet the requirements of adaptively adjusting the target gravity. In this embodiment, the range of α in f(α) is mapped to [-2,0], and the expression of the target gravity bias function f(α) is obtained as follows:

[0086]

[0087] Where G is the gravitational coefficient;

[0088] The transformed target point gravity component F g The expression is as follows:

[0089]

[0090] Figure 4 When G=1, the gravity component F of the target point g Changes in size and angle α.

[0091] S225. Calculate the transformed resultant force according to steps S221 to S224; the expression of the resultant force F is as follows:

[0092]

[0093] Target point gravity component F g Represents the target point X goal To the nearest point X nearestAfter adding the target bias force, the random tree will grow in the direction of the resultant force F, and the resultant force F is determined by the gravity coefficient G and the angle α.

[0094] When the gravitational coefficient G = 0, the angle α has no effect, and the target point gravity component F g is 0, the random tree moves to the random sampling point X rand When the angle α = 0, it means that the random sampling point X rand With the target point X goal The direction is consistent, no bias is required, and the gravity component F of the random sampling point r is 0, the growth direction of the random tree is the direction of the target point; when the gravitational coefficient G ≥ 0, as the angle α changes, the larger the angle α, the stronger the target bias force, which can correct the growth direction of the random tree and optimize its inflection point to make it biased toward the target point, making the generated path smoother.

[0095] S3, generate a new node and record it as X new , find out the distance X on the current random tree new The nearest node X nearest , from the nearest point X nearest The step length calculated by the dynamic step length formula is X new Expand; the dynamic step formula is specifically:

[0096]

[0097] In the formula, S l , S are the selected dynamic step size and original step size respectively, d nearest is the closest point after expansion nearest Distance to the nearest obstacle, d rand is the shortest distance allowed from the obstacle, and d is the distance threshold, which is used to determine whether the random tree is close to the obstacle.

[0098] When the random tree is at a distance d from the obstacle nearest When the distance between the random tree and the obstacle is d, a fixed step size (i.e., the original step size S) is used. nearest When it is less than the threshold d, the fixed step length is dynamically optimized, and the dynamic step length function changes as follows: Figure 5 As shown, the characteristic of this function is that when the independent variable (in this embodiment, d nearest ) tends to 0, the function tends to 0.5; when d nearest As it approaches infinity, the function approaches 1.

[0099] S4, reconnect the random tree through the parent node reselection strategy of F-RRT*;

[0100] Specifically, the F-RRT* optimization algorithm uses the FindReachest and CreateNode processes to find random sampling points x rand Select the parent node. The FindReachest process searches for the nearest point x nearest ancestor of , and find a point x that can be connected to the random sample without conflict rand The vertex x reachest ; The CreateNode process is to create a node x create , which can simultaneously connect the random sampling points x rand and vertex x reachest The parent node reselection strategy involved in this embodiment can refer to the existing path planning algorithm based on F-RRT* improvement, which is a prior art and will not be described in detail here.

[0101] S5, determine whether the target point has been reached, if so, end the path planning, otherwise return to step S2;

[0102] The present invention provides a robot path planning method based on an adaptive expansion sampling strategy. On the basis of the F-RRT* optimization algorithm, the dynamic step length can be adaptively expanded. By introducing the concept of target point gravity deviation, the strategy is adaptively expanded according to the complexity of obstacles in the environment, and the expansion is performed with a dynamic step length, thereby improving the stability and accuracy of the trajectory. In view of the problems of strong randomness, many searches, and long convergence time in the RRT optimization algorithm, after obtaining the random sampling points, the growth direction of the random tree is corrected by introducing a target bias force to make it biased toward the target point. After the final path is generated, its inflection point is optimized to make the generated path smoother and more in line with the motion characteristics of the actual mobile robot, thereby reducing the generated path length, sampling times, and convergence time.

[0103] The present invention also provides a robot arm path planning system based on adaptive extended sampling, which is characterized by comprising an obstacle modeling module, a new node judgment module, a dynamic step extension module, a parent node reselection module and a target point judgment module;

[0104] The obstacle modeling module is used to record the starting point as X init , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest ;

[0105] The new node judgment module is used to judge the expansion mode of the new node based on the adaptive expansion sampling strategy;

[0106] The dynamic step extension module is used to generate a new node, and expand from the nearest point to the new node using the step length calculated by the dynamic step length formula;

[0107] The parent node reselection module is used to reconnect the random tree through the parent node reselection strategy of F-RRT*;

[0108] The target point judgment module is used to judge whether the target point has been reached. If the target point has been reached, the path planning is terminated; otherwise, the path planning is returned to the new node judgment module.

[0109] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot arm path planning method based on adaptive extended sampling, characterized in that: The following steps are involved: S1. Obstacle modeling, remember the starting point is X init , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest ; S2, based on the adaptive expansion sampling strategy, determine the expansion method of the new node; S3, generate a new node, and expand from the nearest point to the new node using the step length calculated by the dynamic step length formula; S4, reconnect the random tree through the parent node reselection strategy of F-RRT*; S5. Determine whether the target point has been reached. If so, end the path planning; otherwise, return to step S2.

2. The method for robot arm path planning based on adaptive extended sampling according to claim 1, characterized in that: The expansion method of the new node based on the adaptive expansion sampling strategy described in step S2 is specifically as follows: S21. First generate a random probability P through a random function r , and then calculate the probability of collision detection failure P cfd , by comparing P r and P cfd The size relationship determines the new node expansion strategy; if P r ≥P cfd , then execute step S22 to implement the target gravity bias sampling strategy; if P r <P cfd , then a random sampling strategy is used to expand new nodes; S22. Based on the target gravity bias sampling strategy, the growth direction of the random tree is corrected to make it biased towards the target point.

3. The method for robot arm path planning based on adaptive extended sampling according to claim 2, characterized in that: The collision detection failure probability P in step S21 cfd The expression is as follows: Where P min , P max The random probability P r The minimum and maximum values ​​that can be obtained, T failed The number of collision detection failures.

4. The method for robot arm path planning based on adaptive extended sampling according to claim 3 is characterized in that: The random sampling strategy may adopt one or more sampling methods of uniform sampling, Gaussian sampling, and adaptive sampling.

5. The method for robot arm path planning based on adaptive extended sampling according to claim 2, characterized in that: The method of correcting the growth direction of the random tree based on the target gravity bias sampling strategy in step S22 so that it is biased toward the target point is as follows: S221, introduce a resultant force F, which is the gravity component F of the target point g and the gravity component F of the random sampling point r The expression of the resultant force F is as follows: F=F g (α)+F r In the formula, F r is a random sampling point X rand The corresponding nearest point X nearest The gravity, F g The target point X goal The corresponding nearest point X nearest The gravity of the target point, α is the angle between the gravity of the target point and the gravity of the random sampling point; S222, calculate the gravity component F of the random sampling point r ; The gravity component F of the random sampling point r The expression is as follows: In the formula, is the gravity component F at the random sampling point r Unit vector in direction; S223, calculate the gravity component F of the target point g ; The target point gravity component F g The expression is as follows: Where f(α) is the target gravity bias function; S224, according to the change characteristics of the target gravity bias function f(α), calculate the transformed target point gravity component F g ; The target gravity bias function f(α) is expressed as follows: Where G is the gravitational coefficient; S225. Calculate the transformed resultant force F according to steps S221 to S224.

6. The method for robot arm path planning based on adaptive extended sampling according to claim 5, characterized in that: The target point gravity component F after transformation in step S224 g The expression is as follows:

7. The method for robot arm path planning based on adaptive extended sampling according to claim 6, characterized in that: The expression of the resultant force F after transformation in step S225 is as follows:

8. The method for robot arm path planning based on adaptive extended sampling according to claim 1, characterized in that: The dynamic step length formula in step S3 is as follows: In the formula, S l , S are the selected dynamic step size and original step size respectively, d neatest is the closest point after expansion neatest Distance to the nearest obstacle, d rand is the shortest distance allowed from the obstacle, and d is the distance threshold, which is used to determine whether the random tree is close to the obstacle.

9. The method for robot arm path planning based on adaptive extended sampling according to claim 1, characterized in that: In step S4, the random tree is reconnected by the parent node reselection strategy of F-RRT* as follows: the nearest point x is searched by the FindReachest process. nearest ancestor of , and find a point x that can be connected to the random sample without conflict rand The vertex x reachest ; Create a node x through the CreatNode process create , while connecting the random sampling points x rand and vertex x reachest The parent node of .

10. A robotic arm path planning system based on adaptive extended sampling, characterized in that: It includes obstacle modeling module, new node judgment module, dynamic step extension module, parent node reselection module and target point judgment module; The obstacle modeling module is used to record the starting point as X init , the target point is X goal , the random sampling point is X rand , the nearest point is X nearest ; The new node judgment module is used to judge the expansion mode of the new node based on the adaptive expansion sampling strategy; The dynamic step extension module is used to generate a new node, and expand from the nearest point to the new node using the step length calculated by the dynamic step length formula; The parent node reselection module is used to reconnect the random tree through the parent node reselection strategy of F-RRT*; The target point judgment module is used to judge whether the target point has been reached. If the target point has been reached, the path planning is terminated; otherwise, the path planning is returned to the new node judgment module.

Images