Method for planning picking sequence of mechanical arm for picking spheroidal fruits cultivated on a cultivation rack

Abstract

The application discloses a picking sequence planning method of a mechanical arm for picking spherical fruits cultivated by a cultivation frame, which comprises the following steps: identifying a tomato area by using a deep learning framework, identifying a fruit to be picked, modeling a traveling salesman problem and solving the problem by using a genetic algorithm to plan an optimal picking sequence for all string tomatoes to be picked, and guiding the mechanical arm to continuously pick multiple target fruits. In addition to the shortest total path, the minimum factor of each rotating joint of the mechanical arm is added in the path evaluation index for comprehensive evaluation. The planned picking sequence can not only make the total path tend to be the shortest, but also prevent the rotating angle of each joint from being too large, thereby preventing the mechanical arm from being damaged. The application can improve the picking efficiency and the picking success rate, and avoid the damage of the mechanical arm caused by the large rotating angle of the joint, and has practical significance for promoting the yield and income of the string tomato industry and the intelligentization of automatic picking equipment.

Description

Technical Field

[0001] This invention belongs to the field of fruit and vegetable harvesting robots, specifically relating to a grasping path planning method for a robotic arm for harvesting spherical fruits cultivated on a binocular vision-based cultivation rack, particularly for harvesting bunches of tomato fruits. Background Technology

[0002] Tomatoes are a widely cultivated vegetable in most parts of my country, and harvesting them is the most costly part of the process. Harvesting is greatly affected by the season and requires extremely high labor costs. Therefore, ensuring timely harvesting and reducing labor costs are crucial for increasing tomato yields. Many universities and research institutions have already conducted research in this area, such as the tomato harvesting robot developed by Panasonic in Japan, the Virgo-type tomato harvesting robot developed by Root AI in the United States, and the dual-arm tomato harvesting robot developed by Shanghai Jiao Tong University. Tomato harvesting robot technology is maturing and moving towards commercialization.

[0003] However, most current research focuses on obstacle avoidance and fruit recognition in harvesting robots, with little attention paid to the harvesting order of multiple fruits. Harvesting robots typically harvest in a random order, resulting in low efficiency and unnecessary wear and tear on the robotic arm due to excessive joint rotation. Existing research also has limitations. For example, patent publication CN105773614A, "A Fruit Harvesting Sequence Planning System and Method Based on Binocular Vision Spatial Dimensionality Reduction," only considers the straight-line distance between points in a two-dimensional plane when planning the harvesting sequence, without taking into account the joint movement limitations of the robotic arm. Another example is patent publication CN112369208B, "A Dynamic Planning Method for Harvesting Sequence of Quasi-Spherical Fruits," which describes a method that addresses the problem of fruit occlusion by establishing priority evaluation indicators for the harvesting sequence. However, cluster tomatoes are cultivated using trellises, where fruit occlusion is rare. Harvesting bunch tomatoes grown on trellises involves harvesting tomatoes that grow side-by-side on vines, resulting in a large number of fruits and minimal shading. Continuous harvesting of multiple fruits requires consideration not only of harvesting efficiency but also of wear and tear on the robotic arm joints. Therefore, a reasonable harvesting sequence planning method is needed to improve overall harvesting efficiency and reduce wear and tear on the robotic arm joints. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a method for planning the grasping path of a robotic arm for harvesting spherical fruits cultivated on a binocular vision-based cultivation rack. This method uses a binocular camera to acquire information about the tomato stems to be harvested, then models a traveling salesman problem using the total path length and joint torque as optimization indicators, and finally uses a genetic algorithm to solve for the optimal harvesting order. This method ensures harvesting efficiency and minimizes the harvesting path while avoiding wear and tear on the robotic arm caused by excessive joint rotation angles. It has practical significance for promoting increased production and income in my country's tomato industry and for the intelligentization of automated harvesting equipment.

[0005] The technical solution of the present invention is as follows:

[0006] A method for planning the grasping path of a robotic arm for harvesting spherical fruits cultivated on a binocular vision-based cultivation rack is proposed. This method enables the harvesting robot to complete the overall harvesting task with the shortest planned distance and the least joint torque while ensuring a high success rate. The hardware consists of an Intel D435i binocular depth camera, an Aubo i5 robotic arm, a gripper-shear integrated harvesting claw, a four-wheel drive steering vehicle 1 (chassis vehicle), and a four-wheel drive steering vehicle 2 (logistics vehicle). The steps are as follows:

[0007] Step S1: The picking robot arrives at the picking area. The robotic arm of the picking robot is equipped with a binocular vision camera.

