A method and system for controlling picking of agaricus bisporus by multiple robots

Abstract

The present application relates to the technical field of control, specifically a kind of double-spore mushroom control method and system of picking of multiple manipulator, to solve the technical problem of low work efficiency of agricultural product picking in prior art, the position information of double-spore mushroom to be picked on mushroom bed obtained based on the image of mushroom bed shot by camera and the number of manipulator, the area of mushroom bed is zoned, obtain several picking areas;Then based on the position information of double-spore mushroom to be picked on mushroom bed in each picking area, construct graph structure and then convert into tree structure, so that the initial path is more hierarchical and clearer, the initial path is crossed and mutated, to obtain picking path that can improve picking efficiency;Finally, several manipulators pick double-spore mushroom according to corresponding picking path, and different avoidance strategies are implemented according to movement stage during double-spore mushroom picking, realize multi-arm dynamic coordination planning, improve double-spore mushroom picking work efficiency.

Description

Technical Field

[0001] The present invention relates to the field of control technology, and in particular to a control method and system for picking Agaricus bisporus with multiple manipulators. Background Art

[0002] Currently, Agaricus bisporus harvesting in China is largely manual, accounting for approximately 50% to 70% of total production costs. This high labor cost is also inefficient and can easily affect the mushroom house environment, hindering subsequent growth and hindering production efficiency and product quality. With the increasing labor shortage, the design of advanced sorting and harvesting robots is imperative.

[0003] Currently, Agaricus bisporus harvesting robots primarily utilize single-arm and multi-arm harvesting. The efficiency of single-arm harvesting robots falls far short of market demand, making them difficult to replace manual labor. While multi-arm harvesting robots are several times more efficient than single-arm harvesting robots, they can be significantly reduced without high-quality Agaricus bisporus path planning and multi-arm dynamic coordination. Summary of the Invention

[0004] The purpose of the present invention is to provide a control method and system for picking Agaricus bisporus with multiple manipulators.

[0005] The technical solutions of the present invention are as follows:

[0006] A control method for picking Agaricus bisporus with multiple manipulators includes the following operations:

[0007] S1. Based on the conversion matrix from the calibration plate coordinate system to the camera coordinate system and the conversion matrix from the manipulator base coordinate system to the manipulator end coordinate system, obtain the conversion matrix from the camera coordinate system to the manipulator base coordinate system; according to the conversion matrix from the camera coordinate system to the manipulator base coordinate system, align the camera acquisition data and the manipulator end manipulator movement data so that the manipulator end manipulator can move to the corresponding target position in the camera target data;

[0008] S2. Based on the mushroom bed image acquired by the camera, obtain the position information of the Agaricus bisporus to be picked on the mushroom bed; partition the area where the mushroom bed is located according to the position information of the Agaricus bisporus to be picked, as well as the number and grasping range of the manipulator, to obtain several areas to be picked; based on the position information of the Agaricus bisporus to be picked in each of the several areas to be picked, respectively construct a graph structure to obtain several graph structures of the areas to be picked; in the graph structure, the nodes are the Agaricus bisporus, and the edges are the total motion time of the manipulator between the nodes;

[0009] S3. Based on the graph structures of several areas to be picked, a picking path for each area to be picked is obtained; specifically, a tree structure is constructed based on the graph structure of the areas to be picked; all paths from the root node to the tail node in the tree structure are obtained to obtain several initial paths; several initial paths are subjected to cross processing and mutation processing in turn to obtain several optimized paths; it is determined whether there is an optimized path with a path cost less than a cost threshold among the several optimized paths; if so, the optimized path is used as the picking path; if not, the several optimized paths are subjected to several cross processing, mutation processing and path cost judgment operations until a path with a path cost less than the cost threshold is found and used as the picking path;

[0010] S4. Several manipulators pick Agaricus bisporus according to corresponding picking paths.

[0011] The total manipulator motion time between nodes in S2 is obtained based on the vertical distance between the current node and the Agaricus bisporus collecting crawler, the horizontal distance between the current node and the next node, the vertical distance between the next node and the Agaricus bisporus collecting crawler, and the Agaricus bisporus height of the next node.

[0012] In the tree structure of S3, the root node is any node in the graph structure of the area to be picked. The node set formed by the root node and the next-level leaf node is equal to the node set in the graph structure of the area to be picked. The tree nodes on the same initial path are not repeated. The edges between adjacent tree nodes are fitness. The fitness is the total movement time of the manipulator of the corresponding nodes between adjacent tree nodes. The sum of all fitnesses in the initial path is the path cost.

