A bidirectional dynamic growth Informed-RRT* path planning method

Abstract

The invention discloses a bidirectional dynamic growth Informed-RRT* path planning method, comprising the following steps: modeling and binarization according to an obstacle map, determining a starting point and a target point, and setting initial parameters; initializing two random trees according to the obstacle map; generating sampling points by fusing P probability fan constraints using bidirectional search; determining the node expansion direction using growth angle bias; determining the node dynamic expansion step length using variable step length growth, and generating new expansion nodes; performing collision detection and updating the main and secondary tree atlases, and outputting the initial path; determining the dynamic ellipse sampling range; generating new expansion nodes using dynamic growth in the dynamic ellipse area; re-searching for the parent node for the new expansion node within the specified range and reconnecting, and finding the path with the minimum cost. The invention can improve the generation efficiency of the initial path, increase the target orientation and path planning fault tolerance, accelerate the algorithm convergence speed, and solve the problem of low efficiency of path planning due to complex environment.

Description

Technical Field

[0001] The present invention relates to the field of robot path planning, and in particular to a bidirectional dynamic growing Informed-RRT* path planning method. Background Art

[0002] With the rapid development of society, intelligent robots are increasingly used in smart factories, smart inspections, housekeeping services, space exploration and other fields. However, there are still shortcomings in robot path planning, such as low search efficiency and non-optimal planning paths.

[0003] Common motion planning methods are divided into three categories: search, intelligence, and sampling. Among them, the common Dijkstra and A* algorithms belong to search path planning, and the ant colony algorithm and genetic algorithm belong to intelligent path planning algorithms. The above two types of algorithms require the establishment of a spatial environment obstacle model and are not suitable for path planning in a space with a complex environment. Sampling-based path planning algorithms such as Rapidly-exploring Random Trees (RRT), RRT*, and Informed-RRT* can not only be widely used in complex high-dimensional spaces, but also do not require the description of configuration space obstacles. They are suitable for the path planning problems of multi-degree-of-freedom robots in complex and dynamic environments.

[0004] The traditional Informed-RRT* algorithm is developed based on RRT and RRT*. The RRT algorithm guides the blank area through random sampling of the state space, so as to quickly find a feasible planning path, but the path it generates is not optimal and the complexity of the environment will affect the convergence speed. In view of the above shortcomings of the RRT algorithm, RRT* generates an optimal path by re-finding the parent node for the current node within the specified range and reconnecting the nodes within the range, but the planning efficiency is low due to the large range of pruning redundant branches and leaves. In view of the above shortcomings of RRT*, Informed-RRT* establishes a gradient ellipse to specify the sampling space when pruning branches and leaves at the end on the basis of the initial path that has been generated, so as to improve the planning efficiency. However, the algorithm does not constrain the sampling space when generating the initial path, and does not optimize the node expansion method when generating the initial path and pruning redundant branches and leaves, resulting in the planning efficiency not meeting the timeliness requirements of the robot working in a complex environment. Therefore, for the above problems, it is urgent to propose a path planning method that can guide the Informed-RRT* algorithm to quickly find an asymptotically optimal path. Summary of the invention

[0005] The invention aims to provide a bidirectional dynamic growth Informed-RRT* path planning method. A bidirectional search method for the initial path is adopted to effectively improve the efficiency of generating the initial path; a P probability fan-shaped constraint sampling method is adopted to increase the target orientation and path planning fault tolerance; a growth angle bias is adopted when the node is expanded to effectively speed up the algorithm convergence speed; and a variable step-size growth expansion method is adopted to solve the problem of low efficiency of path planning caused by complex environment during the path planning process.

[0006] In order to achieve the above-mentioned purpose, the present invention adopts the following technical scheme:

[0007] The present invention aims to provide a bidirectional dynamically growing Informed-RRT* path planning method to improve the planning efficiency of path planning in complex environments in the prior art, comprising the following steps:

[0008] S1. Model and binarize the obstacle map, determine the starting point and target point, and set the initial parameters;

[0009] S2, initialize two random trees according to the obstacle map;

[0010] S3, using a sampling strategy combining bidirectional search and P probability fan constraints to generate sampling points;

[0011] S4, using the growth angle bias strategy to determine the direction of node expansion;

[0012] S5. Use the variable step growth strategy to determine the dynamic expansion step when the node is expanded, and generate the latest expansion node;

[0013] S6, perform collision detection and update the main and secondary tree atlases, and output the initial path;

[0014] S7, determining the dynamic ellipse sampling range;

