Mechanical arm obstacle avoidance path planning method based on improved RRT algorithm

Abstract

The invention discloses a mechanical arm obstacle avoidance path planning method based on an improved RRT algorithm. Comprising the steps of 1, initializing map information, and setting algorithm parameters; 2, taking the starting point and the ending point as root nodes, and sampling in a space to generate expansion trees T1 and T2; 3, enhancing the tendency of searching towards a target point by adopting a target bias strategy so as to reduce useless search; 4, continuously reselecting father nodes and rewiring in the path searching process of the extension tree T1 and / or T2; and 5, keeping judgment in the path planning process by applying a greedy thought until no obstacle exists between two nodes in the extension trees T1 and T2 and a set threshold value is met, directly connecting the two nodes, and generating a path from the starting point to the ending point. According to the method, the thought of target bias is applied, the tendency of searching towards a target point is enhanced, the randomness of sampling and generating child nodes is kept, the blindness of the sampling process is reduced, and the convergence speed of the algorithm is increased.

Description

Technical Field

[0001] The present invention discloses a robot arm obstacle avoidance path planning method based on improved RRT algorithm Background Art

[0002] As an indispensable part of robot technology research, the robot arm has the advantages of large workspace, easy operation, flexibility and high degree of freedom. When the robot arm performs a task, whether the movement is continuous, whether the operation is efficient and smooth, and whether the planned path will collide with obstacles directly affect the quality and efficiency of the operation. Therefore, it is very meaningful to study the motion planning algorithms of the robot arm.

[0003] Path planning, as the name implies, is to plan the path of the robot's movement. It can help the robot move autonomously in complex environments and improve its automation level. The path planning of the robot arm is similar to that of the mobile robot chassis. Both take the current position of the robot as the starting point and plan a path to the end point without collision. However, the path planning of the robot arm is mainly used in multi-dimensional space. The accurate description and modeling of obstacles in the environment are more difficult than the mobile chassis; and the robot arm itself has multiple degrees of freedom, and it is more difficult to plan the path in the joint space of the robot arm. Therefore, in the past few decades, domestic and foreign scholars have tried many methods to solve the problems in the field of robot arm path planning, which can be mainly divided into four categories: path planning methods based on graph search algorithms, path planning methods based on potential fields, path planning methods based on intelligent algorithms, and path planning methods based on random sampling.

[0004] Path planning algorithms based on random sampling, represented by the Rapidly-exploring Random Tree (RRT) algorithm, do not require preprocessing or advance mapping, and are suitable for path planning in high-dimensional space. They are widely used in path planning for robot obstacle avoidance. However, the RRT algorithm still has shortcomings such as slow algorithm convergence, high cost of planned paths, and difficulty in planning paths in complex environments. In addition, the algorithm itself is random in the process of searching for paths, and in most cases, only feasible paths are planned, not optimal paths. Although various researchers have proposed many improved algorithms based on the RRT algorithm, none of them has absolute universality. Since the robot is usually unstable when planning paths in three-dimensional space, improvements need to be made at different levels. Summary of the invention

[0005] To overcome the above problems, the purpose of the present invention is to provide a manipulator obstacle avoidance path planning method based on an improved RRT algorithm. This method applies the target bias strengthening algorithm to enhance the tendency to search for the target point, introduces the greedy idea to improve the algorithm efficiency, combines the cubic B-spline curve to smooth and optimize the planned path. In addition, during the process of searching for the path, collision detection is maintained to achieve the obstacle avoidance effect. And because this method continuously reselects the parent node and rewires, the finally generated path approaches the optimal path, making this method have both search efficiency and optimality.

[0006] To achieve the above purpose, the manipulator obstacle avoidance path planning method based on the improved RRT algorithm of the present invention includes the following steps:

[0007] Step 1: Initialize the map information and set the algorithm parameters;

[0008] Step 2: Respectively use the starting point and the ending point as the root nodes to sample in the space to generate the expansion trees T1 and T2;

[0009] Step 3: Adopt the target bias strategy to enhance the tendency to search for the target point to reduce useless searches, that is, artificially guide the generation of random points during the sampling process. When generating a random node, with probability p, let this node generate a neighboring node towards the target point, that is, the expansion tree T1 expands and grows towards the target point F;

[0010] Step 4: Continuously reselect the parent node and rewire during the process of searching for the path in the expansion trees T1 and / or T2;

[0011] Step 5: Apply the greedy idea to maintain the judgment during the path planning process until there are no obstacles between two nodes in the expansion trees T1 and T2 and the set threshold is met, then directly connect these two nodes and generate a path from the starting point to the ending point.