[0013] In the tree structure of S3, the operations for obtaining several optimized paths are as follows: sorting several initial paths by path cost from large to small, selecting the initial paths ranked in the top first percentage, exchanging some paths between adjacent sorted initial paths, and obtaining several cross paths; taking the cross paths whose path costs are less than the cross path cost threshold as learning paths, and all learning paths form a learning path set; selecting any one learning path from the learning path set as the corresponding target learning path for the remaining cross paths, and replacing the intermediate tree nodes of the target learning path with the intermediate tree nodes of the corresponding cross paths, to obtain several variant paths, which are used as several optimized paths.

[0014] The specific operation of obtaining the conversion matrix from the camera coordinate system to the manipulator base coordinate system in S1 is as follows: the camera is fixed on the outside of the manipulator and is stationary relative to the manipulator base; the manipulator at the end of the manipulator holds the calibration plate to change the position of the calibration plate, keeping the relative position of the manipulator end and the calibration plate unchanged, and the relative position of the camera and the calibration plate unchanged, and recording the position changes of the calibration points on the calibration plate observed by the camera in the camera coordinate system when the manipulator end is in different positions, as well as the position changes of the manipulator end relative to the manipulator base, and respectively obtaining several conversion matrices from the calibration plate coordinate system to the camera coordinate system and several conversion matrices from the manipulator base coordinate system to the manipulator end coordinate system; based on several conversion matrices from the calibration plate coordinate system to the camera coordinate system and several conversion matrices from the manipulator base coordinate system to the manipulator end coordinate system, a linear equation group is constructed, and solved based on the singular value decomposition method to obtain the conversion matrix from the camera coordinate system to the manipulator base coordinate system.

[0015] In S3, if there is more than one optimized path with a path cost less than the cost threshold, the path corresponding to the maximum sum of the path node position differences between the multiple picking paths in the current area to be picked and the determined picking paths in other areas to be picked will be taken as the optimal picking path for the current area to be picked, and the operation in S4 will be executed.

[0016] The path node position difference between two picking paths is obtained based on the sum of the distance differences between the nodes with the same sorting.

[0017] In S4, when several manipulators are picking Agaricus bisporus, when the distance between two manipulators is less than the safety distance threshold, the manipulator in the returning stage avoids the manipulator in the picking or / and pre-picking stage, and the manipulator in the picking stage avoids the manipulator in the pre-picking stage.

[0018] A multi-manipulator Agaricus bisporus picking control system, used to implement the above-mentioned multi-manipulator Agaricus bisporus picking control method, comprising:

[0019] A camera, used to acquire images;

[0020] The picking robot has several robotic arms on its base, and a robotic hand at the end of each robotic arm. The picking robot moves, picks Agaricus bisporus, and places Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each area to be picked transmitted by the control unit;

[0021] A computing unit is configured to obtain a conversion matrix from the camera coordinate system to the robot arm base coordinate system based on a conversion matrix from the calibration plate coordinate system to the camera coordinate system and a conversion matrix from the robot arm base coordinate system to the robot arm end coordinate system; and to obtain position information of Agaricus bisporus to be picked on the mushroom bed based on an image of the mushroom bed acquired by the camera; partition an area where the mushroom bed is located according to the position information of Agaricus bisporus to be picked on the mushroom bed, as well as the number and movable range of the manipulator, to obtain a plurality of to-be-picked areas; construct a graph structure based on the position information of Agaricus bisporus to be picked in each of the plurality of to-be-picked areas, to obtain a plurality of to-be-picked area graph structures; and obtain a picking path for each to-be-picked area based on the plurality of to-be-picked area graph structures;

[0022] A control unit is used to align the camera acquisition data and the movement data of the manipulator at the end of the manipulator arm according to the transformation matrix from the camera coordinate system to the manipulator arm base coordinate system in the calculation unit, and control the manipulator at the end of the manipulator arm to move to the corresponding target position in the camera target data; and is used to decompose the picking path of each area to be picked into a picking path action execution signal of each area to be picked, and transmit the picking path action execution signal of each area to be picked to the picking robot.

[0023] When the computing unit constructs the graph structure, the nodes are Agaricus bisporus and the edges are the total movement time of the manipulator between the nodes.

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

