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

Abstract

The invention discloses a mechanical arm obstacle avoidance path planning method based on an improved bidirectional RRT * algorithm, and the method comprises the steps: firstly obtaining parameter information needed by path planning, constructing a starting tree and a target tree, determining an initial search step size according to environment information, then carrying out the target bias sampling, and obtaining a random tree sampling point; finding a node closest to a sampling point on the random tree and pixelating a surrounding space of the node; calculating a dynamic extension step length; growing from the closest node to the sampling point of the random tree according to the dynamic extension step length to obtain a new node; carrying out collision detection; and applying a bidirectional end enhancement mechanism to obtain an initial path, finally optimizing a path turning position based on a path optimization method guided by a rotation vector, and outputting a final optimized path. According to the method, the path length can be remarkably shortened, the path quality can be remarkably improved, and the method is simple, easy to implement and suitable for mechanical arm path planning in the complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm path planning, and in particular relates to a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm. Background Art

[0002] With the rapid development of industrial robot technology, its application scenarios are becoming increasingly diverse, such as workpiece assembly, metal welding, material handling, etc. Path planning is an important technical means for the systematic intelligence of industrial robots. A good path planning algorithm can ensure the safety and reliability of industrial robots during operation. At present, the path planning methods of mobile robots are usually Dijkstra and A* algorithms based on graph search and APF algorithms based on potential fields. Although algorithms such as A* can find feasible paths, the complexity and computational complexity of these algorithms will increase significantly in high-dimensional space; and the APF algorithm is prone to fall into local optimal solutions and cannot ensure that the robot reaches the target point. Motion planning methods based on random sampling can better solve these problems.

[0003] The Rapidly Exploring Random Tree (RRT) algorithm based on random sampling is an existing robot path planning algorithm. Due to its high efficiency in high-dimensional space exploration and strong exploration in the global space, it is widely used in robot obstacle avoidance path planning. The RRT* algorithm is an improved and optimized version of the RRT algorithm, an asymptotically optimal path planning algorithm, which belongs to the commonly used technical terms in this technical field and is common knowledge. The RRT* algorithm has asymptotic optimality. As the number of sampling points increases, the path found by the algorithm will be close to the optimal path. However, the RRT* algorithm also has some limitations that are difficult to overcome. Since the sampling points are generated by a random uniform sampling algorithm without any guidance, most sampling points may deviate from the target, resulting in a slow convergence of the algorithm; the RRT* algorithm uses a fixed step search, which will cause the expansion speed to slow down in large open areas, and in narrow channels, the algorithm is easy to skip feasible paths, resulting in a locking phenomenon; the initial path generated by the RRT* algorithm has many nodes and bends, and further optimization is needed to reduce redundant nodes and bends so that the path is more in line with the actual execution requirements. Summary of the invention

[0004] In order to solve the above technical problems, the present invention provides a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm, which can plan a high-quality path avoiding obstacles in a complex environment in a relatively short time.

[0005] The technical solution adopted by the present invention is: a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm, and the specific steps are as follows:

[0006] S1. Construct and initialize the starting tree and target tree, determine the non-obstacle space as a random sampling space, and then determine the initial search step length based on the environmental information;

[0007] First, the parameter information required for path planning is initialized. The robot arm builds a map model based on the planning space and obstacle information, determines that the feasible space for target bias sampling is a non-obstacle space, and the starting point X of the robot arm movement init and the target point X goal , and construct the starting tree T with the starting point and the target point respectively A and the target tree T B , and then calculate the initial search step length Step0 and the environmental coefficient ε.

[0008] The parameter information includes: the spatial range of path planning, obstacle information, the starting point X of the robot arm, init and the target point X goal , the spatial size of the map MapL, MapH, the number of obstacles N ob , total obstacle area S ob , sampling target guide probability threshold P0, reselect parent node range radius r, angle rotation step Step angle , intersection point set I, collision state set C.

[0009] Among them, the calculation expressions of the environmental coefficient ε and the initial search step length Step0 are as follows:

[0010]

