A robot path planning method based on variable probability constraint sampling

Abstract

The present invention belongs to the field of robot path selection technology, specifically, it is a robot path planning method based on variable probability constraint sampling. First, by introducing the target bias mechanism into the sampling process, the time of forming the first path is reduced, so as to quickly generate an ellipsoid sampling set, which can reduce the number of search paths; adopt a variable step length strategy to reduce the probability that the robot is difficult to search for a path; backtrack the generated near-optimal path to weaken the relatively rigid path line. This ensures that the algorithm can effectively improve the search efficiency, reduce the search time, and ensure that the searched path is a near-optimal path.

Description

Technical Field

[0001] The invention belongs to the technical field of robot path selection, and in particular, is a robot path planning method based on variable probability constraint sampling. Background Art

[0002] In recent years, path planning has played an important role in the development of robotics technology. The main reason is that it enables the robot to effectively plan a feasible path connecting the starting point and the target point in the current working environment. This path not only needs to meet certain constraints but also needs to meet certain performance indicators.

[0003] At present, path planning methods in the field of robotics are mainly divided into the following categories: (1) Search-based path planning, such as A* and Dijkstra algorithms. These algorithms determine the next position by comparing the eight nearest neighbor heuristic values ​​of the current grid map. When there are multiple minimum nearest neighbor heuristic values, it cannot be guaranteed that the solution to the next position is optimal. At the same time, these algorithms are difficult to achieve satisfactory planning results in multi-dimensional environments. (2) Path planning based on intelligent algorithms, such as genetic algorithms and ant colony algorithms. Genetic algorithms are usually inefficient and have poor local search capabilities. Ant colony algorithms converge slowly, are prone to local optimal situations, and have poor environmental adaptability. (3) Sampling-based path planning, such as expanded space trees (ESTs), probabilistic roadmap methods (PRM), and rapidly-exploring random trees (RRT). These algorithms can avoid some low efficiency and low success rate problems when sampling in complex environments. The PRM algorithm has a faster calculation speed, but it cannot perform the search task well when the sampling points are sparse or unevenly distributed. Compared with the PRM algorithm, the RRT algorithm is more flexible, but the searched path is usually not optimal.

[0004] Through the above analysis and discussion, it is not difficult to find that the RRT algorithm also has certain shortcomings. In order to optimize the RRT algorithm, many researchers have proposed different improved algorithms. Kuffner et al. proposed the RRT Connect algorithm. Compared with the RRT algorithm, the RRTConnect algorithm uses a dual-tree search mechanism when searching for paths. One tree starts from the starting point and the other tree starts from the end point. The search stops when the two trees are connected. Therefore, this algorithm can improve the efficiency of searching paths and reduce the path search time.

[0005] However, the above algorithms still have certain limitations and cannot better optimize the search process of the search path, shorten the path length, and reduce the path search cost.

[0006] First, when generating new nodes, the algorithm constrains the biased sampling conditions of the target sampling points, so that the search time can be effectively reduced during the search process; secondly, a variable step size strategy is adopted to increase the probability of searching for a path; finally, the searched path is backtracked, rewired, and a more optimal route is found; finally, it is iterated repeatedly until a near-optimal path is found Summary of the invention

[0007] In order to solve the above problems, the present invention discloses a robot path planning method based on variable probability constrained sampling. Firstly, when generating new nodes, the algorithm constrains the biased sampling conditions of the target sampling points, so that the search time can be effectively reduced during the search process; secondly, a variable step size strategy is adopted to increase the probability of searching the path; finally, the searched path is backtracked and rewired to find a more optimal route; finally, it is iterated repeatedly until a near-optimal path is found.

[0008] The specific technical solutions of the present invention are as follows:

[0009] A robot path planning method based on variable probability constraint sampling includes the following steps:

[0010] Step 1: Generate map and obstacle information, given the robot's starting position Q s and the target position Q g ;

[0011] Specifically, the map size of the robot movement is set to a two-dimensional space of 20 units × 17 units, and the robot's starting position Q s , target position Q g , and finally generate obstacles.

[0012] Step 2: Q s , Q g Use them as the initial starting points of the two trees respectively, expand the random trees in opposite directions, and control the sampling direction according to the randomly generated probability p;