[0008] Step S2: Using the location of the picking robot as a monitoring point, adjust the pose of the robotic arm of the picking robot, and use the binocular vision camera to perceive and identify the environment to obtain the spatial coordinates of the root and stem picking points of each mature fruit and the tilt angle of the root and stem in the camera coordinate system.

[0009] Step S3: Based on the workspace that the robotic arm of the harvesting robot can reach, remove the harvesting points located outside the workspace and update the harvesting point set V of the current harvesting area;

[0010] Step S4: Based on the total harvesting cost of the harvesting robot, use a genetic algorithm to plan the harvesting sequence of the robotic arm and form the optimal harvesting path. This specifically includes the following steps:

[0011] 1) Assign a serial number to all picking points in numerical order, arrange them according to the picking order to form a code, and generate an initial population according to the coding mechanism.

[0012] 2) Calculate the fitness of each individual in the population. The fitness function is: Where f(L) i f(R) represents the total path length generated. i ) represents the sum of the absolute values ​​of the total rotation angles of all joints, while λ1 and λ2 are weighting coefficients, and D1 and D2 are constants.

[0013] 3) The selection operator adopts an elite individual preservation strategy and a roulette wheel strategy, that is, the individual with the highest fitness will definitely be selected, while other individuals also have a probability of being selected.

[0014] 4) Based on the crossover probability P c Select several parent individuals and pair them up. Generate new individuals according to the single-point crossover rule, and then calculate the mutation probability P. r Randomly identify mutated individuals and perform corresponding mutation operations to maintain the diversity of the population and prevent it from getting trapped in local optima.

[0015] 5) After iteration, the individual fitness is repeatedly calculated to select elite individuals. When the maximum number of iterations is reached, the iteration is terminated, and the most satisfactory harvesting path is considered to have been obtained.

[0016] As a further improvement of the present invention, step S2 detects the maturity information of each fruit within the field of view and outputs the picking point information, mainly including five steps:

[0017] (2-1): Tomato targets were identified using a deep learning target detection algorithm.

[0018] (2-2): For the identified target, calculate the mature and immature components respectively to obtain the preliminary tomato maturity.

[0019] (2-3): For fully ripe tomatoes, the bias ε of the immature component is obtained based on statistical principles. Then, the initial ripeness of the tomatoes is compensated to obtain the final ripeness detection result and the RGB image of the ripe fruit is obtained.

[0020] (2-4): Map the RGB image to the depth image to obtain the root and stem harvesting point P. s Distance threshold dis is used to remove distant interference from the root and stem depth image, and then image processing is performed to obtain the interference-free root and stem picking point P. D For P s and P D The three-dimensional spatial coordinates P of the root and stem harvesting center point are obtained by weight allocation calculation. i (X i ,Y i Z i ).

[0021] (2-5): Then, the Canny operator is used for edge detection and the Hough line transform is used to detect straight lines, obtain the straight lines, and calculate the inclination angle θ of the rootstock. i .

[0022] As a further improvement of the present invention, in step S5, when the genetic algorithm plans the harvesting order, the fitness function of the individuals in the population is calculated as follows: Where f(L)i f(R) represents the total path length generated. i ) represents the sum of the absolute values ​​of the total rotation angles of all joints, while λ1 and λ2 are weighting coefficients, and D1 and D2 are constants.

[0023] As a further improvement to the present invention, the total path length f(L) i The robotic arm's end effector will reach two designated picking points c. i ,c j The summation of the arc distances traveled is obtained, while the sum of the total rotation angles of all joints, f(R), is the sum of the total rotation angles. i When the robotic arm's planner plans the path between any two points in V, it uses inverse kinematics to obtain the angular positions of each joint between the two points. After calculating the absolute value of the difference between each angle, it sums them up. The joint torque is the weighted sum of the rotation angles of the robotic arm joints, and the total angular cost can be obtained by the weighted sum of the angular velocities ω of each joint.

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

[0025] Because the growth of bunch tomatoes is highly random and includes unripe fruit, a reasonable harvesting sequence needs to be planned. This invention considers the rotation angle of the robotic arm joints when planning the harvesting sequence. Sometimes, even two points very close in a straight line in three-dimensional space can move in an unusual configuration due to inverse kinematics problems, resulting in excessive joint angles, low harvesting efficiency, and waste. By comprehensively considering path length and joint rotation angles when planning the harvesting sequence, a harvesting path more suitable for the three-dimensional movement of the robotic arm can be planned. Compared to traditional random harvesting, this method uses less energy to harvest fruit faster, improving harvesting efficiency. Attached Figure Description

