Mechanical arm path planning method and system based on RRT*

Abstract

The invention relates to the technical field of path planning, and discloses a mechanical arm path planning method and system based on RRT *, and the method comprises the steps: sampling in a map space to obtain a sampling point, finding a node closest to the current sampling point from an expansion tree, determining the generation direction of a new node in combination with an artificial potential field method, and expanding to generate the new node; judging whether the new node collides with an obstacle or not, if not, adding the new node into the extension tree, and otherwise, re-sampling; constructing a father node according to whether collision occurs or not, performing rewiring, judging whether the distance between a new node and a target node in the extension tree is smaller than a preset distance threshold value or not and whether collision does not occur on a connection path between the two points or not, if yes, ending generation of the new node, and if not, performing resampling; and connecting the initial node, all the new nodes and the target node to obtain an initial planning path, removing redundant nodes in the initial planning path, and performing smoothing processing to obtain a final planning path. According to the invention, environment adaptability, efficiency and path quality can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a method and system for manipulator path planning based on RRT*. Background Art

[0002] With the increasing popularity of robots, multi-degree-of-freedom robotic arms are playing a vital role in various fields due to their flexibility, high precision, and high operability. Path planning, at the core of robotic arms, aims to efficiently find a safe, collision-free path from a starting point to a destination.

[0003] In the prior art, there is a practice of using Rapidly-exploring Random Tree (RRT) for path planning of robotic arms. RRT can efficiently explore high-dimensional space and has the advantages of high computational efficiency and simple implementation. However, RRT has the problems of non-optimal path and slow convergence. RRT* is an improvement based on RRT. It is an asymptotically optimal path planning method that reduces the path cost by reselecting and connecting the surrounding nodes with the lowest cost; however, RRT* has the problems of high computational complexity, slow convergence speed and uneven generated path. The uneven path is an important factor affecting the path quality. Especially in the working process of agricultural robotic arms, the smoothness of the path directly affects the stability and accuracy of the robotic arm's execution.

[0004] To improve the problems of RRT*, many improvement studies have been conducted: for example, limiting the sampling space to the elliptical region of the initial path to improve path optimization efficiency, improving the efficiency of robot path planning through accumulator sampling node selection and adaptive step-size adjustment mechanisms, improving search efficiency and path quality by combining artificial potential field methods to avoid blind expansion, and controlling the sampling direction and spatial distribution by incorporating dynamic step-size adjustment mechanisms to avoid local optimality. Although these studies can improve the problems of RRT* to a certain extent, they still suffer from low efficiency, high computational complexity, and unstable path quality in different usage environments. Summary of the Invention

[0005] To this end, the technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide a robot arm path planning method and system based on RRT*, which can improve adaptability to complex environments, reduce computational complexity, and improve efficiency and path quality.

[0006] To solve the above technical problems, the present invention provides a robot arm path planning method based on RRT*, comprising:

[0007] Get the map space, starting node and target node of the robot arm, build and initialize the expansion tree;

[0008] Sampling points in the map space, finding the node closest to the current sampling point in the expansion tree, determining the generation direction of new nodes on the planned path by combining the artificial potential field method, and expanding along the generation direction to generate new nodes;

[0009] Determine whether there is an obstacle on the connection path between the new node and the previous new node; if there is no collision, add the new node to the expansion tree; otherwise, return to the step of sampling in the map space to obtain a sampling point;

[0010] Constructing a parent node and rewiring according to whether an obstacle will be collided with, thereby optimizing the expansion tree;

[0011] Determine whether the following conditions are met: the distance between the new node in the expanded tree and the target node is less than a preset distance threshold and there is no collision with an obstacle on the connection path between the new node and the target node. If so, terminate the generation of the new node; otherwise, return to the step of sampling in the map space to obtain a sampling point.

[0012] Connect the starting node, all new nodes in the expanded tree at this time, and the target node in sequence to obtain the initial planned path;

[0013] The redundant nodes in the initial planned path are removed, and the planned path after removing the redundant nodes is smoothed to obtain the final planned path.

[0014] Furthermore, when sampling in the map space to obtain sampling points, the sampling points are obtained by an adaptive target bias strategy, specifically:

[0015] Set the target bias threshold, denoted as p, and generate a random number, denoted as rand, when generating sampling points in the expansion tree; if rand ≥ p, obtain the sampling point through random sampling, otherwise use the target node as the sampling point;

[0016] The target bias threshold is calculated as follows:

[0017]

[0018] Among them, α is the coefficient that controls the growth rate, d(x,x goal ) represents the distance between the current sampling point and the target node.

[0019] Furthermore, when the artificial potential field method is used to determine the generation direction of a new node on the planned path, the generation direction vector of the new node is:

[0020] F total =F goal +F obs+F rand , where F total is the direction vector of the new node, F goal is the target node gravity, F obs is the obstacle repulsion, F rand is the gravity of random sampling points;

[0021] The calculation method of the random sampling point gravity is:

[0022] F rand =-k rand (xx rand ), where k rand Indicates the gravity gain coefficient of the random sampling point, x represents the current sampling point, x rand represents a random sampling point;

[0023] The calculation method of the gravity gain coefficient of the random sampling point is:

[0024] k rand =k rand_max e -λt ,

[0025] Among them, k rand_max represents k rand The initial value of , λ is the attenuation coefficient, and t is the current number of iterations.

[0026] Furthermore, the target node gravity is calculated as follows:

[0027] F goal =-k att (xx goal ),

[0028] Among them, F goal is the target node gravity, k att is the gravitational gain coefficient, x goal Indicates the target node;

[0029] The calculation method of the gravity gain coefficient is:

[0030] k att =k att_base (1+α att d(x,x goal )),

[0031] Among them, k att_base represents the initial gravitational gain coefficient value, α att is the gravitational adjustment coefficient, d(x,x goal ) represents the distance between the current sampling point and the target node.

[0032] Furthermore, the calculation method of the obstacle repulsion is:

[0033]

[0034] Among them, F obs represents the obstacle repulsion, k rep represents the repulsion gain coefficient, d(x,x obs ) represents the distance between the current sampling point and the nearest obstacle, d0 represents the influence range of the obstacle, x obs Indicates the obstacle closest to the current sampling point.

[0035] Furthermore, the repulsion gain coefficient is calculated as follows:

[0036]

[0037] Among them, k rep_base represents the initial repulsion gain coefficient value, ρ obs Indicates the obstacle density within the current sampling point unit, α rep and β rep is the repulsion adjustment coefficient.

[0038] Furthermore, when a new node is generated by expanding along the generation direction, the new node is:

[0039] x new =x near +step*F total ,

[0040] Among them, x new is a new node, x near is the current sampling point, step is the expansion step, F total The direction vector for generating the new node;

[0041] The calculation method of the expansion step length is:

[0042]

[0043] Among them, step max is the initial step size, η1, η2, η3 are the coefficients for adjusting the influence of each parameter on the sampling step size, d(x,x goal ) represents the distance between the current sampling point and the target node, ρ obs Indicates the obstacle density within the current sampling point unit, d narrow Indicates the shortest distance between obstacles near the current sampling point.

[0044] Furthermore, the parent node is constructed and rewired according to whether an obstacle will be collided, specifically:

[0045] Let the new node currently expanded be x new , generate xnew The nearest sampling point is x near , from x near Start tracing back along its parent node and find the node with x new The furthest ancestor node x1 that is connected without causing collision, and the parent node of x1 is x2;

[0046] Using x2 and x new Construct a rectangle as the diagonal point, select a vertex in the rectangle away from the obstacle, record it as x np1 , ensure x np1 with x new The line connecting does not pass through obstacles;

[0047] In x np1 with x new On the line connecting np2 , so that x np2 The line connecting x2 does not collide with obstacles; if x np2 The line connecting to x2 collides with an obstacle, and then np1 with x new Use binary search on the line until a point that meets the conditions is found as x np2 ;

[0048] Note that the parent node of x2 is x3. np2 On the line connecting x and x2, find a point x by bisection. np3 , so that x np3 The line connecting x3 does not collide with obstacles; if x np3 The line connecting to x3 collides with an obstacle, and then np3 Use binary search on the line connecting x2 until a point that meets the conditions is found as x np3 ;

[0049] x np3 As x new Construct the parent node and rewire. If no point x that meets the conditions can be found, np3 , then use the RRT* rewiring method.

[0050] Furthermore, the removal of redundant nodes in the initial planned path is specifically as follows:

[0051] Obtain a start point and an end point on an initial planning path, connect the start point and the end point, and determine whether a connection path of the start point and the end point collides with an obstacle; if no collision occurs, remove points on the initial planning path except the start point and the end point as redundant nodes; if a collision occurs, backtrack from the end point to a parent node, check whether a connection line between the start point and the backtracking point collides with the obstacle, remove points between the start point and a first backtracking point without collision as redundant nodes, take the first backtracking point without collision as a direct child node of the start point, take the direct child node as a new start point, and return to the step of connecting the start point and the end point until the new start point is the end point.

[0052] The application further provides a mechanical arm path planning system based on RRT*, comprising:

[0053] A data initialization module is configured to obtain a map space, a start node and a target node in which a mechanical arm works, and construct and initialize an extended tree.

[0054] A sampling module is configured to sample a sampling point in the map space.

[0055] A new node generation module is configured to find a node closest to the current sampling point from the extended tree, determine a generation direction of a new node on a planning path by combining an artificial potential field method, and generate the new node along the generation direction.

[0056] An extended tree calculation module is configured to determine whether a connection path between the new node and a last new node collides with an obstacle, add the new node to the extended tree if no collision occurs, or make the sampling module execute the step of sampling a sampling point in the map space if a collision occurs.

[0057] An extended tree optimization module is configured to construct a parent node and rewire according to whether a collision with the obstacle occurs, and optimize the extended tree.

[0058] A new node end generation module is configured to determine whether a distance between the new node and the target node in the extended tree is less than a preset distance threshold and a connection path between the new node and the target node does not collide with the obstacle, end the generation of the new node if the determination is yes, or make the sampling module execute the step of sampling a sampling point in the map space if the determination is no.

[0059] An initial planning path generation module is configured to connect the start node, all new nodes in the extended tree at this time and the target node in sequence to obtain an initial planning path.

[0060] A planning path optimization module is configured to remove redundant nodes in the initial planning path, and perform smoothing processing on the planning path after the removal of the redundant nodes to obtain a final planning path.

[0061] The above technical solution of the present invention has the following beneficial effects compared with the prior art:

[0062] The present invention uses an artificial potential field method to effectively guide the path to avoid obstacles and toward the target point, thereby improving overall obstacle avoidance capabilities, reducing path search time, and increasing efficiency. By determining whether there will be a collision with an obstacle, the parent node is constructed and rewired, reducing computational complexity and improving adaptability to complex environments. By removing redundant points and reducing unnecessary nodes in the generated path, the continuity of the path and the executability of the trajectory are improved through smoothing processing, thereby improving path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below based on specific embodiments of the present invention in conjunction with the accompanying drawings, wherein:

[0064] Figure 1 Flowchart of the method in the preferred embodiment of the present invention.

[0065] Figure 2 1 is a step diagram of a method in a preferred embodiment of the present invention.

[0066] Figure 3 Schematic diagram of path planning results in a two-dimensional simple map environment in a simulation experiment in a preferred embodiment of the present invention.

[0067] Figure 4 Schematic diagram of path planning results in a two-dimensional complex map environment in a simulation experiment in a preferred embodiment of the present invention.

[0068] Figure 5 Schematic diagram of path planning results in a three-dimensional simple map environment in a simulation experiment in a preferred embodiment of the present invention.

[0069] Figure 6 Schematic diagram of path planning results in a three-dimensional complex map environment in a simulation experiment in a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0070] The present invention will be further described below with reference to the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it. However, the embodiments are not intended to limit the present invention.

[0071] Reference Figure 1 、 Figure 2 As shown, the present invention discloses a robot arm path planning method based on RRT*, comprising the following steps:

[0072] S1: Get the map space, starting node and target node where the robot arm works, and build and initialize the expansion tree.

[0073] S2: Sample in the map space to obtain sampling points.

[0074] In this embodiment, the sampling points are obtained through an adaptive target bias strategy, specifically:

[0075] Set the target bias threshold, denoted as p, and generate a random number when expanding the tree to generate a sampling point, denoted as rand; if rand ≥ p, then obtain the sampling point by random sampling, denoted as x rand Otherwise, the target node is recorded as x goal As sampling points;

[0076] The target bias threshold is calculated as follows:

[0077]