[0012] Further, the parameters to be set in the step 1 include the starting point S, the ending point F, the target bias probability p, the reconnecting radius r, the step size es, the node connection threshold α, and the obstacle information between the starting point and the ending point.

[0013] Further, the specific content of the step 3 is: Generate a random number x within the range of (0, 1) rand , and when generating any child node T of the expansion tree T1 n1 , probability judgment is performed according to the set target bias probability p. And to maintain the search ability of the search tree for the unknown space, the value of the probability p is generally set between 0.05 and 0.10; if 0 < x rand < p, then T n1 generates a neighboring node T rand towards F, if p < x rand < 1, randomly generate a neighboring node T n1+1And repeat step 3.

[0014] Furthermore, the step 3 also includes: expanding the tree T2 to randomly generate a candidate node T n2 , its neighboring node T n2+1 The generation of rand As the goal, continuously generate adjacent nodes T close to the target node n2+1 .

[0015] Furthermore, collision detection is introduced each time a neighboring node is generated. If the neighboring node T rand With T n1 If there is no obstacle between rand Add T1, otherwise abandon the neighboring node T obtained this time rand ; If the neighboring node T n2+1 With T n2 If there is no obstacle between n2+1 Add T2, otherwise abandon the neighboring node T obtained this time n2+1 .

[0016] Furthermore, the specific contents of step 4 include:

[0017] Step 4-1: T1 expansion sampling generates node T n1 And generate neighboring nodes T towards F with a fixed step size es rand , at this time T n1 That is T rand The adjacent parent node of

[0018] Step 4-2: In T rand Nearby searches for neighboring nodes within the specified radius r, and uses the found neighboring nodes as candidates to replace the original parent node;

[0019] Step 4-3: Calculate the neighboring nodes to the starting point S plus the neighboring nodes to the node T in sequence rand The path cost of T is selected to replace the neighboring node with the minimum path cost. n1 As T rand New parent node;

[0020] Step 4-4: Replace T rand The parent node is rewired and the extended tree T1 is regenerated.

[0021] Furthermore, the collision detection model simplifies the robotic arm into a cylinder, simplifies the spatial obstacle into a sphere, and abstracts the connecting rod into a straight line without width to simplify the calculation; after the model simplification, the robotic arm and the obstacle can determine whether a collision occurs by judging the distance between the cylinder and the sphere. If the distance is less than 0, it is considered that a collision has occurred, otherwise no collision has occurred.

[0022] Furthermore, the specific contents of step 5 include: when there is T in the expansion trees T1 and T2 rand and T n2+1 When the distance between the two points meets the set node connection threshold α, the feasibility of directly connecting the two nodes is judged. If there is an obstacle between the two points, the two points continue to sample and expand to generate new nodes. If there is no obstacle between the two points, the sampling is stopped, and the two nodes are directly connected without being restricted by the fixed step size es, and the path between the two points is placed in the final planned path.

[0023] Furthermore, the method further includes step 6: using a cubic B-spline interpolation function to smooth the path to generate a final path.

[0024] Furthermore, the step 6 is: without changing the entire path, the cubic B-spline interpolation function is used to smoothly optimize only the local key position points, thereby optimizing the entire path to make the robot arm movement process smoother and more efficient.

[0025] The present invention has the following advantages:

[0026] (1) During the random sampling process of the RRT algorithm, the expansion tree often expands to a place far away from the target, that is, the "useless area", resulting in a large number of useless nodes and paths in the search path process. Therefore, the present invention applies the idea of ​​target bias to artificially guide the generation of random points in the sampling process. When generating a random node, the node is allowed to generate a neighboring node toward the target point with a certain probability, even if the expansion tree expands and grows toward the target point. This method strengthens the tendency to search toward the target point and reduces useless searches. It not only maintains the randomness of the sampling generation of sub-nodes, but also reduces the blindness of the sampling process to a certain extent, improves the convergence speed of the algorithm, and maximizes the search capability of the RRT algorithm in multi-dimensional space.

[0027] (2) When the RRT algorithm randomly samples the area near the target point, it is limited by the fixed step size. Even if there is no obstacle between the sampling point and the target point, the expansion tree cannot directly reach the target point. Instead, it will continue to perform a large number of invalid searches, which makes the path complex and increases the planning time. In response to the above problems, the present invention introduces the greedy idea, which keeps judging during the path search process until there are two nodes in the two expansion trees without obstacles and the set threshold is met. The two nodes are directly connected and a path from the starting point to the end point is generated, which greatly improves the speed of the algorithm's path search.