[0026] Figure 1 This is a flowchart of the robotic arm's picking sequence planning method.

[0027] Figure 2 This is a schematic diagram of a genetic algorithm.

[0028] Figure 3 This is a flowchart of the process for picking tomatoes.

[0029] Figure 4 This is a diagram illustrating the randomized picking order.

[0030] Figure 5 This is a diagram illustrating the planned harvesting sequence. Detailed Implementation

[0031] The technical solutions in the embodiments of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0032] A method for planning the picking order of bunch tomatoes based on binocular vision includes the following steps:

[0033] Step S1: Start the system. The picking robot locates and moves to the picking position by scanning the QR code, then sends serial port information to the decision control system to notify the system that the robot has arrived at the picking area, and initializes the robotic arm and binocular vision. The serial communication data frame includes a 2-byte frame header flag, a 1-byte data length, 4 bytes of data, and a 1-byte checksum.

[0034] Step S2: The robotic arm adjusts its pose to reach the observation point. At this point, the binocular vision camera at the end of the robotic arm identifies each fruit. Then, the ripeness is determined using the YOLOv5 deep learning framework and HSV color gamut segmentation. The picking point and rootstock tilt angle are obtained through three steps: edge extraction using the Canny operator and line detection using the Hough line transform. Specifically, the steps are as follows:

[0035] 1) After identifying the cluster of tomatoes within the camera's visual area using a deep learning object recognition framework, ripening is determined using color features (ρ). m ) and immature component (ρ im The ripeness information of the bunch of tomatoes was calculated based on the determination of the ripeness of the bunch.

[0036] 2) Combining maturity information A, identify the stem regions S of tomatoes that are above a set threshold θ. i Extract it.

[0037] 3) Regarding S i The stem picking point P is obtained using deep learning. s and S i The corresponding depth image D i After removing distant scene interference using a distance threshold dis, image processing methods are used to obtain the stem picking point P. D .

[0038] 4) respectively to P s and P D Assign weights ω S and ω D (Note: ω) S +ω D =1), through the formula P=ω S ×PS +ω D ×P D Obtain the final stem harvesting point P i (X i ,Y i Z i ).

[0039] 5) Then use the Canny operator to perform edge detection and Hough line transform to detect lines, and finally output the root point and tilt angle.

[0040] 6) Determine the three-dimensional coordinates (X, Y, Z) of the center point of the stem of each ripe tomato bunch. i ,Y i Z i The harvested fruits are published through topics in ROS, forming a collection of fruits V = {a, b, c, ..., n}.

[0041] Step S3: For all the obtained tomato coordinates, remove points outside the robotic arm's workspace and update the set of fruits to be picked, V. The Aubo i5 robotic arm user manual indicates that the robotic arm's reachable workspace is accessible.

[0042] Step S4: As Figure 1 As shown, a Traveling Salesman Problem (TSP) is modeled for the multi-objective fruit picking problem. Using the picking sequence as the cost function and the total path cost and joint torque as optimization indices, a TSP model is established, transforming the multi-objective fruit picking problem into a multi-objective TSP problem that minimizes the total movement cost.

[0043] Given a size L R xL C xL D For the operating region 'a', the following variables can be defined:

[0044] 1) The set of picking points V = {a, b, c, ..., n}, where a to n are the locations of each picking point that need to be traversed and the picking task executed; N FT =∑(a~n) represents traversing all picking points.

[0045] 2) Perceived cost S C The time cost of using binocular vision to detect the coordinates of all target fruits at the monitoring point is the image processing time for multi-target recognition.

[0046] 3) Mobility cost W C (c i ,c j The cost of movement between any two picking points, including the cost of the robotic arm end effector at the planned two picking points c. i ,c jThe arc distance traveled, and the sum of the absolute values ​​of the rotation angles of each joint of the robotic arm.

[0047] 4) Harvesting cost H C The cost of pruning and harvesting the fruit at the picking point.

[0048] Seq of harvesting T The robot picks from a list of ordered coordinates, and the total movement cost can be defined as T in the following formula. PL .

[0049]

[0050] The total cost can be defined as follows:

[0051] T C =T PL +(H C +S C )N FT #

[0052] Perception cost and harvest cost are generally considered constant because perception and harvest are repetitive tasks independent of target location. For a constant number of target points, the total cost can be considered as the movement cost plus a constant, where Seq is the sequence that minimizes the total cost.