[0078] Among them, α is the coefficient that controls the growth rate, d(x,x goal ) represents the distance between the current sampling point and the target node. By setting the adaptive target bias threshold p, it can better adapt to environmental changes.

[0079] S3: Find the node closest to the current sampling point from the expansion tree, and determine the generation direction of the new node on the planned path by combining the artificial potential field method.

[0080] S3-1: When combining the artificial potential field method to determine the generation direction of new nodes on the planned path, the gravitational field and repulsive field are improved.

[0081] The method of constructing the gravitational field is:

[0082]

[0083] Among them, U att (x) is the gravitational potential field function of the gravitational field, k att is the gravitational gain coefficient, d(x,x goal ) represents the distance between the current sampling point and the target node.

[0084] The method of constructing the repulsive field is:

[0085]

[0086] Among them, U rep (x) is the repulsive potential field function of the repulsive field, k rep represents the repulsion gain coefficient, d(x,x obs ) represents the distance between the current sampling point and the nearest obstacle, and d0 represents the influence range of the obstacle.

[0087] S3-2: Calculate the target node's attraction and obstacle repulsion

[0088] According to the gravitational potential field function U att (x) Calculate the target node gravity as:

[0089]

[0090] Among them, F goal is the target node gravity, k att is the gravitational gain coefficient, x is the current sampling point (i.e. x near ), x goal Represents the target node. The calculation method of the gravity gain coefficient is:

[0091] k att =k att_base (1+α att d(x,x goal )),

[0092] Among them, k att_base represents the initial gravitational gain coefficient value, α att is the gravity adjustment coefficient, used to control the distance between the target point and k att The influence weight of d(x,x goal ) represents the distance between the current sampling point and the target node.

[0093] The obstacle repulsion is calculated based on the repulsive potential field function:

[0094]

[0095] Among them, F obs represents the obstacle repulsion, k rep represents the repulsion gain coefficient, d(x,x obs ) represents the distance between the current sampling point and the nearest obstacle, d0 represents the influence range of the obstacle, x represents the current sampling point, x obs Indicates the obstacle closest to the current sampling point; when the range is greater than d0, the repulsion is negligible. The calculation method of the repulsion gain coefficient is:

[0096]

[0097] Among them, k rep_base represents the initial repulsion gain coefficient value, ρ obs Indicates the obstacle density within the current sampling point unit (i.e. unit area or volume), α rep and β rep is the repulsion adjustment coefficient, which is used to control the distance and density k rep The influence weight of .

[0098] S3-3: Calculate the gravity of random sampling points as:

[0099] F rand =-krand (xx rand ),

[0100] Among them, k rand Indicates the gravity gain coefficient of the random sampling point, x represents the current sampling point, x rand represents a random sampling point; the calculation method of the gravity gain coefficient of the random sampling point is:

[0101] k rand =k rand_max e -λt ,

[0102] Among them, k rand_max Represents the maximum random gravity gain coefficient, i.e. k rand The initial value of λ is the attenuation coefficient, and the control k rand The decay rate of ; t is the current number of iterations.

[0103] S3-4: The movement direction of the new node is affected by the obstacle repulsion F obs , target node gravity F goal , random sampling point gravity F rand Under the combined influence of the three forces, the direction vector of the new node is:

[0104] F total =F goal +F obs +F rand ,

[0105] Among them, F total is the direction vector of the new node, F goal is the target node gravity, F obs is the obstacle repulsion, F rand is the gravity of the random sampling point.

[0106] S4: Expand along the generation direction to generate a new node. The new node is:

[0107] x new =x near +step*F total ,

[0108] Among them, x new is a new node, x near is the current sampling point, step is the expansion step, F total The direction vector for generating new nodes.

[0109] The expansion step size is an adaptive step size, and the calculation method is:

[0110]

[0111] Among them, stepmax is the initial step size, η1, η2, η3 are the coefficients for adjusting the influence of each parameter on the sampling step size, d(x,x goal ) represents the distance between the current sampling point and the target node, ρ obs Indicates the obstacle density within the current sampling point unit (i.e., unit area or volume), that is, the ratio of the area (two-dimensional) or volume (three-dimensional) occupied by obstacles within a certain range around x, d narrow Indicates the shortest distance between obstacles near the current sampling point, that is, the narrow space that exists.

