A fast adaptive sampling optimization path planning method in complex environments

Abstract

The present invention provides a fast adaptive sampling optimization path planning method in a complex environment, belonging to the field of robot real application technology. 1) Establish a spatial sampling point set Qv, and add the starting point and the end point to Qv; 2) Spatial batch sampling, first perform uniform batch sampling in the map space, uniform batch sampling ensures that the sampling points are randomly and uniformly distributed in the map space, and generate s sampling points each time; 3) Traverse and calculate the distance r from each point to the nearest obstacle, take r as the radius, establish the neighborhood circle space of each point, if the map environment is a three-dimensional space, establish the neighborhood sphere space of each point, if the map environment is a high-dimensional space, establish a super neighborhood sphere space; 4) Traverse each neighborhood circle, and connect the sampling points in each circle to each other; 5) Starting from the starting point, traverse the adjacency matrix of each point in Qv to find out whether there is a path to the end point; 6) After finding the path, determine whether the nodes and edges on the path collide with obstacles. The present invention has the advantages of high path planning efficiency.

Description

Technical Field

[0001] The invention belongs to the technical field of practical application of robots and relates to a fast adaptive sampling optimization path planning method in a complex environment. Background Art

[0002] As one of the key technologies for the practical application of mobile robots, obstacle avoidance path planning is the focus and pain point of industry research. The main purpose of path planning is to find an optimal or suboptimal collision-free feasible path from the starting point to the target point in a bounded space with obstacles, while also meeting certain optimization indicators, such as path cost, planning time, continuity, etc.

[0003] In the process of global path planning, the mobile robot needs to find and select the best (or suboptimal) obstacle-free path from the starting point to the desired point based on a full understanding of the environmental map and certain evaluation indicators.

[0004] According to the chronological order of the development of path planning algorithms and the basic principles of the algorithms, they can be divided into graph-based search algorithms, sampling-based planning algorithms, intelligent bionics algorithms and other algorithms. Among them, graph-based search algorithms include Dijstra, A* and other algorithms, sampling-based planning algorithms include PRM, RRT and other algorithms, intelligent bionics-based path planning algorithms include ant colony, genetic, particle swarm, neural network, reinforcement learning and other algorithms, and other algorithms include simulated annealing, artificial potential field method and so on. The above methods have shortcomings such as poor robustness, low efficiency and poor stability when facing complex environments. There are also problems such as long search paths and difficulty in achieving local and global convergence balance.

[0005] For the sampling strategy path planning algorithm, there are two main factors that affect the efficiency of the algorithm: one is the environmental space sampling strategy, and the other is the neighborhood generation strategy. Neighborhood generation mainly includes obstacle collision detection and step distance calculation (within the size range of the constraints). Usually, a sufficient number of sampling points are required in the map environment to fully obtain the information of the global environmental space. The more sampling points, the higher the possibility of finding the optimal path. However, this will cause a large number of useless sampling points, and the path connection and collision detection time will increase sharply, causing the algorithm performance to seriously deteriorate. On the contrary, if the sampling density is controlled, it is difficult to fully obtain information about the multi-obstacle and narrow environment, which is likely to cause the path in this area to be lost and the optimal or suboptimal path cannot be found. Summary of the invention

[0006] The purpose of the present invention is to provide a fast adaptive sampling optimization path planning method in a complex environment in view of the above-mentioned problems existing in the prior art. The technical problem to be solved by the present invention is how to improve the execution efficiency of path planning.

[0007] The object of the present invention can be achieved by the following technical solutions: A fast adaptive sampling optimization path planning method in a complex environment, characterized in that it includes a path planner, and the path planner quickly finds a collision-free path from a starting point to an end point by improving a batch sampling algorithm, and the specific steps are as follows:

[0008] 11) Establish a spatial sampling point set Qv and add the start point and the end point to Qv;

[0009] 12) Spatial batch sampling: first, uniform batch sampling is performed in the map space. Uniform batch sampling ensures that the sampling points are randomly and uniformly distributed in the map space, and s sampling points are generated each time.

[0010] For any sampling point p in s, define the relationship function O(p) between point p and the obstacle position in the map space, where O(p) = 1 means that point p is within the obstacle, and O(p) = 0 means that point p is in the non-obstacle area;

[0011] For sampling points with O(p) = 0, directly add them to the sampling point set Qv;

