A robotic arm path planning method based on A* algorithm with variable grid

Abstract

The present invention relates to a robotic arm path planning method based on the A* algorithm with a variable grid, comprising the following steps: S1, obtaining the position and posture of the starting point and target point of the robotic arm end effector; S2, rasterizing the workspace between the starting point and target point, and optimizing the size and distribution of the grid using a particle swarm algorithm; S3, using the A* algorithm to perform path planning based on the grid optimized in step S2, to obtain all path points of the robotic arm end effector; S4, smoothing the lines connecting the path points to obtain the final path of the robotic arm end effector. Compared with existing technologies, this invention can improve the efficiency of the algorithm while maintaining trajectory planning accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm path planning, and in particular relates to a robot arm path planning method based on a variable grid A* algorithm. Background Art

[0002] In recent years, traditional agriculture has gradually evolved into modern intelligent agriculture. Intelligent harvesting, which uses harvesting tools mounted on robotic arms to harvest fruit, can adapt to harsh agricultural working environments and reduce manual labor intensity and costs. Path planning algorithms are the core technology of intelligent harvesting and play a decisive role in the efficiency and quality of intelligent harvesting operations. Robotic arm path planning algorithms can be divided into those based on swarm intelligence optimization theory, those based on optimality theory, and those based on graph search methods. Among these, the A* algorithm, a graph search-based algorithm, is one of the most effective methods for finding the shortest path in a static environment. Traditional A* algorithms are primarily used for path planning in two-dimensional planes. Chinese invention patent CN106527151B discloses a path search method for a six-degree-of-freedom spatial robotic arm with a load. After selecting a search step size, the dynamic load capacity workspace of the six-degree-of-freedom spatial robotic arm is rasterized. The dynamic load capacity value of each small cube after rasterization is calculated to complete the rasterization of the robotic arm's dynamic load capacity workspace. However, this method can pose problems such as a large number of nodes to be traversed during the robotic arm path planning process. Therefore, it is necessary to design a robotic arm path planning algorithm that can reduce the number of traversed nodes and speed up operation efficiency while ensuring the accuracy of path planning. Summary of the Invention

[0003] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a robot arm path planning method based on the A* algorithm with a variable grid, which can speed up the efficiency of the algorithm operation while ensuring the accuracy of the robot arm path planning.

[0004] The purpose of the present invention can be achieved by the following technical solutions:

[0005] A method for manipulator path planning based on an A* algorithm with a variable grid comprises the following steps:

[0006] S1, obtain the position and posture of the starting point and target point of the end effector of the robot arm;

[0007] S2, rasterize the workspace between the starting point and the target point, and use the particle swarm algorithm to optimize the size and distribution of the grid;

[0008] S3. Based on the grid optimized in step S2, the A* algorithm is used for path planning to obtain all the path points of the end effector of the robotic arm;

[0009] S4. Smooth the lines connecting the path points to obtain the final path of the end effector of the robotic arm.

[0010] Furthermore, the process of rasterizing the workspace is as follows: the obstacle is enclosed by a cube, the space near the obstacle is divided into a small grid with a side length of b, and the rest of the space is divided into a large grid with a side length of a, where a>b.

[0011] Furthermore, the particle swarm optimization algorithm is used to optimize the size and distribution of the grid, which includes the following steps:

[0012] S201. Setting the objective function Where V is the volume of the entire workspace, t is the time to search a grid, and c is the number of small grids; set the number of swarm particles to m and the particle dimension to 3;

[0013] S202, randomly initialize the speed and position of each particle in the population;

[0014] S203, comparing the objective function values ​​of each possible solution during iteration, and updating the optimal position of each particle and the optimal position of the particle population;

[0015] S204, updating the speed and position of each particle;

[0016] S205: Determine whether the iteration termination condition is met. If so, proceed to step S206; if not, return to step S203;

[0017] S206: Output a, b, and c to achieve grid optimization.

[0018] Furthermore, the update formula for each particle's velocity and position is as follows:

[0019]

[0020]

[0021] Where k is the current iteration number, is the velocity of the i-th particle at the k-th iteration, is the position of the i-th particle at the k-th iteration, w is the inertia weight, r1 and r2 are random numbers distributed in the interval [0,1], is the optimal position of the i-th particle at the k-th iteration, is the optimal position of the particle population at the kth iteration, c1 and c2 are constants.

[0022] Furthermore, the specific process of step S3 is as follows:

[0023] S301. Set the grid size and distribution of the workspace according to the optimization result of step S2, input the coordinates of the starting point, the target point, and the obstacle, and initialize the open list, close list, and point list. The open list is used to record all nodes considered for finding the shortest path, the close list is used to record nodes no longer considered, and the point list is used to record the final planned path nodes.