[0015] S8, using dynamic growth strategy to generate new expansion nodes in the dynamic ellipse area;

[0016] S9. Re-search for the parent node and reconnect within the specified range of the new extended node to find the path with the minimum cost.

[0017] The bidirectional dynamic growth Informed-RRT* path planning method described in the present invention is also characterized in that step S1 comprises:

[0018] The obstacle map includes obstacles and blank areas in the map, and both are binarized to determine the initial point q init and the target point q goal .

[0019] The step S2 comprises:

[0020] Initialize the vertex sets V1, V2 and edge sets E1, E2 in the two random trees according to the obstacle map, and add the vertex sets V1, V2 and edge sets E1, E2 to the graph sets G1, G2 of the main tree and the secondary tree respectively.

[0021] The step S3 comprises:

[0022] S31, generate P probability sector constraint sampling random value and compare it with the specified threshold B, if it is less than the threshold B, use sector sampling, otherwise choose ordinary random sampling. Generate sampling point q rand ;

[0023] S311, fan sampling refers to connecting the last updated node q new(i-1) and the target point q goal As the midline of the fan, if it is the initial sampling, the initial point q init and the target point q goal As the midline of the sector. With 2θ as the sector angle, the sector area can be determined by the midline and the sector angle, and the sampling area of ​​the sector sampling can be determined by the intersection of the sector area and the map boundary;

[0024] S312, the sampling area of ​​ordinary random sampling is determined by the map boundary;

[0025] S32, determine whether the number of vertices numv1 in the vertex set V1 of tree Tree1 is less than or equal to the number of vertices numv2 in the vertex set V2 of tree Tree2. If so, take Tree1 as the main tree and Tree2 as the secondary tree. Find the node q closest to the sampling point on the main tree nearst , through the growth angle offset and variable step length growth to complete the new expansion node q new extension.

[0026] The step S4 comprises:

[0027] The growth angle bias strategy is to change the direction of the target node expansion, and the angle α is the closest node q to the straight line. nearst and the target node q goal The resulting straight line L(q nearst ,q goal ) and q near1 and sampling node q rand The straight line L(q nearst ,q rand ) between them, if there is no growth direction bias, then the online L(q nearst ,q goal ), the bias direction can be changed by adding an adjustable bias coefficient μ (0<μ<1).

[0028] The step S5 comprises:

[0029] S51, set the initial fixed step size to r, if the sampling node q is known rand and the nearest node q nearst , with q nearst As the center of the semicircle, draw a semicircle with a diameter of 2M (M>r) and a direction similar to q. nearst to q rand The direction is vertical;

[0030] S52, semicircle area is q nearst The scope of environmental complexity is considered. All nodes within the scope are circled with a radius of N. The environmental complexity index is defined as η:

[0031]

[0032] In the above formula, S is the number of intersections between the circle formed by the nodes within the range and the obstacles, and U and V are two threshold levels (0<U<V);

[0033] S53, the size of S can be obtained through each sampling, and the dynamic expansion step length r is defined according to the complexity of the environment d as follows:

[0034]

[0035] The expansion direction can be determined by step S4, and the dynamic expansion step length r can be determined by step S5. d , so far the new extended node q can be completed new extension.

[0036] The step S7 comprises:

[0037] The major axis of the dynamic ellipse is the length c of the current generated path best , starting point q star and the target point q goal are the two foci of the ellipse, and their distance is set to c min , the length of the minor axis of the ellipse is The elliptical sampling area can be determined by two foci and known major and minor axes.

[0038] The step S8 comprises:

[0039] After the elliptical area sampling is determined in step S7, the probability P is generated in step S31, the sampling method is selected and the sampling is performed, and the expansion direction is determined by using the growth angle offset in step S4, and the dynamic expansion step length r is determined by using variable step length growth in step S5. d , and finally generate a new extended node q new .

[0040] The step S9 comprises:

[0041] S91, generate the new extended node q in step S8 new Reselect the parent node q within the specified range father ;

[0042] S911, the node q closest to the current sampling point nearest Temporarily store in the variable q with the minimum cost min middle;

[0043] S912, according to the current new expansion node q new Specify a range, that is, the range of pruning branches and leaves. This specified range is generally based on the current new expansion node q new The circle C is centered and has a radius of 1.5 times the basic step length r. near . Traverse the specified range C near All nodes in the are new extended nodes q new Re-search the parent node q father ;