[0053] Two spatial coordinates c i ,c j The cost of movement between W C (c i ,c j The cost depends on the robotic arm's degrees of freedom, path, and motion trajectory (related to speed). The estimation method for movement cost is as follows:

[0054] 1) Path length of point-to-point motion of the robotic arm: In reality, due to obstacle avoidance requirements, the path planned by the motion planner is usually not a straight line, and a long straight path often leads to failure of inverse kinematics solution. Therefore, the actual path is usually a curve that is approximately a straight line. The distance of the robotic arm end effector before and after planning can be obtained through a plugin in ROS.

[0055] 2) Weighted sum of the planned robotic arm joint rotation angles: By calling the rqt_plot function and subscribing to the joint_states message in ROS, we can view the position of each joint angle before and after the movement. The sum of the angle costs can be obtained by weighted summation of the angular velocities ω of each joint.

[0056] Step S5: For the Traveling Salesman Problem, use a genetic algorithm to plan the picking order, specifically as follows: Figure 2 As shown, it includes the following steps:

[0057] 1) Assign a number to all picking points in numerical order, arrange them according to the picking order to form a code, and generate an initial population according to the coding mechanism. For example, generate a random code {9,6,5,3,8,0,2,7,1,4} for 10 target fruits.

[0058] 2) Calculate the fitness of each individual in the population. The fitness function is: Where f(L) i f(R) represents the total path length generated. i The sum of the absolute values ​​of the total rotation angles of all joints is denoted as f(R), where λ1 and λ2 are weighting coefficients. Here, λ1 is chosen to be 0.75, λ2 is chosen to be 0.25, and D1 and D2 are both 1. i The path planner in the decision control system is needed to calculate the difference between the starting point and the ending point by measuring the angle of each joint when planning the path between two points of the robotic arm.

[0059] 3) The selection operator adopts an elite individual preservation strategy and a roulette wheel strategy, that is, the individual with the highest fitness will definitely be selected, while other individuals also have a probability of being selected. The selection probability and cumulative probability of each individual in the entire population fitness are calculated as follows:

[0060]

[0061] 4) Based on the crossover probability P c Select several parent individuals and pair them up. Generate new individuals according to the single-point crossover rule, and then calculate the mutation probability P. r Randomly identify mutated individuals and perform corresponding mutation operations to maintain the diversity of the population and prevent it from getting trapped in local optima.

[0062] 5) After iteration, the individual fitness is repeatedly calculated to select elite individuals. When the maximum number of iterations is reached, the iteration is terminated, and the most satisfactory harvesting path is considered to have been obtained.

[0063] Step S6: After obtaining the harvesting order, the host computer planner plans the motion trajectory to guide the robotic arm to harvest continuously. The harvested fruits are placed in the fruit collection box. After one round of harvesting is completed, the robotic arm will return to the observation point and use a binocular vision camera to detect whether there are any unharvested fruits. When the number of unharvested fruits is 0, it is considered that the harvesting of this area is complete; otherwise, harvesting continues in the current area.

[0064] Step S7: After all tomatoes have been picked, the robotic arm sends a serial port command to the chassis, instructing the chassis to move to the next picking area, and repeats steps S2 to S6.

[0065] The picking order of random traversal is as follows: Figure 4 As shown, the planned harvesting order is as follows: Figure 5As shown in the diagram, a comparison between the random harvesting path and the planned harvesting path reveals significant improvements in several aspects. First, the planned path is more efficient, significantly shortening the path length of the robotic arm and thus reducing movement time. Second, the planned path better adapts to environmental changes, improving the robotic arm's adaptability in different scenarios. Furthermore, by planning the joint torque of the robotic arm, the peak value of the joint torque can be effectively reduced, thereby extending the service life of the robotic arm and improving harvesting efficiency.

[0066] The above description is merely an embodiment of the present invention, but the implementation of the present invention is not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention are equivalent substitutions and are included within the protection scope of the present invention.

Claims