[0013] Specifically, a random probability p ranging from 0 to 100 and a variable value ε are set. If p is less than ε, the target point is used as the sampling point, otherwise elliptical constrained sampling is performed. If obstacles are determined to exist around, the value of ε is changed so that the sampling method is more inclined to elliptical constrained sampling.

[0014] Step 3: When p is greater than ε, perform elliptical constraint sampling;

[0015] Specifically, starting point Q s and the end point Qg are the two foci of the ellipse, and the conjugate diameter of the ellipse is d i , where d i is defined as:

[0016]

[0017] The eccentricity is calculated by cmin / cbest. Under the condition of bidirectional search tree, the robot starts from Q s To Q g The path cost can be considered to consist of two segments, namely, from Q s The cost to x, and from x to Q g The cost of using Euclidean distance is the distance c between the two foci. min .The transverse diameter of the ellipse is c best , which represents the currently recorded optimal path, so the ellipse sampling set can be defined as follows:

[0018]

[0019] Use Euclidean distance to get the distance c between the two foci min .The transverse diameter of the ellipse is c best , which represents the currently recorded optimal path. Elliptical constraint sampling transforms uniformly distributed samples from a unit n-dimensional hypersphere into an n-dimensional elliptical subset x ellipse -μ(X ellipse ), thus completing uniform distribution sampling in the ellipse subset, that is,

[0020] x ellipse =Lx ball +x center , (3)

[0021] x center =(x f1 +x f2 ) / twenty four)

[0022] x ball ={x∈X|||x||2≤1}, (5)

[0023] where x center is the center of the ellipse, x f1 , x f2 are the two foci of the ellipse, x ball are random sampling points;

[0024] The above transformation is then calculated by Cholesky decomposition of the elliptic matrix S, S∈Rn×n, with

[0025] LL T ≡S, (6)

[0026] (xx center ) T S(xx centet )=I, (7)

[0027] The eigenvalue of S is the square of the radius of the ellipse {r i 2}, r i The value of

[0028] r1=c best / 2, (8)

[0029]

[0030] The change of the sampling point subset can be obtained by the cross-section radius and the major axis, where diag{.} is a diagonal matrix, and the diagonal matrix and decomposition operation of the major axis are respectively

[0031]

[0032]

[0033] After obtaining the sampling set, the ellipse at this time needs to be transferred to the world coordinate system, which can be solved by the Kabsch algorithm, that is,

[0034] C=Udiag{1,...1,det(U)det(V)}V t (12)

[0035] The present invention directly uses the rotation matrix C(θ) to calculate, thereby obtaining and returning the sampling set.

[0036]

[0037]

[0038] Step 4: Select the nearest neighbor point Q in the tree nest , according to the nearest neighbor point Q nest And the position of the obstacle is determined to obtain the dynamic step length L, thereby generating new nodes, including:

[0039] (1) Determine the dynamic step length L:

[0040] For tree 1, we randomly search for a sampling point Q r1 Then, search for distance Q r1 The nearest point Q nest1 , then move to point Q with a step length of L r1 The expansion operation is

[0041]

[0042] For tree 2, at the new node Q new1 After generation, search to get Q new1 The point Q closest to tree 2 nest2 , then move to point Q with a step length of L new1 The expansion operation is

[0043]

[0044] The step length L in the operation expression is a dynamic step length. Compared with the static step length, this processing makes the robot more flexible when facing complex paths, reduces the probability of path search failure, and speeds up the convergence time of searching for near-optimal paths. The design is as follows:

[0045] When the distance D between the two trees tree When it is less than the preset value s1, use a small step size L s To expand; when D tree When it is greater than the preset value s1, it means that the distance between the two trees is far, and the step length is determined by the robot and the obstacle environment; if the distance D between the robot and the obstacle is obs When it is greater than the preset value s2, it is considered that there is no debris near the robot, and a large step length L is used. m On the contrary, when D obs When it is less than the preset value s2, the robot's surrounding environment is considered complex and a small step length Ls is used, which is expressed as

[0046]

[0047] (2) Generate a new node:

[0048] Q s , Q g are the starting point and end point of the path, Q r is a random sampling point in space, Q nest For Q r The nearest node, Q new For Q nest To Q r The function first randomly searches for a sampling point Q in the map. r , then find the distance sampling point Q r The nearest point Q nest , and the point is a node that already exists in the tree, determine the nearest neighbor point Q nest To sampling point Q r Is the distance less than the step size set by the algorithm? If so, Q r Set as new node Q new If the step size is exceeded, the nearest neighbor point Q nest The path is based on the sampling point Qr The point obtained after extending one step is set as the new node Q new . At this time, there is a neighboring node Q near , potential parent node Q potential-parent And the true parent node Q parent . With the new node Q new Draw a circle with a certain radius as the center, and all the points in the circle form a new set. Then traverse the formed set and find all the neighboring nodes Q near And use them one by one as potential parent nodes Q potential-parent With the new node Q new Connect and calculate the starting point Q at the same time s , potential parent node, the path length of the new node and compare it with the original path length to screen out the best potential parent node Q potential-parent , and set it as the new parent node Q parent Finally, determine the new node Q new and the nearest neighbor Q nest Whether the connection line between them collides with an obstacle, if it is judged as no, then the new path is added to the tree and the original connection path is deleted.

[0049] Step 5: Determine the nearest neighbor point Q nest With the new node Q new Are there any obstacles in the path between them?

[0050] The nearest neighbor point Q nest With the new node Q new Set them as the two end points of the road section to be detected, T is the center of the obstacle, and o is the vertical point from the obstacle to the road section to be detected, then we have

[0051]

[0052] Based on the model size of the robot, it can be determined whether the robot has any collision during its movement.

[0053] Step 6: After the random tree generates a new node, determine whether the latest node of the two trees is less than the threshold. If the condition is met, the path has been found and backtracking is performed;

[0054] Specifically, after searching for a path through the dual-tree search path strategy, it is determined whether the two trees meet the connection conditions. At this time, it is defective to only determine whether the distance between the new node and the other tree is less than the set constant value, which will cause the entire path to have too many broken lines and too large corners at the connection. Therefore, in order to search for a near-optimal path, it is necessary to backtrack the generated optimal path, traverse the nodes of the entire optimal path, and start from the starting point to find the nodes on the path that can guarantee the greatest degree of non-collision with obstacles, connect the starting point to it and record this point; then start from this point, find the collision-free point after it until it can be connected to the end point without collision. Save these points and path information, search for a new optimal path by comparing the path information, iterate repeatedly, and finally select the path with the shortest total distance as the final search path of the algorithm.

[0055] The beneficial effects of the present invention are as follows: firstly, by introducing the target bias mechanism into the sampling process, the speed of forming the first path is accelerated, thereby quickly generating an ellipsoid sampling set, which can reduce the number of search paths and is conducive to the generation of the optimal path; a variable step size strategy is adopted, so that the robot is more flexible when facing complex paths, and the probability that the robot has difficulty in searching for a path is reduced; the generated near-optimal path is backtracked to optimize the problems of too many broken lines in the path and too large corners at the connection, and a better and more optimal path is searched by comparing the path information, so as to determine the final path, thereby ensuring that the algorithm can effectively improve the search efficiency and reduce the search time, the running time of the robot on the path is also shortened, and it is ensured that the searched path is a near-optimal path. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 The figure is a flow chart of a path planning method according to an embodiment of the present invention.

[0057] Figure 2 Schematic diagram of a simulation model of a simple environment according to an embodiment of the present invention.

[0058] Figure 3 Schematic diagram of the ellipse subset sampling concept of an embodiment of the present invention.

[0059] Figure 4 Schematic diagram of the random number growth process according to an embodiment of the present invention.

[0060] Figure 5 Schematic diagram of a new parent node generation process according to an embodiment of the present invention.

[0061] Figure 6 Schematic diagram of simulation result comparison of scenario 1 according to an embodiment of the present invention.

[0062] Figure 7 Schematic diagram of simulation result comparison of scenario 2 according to an embodiment of the present invention. DETAILED DESCRIPTION

[0063] In order to deepen the understanding of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. The embodiments are only used to explain the present invention and do not limit the protection scope of the present invention.