[0028] (3) If the path planned by the RRT algorithm is not smoothly optimized, it will often have inflection points with large bending amplitudes, which will cause jitter and oscillation during the operation of the robot arm, thereby affecting the working accuracy and efficiency of the robot arm. The present invention uses a cubic B-spline interpolation function to smoothly optimize the local key position points. The advantage of this is that the path is optimized without changing the entire path, so that the inflection points of the path with large bending amplitudes become gentle, ensuring the smooth and efficient movement of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 The figure is a flow chart of the optimization method of the present invention.

[0030] Figure 2 This is a schematic diagram of the target bias principle of the present invention.

[0031] Figure 3 This is a comparison diagram of the path curves before and after the target offset is adopted in an embodiment of the present invention.

[0032] Figure 4 It is the principle diagram of the collision detection model of the present invention.

[0033] Figure 5 The schematic diagram of the parent node reselection and rewiring process of the present invention.

[0034] Figure 6 It is the principle diagram of the greedy idea of ​​the present invention.

[0035] Figure 7 This is a comparison diagram of the path curve before and after optimization by cubic B-spline smoothing in an embodiment of the present invention.

[0036] Figure 8 The figures are the simulation experiment result diagrams of the traditional RRT algorithm and the improved algorithm of the present invention in the same three-dimensional simulation environment, wherein Figure (a) is the RRT algorithm, Figure (b) is the RRT* algorithm, and Figure (c) is the improved algorithm of the present invention. DETAILED DESCRIPTION

[0037] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0038] In the description of the present invention, it should be understood that the terms "center", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0039] The terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "plurality" means two or more.

[0040] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0041] The following is combined with Figure 1-8 The algorithm of the present invention is further described in detail with specific embodiments.

[0042] Example 1

[0043] This embodiment provides a robot arm obstacle avoidance path planning method based on an improved RRT algorithm. Figure 1 As shown, the following steps are included:

[0044] Step 1: Initialize map information and set algorithm parameters;

[0045] Step 2: Use the starting point and the end point as the root nodes to sample the space and generate the extended trees T1 and T2;

[0046] Step 3: Use the target bias strategy to strengthen the tendency of searching towards the target point and reduce useless searches, that is, artificially guide the generation of random points in the sampling process. When generating a random node, let the node generate a neighboring node towards the target point with probability p, that is, expand the tree T1 to expand and grow towards the target point F;

[0047] Step 4: During the path search process, the parent node is continuously reselected and the wiring is re-routed;

[0048] Step 5: The path planning process applies the greedy idea to keep the judgment until there are no obstacles between two nodes in the expanded trees T1 and T2 and the set threshold is met. Then, directly connect these two nodes and generate a path from the starting point to the ending point.

[0049] Step 6: Use the cubic B-spline interpolation function to smooth the path and generate the final path.

[0050] Preferably, the parameters to be set in Step 1 include the starting point S, the ending point F, the target bias probability p, the reconnecting radius r, the step size es, the node connection threshold α, and the obstacle information between the starting point and the ending point.

[0051] Embodiment 2

[0052] Based on the above embodiment, as Figure 2 shown, the specific content of Step 3 is: Generate a random number xrand within the range of (0, 1). When the expanded tree T1 generates any child node Tn1, perform a probability judgment according to the set target bias probability p. And to maintain the search ability of the search tree for the unknown space, the value of the probability p is generally set between 0.05 and 0.10. If 0 < xrand < p, then Tn1 generates a neighboring node Trand towards F. If p < xrand < 1, randomly generate a neighboring node Tn1+1 and repeat Step 3. The effect of the target bias is as Figure 3 shown.

[0053] Embodiment 3

[0054] Based on the above embodiment, while Step 3 is being performed, it also includes: The expanded tree T2 randomly generates a candidate node Tn2, and the generation of its adjacent node Tn2+1 is no longer random. Instead, taking Trand as the target, continuously generate adjacent nodes Tn2+1 that approach the target node.

[0055] Embodiment 4

[0056] Based on the above embodiment, the improved method proposed by the present invention introduces collision detection each time a neighboring node is generated. If there is no obstacle between the neighboring node Trand and Tn1, then add Trand to T1. Otherwise, discard the obtained neighboring node Trand. If there is no obstacle between the neighboring node Tn2+1 and Tn2, then add Tn2+1 to T2. Otherwise, discard the obtained neighboring node Tn2+1.

