Picking robot and control method

Abstract

The invention discloses a picking machine and a control method. The picking machine comprises a robot body, a fruit diameter separating device and a mechanical arm, wherein the fruit diameter separating device and the mechanical arm are installed on the robot body. The laser radar and the 2D camera are integrated on the robot body, and the depth camera is integrated on the mechanical arm. The processor is in communication connection with the laser radar, the 2D camera and the depth camera; the laser radar scans environment point cloud, the processor constructs a grid map and plans a driving path, mature fruits on the corresponding side are recognized through the depth camera in the driving process, the poses of the mature fruits in a mechanical arm coordinate system are determined, and the mechanical arm is controlled to pick the mature fruits. And the collected fruits are placed in an inlet area of the fruit diameter separation device. The picking robot integrates the functions of environment perception, autonomous navigation, automatic identification and picking of mature fruits and simultaneous automatic classification of fruits with different diameters, so that the picking efficiency is improved while the fruit picking process is greatly simplified.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic picking, and more specifically, the present invention relates to a picking robot and a control method. Background Art

[0002] The rapid development of the fruit planting industry has increased the market demand for orchard machinery. The labor used in fruit picking accounts for 33% to 50% of the labor used in the entire production process. Currently, the majority of fruit picking in my country is still done manually.

[0003] The picking operation is relatively complicated and has a strong seasonal characteristic. Manual picking is not only inefficient and labor-intensive, but the existing manual picking method can no longer meet the requirements of modern production.

[0004] With the development of technology, the continuous increase in demand and the continuous development of automated fruit picking technology, picking work is moving towards automation and intelligence. There are already some picking robots on the market, but these picking robots often have single functions and can only complete specific tasks. Summary of the Invention

[0005] The present invention provides a picking robot, aiming to improve the above-mentioned problem.

[0006] The present invention is implemented as follows: a picking robot, the picking robot comprising:

[0007] The robot body, the fruit diameter separation device installed on the robot body, and the robotic arm;

[0008] LiDAR and 2D camera integrated into the robot body, and depth camera integrated into the robotic arm;

[0009] A processor that communicates with the lidar, 2D camera, and depth camera;

[0010] The lidar scans the environmental point cloud, the processor builds a raster map, and plans the driving path. During the driving process, the depth camera is used to identify the ripe fruits on the corresponding side and determine the position of the ripe fruits in the robotic arm coordinate system. The robotic arm is controlled to pick the ripe fruits and place the collected fruits in the entrance area of ​​the fruit diameter separation device. The fruits enter the fruit diameter separation device through the entrance area, and the fruit diameter separation device classifies fruits of different diameters.

[0011] Furthermore, the fruit diameter separation device includes:

[0012] A cavity is provided, and a separation plate is provided on the bottom plate of the cavity. A plurality of separation grooves are arranged on the separation plate. The width of the separation grooves widens along the rolling direction of the fruit. One end of the separation plate is connected to the entrance area, and the other end is connected to the side wall of the cavity; a plurality of collectors are provided on the side of the separation plate close to the bottom of the cavity, and the collectors are arranged in sequence along the rolling direction of the fruit. A guide plate is provided on the bottom plate between adjacent separation grooves, and the guide plate is arranged perpendicular to the separation plate.

[0013] Furthermore, the separation plate is arranged at an angle, and the height of the separation plate from the bottom of the cavity decreases along the extension direction of the separation slot.

[0014] Furthermore, the inlet area is arranged at an angle, and the end connected to the separation plate is at a lower height from the bottom of the cavity.

[0015] The present invention is implemented as follows: a control method for a picking robot, the method is specifically as follows:

[0016] (1) Scan the environmental point cloud of the operation area using LiDAR and construct a two-dimensional occupancy map using the Cartographer algorithm;

[0017] (2) The driving path from the starting position to the target position is planned based on the improved A* algorithm, and the picking robot drives based on the planned driving path;

[0018] (3) The picking robot starts the 2D camera while driving and captures an image of the first side area on the side where the 2D camera is located;

[0019] (4) Detect whether there is fruit in the first side area image. If so, reduce the moving speed of the picking robot and execute step (5). If not, the picking robot continues to move at the current speed and execute step (3);