[0064] Example: Figure 1-Figure 7 As shown, a robot path planning method based on variable probability constraint sampling includes the following steps:

[0065] Step 1: Generate map and obstacle information, given the robot's starting position Q s and the target position Q g ;

[0066] Specifically, the map size of the robot movement is set to a two-dimensional space of 20 units × 17 units, and the robot's starting position Q s , target position Q g , and finally generate obstacles; Figure 2 As shown in the figure, the shaded part is the obstacle and the robot cannot pass through.

[0067] Step 2: Q s , Q g Use them as the initial starting points of the two trees respectively, expand the random trees in opposite directions, and control the sampling direction according to the randomly generated probability p;

[0068] Specifically, a random probability p ranging from 0 to 100 and a variable value ε are set. If p is less than ε, the target point is used as the sampling point, otherwise elliptical constrained sampling is performed. If obstacles are determined to exist around, the value of ε is changed so that the sampling method is more inclined to elliptical constrained sampling.

[0069] Step 3: When p is greater than ε, perform elliptical constraint sampling;

[0070] Specifically, ellipse constraint sampling is as follows Figure 3 As shown, starting point Q s and the end point Q g are the two foci of the ellipse, and the conjugate diameter of the ellipse is d i , where d i is defined as:

[0071]

[0072] The eccentricity is calculated by cmin / cbest. Under the condition of bidirectional search tree, the robot starts from Q s To Q g The path cost can be considered to consist of two segments, namely, from Q s The cost to x, and from x to Q g The cost of using Euclidean distance is the distance c between the two foci.min .The transverse diameter of the ellipse is c best , which represents the currently recorded optimal path, so the ellipse sampling set can be defined as follows:

[0073]

[0074] Use Euclidean distance to get the distance c between the two foci min .The transverse diameter of the ellipse is c best , which represents the currently recorded optimal path. Elliptical constraint sampling transforms uniformly distributed samples from a unit n-dimensional hypersphere into an n-dimensional elliptical subset x ellipse -μ(X ellipse ), thus completing uniform distribution sampling in the ellipse subset, that is,

[0075] x ellipse =Lx ball +x center , (3)

[0076] x center =(x f1 +x f2 ) / twenty four)

[0077] x ball ={x∈X|||x||2≤1}, (5)

[0078] where x center is the center of the ellipse, x f1 , x f2 are the two foci of the ellipse, x ball are random sampling points;

[0079] The above transformation is then calculated by Cholesky decomposition of the elliptic matrix S, S∈Rn×n, with

[0080] LL T ≡S, (6)

[0081] (xx center ) T S(xx centet )=I, (7)

[0082] The eigenvalue of S is the square of the radius of the ellipse {r i 2}, r i The value of

[0083] r1=c best / 2, (8)

[0084]

[0085] The change of the sampling point subset can be obtained by the cross-section radius and the major axis, where diag{.} is a diagonal matrix, and the diagonal matrix and decomposition operation of the major axis are respectively

[0086]

[0087]

[0088] After obtaining the sampling set, the ellipse at this time needs to be transferred to the world coordinate system, which can be solved by the Kabsch algorithm, that is,

[0089] C=Udiag{1,...1,det(U)det(V)}V t (12)

[0090] The present invention directly uses the rotation matrix C(θ) to calculate, thereby obtaining and returning the sampling set.

[0091]

[0092]

[0093] Step 4: Select the nearest neighbor point Q in the tree nest , according to the nearest neighbor point Q nest And the position of the obstacle is determined to obtain the dynamic step length L, thereby generating new nodes, including:

[0094] (1) Determine the dynamic step length L:

[0095] For tree 1, we randomly search for a sampling point Q r1 Then, search for distance Q r1 The nearest point Q nest1 , then move to point Q with a step length of L r1 The expansion operation is

[0096]

[0097] For tree 2, at the new node Q new1 After generation, search to get Q new1 The point Q closest to tree 2 nest2 , then move to point Q with a step length of L new1 The expansion operation is

[0098]

[0099] The step length L in the operation expression is a dynamic step length. Compared with the static step length, this processing makes the robot more flexible when facing complex paths, reduces the probability of path search failure, and speeds up the convergence time of searching for near-optimal paths. The design is as follows:

[0100] When the distance D between the two trees tree When it is less than the preset value s1, use a small step size L s To expand; when D tree When it is greater than the preset value s1, it means that the distance between the two trees is far, and the step length is determined by the robot and the obstacle environment; if the distance D between the robot and the obstacle is obs When it is greater than the preset value s2, it is considered that there is no debris near the robot, and a large step length L is used. m On the contrary, when D obs When it is less than the preset value s2, the robot's surrounding environment is considered complex and a small step length Ls is used, which is expressed as

[0101]

[0102] (2) Generate a new node:

[0103] Extend* functions such as Figure 4 , 5 As shown, Q s , Q g are the starting point and end point of the path, Q r is a random sampling point in space, Q nest For Q r The nearest node, Q new For Q nest To Q r The function first randomly searches for a sampling point Q in the map. r , then find the distance sampling point Q r The nearest point Q nest , and the point is a node that already exists in the tree, determine the nearest neighbor point Q nest To sampling point Q r Is the distance less than the step size set by the algorithm? If so, Q r Set as new node Q new If the step size is exceeded, the nearest neighbor point Q nest The path is based on the sampling point Q r The point obtained after extending one step is set as the new node Q new . At this time, there is a neighboring node Q near , potential parent node Q potential-parent And the true parent node Q parent . With the new node Q new Draw a circle with a certain radius as the center, and all the points in the circle form a new set. Then traverse the formed set and find all the neighboring nodes Q near And use them one by one as potential parent nodes Q potential-parent With the new node Q new Connect and calculate the starting point Q at the same times , potential parent node, the path length of the new node and compare it with the original path length to screen out the best potential parent node Q potential-parent , and set it as the new parent node Q parent Finally, determine the new node Q new and the nearest neighbor Q nest Whether the connection line between them collides with an obstacle, if it is judged as no, then the new path is added to the tree and the original connection path is deleted.

[0104] Step 5: Determine the nearest neighbor point Q nest With the new node Q new Are there any obstacles in the path between them?

[0105] The nearest neighbor point Q nest With the new node Q new Set them as the two end points of the road section to be detected, T is the center of the obstacle, and o is the vertical point from the obstacle to the road section to be detected, then we have

[0106]

[0107] Based on the model size of the robot, it can be determined whether the robot has any collision during its movement.

[0108] Step 6: After the random tree generates a new node, determine whether the latest node of the two trees is less than the threshold. If the condition is met, the path has been found and backtracking is performed;

[0109] Specifically, after searching for a path through the dual-tree search path strategy, it is determined whether the two trees meet the connection conditions. At this time, it is defective to only determine whether the distance between the new node and the other tree is less than the set constant value, which will cause the entire path to have too many broken lines and too large corners at the connection. Therefore, in order to search for a near-optimal path, it is necessary to backtrack the generated optimal path, traverse the nodes of the entire optimal path, and start from the starting point to find the nodes on the path that can guarantee the greatest degree of non-collision with obstacles, connect the starting point to it and record this point; then start from this point, find the collision-free point after it until it can be connected to the end point without collision. Save these points and path information, search for a new optimal path by comparing the path information, iterate repeatedly, and finally select the path with the shortest total distance as the final search path of the algorithm.

[0110] In order to verify the performance advantages of the improved algorithm proposed in this paper, the simulation results of the experimental scenario 1 are as follows Figure 6As shown in the figure, there are multiple paths from the starting point to the end point in the map of scenario 1. From the figure, we can see that the improved algorithm has a smoother search path and simpler lines than Informed RRT* and Informed RRT*-Connect. The main reason is that the improved algorithm adopts a backtracking processing mechanism to reduce the search cost of the path. From Table 1, it is not difficult to see that the average first search time for the near-optimal path of the improved algorithm is about 70% of that of the Informed RRT* algorithm, while the comparison of the InformedRRT*-Connect algorithm is about 80%. It can be seen that the use of dual trees for searching can significantly shorten the time required for path convergence, but the final near-optimal paths searched by the three algorithms are relatively similar. In the following table, Apl represents the average path length, Afg represents the average first generation time of the near-optimal path, and Sn represents the shortest near-optimal path.