[0012] For sampling points where O(p) = 1, find the sampling point q in Qv that is closest to point p, and calculate the Euclidean distance d between pq; generate s' sampling points on a circle with p as the center and d as the radius;

[0013] For any sampling point p' in s', if O(p') = 0, then p' is added to the sampling point set Qv, otherwise the point is discarded;

[0014] 13) Traverse and calculate the distance r from each point to the nearest obstacle, and use r as the radius to establish the neighborhood circle space of each point. If the map environment is a three-dimensional space, then establish the neighborhood sphere space of each point. If the map environment is a high-dimensional space, then establish the super neighborhood sphere space. The calculation formula of r is as follows:

[0015] r=||v start -v goal ||*ln(n) / n

[0016] Among them, ||v start -v goal || is the spatial Euclidean distance from the starting point to the end point, n = length(Qv), is the number of elements in the set Qv;

[0017] As the number of sampling points increases, r gradually decreases, which greatly reduces the complexity of the algorithm.

[0018] 14) Traverse each neighborhood circle and connect the sampling points in each circle;

[0019] 15) Starting from the starting point, traverse the adjacency matrix of each point in Qv to find out whether there is a path to the end point. If so, continue; if not, go to step 12);

[0020] 16) After finding the path, determine whether the nodes and edges on the path collide with obstacles. If so, delete the collision point and the edge on which it is located; continue to find whether there is a path to the end point. If not, go to step 15). If so, the path planner has completed its work and the algorithm continues to the path optimizer part.

[0021] Furthermore, s in step 12) can be generated according to specific environmental changes and can be 50.

[0022] Furthermore, s' in step 12) varies in the interval [5,30] according to the size of the following radius d, and the larger d is, the larger s' is.

[0023] Furthermore, the value of s' in step 12) is 15.

[0024] Furthermore, the planning method also includes a path optimizer. Based on the path planner, the implicit circular space area is sampled and the path is optimized to obtain a collision-free asymptotically optimal path. The specific steps are as follows:

[0025] 21) Establish a path node priority queue U based on the path distance from each point on the path to the starting point;

[0026] 22) Determine whether the algorithm converges or reaches the maximum number of steps. The maximum number of steps is calculated based on the length of U. The default value is length(U)*s. If yes, the algorithm ends, otherwise the algorithm continues.

[0027] 23) Determine whether the tree node priority queue is empty, if so, go to step 7), otherwise the algorithm continues;

[0028] 24) Take the element v with the minimum cost from the queue and delete it from the queue U; the minimum cost means the shortest distance to the starting point on the tree;

[0029] 25) With node v as the center and the distance to the nearest tree node as the radius, establish the implicit circular space of the node; if the map environment is a three-dimensional space, it is the implicit spherical space of the node; if the map environment is a high-dimensional space, it is the implicit hyperspherical space of the node;

[0030] 26) In the implicit circular space of v, sample the side of the path passing through v where the angle is less than 180 degrees, and replace the v point on the path with the sampled point. The operation algorithm is completed and goes to step 3);

[0031] 27) Based on the cumulative sum of the lengths of the path tree edges, determine whether the new path is better than the original path. If the length is greater than the original path, the algorithm goes to step 22), otherwise the new path is better and the algorithm continues;

[0032] 28) Update the path and the algorithm goes to step 21).

[0033] Regarding the sampling strategy in the environmental space, this method distinguishes and identifies spatially uniform batch sampling points, and proposes an adaptive neighborhood circular space resampling strategy for sampling points, which can greatly improve the effective sampling in multi-obstacle and multi-narrow environments; regarding the domain generation strategy, the present invention proposes sampling delayed collision detection and obstacle implicit circular space calculation, which can effectively utilize non-obstacle space, improve sampling density and expand the step distance of sampling points.

[0034] This method cleverly uses the concepts of primary planning and secondary optimization. The path planner improves the sampling strategies of ordinary RRT and related algorithms, distinguishes and identifies spatially uniform batch sampling points, and proposes an adaptive neighborhood circular space resampling strategy for sampling points, which increases the adaptability to multi-obstacle and multi-narrow areas and greatly improves the possibility of the algorithm finding a path quickly. On this basis, the path optimizer introduces the concept of implicit circular space of path tree nodes, quickly samples in the implicit circular space of tree nodes on the existing path, and iteratively optimizes the path.

