Adaptive step length Combine-RRT* path planning method

Abstract

The invention relates to an adaptive step length Combine-RRT * path planning method, which belongs to the technical field of robot path planning, and comprises the following steps: modeling and binarizing according to an obstacle map, determining a starting point and a target point, and setting initial parameters; initializing a random tree according to the obstacle map; determining a normal distribution sampling process and parameters; a sampling strategy of a sampling process is selected by using a dynamic greedy factor for sampling to generate sampling points; determining a dynamic expansion step length during node expansion by using a variable step length growth strategy, and generating a new expansion node; performing collision detection, updating the tree atlas, and outputting an initial path; determining an elliptical sampling range; using a dynamic greedy factor to select elliptical region sampling or normal distribution sampling to generate sampling points; re-searching father nodes and re-connecting in a specified range of the new extended nodes, and searching a path with the minimum cost; and a B spline curve rear end optimization path is introduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and relates to a Combine-RRT* path planning method with an adaptive step size. Background Art

[0002] In recent years, mobile robots have been widely used in scenarios such as freight service, search and rescue, inspection, and environmental detection due to their advantages of mature structure, relatively energy-saving, convenient drive control, high driving speed, and high driving efficiency. However, there are still deficiencies in robot path planning. For example, in unstructured scenarios with narrow channels and many obstacles, the success rate of path planning is low, and the algorithm convergence is poor.

[0003] Currently, global path planning algorithms mainly include intelligent optimization, graph search, and sampling algorithms. Intelligent optimization algorithms mainly include genetic algorithms, ant colony algorithms, particle swarm algorithms, etc. Graph search algorithms are mainly represented by Dijkstra and A* algorithms. The above two types of algorithms have disadvantages such as large memory occupation and low search efficiency. Sampling-based path planning algorithms such as Rapidly-exploring Random Trees (RRT), RRT*, Informed-RRT*, and RRT*-N can not only be widely applied to complex high-dimensional spaces, but also have asymptotic optimality and probabilistic completeness, and are suitable for path planning problems of multi-degree-of-freedom robots in complex environments and narrow channels.

[0004] RRT* enables the RRT algorithm to achieve a breakthrough in asymptotic optimality. It guides to blank areas through random sampling in the state space, so as to quickly find a feasible planning path, and then optimizes the path by re-finding the parent node for the current node within a specified range and reconnecting the nodes within the range. Since random sampling in the RRT* algorithm will lead to exhaustive exploration in the entire obstacle-free space, the convergence speed becomes slow. On this basis, improved algorithms such as Informed-RRT* and RRT*-N are proposed to narrow the exploration range through purposeful sampling and accelerate convergence. However, the exploration is limited, the ability to develop unknown environments is lacking, and the planned path is prone to falling into local minima, resulting in the planning efficiency not meeting the timeliness requirements of robots operating in complex environments. Moreover, the sampling processes of the above sampling algorithms all use a fixed step size, which leads to a low success rate of path planning in narrow channels and obstacle-filled environments. Therefore, for the above problems, there is an urgent need to propose a path planning algorithm that can be highly efficient in narrow and obstacle-filled environments and has both exploratory and purposeful characteristics. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a Combine-RRT* method that adapts the step size under uniform sampling and non-uniform sampling. According to the greedy strategy, the greedy factor is designed to combine uniform sampling and non-uniform sampling, and a sampling strategy for balancing exploration and exploitation is proposed, which improves the path convergence speed while having the ability to explore the optimal path; an expansion method with variable step size growth is adopted to solve the problem of low success rate of path planning due to complex environment during path planning; a cubic B-spline curve is introduced for path smoothing optimization to ensure that the generated path conforms to the robot dynamics constraints.

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

[0007] A Combine-RRT* path planning method with an adaptive step size, comprising the following steps:

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

[0009] S2: Initialize a random tree according to the obstacle map;

[0010] S3: Determine the normal distribution sampling process and parameters;

[0011] S4: Use the dynamic greedy factor to select the sampling strategy in the sampling process to sample and generate sampling points;

[0012] S5: Use the variable step size growth strategy to determine the dynamic expansion step size when the node expands, and generate new expanded nodes;

[0013] S6: Perform collision detection and update the tree atlas, and output the initial path;

[0014] S7: Determine the elliptical sampling range;