[0011] Step 0 = (MapL·MapH) 1 / 2 ·ε

[0012] S2, based on step S1, after the initialization of the starting tree and the target tree is completed, the target bias sampling is started, the sampling area is the non-obstacle space, the starting tree and the target tree are sampled in turn, a random probability P is generated, and it is determined whether the random probability is less than the target guidance probability. If so, a sampling point is randomly generated, otherwise the starting point of another tree is used as the sampling point;

[0013] Among them, the random tree is X rand , the sampling points of the starting tree and the target tree are X randA , X randB The sampling points X of the starting tree and the target tree rand (X randA , X randB ) is generated as follows:

[0014]

[0015] S3, based on step S2, after obtaining their respective sampling points, the starting tree and the target tree traverse the random tree and respectively find the distance from the sampling point X on the random tree rand The nearest node X nearest , Pixelated X nearest The nearby space and take the rectangular range, by X nearest The overlapping area S of the surrounding obstacle rectangle and the rectangular range area S range , determine the node environment coefficient η and the dynamic expansion step Step;

[0016] Among them, the length of the rectangular range is X range , Width Y range The calculation expression is as follows:

[0017]

[0018] in, Respectively represent X nearest The horizontal and vertical coordinates of .

[0019] Then the node environment coefficient η and the dynamic expansion step length Step are calculated as follows:

[0020]

[0021] Step=floor(Step0·η)

[0022] S4. Based on step S3, determine that the expansion direction is X nearest To X rand , by X nearest To X rand According to the dynamic expansion step length Step, a new node X is grown new ;

[0023] S5. Use X new Perform collision detection. If no collision occurs, X new Add to the random tree and go to step S6. If a collision occurs, return to step S2 and re-sample the target bias.

[0024] S6. For the new node X new Reselect parent nodes and rearrange lines;

[0025] For X new is the center of the circle and r is the radius of the circle node, and compares the nodes from the starting point of the random tree through each node in the circle to X new The path cost of the circle is selected as X. new The new parent node X newparent , and change the tree structure; for the changed tree structure and newis the center of the circle, r is the radius of the circle node, if there is a node X near , X near X new The path cost can be further reduced by setting X near The parent node is X new , re-arrange the random trees.

[0026] The starting point of the initial tree is X init , the starting point of the target tree is X goal .

[0027] S7, based on step S6, determine whether the starting tree and the target tree can be directly connected;

[0028] Applying the bidirectional end reinforcement mechanism, if the new node X generated by the starting tree newA Or the new node X generated by the target tree newB , can be directly connected to a node on another tree, the algorithm converges, generates an initial path, and enters step S8, otherwise returns to step S2.

[0029] S8. Based on the final path obtained in step S7, perform path optimization, remove unnecessary intermediate nodes, simplify the path and reduce redundancy, apply the path optimization method based on rotation vector guidance at the path turning point to generate new nodes, further optimize the turning point to reduce the path cost, and output the final optimized path, that is, the actual path suitable for the movement of the robot arm.

[0030] Furthermore, the step S8 is specifically as follows:

[0031] S81, input the path to be optimized, perform path pruning, connect the subsequent nodes in sequence from the path starting point, remove the intermediate nodes if no collision occurs, obtain the pruned path, and then determine whether to traverse all nodes of the path, if not, proceed to step S82, if yes, the path has completed the initial optimization, proceed to step S85;

[0032] S82, based on the pruned path obtained in step S81, starting from the starting point q j To begin, take node q j ,q j+1 and q j+2 , by q j ,q j+1 Generate a sufficiently long ray in the direction of the vector, rotate the ray counterclockwise, and rotate Step each time angle , and determine whether it has rotated 360°. If not, store the ray and q for each rotation. j+1 With q j+2 All the intersection points of the connected lines are stored in the intersection set I, and q is detected and stored. jThe result of collision detection with each point in the intersection point set I is stored in the collision state set C until the ray rotates to 360° and enters step S83;

[0033] S83, based on the Boolean value in the collision state set C obtained in step S82, detect the mutation of the collision state set, and when the collision state mutates, store the intersection coordinates q when there is no collision at the time of mutation. NC ;