[0024] S302: Set the parent node. Initially, set the starting point A as the parent node. Then, set the node with the smallest cost in the open list as the new parent node.

[0025] S303, traverse the adjacent child nodes around the parent node in sequence;

[0026] S304: Determine whether all adjacent child nodes of the current parent node have been traversed. If not, return to step S303; if so, proceed to step S305.

[0027] S305. Determine whether the target point appears in the open list. If so, search for the parent node from the target point in sequence until the starting point is found, form a point list, and obtain the three-dimensional coordinate values ​​of the path points of the optimal trajectory of the end effector of the robot arm; if not, determine whether the open list is empty. If the open list is not empty, select the node with the smallest cost in the open list, delete the node from the open list, add it to the close list, and return to step S302; if the open list is empty, the path planning fails and the program terminates.

[0028] Furthermore, the specific steps for traversing the adjacent child nodes of the parent node are as follows:

[0029] S311, skipping the subnodes beyond the workspace, obstacle points, and subnodes that cause the robot arm link to collide with obstacles;

[0030] S312, calculating the cost functions of the remaining child nodes in sequence;

[0031] S313. Determine whether the child node is in the open list. If the current child node is not in the open list, add the child node to the open list and set its parent node. If the current child node is in the open list, determine whether the cost function under the path is less than the cost function under the original path. If it is less, update the cost function and parent node of the child node in the open list. If it is not less, go to step S304.

[0032] Furthermore, the cost function is F=G+H, where G represents the movement cost of moving from the starting point to a child node, and H represents the estimated cost of moving from the child node to the target point B.

[0033] Furthermore, the calculation formula of the movement cost is as follows:

[0034]

[0035] Where (x A ,y A ,z A ) is the coordinate of the starting point, (x C ,y C ,z C ) are the coordinates of the child node.

[0036] Furthermore, the calculation formula for the estimated cost is as follows:

[0037] H C =|x B -x C |+|y B -y C |+|z B -z C |

[0038] Where (x B ,y B ,z B ) is the coordinate of the target point, (x C ,y C ,z C ) are the coordinates of the child node.

[0039] Furthermore, the quadratic B-spline interpolation method is used to smooth the lines connecting the path points.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] 1. The present invention adopts a method of varying grid size to divide the workspace. Small grids are used near obstacles to improve path accuracy, and large grids are used in the remaining space to improve algorithm efficiency. The size and distribution of the workspace grids are optimized through the particle swarm optimization algorithm. Based on the optimized grid, the A* algorithm is applied to three-dimensional space to achieve obstacle avoidance for the robot arm linkage, which ensures the accuracy of the robot arm path planning while accelerating the efficiency of the algorithm operation.

[0042] 2. The present invention uses the quadratic B-spline interpolation method to smooth the connection of the path points to obtain the final path of the end effector of the robotic arm, thereby reducing the impact and vibration of the motor and slowing down the wear of the machine. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 is a flow chart of the method of the present invention;

[0044] Figure 2Rasterize the local schematic diagram of the workspace,

[0045] Among them, a is the side length of the large grid, and b is the side length of the small grid;

[0046] Figure 3 This is the flow chart of the A* algorithm;

[0047] Figure 4 Schematic diagram of smoothing processing using the quadratic B-spline interpolation method. DETAILED DESCRIPTION

[0048] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0049] Example:

[0050] This embodiment provides a robot path planning method based on the A* algorithm of variable grid. Figure 1 As shown, the following steps are included:

[0051] S1, obtain the position of the starting point and target point (i.e., the fruit point) of the six-degree-of-freedom robot end effector;

[0052] S2, rasterize the workspace between the starting point and the target point, and use the particle swarm algorithm to optimize the size and distribution of the grid;

[0053] The larger the grid size, the higher the efficiency, but the lower the trajectory planning accuracy; the smaller the grid size, the higher the trajectory planning accuracy, but the lower the efficiency. In order to ensure the trajectory planning accuracy while speeding up the efficiency of the algorithm, the present invention adopts a method of changing the grid size. The local schematic diagram of the gridded workspace is as follows Figure 2 As shown in the figure, for ease of analysis and calculation, obstacles are enclosed in a cube. The space around the obstacle is divided into small grids with a side length of b to improve the accuracy of the planned path. The small grids surround the obstacle layer by layer, and the number of layers (a positive integer) is related to the number of small grids c. The remaining space is divided into large grids with a side length of a (a>b) to improve the efficiency of the algorithm. A particle swarm algorithm is used to optimize the ratio of the side lengths and the number of small and large grids (i.e., parameters a, b, and c) to find a suitable set of parameters that balances the two indicators of planned trajectory accuracy and algorithm efficiency.