[0035] This method can effectively improve the efficiency and success rate of path planning algorithms, especially in high-dimensional environment spaces. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a flow chart of the adaptive sampling optimization path planning method. DETAILED DESCRIPTION

[0037] The following are specific embodiments of the present invention and the accompanying drawings to further describe the technical solution of the present invention, but the present invention is not limited to these embodiments.

[0038] like Figure 1 As shown, this algorithm model is divided into two parts:

[0039] 1. The path planner quickly finds a collision-free path from the starting point to the end point by improving the batch sampling algorithm. The specific steps are as follows:

[0040] 1) Establish a spatial sampling point set Qv and add the start point and end point to Qv;

[0041] 2) Spatial batch sampling: First, uniform batch sampling is performed in the map space. Uniform batch sampling ensures that the sampling points are randomly and evenly distributed in the map space. Each time, s sampling points are generated, and s can be generated according to the specific environment. The default value is 50.

[0042] For any sampling point p in s, define the relationship function O(p) between point p and the obstacle position in the map space, with O(p) = 1 indicating that point p is inside the obstacle, and O(p) = 0 indicating that point p is outside the obstacle, i.e., in the non-obstacle area.

[0043] For sampling points with O(p) = 0, directly add them to the sampling point set Qv;

[0044] For sampling points with O(p) = 1, find the sampling point q closest to point p in Qv, calculate the Euclidean distance d between pq, and generate s' sampling points on the circle with p as the center and d as the radius. s' varies in the interval [5,30] according to the size of the following radius d. The larger d is, the larger s' is. The default value is 15.

[0045] For any sampling point p' in s', if O(p') = 0, then p' is added to the sampling point set Qv, otherwise the point is discarded.

[0046] 3) Traverse and calculate the distance r from each point to the nearest obstacle, and use r as the radius to establish the neighborhood circle space of each point. If the map environment is a three-dimensional space, then establish the neighborhood sphere space of each point. If the map environment is a high-dimensional space, then establish the super neighborhood sphere space. The calculation formula of r is as follows:

[0047] r=||v start -v goal ||*ln(n) / n

[0048] Among them, ||v start -v goal || is the spatial Euclidean distance from the starting point to the end point, n = length(Qv), is the number of elements in the set Qv.

[0049] Obviously, as the number of sampling points increases, r gradually decreases, which greatly reduces the complexity of the algorithm.

[0050] 4) Traverse each neighborhood circle and connect the sampling points in each circle;

[0051] 5) Starting from the starting point, traverse the adjacency matrix of each point in Qv to find out whether there is a path to the end point. If so, continue; if not, go to step 2);

[0052] 6) After finding the path, determine whether the nodes and edges on the path collide with obstacles. If so, delete the collision point and the edge on which it is located; continue to find whether there is a path to the end point. If not, go to step 5). If so, the path planner has completed its work and the algorithm continues to the path optimizer part.

[0053] 2. The path optimizer, based on the path planner, performs path optimization by focusing on sampling in the implicit circular space area to obtain a collision-free and asymptotically optimal path.

[0054] 1) Establish a path node priority queue U based on the path distance from each point on the path to the starting point;

[0055] 2) Determine whether the algorithm converges or reaches the maximum number of steps. The maximum number of steps is calculated based on the length of U. The default value is length(U)*s. If yes, the algorithm ends, otherwise the algorithm continues.

[0056] 3) Determine whether the tree node priority queue is empty, if so, go to step 7), otherwise the algorithm continues;

[0057] 4) Take out the element v with the minimum cost from the queue and delete it from the queue U; the minimum cost means the shortest distance on the tree to the starting point.

[0058] 5) With node v as the center and the distance to the nearest tree node as the radius, establish the implicit circular space of the node; if the map environment is a three-dimensional space, it is the implicit spherical space of the node; if the map environment is a high-dimensional space, it is the implicit hyperspherical space of the node.

[0059] 6) In the implicit circular space of v, sample the inner side of the path line passing through v (i.e. the side with an angle less than 180 degrees), and replace the v point on the path with the sampled point. The operation algorithm is completed and goes to step 3);

[0060] 7) Based on the cumulative sum of the lengths of the path tree edges, determine whether the new path is better than the original path. If the length is greater than the original path, the algorithm goes to step 2), otherwise the new path is better and the algorithm continues.

