A space path planning method for manipulator joints based on a probabilistic virtual potential field-guided bidirectional RRT* algorithm

Abstract

The present invention proposes a robot joint space path planning method based on a probabilistic virtual potential field guided bidirectional RRT* algorithm. On the basis of the traditional bidirectional RRT algorithm, before the initial collision-free path is generated, the target virtual potential field guidance strategy and the historical node bias strategy are integrated, and the path search efficiency is improved by using the potential field guidance of the target point and the historical node information in the target random tree; after the initial collision-free path is generated, the framework of the traditional RRT* algorithm is integrated to optimize the initial path, and based on the sampling in the continuous change interval near the node in the initial path, the node in the two random trees is reselected. Parent and child node operations are performed to improve the path quality. The method is based on the framework of the RRT algorithm as a whole to ensure that the search does not fall into the local minimum state of the virtual potential field, while maximizing the consideration of the smoothness of the trajectory of the robot during operation. The improved algorithm enables the robot to find a collision-free asymptotic optimal path in the workspace without sacrificing search efficiency and ensuring path quality.

Description

Technical Field

[0001] The present invention belongs to the field of path planning of industrial robot arms, and in particular to a robot arm joint space planning method based on a probabilistic virtual potential field guided bidirectional RRT* (PVFB-RRT*) algorithm. Background Art

[0002] The path planning of the robot arm is based on collision detection. It uses the discrete structure graph G(V,E) to plan a collision-free path from the initial posture to the target posture, where V is the set of nodes in the graph, each node corresponds to a configuration q; E is the set of edges in the graph, and each edge corresponds to a collision-free path between two configurations. Currently, there are three main common path planning algorithms: combined path planning algorithms, represented by Dijkstra and Astar based on grid search strategies; local path planning methods, such as the artificial potential field method; sampling-based path planning algorithms, represented by RRT, PRM and other algorithms. The combined path planning algorithm has a very high time complexity and space complexity for high-degree-of-freedom objects such as robotic arms due to its explicit representation of obstacle space, so it is not applicable; although the artificial potential field method provides an algorithm framework for high-dimensional objects, it is very easy to fall into a local optimal state and is difficult to meet actual needs; sampling-based path planning algorithms, such as RRT, have the advantages of being roughly complete and not requiring explicit obstacle space, but they are also inefficient in certain specific environments or objects, and the optimization process for path quality is also very time-consuming. Therefore, it is very meaningful to design a robotic arm path planning method that improves the rapidly expanding random tree (RRT) algorithm.

[0003] In view of the shortcomings of the prior art, the present invention improves the corresponding links of the traditional RRT from the perspectives of quality and efficiency, greatly reducing the efficiency of random tree expansion and improving the quality of the final path, enabling the robotic arm to find a collision-free path in the workspace without sacrificing search efficiency and ensuring path quality. Summary of the invention

[0004] The purpose of the present invention is to propose a method for manipulator joint space planning based on the probabilistic virtual potential field guided bidirectional RRT* (PVFB-RRT*) algorithm. The method enables the manipulator to find an asymptotically optimal collision-free path in the workspace without sacrificing search efficiency and ensuring path quality.

[0005] To achieve the above object, the present invention proposes a robot arm joint space path planning method based on a probabilistic virtual potential field guided bidirectional RRT* (Probabilistic Virtual Potential Field Bidirectional-RRT*, PVFB-RRT*) algorithm, characterized in that it includes the following steps:

[0006] Step 1: Determine the joint configuration X of the robot arm in the initial state and the target state s and X g , the robot DH parameter table and specific structural parameters, environmental information and two random trees T expanded from the initial state and target state respectively s and T g . Where X={θ1, θ2,..., θ n}, n represents the degree of freedom of the robot arm.

[0007] Step 2: Set the appropriate joint search step size Δθ and the probability interval of the sampling method as the basis for each step of expansion, set the appropriate termination threshold as the final connection range of the two random trees, and set the pruning radius and the maximum number of iterations NUM for path update to optimize the current path.

[0008] Step 3: According to the sampling method guided by the target virtual potential field, set the joint attraction coefficient, repulsion coefficient and repulsion field range.