[0034] The collision state changes suddenly from collision to non-collision or from non-collision to collision.

[0035] S84. Remove node q j+1 And create a new node q NC Add to the path, starting with q NC As the starting point, let q j =q NC , repeat the path optimization based on the rotation vector to generate new nodes, that is, repeat steps S82-S83 until all nodes in the pruned path in step S81 are traversed, the first optimized path is generated, and then go to step S85;

[0036] S85, pruning the first optimized path obtained in step S84, then reversing the pruned path and performing path optimization based on the rotation vector guidance again, i.e., repeating steps S82-S84 to obtain a second optimized path;

[0037] S86. Based on the secondary optimization path obtained in step S85, continue pruning, then reverse the path, and output the final optimization path.

[0038] Beneficial effects of the present invention: The method of the present invention first obtains the parameter information required for path planning, constructs a starting tree and a target tree, determines the initial search step length according to environmental information, then performs target bias sampling, obtains random tree sampling points, finds the node closest to the sampling point on the random tree and pixelates the surrounding space, calculates the dynamic expansion step length, grows from the nearest node to the random tree sampling point according to the dynamic expansion step length to obtain a new node, and performs collision detection. If there is a collision, resampling is performed. If there is no collision, the new node is added to the random tree and the parent node is reselected and the line is rearranged. A bidirectional terminal reinforcement mechanism is applied to obtain the initial path, and finally a path optimization method guided by a rotation vector is used to optimize the path turning point, and a final optimized path is output. The target bias sampling strategy proposed by the method of the present invention increases the probability of the sampling point biasing toward the target while maintaining the randomness of the sampling point, which can accelerate the convergence of the algorithm, reduce circuitous and unnecessary paths, and improve the quality of path planning; the proposed dynamic step adjustment strategy can dynamically adjust the step length according to the obstacle information around the node, thereby improving the flexibility of path planning, reducing the number of collisions, and saving collision detection time; the proposed bidirectional terminal reinforcement mechanism can significantly speed up the path generation speed of the algorithm, reduce the possibility of the algorithm falling into the local optimum, and enhance the adaptability of the algorithm to complex environments, especially those with narrow channel environments; the path optimization method based on the guidance of the rotation vector can significantly shorten the path length and improve the path quality. In addition, the method of the present invention is simple and easy to implement, and is suitable for the path planning of the manipulator in a complex environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 The present invention is a flow chart of a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm.

[0040] Figure 2 Schematic diagram of the target bias sampling strategy in an embodiment of the present invention.

[0041] Figure 3 Schematic diagram of the step length adjustment near an obstacle in an embodiment of the present invention.

[0042] Figure 4 The bidirectional end reinforcement mechanism principle diagram of the embodiment of the present invention

[0043] Figure 5 The figure is a flow chart of a path optimization method based on rotation vector guidance in an embodiment of the present invention.

[0044] Figure 6 Schematic diagram of a path optimization method based on rotation vector guidance in an embodiment of the present invention.

[0045] Figure 7 Schematic diagram of planning space for two obstacle scenarios in an embodiment of the present invention.

[0046] Figure 8 Schematic diagram of path planning using three algorithms in a single narrow channel environment in an embodiment of the present invention.

[0047] Fig. 9 Schematic diagram of path planning of three algorithms in a trap space environment in an embodiment of the present invention. DETAILED DESCRIPTION

[0048] In order to verify the performance of the method of the present invention, an obstacle environment needs to be built and the method of the present invention is used in an existing AUBO robot (AUBO-I5). The method of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0049] like Figure 1 As shown, a flow chart of a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm of the present invention, the specific steps are as follows:

[0050] S1. Construct and initialize the starting tree and target tree, determine the non-obstacle space as a random sampling space, and then determine the initial search step length based on the environmental information;