[0061] 8) Update the path and the algorithm goes to step 1).

[0062] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in similar ways, but they will not deviate from the spirit of the present invention or exceed the scope defined by the appended claims.

Claims

1. A fast adaptive sampling optimization path planning method in complex environments, It is characterized in that A path planner is included, which quickly finds a collision-free path from the starting point to the end point by improving the batch sampling algorithm. The specific steps are as follows: 11) Establish a spatial sampling point set Qv and add the start point and the end point to Qv; 12) Spatial batch sampling: first, uniform batch sampling is performed in the map space. Uniform batch sampling ensures that the sampling points are randomly and uniformly distributed in the map space, and s sampling points are generated each time. For any sampling point p in s, define the relationship function O(p) between point p and the obstacle position in the map space, where O(p) = 1 means that point p is within the obstacle, and O(p) = 0 means that point p is in the non-obstacle area; For sampling points with O(p) = 0, directly add them to the sampling point set Qv; For sampling points where O(p) = 1, find the sampling point q in Qv that is closest to point p, and calculate the Euclidean distance d between pq; Generate s' sampling points on a circle with p as the center and d as the radius; For any sampling point p' in s', if O(p') = 0, then p' is added to the sampling point set Qv, otherwise the point is discarded; 13) Traverse and calculate the distance r from each point to the nearest obstacle, and use r as the radius to establish the neighborhood circle space of each point. If the map environment is a three-dimensional space, then establish the neighborhood sphere space of each point. If the map environment is a high-dimensional space, then establish the super neighborhood sphere space. The calculation formula of r is as follows: r=v start -v goal *ln(n) / n Among them, v start -v goal is the spatial Euclidean distance from the starting point to the end point, n = length (Qv), is the number of elements in the set Qv; 14) Traverse each neighborhood circle and connect the sampling points in each circle; 15) Starting from the starting point, traverse the adjacency matrix of each point in Qv to find out whether there is a path to the end point. If so, continue; if not, go to step 12); 16) After finding the path, determine whether the nodes and edges on the path collide with obstacles. If so, delete the collision point and the edge on which it is located; continue to search whether there is a path to the end point. If not, go to step 15). If so, the path planner has completed its work.

2. According to claim 1, a fast adaptive sampling optimization path planning method in a complex environment, It is characterized in that The s in step 12) can be generated according to specific environmental changes and can be 50.

3. According to the method of fast adaptive sampling optimization path planning in complex environment as described in claim 1, It is characterized in that In step 12), s' varies in the interval [5,30] according to the size of the following radius d. The larger d is, the larger s' is.

4. According to claim 1, a fast adaptive sampling optimization path planning method in a complex environment, It is characterized in that The value of s' in step 12) is 15.

5. A fast adaptive sampling optimization path planning method in a complex environment according to any one of claims 1 to 4, It is characterized in that The planning method also includes a path optimizer. Based on the path planner, the implicit circular space area is sampled and the path optimization is performed to obtain a collision-free asymptotically optimal path. The specific steps are as follows: 21) Establish a path node priority queue U based on the path distance from each point on the path to the starting point; 22) Determine whether the algorithm converges or reaches the maximum number of steps. The maximum number of steps is calculated based on the length of U. The default value is length(U)*s. If yes, the algorithm ends, otherwise the algorithm continues. 23) Determine whether the tree node priority queue is empty, if so, go to step 7), otherwise the algorithm continues; 24) Take the element v with the minimum cost from the queue and delete it from the queue U; the minimum cost means the shortest distance to the starting point on the tree; 25) With node v as the center and the distance to the nearest tree node as the radius, establish the implicit circular space of the node; if the map environment is a three-dimensional space, it is the implicit spherical space of the node; if the map environment is a high-dimensional space, it is the implicit hyperspherical space of the node; 26) In the implicit circular space of v, sample the side of the path passing through v where the angle is less than 180 degrees, and replace the v point on the path with the sampling point. The operation algorithm is completed and goes to step 3); 27) Based on the cumulative sum of the lengths of the path tree edges, determine whether the new path is better than the original path. If the length is greater than the original path, the algorithm goes to step 22), otherwise the new path is better and the algorithm continues; 28) Update the path and the algorithm goes to step 21).

Images