[0009] Step 4: Based on the traditional bidirectional RRT algorithm, the robot generates two random trees T from the initial state and the target state according to the above strategy. s and T g ,During the expansion process, a method of generating random numbers between 0 and 1 is used to select the ,sampling method, and further new nodes are generated through the ,forward kinematics and collision detection of the robot arm.

[0010] Step 5: Determine the two random trees T according to the termination threshold s and T g Whether it is in the range of direct connection, if it is below the termination threshold, the search is stopped and the initial path is connected; otherwise, the robot arm continues to follow the original strategy until the termination threshold is met.

[0011] Step 6: After the initial path is generated, the parent node and child node reselection strategy of the RRT* algorithm is used in the path optimization process. Samples are taken in the continuous change interval near the nodes in the initial path, and the parent and child node reselection operations are performed on the nodes in the two random trees respectively. The path is updated after 50 repetitions. If the path cannot be connected, the parent and child node reselection operations are restarted 50 times; if the path can be connected, the updated path is output as the current path, and the above operations are repeated.

[0012] Step 7: After reaching the maximum number of path update iterations NUM, output the current path.

[0013] Furthermore, the purpose of step 1 is to clarify the planning problem and the position, shape and size of obstacles in the environment, and to establish a kinematic model of the corresponding robotic arm using the robotic arm DH parameter table.

[0014] Furthermore, the initialization parameters of the PVFB-RRT* algorithm in step 2 play a crucial role in the selection of nodes at each step of the robot path search and the optimization of node connection lines after the initial path is generated.

[0015] Furthermore, the purpose of step 3 is to initialize parameters for the sampling method guided by the virtual potential field, so as to make more effective use of the historical nodes in the target random tree as guides, thereby improving the efficiency of path search.

[0016] Furthermore, the strategy principles involved in steps 4 to 6 are described as follows:

[0017] (1) Bidirectional RRT algorithm principle: construct two random trees T from the initial state and the target state respectively. s and T g , T is used in each step of the expansion s and T g Add a new node X respectively new So far, the following description is based on T s Take the expansion as an example, T g The same applies to the extension. Randomly sample in the search space (except the obstacle space) to obtain a random node X rand As target point X d ; Traverse T s Find all the nodes in T s Center with target point X d The nearest neighboring node X near ; Along X near →X d The direction of expansion specifies the step size and determines the new node X new ; Perform forward kinematics of the robot arm based on the joint angles in the current state to obtain the positions of each joint and connecting rod; perform collision detection to check the new node X new Whether there is a collision with obstacles in the environment, if not, X new Deposit T s , link X near With X new Get a new branch, switch the random tree and repeat the above steps, otherwise resample until there is no collision X new Generation; continuously update the two random trees T in the above way s and Tg The nodes and branches in T s and T g The nodes in are within the range that can be directly connected, that is, below the termination threshold, and the initial path generation is completed.

[0018] (2) Principle of target virtual potential field guidance strategy: First, it is clear that each joint of the robot arm has three different change trends at a certain moment, namely +Δθ, remain unchanged, and -Δθ. Therefore, there are 3 changes from the current joint configuration state to the next joint configuration state. n configuration combination; secondly, the virtual force sources are the attraction force of the target and the repulsion force of the obstacle; finally, the 3 n The configuration combinations are combined and the power values ​​of all possible configuration combinations are calculated. The joint configuration with the smallest power value is taken as the new node X. new .

[0019] (3) Principle of historical node bias strategy: The node with the smallest cost value in the target random tree and the root node of the currently expanding random tree is regarded as the target point X d , using this as a goal guide to guide the currently expanding random tree to quickly expand to the target random tree.

[0020] (4) Random sampling method: randomly generate target points X d={θ d1 ,θ d2 ,...,θ dn}, calculate the target point X d The Euclidean distance to all nodes in the current random tree, taking the node with the smallest distance as X near , the joint components of the robot arm are along the X near →X d The direction of the corresponding component in is expanded by a specified step length Δθ to obtain X new .

[0021] (5) Based on the principle of guiding strategy based on probabilistic virtual potential field: mainly to solve the problem of new node X new The generation problem. When expanding the new node X new A random number between 0 and 1 is generated before, and the sampling method is determined according to the interval of the random number, thereby determining the new node X new , which can make the random tree expansion process blindly random and goal-oriented, thereby accelerating the search efficiency. The sampling methods are as described in (2) to (4) above, including random sampling, historical node bias, and target virtual potential field guidance. That is, each sampling method has The probability of being selected.

