Path planning method based on obtuse triangle local area fixed step length sampling RRT algorithm

Abstract

The invention relates to a path planning method based on an obtuse triangle local area fixed step length sampling RRT algorithm. The path planning method comprises the following steps: 1, taking a starting point of path planning as a root node of a random tree; 2, generating random points; 3, judging whether a random point is within the range of the search area by an obtuse triangle method, if so, turning to the step 4, otherwise, returning to the step 2; 4, finding out a node closest to the random point as an adjacent point; 5, expanding from the adjacent nodes to the random point direction at a fixed step length to generate new nodes; 6, judging whether an obstacle exists between the new node and the adjacent point, if so, returning to the step 2, and otherwise, adding the new node into the random tree; 7, judging whether the new node is in the target point area, if so, adding the target point into the random tree, and backtracking from the target point to the root node to obtain the path point of the planned path, otherwise, returning to the step 2; and 8, connecting the nodes in the random tree T to form a planned path. According to the path planning algorithm based on the obtuse triangle local area fixed step length sampling RRT algorithm, the calculated amount of new node search can be reduced, the length of the planned path and the number of the nodes can be reduced, and the path planning efficiency is improved.

Description

TECHNICAL FIELD

[0001] The application relates to the field of path planning, in particular to a path planning method based on an obtuse triangle local region fixed step sampling RRT algorithm. BACKGROUND

[0002] In the presence of obstacles, how a robot efficiently plans a path from the starting position to the target position is a hot issue that researchers have been studying. As the core problem of the technology, the main task of path planning is to find a feasible path from the initial state position to the target state position in space, and to reduce the computational amount of the path planning new node search, and to reduce the requirements of the path length and the number of nodes.

[0003] According to the degree of understanding of the environmental information, the path planning can be divided into global path planning and local path planning. The global path planning needs to collect map information for path planning, and the traditional algorithms include A* algorithm, Dijkstra algorithm, genetic algorithm (GA), rapidly-exploring random trees (RRT) algorithm and the like. The precision of mathematical modeling of complex space and obstacles directly affects the search efficiency of this kind of algorithm. Based on intelligent algorithms, there are genetic algorithm, particle swarm algorithm, ant colony algorithm and the like. Although this kind of algorithm has very strong learning ability, it has poor real-time performance, large amount of calculation and needs to occupy a large amount of storage space. Due to the simple structure, probability completeness, strong search expansion capability and the like of the RRT algorithm, the RRT algorithm is widely used in robot path planning. However, the existing RRT algorithm has the shortcomings of low search efficiency and unsatisfactory path planning length.

[0004] Wang Kun et al. [1] On the basis of the basic RRT algorithm, the RRT-Connect algorithm for improving the bidirectional random search is proposed, the path planning efficiency is improved, and the calculation amount and memory consumption are also increased. Zhang Weibo et al. [2] The generation efficiency and length of the shortest path planning of the RRT algorithm based on concentric circle sampling guidance are still not ideal. Tan Bo et al. [3] An improved RRT algorithm is proposed, which can effectively solve the global path optimization problem, but the path node pruning amount of the complex map is large, resulting in large computer memory consumption, and the path planning length is still not ideal. He Zhaochu et al. [4] The artificial potential field method and the RRT algorithm are combined to avoid falling into local minimum value, the calculation is relatively complex, and the global path is not optimal. REFERENCES [1] Wang Kun, Huang Bo, Zeng Guohui, et al. Fast path planning algorithm based on improved RRT-Connect [J]. Journal of Wuhan University (Natural Science Edition), 2019, 65(03): 283-289. [2] Zhang, W. B., Xiao, J. L., & Liu, P. A path planning method based on concentric circle sampling guided RRT algorithm [P]. Fujian Province: CN111141304B, 2021-11-02. [3] Tan, B., Luo, J., Luo, Y. S., et al. Robot path planning based on improved RRT algorithm [J]. Journal of Chongqing University, 2023, 46(09): 13-22. [4] He, Z. C., He, Y. L., & Zeng, B. RRT and artificial potential field method combined with mechanical arm obstacle avoidance planning [J]. Industrial Engineering, 2017, 20(02): 56-63. SUMMARY

[0005] The purpose of the present application is to provide a path planning method based on obtuse triangle local area fixed step sampling RRT algorithm, which can reduce the calculation amount of new node search, reduce the length of planned path and the number of nodes, and improve the efficiency of path planning.

[0006] In order to achieve the above purpose, the technical scheme adopted by the present application is: a path planning method based on obtuse triangle local area fixed step sampling RRT algorithm, characterized by providing the starting point, target point, fixed step, obstacle position and planning map of path planning, and performing path planning according to the following steps:

[0007] Step 1: define the extended fixed step r, take the starting point X init of path planning as the root node of the random tree T, and take it as the parent node of the next expansion random tree;

[0008] Step 2: generate a random point X rand ;

[0009] Step 3: traverse each node in the random tree T, find the nearest node to the random point as the adjacent point X nearest by calculating the distance between the nodes in the random tree T and the random point;

[0010] Step 4: expand the fixed step r from the adjacent point X nearest to the random point X rand direction, get the new node X new ;

[0011] Step 5: determine whether the new node is on the circular arc L1 by the obtuse triangle method, if not on the circular arc L1, return to step 2 to generate a random point, if on the circular arc L1, go to step 2;

[0012] Step 6: determine whether there is an obstacle between the new node X new and the adjacent point X nearest , if there is no obstacle, take the new node Xnew If yes, go to step 7, otherwise, return to step 2 to resample;

[0013] Step 7: judge whether the new node X new is in the target point X goal region, if yes, add the target point X goal to the random tree T, backtrack from the target point X goal to the root node X init , and get the path point of the planning path, if not, return to step 2 to resample;

[0014] Step 8: connect the nodes in the random tree T to form the planning path.

[0015] Further, in the step 2, when the map is rectangular, the random point is obtained by the following method:

[0016]

[0017] In the above formula, x rand , y rand are the horizontal and vertical coordinates of the random point X rand , xL and yL are the length and width of the map, and rand ∈ (0, 1).

[0018] Further, in the step 3, the distance between all nodes in the random tree T and the random point X rand is traversed, and after comparison, the node with the minimum distance is obtained, which is the adjacent point X nearest .

[0019] Further, in the step 4, the method for obtaining the new node X new is as follows:

[0020] The distance dist between the adjacent point X nearest and the random point X rand is calculated:

[0021]

[0022] In the above formula, x nearest , y nearest are the horizontal and vertical coordinate values of the adjacent point X nearest , and x rand , y rand are the horizontal and vertical coordinate values of the adjacent point X rand .

[0023] The new node X new can be calculated by the following formula:

[0024]

[0025] In the above formula, r is the fixed step size of the expansion.

[0026] Furthermore, in step 5, the new node X is determined by the obtuse triangle method. new The method for arc L1 is as follows:

[0027] With target point X goal and the nearest node X nearest Draw a circle S1 with the line connecting the two points as the diameter d, and take the nearest node X as the boundary. nearest Draw a circle S2 with center at point A and fixed expansion step size r as radius. The two circles intersect at points A and E, passing through the nearest root node X. nearest X-rays were drawn separately. nearest F and X nearest K, draw the line CX connecting the endpoints of the two circles. goal The perpendicular line GH intersects circle S2 at points B and D, dividing circle S2 into arcs L1, L2, and L3. Arc L1 corresponds to FX. nearest The search region I formed to the right of K corresponds to arc L2, which is FX. nearest G upper side and HX nearest The search region II formed below K, with arc L3 corresponding to GX. nearest Search region Ⅲ is formed to the left of H.

[0028] New node X new The following formula is used to determine the arc L1:

[0029] -1≤cosα≤0

[0030] In the above formula, α=∠X nearest X new X goal .

[0031] Furthermore, in step 7, the target point X goal The area refers to the area centered on the target point X. goal Given a circular region centered at Thr with radius , determine the new node X. new At target point X goal The method for defining regions is as follows:

[0032] |X new -X goal |<Thr

[0033] In the above formula, X new X goal The vector represents the new node and the target point, and the || symbol represents the 2-norm.

[0034] Furthermore, in step 8, the nodes in the random tree T need to be arranged according to the target point X. goal To the starting point Xinit The connection is sequentially carried out, and a planning path is formed.

[0035] Compared with the prior art, the present application has the following beneficial effects: a path planning method based on obtuse triangle local area fixed step length sampling RRT algorithm is provided, the method introduces an obtuse triangle method to obtain random points, reduces the blindness of random point acquisition, can reduce the calculation amount of path planning new node search, can reduce the planning path length and the number of nodes, improves the path planning efficiency, has strong practicability and broad application prospect. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 is a method implementation flowchart of the embodiment of the present application.

[0037] Figure 2 is a node generation schematic diagram in the embodiment of the present application.

[0038] Figure 3 is an extended fixed step length principle schematic diagram of the method of the present application in the embodiment of the present application.

[0039] Figure 4 is an obtuse triangle method principle schematic diagram of the method of the present application in the embodiment of the present application.