[0025] The present invention provides a control method for picking Agaricus bisporus by multiple manipulators. First, based on the conversion matrix from the camera coordinate system to the manipulator base coordinate system, the camera acquisition data and the movement data of the manipulator at the end of the manipulator arm are aligned, so that the manipulator at the end of the manipulator arm can be moved to the corresponding target position in the camera target data, thereby realizing accurate picking of Agaricus bisporus. Then, based on the position information of Agaricus bisporus to be picked on the mushroom bed obtained from the image of the mushroom bed taken by the camera and the number of manipulators, the area where the mushroom bed is located is partitioned to obtain several areas to be picked, so as to realize the simultaneous operation of multiple manipulators. Then, based on the position information of Agaricus bisporus to be picked on the mushroom bed in each area to be picked, the number of manipulators is determined. The location information of Agaricus bisporus is collected and a graph structure is constructed to facilitate the intuitive analysis of the distribution of Agaricus bisporus in each area to be picked. The graph structure of the area to be picked is further converted into a tree structure, making the initial path more hierarchical and clearer. The initial path is crossed and mutated to enhance path diversity, so that the path evolves in the direction of smaller path cost, and a picking path that can improve picking efficiency is obtained. Finally, several robotic arms pick Agaricus bisporus according to the corresponding picking path, and implement different avoidance strategies according to the movement stage during the picking process, realizing multi-arm dynamic coordinated planning and improving work efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] By reading the detailed description of the preferred embodiment below, the solutions and advantages of the present application will become clear to those skilled in the art. The accompanying drawings are only for illustrating the preferred embodiment and are not to be considered as limiting the present invention.

[0027] In the attached figure:

[0028] Figure 1 Schematic diagram of the structure of multiple manipulators in this embodiment. DETAILED DESCRIPTION

[0029] This embodiment provides a multi-manipulator Agaricus bisporus picking control system, which is used to implement a multi-manipulator Agaricus bisporus picking control method, including:

[0030] A camera, used to capture images, specifically images of the calibration plate and the mushroom bed. For example, the camera can be mounted on the outside of the harvesting robot, with its field of view covering the calibration plate and the mushroom bed. The camera is used to capture images of the calibration plate and the mushroom bed, and upload the images to the computing unit.

[0031] The picking robot consists of a robotic arm base, several robotic arms, and several manipulators. The robotic arm base is provided with several robotic arms, each of which is provided with a manipulator. The picking robot moves, picks Agaricus bisporus, and places Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each area to be picked transmitted by the control unit. The schematic diagram of the multiple robotic arms of the picking robot can be found in Figure 1 , the manipulator can move in the vertical direction (y-axis direction) through the slide rail on the manipulator arm, and can move in the vertical direction (z-axis direction) through the telescopic rod on the manipulator arm. At the same time, when the manipulator arm moves upward in the horizontal direction (x-axis direction), it can drive the manipulator to move in the horizontal direction (x-axis direction);

[0032] A computing unit is configured to obtain a conversion matrix from the camera coordinate system to the robot arm base coordinate system based on a conversion matrix from the calibration plate coordinate system to the camera coordinate system and a conversion matrix from the robot arm base coordinate system to the robot arm end coordinate system; and to obtain position information of Agaricus bisporus to be picked on the mushroom bed based on an image of the mushroom bed acquired by the camera; partition an area where the mushroom bed is located according to the position information of Agaricus bisporus to be picked on the mushroom bed, as well as the number and movable range of the manipulator, to obtain a plurality of to-be-picked areas; construct a graph structure based on the position information of Agaricus bisporus to be picked in each of the plurality of to-be-picked areas, to obtain a plurality of to-be-picked area graph structures; and obtain a picking path for each to-be-picked area based on the plurality of to-be-picked area graph structures;

[0033] A control unit is used to align the camera acquisition data and the movement data of the manipulator at the end of the manipulator arm according to the conversion matrix from the camera coordinate system to the manipulator arm base coordinate system in the calculation unit, and control the manipulator at the end of the manipulator arm to move to the corresponding target position in the camera target data; and is used to decompose the picking path of each area to be picked into a picking path action execution signal for each area to be picked, and transmit the picking path action execution signal for each area to be picked to the picking robot;

[0034] The Agaricus bisporus collecting crawler is arranged along the horizontal direction (x-axis direction) and is used for holding and transferring the Agaricus bisporus.

[0035] The control unit can adopt a PLC controller, and adopt PLC to receive and send signals. In this embodiment, Panasonic AFP0HC32ET is selected as the control unit, which has the characteristics of good openness, good portability, high reliability, strong control function, small size and high cost performance.

[0036] A multi-manipulator Agaricus bisporus picking control system may also selectively include the following units or modules:

[0037] The interface unit is used for human-computer interaction interface, parameter setting, image viewing, debugging, etc., including WebServer module, CLI module and ExtServer module.

[0038] The WebServer module is used to provide a Web-based user interface for reading and setting detection parameters and PLC parameters, displaying module operating status and images and detection results, and firmware upgrades.

[0039] The CLI module is used to provide command line interface functions and is the entrance to the system shell. It is used for parameter debugging, automated processing, etc.

[0040] The ExtServer module is used to provide an external interface (TCP / IP) for external management software to call. Third-party software can use it to set and read common detection parameters, etc.