[0022] (6) Sampling within the continuous change interval near the initial path: The change trend of each joint of the robot arm at a certain moment is regarded as a continuous interval [-2Δθ, +2Δθ]. That is, the configuration of a node in the generated path can be considered as a continuous interval. Each joint randomly takes a value within this continuous interval, and the collision-free node after collision detection is taken as the new node X. new Execute the parent and child reselection operation, and then try to update the path after every 50 parent and child node reselection operations, and optimize the path within the specified number of iterations.

[0023] (7) Principle of the strategy for reselecting parent and child nodes: With the new node as the center, the pruning radius defines the replanning range (i.e., traverse all nodes in the current tree, calculate the distance d between all nodes and the current new node, and select the nodes whose distance value is less than the pruning radius). Add the cost value of these nodes to d, and select the node with the shortest path as X. new The new parent node of X is calculated from the starting point through the selected node to the new node X new Similarly, with the new node as the center, the pruning radius is used to define the replanning range (i.e., traverse all nodes in the current tree, calculate the distance d between all nodes and the current new node, and select the nodes whose distance value is less than the pruning radius). new The cost value of the node is added to d and compared with the current cost value of each node. If it is less than the current cost value, the new node X is added. new As the new parent node of this node, calculate the node from the starting point through the new node X new To select the cost value of the node.

[0024] Furthermore, step 7 serves as a final complete collision-free path from the starting position to the target position.

[0025] Compared with the prior art, the advantages of the present invention are: improving the corresponding links of the traditional RRT from the two perspectives of quality and efficiency. The efficiency perspective corresponds to selecting from three sampling methods by generating random numbers between 0 and 1 before the initial path is generated. While retaining the randomness of the traditional RRT algorithm, the sampling method is optimized by drawing on the idea of ​​the artificial potential field method, so that the initial path is quickly connected with better quality. The quality optimization perspective corresponds to optimizing the connected path by using the strategy of reselecting parent and child nodes by the RRT* algorithm after the initial path is generated, so as to achieve the goal of asymptotic optimization in a finite number of iterations. The improved algorithm enables the robot arm to find an asymptotically optimal collision-free path in the workspace without sacrificing search efficiency and ensuring path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 Flowchart of the robot path planning method for the PVFB-RRT* algorithm.

[0027] Figure 2 Schematic diagram of the target virtual potential field guidance strategy in the PVFB-RRT* algorithm.

[0028] Figure 3 Schematic diagram of the strategy principle for reselecting parent nodes in the PVFB-RRT* algorithm.

[0029] Figure 4 Schematic diagram of the principle of child node reselection strategy in the PVFB-RRT* algorithm. DETAILED DESCRIPTION

[0030] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further described below.

[0031] The purpose of this invention is to propose a robot arm joint space planning method based on the probabilistic virtual potential field guided bidirectional RRT* (PVFB-RRT*) algorithm. Figure 1 As shown, the steps are as follows:

[0032] Step 1: Determine the joint configuration X of the robot arm in the initial state and the target state s and X g , the robot DH parameter table and specific structural parameters, environmental information and two random trees T expanded from the initial state and target state respectively s and T g . Where X={θ1, θ2,..., θ n}, n represents the degree of freedom of the robot arm.

[0033] Step 2: Set the appropriate joint search step size Δθ and the probability interval of the sampling method as the basis for each step of expansion, set the appropriate termination threshold as the final connection range of the two random trees, and set the pruning radius and the maximum number of iterations NUM for path update to optimize the current path.

[0034] Step 3: According to the sampling method guided by the target virtual potential field, set the joint attraction coefficient, repulsion coefficient and repulsion field range.

[0035] Step 4: Construct two random trees T from the initial state and the target state respectively s and T g The following description is based on T s Take the expansion as an example, T g The same applies to the expansion. Generate a random number between 0 and 1 and determine the interval it is in: Then the potential field guidance strategy of the target point is used to directly select the new node X new If in The historical node bias strategy principle is adopted to replace the node X in the target random tree with the smallest cost value of the root node of the current random tree. nearest As target point X d , and further derive the new node X new If in Then a random node is obtained as the target point X by random sampling d , and further derive the new node X new The potential value calculation formula in the potential field guidance strategy of the target point is explained as follows:

[0036] Attract Power V a calculate:

[0037] V a =η a ·|XX target | 2

[0038] Among them, η a represents the attraction coefficient, X represents a set of joint configurations, X target represents the target configuration, that is, T s and T g The joint configuration at the root node of the random tree T s and T g Switch due to switching.

[0039] Repulsion Force V r calculate:

[0040]

[0041]

[0042] Among them, η r represents the repulsion coefficient, d0 represents the range of the repulsion field, d st represents the minimum distance between the surface of the s-th joint arm and the surface of the t-th obstacle, V rst represents the repulsive force of the t-th obstacle on the s-th joint, V r Represents the sum of the repulsive forces of all m obstacles on a set of configurations.

[0043] The total force V of a set of joint configurations X is:

[0044] V=ω1V a +ω2V r

[0045] Among them, ω1 and ω2 represent weight coefficients.

[0046] Step 5: For the newly generated new node X newPerform forward kinematics and collision detection. If no collision occurs, move the new node X new And the connected paths are stored in T s If a collision occurs, return to step 4.

[0047] Step 6: Check whether the current state meets the termination threshold condition. If so, output an initial path and continue to step 7; if not, switch the random tree and return to step 4.

[0048] Step 7: After the initial path is generated, the path optimization is performed and the path update loop variable j=1.

[0049] Step 8: Reselect the parent and child nodes loop variable k=1.

[0050] Step 9: Sample the new node X in the continuous change interval near the node in the current path knew , that is, the configuration executable change trend of a node in the current path is a continuous interval [-2Δθ, +2Δθ].

[0051] Step 10: Reselect the parent node: Figure 3 As shown in the figure, with the new node as the center, the pruning radius defines the replanning range (that is, traverse all nodes in the current tree, calculate the distance value d between all nodes and the current new node, and select the nodes whose distance value is less than the pruning radius), add the cost value of these nodes to d, and select the node with the shortest path as X knew The new parent node of X is calculated from the starting point through the selected node to the new node X knew The cost value.

[0052] Step 11: Reselect child nodes: With the new node as the center, the pruning radius is used to define the replanning range (i.e., traverse all nodes in the current tree, calculate the distance d between all nodes and the current new node, and select nodes whose distance value is less than the pruning radius). knew The cost value of the node is added to d and compared with the current cost value of each node. If it is less than the current cost value, the new node X is added. knew As the new parent node of this node, that is, calculate the node from the starting point through the new node X knew To select the cost value of the node.

[0053] Step 12: After executing steps 10 and 11, k++, if k<50, return to step 9; otherwise j++, execute step 13.

[0054] Step 13: Try to update the current path. If the path is not connected, restart the parent and child node reselection operation 50 times, that is, return to step 8; if the path is connected, update the path and use it as the current path and return to step 8.

[0055] Step 14: Repeat steps 8 to 13, continuously updating the nodes in the path until all nodes in the path are traversed, and finally outputting an asymptotically optimal collision-free path.