1. A method for planning the harvesting sequence of a robotic arm for harvesting spherical fruits grown on a cultivation rack, characterized in that, The steps include the following: Step S1: The picking robot arrives at the picking area. The robotic arm of the picking robot is equipped with a binocular vision camera. Step S2: Using the location of the picking robot as a monitoring point, adjust the pose of the robotic arm of the picking robot, and use the binocular vision camera to perceive and identify the environment to obtain the spatial coordinates of the root and stem picking points of each mature fruit and the tilt angle of the root and stem in the camera coordinate system. Step S3: Based on the workspace reachable by the robotic arm of the harvesting robot, remove harvesting points located outside the workspace and update the harvesting point set of the current harvesting area. ; Step S4: Based on the total harvesting cost of the harvesting robot, use a genetic algorithm to plan the harvesting sequence of the robotic arm and obtain the optimal harvesting path, specifically including: S4.1 Designate the harvesting area Size A collection of picking spots in the picking area ,in, Information on each picking point that needs to be traversed and the picking task executed; This indicates traversing all picking points; the total picking cost of the picking robot. , in, For mobility costs, this refers to the cost of the robotic arm operating between any two picking points. The cost of movement between the two planned picking points, including the robotic arm end effector. The sum of the arc distance traveled and the absolute values ​​of the rotation angles of each joint of the robotic arm; The picking sequence refers to the list in which the picking robot picks according to ordered coordinates; Harvesting cost refers to the cost of using a robotic arm to clamp and harvest fruit at the picking point; Perception cost refers to the time cost of using binocular vision at the monitoring point to detect the coordinates of all target fruits, i.e., the image processing time for multi-target recognition. S4.2 Calculate the movement cost: The path length of the robotic arm performing point-to-point motion: the distance the robotic arm end effector moves before and after the planning is obtained through a plugin in ROS; The planned weighted sum of the joint rotation angles of the robotic arm: By calling the rqt_plot function and subscribing to the joint_states message in ROS, the position of each joint angle before and after movement can be viewed. The sum of the angle costs is determined by the angular velocities of each joint. We get the result by weighted summation; S4.3 The Traveling Salesman Problem is solved using a genetic algorithm, and the optimal harvesting route is planned. Specifically: S4.3.1 Collection of picking points in S4.1 A sequence number is assigned according to numerical order, and the numbers are arranged according to the harvesting order to form a code. An initial population is generated according to the coding mechanism. S4.3.2 Calculate the fitness of each individual in the population. in The total length of the generated path. It is the sum of the absolute values ​​of the total rotation angles of all joints. fitness function The weighting coefficients, The path planner in the decision control system is needed to calculate the difference between the starting point and the ending point by measuring the angle of each joint when planning the path between the initial point and the picking point of the robotic arm. S4.3.3 The selection operator employs an elite individual preservation strategy and a roulette wheel selection strategy, meaning the individual with the highest fitness is guaranteed to be selected, while other individuals also have a probability of being selected. The selection probability and cumulative probability of each individual in the overall population fitness are calculated as follows: S4.3.4 by crossover probability Select several parent individuals and pair them up. Generate new individuals according to the single-point crossover rule, and then determine the mutation probability. Randomly identify mutated individuals and perform corresponding mutation operations to maintain the diversity of the population and prevent it from getting trapped in local optima; S4.3.5 After iteration, the individual fitness is repeatedly calculated to select elite individuals. When the maximum number of iterations is reached, the iteration is terminated, and the optimal harvesting path is considered to have been obtained.

2. The harvesting sequence planning method for the robotic arm harvesting spherical fruits cultivated on a cultivation rack according to claim 1, characterized in that, Also includes: Step S5: Guide the robotic arm to continuously harvest according to the optimal harvesting path of the robotic arm, and put the harvested fruits into the fruit collection box; Step S6: After one round of picking is completed, the robotic arm returns to the monitoring point and uses a binocular vision camera to detect whether there are any unpicked fruits. When the number of unpicked fruits is 0, the picking area is considered complete and proceeds to step S7; otherwise, picking continues in the current area. Step S7: The robotic arm sends a serial port command to the chassis, instructing the chassis to move to the next picking area, and repeats steps S2 to S5.

3. The harvesting sequence planning method for the robotic arm harvesting spherical fruits cultivated on a cultivation rack according to claim 1, characterized in that, Step S2 uses the location of the harvesting robot as a monitoring point to adjust the pose of the robot's robotic arm. It uses a binocular vision camera to perceive and identify the environment, obtaining the spatial coordinates of the rootstock harvesting points of each mature fruit and the tilt angle of the rootstock in the camera coordinate system. Specifically, this includes: After identifying clusters of tomatoes within the camera's visual area using a deep learning object recognition framework, color features are used to determine the ripeness component. With immature portion The ripeness information of the bunch of tomatoes was determined and calculated. ; Combining maturity information It will be greater than the set threshold. The stem area of ​​the tomato Extract; against Using deep learning to determine the stem picking points and will Corresponding depth image Use distance threshold After removing background interference, image processing methods were used to obtain the stem picking point. ; To each Assign weights and Through formula Obtain the final stem harvesting point ; Then, the Canny operator is used for edge detection and the Hough line transform is used to detect straight lines, and finally the stem picking point and tilt angle are output. The three-dimensional coordinates of the stem picking points of each ripe bunch of tomatoes. Postings via topics in ROS create a collection of picking spots in the picking area. ,in, Information on each picking point that needs to be traversed and the picking task executed.

Images