[0015] S8: Use the dynamic greedy factor to select elliptical region sampling or normal distribution sampling to generate sampling points;

[0016] S9: Re-search for the parent node and reconnect within the specified range of the new expanded node, and find the path with the minimum cost;

[0017] S10: Introduce a B-spline curve to optimize the path at the back end.

[0018] Further, in step S1, the obstacle map includes the obstacle region O obs and the free region O free , and both are binarized, and the starting point q start and the target point q target are determined;

[0019] In step S2, the vertex set V and edge set E in the randomly generated tree initialized according to the obstacle map are added to the tree map set G respectively.

[0020] Furthermore, the determination of the normal distribution sampling process and parameters in step S3 specifically includes the following steps:

[0021] S31: Calculate the unit direction vector d from the starting point to the ending point i :

[0022]

[0023] S32: Multiply the longitudinal value of d i by a negative number, and generate a vertical vector d I by rotating the direction vector by 90 degrees to construct a two-dimensional orthogonal basis, and the vertical vector is used for the lateral expansion direction of Gaussian sampling;

[0024] S33: Generate a basic sampling point P base on the line connecting the starting point and the target point according to a uniform distribution, as the center point of the Gaussian distribution:

[0025] P base = q start + ξ·L·d i , ξ ~ u(0, 1)

[0026] where L = ||q target - q start || is the Euclidean distance between the starting point and the target point, and ξ is a uniformly distributed random number;

[0027] S34: Use the Gaussian distribution to superimpose a normal distribution lateral offset on the basic sampling point, and finally generate a sampling point:

[0028] P final = P base + η·δ·d I , η ~ N(0, 1)

[0029] In the formula, δ is the standard deviation, and η is a standard normal distribution random number.

[0030] Furthermore, the use of the dynamic greedy factor to select the sampling strategy for the sampling process in step S4 to generate sampling points specifically includes the following steps:

[0031] S41: Record the current iteration number k and the total iteration number K at each step max , and calculate the dynamic greedy factor B:

[0032]

[0033] where a and b are coefficients;

[0034] S42: Generate a random value between 0 and 1 and compare it with the greedy factor B. If it is less than the greedy factor B, then use normal distribution sampling; otherwise, select ordinary random sampling. Finally, generate a sampling point q rand ; The ordinary random sampling is to uniformly sample in the entire obstacle-free free space O free Uniform sampling.

[0035] Furthermore, step S5 uses a variable step size growth strategy to determine the dynamic expansion step size when expanding nodes, generating new expanded nodes, which specifically includes the following steps:

[0036] S51: Given the sampling node q rand and the nearest node q nearest , use the obstacle density to represent the influence of the surrounding environment on the sampling step size, and define the obstacle density function:

[0037]

[0038] In the formula, N is the number of obstacles, and d i (x) is the distance from a single obstacle to the nearest node q nearest . When the distance exceeds the set maximum distance D max , this obstacle distance is not included in the obstacle density function;

[0039] S52: Given the set fixed sampling step size R, minimum step size r min , maximum step size r max and the obstacle density near the nearest node q nearest , the greater the obstacle density, the shorter the sampling step size; the smaller the obstacle density, the longer the sampling step size. Then the dynamic expansion step size is calculated as follows:

[0040] r d = r min +(r max - r min )(1 - Density(k)) 2

[0041] Determine the sampling strategy through step S4, and determine the dynamic expansion step size r d through step S5. Thus, the expansion of the new expanded node q new is completed.

[0042] Furthermore, step S6 performs collision detection and updates the tree map set, outputting the initial path, which specifically includes the following steps:

[0043] S61: Perform collision detection on the new expanded node, and check whether the expanded node q new is in the obstacle space O obsInside; if not, add it to the atlas G. If it is in the obstacle space O obs , discard this expansion point;

[0044] S62: Calculate the distance from the expansion node q new to the target point q target , and determine whether it is close to the target point q target . If it is close, output the initial path.

[0045] Furthermore, for the determination of the elliptical sampling range in step S7, the major axis of the dynamic ellipse is the length c of the currently generated path best , the starting point q start and the target point q target are respectively the two foci of the ellipse, and the distance between them is set as c min , and the minor axis length of the ellipse Determine the elliptical sampling area from the two foci and the known major and minor axes.

[0046] Furthermore, the specific steps of step S8 are as follows:

[0047] S81: After the initial path exists, the sampling strategy selection in step S4 becomes normal distribution sampling and elliptical sampling within a defined area; as the number of iterations increases, the probability of selecting the elliptical sampling strategy becomes greater, accelerating the convergence;

[0048] S82: After selecting the sampling strategy, sample to generate a sampling node q rand , and use step S5 to determine the dynamic expansion step length r by variable step length growth d , and finally generate a new expansion node q new .

[0049] Furthermore, the specific steps of step S9 are as follows:

[0050] S91: For the new expansion node q that passes the obstacle detection in step S6 new , reselect the parent node q father within a specified range; specifically, it includes the following steps:

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

[0052] S912: According to the current new expansion node q new , specify a range C near , and traverse all the nodes within the specified range C near as the new expansion node q new to re-find the parent node q father ;

[0053] S913: Perform collision detection between the new extended node q new and each traversed node. For the points C near passing the collision detection, add the cost of these points to the cost between the new extended node q new and q nearest and temporarily store it in c'. If the cost c' of the newly constructed parent node q father is smaller than the original cost of q new , then store the current traversed point q new in the minimum-cost parent node q nearest of the current new extended node q new . After the traversal is completed, find the minimum-cost parent node q min of the current new extended node q new within the specified range C near and update the edge set E; min

[0054] S92: Reconnect all nodes within the current specified range C near ;

[0055] S921: For the other nodes within the specified range C near except the minimum-cost parent node q min , use the new extended node q new as the parent node q father again and compare with the previous cost;

[0056] S922: For these traversed points and the new extended node q new , if they pass the collision detection and the original path cost of the traversed point is greater than the cost of the new connection, then update the parent node q father of the current traversed point and update the total graph set G.

[0057] Furthermore, in step S10, use a cubic B-spline curve to optimize the path generated in step S9. In the total graph set G, select control points for cubic spline curve interpolation operations; then perform difference processing on the points in the set at fixed intervals to obtain a uniformly distributed discrete point set G*; the definition of the third-order B-spline curve is as follows:

[0058]

[0059] where B i,3 (u) is the basis function and p i is the control point.

[0060] ​The beneficial effects of the present invention are as follows: The present invention aims to propose a Combine-RRT* algorithm that integrates adaptive step sizes under uniform sampling and non-uniform sampling. First, a greedy factor is designed according to the greedy strategy, combining uniform sampling and non-uniform sampling, and a sampling strategy that balances exploration and exploitation is proposed, which improves the path convergence speed while having the ability to explore the optimal path. Then, an expansion method with variable step sizes is adopted to solve the problem of low success rate of path planning caused by complex environments during path planning. Finally, a cubic B-spline curve is introduced for path smoothing optimization to ensure that the generated path meets the robot dynamics constraints.

[0061] Other advantages, objectives, and features of the present invention will be elaborated to some extent in the subsequent specification, and to some extent, will be obvious to those skilled in the art based on the study of the following text, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following specification. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] In order to make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be described in detail preferably with reference to the accompanying drawings, where:

[0063] Figure 1 is the overall flowchart of the Combine-RRT* path planning method with adaptive step sizes according to the present invention;

[0064] Figure 2 In (a) is a schematic diagram of the Gaussian probability distribution of the starting point and the target point, and in (b) is the contour line of the joint probability density;

[0065] Figure 3 is a schematic diagram of the adaptive step size according to the environmental obstacles;

[0066] Figure 4 is a schematic diagram of the elliptical sampling area;

[0067] Figure 5 is a schematic diagram of the spline curve smoothing process;

[0068] Figure 6 is the effect diagram of the path generated by the RRT* algorithm, where (a)-(e) are the effect diagrams of the ordinary, cluttered, dense, maze, and narrow simulation environments respectively;

[0069] Figure 7 is the effect diagram of the path generated by the Informed-RRT* algorithm, where (a)-(e) are the effect diagrams of the ordinary, cluttered, dense, maze, and narrow simulation environments respectively;

[0070] Figure 8Generate the path effect diagram for the RRT*-N algorithm, where (a)-(e) are the effect diagrams of the ordinary, cluttered, dense, maze, and narrow simulation environments respectively;

[0071] Figure 9 Generate the path effect diagram for the Combine-RRT* algorithm, where (a)-(e) are the effect diagrams of the ordinary, cluttered, dense, maze, and narrow simulation environments respectively;

