Target-guided apple multi-target continuous picking path planning method

Abstract

The invention discloses a target-guided apple multi-target continuous picking path planning method. The method comprises the following steps: 1, initializing; the method comprises the following steps: acquiring an image of an apple tree, establishing a simulation three-dimensional model of the apple tree, and generating a tree T in a three-dimensional working space X; 2, selecting one apple to determine a current target node qgoal; taking the position of the apple collection container as an initial node qinit; 3, performing iterative expansion; repeating the following steps 1-4 until the path meeting the condition is found: 1, random sampling: randomly sampling a common node qrand from a specific area; the specific area is a conical area, and the specification of the specific area is as follows: qinit is a vertex, and qgoal is the circle center of the bottom surface; the distance from qinit to qgoal is h, and the radius of the bottom surface is h / 30.5; 2, selecting a nearest node; 3, expanding the tree; and 4, checking collision. And 4, path generation: generating a path through backtracking. According to the method, the RRT algorithm is improved, so that the method better fits the actual condition of apple picking, the path planning efficiency is improved, the method has high expansion capability, and the finally generated path is smoother.

Description

Technical Field

[0001] The invention relates to a path planning technology, in particular to an apple picking path planning technology. Background Art

[0003] 1. In apple planting, apple picking is a very important link. The current apple picking technologies mainly include: 1. Manual picking. Although mechanization and automation technology are developing rapidly, manual picking is still the most important way to pick apples. The advantages of manual picking are flexibility and less damage to the fruit; the disadvantages are low efficiency and high labor costs.

[0004] 2. Mechanical picking. Mechanical picking mainly uses vibration or shaking to make the apples fall off the tree and then collect them. This method is highly effective, but it causes greater damage to the apples.

[0005] 3. Semi-automatic assisted picking. This method uses a picking assistance platform to enable multiple people to pick apples manually at the same time. The picked apples enter the collection device through a conveyor mechanism. The advantage is that the efficiency is higher than that of pure manual picking, but the disadvantage is that it still relies on manual operation.

[0006] 4. Robotic picking. The picking robot uses a machine vision system to identify the location of the apples, and then completes the picking through a robotic arm and end effector. The robotic arm and end effector need path planning technology to guide their picking actions during work.

[0007] The present invention studies the path planning technology in robot picking.

[0008] 2. Existing path planning algorithms.

[0009] 1. Dijkstra algorithm.

[0010] The advantages are simple principles, easy to understand and implement, applicable to various graph structures, able to find the shortest path, and suitable for path planning in static environments.

[0011] The disadvantages are high time complexity and low processing efficiency.

[0012] 2. Group optimization algorithm.

[0013] The advantages are global search capability, parallel processing, and suitability for multi-robot systems; the disadvantages are many parameters, slow convergence, and the possibility of falling into a local optimal solution and failing to explore the global optimal solution.

[0014] 3. Graph search algorithms (such as RRT).

[0015] Graph search algorithms are suitable for high-dimensional spaces, have strong adaptability to dynamic environments, can quickly explore space through random sampling and tree structures, can avoid falling into local optimal solutions, and can incrementally build trees. The disadvantage is that the generated path may not be the shortest path, the path quality may be poor, and the computational complexity is large, requiring a large amount of sampling and tree structure construction.

[0016] 3. Technical selection of the present invention.

[0017] The commonly used existing path planning algorithms were studied. In comparison, the RRT algorithm (i.e., rapidly exploring random trees) is relatively more suitable for path planning tasks in apple picking.

[0018] The existing RRT algorithm is as follows: 1. Initialization Create a tree T, the root node of which is the initial state q_init; (ii) Define a target state q_goal and a workspace X.

[0019] 3. Iterative expansion Repeat the following sub-steps until a path that satisfies the conditions is found: 1. Random sampling: randomly sample a state q_rand from the workspace X; 2. Select the nearest node: find the node q_nearest closest to q_rand in the tree T; 3. Expand the tree: Expand one step from q_nearest to q_rand to generate a new node q_new; this step is achieved by moving a small fixed distance in the direction of q_rand.

[0020] 4. Check for collision: If q_new is close enough to the goal state q_goal, q_new can be added to the tree T, with q_nearest as its parent node.