[0044] S913, the new extension node q new Perform collision detection on each traversed node. If the point q passes the collision detection, near The cost plus the new extended node q new and q near The cost between them is temporarily stored in c'. If the new parent node q father Time new The cost c' is greater than the original q new The cost is small, then the current traversal point q near Store the current new extension node q new The minimum cost parent node q min In this way, after traversing, the current new expansion node q can be found new In the specified range C near The minimum cost parent node q min , update edge sets E1 and E2;

[0045] S92, within the current specified range C near Reconnect all nodes within;

[0046] S921, within the specified range C near Inside (except the minimum cost parent node q min Other nodes outside the node) put the new expansion node q new Re-parent node q father Compare the previous costs;

[0047] S922, these traversed points and the new extended node q new If the collision detection is passed and the original path cost of the traversal point is greater than the cost of the new connection, the parent node q of the current traversal point is updated father , update the total atlas G.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] The invention aims to provide a bidirectional dynamic growth Informed-RRT* path planning method. First, a bidirectional search method for the initial path is adopted to effectively improve the efficiency of generating the initial path; secondly, a P probability fan-shaped constraint sampling method is adopted to increase the target orientation and path planning fault tolerance; then, the growth angle bias is adopted when the node is expanded to effectively speed up the algorithm convergence speed; finally, a variable step-size growth expansion method is adopted to solve the problem of low efficiency of path planning caused by complex environment during the path planning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is an overall flow chart of a bidirectional dynamic growing Informed-RRT* path planning method in the present invention;

[0051] Figure 2 Schematic diagram of the P probability sector constraint sampling principle of the present invention;

[0052] Figure 3 It is a schematic diagram of the growth angle bias principle of the present invention;

[0053] Figure 4 This is a schematic diagram of the variable step length growth principle of the present invention;

[0054] Figure 5 This is a schematic diagram of the elliptical sampling space of the present invention;

[0055] Figure 6 This is a graph showing the effect of testing different algorithm path planning for the present invention;

[0056] Figure 7 It is a line graph comparing the running time of the algorithms of the present invention. DETAILED DESCRIPTION

[0057] The present invention is further described in detail below in conjunction with the accompanying drawings and implementation examples. The specific embodiments described in this application are only used to explain the relevant invention, rather than to limit the invention. It should also be noted that, for ease of description, only the parts related to the present invention are shown in the accompanying drawings.

[0058] The present invention proposes a bidirectional dynamic growth Informed-RRT* path planning method, such as Figure 1 As shown, the following steps are included:

[0059] Step 1: Model and binarize the obstacle map, which includes obstacles and blank areas in the map, and determine the initial point q init and the target point q goal .

[0060] Step 2: Initialize the vertex sets V1, V2 and edge sets E1, E2 in the two random trees according to the obstacle map, and add the vertex sets V1, V2 and edge sets E1, E2 to the graph sets G1, G2 of the main tree and the secondary tree respectively.

[0061] Step 3: Use the sampling strategy of bidirectional search and P probability sector constraint to generate sampling points in the initial map space. Generate the random value of P probability sector constraint sampling and compare it with the specified threshold B. If it is less than the threshold B, sector sampling is used, otherwise ordinary random sampling is selected. Figure 2 As shown, the sampling point q is generated rand . Fan sampling refers to connecting the last updated node q new(i-1) and the target point q goal As the midline of the fan, if it is the initial sampling, the initial point q init and the target point q goal As the midline of the sector. With 2θ as the sector angle (θ=60°), the sector area can be determined by the midline and the sector angle, and the sampling area of ​​the sector sampling can be determined by the intersection of the sector area and the map boundary. The sampling area of ​​ordinary random sampling is determined by the map boundary. Determine whether the number of vertices numv1 in the vertex set V1 of the tree Tree1 is less than or equal to the number of vertices numv2 in the vertex set V2 of the tree Tree2. If so, take Tree1 as the main tree and Tree2 as the secondary tree. Find the node q on the main tree that is closest to the sampling point nearst , through the growth angle offset and variable step length growth to complete the new expansion node q new extension.

[0062] Step 4: Based on the newly generated sampling points, use the growth angle bias strategy to determine the direction of node expansion. The growth angle bias strategy is to change the direction of target node expansion, such as Figure 3 As shown, the angle α is the closest node q nearst and the target node q goal The resulting straight line L(q near1 ,q goal ) and q near1 and sampling node q rand The straight line L(q near1 ,q rand ) between them, if there is no growth direction bias, then the online L(q near1 ,q goal), the bias direction can be changed by adding an adjustable bias coefficient μ (0<μ<1).

