Seven-degree-of-freedom robot arm motion planning method based on hybrid multi-policy RRT

Abstract

A kind of seven degrees of freedom manipulator motion planning method based on hybrid multi-policy RRT, adaptive step growth strategy is introduced to fast search random tree, the node growth step length of fast search random tree in different subspace in the process of generating path is obtained;Subspace incremental sampling strategy is introduced to fast search random tree, so that fast search random tree tends to sample in unexplored area;The breadth-first search strategy is introduced to fast search random tree, the search result is refined, and invalid nodes are removed.The present application can generate different length of node growth step length according to the different obstacle density in the environment, quickly cover unknown area and get the effective motion path between initial point and target point by introducing three strategies.The present application significantly improves the motion planning efficiency of manipulator in unstructured environment, can meet the high efficiency, fast response demand in the process of precision operation, and provides convenience for subsequent speed planning and control of manipulator.

Description

Technical Field

[0001] This invention belongs to the field of modern intelligent manufacturing technology, specifically relating to a method for planning the motion of robotic arms in the field of intelligent robots. Background Technology

[0002] Dexterous robots possess advantages such as lightweight design, high precision, fast response, and easy deployment. They are currently widely used in modern industrial fields such as biomedicine, intelligent manufacturing, warehousing and logistics, and surveying and inspection, and are a key driving force behind the rapid development of the industrial robot market. Among dexterous robots, seven-DOF robotic arms, due to their redundant degrees of freedom (i.e., dexterous motion characteristics), exhibit superior dynamic motion performance, effectively avoiding singular configurations and thus completing more complex tasks. However, research on seven-DOF robotic arms started relatively late both domestically and internationally, and existing theories and methods are not yet perfect. Furthermore, compared to traditional six-DOF industrial robots, there are significant differences in structural configuration and operating environment. Especially in motion planning in unstructured environments, traditional planning algorithms struggle to leverage the advantages of seven-DOF robotic arms, resulting in long planning times and low success rates, making it difficult to meet the demands of precision operations.

[0003] Currently, robotic arm motion planning methods can be broadly categorized into three types: 1) Classical motion planning algorithms, mainly including grid methods, artificial potential field methods, and probabilistic path graph methods. These methods generally require significant information storage space and have long path planning times. 2) Motion planning methods based on random sampling, mainly including probabilistic path graphs (PRM), exploration / utilization tree algorithms (EET), and fast random tree search (RRT). These methods have lower time complexity and are generally unaffected by spatial dimensions. 3) Motion planning methods based on intelligent optimization algorithms, including A* algorithm, neural networks, Q-Learning, ant colony optimization, and simulated annealing. While these algorithms greatly simplify robot modeling steps, optimization or pre-training times are long, so they are not commonly used in engineering. Among these three types of methods, random sampling motion planning algorithms are widely used in the robotics field. The fast random tree search (RRT) algorithm, in particular, has become a mainstream algorithm for robotic arm motion planning due to its simple working principle and lack of spatial dimension limitations. However, current RRT and its variant algorithms can only achieve good motion planning results in small environments. As the scale of the environment and the density of obstacles continue to increase, the planning effect of the algorithm will be greatly reduced, making it difficult to meet the high efficiency and fast response requirements in precision operations. Summary of the Invention

[0004] The technical problem to be solved by this invention is to overcome the shortcomings of the prior art and provide a motion planning method for a seven-degree-of-freedom robotic arm based on hybrid multi-strategy RRT with high motion planning efficiency.

[0005] The technical solution adopted to solve the above technical problems is: a motion planning method for a seven-degree-of-freedom robotic arm based on hybrid multi-strategy RRT, including the following steps:

[0006] Step 1. Introduce an adaptive step size growth strategy into the fast search random tree to obtain the node growth step size in different subspaces during the path generation process;

[0007] Step 1.1. Divide the entire environment region X into N subspaces using a Voronoi diagram, where N is a positive integer. Perform random sampling in each subspace using Monte Carlo sampling, and obtain the proportion of obstacles in each subspace according to the following formula.

[0008]

[0009] In the formula, M i Let be the total number of samples in the i-th subspace. The number of times an obstacle is randomly sampled in the i-th subspace;