[0112] S5: Determine whether there is an obstacle on the connection path between the new node and the previous new node. If there is no collision, add the new node to the expansion tree; otherwise, return to S2.

[0113] S6: Constructing a parent node and rewiring according to whether an obstacle will be collided with, thereby optimizing the expansion tree.

[0114] S6-1: Let the new node currently being expanded be x new , generate x new The nearest sampling point is x near , from x near Start tracing back along its parent node and find the node with x new The furthest ancestor node x1 that is connected without causing collision, and the parent node of x1 is x2.

[0115] S6-2: Using x2 and x new Construct a rectangle as the diagonal point, select a vertex in the rectangle away from the obstacle, record it as x np1 , ensure x np1 with x new The connecting line does not pass through obstacles.

[0116] S6-3: At x np1 with x new On the line connecting np2 , so that x np2 The line connecting x2 does not collide with obstacles; if x np2 The line connecting to x2 collides with an obstacle, and then np1 with x new Use binary search on the line until a point that meets the conditions is found as x np2 .

[0117] S6-4: Let x2’s parent be x3. np2 On the line connecting x and x2, find a point x by bisection. np3 , so that x np3 The line connecting x3 does not collide with obstacles; if xnp3 The line connecting to x3 collides with an obstacle, and then np3 Use binary search on the line connecting x2 until a point that meets the conditions is found as x np3 .

[0118] S6-5: x np3 As x new Construct the parent node and rewire. If no point x that meets the conditions can be found, np3 , then the construction of the parent node fails, and the RRT* rewiring method is used.

[0119] S7: Calculate the distance between the new node and the target node in the expanded tree and determine whether the following conditions are met: the distance is less than a preset distance threshold and the path connecting the new node and the target node does not collide with any obstacles. If so, the new node generation ends; otherwise, the process returns to S2. A maximum number of iterations is also set in this step. If the maximum number of iterations is reached, or if the distance is less than a preset threshold and the path connecting the new node and the target node does not collide with any obstacles, the new node generation ends.

[0120] S8: Connect the starting node, all new nodes in the expanded tree at this time, and the target node in sequence to obtain the initial planned path.

[0121] S9: Use a greedy strategy to remove redundant nodes in the initial planned path.

[0122] S9-1: Get the starting point and end point of the initial planning path.

[0123] S9-2: Connect the starting point and the end point, and determine whether there is a collision with an obstacle on the connection path between the starting point and the end point; if there is no collision, execute S9-3; if there is a collision, execute S9-4.

[0124] S9-3: Remove points other than the starting point and the end point on the initial planned path as redundant nodes, and execute S9-5.

[0125] S9-4: Starting from the end point, backtrack upward along its parent node, and check whether the line between the starting point and the backtracking point collides with obstacles. The first backtracking point found without collision is used as the direct child node of the starting point. The points between the starting point and the direct child node on the initial planned path are removed as redundant nodes. The found direct child node is used as the new starting point. Return to execute S9-2 until the new starting point is the end point and execute S9-5.

[0126] S9-5: Connect all remaining nodes at this point to form an efficient path from the starting point to the end point after removing redundant nodes.

[0127] S10: Smoothing the planned path after removing redundant nodes. In this embodiment, the smoothing method used is B-spline curve smoothing. After three B-spline curve smoothings, the final planned path is obtained.

[0128] The present invention also discloses a robot arm path planning system based on RRT*, comprising:

[0129] The data initialization module is used to obtain the map space, starting node and target node of the robot arm, and to build and initialize the expansion tree;

[0130] Sampling module, used to sample in the map space to obtain sampling points;

[0131] A new node generation module is used to find the node closest to the current sampling point in the expansion tree, determine the generation direction of the new node on the planned path by combining the artificial potential field method, and expand along the generation direction to generate the new node;

[0132] An expansion tree calculation module is used to determine whether there is an obstacle on the connection path between the new node and the previous new node. If there is no collision, the new node is added to the expansion tree. Otherwise, the sampling module executes the step of sampling in the map space to obtain sampling points;

[0133] An expansion tree optimization module, configured to construct a parent node and rewire the network based on whether the node will collide with an obstacle, thereby optimizing the expansion tree;