[0072] Figure 10 Generate the comparison diagram of the indexes between the present invention and other benchmark algorithms, where (a) is the running time, (b) is the path length, (c) is the number of turning points, and (d) is the success rate. Specific implementation mode

[0073] The following uses specific specific examples to illustrate the implementation mode of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation modes. The details in this specification can also be based on different viewpoints and applications, and various modifications or changes can be made without departing from the spirit of the present invention. It should be noted that the diagrams provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner. Without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0074] Embodiment 1:

[0075] The present invention provides an adaptive step size Combine-RRT* path planning method, as Figure 1 shown, including the following steps:

[0076] Step 1: The obstacle map includes the obstacle area O obs and the free area O free in the map, and perform binary processing on both to determine the starting point q start and the target point q target .

[0077] Step 2: Initialize the vertex set V and edge set E in the random tree according to the obstacle map, and add the vertex set V and edge set E to the atlas G of the tree respectively.

[0078] Step 3: Determine the normal distribution sampling process and parameters. When selecting the sampling strategy, first describe the process and parameters of the algorithm using normal distribution sampling between the starting point and the target point connection;

[0079] This step is specifically as follows:

[0080] 1) Calculate the unit direction vector d i from the starting point to the end point:

[0081]

[0082] 2) Multiply the longitudinal value of d i by a negative number, and generate a vertical vector d by rotating the direction vector by 90 degrees I , and construct a two-dimensional orthogonal basis. The vertical vector is used for the lateral expansion direction of Gaussian sampling.

[0083] 3) Generate basic sampling points P base uniformly distributed on the line connecting the starting point and the target point as the center point of the Gaussian distribution:

[0084] P base = q start + ξ·L·d i , ξ ~ u(0,1)

[0085] where L = ||q target - q start || is the Euclidean distance between the starting point and the target point, and ξ is a uniformly distributed random number.

[0086] 4) Use the Gaussian distribution to superimpose the normal distribution lateral offset on the basic sampling points, and finally generate sampling points:

[0087] P final = P base + η·δ·d I , η ~ N(0,1)

[0088] In the formula, δ is the standard deviation, and η is a standard normal distribution random number. Visualize the Gaussian probability distribution sampling of the starting point and the target point, as shown in Figure 2 (a) and (b) in.

[0089] Step 4: Use the dynamic greedy factor to select the sampling strategy for the sampling process to generate sampling points.

[0090] The specific steps are as follows:

[0091] 1) Record the current iteration number k and the total iteration number K at each step max , where a and b are coefficients in the following formula, and calculate the dynamic greedy factor B:

[0092]

[0093] 2) Generate a random value between 0 and 1 and compare it with the greedy factor B. If it is less than the greedy factor B, then use normal distribution sampling; otherwise, select ordinary random sampling to finally generate the sampling point q rand . Ordinary random sampling is to sample uniformly in the entire obstacle-free free area O free .

[0094] Step 5: Use the variable step-size growth strategy to determine the dynamic expansion step size when expanding nodes, and generate new expansion nodes.

[0095] The specific steps are as follows:

[0096] 1) Given the sampling node q rand and the nearest node q nearest , to represent the influence of the surrounding environment on the sampling step size, it is represented by the obstacle density, and the obstacle density function is defined as:

[0097]

[0098] In the above formula, N is the number of obstacles, and d i (x) is the distance from a single obstacle to the nearest node q nearest . It means that when the distance exceeds the set maximum distance D max , this obstacle distance is not included in the obstacle density function.

[0099] 2) Given the set fixed sampling step size R, minimum step size r min , maximum step size r max and the obstacle density near the nearest node q nearest . As Figure 3 shown, the greater the obstacle density, the shorter the sampling step size should be for exploration; the smaller the obstacle density, the longer the sampling step size should be to accelerate convergence. Calculate the variable step size:

[0100] r d = r min +(r max -r min )·(1 - Density(k)) 2

[0101] Through Step 4, the sampling strategy can be determined, and through Step 5, the dynamic expansion step size r d can be determined. Thus, the expansion of the new expansion node q new can be completed.

[0102] Step 6: Perform collision detection and update the tree atlas, and output the initial path.

[0103] The specific steps are as follows:

[0104] 1) Perform collision detection on the expansion node to check whether the expansion node q new is within the obstacle space O obs . If not, add it to the atlas G. If it is within the obstacle space O obs , then discard this expansion point.

[0105] 2) Calculate the distance from the latest expansion node q new to the target point qtarget Based on the distance, determine whether it is close to the target point q target If it is close, output the initial path.

[0106] Step Seven: Determine the elliptical sampling range. The major axis of the dynamic ellipse is the length c of the currently generated path best The starting point q start and the target point q target are respectively the two foci of the ellipse, and the distance between them is set as c min The minor axis length of the ellipse Given the two foci and the known major and minor axes, the elliptical sampling area can be determined, as shown Figure 4 in the figure.

[0107] Step Eight: Use the dynamic greedy factor to select either elliptical area sampling or normal distribution sampling to generate sampling points.

[0108] The specific steps are as follows:

[0109] 1) After the initial path exists, the sampling strategy selection in Step Four becomes normal distribution sampling and elliptical sampling within the defined area. As the number of iterations increases, the probability of selecting the elliptical sampling strategy becomes greater, accelerating the convergence.

[0110] 2) After selecting the sampling strategy, sample to generate a sampling node q rand Using Step Five, determine the dynamic expansion step size r by variable step - length growth d Finally, generate a new expanded node q new .

[0111] Step Nine: Re - search for the parent node and reconnect within the specified range of the new expanded node, and find the path with the minimum cost.

[0112] The specific steps are as follows:

[0113] 1) For the new expanded node q that passed the obstacle detection in Step Six new re - select the parent node q within the specified range father ;

[0114] 1.1) Temporarily store the node q closest to the current sampling point in the variable q with the minimum cost nearest ; min ;

[0115] 1.2) Define a range based on the current new expanded node q new That is, the range for pruning branches and leaves. This specified range is generally a circle C with the current new expanded node q new as the center and 1.5 times the basic step - length R as the radius near . Traverse all the nodes within the specified range C near as the new expanded node qnew Re - search for the parent node q father ;

[0116] 1.3) Perform collision detection between the new extended node q new and each traversed node. If the point C near passes the collision detection, the cost of point C new plus the cost between the new extended node q nearest and q father is temporarily stored in c'. If the cost c' of the newly constructed parent node q new is smaller than the original cost of q new , then store the currently traversed point q nearest into the parent node q new with the minimum cost of the current new extended node q min . In this way, after traversal, the parent node q new with the minimum cost of the current new extended node q near within the specified range C min can be found, and the edge set E is updated;

[0117] 2) Re - connect all nodes within the current specified range C near ;

[0118] 2.1) For the nodes within the specified range C near (except for the parent node q min with the minimum cost), the new extended node q new is re - used as the parent node q father to compare the previous cost;

[0119] 2.2) If the traversed points and the new extended node q new pass the collision detection and the original path cost of the traversed point is greater than the cost of the new connection, then update the parent node q father of the current traversed point, and update the total graph set G. After completing the specified number of iterations or reaching the convergence condition, the planned path is output at the front - end.

[0120] Step Ten: Introduce a B - spline curve to optimize the path at the back - end. Use a cubic B - spline curve to optimize the path generated in Step S9. In the total graph set G, select control points for cubic spline curve interpolation operations. Then, interpolate the points in the set at fixed intervals to obtain a uniformly distributed discrete point set G*, as Figure 5 shown. The blue path in the figure is the path generated by the Combine - RRT* front - end, and the green path is the path after smooth processing at the back - end. The definition of the third - order B - spline curve is as follows:

[0121]

[0122] Example 2:

[0123] To verify the effectiveness of the method in solving the path planning problem, a comparative experiment on the effects of different path planning algorithms was designed under the same environmental complexity. The simulation environments are: ordinary, cluttered, dense, maze, and narrow environments. With the same basic step size of 50 and the number of iterations of 100, the path quality and algorithm performance planned by RRT*, Informed-RRT*, and RRT*-N and the Combine-RRT* path planning method with an adaptive step size proposed by the present invention were compared under the same environmental complexity. The map size is 1000*1000, and the obstacles are as Figures 6 - 9 shown in the pink part. The comparison results are as Figure 10 shown in (a)-(d) in which the algorithm converges to the optimal solution, and the average running time, path length, number of turning points, and success rate for 100 runs are visualized, and the values are shown in Table 1.