[0020] (5) starting the depth camera to capture an image of a second side area on the side, detecting the maturity level of the fruit in the second side area image, determining the image coordinates of the ripe fruit whose maturity level reaches the set level in the image, and converting the image coordinates of the ripe fruit into the robotic arm coordinate system;

[0021] (6) Calculate the position of each joint of the robotic arm when the end of the robotic arm moves to the position where the ripe fruit is located, and control the end of the robotic arm to move to the position where the ripe fruit is located, pick the ripe fruit, and place the collected fruit in the entrance area of ​​the fruit diameter separation device. The fruit diameter separation device separates fruits of different fruit diameters.

[0022] Furthermore, the A* algorithm is used to find the driving path from the starting position to the target position. The A* algorithm finds the driving path with the minimum cost from the starting position to the target position based on the cost function. The cost function is as follows:

[0023] f(n)=g(n)+λ·h(n)·exp(-γ·D(n));

[0024] Where g(n) is the actual cost of node n, that is, the distance between node n and the starting position; h(n) is the heuristic function of node n, D(n) is the Euclidean distance from node n to the nearest obstacle, λ is the adaptive weight coefficient, and γ is the attenuation coefficient;

[0025] Among them, the heuristic function h(n) is as follows:

[0026] h(n)=max(|x g -x n |,|y g -y n |)+η·(1-cosθ n );

[0027] Among them, (x g ,y g ) is the grid coordinate of the target position, (x n ,y n ) is the grid coordinate of node n, θ n is the angle between the line connecting node n and the target position and the x-axis, and η is the adjustment coefficient.

[0028] Furthermore, the 2D camera inputs the captured first side area image into the YOLOv5 model, and the YOLOv5 model outputs the region of interest where the fruit is located in the first side area image.

[0029] Furthermore, the fruit maturity is detected based on the hue value and texture characteristics of the fruit in the HSV color space. The detection formula is as follows:

[0030] M score =w c ·||HH std ||2+w s ·S+w t Entropy(I ROI );

[0031] Among them, M score represents the comprehensive score of fruit maturity, H represents the hue value of the fruit in the second side area image, and H std The hue value of the first-grade mature fruit, w c 、w s 、w t is the weight coefficient, S represents the saturation of the current fruit in the HSV color space, Entropy(I ROI ) is the image entropy of the region of interest output by the YOLOv5 model.

[0032] Furthermore, based on the comprehensive score of fruit maturity M score The maturity level grading rules are as follows:

[0033]

[0034] Among them, τ1 and τ2 are the set maturity classification thresholds.

[0035] The fruit picking robot provided by the present invention integrates the functions of environmental perception, autonomous navigation, automatic identification and picking of ripe fruits, and simultaneous automatic classification of fruits of different diameters, which greatly simplifies the fruit picking process and improves picking efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 A schematic diagram of the structure of a picking robot provided by an embodiment of the present invention;

[0037] Figure 2 A schematic diagram of the structure of a fruit diameter separation device provided in an embodiment of the present invention;

[0038] Figure 3 A flow chart of a control method for a picking robot provided by an embodiment of the present invention;

[0039] 1. Robot body, 2. Fruit diameter separation device, 21. Entrance area, 22. Separation trough, 23. Collector, 3. Robotic arm, 31. Depth camera, 32. End gripper. DETAILED DESCRIPTION

[0040] The specific implementation methods of the present invention will be further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0041] Figure 1 The following is a schematic diagram of the structure of a harvesting robot provided in an embodiment of the present invention. For ease of explanation, only the parts related to the embodiment of the present invention are shown. The harvesting robot includes:

[0042] The robot body, the fruit diameter separation device installed on the robot body, and the robotic arm;

[0043] LiDAR and 2D camera integrated into the robot body, and depth camera integrated into the robotic arm;

[0044] A processor that communicates with the lidar, 2D camera, and depth camera;