[0056] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A method for manipulator joint space path planning based on a probabilistic virtual potential field guided bidirectional RRT* algorithm, characterized in that: The following steps are involved: Step 1: Determine the joint configuration of the robot in the initial state and the target state and , the robot DH parameter table and specific structural parameters, environmental information and two random trees extended from the initial state and the target state respectively and ,in , n represents the degree of freedom of the robot; Step 2: Set the appropriate joint search step size , the probability interval of the sampling method is used as the basis for each step of expansion, an appropriate termination threshold is set as the final connection range of the two random trees, and the pruning radius and the maximum number of iterations NUM for path update are set to optimize the current path; Step 3: According to the sampling method guided by the target virtual potential field, set the joint attraction coefficient, repulsion coefficient and repulsion field range; Step 4: Based on the traditional bidirectional RRT algorithm, the robot generates two random trees from the initial state and the target state according to the above strategy. and , when expanding the new node Previously, a random number between 0 and 1 was generated, and the sampling method was determined according to the interval of the random number. New nodes were further generated through the forward kinematics and collision detection of the robot arm. , so that the expansion process of the random tree has blind randomness and goal-orientedness, thereby accelerating the search efficiency; the sampling methods are random sampling, historical node bias and target virtual potential field guidance, and each sampling method has The probability of being selected; The principle of the target virtual potential field guidance strategy is as follows: First, it is clear that each joint of the robot arm has three different change trends at a certain moment, which are: , keep still, , so there are a total of configuration combination; secondly, the virtual force sources are the attraction force of the target and the repulsion force of the obstacle; finally, the set is traversed The force values ​​of all possible configuration combinations are calculated, and the joint configuration with the smallest force value is used as the new node. ; Step 5: Determine the two random trees based on the termination threshold and Is it in the range of direct connection? If it is below the termination threshold, the search stops and the initial path is connected; otherwise, the robot arm continues to follow the original strategy until the termination threshold is met; Step 6: After the initial path is generated, the parent node and child node reselection strategy of the RRT* algorithm is used in the path optimization process. Samples are taken in the continuous change interval near the nodes in the initial path, and the parent and child node reselection operations are performed on the nodes in the two random trees respectively. The path is updated after 50 repetitions. If the path cannot be connected, the parent and child node reselection operations are restarted 50 times. If the path can be connected, the updated path is output as the current path, and the above operations are repeated. The principle of the strategy for reselecting parent and child nodes is as follows: With the new node as the center, the pruning radius defines the replanning range, that is, traverse all nodes in the current tree, calculate the distance value d between all nodes and the current new node, select the nodes whose distance value is less than the pruning radius, add the cost value of these nodes to d, and select the node with the shortest path as The new parent node of Similarly, with the new node as the center, the pruning radius defines the replanning range, that is, traverse all nodes in the current tree, calculate the distance value d between all nodes and the current new node, select the nodes whose distance value is less than the pruning radius, and put the new node The cost value of is added to d and compared with the current cost value of each node. If it is less than the current cost value, the new node is added. As the new child node of this node, that is, calculate the number of nodes from the starting point through the new node To the cost value of the selected node; Step 7: After reaching the maximum number of path update iterations NUM, output the current path.

2. The method for manipulator joint space path planning based on probabilistic virtual potential field guided bidirectional RRT* algorithm according to claim 1 is characterized in that: The principle of the bidirectional RRT algorithm is to construct two random trees from the initial state and the target state respectively. and , in each step of the expansion and Add a new node respectively So far, the following description is based on Take the expansion as an example, The same applies to the extension; random sampling is performed in the search space (except the obstacle space) to obtain random nodes As a target point ; Traverse Find all nodes in Center and target point The nearest neighboring node ; Along → The direction of expansion specifies the step size and determines the new node ; Perform forward kinematics of the robot arm based on the joint angles in the current state to obtain the positions of each joint and connecting rod; perform collision detection to check new nodes Whether there is a collision with obstacles in the environment, if not, Deposit In, link and Get a new branch, switch the random tree and repeat the above steps, otherwise resample until there is no collision Generation; keep updating two random trees in the above way and The nodes and branches in and The nodes in are within the range that can be directly connected, that is, below the termination threshold, and the initial path generation is completed.

3. The method for manipulator joint space path planning based on probabilistic virtual potential field guided bidirectional RRT* algorithm according to claim 1 is characterized in that: The principle of the historical node bias strategy is to regard the node with the smallest cost value in the target random tree and the root node of the currently expanding random tree as the target point. , using this as a goal guide to guide the currently expanding random tree to quickly expand to the target random tree.

4. The method for manipulator joint space path planning based on probabilistic virtual potential field guided bidirectional RRT* algorithm according to claim 1, characterized in that: The random sampling method: randomly generate target points , calculate the target point The Euclidean distance of all nodes in the current random tree, taking the node with the smallest distance as , the joint components of the robot arm are along → The direction of the corresponding component in the expansion specifies the step size get .

5. The method for manipulator joint space path planning based on probabilistic virtual potential field guided bidirectional RRT* algorithm according to claim 1, characterized in that: Sampling within the continuous change interval near the initial path node: The change trend of each joint of the robot arm at a certain moment is regarded as a continuous interval , that is, the configuration of a node in the generated path can be executed as a continuous interval, each joint randomly takes a value in this continuous interval, and the collision-free node after collision detection is used as the new node Reselect parent and child.

6. The method for manipulator joint space path planning based on probabilistic virtual potential field guided bidirectional RRT* algorithm according to claim 1, characterized in that: The step 7 serves as a final complete collision-free path from the starting position to the target position.

Images