[0010] Step 1.2. Determine the adaptive step size factor based on the proportion of obstacles in the subspace, construct the adaptive growth step size, and obtain the adaptive growth step size in different subspaces. For areas with dense obstacles, the growth step size will be shorter, and for areas with sparse obstacles, the growth step size will be longer.

[0011] The adaptive growth step size γ in different subspaces i for:

[0012]

[0013] In the formula, x new For the newly generated node, x rand For the nodes obtained from sampling, x near Let x be the distance rand The nearest node, γ represents the maximum change in the growth step size, where γ is the growth step size of the tree node.

[0014] Step 2. Introduce a subspace incremental sampling strategy into the fast search random tree, so that the fast search random tree tends to sample in unexplored regions;

[0015] Step 2.1. Define the information within environmental region X in Step 1 as set I, and the information of the searched environmental region as set I. exp There are M obstacles in the environment region X. The collision probability of the obstacles in the searched environment region is constructed by applying the Gaussian distribution function, and then the collision probability F(x) of the obstacles in the entire environment region X is obtained according to the following formula.

[0016]

[0017]

[0018] In the formula, f j (x) represents the collision probability of the j-th obstacle within the searched environment area, x is any point in the state space of the robot arm's motion planning, and σ j Let μ be the range factor of the j-th obstacle, and μ be the expected value.

[0019] Step 2.2. Based on the collision probability of obstacles in the entire environment, construct the incremental sampling probability P that can adaptively adjust the sampling probability according to the density of nodes and obstacles, according to the following formula. i ;

[0020]

[0021] In the formula, D i σ represents the density of tree nodes in the environmental space, and σ is the range factor of obstacles.

[0022] Step 3. Introduce a breadth-first search strategy into the fast search random tree to refine the search results and remove invalid nodes;

[0023] The tree node results generated in steps 1 and 2 are output to the breadth-first search. The breadth-first search will take out tree nodes in sequence from these tree nodes and determine whether there are obstacles in the adjacent grids around the taken tree node. If there are no obstacles, it is marked as a valid path point; if there are obstacles, it will be removed.

[0024] As a preferred technical solution, the adaptive step size factor in step 1.2 is determined according to the following formula;

[0025]

[0026] In the formula, Let k be the adaptive step size factor, and k be a constant scaling factor, where k > 0. This refers to the proportion of obstacles in the entire environmental area.

[0027] As a preferred technical solution, in step 2.2, the density D of tree nodes in the environmental space i for:

[0028]

[0029] In the formula, n i Let C be the number of nodes currently existing in the i-th subspace. i Let be the number of existing nodes in the i-th subspace.

[0030] As a preferred technical solution, the number of existing nodes C in the i-th subspace i Determined according to the following formula;

[0031]

[0032] In the formula, V i Let V be the volume of the i-th subspace. nodei The volume required for each tree node, ρ d Let r be the standard volume of a unit sphere in the global workspace. i Let α be the distance between nodes in the i-th subspace, where α is a constant value, α∈(0,1).

[0033] The beneficial effects of this invention are as follows:

[0034] This invention introduces three strategies—adaptive step-size growth, subspace incremental sampling, and breadth-first search—to enable a robotic arm to generate node growth step sizes of varying lengths based on the density of obstacles in the environment. This allows for rapid coverage of unknown regions and the acquisition of an effective motion path from the initial point to the target point. This invention significantly improves the motion planning efficiency of the robotic arm in unstructured environments, meeting the high-efficiency and fast-response requirements of precision operations and facilitating subsequent speed planning and control of the robotic arm. The method is effective, simple, and straightforward, demonstrating good practicality and versatility. Attached Figure Description

[0035] Figure 1 This is a flowchart illustrating the present invention.

[0036] Figure 2 This is a schematic diagram of node generation according to the present invention.

[0037] Figure 3 This is the incremental sampling probability curve of the present invention.

[0038] Figure 4 This is a simulation result diagram of the present invention in a three-dimensional raster map.

[0039] Figure 5 This is a comparison chart of simulated data from the present invention.

[0040] Figure 6 This is a measured effect diagram of the invention on a robotic arm prototype.

[0041] Figure 7 This is a comparison chart of actual test data from the prototype. Detailed Implementation