[0045] The lidar scans the environmental point cloud, the processor builds a raster map, and plans the driving path. During the driving process, the depth camera is used to identify the ripe fruits on the corresponding side and determine the position of the ripe fruits in the robotic arm coordinate system. The robotic arm is controlled to pick the ripe fruits and place the collected fruits in the entrance area of ​​the fruit diameter separation device. The fruits enter the fruit diameter separation device through the entrance area, and the fruit diameter separation device classifies fruits of different diameters.

[0046] Figure 2 The following is a schematic diagram of the structure of a fruit diameter separation device provided in an embodiment of the present invention. For ease of explanation, only the parts related to the embodiment of the present invention are shown. The fruit diameter separation device includes:

[0047] The cavity has a separation plate on the bottom plate of the cavity. The separation plate has a plurality of separation grooves. The width of the separation grooves widens along the rolling direction of the fruit. One end of the separation groove is connected to the entrance area, and the other end is connected to the side wall of the cavity. A plurality of collectors are arranged on the side of the separation groove close to the bottom of the cavity. The collectors are arranged in sequence along the rolling direction of the fruit and are used to collect fruits of different diameters. A guide plate is provided on the bottom plate between adjacent separation grooves, and the guide plate is arranged perpendicular to the separation plate.

[0048] The adjacent guide plates on the separation plate form a fruit diameter separation channel with separation grooves between the adjacent guide plates. The picked fruits enter each separation channel from the entrance area. As the fruits roll along the separation channels, the fruits with small diameters fall into the collector at the bottom first, and the fruits with large diameters continue to roll and then fall into other collectors at the bottom, realizing the automatic separation of fruits with different diameters.

[0049] In an embodiment of the present invention, the separation plate is arranged at an angle, and the height of the separation plate from the bottom of the cavity gradually decreases along the extension direction of the separation groove. After the fruit enters each separation channel from the entrance area, it automatically rolls down along the separation channel, reducing the risk of the fruit blocking the separation channel.

[0050] In an embodiment of the present invention, the entrance area is tilted, and the end connected to the separation plate is lower than the bottom of the cavity. After the robotic arm places the fruit into the entrance area, the fruit will automatically roll to the side of the separation plate and then enter different separation channels.

[0051] Figure 3 The flowchart of the harvesting robot control method provided by the embodiment of the present invention is as follows:

[0052] (1) Scan the environmental point cloud of the operation area using LiDAR and construct a two-dimensional occupancy map using the Cartographer algorithm;

[0053] The point cloud data of the working environment is obtained by LiDAR, and the Cartographer algorithm is used to construct a two-dimensional occupancy map. The grid probability value is calculated using the following Bayesian update formula:

[0054]

[0055] Among them, m x,y Represents the grid coordinates in the grid map, z 1:t represents the observation data of the laser radar before time t; P(m x,y |z 1:t ) represents the probability of observing the grid (x, y) under the observation data of the lidar before time t, P(z t |m x,y ) represents the probability of observing the laser radar observation data at time t under the grid (x, y), P(m x,y |z 1:t-1 ) represents the probability of observing the grid (x, y) under the observation data of the lidar before time t-1, P(z t |z 1:t-1 ) represents the probability of observing the lidar observation data at time t under the lidar observation data before time t-1.

[0056] (2) Based on the improved A* algorithm, a driving path from the starting position to the specified target position is planned, and the picking robot drives based on the planned driving path;

[0057] The nodes on each path are marked on the constructed grid map. The starting position and target position of the picking robot are set as nodes on the grid map. The starting position is generally the charging position in the waiting area. The driving path from the starting position to the target position is composed of a series of node sequences. The A* algorithm finds the driving path with the minimum cost from the starting position to the target position based on the cost function. The cost function is as follows:

[0058] f(n)=g(n)+λ·h(n)·exp(-γ·D(n));

[0059] Where g(n) is the actual cost of node n, that is, the distance between node n and the starting position; h(n) is the heuristic function of node n, D(n) is the Euclidean distance from node n to the nearest obstacle, λ is the adaptive weight coefficient, and γ is the attenuation coefficient.

[0060] The heuristic function h(n) is a combination of Chebyshev distance and direction weight, and its calculation formula is as follows:

[0061] h(n)=max(|x g -x n |,|y g -yn |)+η·(1-cosθ n );