[0051] First, the parameter information required for path planning is initialized. The robot arm builds a map model based on the planning space and obstacle information (i.e., determines the size of the map, the number, area and location information of obstacles), determines the feasible space (random sampling space) of target bias sampling as a non-obstacle space, and the starting point X of the robot arm movement. init and the target point X goal , and construct the starting tree T with the starting point and the target point respectively A and the target tree T B , and then calculate the initial search step length Step0 and the environmental coefficient ε.

[0052] The parameter information includes: the spatial range of path planning, obstacle information, the starting point X of the robot arm, init and the target point X goal , the spatial size of the map MapL, MapH, the number of obstacles N ob , total obstacle area S ob , sampling target guide probability threshold P0, reselect parent node range radius r, angle rotation step Step angle , intersection point set I, collision state set C.

[0053] Among them, the calculation expressions of the environmental coefficient ε and the initial search step length Step0 are as follows:

[0054]

[0055] Step 0 = (MapL·MapH) 1 / 2 ·ε

[0056] like Figure 2 As shown in FIG. 1 , the principle diagram of the target bias sampling strategy in this embodiment is shown in FIG. In the planned sampling process, the sampling points are not generated completely randomly, but are directly sampled at the target point with a certain probability.

[0057] S2, based on step S1, after the initialization of the starting tree and the target tree is completed, the target bias sampling is started, the sampling area is the non-obstacle space, the starting tree and the target tree are sampled in turn, a random probability P is generated, and it is determined whether the random probability is less than the target guidance probability. If so, a sampling point is randomly generated, otherwise the starting point of another tree is used as the sampling point;

[0058] Among them, the random tree is X rand , the sampling points of the starting tree and the target tree are X randA , X randB The sampling points X of the starting tree and the target tree rand (X randA , X randB ) is generated as follows:

[0059]

[0060] S3, based on step S2, after obtaining their respective sampling points, the starting tree and the target tree traverse the random tree and respectively find the distance from the sampling point X on the random tree rand The nearest node X nearest , Pixelated X nearest The nearby space and take the rectangular range, by X nearest The overlapping area S of the surrounding obstacle rectangle and the rectangular range area S range , determine the node environment coefficient η and the dynamic expansion step Step;

[0061] Among them, the length of the rectangular range is X range , Width Y range The calculation expression is as follows:

[0062]

[0063] in, Respectively represent X nearest The horizontal and vertical coordinates of .

[0064] Then the node environment coefficient η and the dynamic expansion step length Step are calculated as follows:

[0065]

[0066] Step=floor(Step0·η)

[0067] like Figure 3As shown in FIG. 1 , a schematic diagram of the step size adjustment near obstacles in this embodiment is shown. When the random tree grows toward the sampling point, it will nearest The step size is adjusted dynamically based on the obstacle information around the node. When approaching an obstacle, the step size is reduced to find the path in a more refined way.

[0068] S4. Based on step S3, determine that the expansion direction is X nearest To X rand , by X nearest To X rand According to the dynamic expansion step length Step, a new node X is grown new ;

[0069] S5. Use X new Perform collision detection. If no collision occurs, X new Add to the random tree and go to step S6. If a collision occurs, return to step S2 and re-sample the target bias.

[0070] S6. For the new node X new Reselect parent nodes and rearrange lines;

[0071] For X new is the center of the circle and r is the radius of the circle node, and compares the nodes from the starting point of the random tree through each node in the circle to X new The path cost of the circle is selected as X. new The new parent node X newparent , and change the tree structure; for the changed tree structure and new is the center of the circle, r is the radius of the circle node, if there is a node X near , X near X new The path cost can be further reduced by setting X near The parent node is X new , re-arrange the random trees.

[0072] The starting point of the initial tree is X init , the starting point of the target tree is X goal .

[0073] S7, based on step S6, determine whether the starting tree and the target tree can be directly connected;

[0074] Applying the bidirectional end reinforcement mechanism, if the new node X generated by the starting tree newA Or the new node X generated by the target tree newB , can be directly connected to a node on another tree, the algorithm converges, generates an initial path, and enters step S8, otherwise returns to step S2.