[0041] Business unit, including the MJManager module.

[0042] The MJManager module is used to provide a unified interface for external queries and setting instructions, including setting server parameters, PLC communication management, and configuration file management (saving, loading, backup / restore, etc.).

[0043] Support unit, used to provide network access, file transfer, file sharing and other support services; including SSH Server module, FTP Server module, TFTP Client module and Samba Server module.

[0044] SSH Server module, used to provide SSH services.

[0045] The FTP Server module provides file transfer services to facilitate debugging and firmware upgrades using the CLI.

[0046] The TFTP Client module is used to download files from or upload files to a TFTP server.

[0047] Samba Server module, used to provide file sharing services.

[0048] This embodiment also provides a multi-manipulator Agaricus bisporus picking control method based on the above multi-manipulator Agaricus bisporus picking control system, and the operation is as follows.

[0049] S1. Based on the conversion matrix from the calibration plate coordinate system to the camera coordinate system and the conversion matrix from the robot base coordinate system to the robot end coordinate system, the conversion matrix from the camera coordinate system to the robot base coordinate system is obtained; according to the conversion matrix from the camera coordinate system to the robot base coordinate system, the camera acquisition data and the robot end manipulator movement data are aligned so that the robot end manipulator can move to the corresponding target position in the camera target data.

[0050] Based on the conversion matrix from the camera coordinate system to the robotic arm base coordinate system, the camera acquisition data and the movement data of the manipulator at the end of the robotic arm are aligned, so that the manipulator at the end of the robotic arm moves to the corresponding target position in the camera target data, achieving accurate picking of Agaricus bisporus.

[0051] The specific operation of obtaining the transformation matrix from the camera coordinate system to the manipulator base coordinate system is as follows: the camera is fixed on the outside of the manipulator and is stationary relative to the manipulator base; the manipulator at the end of the manipulator holds the calibration plate to change the position of the calibration plate, and keeps the relative position of the manipulator end and the calibration plate unchanged, and the relative position of the camera and the calibration plate unchanged (the transformation matrix from the camera coordinate system to the manipulator base coordinate system remains unchanged). At the same time, the position changes of the calibration points on the calibration plate observed by the camera in the camera coordinate system and the position changes of the manipulator end relative to the manipulator base are recorded when the manipulator end is in different positions, and several transformation matrices from the calibration plate coordinate system to the camera coordinate system and several transformation matrices from the manipulator base coordinate system to the manipulator end coordinate system are obtained respectively; based on several transformation matrices from the calibration plate coordinate system to the camera coordinate system and several transformation matrices from the manipulator base coordinate system to the manipulator end coordinate system, a linear equation system is constructed, and solved based on the singular value decomposition method to obtain the transformation matrix from the camera coordinate system to the manipulator base coordinate system.

[0052] The above linear equations are as follows:

[0053] ,

[0054] ,

[0055] is the transformation matrix from the manipulator base coordinate system to the manipulator end coordinate system when the calibration plate is in the first pose, is the transformation matrix from the camera coordinate system to the robot base coordinate system, is the conversion matrix from the calibration plate coordinate system to the camera coordinate system when the calibration plate is in the first pose, is the conversion matrix from the calibration plate coordinate system to the robotic arm coordinate system, is the transformation matrix from the manipulator base coordinate system to the manipulator end coordinate system when the calibration plate is in the second posture, It is the transformation matrix from the calibration plate coordinate system to the camera coordinate system when the calibration plate is in the first pose.

[0056] Combining the above formulas we get:

[0057] ,

[0058] The adjustment formula is:

[0059] ( ,

[0060] It can be seen that the above formula can be regarded as A×X =X×B, A=( ,B= , the singular value decomposition method is used to solve this equation and obtain the transformation matrix X from the camera coordinate system to the robot base coordinate system.

[0061] According to the conversion matrix from the camera coordinate system to the robot base coordinate system, and the conversion matrix from the robot base coordinate system to the robot end coordinate system, the camera acquisition data and the movement data of the manipulator at the end of the robot arm are aligned, and the position of the Agaricus bisporus seen by the camera is accurately converted to a coordinate position that the robot arm can understand, so that the manipulator at the end of the robot arm can accurately move to the corresponding target position in the camera target data.

[0062] If there is more than one robotic arm on the robotic arm base, the camera acquisition data and the movement data of several robotic arm end manipulators can be aligned separately based on the conversion matrix from the camera coordinate system to the robotic arm base coordinate system, and the conversion matrices from several robotic arm base coordinate systems to the robotic arm end coordinate systems. The position of the Agaricus bisporus seen by the camera can be accurately converted to a coordinate position that can be understood by each robotic arm, so that the manipulator at the end of each robotic arm can accurately move to the corresponding target position in the camera target data.

[0063] S2. Based on the mushroom bed image acquired by the camera, the position information of the Agaricus bisporus to be picked on the mushroom bed is obtained; according to the position information of the Agaricus bisporus to be picked on the mushroom bed, as well as the number and movable range of the manipulator, the area where the mushroom bed is located is partitioned to obtain several areas to be picked; based on the position information of the Agaricus bisporus to be picked in each of the several areas to be picked, a graph structure is constructed respectively to obtain several graph structures of the areas to be picked; the nodes in the graph structure are the Agaricus bisporus, and the edges are the total movement time of the manipulator between the nodes.

[0064] Based on the position information of the Agaricus bisporus to be picked on the mushroom bed and the number of robots obtained from the mushroom bed image, the area where the mushroom bed is located is partitioned to obtain several areas to be picked, which are used to realize the simultaneous operation of multiple robots; at the same time, based on the position information of the Agaricus bisporus to be picked on the mushroom bed in each area to be picked, a graph structure is constructed to facilitate the intuitive analysis of the distribution of Agaricus bisporus in each area to be picked, which is conducive to the subsequent planning of the robot picking path.

[0065] First, based on the camera-captured mushroom bed image, the location information of the Agaricus bisporus to be harvested on the bed is obtained. Specifically, target detection (including but not limited to target detection using the Yolov8 model) is performed on the mushroom bed image, which contains depth information and RGB pixel information. Agaricus bisporus is identified and the location information of the Agaricus bisporus to be harvested is obtained.

[0066] Then, according to the location information of the Agaricus bisporus to be picked on the mushroom bed, as well as the number of manipulators used for picking Agaricus bisporus and the movable range of the manipulators, the area where the mushroom bed is located is divided into several areas to be picked, so that multiple manipulators can work simultaneously and improve the subsequent Agaricus bisporus picking efficiency.

[0067] Finally, based on the position information of the mushrooms to be picked in each of the several areas to be picked, a graph structure was constructed to obtain several graph structures of the areas to be picked that can reflect the growth environment of the mushrooms and the positional relationship between the mushrooms. In the graph structure, the nodes are the mushrooms, and the edges are the total robot movement time between the nodes (the mushrooms). The total robot movement time between the nodes is calculated based on the vertical distance between the current node and the mushroom collection track, the horizontal distance between the current node and the next node, the vertical distance between the next node and the mushroom collection track, and the height of the next node. Specifically, it is the time it takes for the robot to move the vertical distance between the current node and the mushroom collection track, the time it takes for the robot to place the mushroom from the current node to the mushroom collection track, the time it takes for the robot to move the horizontal distance between the current node and the next node, the time it takes for the robot to move the vertical distance between the next node and the mushroom collection track, the time it takes for the robot to move from a preset height to the height of the next node, the time it takes for the robot to grab the mushroom from the next node, and the time it takes for the robot to move from the height of the next node to the preset height.

[0068] For example, the total movement time of the manipulator between the Agaricus bisporus node No. 1 and the Agaricus bisporus node No. 2 = the time for the manipulator to move the vertical distance between node No. 1 and the Agaricus bisporus collection track + the time for the manipulator to place the Agaricus bisporus at node No. 1 to the Agaricus bisporus collection track + the time for the manipulator to move the horizontal distance between node No. 1 and node No. 2 + the time for the manipulator to move the vertical distance between node No. 2 and the Agaricus bisporus collection track + the time for the manipulator to move down from the preset height to the height of the Agaricus bisporus at node No. 2 + the time for the manipulator to grab the Agaricus bisporus at node No. 2 + the time for the manipulator to move from the height of the Agaricus bisporus at node No. 2 to the preset height.

[0069] S3. Based on the graph structures of several areas to be picked, a picking path for each area to be picked is obtained.

[0070] The graph structure of the area to be picked is converted into a tree structure, making the initial path more hierarchical and clearer. The initial path is then crossed and mutated to enhance path diversity and evolve towards a direction with lower path cost, thereby helping to find a more time-saving picking path and obtaining a picking path that can improve subsequent picking efficiency.

[0071] In Agaricus bisporus picking path planning, the transformed tree structure organizes possible paths into a tree hierarchy.

[0072] Taking a single to-be-picked area graph structure as an example, the operation of obtaining the picking path is as follows.

[0073] Step 1: Based on the graph structure of the area to be picked, a tree structure is constructed. In the tree structure, the root node is any node in the graph structure of the area to be picked, and the node set formed by the root node and the next-level leaf nodes is equal to the node set in the graph structure of the area to be picked to ensure that no mushrooms are missed. For example, there are 5 Agaricus bisporus in the current area to be picked: No. 1, No. 2, No. 3, No. 4, and No. 5. Then there are a total of 5 nodes in the graph structure of the area to be picked. If No. 1 is selected as the root node in the tree structure, the next-level leaf nodes of the root node are No. 2, No. 3, No. 4, and No. 5.

[0074] Step 2: Obtain all paths from the root node to the tail node in the tree structure to obtain several initial paths. Tree nodes on the same initial path are not repeated to prevent repeated picking. For example, starting from root node 1, there can be an initial path of 1-2-3-4-5, and there can also be initial paths with non-repeated tree nodes such as 1-2-4-3-5, 1-3-5-4-2, etc. to avoid repeated picking. At the same time, the edge between adjacent tree nodes is the fitness, and the fitness is the total movement time of the robot arm corresponding to the nodes between adjacent tree nodes. The sum of all fitness in the initial path is the path cost.

[0075] Step 3: When the number of Agaricus bisporus to be picked is less than the picking number threshold, that is, when the number of Agaricus bisporus to be picked is small, the initial path corresponding to the minimum path cost is used as the picking path of the area to be picked, or in order to obtain the picking path with the shortest picking time, several initial paths are sequentially cross-processed and mutated to enrich the paths, and the initial paths are evolved in a better direction to obtain several optimized paths; when the number of Agaricus bisporus to be picked is not less than the picking number threshold, that is, when the number of Agaricus bisporus to be picked is large, in order to quickly obtain the path with the shortest picking time, several initial paths are sequentially cross-processed and mutated to enrich the paths, and the initial paths are evolved in a better direction to obtain several optimized paths.

[0076] The process of obtaining several optimized paths involves sorting the initial paths from highest to lowest path cost, selecting the initial paths that rank in the top percentile (an even number of paths), and then performing partial path swaps between adjacent sorted initial paths to obtain several intersecting paths. During this partial path swapping process, if duplicate tree nodes appear on the swapped paths, only the tree nodes at the top of the execution order are retained. If a tree node in the set of tree nodes is missing from the swapped paths, the missing node is used as the tail node of the swapped paths. For example, if two initial paths are ordered 1-5-4-2-3 and 1-2-3-4-5, with the third node as the path change point (the path change point can be set according to actual needs), after partial path swapping, the paths 1-5-3-4-5 and 1-2-4-2-3 are obtained. Duplicate nodes are then removed and missing nodes are added, resulting in the paths 1-5-3-4-2 and 1-2-4-3-5. Cross paths with path costs less than a cross-path cost threshold (i.e., cross paths with short picking times) are then selected as learning paths. All learning paths form a learning path set. For each of the remaining cross paths, a learning path is selected from the learning path set as the corresponding target learning path. The intermediate tree nodes of the target learning path are then replaced with the intermediate tree nodes of the corresponding cross path (the number of intermediate tree nodes replaced can be selected based on the length of the path). This reduces the path cost at the intermediate tree nodes of the paths, resulting in several mutated paths, which are then used as the optimized paths. Furthermore, when replacing intermediate tree nodes in cross paths, if duplicate tree nodes appear on the replaced path, the duplicated tree nodes on the original cross path are deleted. If a tree node from the tree node set is missing on the replaced path, the missing tree node is used as the tail node of the replaced path. For example, if the target learning path is 1-5-3-4-2 and the corresponding cross path is 1-2-4-3-5, after mutation (with one intermediate tree node), the path 1-2-3-3-5 is obtained. Then, the duplicated nodes are deleted and the missing nodes are added, resulting in the mutated path 1-2-3-5-4.

[0077] In addition, if there is more than one optimized path with a path cost less than the cost threshold, that is, if there are multiple optimized paths with a path cost less than the cost threshold, the path with the minimum path cost is used as the picking path. Alternatively, to prevent multiple manipulators from colliding due to being too close to each other during the picking process, the path with the maximum path node position difference and the maximum path node position difference with the determined picking paths or optimal picking paths in other picking areas is used as the optimal picking path for the current picking area, and the operation in S4 is executed.

[0078] The path node position difference between two picking paths is based on the sum of the distance differences between nodes with the same sorting. The farther the distance between nodes with the same sorting, the greater the path node position difference. For example, if picking path A is 1-4-3-5-2 and picking path B is abcdef, the path node position difference between picking paths A and B is: f =( △p (1,a)+ △ p(4,b)+ △p (3,c)+ △p (5,d)+ △p (2,e)+ △p (2,f)) / 6, △p ( i , j ) is a node i With node j The distance difference between i =1,2,3,4,5, j =a,b,c,d,e,f, f is the path node position difference.

[0079] S4. Several manipulators pick Agaricus bisporus according to corresponding picking paths.

[0080] To further prevent robot collisions, when multiple robots are harvesting Agaricus bisporus, if the distance between them falls below a safety distance threshold, the robot in the returning phase will yield to the robot in the harvesting or / and pre-harvesting phases, and the robot in the harvesting phase will yield to the robot in the pre-harvesting phase. A robot in the pre-harvesting phase is one that is moving from its starting point (the Agaricus placement point) toward the target Agaricus bisporus.

[0081] Taking three robotic arms as an example, the low-priority robotic arms are controlled to avoid the high-priority robotic arms. At the beginning, the priorities of the three robotic arms are set to 1, 2, and 3 respectively. If robotic arm No. 1 returns after picking, the priority of robotic arm No. 1 is set to 0, while the priority of the other robotic arms No. 2 and No. 3 that are picking is increased by 1, with a maximum of no more than 5, to ensure that each robotic arm has a different priority. When the distance between robotic arm No. 1 and robotic arm No. 2 or 3 is less than the safety distance threshold, robotic arm No. 1 avoids. When robotic arm No. 1 places the Agaricus bisporus at the starting point and starts a new picking task, the priority of robotic arm No. 1 is set to 1.

[0082] The present embodiment provides a control method for picking Agaricus bisporus with multiple manipulators. First, based on the conversion matrix from the camera coordinate system to the manipulator base coordinate system, the camera acquisition data and the movement data of the manipulator at the end of the manipulator arm are aligned, so that the manipulator at the end of the manipulator arm can move to the corresponding target position in the camera target data, thereby realizing accurate picking of Agaricus bisporus. Then, based on the position information of Agaricus bisporus to be picked on the mushroom bed obtained from the image of the mushroom bed taken by the camera and the number of manipulators, the area where the mushroom bed is located is partitioned to obtain several areas to be picked, so as to realize the simultaneous operation of multiple manipulators ... The location information of the Agaricus bisporus to be picked is used to construct a graph structure, which facilitates the intuitive analysis of the distribution of Agaricus bisporus in each area to be picked. The graph structure of the area to be picked is further converted into a tree structure, making the initial path more hierarchical and clearer. The initial path is crossed and mutated to enhance path diversity, so that the path evolves in the direction of smaller path cost, and a picking path that can improve picking efficiency is obtained. Finally, several robotic arms pick Agaricus bisporus according to the corresponding picking path, and implement different avoidance strategies according to the movement stage during the picking process, realizing multi-arm dynamic coordinated planning and improving work efficiency.

Claims

1. A multi-manipulator picking Agaricus bisporus control method, characterized in that: The following operations are included: S1. Based on the conversion matrix from the calibration plate coordinate system to the camera coordinate system and the conversion matrix from the manipulator base coordinate system to the manipulator end coordinate system, obtain the conversion matrix from the camera coordinate system to the manipulator base coordinate system; according to the conversion matrix from the camera coordinate system to the manipulator base coordinate system, align the camera acquisition data and the manipulator end manipulator movement data so that the manipulator end manipulator can move to the corresponding target position in the camera target data; S2. Based on the mushroom bed image acquired by the camera, obtaining the position information of the Agaricus bisporus to be picked on the mushroom bed; Based on the location information of the Agaricus bisporus to be picked, as well as the number of manipulators and their grasping range, the mushroom bed area is partitioned to obtain several areas to be picked. Based on the location information of the Agaricus bisporus to be picked in each of the areas to be picked, a graph structure is constructed to obtain several graph structures of the areas to be picked. The nodes in the graph structure are Agaricus bisporus, and the edges are the total movement time of the manipulators between the nodes. S3. Based on the graph structures of several areas to be picked, obtain a picking path for each area to be picked; Specifically, a tree structure is constructed based on the graph structure of the area to be picked; all paths from the root node to the tail node in the tree structure are obtained to obtain several initial paths; the initial paths are subjected to cross processing and mutation processing in turn to obtain several optimized paths; the operations for obtaining the several optimized paths are specifically as follows: the initial paths are sorted from large to small by path cost, the initial paths ranked in the first percentile are selected, and some paths are exchanged between adjacent sorted initial paths to obtain several cross paths; the cross paths with path costs less than the cross path cost threshold are used as learning paths, and all learning paths form a learning path set; the remaining cross paths each select one learning path from the learning path set as the corresponding target learning path, and the intermediate tree node of the target learning path is replaced with the intermediate tree node of the corresponding cross path to obtain several mutated paths as several optimized paths; Determine whether there is an optimization path with a path cost less than the cost threshold among several optimization paths; if so, take the optimization path as the picking path; if not, perform several cross processing, mutation processing and path cost judgment operations on several optimization paths until a path with a path cost less than the cost threshold appears and is taken as the picking path; S4. Several manipulators pick Agaricus bisporus according to corresponding picking paths.

2. The multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: In S2, the total manipulator movement time between nodes is obtained based on the vertical distance between the current node and the Agaricus bisporus collection track, the horizontal distance between the current node and the next node, the vertical distance between the next node and the Agaricus bisporus collection track and the Agaricus bisporus height of the next node.

3. The multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: In the tree structure of S3, the root node is any node in the graph structure of the area to be picked. The node set formed by the root node and the next-level leaf node is equal to the node set in the graph structure of the area to be picked. The tree nodes on the same initial path are not repeated. The edges between adjacent tree nodes are fitness. The fitness is the total movement time of the manipulator of the corresponding nodes between adjacent tree nodes. The sum of all fitnesses in the initial path is the path cost.

4. The multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: In S1, the operation of obtaining the conversion matrix from the camera coordinate system to the robot base coordinate system is specifically as follows: The camera is fixed on the outside of the robotic arm and is stationary relative to the robotic arm base; the robotic handheld calibration plate at the end of the robotic arm changes the position of the calibration plate, keeping the relative position of the robotic arm end and the calibration plate unchanged, and the relative position of the camera and the calibration plate unchanged, and records the position changes of the calibration points on the calibration plate observed by the camera in the camera coordinate system when the robotic arm end is in different positions, as well as the position changes of the robotic arm end relative to the robotic arm base, and obtains several conversion matrices from the calibration plate coordinate system to the camera coordinate system and several conversion matrices from the robotic arm base coordinate system to the robotic arm end coordinate system respectively; based on several conversion matrices from the calibration plate coordinate system to the camera coordinate system and several conversion matrices from the robotic arm base coordinate system to the robotic arm end coordinate system, a linear equation group is constructed, and solved based on the singular value decomposition method to obtain the conversion matrix from the camera coordinate system to the robotic arm base coordinate system.

5. The multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: In S3, if there are more than one optimized paths whose path cost is less than the cost threshold, the path corresponding to the maximum sum of the path node position differences between the multiple picking paths of the current area to be picked and the picking paths determined in other areas to be picked will be taken as the optimal picking path for the current area to be picked, and the operation in S4 will be executed.

6. The multi-manipulator picking Agaricus bisporus control method according to claim 5, characterized in that: The path node position difference between two picking paths is obtained based on the sum of the distance differences between the nodes with the same sorting.

7. The multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: In said S4, during the process of several manipulators picking Agaricus bisporus, when the distance between two manipulators is less than the safety distance threshold, the manipulator in the returning stage avoids the manipulator in the picking or / and pre-picking stage, and the manipulator in the picking stage avoids the manipulator in the pre-picking stage.

8. A multi-manipulator picking Agaricus bisporus control system, used to implement the multi-manipulator picking Agaricus bisporus control method according to claim 1, characterized in that: include: A camera, used to acquire images; The picking robot has several robotic arms on its base, and a robotic hand at the end of each robotic arm. The picking robot moves, picks Agaricus bisporus, and places Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each area to be picked transmitted by the control unit; A calculation unit, configured to obtain a conversion matrix from the camera coordinate system to the manipulator base coordinate system based on a conversion matrix from the calibration plate coordinate system to the camera coordinate system and a conversion matrix from the manipulator base coordinate system to the manipulator end coordinate system; and for obtaining position information of Agaricus bisporus to be picked on the mushroom bed based on the mushroom bed image acquired by the camera; According to the location information of Agaricus bisporus to be picked on the mushroom bed, as well as the number and movable range of the manipulators, the area where the mushroom bed is located is divided into several areas to be picked; Based on the position information of the Agaricus bisporus to be picked in each of the plurality of areas to be picked, a graph structure is constructed to obtain a plurality of graph structures of the areas to be picked; and for obtaining a picking path for each area to be picked based on a plurality of graph structures of areas to be picked; A control unit is used to align the camera acquisition data and the movement data of the manipulator at the end of the robotic arm according to the conversion matrix from the camera coordinate system to the manipulator base coordinate system in the calculation unit, and control the manipulator at the end of the robotic arm to move to the corresponding target position in the camera target data; And it is used to decompose the picking path of each area to be picked into the picking path action execution signal of each area to be picked, and transmit the picking path action execution signal of each area to be picked to the picking robot.

9. The multi-manipulator picking Agaricus bisporus control system according to claim 8, characterized in that: When the computing unit constructs the graph structure, the nodes are Agaricus bisporus and the edges are the total movement time of the manipulator between the nodes.

Images