[0042] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments, but the present invention is not limited to the following embodiments.

[0043] exist Figure 1 In this embodiment, a motion planning method for a seven-DOF robotic arm based on hybrid multi-strategy RRT includes the following steps:

[0044] Step 1. Introduce an adaptive step-size growth strategy into the fast search random tree to obtain the node growth step size in different subspaces during the path generation process, such as... Figure 2 ;

[0045] Step 1.1. Divide the entire environment region X into N subspaces using a Voronoi diagram, where N is a positive integer. Perform random sampling in each subspace using Monte Carlo sampling, and obtain the proportion of obstacles in each subspace according to the following formula.

[0046]

[0047] In the formula, M i Let be the total number of samples in the i-th subspace. The number of times an obstacle is randomly sampled in the i-th subspace;

[0048] Step 1.2. Determine the adaptive step size factor based on the proportion of obstacles in the subspace, construct the adaptive growth step size, and obtain the adaptive growth step size in different subspaces. For areas with dense obstacles, the growth step size will be shorter, and for areas with sparse obstacles, the growth step size will be longer.

[0049] The adaptive growth step size γ in different subspaces i for:

[0050]

[0051] In the formula, x new For the newly generated node, x rand For the nodes obtained from sampling, x near Let x be the distance rand The nearest node, γ represents the maximum change in the growth step size, where γ is the growth step size of the tree node.

[0052] The adaptive step size factor is determined according to the following formula;

[0053]

[0054] In the formula, Let k be the adaptive step size factor, and k be a constant scaling factor, where k > 0. The proportion of obstacles in the entire environmental area;

[0055] Step 2. Introduce a subspace incremental sampling strategy into the fast search random tree, so that the fast search random tree tends to sample in unexplored regions;

[0056] Step 2.1. Define the information within environmental region X in Step 1 as set I, and the information of the searched environmental region as set I. exp There are M obstacles in the environment region X. The collision probability of the obstacles in the searched environment region is constructed by applying the Gaussian distribution function, and then the collision probability F(x) of the obstacles in the entire environment region X is obtained according to the following formula.

[0057]

[0058]

[0059] In the formula, f j (x) represents the collision probability of the j-th obstacle within the searched environment area, x is any point in the state space of the robot arm's motion planning, and σ j Let μ be the range factor of the j-th obstacle, and μ be the expected value.

[0060] Step 2.2. Based on the collision probability of obstacles in the entire environment, construct the incremental sampling probability P that can adaptively adjust the sampling probability according to the density of nodes and obstacles, according to the following formula. i ,like Figure 3 ;

[0061]

[0062] In the formula, D i σ represents the density of tree nodes in the environmental space, and σ is the range factor of obstacles.

[0063] Among them, the density D of tree nodes in the environmental space i for:

[0064]

[0065] In the formula, n i Let C be the number of nodes currently existing in the i-th subspace. i Let i be the number of existing nodes in the i-th subspace;

[0066] The number of nodes in the i-th subspace is C. i Determined according to the following formula;

[0067]

[0068] In the formula, V i Let V be the volume of the i-th subspace. nodei The volume required for each tree node, ρ dLet r be the standard volume of a unit sphere in the global workspace. i Let α be the distance between nodes in the i-th subspace, where α∈(0,1) is a constant value;

[0069] Step 3. Introduce a breadth-first search strategy into the fast search random tree to refine the search results and remove invalid nodes;

[0070] The tree node results generated in steps 1 and 2 are output to the breadth-first search. The breadth-first search will take out tree nodes in sequence from these tree nodes and determine whether there are obstacles in the adjacent grids around the taken tree node. If there are no obstacles, it is marked as a valid path point; if there are obstacles, it will be removed.

[0071] Experiment 1

[0072] To verify the beneficial effects of the present invention, simulation experiments were conducted on the present invention, such as... Figure 4 This invention is compared and analyzed with current mainstream RRT, Fast RRT, and Lower Bound Tree-RRT (LBT-RRT). The comparison metrics include search time (ST / s), number of nodes (NN), number of obstacle detections (NOD), and path length (MPD / mm). Considering the randomness inherent in numerical simulations, each experiment was repeated 10 times, and the average value was used as the final simulation result. Figure 5 As can be seen, compared with the LBT-RRT algorithm, the present invention (HMS-RRT) reduces search time by 35.9%, the number of nodes by 12.2%, and the number of obstacle detections by 5.5% while maintaining almost the same motion path length. This demonstrates that the present invention significantly improves search efficiency and motion planning quality in a 3D grid map environment.