[0040] Figure 5 is a path planning diagram of the RRT algorithm in the embodiment of the present application.

[0041] Figure 6 is a path planning diagram of the method of the present application in the embodiment of the present application. DETAILED DESCRIPTION

[0042] The present application will be further described in detail below in combination with the drawings and specific embodiments.

[0043] In the embodiment, the range of the planning map is 1000*1000, the starting point coordinates of the path planning are (100, -100), the target point coordinates are (900, -900), and the obstacle positions are as shown in the following table. Figure 6

[0044] The present application provides a path planning method based on an obtuse triangle local area fixed step length sampling RRT algorithm, as shown in the following table, based on the starting point, the target point, the fixed step length, the obstacle position and the planning map of the above planning path, the path planning is carried out according to the following steps. Figure 1

[0045] Step 1: define the extended fixed step length r, take the starting point X init of the path planning as the root node of the random tree T, and take it as the parent node of the next extension of the random tree. ​​

[0046] Step 2: As shown in Figure 2 , generate a random point X rand .

[0047] Where, when the map is rectangular, the random point is obtained by the following method:

[0048]

[0049] In the above formula, x rand , y rand are the horizontal and vertical coordinates of the random point X rand , xL and yL are the length and width of the map, and rand ∈ (0, 1).

[0050] Step 3: Traverse each node in the random tree T, find the nearest node to the random point as the nearby point X nearest by calculating the distance between the nodes in the random tree T and the random point, where the distance between the nodes in the random tree T and the random point is calculated using the Euclidean distance formula.

[0051] Step 4: As shown in Figure 2 , expand the fixed step length r from the nearby point X nearest to the random point X rand direction to get the new node X new .

[0052] Where, the distance dist between the nearby point X nearest and the random point X rand is calculated:

[0053]

[0054] Where, x nearest , y nearest are the horizontal and vertical coordinate values of the nearby point X nearest , x rand , y rand are the horizontal and vertical coordinate values of the nearby point X rand .

[0055] The new node X new can be calculated by the following formula:

[0056]

[0057] Where, x new , y new are the horizontal and vertical coordinates of the new node X new , and r is the fixed step length of expansion.

[0058] Step 5: Determine whether the new node is on the arc L1 by the obtuse triangle method. If not, return to Step 2 to generate a new random point. If yes, go to Step 2.

[0059] As shown in Figure 3 , a circle S1 is drawn with the line connecting the target point X goal and the nearest node X nearest as the diameter d, and a circle S2 is drawn with the nearest node X nearest as the center and a fixed extension step r as the radius. The two circles intersect at points A and E. A ray X nearest F and a ray X nearest K are drawn from the nearest root node X nearest , and a perpendicular GH is drawn to the line CX goal . The perpendicular GH intersects the circle S2 at points B and D, dividing the circle S2 into arcs L1, L2, and L3. The arc L1 corresponds to the search area I to the right of FX nearest K, the arc L2 corresponds to the search area II above FX nearest G and below HX nearest K, and the arc L3 corresponds to the search area III to the left of GX nearest H.

[0060] As shown in Figure 4 , the new node X new is on the arc L1 and is determined by the following formula:

[0061] -1 ≤ cos α ≤ 0

[0062] In the formula, α = ∠X nearest X new X goal .

[0063] Step 6: Determine whether there is an obstacle between the new node X new and the nearest point X nearest . If not, add the new node X new to the random tree T and go to Step 7. Otherwise, return to Step 2 to resample.

[0064] Step 7: Determine whether the new node X new is in the target point X goal region. If yes, add the target point X goal to the random tree T, backtrack from the target point X goal to the root node X init , and obtain the path points of the planned path. If not, return to Step 2 to resample.

[0065] In the formula, the new node X new is in the target point X​goal The formula of the class of the region is as follows:

[0066] |X new -X goal |<Thr

[0067] wherein, X new , X goal represent the new node and the target point vector, and the symbol || represents the 2-norm.

[0068] Step 8: As shown in FIG. 8, the nodes in the random tree T are connected to form a planning path. Figure 5

[0069] In order to verify the effectiveness of the method of the present application, 20 path planning experiments are respectively performed on the RRT path planning method and the method of the present application, and the average total number of nodes required for finding a planning path, the average length of a planning path and the average number of nodes of a planning path are compared. As shown in Table 1, compared with the RRT path planning method, the total number of nodes required for finding a path in the same environment is almost half of the original, so the calculation amount of the new node search of path planning is greatly reduced. Secondly, the length and the average number of nodes of the planning path are reduced. The comparison of the data of 20 path planning experiments is shown in Table 1.