[0054] Assuming that the time to search a grid is t and the volume of the entire workspace is V, the time required to search the entire workspace is cost(a,b,c) is the objective function, and the variables to be optimized are a, b, and c. The particle swarm optimization algorithm is used to find the variables to be optimized that minimizes the objective function, including the following steps:

[0055] S201, set the number of swarm particles to m and the particle dimension to 3;

[0056] S202, randomly initialize the speed and position of each particle in the population to obtain the initial population position x 1 and the initial population velocity v 1 ,in, represents the initial velocity of the mth particle, is a three-dimensional vector representing the initial position of the mth particle;

[0057] S203, comparing the objective function values ​​of each possible solution during iteration, and updating the optimal position of each particle and the optimal position of the particle population;

[0058] S204. Update the speed and position of each particle based on the current position, speed, and optimal position. The update formulas for the speed and position of the i-th particle are as follows:

[0059]

[0060]

[0061] Where k is the current iteration number, is the velocity of the i-th particle at the k-th iteration, is the position of the i-th particle at the k-th iteration, w is the inertia weight, r1 and r2 are random numbers distributed in the interval [0,1], with an initial value of 1, is the optimal position of the i-th particle at the k-th iteration, is the optimal position of the particle population at the kth iteration, c1 and c2 are constants, and the population position at the k+1th iteration can be obtained

[0062] S205: Determine whether the iteration termination condition is met (the objective function meets the preset condition or the number of iterations exceeds the limit). If so, proceed to step S206; if not, return to step S203.

[0063] S206: Output the optimal a, b, and c.

[0064] S3. Based on the grid optimized in step S2, the A* algorithm is used for path planning to avoid the singular points of the robotic arm and the points where the robotic arm may collide, and all the path points of each joint of the robotic arm are obtained;

[0065] like Figure 3 As shown, the specific steps include:

[0066] S301, set the workspace grid size and distribution according to the parameters a, b, c obtained in step S2, and input the coordinates A (x A ,y A ,z A ) and the coordinates of the target point B(x B ,y B ,z B ), input the obstacle coordinates, initialize the open list (used to record all nodes considered for finding the shortest path), the close list (used to record nodes that will no longer be considered), and the point list (used to save the final planned path nodes);

[0067] S302: Set the parent node. Initially, set the starting point A as the parent node. Then, set the node with the smallest cost in the open list as the new parent node.

[0068] S303: Traverse the 26 adjacent child nodes around the parent node in sequence. The specific steps are as follows:

[0069] S311, skipping the subnodes beyond the workspace, obstacle points, and subnodes that cause the robot arm link to collide with obstacles;

[0070] S312, calculate the cost function F of the remaining child nodes in sequence,

