RRT-Connect mechanical arm path planning method fused with beetle beetle algorithm

Abstract

The invention relates to an RRT-Connect mechanical arm path planning method fused with a beetle antennae algorithm, and belongs to the technical field of mechanical arm path planning. The method comprises the following steps: taking a midpoint between a given starting point and a target point as a third node, and if an obstacle exists at the midpoint between the starting point and the target point, selecting one or more unit distance points on a perpendicular bisector of a connecting line of the starting point and the target point to translate upwards or downwards from the midpoint of the connecting line; the RRT-Connect algorithm fused with the beetle antennae can grow four trees from a starting point to a third node, from the third node to the starting point, from the third node to a target point, and from the target point to the third node. And meanwhile, in combination with the beetle antennae algorithm, the odor concentration sensing capability of the left and right antennas of the beetle antennae algorithm is fused into the RRT-Connect algorithm, so that the number of expanded redundant points is reduced, the path length is shortened, and the search efficiency is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm path planning, and relates to a path planning algorithm for a six-degree-of-freedom robot arm in an environment with obstacles, and specifically to an RRT-Connect robot arm path planning method integrating a longicorn beard algorithm. Background Art

[0002] Industrial robots in the manufacturing industry will become the main force of intelligent manufacturing. They can replace people or work with people to complete work. As the application scenarios become more and more complex, higher requirements are placed on the safety and intelligence of robotic arms. Path planning is one of the important contents of robotic arm control, that is, to select a path from the starting point to the target point so that the robot can reach the target point safely and quickly, and ensure that the robot does not collide with obstacles.

[0003] The RRT algorithm can effectively search high-dimensional space and does not require an accurate description of obstacles in the configuration space. Therefore, the RRT algorithm is widely used in the path planning of robotic arms. However, the algorithm has problems such as strong sampling randomness, long search expansion time, and slow convergence. Many scholars have improved it. KUFFNER et al. proposed an improved RRT-Connect algorithm that simultaneously expands two random trees from the starting point and the target point until they are connected together, which speeds up the convergence speed and reduces the search time. However, there are many redundant nodes in the path search process in scenes with obstacles, and the planned path is longer.

[0004] Therefore, a new robot arm path planning method is urgently needed to achieve optimal path planning while reducing the number of redundant nodes. Summary of the invention

[0005] In view of this, the purpose of the present invention is to provide an RRT-Connect robotic arm path planning method integrating the beetle whisker algorithm, by adding a third point between the starting point and the target point, generating four random trees, and performing a bidirectional search at the same time, and secondly introducing the beetle whisker algorithm for node expansion to enhance the target orientation of node expansion and reduce invalid searches until all random trees are connected together to select the optimal path from the starting point to the target point.

[0006] In order to achieve the above object, the present invention provides the following technical solutions:

[0007] A RRT-Connect robot arm path planning method that integrates the longicorn whisker algorithm, taking the midpoint between a given starting point and a target point as the third node, and if there is an obstacle at the midpoint between the starting point and the target point, selecting the midpoint of the distance line on the perpendicular median line connecting the starting point and the target point and translating it upward or downward by one or more unit distance points; so that the RRT-Connect algorithm that integrates the longicorn whisker can grow four trees from the starting point to the third node, the third node to the starting point, the third node to the target point, and the target point to the third node; at the same time, combined with the longicorn whisker algorithm, the ability of the left and right antennae of the longicorn whisker algorithm to sense odor concentration is integrated into the RRT-Connect algorithm. The method specifically includes the following steps:

[0008] Step 1: In the known environment map, set the starting point, target point and obstacles of the robot path planning;

[0009] Step 2: Add the third node, take the midpoint between the starting point and the target point. If there is an obstacle at the midpoint, select a point on the perpendicular median of the line connecting the starting point and the target point that is one or more unit distances up or down from the midpoint of the line.

[0010] Step 3: The starting point is used as the first random tree, the third node with the starting point as the target is used as the second random tree, the target point is used as the third random tree, and the third node with the target point as the target is used as the fourth random tree; the first random tree and the second random tree are bidirectionally extended, and the third random tree and the fourth random tree are bidirectionally extended;

[0011] Step 4: The first to fourth random trees are all re-expanded according to steps 5 and 6 to obtain a planned path;

[0012] Step 5: Treat each node of the random tree as a longhorn beetle. Each longhorn beetle has two tentacles, left and right. Compare the fitness functions of the left and right tentacles, y = f(x), that is, the fitness of the left beard y left and the right whisker fitness y right Size, determines the direction of movement;

[0013] Step 6: Each random tree acquires a new node q along the determined direction according to the step length step of the expanded node and the left and right tentacle coordinate values new , and determine whether there is an obstacle between the new node and the nearest node. If so, delete the new node, change the direction of the starting node Longicorn and return to step 5. Otherwise, add the new node to the path of the random tree until the distance between the new node of the random tree and the new node of the other tree of the bidirectional tree is less than the step length step, and no obstacles are passed, completing the bidirectional expansion of the four trees and entering step 7;