[0063] Step 5: Use the variable step growth strategy to determine the dynamic expansion step length r when the node is expanded d , generate the latest extended node q new .like Figure 4 As shown, the initial fixed step size is set to r. If the sampling node q is known rand and the nearest node q nearst , with q netsra As the center of the semicircle, draw a semicircle with a diameter of 2M (M>r) and a direction similar to q. nearst to q rand The semicircle area is q nearst The scope of environmental complexity is considered. All nodes within the scope are circled with a radius of N. The environmental complexity index is defined as η:

[0064]

[0065] In the above formula, S is the number of intersections between the circle formed by the nodes in the range and the obstacles, and U and V are two threshold levels (0<U<V). The size of S can be obtained through each sampling, and the dynamic expansion step length r is defined according to the complexity of the environment. d as follows:

[0066]

[0067] Step 4 can determine the expansion direction, and step 5 can determine the dynamic expansion step length r. d , so far the new extended node q can be completed new extension.

[0068] Step 6: For the new extended node q new Perform collision detection and update the main and secondary tree atlases G1 and G2. After iteration, if the latest expansion node threshold conditions of the main and secondary trees are met, the initial path is output.

[0069] Step 7: Generate a dynamic ellipse sampling range based on the initial point, target point, and planned initial path. Figure 5 The major axis of the dynamic ellipse is the length c of the current generated path best , starting point q star and the target point q goal are the two foci of the ellipse, and their distance is set to c min , the length of the minor axis of the ellipse is The elliptical sampling area can be determined by two foci and known major and minor axes.

[0070] Step 8: Generate new extended nodes q in the dynamic ellipse region using the P probability fan constraint sampling strategy, growth angle bias strategy and variable step size growth strategy new After determining the sampling of the elliptical area in step 7, use step 3 to generate the probability P, select the sampling method and sample, use step 4 to determine the expansion direction using the growth angle bias, and use step 5 to determine the dynamic expansion step length r using variable step length growth. d , and finally generate a new extended node q new .

[0071] Step 9: Generate a new extended node q in step 8 new Reselect the parent node q within the specified range father , the node q closest to the current sampling point nearest Temporarily store the variable q with the lowest cost min In the example, according to the current new expansion node q new Specify a range, that is, the range of pruning branches and leaves. This specified range C near Generally, the current new expansion node q new The circle C is centered and has a radius of 1.5 times the basic step length r. near . Traverse the specified range C near All nodes in the are new extended nodes q new Re-search the parent node q father , put the new extension node q new Perform collision detection on each traversed node. If the point q passes the collision detection, near The cost plus the new extended node q new and q near The cost between them is temporarily stored in c'. If the new parent node q father Time new The cost c' is greater than the original q new The cost is small, then the current traversal point q near Store the current new extension node q new q new The minimum cost parent node q min In this way, after traversing, the current new expansion node q can be found new In the specified range C near The minimum cost parent node q min , update edge sets E1 and E2. In the current specified range C near Reconnect all nodes within the specified range C near Inside (except the minimum cost parent node q min Other nodes outside the node) put the new expansion node q new q new Re-parent node q fatherCompare the previous cost, the traversed points and the new expanded node q new If the collision detection is passed, if the original path cost of the traversal point is greater than the cost of the new connection, the parent node q of the current traversal point is updated. father , update the total atlas G. After completing the specified number of iterations, output the final planned path.

[0072] In order to verify the effectiveness of this method in solving path planning problems, two groups of test experiments were designed. One group is a comparative experiment on the effects of different path planning algorithms under the same environmental complexity; the second group is a test experiment on algorithm performance indicators. The system environment is Windows 10 operating system and PyCharm 2020.3.3 (Community Edition).

[0073] Experiment 1 is to compare the path quality of RRT, RRT*, Informed-RRT* and the bidirectional dynamic growth Informed-RRT* path planning method proposed by the present invention under the same basic step size of 2 and the number of iterations of 300 under the same environmental complexity. The map size is 25*25, and the obstacles are as follows Figure 6 The black part is shown. The comparison results are as follows Figure 6 As shown in the figure, the planned path of the improved algorithm is shorter and has a smaller turning range than other paths. Compared with other planned paths, when the number of sampling is limited and the same, the improved algorithm is more adaptable to environments with higher complexity.