[0075] like Figure 4As shown in FIG. 1 , the principle diagram of the bidirectional terminal reinforcement mechanism in this embodiment. When a random tree generates a new node, if a node exists on another random tree, it can be connected to the new node regardless of the current step size, and the algorithm converges. The bidirectional terminal reinforcement mechanism can quickly connect the two random trees when there is no obstacle at the corresponding nodes, thereby improving the convergence speed of the algorithm.

[0076] S8. Based on the final path obtained in step S7, perform path optimization, remove unnecessary intermediate nodes, simplify the path and reduce redundancy, apply the path optimization method based on rotation vector guidance at the path turning point to generate new nodes, further optimize the turning point to reduce the path cost, and output the final optimized path, that is, the actual path suitable for the movement of the robot arm.

[0077] like Figure 5 As shown, the flow chart of the path optimization method based on rotation vector guidance, in this embodiment, the step S8 is specifically as follows:

[0078] S81, input the path to be optimized (the initial path obtained in step S7), perform path pruning, connect the subsequent nodes in sequence from the path starting point, remove the intermediate nodes if no collision occurs, obtain the pruned path, and then determine whether to traverse all nodes of the path, if not, proceed to step S82, if yes, the path has completed the initial optimization, proceed to step S85;

[0079] S82, based on the pruned path obtained in step S81, starting from the starting point q j To begin, take node q j ,q j+1 and q j+2 , by q j ,q j+1 Generate a sufficiently long ray in the direction of the vector, rotate the ray counterclockwise, and rotate Step each time angle , and determine whether it has rotated 360°. If not, store the ray and q for each rotation. j+1 With q j+2 All the intersection points of the connected lines are stored in the intersection set I, and q is detected and stored. j The result of collision detection with each point in the intersection point set I is stored in the collision state set C until the ray rotates to 360° and enters step S83;

[0080] S83, based on the Boolean value in the collision state set C obtained in step S82, detect the mutation of the collision state set, and when the collision state mutates, store the intersection coordinates q when there is no collision at the time of mutation. NC ;

[0081] The collision state changes suddenly from collision to non-collision or from non-collision to collision.

[0082] S84. Remove node q j+1 And create a new node q NC Add to the path, starting with q NC As the starting point, let q j =q NC , repeat the path optimization based on the rotation vector to generate new nodes, that is, repeat steps S82-S83 until all nodes in the pruned path in step S81 are traversed, the first optimized path is generated, and then go to step S85;

[0083] like Figure 6 As shown, a path optimization method based on rotation vector guidance is used for the pruned path. Figure 6 (a) means starting from the starting point and traversing the nodes one by one. j With the intersection point q NC The line does not collide with the obstacle, q j With q NC When the next intersection line collides with an obstacle, remove q j+1 node, and put q NC Add to the path. Figure 6 (b) is the path after the initial optimization. The optimized path replaces the turning nodes and the path quality is improved.

[0084] S85, pruning the first optimized path obtained in step S84, then reversing the pruned path and performing path optimization based on the rotation vector guidance again, i.e., repeating steps S82-S84 to obtain a second optimized path;

[0085] S86. Based on the secondary optimization path obtained in step S85, continue pruning, then reverse the path, and output the final optimization path.

[0086] After path optimization is completed, the actual path suitable for the operation of the robotic arm is obtained. After planning is completed, the robotic arm avoids obstacles and accurately reaches the target point according to the planned path.

[0087] To further verify the planning ability of the method of the present invention, this embodiment also tests the pathfinding ability of standard RRT, RRT*-CONNECT and the method of the present invention in a single narrow channel environment and a trap space environment. RRT (Rapidly Exploring Random Tree) is an existing robot path planning algorithm, and the RRT* algorithm is an asymptotically optimal path planning algorithm, which is an optimized version of the RRT algorithm, a commonly used technical term in the technical field, and is common knowledge.

[0088] Due to the randomness of sampling, the existing algorithm and the method of the present invention were subjected to 50 simulation experiments in various scenarios, and the experimental average values ​​of their performance indicators were taken for analysis. Figure 7 Schematic diagram of two obstacle scenario planning spaces used in this embodiment.