[0111] Table 1 Comparison of experimental results of three different algorithms in scenario 1

[0112]

[0113] Compared with scenario 1, the map design of scenario 2 is more complex, the robot can consider fewer paths to choose, and the obstacles are more interference. The purpose is to test the adaptability of the improved algorithm in complex environments. The experimental simulation results in this scenario are as follows: Figure 7 As shown in the figure, the performance of the improved algorithm in scenario 1 is also shown in scenario 2, which proves that the improved algorithm has high adaptability. Compared with scenario 1, the average time consumption of the near-optimal path first searched in scenario 2 is less, and the time consumption of the improved algorithm is less than 50% of the time consumption of the Informed RRT* algorithm, and the difference in time consumption with the InformedRRT*-Connect algorithm is more obvious, which shows that the target bias mechanism is particularly critical in the sampling process in complex environments, and the unimproved algorithm is prone to increase the search time consumption when searching for new nodes due to the large sampling area.

[0114] Table 2 Comparison of experimental results of three different algorithms in scenario 2

[0115]

[0116] As an implementation method, the present invention introduces the idea of ​​target bias, constrains the sampling points, reduces the sampling area, and distributes the sampling points around the path that can reach the target point, thereby effectively improving the efficiency of the search path and reducing the cost of the search path. At the same time, it solves the problem that the node expansion direction is unclear in a complex environment, resulting in poor search results. The improved algorithm also sets a probability bias to prevent the search results from being local optimal. A variable step size strategy is used to increase the probability of searching for a path. Finally, the searched path is backtracked to reduce the number of turning points in the path, avoid irregular situations at the connection between the two tree searches, and further reduce the total path length, thereby reducing the running time of the robot.

[0117] The above description is an exemplary embodiment of the present invention, and does not limit the scope of patent protection of the present invention. Any equivalent structure or equivalent process transformation made by using the contents of the specification and drawings of the present invention, or directly or indirectly applied in other related technical fields, are also included in the scope of patent protection of the present invention.

Claims