[0134] A new node end generation module is used to determine whether the following conditions are met: the distance between the new node in the expansion tree and the target node is less than a preset distance threshold and there is no obstacle on the connection path between the new node and the target node; if so, the generation of the new node is terminated; otherwise, the sampling module executes the step of sampling in the map space to obtain a sampling point;

[0135] An initial planning path generation module is used to sequentially connect the starting node, all new nodes in the expanded tree at this time, and the target node to obtain the initial planning path;

[0136] The planning path optimization module is used to remove redundant nodes in the initial planning path, smooth the planning path after removing redundant nodes, and obtain the final planning path.

[0137] The present invention also discloses a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for planning a robot path based on RRT* is implemented.

[0138] The present invention also discloses a device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements a robot arm path planning method based on RRT* when executing the computer program.

[0139] Compared with the prior art, the advantages of the present invention are:

[0140] 1. The artificial potential field method is used to effectively guide the path to avoid obstacles and toward the target point, thereby improving the overall obstacle avoidance capability, reducing the path search time, and improving efficiency.

[0141] 2. By judging whether there will be collision with obstacles, the parent node is constructed and rewired to reduce the computational complexity and optimize the path, thus improving the adaptability to complex environments.

[0142] 3. After the path is planned, since there are still unnecessary nodes in the path, redundant points are removed to reduce the unnecessary nodes in the generated path, and the continuity and trajectory executability of the path are improved through smoothing, making the path more suitable for the motion trajectory of the robot arm during work and improving the path quality.

[0143] 4. By introducing adaptive target bias to improve sampling efficiency, the target point is directly used as the sampling point with a certain probability, which accelerates the convergence of the path to the target point, significantly shortens the path search time, and further improves the path planning efficiency.

[0144] 5. By combining the adaptive step size and artificial potential field method to guide the generation process of new nodes, new nodes are expanded with a reasonable step size under the joint influence of obstacles, target points, and random sampling points, thereby enhancing the adaptability to complex environments.

[0145] In order to further illustrate the advantages of the present invention, four different environmental spaces, namely, two-dimensional simple, two-dimensional complex, three-dimensional simple, and three-dimensional complex, were constructed in Matlab in this embodiment. 1,000 path planning simulation experiments were carried out in four environments using the method of the present invention and RRT*, AS-RRT*, and APF-RRT* respectively. Among them, AS-RRT* is a method that adds accumulator sampling node selection and adaptive step adjustment mechanism on the basis of RRT*, which can improve the efficiency, quality and obstacle avoidance capability of the robot arm path planning. APF-RRT* is a method that combines the artificial potential field method on the basis of RRT*, which can avoid blind expansion, improve search efficiency and path quality.

[0146] The size of the two-dimensional map constructed in MATLAB is 1000*1000, the starting point is (0, 0), and the end point is (999, 999); the size of the three-dimensional map is 1000*1000*1000, the starting point is (0, 0, 0), and the end point is (700, 900, 900); obstacles such as ellipses, rectangles, spheres or cylinders are set in different maps.

[0147] Initialization parameters include: initial target bias threshold p is set to 0.3, target bias threshold control growth rate coefficient α=0.5; initial step size stepmax Set to 2, adjust the size of each parameter on the sampling step impact coefficient η1=1, η2=1, η3=1; initial attractive gain coefficient k att_base Set to 1, attractive adjustment coefficient α att =1; initial repulsive gain coefficient k rep_base Set to 10, repulsive adjustment coefficient α rep =1, β rep =1; in two-dimensional map, the maximum random attractive gain coefficient k rand_max Set to 1, decay coefficient λ=1; limit the maximum iteration number to 5000, the preset distance threshold in S7 is 1; in three-dimensional map, the maximum random attractive gain coefficient k rand_max Set to 10.

[0148] Figure 3 、 Figure 4 、 Figure 5 、 Figure 6 Respectively represent the path planning results of the method in the two-dimensional simple, two-dimensional complex, three-dimensional simple, three-dimensional complex four kinds of maps, wherein the solid line represents the expansion process of the expansion tree when planning the path, and the dotted line represents the finally planned path.

[0149] The results of the method and RRT*, AS-RRT*, APF-RRT* after 1000 times of path planning in the four kinds of environments are shown in Table 1.

[0150] Table 1 Comparison table of path planning results of different methods in four kinds of maps

[0151]