[0057] And as Figure 4As shown in the figure, the collision detection model simplifies the robot arm into a cylinder, simplifies the space obstacle into a sphere, and abstracts the connecting rod into a straight line without width to simplify the calculation. Let the radius of the envelope sphere S be r1, the radius of the connecting rod of the robot arm be r2, and the distance from the robot arm to the obstacle be D. When P1P2 2 +OP2 2 ≤OP1 2 When P1P2 2 +OP1 2 ≤OP2 2 When OP1P2 is greater than or equal to 0, D = OP1-r1-r2; when the triangle formed by the three points OP1P2 in space is an acute triangle, D = OP0-r1-r1. Combining the above three situations, when D is greater than or equal to 0, it is considered that there is no collision between the robot arm and the obstacle; otherwise, it is considered that there is a collision between the robot arm and the obstacle.

[0058] Example 5

[0059] Based on the above embodiments, Figure 5 As shown, the specific contents of step 4 include:

[0060] Step 4-1: T1 expands the sampling to generate node Tn1 and generates the adjacent node Trand towards F with a fixed step length es. At this time, Tn1 is the adjacent parent node of Trand;

[0061] Step 4-2: Search for neighboring nodes within a specified radius r near Trand, and use the found neighboring nodes as candidates to replace the original parent node;

[0062] Step 4-3: Calculate the path cost from the neighboring node to the starting point S plus the path cost from the neighboring node to the node Trand in turn, and select the neighboring node with the minimum path cost to replace Tn1 as the new parent node of Trand;

[0063] Step 4-4: Rewire after replacing the Trand parent node and regenerate the extended tree T1.

[0064] Example 6

[0065] Based on the above embodiment, the greedy idea diagram in step 5 is as follows: Figure 6As shown in the figure, when the distance between the new node generated by the expansion tree and the target point is within the set threshold α, the feasibility of directly connecting the new node Tn1~Tn5 with the target point F is judged. If there is an obstacle between the node and the target point (such as Tn1, Tn3, Tn4, Tn5), the node continues to randomly sample and expand; if there is no obstacle between the node and the target point (such as Tn2), the node stops random sampling and is not restricted by the fixed step size. As shown by the green dotted line in the figure, the node Tn2 is directly connected to the target point F, and the path between the two points is placed in the final planned path, as shown in the figure. Figure 6 Shown by the green dashed line.

[0066] Example 7

[0067] Based on the above embodiment, in step 6, the cubic B-spline interpolation function is used to smoothly optimize only the local key position points without changing the entire path, thereby optimizing the entire path and making the robot arm movement process smoother and more efficient.

[0068] The mathematical expression of the k+1 order (k degree) B-spline curve is:

[0069]

[0070] Where i = 1, 2, ..., n, Pi represents the n+1 local position points in the curve that need to be controlled; the node vector U = {u0, u1, ..., um}; k∈[2, n+1] and satisfies m = k+n+1; Bi,k(u) is the k-order B-spline basis function, corresponding to the local control point Pi, and is essentially a k-order piecewise polynomial determined by a sequence of non-decreasing parameters u. The deBoor-Cox recursive formula of Bi,k(u) is:

[0071]

[0072] According to the calculated local position points, when k = 4, the cubic B-spline basis function expression can be obtained as:

[0073]

[0074] According to the above formula, the key nodes in the path obtained by the algorithm planning are selected, and the paths between these nodes are smoothed by cubic B-spline. The optimization effect is as follows: Figure 7 shown.

[0075] In order to verify the feasibility of the improved algorithm of the present invention, a simulation comparison experiment was carried out in the MATLAB simulation platform, as follows:

[0076] The 3D space map used in the simulation is 100×100×100 in size, the space obstacles are 8 cubes, the starting coordinates of the path planning are set to (0,0,0), and the end coordinates are set to (100,100,100), respectively. Figure 8 As shown in the yellow and green dots in the middle; the sampling step is set to 3, and when the sampling exceeds 1000 times, the path planning is considered to have failed; the blue redundant line segment in the figure is the tree structure generated by the extended tree sampling process, and the red line segment connecting the starting point to the end point is the final planned path.

[0077] Under the same simulation environment and experimental parameters, 50 experiments were carried out using the RRT algorithm, the RRT* algorithm and the improved method proposed in this invention. 50 groups of experimental results were recorded to compare the average path length, calculation time and number of sampling points planned by the three algorithms, as shown in Table 1.

[0078] Table 1

[0079]

[0080] As can be seen from Table 1, although the RRT* algorithm plans the best path, its calculation time is the longest among the four algorithms, and the number of sampling points is also the largest; the improved algorithm proposed in the present invention has the shortest calculation time, the least number of sampling points in the planning process, and the planned path is also close to the optimal. The experimental results show that the robot arm obstacle avoidance path planning method based on the improved RRT algorithm proposed in the present invention can plan a feasible path with a path cost close to the optimal in a relatively short time, which verifies the efficiency and feasibility of the algorithm.