[0062] Among them, (x g ,y g ) is the grid coordinate of the target position, (x n ,y n ) is the grid coordinate of node n, θ n is the angle between the line connecting node n and the target position and the x-axis, and η is the adjustment coefficient.

[0063] (3) The picking robot starts the 2D camera while driving and captures an image of the first side area on the side where the 2D camera is located;

[0064] Since fruits in greenhouses are generally planted in rows, a passage is set between two rows of fruit trees to facilitate the management of each row of fruit trees. The passage between the two rows of fruit trees is the path in the raster map, and special location points on the path are set as nodes, such as forks, the two end points of the passage, and the starting position in the waiting area. The picking robot starts from the starting position and drives along the set driving path. At the same time, the 2D camera is started. The 2D camera captures the first side area image on the side. The first side area image is used to detect whether there is fruit in the shooting area. The 2D camera inputs the captured first side area image into the YOLOv5 model, and the YOLOv5 model outputs the region of interest where the fruit is located in the first side area image.

[0065] (4) Detect whether there is fruit in the first side area image. If so, reduce the moving speed of the picking robot and execute step (5). If not, the picking robot continues to move at the current speed and execute step (3);

[0066] When the presence of fruit in the shooting area is detected based on the first side area image, the moving speed of the picking robot is reduced to facilitate the depth camera to capture clearer images of the area where the fruit is present, thereby facilitating more accurate detection of the fruit maturity.

[0067] (5) starting the depth camera to capture an image of a second side area on the side, detecting the maturity level of the fruit in the second side area image, determining the image coordinates of the ripe fruit whose maturity level reaches the set level in the image, and converting the image coordinates of the ripe fruit into the robotic arm coordinate system;

[0068] The present invention divides the fruit maturity level into three maturity levels: primary maturity, secondary maturity, and immature. The present invention only picks the primary maturity fruits and detects the fruit maturity based on the hue value and texture characteristics of the fruit in the HSV color space. The specific detection formula is as follows:

[0069] Mscore =w c ·||HH std ||2+w s ·S+w t Entropy(I ROI );

[0070] Among them, M score represents the comprehensive score of fruit maturity, H represents the hue value of the fruit in the second side area image, and H std The hue value of the first-grade mature fruit, w c 、w s 、w t is the weight coefficient, S represents the saturation of the current fruit in the HSV color space, Entropy(I ROI ) is the image entropy of the region of interest output by the YOLOv5 model.

[0071] In the embodiment of the present invention, based on the comprehensive score value M of fruit maturity, score The maturity level grading rules are as follows:

[0072]

[0073] Therefore, only when M score When ≥τ1, a picking instruction is generated, where τ1 and τ2 are the set fruit maturity classification thresholds.

[0074] (6) Calculate the position of each joint of the robotic arm when the end of the robotic arm moves to the position where the ripe fruit is located, and control the end of the robotic arm to move to the position where the ripe fruit is located, pick the ripe fruit, and place the collected fruit in the entrance area of ​​the fruit diameter separation device. The fruit diameter separation device separates fruits of different fruit diameters.

[0075] The picking robot provided by the present invention integrates the functions of environmental perception, autonomous navigation, automatic identification and picking of ripe fruits, and simultaneous automatic classification of fruits of different diameters, which greatly simplifies the fruit picking process and improves picking efficiency.

[0076] The present invention has been described exemplarily. Obviously, the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the scope of protection of the present invention.

Claims

1. A picking robot, characterized in that: The picking robot comprises: The robot body, the fruit diameter separation device installed on the robot body, and the robotic arm; LiDAR and 2D camera integrated into the robot body, and depth camera integrated into the robotic arm; A processor that communicates with the lidar, 2D camera, and depth camera; The lidar scans the environmental point cloud, the processor builds a raster map, and plans the driving path. During the driving process, the depth camera is used to identify the ripe fruits on the corresponding side and determine the position of the ripe fruits in the robotic arm coordinate system. The robotic arm is controlled to pick the ripe fruits and place the collected fruits in the entrance area of ​​the fruit diameter separation device. The fruits enter the fruit diameter separation device through the entrance area.