[0021] (iv) Path generation: If q_new is close enough to the target state q_goal, a path is generated by backtracking from q_new to the root node q_init.

[0022] 4. The above existing RRT algorithm is directly used for path planning in apple picking, which has the following defects: 1. When picking apples, the environment is very complex, and most apples are blocked by obstacles such as branches and leaves. The existing RRT algorithm has no restrictions on the sampling area. In three-dimensional space, the sparsity of random sampling may make it difficult for the algorithm to effectively cover the entire space; the distance between sampling points may be large, making it difficult to quickly find a path connecting the starting point and the target point. This leads to low planning efficiency.

[0023] 2. The existing RRT algorithm focuses on finding a reachable path rather than the optimal path, so the generated path may not be the shortest or optimal, and the generated path may be long or not smooth.

[0024] 3. An apple is a q_goal in the RRT algorithm. In the existing RRT algorithm, the order of picking is determined only by the distance between apples, that is, after picking one apple, the closest apple is picked. The "closest distance" here does not take into account the obstacles between apples. If obstacles need to be avoided, the two apples with the closest straight-line distance may not be the two apples with the closest motion path.

[0025] 4. It may fall into a local optimal solution.

[0026] 5. Since the sampling points are randomly sampled and the step size is fixed, the new nodes of the expansion tree may not directly reach the target point, which will result in invalid search and extend the total time of path planning.

[0027] 6. The path planned by the algorithm is composed of several nodes. The generated path will have many redundant points and inflection points. Redundant points will increase the distance of path planning and the time consumed by the robot arm. The appearance of inflection points may cause the robot arm to shake during the path planning movement, thereby reducing the stability and accuracy of the robot arm. Summary of the invention

[0028] The purpose of the present invention is to provide a target-guided apple multi-target continuous picking path planning method, which is adapted to the actual situation of apple picking, reduces the sampling space, reduces invalid calculations, and improves the path planning efficiency.

[0029] To achieve the above object, the present invention discloses a target-guided multi-target continuous apple picking path planning method, wherein apple picking is performed by a picking robot, the picking robot comprises an industrial control computer, the industrial control computer is connected to a mechanical arm, an end effector is installed at the end of the mechanical arm, the industrial control computer is built-in with Blender three-dimensional modeling software and Matlab software, a camera is installed on the mechanical arm, and the camera is connected to the industrial control computer; the mechanical arm picks apples through the end effector and puts the apples into an apple collection container; Follow these steps: 1. Initialization; The camera collects the image of the apple tree, and the industrial computer uses Blender 3D modeling software to build a simulated 3D model of the apple tree according to the image of the apple tree, and imports the simulated 3D model of the apple tree into Matlab software to generate a tree T in the 3D workspace X; The nodes in the tree T are called ordinary nodes, and the ordinary nodes representing the positions of apples are called target nodes. The industrial control computer uses an image recognition algorithm to identify apples in the apple tree image captured by the camera, and defines each apple as a target node q in the three-dimensional workspace X. 2. Determine the current target node q_goal; in the set of nodes q, select a target node q that is closest to the current end effector as the current target node q_goal; and use the location of the apple collection container as the initial node q_init; 3. Iterative expansion; Repeat the following sub-steps until a path that satisfies the conditions is found: 1. Random sampling: randomly sample a common node q_rand from a specific area in the three-dimensional workspace X; The specific area is a cone-shaped area, and the rules of the cone-shaped area are: q_init is the vertex of the cone-shaped area, q_goal is the center of the bottom of the cone-shaped area; the distance from q_init to q_goal is h, and the radius of the bottom of the cone-shaped area is h / 3 0.5 ; 2. Select the nearest node: find the node q_nearest closest to q_rand in the tree T; 3. Expand the tree: Expand one step from q_nearest to q_rand to generate a new node q_new; this step is achieved by moving one step in the direction of q_rand; 4. Check collision: the maximum picking distance of the end effector is dmax, and the distance between q_new and q_goal is ds; If q_new is within the obstacle, ignore this q_new and re-execute the iterative expansion step; If q_new is not within the obstacle and ds≤dmax, then add q_new to the tree T with q_nearest as its parent node; If q_new is not within the obstacle and ds>dmax, the iterative expansion step is re-executed until ds≤dmax, q_new is added to the tree T, and the iterative expansion step ends; 4. Path generation: Generate a path by backtracking from the last q_new to the root node q_init, which serves as the path for the industrial computer to control the robotic arm to pick apples using the end effector; After picking the apples at the current q_goal, the robot arm retracts and places the apples into the apple collection container, and then repeats the second to fourth steps, continuously planning the path to pick the next apple until all the apples on the current apple tree are picked.