[0081] The present invention is described in detail above in conjunction with the accompanying drawings, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge of ordinary technicians in the field without departing from the purpose of the present invention. Many other changes and modifications that do not depart from the concept and scope of the present invention should be regarded as the protection scope of the present invention.

[0082] In the description of this specification, specific features, structures, materials or characteristics may be combined in an appropriate manner in any one or more embodiments or examples.

[0083] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A robot arm obstacle avoidance path planning method based on an improved RRT algorithm, characterized in that: The method comprises the following steps: Step 1: Initialize map information and set algorithm parameters; Step 2: Use the starting point and the end point as the root nodes to sample the space and generate the extended trees T1 and T2; Step 3: Use the target bias strategy to strengthen the tendency of searching towards the target point to reduce useless searches, that is, artificially guide the generation of random points in the sampling process. When generating a random node, the node is allowed to generate a neighboring node towards the target point with probability p, that is, the expansion tree T1 expands and grows towards the target point F; Step 4: Continuously reselect parent nodes and rewire in the process of searching paths in the expansion trees T1 and / or T2; Step 5: The path planning process applies the greedy idea to maintain judgment until there are two nodes in the expansion tree T1 and T2 without obstacles and meet the set threshold, then the two nodes are directly connected and a path from the starting point to the end point is generated.

2. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The parameters to be set in step 1 include the starting point S, the end point F, the target bias probability p, the reconnection radius r, the step size es, the node connection threshold α, and the obstacle information between the starting point and the end point.

3. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The specific content of step 3 is: generate a random number x in the range (0, 1) rand , and when the expansion tree T1 generates any child node T n1 , perform a probability judgment according to the set target bias probability p. And to maintain the search ability of the search tree for the unknown space, the value of the probability p is generally set between 0.05 and 0.10; if 0 < x rand < p, then T n1 generates an adjacent node T towards F rand , if p < x rand < 1, randomly generate an adjacent node T n1+1 and repeat step 3.

4. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 3 is characterized in that: The step 3 also includes: expanding the tree T2 to randomly generate a candidate node T n2 , its neighboring node T n2+1 The generation of rand As the goal, continuously generate adjacent nodes T close to the target node n2+1 .

5. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 3 or claim 4, characterized in that: Collision detection is introduced every time a neighboring node is generated. If the neighboring node T rand With T n1 If there is no obstacle between rand Add T1, otherwise abandon the neighboring node T obtained this time rand ; If the neighboring node T n2+1 With T n2 If there is no obstacle between n2+1 Add T2, otherwise abandon the neighboring node T obtained this time n2+1 .

6. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The specific contents of step 4 include: Step 4-1: T1 expansion sampling generates node T n1 And generate neighboring nodes T towards F with a fixed step size es rand , at this time T n1 That is T rand The adjacent parent node of Step 4-2: In T rand Nearby searches for neighboring nodes within the specified radius r, and uses the found neighboring nodes as candidates to replace the original parent node; Step 4-3: Calculate the neighboring nodes to the starting point S plus the neighboring nodes to the node T in sequence rand The path cost of T is selected to replace the neighboring node with the minimum path cost. n1 As T rand New parent node; Step 4-4: Replace T rand The parent node is rewired and the extended tree T1 is regenerated.

7. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 5, characterized in that: The collision detection model simplifies the robot arm into a cylinder, simplifies the spatial obstacle into a sphere, and abstracts the connecting rod into a straight line without width to simplify the calculation; after the robot arm and the obstacle are simplified, the distance between the cylinder and the sphere can be judged to determine whether a collision occurs. If the distance is less than 0, it is considered that a collision occurs, otherwise no collision occurs.

8. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The specific content of step 5 includes: when there is T in the expanded trees T1 and T2 rand and T n2+1 When the distance between the two points meets the set node connection threshold α, the feasibility of directly connecting the two nodes is judged. If there is an obstacle between the two points, the two points continue to sample and expand to generate new nodes. If there is no obstacle between the two points, the sampling is stopped, and the two nodes are directly connected without being restricted by the fixed step size es, and the path between the two points is placed in the final planned path.

9. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 1, characterized in that: The method also includes step 6: using a cubic B-spline interpolation function to smooth the path and generate a final path.

10. The robot arm obstacle avoidance path planning method based on the improved RRT algorithm according to claim 9, characterized in that: The step 6 is: without changing the entire path, using the cubic B-spline interpolation function to perform smooth optimization only on the local key position points, thereby optimizing the entire path to make the robot arm movement process more stable and efficient.

Images