1. A robot path planning method based on variable probability constraint sampling, characterized in that: The following steps are involved: Step 1: Generate a map and obstacle information, given the robot’s starting position Q s and the target position Q g ; Step 2: Q s , Q g Use them as the initial starting points of the two trees respectively, expand the random trees in opposite directions, and control the sampling direction according to the randomly generated probability p; Step 3: When p is greater than the set variable value ε, the sampling nodes are transformed from a unit n-dimensional hypersphere to an n-dimensional elliptical subset x ellipse -μ(X ellipse ), thus completing the sampling within the ellipse subset; Step 4: Select the nearest neighbor point Q in the tree nest , according to the nearest neighbor point Q nest And the position of the obstacle is determined to obtain the dynamic step length L, thereby generating a new node; Step 5: Determine whether there are any obstacles on the path between the nearest neighbor point Qnest and the new node Qnew; Step 6: After the random tree generates a new node, determine whether the latest node of the two trees is less than the threshold. If the condition is met, the path has been found and backtracking is performed; In step 2, a random probability p and a variable value ε in the range of 0-100 are set. If p is less than ε, the target point is used as the sampling point, otherwise elliptical constraint sampling is performed. If it is determined that there are obstacles around, the value of ε is changed so that the sampling method is more inclined to elliptical constraint sampling. In step 3, the starting point Q s and the end point Q g are the two foci of the ellipse, and the conjugate diameter of the ellipse is d i , where d i is defined as: The eccentricity is given by c min / c best It is calculated that under the condition of bidirectional search tree, the robot starts from Q s To Q g The path cost is considered to consist of two segments, namely, from Q s The cost to x, and from x to Q g The cost of using Euclidean distance is the distance c between the two foci. min. The transverse diameter of the ellipse is c best , which represents the currently recorded optimal path, so the ellipse sampling set is defined as follows: Use Euclidean distance to get the distance c between the two foci min , the transverse diameter of the ellipse is c best , which represents the currently recorded optimal path, elliptical constraint sampling transforms uniformly distributed samples from a unit n-dimensional hypersphere into an n-dimensional elliptical subset x ellipse -μ(X ellipse ), thus completing uniform distribution sampling in the ellipse subset, that is, x ellipse =Lx ball +x center , x center =(x f1 +x f2 ) / 2, x ball ={x∈X|‖x‖2≤1}, where x center is the center of the ellipse, x f1 , x f2 are the two foci of the ellipse, x ball are random sampling points; The transformation is then calculated by Cholesky decomposition of the elliptic matrix S, S∈Rn×n, with LL T =S (x-x center ) T S(x-x centet )=I, The eigenvalue of S is the square of the radius of the ellipse {r i 2 }, r i The value of r1＝c best / 2， The change of the sampling point subset can be obtained by the cross-section radius and the major axis, where diag{.} is a diagonal matrix, and the diagonal matrix and decomposition operation of the major axis are respectively After obtaining the sampling set, the rotation matrix C(θ) is used to calculate, so as to obtain the sampling set and return it: In step 4, the nearest neighbor point Q is selected in the tree nest , according to the nearest neighbor point Q nest And the position of the obstacle is determined to obtain the dynamic step length L, so as to generate a new node, which includes two processes: determining the dynamic step length L and generating a new node; In step 4, in the process of determining the dynamic step length L: For tree 1, we randomly search for a sampling point Q r1 Then, search for distance Q r1 The nearest point Q nest1 , then take a step length L to point Q r1 The expansion operation is For tree 2, at the new node Q new1 After generation, search to get Q new1 The point Q closest to tree 2 nest2 , then move to point Q with a step length of L new1 The expansion operation is The step size L in the operation expression is a dynamic step size, which is designed as follows: When the distance D between the two trees tree When it is less than the preset value s1, use a small step size L s To expand, when D tree When it is greater than the preset value s1, it means that the distance between the two trees is far. The step length is determined by the robot and the obstacle environment. If the distance D between the robot and the obstacle is obs When it is greater than the preset value s2, it is considered that there is no debris near the robot, and a large step length L is used. m On the contrary, when D obs When it is less than the preset value s2, the robot's surrounding environment is considered complex and a small step length Ls is used, which is expressed as In step 4, the specific process of generating a new node is as follows: Set Q s , Q g are the starting point and end point of the path, Q r is a random sampling point in space, Q nest For Q r The nearest node, Q new For Q nest To Q r The function first randomly searches for a sampling point Q in the map. r , then find the distance sampling point Q r The nearest point Q nest , and the point is a node that already exists in the tree, determine the nearest neighbor point Q nest To sampling point Q r Is the distance less than the step size set by the algorithm? If so, Q r Set as new node Q new If the step size is exceeded, the nearest neighbor point Q nest The path is based on the sampling point Q r The point obtained after extending one step is set as the new node Q new , at this time there is a neighboring node Q near , potential parent node Q potential-parent And the true parent node Q parent , with the new node Q new Draw a circle with a certain radius as the center, and all the points in the circle form a new set. Then traverse the set to find all the neighboring nodes Q near And use them one by one as potential parent nodes Q potential-parent With the new node Q new Connect and calculate the starting point Q at the same time s , potential parent node, the path length of the new node and compare it with the original path length to screen out the best potential parent node Q potential-parent , and set it as the new parent node Q parent , and finally determine the new node Q new and the nearest neighbor Q nest Whether the connection line between them collides with an obstacle. If it is judged as no, the new path is added to the tree and the original connection path is deleted; In step 5, the neighbor point Q nest With the new node Q new Set them as the two end points of the road section to be detected, T as the center of the obstacle, and o as the vertical point from the obstacle to the road section to be detected, then: Based on the model size of the robot, it can be determined whether the robot has any collision during its movement.

2. The robot path planning method based on variable probability constraint sampling according to claim 1 is characterized in that: The specific process of step 6 is as follows: in order to search for a near-optimal path, it is necessary to backtrack the generated optimal path, traverse the nodes of the entire optimal path, and start from the starting point to sequentially search for nodes on the path that can maximize the guarantee of not colliding with obstacles, connect the starting point to it and record this point, and then start from this point to find the collision-free point after it until it is connected to the end point without collision, save these points and path information, search for a new optimal path by comparing the path information, iterate repeatedly, and finally select the path with the shortest total distance as the final search path of the algorithm.

Images