[0152] From Table 1, it can be seen that no matter in which map environment, the average path length, average running time, average node number, average iteration number, and success rate of the application are better than those of other methods, thereby proving the advantages of the application.

[0153] Those skilled in the art should understand that the embodiments of the application can be provided as a method, a system, or a computer program product. Therefore, the application can adopt a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the application can adopt the form of a computer program product implemented on one or more computer usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer usable program code.

[0154] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0155] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0156] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 one or more flow or blocks

[0157] Obviously, the above-described embodiments are only examples for clarity of description and are not limiting on the embodiments. Based on the above description, one of ordinary skill in the art can further make other different forms of changes or modifications. Here, it is not necessary or possible to enumerate all the embodiments. The obvious changes or modifications derived therefrom are still within the protection scope of the present application.

Claims

1. A robot arm path planning method based on RRT*, characterized in that: include: Get the map space, starting node and target node of the robot arm, build and initialize the expansion tree; Sampling points in the map space, finding the node closest to the current sampling point in the expansion tree, determining the generation direction of new nodes on the planned path by combining the artificial potential field method, and expanding along the generation direction to generate new nodes; Determine whether there is an obstacle on the connection path between the new node and the previous new node; if there is no collision, add the new node to the expansion tree; otherwise, return to the step of sampling in the map space to obtain a sampling point; Constructing a parent node and rewiring according to whether an obstacle will be collided with, thereby optimizing the expansion tree; Determine whether the following conditions are met: the distance between the new node in the expanded tree and the target node is less than a preset distance threshold and there is no collision with an obstacle on the connection path between the new node and the target node. If so, terminate the generation of the new node; otherwise, return to the step of sampling in the map space to obtain a sampling point. Connect the starting node, all new nodes in the expanded tree at this time, and the target node in sequence to obtain the initial planned path; The redundant nodes in the initial planned path are removed, and the planned path after removing the redundant nodes is smoothed to obtain the final planned path.

2. The RRT*-based robot arm path planning method according to claim 1, characterized in that: When sampling in the map space to obtain sampling points, the sampling points are obtained by an adaptive target bias strategy, specifically: Set the target bias threshold, denoted as p, and generate a random number, denoted as rand, when generating sampling points in the expansion tree; if rand ≥ p, obtain the sampling point through random sampling, otherwise use the target node as the sampling point; The target bias threshold is calculated as follows: Among them, α is the coefficient that controls the growth rate, d(x,x goal ) represents the distance between the current sampling point and the target node.

3. The RRT*-based robot arm path planning method according to claim 1, characterized in that: When the artificial potential field method is combined to determine the generation direction of the new node on the planned path, the generation direction vector of the new node is: F total =F goal +F obs +F rand , Among them, F total is the direction vector of the new node, F goal is the target node gravity, F obs is the obstacle repulsion, F rand is the gravity of random sampling points; The calculation method of the random sampling point gravity is: F rand =-k rand (x-x rand ), Among them, k rand Indicates the gravity gain coefficient of the random sampling point, x represents the current sampling point, x rand represents a random sampling point; The calculation method of the gravity gain coefficient of the random sampling point is: k rand =k rand_max e -λt , Among them, k rand_max represents k rand The initial value of , λ is the attenuation coefficient, and t is the current number of iterations.

4. The RRT*-based robot arm path planning method according to claim 3, characterized in that: The calculation method of the target node gravity is: F goal =-k att (x-x goal ), Among them, F goal is the target node gravity, k att is the gravitational gain coefficient, x goal Indicates the target node; The calculation method of the gravity gain coefficient is: k att =k att_base (1+α att d(x,x goal )), Among them, k att_base represents the initial gravitational gain coefficient value, α att is the gravitational adjustment coefficient, d(x,x goal ) represents the distance between the current sampling point and the target node.

5. The RRT*-based robot arm path planning method according to claim 3, characterized in that: The calculation method of the obstacle repulsion is: Among them, F obs represents the obstacle repulsion, k rep represents the repulsion gain coefficient, d(x,x obs ) represents the distance between the current sampling point and the nearest obstacle, d0 represents the influence range of the obstacle, x obs Indicates the obstacle closest to the current sampling point.