2. The picking robot according to claim 1, characterized in that: The fruit diameter separation device includes: A cavity is provided, and a separation plate is provided on the bottom plate of the cavity. A plurality of separation grooves are arranged on the separation plate. The width of the separation grooves widens along the rolling direction of the fruit. One end of the separation plate is connected to the entrance area, and the other end is connected to the side wall of the cavity; a plurality of collectors are provided on the side of the separation plate close to the bottom of the cavity, and the collectors are arranged in sequence along the rolling direction of the fruit. A guide plate is provided on the bottom plate between adjacent separation grooves, and the guide plate is arranged perpendicular to the separation plate.

3. The picking robot according to claim 2, characterized in that: The separation plate is arranged obliquely, and the height of the separation plate from the bottom of the cavity decreases along the extending direction of the separation slot.

4. The picking robot according to claim 2, characterized in that: The inlet area is inclined, and the end connected to the separation plate is lower than the bottom of the cavity.

5. A control method for a picking robot according to any one of claims 1 to 4, characterized in that: The method is specifically as follows: (1) Scan the environmental point cloud of the operation area using LiDAR and construct a two-dimensional occupancy map using the Cartographer algorithm; (2) The driving path from the starting position to the target position is planned based on the A* algorithm, and the picking robot drives based on the planned driving path; (3) The picking robot starts the 2D camera while driving and captures an image of the first side area on the side where the 2D camera is located; (4) Detect whether there is fruit in the first side area image. If so, reduce the moving speed of the picking robot and execute step (5). If not, the picking robot continues to move at the current speed and execute step (3); (5) starting the depth camera to capture an image of a second side area on the side, detecting the maturity level of the fruit in the second side area image, determining the image coordinates of the ripe fruit whose maturity level reaches the set level in the image, and converting the image coordinates of the ripe fruit into the robotic arm coordinate system; (6) Calculate the position of each joint of the robotic arm when the end of the robotic arm moves to the location of the ripe fruit, control the end of the robotic arm to move to the location of the ripe fruit, pick the ripe fruit, and place the collected fruit in the entrance area of ​​the fruit diameter separation device.

6. The control method of the picking robot according to claim 5, characterized in that: The A* algorithm is used to find the driving path from the starting position to the target position. The A* algorithm finds the driving path with the minimum cost from the starting position to the target position based on the cost function. The cost function is as follows: f(n)=g(n)+λ·h(n)·exp(-γ·D(n)); Where g(n) is the actual cost of node n, that is, the distance between node n and the starting position; h(n) is the heuristic function of node n, D(n) is the Euclidean distance from node n to the nearest obstacle, λ is the adaptive weight coefficient, and γ is the attenuation coefficient; Among them, the heuristic function h(n) is as follows: h(n)=max(|x g -x n |,|y g -y n |)+η·(1-cosθ n ); Among them, (x g ,y g ) is the grid coordinate of the target position, (x n ,y n ) is the grid coordinate of node n, θ n is the angle between the line connecting node n and the target position and the x-axis, and η is the adjustment coefficient.

7. The control method of the picking robot according to claim 5, characterized in that: The 2D camera inputs the first side area image captured into the YOLOv5 model, and the YOLOv5 model outputs the region of interest where the fruit is located in the first side area image.

8. The control method of the picking robot according to claim 5, characterized in that: The fruit maturity is detected based on the hue value and texture characteristics of the fruit in the HSV color space. The detection formula is as follows: M score =w c ·||H-H std ||2+w s ·S+w t ·Entropy(I ROI ); Among them, M score represents the comprehensive score of fruit maturity, H represents the hue value of the fruit in the second side area image, and H std The hue value of the first-grade mature fruit, w c 、w s 、w t is the weight coefficient, S represents the saturation of the current fruit in the HSV color space, Entropy (I ROI ) is the image entropy of the region of interest output by the YOLOv5 model.

9. The control method of the picking robot according to claim 5, characterized in that: Based on the comprehensive score of fruit maturity M score The maturity level grading rules are as follows: Among them, τ1 and τ2 are the set maturity classification thresholds.

Images