[0030] The present invention adopts a variable step-size expansion strategy, which is specifically: When the iterative expansion step is just started, the step length of the expansion tree is the initial step length; The step size used in the iterative expansion step is called the original step size. When checking for collision, if q_new is within the obstacle, the step size of the expansion tree is adjusted from the original step size to 0.5× the original step size when the iterative expansion step is re-executed.

[0031] When random sampling is performed in the iterative expansion step, a threshold Prand Î (0,1) is specified; a random number P is generated when q_rand is randomly determined in each iterative expansion, and PÎ (0,1); When P < Prand, random sampling adopts the strategy of target biased sampling to sample in the cone area; When P ≥ Prand, random sampling adopts the strategy of target area sampling to sample in the cone area.

[0032] After the path is generated in the fourth step, the operation of removing redundant nodes on the path is performed; The path generated in the fourth step includes n nodes, where n is a natural number greater than or equal to 2; q_init is the first node on the path, and q_new is the last node on the path, i.e. the nth node; The connection operation is: m is the serial number of the current node, and a straight line is connected from the mth node to the bth node, and the initial value of b is m+1; If the connecting line does not pass through the obstacle, then b is incremented by 1 to make b=b+1, and the connection operation is repeated for the current node until the connecting line passes through the obstacle. All nodes between b-1 and m+1 on the path are deleted, and the current mth node is directly connected to the b-1th node, completing the operation of removing redundant nodes on the path. The first node is used as the current node for the connection operation and is executed until the operation of removing redundant nodes on the path is completed; after each operation of removing redundant nodes is completed, m=b-1, then b=m+1, and the connection operation is repeated until b=n, all nodes between b and m+1 on the path are deleted, and all operations of removing redundant nodes on the path are completed.

[0033] After the operation of removing redundant nodes of the path is completed, a spline curve is generated with the remaining nodes of the path and obstacles that are not passed as constraints, and the spline curve is used as the path of the robot.

[0034] The overall design idea of ​​the present invention is to improve the RRT algorithm according to the characteristics of apple picking to better complete the path planning task in apple picking, specifically including: 1. Study the specific characteristics of apple picking and reduce the random sampling space from the entire workspace X to a cone-shaped area with specific regulations, thereby greatly reducing invalid calculations and improving path planning efficiency.

[0035] 2. In the process of random tree expansion, if the initial step size is set too large, the algorithm will not be able to expand complex spaces, and if it is too small, the algorithm will run longer. The present invention changes the fixed step size to automatically reduce the step size when approaching obstacles, and uses the initial step size in other cases, thereby combining the high efficiency of a larger step size with the strong expansion capability of a smaller step size for complex spaces.

[0036] 3. After the path is generated, there may be redundant nodes for picking apples. The present invention identifies and removes these redundant nodes, and the calculation method for removing redundancy is opposite to the backtracking (retrograde) direction, thereby saving the amount of calculation.

[0037] The present invention has the following advantages: The present invention limits the random sampling area from the entire workspace X to a cone-shaped area with specific regulations, greatly reducing the sampling space, thereby reducing the invalid calculation process caused by sampling outside the cone-shaped area, improving the efficiency of iterative expansion, and accelerating the path planning process.