[0073] Experiment 2

[0074] In the actual test of this invention on a robotic arm prototype, the end effector of the robotic arm always maintained a vertically downward posture during the test, such as... Figure 6 .

[0075] like Figure 7 As shown, compared with the LBT-RRT algorithm, the present invention (HMS-RRT) reduces search time by 16.3%, the number of nodes by 19.2%, and the number of obstacle detections by 20.7% in the prototype test environment, while the motion path only increases by 0.9%, which is negligible. It is worth noting that for the prototype experiment, the actual test results differ somewhat from the simulation results due to the influence of communication latency, encoder error, and robotic arm vibration, but overall, the performance improvement of the present invention is significant.

Claims

1. A method for motion planning of a 7-DOF manipulator based on hybrid multi-strategy RRT, characterized in that, The method comprises the following steps: Step 1. Introducing an adaptive step growth strategy to the fast search random tree to obtain the growth step length of the nodes in different subspaces in the path generation process of the fast search random tree; Step 1.

1. Divide the whole environment region X into N subspaces by Voronoi diagram, N is a positive integer, randomly sample in each subspace by Monte Carlo sampling method, and get the proportion of obstacles in each subspace according to the following formula In the formula, M i is the total number of samples in the i-th subspace, is the number of times that the obstacle is randomly sampled in the i-th subspace; Step 1.

2. Determining an adaptive step factor according to the proportion of obstacles in the subspace to construct an adaptive growth step length, and obtaining the adaptive growth step length in different subspaces; for the region dense in obstacles, the growth step length will be correspondingly shortened, and for the region sparse in obstacles, the growth step length will be correspondingly lengthened; The adaptive growing step size γ within the different subspaces i is: In the formula, x new is a new node generated, x rand is a node sampled, x near is the distance x rand is the nearest node, is the maximum change value of the growth step, and γ is the growth step of the tree node. Step 2. Introducing a subspace incremental sampling strategy to the fast search random tree to make the fast search random tree tend to sample in unexplored regions; Step 2.

1. Define the information within the environment region X in step 1 as a set I, and the searched environment region information as a set I exp There are M obstacles in the environment region X, and a Gaussian distribution function is applied to construct the obstacle collision probability in the searched environment region, so that the collision probability F(x) of the obstacles in the entire environment region X is obtained according to the following formula; In the formula, f j (x) is the jth obstacle collision probability in the searched environment region, x is an arbitrary point in the manipulator motion planning state space, σ j is the range factor of the jth obstacle, and μ is the expected value. Step 2.

2. Construct the incremental sampling probability P that can adaptively adjust the sampling probability according to the size of the node and obstacle density according to the collision probability of obstacles within the entire environmental space, as follows i ; In the formula, D i is the density of tree nodes in the environment space, and σ is the range factor of the obstacle. Step 3. Introducing a breadth-first search strategy to the fast search random tree to refine the search result and eliminate invalid nodes; The tree node results generated in steps 1 and 2 are output to the breadth-first search, and the breadth-first search will sequentially take out the tree nodes from the tree nodes and judge whether there are obstacles around the taken-out tree nodes, if not, mark as valid path points, if there are obstacles, will be eliminated.

2. The method of claim 1, wherein, The adaptive step factor in step 1.2 is determined according to the following formula; wherein is an adaptive step factor, k is a constant scaling factor, and k > 0, is the proportion of the environment area occupied by obstacles.

3. The method of claim 1, wherein, The density D of the tree nodes in the environment space in step 2.2 i is: where n i is the number of nodes currently existing in the ith subspace, C i is the number of existing nodes in the ith subspace.

4. The method of claim 3, wherein, C = C + 1 i is determined according to the following formula; where V i is the volume of the ith subspace, V nodei is the volume required for each tree node, p d is the standard volume of the unit sphere in the global workspace, r i is the distance between nodes within the ith subspace, and a is a constant value, a e (0, 1).

Images