[0124] Table 1

[0125]

[0126]

[0127] In the ordinary, cluttered, dense, maze, and narrow environments, in terms of running time, the present invention is superior to the time performance of other benchmark algorithms in any environment; the path length and the number of turning points are crucial in robot navigation to optimize energy use and efficiency. Due to its balanced exploration and exploitation, the present invention shows performance comparable to the baseline algorithms in all environments, and the path length and the number of turning points generated by the present invention are slightly reduced, which is also helpful for improving the path quality; the success rates of the present invention and the benchmark algorithms in scenarios such as ordinary, cluttered, and maze are all 100%. In the cluttered environment, the planning success rate of the present invention reaches 96%, which is 15% higher than that of RRT*, 9% higher than that of Informed-RRT*, and 10% higher than that of RRT*-N; in the narrow environment, the planning success rate of the present invention is 19% higher than that of RRT*, 23% higher than that of Informed-RRT*, and 20% higher than that of RRT*-N. Therefore, compared with the benchmark algorithms, the present invention is more efficient in solving complex path problems; in the cluttered and narrow environments, due to the introduction of the adaptive step size mechanism in the present invention, the search step size can be dynamically adjusted according to the environmental complexity, improving the adaptability of the algorithm in narrow channels. The overall improved algorithm has greatly improved the planning efficiency, planning success rate, quality of the planned path, and environmental applicability.

[0128] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the present technical solution, and they should all be covered within the scope of the claims of the present invention.

Claims

1. An adaptive step-size Combine-RRT* path planning method, characterized in that: It includes the following steps: S1: Model and binarize according to the obstacle map, determine the starting point and the target point, and set initial parameters; S2: Initialize a random tree according to the obstacle map; S3: Determine the normal distribution sampling process and parameters; S4: Use the dynamic greedy factor to select the sampling strategy in the sampling process to generate sampling points; S5: Use the variable step size growth strategy to determine the dynamic expansion step size when the node expands, and generate new expanded nodes; S6: Perform collision detection and update the tree atlas, and output the initial path; S7: Determine the elliptical sampling range; S8: Use the dynamic greedy factor to select elliptical region sampling or normal distribution sampling to generate sampling points; S9: Re-search for the parent node and reconnect within the specified range of the new expanded node, and find the path with the minimum cost; S10: Introduce a B-spline curve to optimize the path at the back end.

2. The adaptive step-size Combine-RRT* path planning method according to claim 1, characterized in that: In step S1, the obstacle map includes the obstacle area O in the map obs and the free area O free , and binarize both to determine the starting point q start and the target point q target ; In step S2, the vertex set V and the edge set E in the random tree initialized according to the obstacle map are respectively added to the tree atlas G of the tree.

3. The adaptive-step Combine-RRT* path planning method according to claim 1, characterized in that: The determination of the normal distribution sampling process and parameters described in step S3 specifically includes the following steps: S31: Calculate the unit direction vector d from the starting point to the ending point i : S32: Multiply the longitudinal value of d i by a negative number to generate a perpendicular vector d by rotating the direction vector by 90 degrees I , construct a two-dimensional orthogonal basis, and the perpendicular vector is used for the lateral expansion direction of Gaussian sampling; S33: Generate basic sampling points P evenly distributed on the line connecting the starting point and the target point base , serving as the center points of the Gaussian distribution: P base = q start + ξ·L·d i , ξ ~ u(0,1) where \(L = ||\mathbf{q} target -\mathbf{q} start ||\) is the Euclidean distance between the starting point and the target point, and \(\xi\) is a uniformly distributed random number; S34: Use the Gaussian distribution on the basic sampling points, superimpose the normal distribution horizontal offset in the vertical direction, and finally generate sampling points: P final = P base + η·δ·d I , η ~ N(0,1) where δ is the standard deviation and η is the standard normal distribution random number.

4. The adaptive step-size Combine-RRT* path planning method according to claim 1, characterized in that: The use of the dynamic greedy factor to select the sampling strategy in the sampling process to generate sampling points described in step S4 specifically includes the following steps: S41: Record the current iteration number k and the total iteration number K at each step max , and calculate the dynamic greedy factor B: where a and b are coefficients; S42: Generate a random value between 0 and 1 and compare it with the greedy factor B. If it is less than the greedy factor B, then use normal distribution sampling; otherwise, select ordinary random sampling. Finally, generate the sampling point q rand ; The ordinary random sampling is to uniformly sample in the entire obstacle-free free area O free Uniform sampling.