[0014] Step 7: Select the optimal path from the starting point through the third node to the target point avoiding obstacles from the path points of the four extended random trees.

[0015] Furthermore, in step 2, the acquisition of the third node is calculated according to the following formula:

[0016] If the midpoint does not pass through an obstacle:

[0017]

[0018] If the midpoint passes through an obstacle:

[0019]

[0020] Among them, the coordinates of the starting point are (x1, y1), the coordinates of the target point are (x2, y2), the coordinates of the third node are (x3, y3), and n represents the movement of n unit distances.

[0021] Further, in step 5, the coordinates of the left tentacle are: x left =x+d×V;

[0022] The coordinates of the right tentacle are: x right = xd × V;

[0023] Among them, the initial value of x is the center of mass of the longhorn beetle at the initial position, d is the given distance between the left and right antennae, and V is the direction vector of the longhorn beetle.

[0024] Furthermore, in step 5, the expression of the beetle's heading vector V is:

[0025]

[0026] Among them, rands(n,1) represents the random generation of an n-dimensional random vector between 0 and 1, which can randomly change the direction of the longicorn.

[0027] Furthermore, in step 5, the fitness function value of the left whisker is y left =f(x left ), the fitness function value of the right whisker is y right =f(x right );

[0028] When f(x left )<f(x right )hour,

[0029] When f(x left )>f(x right )hour,

[0030] Among them, step represents the moving step of the longhorn beetle, t is the number of iterations, and x t and x t-1They represent the centroid value of x at the t-th iteration and t-1-th iteration, i.e., the position of the expanded new node.

[0031] The beneficial effects of the present invention are:

[0032] (1) By adding a third point, the present invention can generate four random trees at the same time, which can quickly perform path search, reduce the time of path planning, and improve the search efficiency of path planning.

[0033] (2) The present invention regards each node of the four random trees as a longhorn beetle and determines the direction of advance by comparing the fitness function values ​​of the left and right tentacles of the longhorn beetle, thereby enhancing the goal-oriented nature of node expansion and reducing invalid searches.

[0034] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0036] Figure 1 It is a flow chart of the RRT-Connect robot arm path planning method incorporating the longicorn beard algorithm in an example of the present invention;

[0037] Figure 2 A schematic diagram for selecting a third node in an example of the present invention;

[0038] Figure 3 A schematic diagram of generating four random trees in an example of the present invention;

[0039] Figure 4 A planned path map in a two-dimensional map containing obstacles before improvement involved in an example of the present invention;

[0040] Figure 5 It is a planned path map in the improved two-dimensional map containing obstacles involved in the example of the present invention. DETAILED DESCRIPTION

[0041] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0042] Preparation: Figure 4-5 As shown in the figure, the environment map is set to 500mm×500mm, the starting point coordinates are (100, 20), the target point coordinates are (480, 480), the black rectangle represents obstacles, the white area is the feasible area, and the thick line represents the planned feasible path of the robotic arm.

[0043] like Figure 1 As shown, an embodiment of the present invention provides an RRT-Connect robot arm path planning method integrating a longicorn beard algorithm, which specifically includes the following steps:

[0044] Step 1: Initialize the map and determine the starting point, target point, obstacles and parameters.

[0045] Specific parameters include the step size of random tree expansion is 20, and the maximum number of iterations of the algorithm is 2000. Figure 3 As shown in the figure, d is the distance between the left and right whiskers of each longhorn beetle, and step is the line connecting the midpoints of the long sides of the two triangles representing the longhorn beetles.

[0046] Step 2: Add a third node. By calculating the midpoint between the starting point and the target point, if there is no obstacle at the midpoint, the third node is Figure 2 If there is an obstacle at the hollow circle position, the third node is Figure 2 The calculation formula for the solid circle position is as follows:

[0047]

[0048] Step 3: Take the starting point as the first random tree, the third node with the starting point as the target as the second random tree, the target point as the third random tree, and the third node with the target point as the fourth random tree. The first random tree and the second random tree are bidirectionally extended, and the third random tree and the fourth random tree are bidirectionally extended. Each extended node is regarded as a longicorn, such as Figure 3As shown in the figure, the starting point and the target point are the vertices of the two triangles on the left and right sides, respectively. The triangle represents a longhorn beetle. The third node generates two longhorn beetles. The two corners of the long side of the triangle represent the left and right antennae of the longhorn beetle. The circle and rectangle represent obstacles. The dotted area indicates that the distance between the latest nodes of the two trees expanded in both directions is less than the step size and does not pass through obstacles.

[0049] Step 4: Determine the expansion direction by comparing the fitness function values ​​of the left and right tentacles of the starting points, i.e., No. 1, No. 2, No. 3, and No. 4. The fitness function value calculation formula of the left and right tentacles is as follows:

[0050] The coordinates of the left tentacle are: x left =x+d×V;

[0051] The coordinates of the right tentacle are: x right = xd × V;

[0052] Among them, the initial value of x is the center of mass of the longicorn in the initial position;

[0053] The fitness function value of the left whisker is y left =f(x left ), the fitness function value of the right whisker is y right =f(x right ). Compare the size of f(xleft) and f(xright):