[0071] The cost function F=G+H represents the total cost of the child node, where G represents the movement from the starting point A to the child node, i.e., the coordinates are (x C ,y C ,z C The movement cost of moving from the starting point A to the middle point C is calculated as follows:

[0072]

[0073] H represents the estimated cost of moving from the child node to the target point B, calculated using Manhattan distance. The estimated cost calculation formula from the intermediate point C to the target point B is as follows:

[0074] H C =|x B -x C |+|y B -y C |+|z B -z C |;

[0075] S313: Determine whether the child node is in the open list. If the current child node is not in the open list, add the child node to the open list and set its parent node. If the current child node is in the open list, determine whether the cost function of the path is less than the cost function of the original path. If so, update the cost function and parent node of the child node in the open list. If not, proceed to step S304.

[0076] S304: Determine whether all 26 adjacent child nodes of the current parent node have been traversed. If not, return to step S303; if so, proceed to step S305;

[0077] S305. Determine whether the target point B appears in the open list. If so, start searching for the parent node from the target point B in sequence until the starting point A is found, form a point list, and obtain the three-dimensional coordinate values ​​of the path points of the optimal trajectory of the robot end effector; if not, determine whether the open list is empty. If the open list is not empty, select the node with the smallest cost in the open list, delete the node from the open list, add it to the close list, and return to step S302; if the open list is empty, the path planning fails and the program terminates.

[0078] S4. Since the path obtained in step S3 is a continuous line segment composed of many discrete points, it is not smooth and will cause the impact and vibration of the motor to accelerate mechanical wear. Figure 4 As shown, the quadratic B-spline interpolation method is used for smoothing.

[0079] If the above method is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, and other media that can store program code.

[0080] The above description of the embodiments is intended to facilitate understanding and use of the invention by those skilled in the art. It will be apparent that those skilled in the art can readily make various modifications to these embodiments and apply the general principles described herein to other embodiments without requiring inventive effort. Therefore, the present invention is not limited to the above-described embodiments. Improvements and modifications made by those skilled in the art based on the disclosure of the present invention, without departing from the scope of the present invention, should be within the scope of protection of the present invention.

Claims

1. A robot arm path planning method based on the A* algorithm with variable grid, characterized in that: The following steps are involved: S1, obtain the position and posture of the starting point and target point of the end effector of the robot arm; S2. Rasterize the workspace between the starting point and the target point, and use the particle swarm algorithm to optimize the size and distribution of the grid. The specific steps include: S201. Setting the objective function ,in is the volume of the entire workspace, is the time to search a grid, a is the side length of the large grid, b is the side length of the small grid, is the number of small grids; set the number of population particles to m and the particle dimension to 3; S202, randomly initialize the speed and position of each particle in the population; S203, comparing the objective function values ​​of each possible solution during iteration, and updating the optimal position of each particle and the optimal position of the particle population; S204, updating the speed and position of each particle; S205: Determine whether the iteration termination condition is met. If so, proceed to step S206; if not, return to step S203; S206, output a, b and c to achieve grid optimization; S3. Based on the grid optimized in step S2, the A* algorithm is used for path planning to obtain all the path points of the end effector of the robotic arm. The specific steps include: S301. Set the grid size and distribution of the workspace according to the optimization result of step S2, input the coordinates of the starting point, the target point, and the obstacle, and initialize the open list, close list, and point list. The open list is used to record all nodes considered for finding the shortest path, the close list is used to record nodes no longer considered, and the point list is used to record the final planned path nodes. S302: Set the parent node. Initially, set the starting point A as the parent node. Then, set the node with the smallest cost in the open list as the new parent node. S303, traverse the adjacent child nodes around the parent node in sequence; S304: Determine whether all adjacent child nodes of the current parent node have been traversed. If not, return to step S303; if so, proceed to step S305. S305. Determine whether the target point appears in the open list. If so, search for the parent node from the target point in sequence until the starting point is found, form a point list, and obtain the three-dimensional coordinate values ​​of the path points of the optimal trajectory of the robot end effector; if not, determine whether the open list is empty. If the open list is not empty, select the node with the lowest cost in the open list, delete the node from the open list, add it to the close list, and return to step S302; if the open list is empty, the path planning fails and the program terminates. S4. Smooth the lines connecting the path points to obtain the final path of the end effector of the robotic arm.

2. The method for manipulator path planning based on the variable grid A* algorithm according to claim 1, characterized in that: The process of rasterizing the workspace is as follows: the obstacle is enclosed by a cube, the space near the obstacle is divided into a small grid with a side length of b, and the rest of the space is divided into a large grid with a side length of a, where a>b.

3. The method for manipulator path planning based on the variable grid A* algorithm according to claim 1, characterized in that: The update formula for each particle's velocity and position is as follows: in k is the current iteration number, For the k The first iteration i The speed of the particle, For the k The first iteration i The position of the particle, is the inertia weight, and is a random number distributed in the interval [0,1]. For the k The first iteration i The optimal position of a particle, For the k The optimal position of the particle population at the iteration, and is a constant.

4. The method for manipulator path planning based on the variable grid A* algorithm according to claim 1, characterized in that: The specific steps to traverse the adjacent child nodes of the parent node are as follows: S311, skipping the subnodes beyond the workspace, obstacle points, and subnodes that cause the robot arm link to collide with obstacles; S312, calculating the cost functions of the remaining child nodes in sequence; S313. Determine whether the child node is in the open list. If the current child node is not in the open list, add the child node to the open list and set its parent node. If the current child node is in the open list, determine whether the cost function under the path is less than the cost function under the original path. If it is less, update the cost function and parent node of the child node in the open list. If it is not less, go to step S304.

5. The method for manipulator path planning based on the variable grid A* algorithm according to claim 1, characterized in that: The cost function is F=G+H, where G represents the movement cost of moving from the starting point to a child node, and H represents the estimated cost of moving from the child node to the target point B.

6. The method for manipulator path planning based on the variable grid A* algorithm according to claim 5, characterized in that: The calculation formula of the movement cost is as follows: in are the coordinates of the starting point, The coordinates of the child node.

7. The method for manipulator path planning based on the variable grid A* algorithm according to claim 5, characterized in that: The calculation formula for the estimated cost is as follows: in is the coordinate of the target point, The coordinates of the child node.

8. The method for manipulator path planning based on the variable grid A* algorithm according to claim 1, characterized in that: The quadratic B-spline interpolation method is used to smooth the lines connecting the path points.

Images