[0074] Experiment 2 is to quantitatively test the algorithm time consumption. First, 20 experiments are conducted to compare the traditional Informed-RRT* algorithm and the improved algorithm after introducing the growth corner bias under the same environment and basic step size 2, and the average planning time and total planning time of the initial path are tested. The map size is 25*25, and the obstacles are as follows: Figure 6 It can be seen that the overall performance has been improved to a certain extent in both the initial path planning process and the total planning process, and the overall planning performance improvement is more obvious than the initial path. The specific experimental results are shown in Table 1 below:

[0075] Table 1 Comparison of performance of introducing growth angle α bias

[0076]

[0077] Then, 20 experiments were conducted to compare the traditional Informed-RRT* algorithm and the improved algorithm of the present invention under the same environment and basic step size 2. Figure 7 As shown, it can be seen that as the number of sampling iterations increases, the time consumption difference between the two algorithms widens, and the path planning algorithm of the present invention is more efficient.

[0078] For simulation experiments, the present invention uses indicators such as path length, planning time, planning success rate, and the number of initial path iterations. The number of edges in the edge sets E1 and E2 planned on the m-tree, L i represents the length of the i-th edge of the plan; the planning time T represents the total time the algorithm runs in the simulation environment; the planning success rate Where C represents the number of successful planning in the known iteration rounds; the number of initial path iterations F represents the iteration round when the first planning is successful. In addition to the above four indicators, the average turning index (ATI) algorithm cost indicator is also proposed. The average turning index refers to the average value of the angles of all nodes connected on the path after the path is planned. The calculation method is shown in Formula 3.

[0079]

[0080] In the above formula (3), n is the total number of nodes in the tree with planned paths, and the i-th node q i The coordinates of (x i y i ), ATI is the average turning index. The performance comparison results of 20 groups of experiments under different environments and the same number of iterations are shown in Table 2 below.

[0081] Whether in a simple environment or a complex environment, the performance of parameters such as the average planning path length, average planning time, number of initial path iterations, planning success rate and ATI have been improved; the performance improvement of the average number of initial path iterations in complex environments and simple environments is similar; except for the average number of initial path iterations and the planning success rate, the performance improvement of other parameters in complex environments is better than that in simple environments, because the improved algorithm optimizes the planning process after the initial path is generated. For the ATI parameter, because the improved algorithm applies the variable step size idea, the performance improvement of the improved algorithm of the present invention for complex environments is more obvious, and it is more suitable for path planning in complex environments. The overall improved algorithm has greatly improved planning efficiency, planning success rate, quality of planned paths and environmental applicability.

[0082] Table 2 Algorithm performance comparison

[0083]

[0084] The above-described embodiments are only preferred embodiments for fully illustrating the present invention, and the protection scope of the present invention is not limited thereto. Any equivalent substitution or transformation made by technicians of the present technology on the basis of the present invention is within the protection scope of the present invention. The protection scope of the present invention shall be subject to the claims.

Claims

1. A bidirectional dynamic growth Informed-RRT* path planning method, characterized in that: The following steps are involved: S1. Model and binarize the obstacle map, determine the starting point and target point, and set the initial parameters; S2, initialize two random trees according to the obstacle map; S3, using a sampling strategy combining bidirectional search and P probability fan constraints to generate sampling points; S4, using the growth angle bias strategy to determine the direction of node expansion; S5. Use the variable step growth strategy to determine the dynamic expansion step when the node is expanded, and generate the latest expansion node; S6, perform collision detection and update the main and secondary tree atlases, and output the initial path; S7, determining the dynamic ellipse sampling range; S8, using dynamic growth strategy to generate new expansion nodes in the dynamic ellipse area; S9. Re-search for the parent node and reconnect within the specified range of the new extended node to find the path with the minimum cost.

2. According to the bidirectional dynamic growth Informed-RRT* path planning method of claim 1, it is also characterized in that: The step S1 comprises: The obstacle map includes obstacles and blank areas in the map, and both are binarized to determine the initial point q init and the target point q goal .

3. According to the bidirectional dynamic growth Informed-RRT* path planning method of claim 1, it is also characterized in that: The step S2 comprises: Initialize the vertex sets V1, V2 and edge sets E1, E2 in the two random trees according to the obstacle map, and add the vertex sets V1, V2 and edge sets E1, E2 to the graph sets G1, G2 of the main tree and the secondary tree respectively.