[0038] The principle of limiting the random sampling area from the entire working space X to a cone-shaped area with specific regulations in the present invention is: In the actual operation of picking apples, it is not necessary to go around in a big circle to pick the target apple. At the same time, the closer to the target apple, the more dense the obstacles (branches, leaves, etc.), and the apple collection container (q_init is at the apple collection container) will not be placed next to the obstacles. When the robot puts the picked apples into the apple collection container and accurately picks the next apple, q_init in the new round of path planning is naturally in the collision-free area, so the robot can move freely near q_init. The closer to q_goal, the more likely the robot will encounter obstacles. Limiting the random sampling area to the cone area will not prevent the robot from approaching q_goal without collision due to the small sampling selection space near q_init (the tip of the cone area), nor will it prevent the robot from reaching q_goal without collision due to the small sampling selection space near q_goal (the bottom part of the cone area). That is, the cone-shaped area with specific regulations in the present invention can define a sampling space that is greatly reduced compared to the entire workspace X but is guaranteed to have a path to reach q_goal, which naturally greatly reduces the amount of calculation and improves the speed of path planning.

[0039] In the present invention, the condition for stopping iteration is not related to the initial step length, but is directly related to the end effector. In this way, when determining the initial step length, there is no need to be restricted by the parameters of the end effector picking apples. The initial step length can be determined to be longer, thereby improving the efficiency of iterative expansion and accelerating the path planning process.

[0040] By adopting the variable step-size expansion strategy of the present invention, the expansion step-size adopted in the initial collision-free area is the initial step-size, so the efficiency is high; Figure 1 The step size is automatically reduced after entering the obstacle area shown. If q_new is continuously within the obstacle during iteration, the step size can be continuously reduced so that the step size near the obstacle can quickly adapt to the density of the obstacle, thereby taking into account both high efficiency and strong scalability to complex spaces.

[0041] After the threshold Prand is introduced, the random tree can expand toward the target more quickly and improve the search efficiency.

[0042] Removing redundant node operations on the path can greatly reduce the turning of the robot arm in motion and improve the smoothness of the robot arm's motion. The existing RRT algorithm generates paths by backtracking, that is, a backward method from q_goal (q_new) to q_init; but the calculation amount of removing redundant node operations by backtracking (from front to back) is large, and it is relatively troublesome to write programs; the specific algorithm of removing redundant node operations on the path of the present invention is from back to front (forward), which is easier to write programs, and in the scene of pursuing extreme efficiency, there is no need to wait for all redundant nodes to be removed before controlling the operation of the robot arm. After completing a redundant node removal operation on the path, the industrial control computer can control the robot arm to move forward. During the forward movement of the robot arm, the industrial control computer must also complete the subsequent operation of removing redundant node operations on the path, so compared with the backtracking method of removing redundant nodes, the actual efficiency of apple picking is slightly higher. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is an area partition diagram that divides the collision-free area and obstacle area between the initial node q_init and the current target node q_goal. Figure 1 The target point in is q_goal, Figure 1 The starting point in is q_init.

[0044] Figure 2 It is a schematic diagram of a structure that expresses the specific regulations of the conical area that limits the sampling space.

[0045] Figure 3 It is the principle diagram of the variable step size expansion strategy.

[0046] Figure 4 It is a schematic diagram of removing redundant nodes from the path.

[0047] Figure 5 It is the schematic diagram for generating spline curves. DETAILED DESCRIPTION

[0048] like Figures 1 to 5 As shown, the present invention provides a target-guided multi-target continuous apple picking path planning method, wherein apple picking is performed by a picking robot, the picking robot comprises an industrial control computer, the industrial control computer is connected to a mechanical arm, an end effector is installed at the end of the mechanical arm, the industrial control computer is built-in with Blender three-dimensional modeling software and Matlab software, a camera is installed on the mechanical arm, and the camera is connected to the industrial control computer; the mechanical arm picks the apples through the end effector and puts the apples into an apple collection container; Follow these steps: 1. Initialization; The camera collects the image of the apple tree, and the industrial computer uses the Blender 3D modeling software to build a simulated 3D model of the apple tree based on the image of the apple tree, and imports the simulated 3D model of the apple tree into the Matlab software to generate a tree T in the 3D workspace X; provide a reasonable algorithm simulation environment, such as Figure 1 shown.

[0049] The nodes in the tree T are called ordinary nodes, and the ordinary nodes representing the positions of apples are called target nodes. The industrial control computer uses an image recognition algorithm to identify apples in the apple tree image captured by the camera, and defines each apple as a target node q in the three-dimensional workspace X. Image recognition algorithms such as the YOLO series algorithms, the Mask R-CNN algorithm, the SSD algorithm, the EfficientDet algorithm, or the Vision Transformer (VIT) algorithm are existing technologies and will not be described in detail.