[0054] When f(x left )<f(x right )hour,

[0055] When f(x left )>f(x right )hour,

[0056] Among them, t is the number of iterations, x t and x t-1 They represent the centroid value of x at the t-th iteration and the t-1-th iteration, i.e., the position of the new node expanded. Figure 2 As shown, the new node No. 5 Longicorn is expanded by the first random tree, the new node No. 6 Longicorn is expanded by the second random tree, the new node No. 7 Longicorn is expanded by the third random tree, and the new node No. 8 Longicorn is expanded by the fourth random tree.

[0057] Step 5: The latest node expanded by the first random tree and the second random tree is as follows Figure 2 As shown, the distance between No. 5 and No. 9 is less than the step length and there are no obstacles, so they can be directly connected. The same is true for No. 8 and No. 10.

[0058] Step 6: The four random trees can be connected to find the optimal path from the starting point through the third node to the target point.

[0059] Comparative experiment:

[0060] Figure 4 is the feasible path from the starting point to the target point obtained by the RRT-Connect algorithm, Figure 5 is a feasible path from the starting point through the third node to the target point obtained by the RRT-Connect algorithm of the present invention integrating the beetle whisker algorithm. Figure 4 and Figure 5 It can be seen that the improved algorithm reduces the expansion of invalid points, the path length is significantly shortened, and the path optimization is verified.

[0061] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A RRT-Connect robot arm path planning method integrating the longicorn beard algorithm, characterized in that: The midpoint between the given starting point and the target point is taken as the third node. If there is an obstacle at the midpoint between the starting point and the target point, select one or more unit distance points on the perpendicular bisector of the line connecting the starting point and the target point and translate upward or downward from the midpoint of the line; The RRT-Connect algorithm integrated with the beetle whiskers can grow four trees from the starting point to the third node, from the third node to the starting point, from the third node to the target point, and from the target point to the third node; at the same time, combined with the beetle whisker algorithm, the ability of the left and right antennae of the beetle whisker algorithm to perceive odor concentration is integrated into the RRT-Connect algorithm; the method specifically includes the following steps: Step 1: In the known environment map, set the starting point, target point and obstacles of the robot path planning; Step 2: Add the third node, take the midpoint between the starting point and the target point. If there is an obstacle at the midpoint, select a point on the perpendicular median of the line connecting the starting point and the target point that is one or more unit distances up or down from the midpoint of the line. Step 3: The starting point is used as the first random tree, the third node with the starting point as the target is used as the second random tree, the target point is used as the third random tree, and the third node with the target point as the target is used as the fourth random tree; the first random tree and the second random tree are bidirectionally extended, and the third random tree and the fourth random tree are bidirectionally extended; Step 4: The first to fourth random trees are all re-expanded according to steps 5 and 6 to obtain a planned path; Step 5: Treat each node of the random tree as a longhorn beetle. Each longhorn beetle has two tentacles on the left and right. Compare the fitness y of the left tentacles left and the right whisker fitness y right Size, determines the direction of movement; Step 6: Each random tree acquires a new node q along the determined direction according to the step length step of the expanded node and the left and right tentacle coordinate values new , and determine whether there is an obstacle between the new node and the nearest node. If so, delete the new node, change the direction of the starting node Longicorn and return to step 5. Otherwise, add the new node to the path of the random tree until the distance between the new node of the random tree and the new node of the other tree of the bidirectional tree is less than the step length step, and no obstacles are passed, completing the bidirectional expansion of the four trees and entering step 7; Step 7: Select the optimal path from the starting point through the third node to the target point avoiding obstacles from the path points of the four extended random trees.

2. The RRT-Connect robot arm path planning method according to claim 1, characterized in that: In step 2, the third node is obtained by calculating the following formula: If the midpoint does not pass through an obstacle: If the midpoint passes through an obstacle: Among them, the coordinates of the starting point are (x1, y1), the coordinates of the target point are (x2, y2), the coordinates of the third node are (x3, y3), and n represents the movement of n unit distances.

3. The RRT-Connect robot arm path planning method according to claim 1, characterized in that: In step 5, The coordinates of the left tentacle are: x left =x+d×V; The coordinates of the right tentacle are: x right = xd × V; Among them, the initial value of x is the center of mass of the longhorn beetle at the initial position, d is the given distance between the left and right antennae, and V is the direction vector of the longhorn beetle.

4. The RRT-Connect robot arm path planning method according to claim 3, characterized in that: In step 5, the expression of the beetle's heading vector V is: Among them, rands(n,1) represents the random generation of an n-dimensional random vector between 0 and 1, which can randomly change the direction of the longicorn.

5. The RRT-Connect robot arm path planning method according to claim 1, characterized in that: In step 5, the fitness function value of the left whisker is y left =f(x left ), the fitness function value of the right whisker is y right =f(x right ); When f(x left )<f(x right )hour, When f(x left )>f(x right )hour, Among them, step represents the moving step of the longhorn beetle, t is the number of iterations, and x t and x t-1 They represent the centroid value of x at the t-th iteration and t-1-th iteration, i.e., the position of the expanded new node.

Images