4. According to claim 1, the bidirectional dynamic growth Informed-RRT* path planning method is characterized in that: The step S3 comprises: S31, generate P probability sector constraint sampling random value and compare it with the specified threshold B, if it is less than the threshold B, use sector sampling, otherwise choose ordinary random sampling, generate sampling point q rand ; S311, fan sampling refers to connecting the last updated node q new(i-1) and the target point q goal As the midline of the fan, if it is the initial sampling, the initial point q init and the target point q goal As the midline of the sector, with 2θ as the sector angle, the sector area can be determined by the midline and the sector angle, and the sampling area of ​​the sector sampling can be determined by the intersection of the sector area and the map boundary; S312, the sampling area of ​​ordinary random sampling is determined by the map boundary; S32, determine whether the number of vertices numv1 in the vertex set V1 of tree Tree1 is less than or equal to the number of vertices numv2 in the vertex set V2 of tree Tree2. If so, take Tree1 as the main tree and Tree2 as the secondary tree, and find the node q on the main tree that is closest to the sampling point nearst , through the growth angle offset and variable step length growth to complete the new expansion node q new extension.

5. The bidirectional dynamic growth Informed-RRT* path planning method according to claim 1 is further characterized in that: The step S4 comprises: The growth angle bias strategy is to change the direction of the target node expansion, and the angle α is the closest node q to the straight line. nearst and the target node q goal The resulting straight line L(q nearst ,q goal ) and q near1 and sampling node q rand The straight line L(q nearst ,q rand ) between them, if there is no growth direction bias, then the online L(q nearst ,q goal ), the bias direction can be changed by adding an adjustable bias coefficient μ, 0<μ<1.

6. The bidirectional dynamic growth Informed-RRT* path planning method according to claim 1 is further characterized in that: The step S5 comprises: S51, set the initial fixed step size to r, if the sampling node q is known rand and the nearest node q nearst , with q nearst As the center of the semicircle, draw a semicircle with a diameter of 2M, M>r, and a direction equal to q. nearst to q rand The direction is vertical; S52, semicircle area is q nearst The scope of environmental complexity is considered. All nodes within the scope are circled with a radius of N. The environmental complexity index is defined as η: In the above formula, S is the number of intersections between the circle formed by the nodes in the range and the obstacles, and U and V are two threshold levels, 0<U<V; S53, the size of S can be obtained through each sampling, and the dynamic expansion step length r is defined according to the complexity of the environment d as follows: The expansion direction can be determined by step S4, and the dynamic expansion step length r can be determined by step S5. d , so far the new extended node q can be completed new extension.

7. The bidirectional dynamic growth Informed-RRT* path planning method according to claim 1 is further characterized in that: The step S7 comprises: The major axis of the dynamic ellipse is the length c of the current generated path best , starting point q star and the target point q goal are the two foci of the ellipse, and their distance is set to c min , the length of the minor axis of the ellipse is The elliptical sampling area can be determined by two foci and known major and minor axes.

8. The bidirectional dynamic growth Informed-RRT* path planning method according to claim 1 is further characterized in that: The step S8 comprises: After the elliptical area sampling is determined in step S7, the probability P is generated in step S31, the sampling method is selected and the sampling is performed, and the expansion direction is determined by using the growth angle offset in step S4, and the dynamic expansion step length r is determined by using variable step length growth in step S5. d , and finally generate a new extended node q new .

9. The bidirectional dynamic growth Informed-RRT* path planning method according to claim 1 is further characterized in that: The step S9 comprises: S91, generate the new extended node q in step S8 new Reselect the parent node q within the specified range father ; S911, the node q closest to the current sampling point nearest Temporarily store the variable q with the lowest cost min middle; S912, according to the current new expansion node q new Specify a range, i.e. the range of pruning branches and leaves. This specified range is generally based on the current new expansion node q new The circle C with the center as the circle and 1.5 times the basic step length r as the radius near , traverse the specified range C near All nodes in the are new extended nodes q new Re-search the parent node q father ; S913, the new extension node q new Perform collision detection on each traversed node. If the point q passes the collision detection, near The cost plus the new extended node q new and q near The cost between them is temporarily stored in c'. If the new parent node q father Time new The cost c' is greater than the original q new The cost is small, then the current traversal point q near Store the current new extension node q new The minimum cost parent node q min In this way, after traversing, the current new expansion node q can be found new In the specified range C near The minimum cost parent node q min , update edge sets E1 and E2; S92, within the current specified range C near Reconnect all nodes within; S921, within the specified range C near In addition to the minimum cost parent node q min Other nodes outside the new extended node q new Re-parent node q father Compare the previous costs; S922, these traversed points and the new extended node q new If the collision detection is passed and the original path cost of the traversal point is greater than the cost of the new connection, the parent node q of the current traversal point is updated father , update the total atlas G.

Images