[0089] (1) Figure 8 As shown, the experimental results of three algorithms in a single narrow channel environment;

[0090] like Figure 7 As shown in (a), a single narrow channel obstacle environment is set, the map size is 500*500 (dimensionless), and the black part is the obstacle. Figure 8 (a), (b), and (c) are schematic diagrams of the path planning results obtained by the RRT algorithm, the RRT*-CONNECT algorithm, and the method of the present invention in a single narrow channel obstacle environment. The time cost, path cost, and number of nodes experimental data of each algorithm in a single narrow channel environment are shown in Table 1 (obstacle avoidance experimental results of each algorithm in a single narrow channel environment).

[0091] Table 1

[0092]

[0093] (2) Fig. 9 As shown, the experimental results of three algorithms in the trap space environment;

[0094] like Figure 7 As shown in (b), a trap space obstacle environment is set up, the map size is 500*500 (dimensionless), and the black part is the obstacle. Fig. 9 (a), (b), and (c) are schematic diagrams of the path planning results obtained by the RRT algorithm, the RRT*-CONNECT algorithm, and the method of the present invention in a single narrow channel obstacle environment. The experimental data of the time cost, path cost, and number of nodes of each algorithm in the trap space environment are shown in Table 2 (obstacle avoidance experimental results of each algorithm in the trap space environment).

[0095] Table 2

[0096]

[0097] In summary, compared with the RRT and RRT*-CONNECT algorithms, the method of the present invention significantly shortens the search time and the number of nodes, especially in narrow and long scenarios with trap spaces. The target bias sampling strategy proposed by the method of the present invention increases the probability of the sampling point biased toward the target while maintaining the randomness of the sampling points, which can accelerate the convergence of the algorithm, reduce detours and unnecessary paths, and improve the quality of path planning; the proposed dynamic step adjustment strategy can dynamically adjust the step length according to the obstacle information around the node, thereby improving the flexibility of path planning, reducing the number of collisions, and saving collision detection time; the proposed bidirectional terminal reinforcement mechanism can significantly speed up the algorithm's path generation speed, reduce the possibility of the algorithm falling into local optimality, and enhance the algorithm's adaptability to complex environments, especially narrow channel environments; the path optimization method based on rotation vector guidance can significantly shorten the path length and improve the path quality. In trap spaces and complex environments, the method of the present invention can also quickly plan a high-quality path suitable for the operation of the robotic arm.