6. The RRT*-based robot arm path planning method according to claim 5, characterized in that: The calculation method of the repulsion gain coefficient is: Among them, k rep_base represents the initial repulsion gain coefficient value, ρ obs Indicates the obstacle density within the current sampling point unit, α rep and β rep is the repulsion adjustment coefficient.

7. The RRT*-based robot arm path planning method according to claim 1, characterized in that: When a new node is generated by expanding along the generation direction, the new node is: x new =x near +step*F total , Among them, x new is a new node, x near is the current sampling point, step is the expansion step, F total The direction vector for generating the new node; The calculation method of the expansion step length is: Among them, step max is the initial step size, η1, η2, η3 are the coefficients for adjusting the influence of each parameter on the sampling step size, d(x,x goal ) represents the distance between the current sampling point and the target node, ρ obs Indicates the obstacle density within the current sampling point unit, d narrow Indicates the shortest distance between obstacles near the current sampling point.

8. The RRT*-based robot arm path planning method according to claim 1, characterized in that: The parent node is constructed and rewired according to whether there will be a collision with an obstacle, specifically: Let the new node currently expanded be x new , generate x new The nearest sampling point is x near , from x near Start tracing back along its parent node and find the node with x new The furthest ancestor node x1 that is connected without causing collision, and the parent node of x1 is x2; Using x2 and x new Construct a rectangle as the diagonal point, select a vertex in the rectangle away from the obstacle, record it as x np1 , ensure x np1 with x new The line connecting does not pass through obstacles; In x np1 with x new On the line connecting np2 , so that x np2 The line connecting x2 does not collide with obstacles; if x np2 The line connecting to x2 collides with an obstacle, and then np1 with x new Use binary search on the line until a point that meets the conditions is found as x np2 ; Note that the parent node of x2 is x3. np2 On the line connecting x and x2, find a point x by bisection. np3 , so that x np3 The line connecting x3 does not collide with obstacles; if x np3 The line connecting to x3 collides with an obstacle, and then np3 Use binary search on the line connecting x and x2 until a point that meets the conditions is found as x np3 ; x np3 As x new Construct the parent node and rewire. If no point x that meets the conditions can be found, np3 , then use the RRT* rewiring method.

9. The RRT*-based robot arm path planning method according to any one of claims 1 to 8, characterized in that: The method of removing redundant nodes in the initial planned path is specifically as follows: Get the starting point and end point on the initial planning path, connect the starting point and the end point, and determine whether there is a collision with an obstacle on the connection path between the starting point and the end point; if there is no collision, remove the points on the initial planning path except the starting point and the end point as redundant nodes; if there is a collision, start from the end point and backtrack upward along its parent node, check in turn whether the connection between the starting point and the backtracking point collides with the obstacle, and use the first non-collision-free backtracking point as the direct child node of the starting point, remove the point between the starting point and the direct child node on the initial planning path as a redundant node, use the direct child node found as the new starting point, and return to the step of connecting the starting point and the end point until the new starting point is the end point.

10. A robotic arm path planning system based on RRT*, characterized in that: include: The data initialization module is used to obtain the map space, starting node and target node of the robot arm, and to build and initialize the expansion tree; Sampling module, used to sample in the map space to obtain sampling points; A new node generation module is used to find the node closest to the current sampling point in the expansion tree, determine the generation direction of the new node on the planned path by combining the artificial potential field method, and expand along the generation direction to generate the new node; An expansion tree calculation module is used to determine whether there is an obstacle on the connection path between the new node and the previous new node. If there is no collision, the new node is added to the expansion tree. Otherwise, the sampling module executes the step of sampling in the map space to obtain sampling points; An expansion tree optimization module, configured to construct a parent node and rewire the network based on whether the node will collide with an obstacle, thereby optimizing the expansion tree; A new node end generation module is used to determine whether the following conditions are met: the distance between the new node in the expansion tree and the target node is less than a preset distance threshold and there is no obstacle on the connection path between the new node and the target node; if so, the generation of the new node is terminated; otherwise, the sampling module executes the step of sampling in the map space to obtain a sampling point; An initial planning path generation module is used to sequentially connect the starting node, all new nodes in the expanded tree at this time, and the target node to obtain the initial planning path; The planning path optimization module is used to remove redundant nodes in the initial planning path, smooth the planning path after removing redundant nodes, and obtain the final planning path.

Images