[0050] 2. Determine q_goal; in the set of nodes q, select a target node q that is closest to the current end effector as the current target node q_goal; at the same time, the position of the apple collection container is used as the initial node q_init (after picking apples, put the apples into the apple collection container); 3. Iterative expansion; Repeat the following sub-steps until a path that satisfies the conditions is found. 1. Random sampling: Randomly sample a common node q_rand from a specific area in the three-dimensional workspace X (q_rand is a spatial node in X, but not necessarily the target node q representing an apple); The specific area is a cone-shaped area, and the specification of the cone-shaped area is: q_init is the vertex of the cone-shaped area (i.e. Figure 1 and Figure 2 The starting point in the middle), q_goal is the center of the bottom of the cone (i.e. Figure 2 and Figure 1 The distance from q_init to q_goal is h, and the radius of the bottom of the cone is h / 3. 0.5 ; 2. Select the nearest node: find the node q_nearest closest to q_rand in the tree T; 3. Expand the tree: Expand one step from q_nearest to q_rand to generate a new node q_new; this step is achieved by moving one step in the direction of q_rand; 4. Check collision: The maximum picking distance of the end effector (at the angle from q_new to q_goal) is dmax (the value of dmax is determined by the specific selection of the end effector), and the distance between q_new and q_goal is ds; If q_new is within the obstacle, ignore this q_new and re-execute the iterative expansion step; If q_new is not within the obstacle and ds≤dmax, then add q_new to the tree T, with q_nearest as its parent node; if the end effector can pick an apple up to 10 cm away, then ds should be less than or equal to 10 cm. Then re-execute the iterative expansion step; If q_new is not within the obstacle and ds>dmax, the iterative expansion step is re-executed until ds≤dmax, q_new is added to the tree T, and the iterative expansion step ends; 4. Path generation: Generate a path by backtracking from the last q_new to the root node q_init, which is used as the path for the industrial computer to control the robotic arm to pick apples (q_goal) using the end effector; After picking the apples at the current q_goal, the robot arm retracts and places the apples into the apple collection container, and then repeats the second to fourth steps, continuously planning the path to pick the next apple until all the apples on the current apple tree are picked.

[0051] The present invention limits the random sampling area from the entire workspace X to a cone-shaped area with specific regulations, greatly reducing the sampling space, thereby reducing the invalid calculation process caused by sampling outside the cone-shaped area, improving the efficiency of iterative expansion, and accelerating the path planning process.

[0052] The principle of limiting the random sampling area from the entire working space X to a cone-shaped area with specific regulations in the present invention is: In the actual operation of picking apples, it is not necessary to go around in a big circle to pick the target apple. At the same time, the closer to the target apple, the more likely obstacles (branches, leaves, etc.) are to be dense, and the apple collection container (q_init is at the apple collection container) will not be placed next to obstacles. When the robot puts the picked apples into the apple collection container and accurately picks the next apple, q_init in the new round of path planning is naturally a collision-free area, so the robot can move freely near q_init. The closer to q_goal, the more likely the robot will encounter obstacles. Limiting the random sampling area to the cone area will not prevent the robot from approaching q_goal without collision due to the small sampling selection space points near q_init (the tip of the cone area), nor will it prevent the robot from reaching q_goal without collision due to the small sampling selection space near q_goal (the bottom part of the cone area). That is, the cone-shaped area with specific regulations in the present invention can define a sampling space that is greatly reduced compared to the entire workspace X but is guaranteed to have a path to reach q_goal, which naturally greatly reduces the amount of calculation and improves the speed of path planning.

[0053] In the present invention, the condition for stopping iteration is not related to the initial step length, but is directly related to the end effector. In this way, when determining the initial step length, there is no need to be restricted by the parameters of the end effector picking apples. The initial step length can be determined to be longer, thereby improving the efficiency of iterative expansion and accelerating the path planning process.

[0054] The present invention adopts a variable step-size expansion strategy, which is as follows: Figure 3 The variable step size expansion strategy is as follows: When the iterative expansion step is just started, the step length of the expansion tree is the initial step length; the initial step length is set by the staff according to the environmental complexity coefficient and the size of the task space. The initial step length is indeed a prior art and will not be described in detail.

[0055] The step size used in the iterative expansion step is called the original step size. When checking for collision, if q_new is within the obstacle, the step size of the expansion tree is adjusted from the original step size to 0.5× the original step size when the iterative expansion step is re-executed.

[0056] By adopting the variable step-size expansion strategy of the present invention, the expansion step-size adopted in the initial collision-free area is the initial step-size, so the efficiency is high; Figure 1 The step size is automatically reduced after entering the obstacle area shown. If q_new is continuously within the obstacle during iteration, the step size can be continuously reduced so that the step size near the obstacle can quickly adapt to the density of the obstacle, thereby taking into account both high efficiency and strong scalability to complex spaces.

[0057] When random sampling is performed in the iterative expansion step, a threshold Prand Î (0,1) is specified; for example, Prand = 0.2; Prand is specified by the designer based on experience, or the industrial computer can provide the user with a Prand setting interface, which is determined by the end user; a random number P is generated when q_rand is randomly determined in each iterative expansion, and PÎ (0,1); When P < Prand, random sampling adopts the strategy of target biased sampling to sample in the cone area; When P ≥ Prand, random sampling adopts the strategy of target area sampling to sample in the cone area.

[0058] Target biased sampling and target area sampling are both existing sampling strategies and will not be described in detail.

[0059] After the threshold Prand is introduced, the random tree can expand toward the target more quickly and improve the search efficiency.

[0060] After the path is generated in the fourth step, the operation of removing redundant nodes on the path is performed; The path generated in the fourth step includes n nodes, where n is a natural number greater than or equal to 2; q_init is the first node on the path, and q_new is the last node on the path, i.e. the nth node; The connection operation is: m is the serial number of the current node, and a straight line is connected from the mth node to the bth node, and the initial value of b is m+1; If the connecting line does not pass through the obstacle, then b is incremented by 1 to make b=b+1, and the connection operation is repeated for the current node until the connecting line passes through the obstacle. Delete all nodes between b-1 and m+1 on the path (such as deleting Figure 4 ), connect the current mth node directly to the b-1th node, and complete an operation of removing redundant nodes on the path; The first node is used as the current node for the connection operation and is executed until the operation of removing redundant nodes on the path is completed; after each operation of removing redundant nodes is completed, m=b-1, then b=m+1, and the connection operation is repeated until b=n, all nodes between b and m+1 on the path are deleted, and all operations of removing redundant nodes on the path are completed.

[0061] Removing redundant node operations on the path can significantly reduce the turning of the robot during movement and improve the smoothness of the robot's movement.

[0062] After the operation of removing redundant nodes of the path is completed, a spline curve is generated with the remaining nodes of the path and obstacles that are not passed as constraints, and the spline curve is used as the path of the robot.

[0063] Using spline curves instead of straight lines between nodes as the robot path further makes the robot run smoother.

[0064] The above embodiments are only used to illustrate rather than limit the technical solutions of the present invention. Although the present invention is described in detail with reference to the above embodiments, those skilled in the art should understand that the present invention can still be modified or replaced by equivalents. Any modification or partial replacement that does not depart from the spirit and scope of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A target-guided multi-target continuous apple picking path planning method. Apple picking is performed by a picking robot. The picking robot includes an industrial control computer. The industrial control computer is connected to a robotic arm. An end effector is installed at the end of the robotic arm. The industrial control computer has built-in Blender 3D modeling software and Matlab software. A camera is installed on the robotic arm. The camera is connected to the industrial control computer. The robotic arm picks apples through the end effector and puts the apples into an apple collection container. Features Follow these steps:

1. Initialization; The camera collects the image of the apple tree, and the industrial computer uses Blender 3D modeling software to build a simulated 3D model of the apple tree according to the image of the apple tree, and imports the simulated 3D model of the apple tree into Matlab software to generate a tree T in the 3D workspace X; The nodes in the tree T are called ordinary nodes, and the ordinary nodes representing the positions of apples are called target nodes. The industrial control computer uses an image recognition algorithm to identify apples in the apple tree image captured by the camera, and defines each apple as a target node q in the three-dimensional workspace X.

2. Determine the current target node q_goal; in the set of nodes q, select a target node q that is closest to the current end effector as the current target node q_goal; and use the location of the apple collection container as the initial node q_init; 3. Iterative expansion; Repeat the following sub-steps until a path that satisfies the conditions is found:

1. Random sampling: randomly sample a common node q_rand from a specific area in the three-dimensional workspace X; The specific area is a cone-shaped area, and the rules of the cone-shaped area are: q_init is the vertex of the cone-shaped area, q_goal is the center of the bottom of the cone-shaped area; the distance from q_init to q_goal is h, and the radius of the bottom of the cone-shaped area is h / 3 0.5 ; 2. Select the nearest node: find the node q_nearest closest to q_rand in the tree T; 3. Expand the tree: Expand one step from q_nearest to q_rand to generate a new node q_new; this step is achieved by moving one step in the direction of q_rand; 4. Check collision: the maximum picking distance of the end effector is dmax, and the distance between q_new and q_goal is ds; If q_new is within the obstacle, ignore this q_new and re-execute the iterative expansion step; If q_new is not within the obstacle and ds≤dmax, then add q_new to the tree T with q_nearest as its parent node; If q_new is not within the obstacle and ds>dmax, the iterative expansion step is re-executed until ds≤dmax, q_new is added to the tree T, and the iterative expansion step ends; 4. Path generation: Generate a path by backtracking from the last q_new to the root node q_init, which serves as the path for the industrial computer to control the robotic arm to pick apples using the end effector; After picking the apples at the current q_goal, the robot arm retracts and places the apples into the apple collection container, and then repeats the second to fourth steps, continuously planning the path to pick the next apple until all the apples on the current apple tree are picked.

2. The target-guided multi-target continuous apple picking path planning method according to claim 1 is characterized in that: The present invention adopts a variable step-size expansion strategy, which is specifically: When the iterative expansion step is just started, the step length of the expansion tree is the initial step length; The step size used in the iterative expansion step is called the original step size. When checking for collision, if q_new is within the obstacle, the step size of the expansion tree is adjusted from the original step size to 0.5× the original step size when the iterative expansion step is re-executed.

3. The target-guided multi-target continuous apple picking path planning method according to claim 1, characterized in that: When random sampling is performed in the iterative expansion step, a threshold Prand Î (0,1) is specified; a random number P is generated when q_rand is randomly determined in each iterative expansion, and PÎ (0,1); When P < Prand, random sampling adopts the strategy of target biased sampling to sample in the cone area; When P ≥ Prand, random sampling adopts the strategy of sampling in the target area to sample in the cone area.

4. The target-guided multi-target continuous apple picking path planning method according to claim 1, characterized in that: After the path is generated in the fourth step, the operation of removing redundant nodes on the path is performed; The path generated in the fourth step includes n nodes, where n is a natural number greater than or equal to 2; q_init is the first node on the path, and q_new is the last node on the path, i.e. the nth node; The connection operation is: m is the serial number of the current node, and a straight line is connected from the mth node to the bth node, and the initial value of b is m+1; If the connecting line does not pass through the obstacle, then b is incremented by 1 to make b=b+1, and the connection operation is repeated for the current node until the connecting line passes through the obstacle. All nodes between b-1 and m+1 on the path are deleted, and the current mth node is directly connected to the b-1th node, completing the operation of removing redundant nodes on the path. The first node is used as the current node for the connection operation and is executed until the operation of removing redundant nodes on the path is completed; after each operation of removing redundant nodes is completed, m=b-1, then b=m+1, and the connection operation is repeated until b=n, all nodes between b and m+1 on the path are deleted, and all operations of removing redundant nodes on the path are completed.

5. The target-guided multi-target continuous apple picking path planning method according to claim 4 is characterized in that: After the operation of removing redundant nodes of the path is completed, a spline curve is generated with the remaining nodes of the path and obstacles that are not passed as constraints, and the spline curve is used as the path of the robot.

Images