5. The adaptive step-size Combine-RRT* path planning method according to claim 1, characterized in that: The use of the variable step size growth strategy to determine the dynamic expansion step size when the node expands and generate new expanded nodes described in step S5 specifically includes the following steps: S51: Known sampling node q rand and the nearest node q nearest , use the obstacle density to represent the influence of the surrounding environment on the sampling step size, and define the obstacle density function: where N is the number of obstacles, and d i (x) is the distance from a single obstacle to the nearest node q nearest , when the distance exceeds the set maximum distance D max , this obstacle distance is not included in the obstacle density function; S52: Given the set fixed sampling step R and the minimum step r min , the maximum step r max and the obstacle density near the nearest node q nearest . The greater the obstacle density, the shorter the sampling step; the smaller the obstacle density, the longer the sampling step. Then the dynamically extended step is calculated as follows: r d = r min + (r max - r min )·(1 - Density(k)) 2 Determine the sampling strategy through step S4, and determine the dynamic expansion step size r through step S5 d , thus completing the expansion of the new expansion node q new .

6. The adaptive step-size Combine-RRT* path planning method according to claim 1, characterized in that: The performance of collision detection and update of the tree atlas and output of the initial path described in step S6 specifically includes the following steps: S61: Perform collision detection on the new extended node, and check whether the extended node q new is within the obstacle space O obs ; if not, add it to the atlas G, and if it is within the obstacle space O obs , then discard this extended point; S62: Calculate the extended node q new to the target point q target distance, and determine whether it is close to the target point q target If it is close, output the initial path.

7. The adaptive step size Combine-RRT* path planning method according to claim 1, characterized in that: Determine the elliptical sampling range described in step S7. The major axis of the dynamic ellipse is the length c of the currently generated path best , the starting point q start and the target point q target are respectively the two foci of the ellipse, and the distance between them is set as c min , and the minor axis length of the ellipse Determine the elliptical sampling area from the two foci and the known major and minor axes.

8. The adaptive step size Combine-RRT* path planning method according to claim 1, wherein: The specific steps of step S8 are as follows: S81: After the initial path exists, the sampling strategy selection in step S4 becomes normal distribution sampling and elliptical sampling in the defined area; as the number of iterations increases, the probability of selecting the elliptical sampling strategy is greater, which speeds up the convergence; S82: After selecting a sampling strategy, sample to generate a sampling node q rand , using step S5, determine the dynamic expansion step size r by variable step size growth d , and finally generate a new expansion node q new .

9. The adaptive step-size Combine-RRT* path planning method according to claim 1, wherein: The specific steps of step S9 are as follows: S91: Re-select the parent node q of the newly expanded node q that has passed the obstacle detection in step S6 new within a specified range father ; specifically including the following steps: S911: Temporarily store the node q closest to the current sampling point nearest in the variable q with the minimum cost min ; S912: According to the current new extended node q new Specify a range C near , traverse all nodes within the specified range C near as the new extended node q new Re - find the parent node q father ; S913: Add the new extended node q new Perform collision detection on each node traversed, and point C that passes the collision detection near The cost plus the new extended node q new and q nearest 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 nearest Store the current new extension node q new The minimum cost parent node q min After the traversal is completed, the new expansion node q is found new In the specified range C near The minimum cost parent node q min , update the edge set E; S92: Reconnect all nodes within the current specified range C near ; S921: Within the specified range C near For all nodes other than the minimum-cost parent node q min regard the new expanded node q new as the new parent node q father and compare the cost with the previous one; S922: These traversed points and the new extended node q new If, through collision detection, the cost of the original path of the traversed point is greater than the cost of the new connection, then update the parent node q of the current traversed point father , and update the total graph set G.

10. The adaptive step size Combine-RRT* path planning method according to claim 1, characterized in that: In step S10, the path generated in step S9 is optimized using a cubic B-spline curve. In the total atlas G, control points are selected for cubic spline curve interpolation operations; then, interpolation is performed on the points in the set at fixed intervals to obtain a uniformly distributed discrete point set G*; the definition of the third-order B-spline curve is as follows: where B i,3 (u) is a basis function, and p i is a control point.

Images