[0070] Table 1 Comparison of data of 20 path planning experiments

[0071]

[0072] The above is the preferred embodiment of the present application, and any change made according to the technical solution of the present application, as long as the function generated does not exceed the scope of the technical solution of the present application, belongs to the protection scope of the present application.​

Claims

1. A path planning method based on the RRT algorithm with fixed step size sampling in a local region of an obtuse triangle, characterized in that, Provide the starting point, target point, fixed step size, obstacle locations, and planning map for path planning, and perform path planning as follows: Step 1: Define a fixed step size r for the expansion, starting from the path planning point X. init As the root node of the random tree T, and as the parent node for the next expansion of the random tree; Step 2: Generate a random point X rand ; Step 3: Traverse each node in the random tree T, and find the node closest to the random point as the nearest neighbor X by calculating the distance between the nodes in the random tree T and the random point. nearest ; Step 4: From the nearest point X nearest To random point X rand Expand the direction with a fixed step size r to obtain the new node X. new ; Step 5: Use the obtuse triangle method to determine whether the new node is on arc L1. If it is not on arc L1, return to step 2 to regenerate the random point. If it is on arc L1, go to step 2. Step 6: Determine the new node X new With neighboring point X nearest Are there any obstacles between them? If there are no obstacles, then move the new node X... new Add it to the random tree T and go to step 7; otherwise, return to step 2 to resample. Step 7: Determine the new node X new Is it at target point X? goal The region is the target point X. goal Add to the random tree, starting from the target point X goal Backtracking to root node X init The path points of the planned path are obtained. If they are not within the target point area, return to step 2 to resample. Step 8: Connect the nodes in the random tree T to form the planned path.

2. The path planning method based on the fixed-step sampling RRT algorithm for local regions of obtuse triangles according to claim 1, characterized in that, In step 2, when the map is rectangular, random points are obtained using the following method: In the above formula, x rand y rand For random point X rand The x and y coordinates are given, xL and yL are the length and width of the map, and rand∈(0,1).

3. The path planning method based on the RRT algorithm with fixed step size sampling in a local region of an obtuse triangle as described in claim 1, characterized in that, In step 3, all nodes in the random tree T and the random point X are traversed. rand The distances are compared, and the node with the smallest distance is the nearest neighbor X. nearest .

4. The path planning method based on the fixed step size sampling RRT algorithm for local regions of obtuse triangles according to claim 1, characterized in that, In step 4, a new node X is obtained. new The method is as follows: Calculate the nearest point X nearest With random point X rand Distance between: In the above formula, x nearest y nearest They are the nearest points X nearest x and y coordinates, x rand y rand They are the nearest points X rand The x and y coordinates of the new node X; new It can be calculated using the following formula: In the above formula, r is the fixed step size of the expansion.

5. The path planning method based on the RRT algorithm with fixed step size sampling in a local region of an obtuse triangle as described in claim 1, characterized in that, In step 5, the new node X is determined by the obtuse triangle method. new The method for arc L1 is as follows: With target point X goal and the nearest node X nearest Draw a circle S1 with the line connecting the two points as the diameter d, and take the nearest node X as the boundary. nearest Draw a circle S2 with center at point A and fixed expansion step size r as radius. The two circles intersect at points A and E, passing through the nearest root node X. nearest X-rays were drawn separately. nearest F and X nearest K, draw the line CX connecting the endpoints of the two circles. goal The perpendicular line GH intersects circle S2 at points B and D, dividing circle S2 into arcs L1, L2, and L3. Arc L1 corresponds to FX. nearest The search region I formed to the right of K corresponds to arc L2, which is FX. nearest G upper side and HX nearest The search region II formed below K, with arc L3 corresponding to GX. nearest Search region Ⅲ is formed to the left of H. New node X new The following formula is used to determine the arc L1: -1≤cosα≤0 In the above formula, α=∠X nearest X new X goal .

6. The path planning method based on the RRT algorithm with fixed step size sampling in a local region of an obtuse triangle as described in claim 1, characterized in that, In step 7, the target point X goal The area refers to the area centered on the target point X. goal Given a circular region centered at Thr with radius , determine the new node X. new At target point X goal The method for defining regions is as follows: |X new -X goal |<Thr In the above formula, X new X goal The vector represents the new node and the target point, and the || symbol represents the 2-norm.

7. The path planning method based on the fixed step size sampling RRT algorithm for local regions of obtuse triangles according to claim 1, characterized in that, In step 8, the nodes in the random tree T need to be arranged according to the target point X. goal To the starting point X init Connect them in order to form a planned path.

Images