[0098] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm, the specific steps are as follows: S1. Construct and initialize the starting tree and target tree, determine the non-obstacle space as a random sampling space, and then determine the initial search step length based on the environmental information; First, the parameter information required for path planning is initialized. The robot arm builds a map model based on the planning space and obstacle information, determines that the feasible space for target bias sampling is a non-obstacle space, and the starting point X of the robot arm movement init and the target point X goal , and construct the starting tree T with the starting point and the target point respectively A and the target tree T B , and then calculate the initial search step Step 0 and environmental coefficient ε; in, The parameter information includes: the spatial range of path planning, obstacle information, the starting point X of the robot arm init and the target point X goal , the spatial size of the map MapL, MapH, the number of obstacles N ob , total obstacle area S ob , sampling target guidance probability threshold P 0 , reselect the parent node range radius r, angle rotation step Step angle , intersection point set I, collision state set C; Among them, the environmental coefficient ε and the initial search step length Step 0 The calculation expression is as follows: Step 0 =(MapL·MapH) 1 / 2 ·ε S2, based on step S1, after the initialization of the starting tree and the target tree is completed, the target bias sampling is started, the sampling area is the non-obstacle space, the starting tree and the target tree are sampled in turn, a random probability P is generated, and it is determined whether the random probability is less than the target guidance probability. If so, a sampling point is randomly generated, otherwise the starting point of another tree is used as the sampling point; Among them, the random tree is X rand , the sampling points of the starting tree and the target tree are X randA , X randB ; Sampling points X of the starting tree and the target tree rand (X randA , X randB ) is generated as follows: S3, based on step S2, after obtaining their respective sampling points, the starting tree and the target tree traverse the random tree and respectively find the distance from the sampling point X on the random tree rand The nearest node X nearest , Pixelated X nearest The nearby space and take the rectangular range, by X nearest The overlapping area S of the surrounding obstacle rectangle and the rectangular range area S range , determine the node environment coefficient η and the dynamic expansion step Step; Among them, the length of the rectangular range is X range , Width Y range The calculation expression is as follows: in, Respectively represent X nearest The horizontal and vertical coordinates of Then the node environment coefficient η and the dynamic expansion step length Step are calculated as follows: Step=floor(Step 0 ·η) S4. Based on step S3, determine that the expansion direction is X nearest To X rand , by X nearest To X rand According to the dynamic expansion step length Step, a new node X is grown new ; S5. Use X new Perform collision detection. If no collision occurs, X new Add to the random tree and go to step S6. If a collision occurs, return to step S2 and re-sample the target bias. S6. For the new node X new Reselect parent nodes and rearrange lines; For X new is the center of the circle and r is the radius of the circle node, and compares the nodes from the starting point of the random tree through each node in the circle to X new The path cost of the circle is selected as X. new The new parent node X newparent , and change the tree structure; for the changed tree structure and new is the center of the circle, r is the radius of the circle node, if there is a node X near , X near X new The path cost can be further reduced by setting X near The parent node is X new , re-arrange the random tree; The starting point of the initial tree is X init , the starting point of the target tree is X goal ; S7, based on step S6, determine whether the starting tree and the target tree can be directly connected; Applying the bidirectional end reinforcement mechanism, if the new node X generated by the starting tree newA Or the new node X generated by the target tree newB , can be directly connected to a node on another tree, the algorithm converges, generates an initial path, and enters step S8, otherwise returns to step S2; S8. Based on the final path obtained in step S7, perform path optimization, remove unnecessary intermediate nodes, simplify the path and reduce redundancy, apply the path optimization method based on rotation vector guidance at the path turning point to generate new nodes, further optimize the turning point to reduce the path cost, and output the final optimized path, that is, the actual path suitable for the movement of the robot arm.

2. According to claim 1, a robot arm obstacle avoidance path planning method based on an improved bidirectional RRT* algorithm, It is characterized in that The step S8 is specifically as follows: S81, input the path to be optimized, perform path pruning, connect the subsequent nodes in sequence from the path starting point, remove the intermediate nodes if no collision occurs, obtain the pruned path, and then determine whether to traverse all nodes of the path, if not, proceed to step S82, if yes, the path has completed the initial optimization, proceed to step S85; S82, based on the pruned path obtained in step S81, starting from the starting point q j To begin, take node q j ,q j+1 and q j+2 , by q j ,q j+1 Generate a sufficiently long ray in the direction of the vector, rotate the ray counterclockwise, and rotate Step each time angle , and determine whether it has rotated 360°. If not, store the ray and q for each rotation. j+1 With q j+2 All the intersection points of the connected lines are stored in the intersection set I, and q is detected and stored. j The result of collision detection with each point in the intersection point set I is stored in the collision state set C until the ray rotates to 360° and enters step S83; S83, based on the Boolean value in the collision state set C obtained in step S82, detect the mutation of the collision state set, and when the collision state mutates, store the intersection coordinates q when there is no collision at the time of mutation. NC ; Wherein, the collision state undergoes a sudden change, that is, from collision to non-collision or from non-collision to collision; S84. Remove node q j+1 And create a new node q NC Add to the path, starting with q NC As the starting point, let q j =q NC , repeat the path optimization based on the rotation vector to generate new nodes, that is, repeat steps S82-S83 until all nodes in the pruned path in step S81 are traversed, the first optimized path is generated, and then go to step S85; S85, pruning the first optimized path obtained in step S84, then reversing the pruned path and performing path optimization based on the rotation vector guidance again, i.e., repeating steps S82-S84 to obtain a second optimized path; S86. Based on the secondary optimization path obtained in step S85, continue pruning, then reverse the path, and output the final optimization path.

Images