Multi-manipulator agaricus bisporus picking control method and system

Abstract

The invention relates to the technical field of control, in particular to a multi-manipulator agaricus bisporus picking control method and system, and aims to solve the technical problem of low agricultural product picking working efficiency in the prior art. The area where the mushroom bed is located is partitioned, and a plurality of areas to be picked are obtained; 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 and then converted into a tree structure, so that the initial path is more hierarchical and clearer, and the initial path is subjected to intersection and variation processing to obtain a picking path capable of improving the picking efficiency; and finally, the multiple manipulators pick the agaricus bisporus according to the corresponding picking paths, different avoiding strategies are implemented according to the movement stages in the agaricus bisporus picking process, multi-arm dynamic coordination planning is achieved, and the agaricus bisporus picking work efficiency is improved.

Description

Technical Field

[0001] The 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] At present, the domestic mushroom picking operation is basically done manually, and its cost accounts for about 50% to 70% of the cost. The labor cost is high, and the manual picking efficiency is low. It is also easy to affect the mushroom house environment and the subsequent growth of mushrooms, which is not conducive to the improvement of production efficiency and product quality. With the increasing shortage of labor, it is imperative to design advanced sorting and picking robots.

[0003] At present, the picking end of Agaricus bisporus picking robots mainly consists of single-arm picking and multi-arm picking. The work efficiency of single-arm picking robots is far from meeting market demand and it is difficult to replace manual labor. Although the work efficiency of multi-arm picking robots is several times higher than that of single-arm picking, if there is no high-quality Agaricus bisporus path planning and multi-arm dynamic coordination planning, the work efficiency will be greatly reduced. 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 solution of the present invention is as follows: A control method for picking Agaricus bisporus with multiple manipulators includes the following operations: 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, obtain the conversion matrix from the camera coordinate system to the robot base coordinate system; according to the conversion matrix from the camera coordinate system to the robot base coordinate system, align the camera acquisition data and the movement data of the manipulator at the end of the robot arm, so that the manipulator at the end of the robot arm can move to the corresponding target position in the camera target data; 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 movement time of the manipulator between the nodes; S3. Based on several graph structures of the 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 whose path cost is less than a cost threshold among several optimized paths; if so, the optimized path is used as a picking path; if not, several optimized paths perform several cross processing, mutation processing and path cost judgment operations until a path whose path cost is less than the cost threshold appears as a picking path; S4. Several manipulators pick Agaricus bisporus according to corresponding picking paths.

[0006] 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.

[0007] 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 fitness in the initial path is the path cost.

[0008] 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 in the 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 for the remaining cross paths as the corresponding target learning path, and replacing the intermediate tree nodes of the corresponding cross paths with the intermediate tree nodes of the target learning paths, and obtaining several variant paths as several optimized paths.

[0009] The operations for obtaining the conversion matrix from the camera coordinate system to the robot base coordinate system in S1 are as follows: the camera is fixed on the outside of the robot and is stationary relative to the robot base; the robot at the end of the robot holds the calibration plate to change the position and posture of the calibration plate, keeping the relative position between the end of the robot and the calibration plate unchanged, and the relative position between the camera and the calibration plate unchanged, and recording the position and posture changes of the calibration points on the calibration plate observed by the camera in the camera coordinate system when the end of the robot is in different positions, as well as the position and posture changes of the end of the robot with respect to the robot base, and obtaining several conversion matrices from the calibration plate coordinate system to the camera coordinate system and several conversion matrices from the robot base coordinate system to the robot 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 robot base coordinate system to the robot 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 robot base coordinate system.

[0010] 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 value of the sum of 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 is taken as the optimal picking path for the current area to be picked, and the operation in S4 is executed.

[0011] 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.

[0012] In 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 and / or pre-picking stage, and the manipulator in the picking stage avoids the manipulator in the pre-picking stage.

[0013] A multi-manipulator picking Agaricus bisporus control system, used to implement the above-mentioned multi-manipulator picking Agaricus bisporus control method, comprising: A camera, used to acquire images; The picking robot has several mechanical arms on its base, and a mechanical hand is provided at the end of each mechanical arm. The picking robot performs the actions of moving, picking Agaricus bisporus, and placing Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each to-be-picked area transmitted by the control unit; A calculation unit is used 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 the Agaricus bisporus to be picked on the mushroom bed based on the mushroom bed image acquired by the camera; partition the area where the mushroom bed is located 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 robot, to obtain a number of 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 a number of graph structures of the areas to be picked; and to obtain a picking path for each area to be picked based on the several graph structures of the 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 transformation matrix from the camera coordinate system to the robot arm 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 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.

[0014] 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.

[0015] The beneficial effects of the present invention are: The present invention 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 be moved to the corresponding target position in the camera target data, so as to achieve accurate picking of Agaricus bisporus. Then, based on the position information of Agaricus bisporus to be picked on the mushroom bed obtained from the mushroom bed image taken by the camera and the number of manipulators, the area where the mushroom bed is located is partitioned to obtain a plurality of areas to be picked, so as to achieve simultaneous work of multiple manipulators. Then, based on the position information of Agaricus bisporus to be picked on the mushroom bed on each area to be picked, the number of manipulators is divided into two parts. The location information of picking Agaricus bisporus is collected and a graph structure is constructed to facilitate 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, so that the initial path is more hierarchical and clearer. The initial path is crossed and mutated to enhance the path diversity, so that the path evolves in the direction of smaller path cost, and a picking path that can improve the picking efficiency is obtained. Finally, several manipulators pick Agaricus bisporus according to the corresponding picking path, and implement different avoidance strategies according to the movement stage during the picking process of Agaricus bisporus, so as to realize multi-arm dynamic coordination planning and improve work efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0017] In the attached picture: Figure 1 Schematic diagram of the structure of multiple manipulators in this embodiment. DETAILED DESCRIPTION

[0018] This embodiment provides a multi-manipulator picking Agaricus bisporus control system, which is used to implement a multi-manipulator picking Agaricus bisporus control method, including: The camera is used to acquire images, specifically, to acquire images of the calibration plate and the mushroom bed. For example, it can be fixed on the outside of the picking robot, with its shooting field of view covering the calibration plate and the mushroom bed, and is used to capture images of the calibration plate and the mushroom bed, and upload the images of the calibration plate and the mushroom bed to the computing unit; The picking robot is composed of a robot base, several robot arms, and several manipulators; the robot base is provided with several robot arms, each robot arm is provided with a manipulator, and the picking robot performs the actions of moving, picking Agaricus bisporus, and placing Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each to-be-picked area transmitted by the control unit; the schematic diagram of the structure of multiple robot 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); A calculation unit is used 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 the Agaricus bisporus to be picked on the mushroom bed based on the mushroom bed image acquired by the camera; partition the area where the mushroom bed is located 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 robot, to obtain a number of 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 a number of graph structures of the areas to be picked; and to obtain a picking path for each area to be picked based on the several graph structures of the areas to be picked; A control unit, used for aligning 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 controlling the manipulator at the end of the manipulator arm to move to the corresponding target position in the camera target data; and for decomposing the picking path of each area to be picked into a picking path action execution signal of each area to be picked, and transmitting the picking path action execution signal of each area to be picked to the picking robot; The Agaricus bisporus collecting crawler is arranged along the horizontal direction (x-axis direction) and is used for holding and transferring the Agaricus bisporus.

[0019] 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.

[0020] A multi-manipulator control system for picking Agaricus bisporus may also selectively include the following units or modules: The interface unit is used for human-computer interaction interface, parameter setting, image viewing, debugging, etc., including WebServer module, CLI module and ExtServer module.

[0021] 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.

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

[0023] 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 commonly used detection parameters, etc.

[0024] Business unit, including MJManager module.

[0025] 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 (save, load, backup / restore, etc.).

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

[0027] SSH Server module, used to provide SSH service.

[0028] FTP Server module, used to provide file transfer services, to facilitate debugging and firmware upgrades using CLI.

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

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

[0031] 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.

[0032] 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, obtain the conversion matrix from the camera coordinate system to the robot base coordinate system; according to the conversion matrix from the camera coordinate system to the robot base coordinate system, align the camera acquisition data and the movement data of the manipulator at the end of the robot arm, so that the manipulator at the end of the robot arm can move to the corresponding target position in the camera target data.

[0033] 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, thereby achieving accurate picking of Agaricus bisporus.

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

[0035] The above linear equations are as follows: , , is the transformation matrix from the robot base coordinate system to the robot end coordinate system when the calibration plate is in the first posture, is the transformation matrix from the camera coordinate system to the robot base coordinate system, is the transformation matrix from the calibration plate coordinate system to the camera coordinate system when the calibration plate is in the first pose, is the transformation matrix from the calibration plate coordinate system to the robot arm coordinate system, is the transformation matrix from the robot base coordinate system to the robot 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.

[0036] Combining the above formulas we get: , The adjustment formula is: ( , It can be seen that the above formula can be regarded as A×X =X×B, A=( ,B= , the equation is solved by the singular value decomposition method to obtain the transformation matrix X from the camera coordinate system to the robot base coordinate system.

[0037] According to the transformation matrix from the camera coordinate system to the robot base coordinate system, and the transformation 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 the 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.

[0038] If there is more than one robotic arm on the robotic arm base, the camera acquisition data and the movement data of several manipulators at the end of the robotic arm can be aligned based on the conversion matrix from the camera coordinate system to the robotic arm base coordinate system and the conversion matrix 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 the coordinate position that each robotic arm can understand, so that the manipulator at the end of each robotic arm can accurately move to the corresponding target position in the camera target data.

[0039] 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 divided into 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, graph structures are 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.

[0040] Based on the position information of the Agaricus bisporus to be picked on the mushroom bed obtained from the mushroom bed image and the number of robots, the area where the mushroom bed is located is partitioned to obtain several areas to be picked, which are used to realize the simultaneous work 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 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.

[0041] First, 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. Specifically, the mushroom bed image containing depth information and RGB pixel information is subjected to target detection processing (target detection includes but is not limited to implementation through the Yolov8 model), the Agaricus bisporus is identified, and the position information of the Agaricus bisporus to be picked on the mushroom bed is obtained.

[0042] 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.

[0043] Finally, based on the position information of the mushrooms to be picked in each of the several areas to be picked, the graph structures are constructed respectively, and several graph structures of the areas to be picked that can reflect the growth environment of the mushrooms and the position relationship between the mushrooms are obtained; in the graph structure, the nodes are the mushrooms, and the edges are the total movement time of the manipulator between the nodes (the mushrooms), and the total movement time of the manipulator between the nodes is obtained 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 mushrooms at the next node. Specifically, it is the time for the manipulator to move the vertical distance between the current node and the mushroom collection track, the time for the manipulator to place the mushrooms at the current node to the mushroom collection track, the time for the manipulator to move the horizontal distance between the current node and the next node, the time for the manipulator to move the vertical distance between the next node and the mushroom collection track, the time for the manipulator to move from the preset height to the height of the mushrooms at the next node, the time for the manipulator to grab the mushrooms at the next node, and the time for the manipulator to move from the height of the mushrooms at the next node to the preset height.

[0044] For example, the total movement time of the manipulator between Agaricus bisporus node No. 1 and Agaricus bisporus node No. 2 = the time for the manipulator to move the vertical distance between node 1 and the Agaricus bisporus collecting track + the time for the manipulator to place the Agaricus bisporus from node 1 to the Agaricus bisporus collecting track + the time for the manipulator to move the horizontal distance between node 1 and node 2 + the time for the manipulator to move the vertical distance between node 2 and the Agaricus bisporus collecting 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.

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

[0046] 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 crossed and mutated to enhance path diversity and evolve in the direction of lower path cost, which helps to find a more time-saving picking path and obtain a picking path that can improve subsequent picking efficiency.

[0047] In Agaricus bisporus picking path planning, the transformed tree structure organizes possible paths into tree hierarchies.

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

[0049] 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 node is equal to the node set in the graph structure of the area to be picked to ensure that no picking is missed. For example, there are 5 Agaricus bisporus in the current area to be picked, namely, No. 1, No. 2, No. 3, No. 4 and No. 5. Then there are 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.

[0050] Step 2: Get all the paths from the root node to the tail node in the tree structure and get several initial paths. The tree nodes on the same initial path are not repeated to prevent repeated picking. For example, starting from root node No. 1, there can be an initial path of 1-2-3-4-5, and there can also be initial paths with non-repeating 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 manipulator of the corresponding nodes between adjacent tree nodes. The sum of all fitness in the initial path is the path cost.

[0051] 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 cross-processed and mutated in turn 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 a path with a short picking time, several initial paths are cross-processed and mutated in turn to enrich the paths, and the initial paths are evolved in a better direction to obtain several optimized paths.

[0052] The operation of obtaining several optimized paths is specifically to sort several initial paths from large to small in terms of path cost, select the initial paths ranked in the first percentage (the total number is an even number), and perform partial path exchange between adjacent sorted initial paths to obtain several cross paths. In the process of partial path exchange, if duplicate tree nodes appear on the exchanged path, only the tree nodes with the front path execution order are retained. If the tree nodes in the tree node set are missing on the exchanged path, the missing tree nodes are used as the tail nodes of the exchanged path. For example, there are two initial paths with adjacent sorting of 1-5-4-2-3 and 1-2-3-4-5, and the third position point is used as the position point of path change (the position point of path change can be set according to actual needs). After partial path exchange, the paths 1-5-3-4-5 and 1-2-4-2-3 are obtained first, and then the duplicate nodes are deleted and the missing nodes are added to obtain the paths 1-5-3-4-2 and 1-2-4-3-5. Then, the cross paths whose path cost is less than the cross path cost threshold, that is, the cross paths with small picking time, are taken as learning paths, and all learning paths form a learning path set; the remaining cross paths select one learning path from the learning path set as the corresponding target learning path, and replace the intermediate tree nodes of the corresponding cross paths with the intermediate tree nodes of the target learning path (the number of intermediate tree nodes to be replaced can be selected according to the length of the path), and reduce the path cost from the intermediate tree nodes of the path, and obtain several variant paths as several optimized paths. At the same time, in the process of replacing the intermediate tree nodes of the cross paths, if duplicate tree nodes appear on the replaced path, the duplicate tree nodes on the original cross paths are deleted, and if the replaced path lacks a tree node in the tree node set, the missing tree node is used as the tail node of the replaced path. For example, there is 1-5-3-4-2 as the target learning path and 1-2-4-3-5 as the corresponding cross path. After mutation (the number of intermediate tree nodes is 1), the path 1-2-3-3-5 is obtained first, and then the duplicate nodes are deleted and the missing nodes are added to obtain the variant path 1-2-3-5-4.

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

[0054] The path node position difference of two picking paths is obtained based on the sum of the distance differences between the nodes with the same sorting. The farther the distance between the nodes with the same sorting, the greater the path node position difference. For example, picking path A is 1-4-3-5-2, and picking path B is abcdef. The path node position difference between picking path A and picking path 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.

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

[0056] To further prevent robot collisions, when several robots are picking Agaricus bisporus, when the distance between two robots is less than the safety distance threshold, the robot in the return stage avoids the robot in the picking or / and pre-picking stage, and the robot in the picking stage avoids the robot in the pre-picking stage. The robot in the pre-picking stage is the robot in the stage of moving from the starting point (Agaricus bisporus placement point) to the target Agaricus bisporus.

[0057] 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 other robotic arms No. 2 and No. 3 that are picking is increased by 1, and the maximum does not exceed 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 No. 3 is less than the safety distance threshold, robotic arm No. 1 avoids. When robotic arm No. 1 places Agaricus bisporus at the starting point and starts a new picking task, the priority of robotic arm No. 1 is set to 1.

[0058] 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, so as to achieve 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 a plurality of areas to be picked, so as to achieve simultaneous work of multiple manipulators ... The location information of the Agaricus bisporus to be picked is used to construct a graph structure, which is convenient for intuitively analyzing the distribution of Agaricus bisporus in each area to be picked, and further converting the graph structure of the area to be picked into a tree structure, so that the initial path is more hierarchical and clearer, and the initial path is crossed and mutated to enhance the path diversity, so that the path evolves in the direction of smaller path cost, and a picking path that can improve the picking efficiency is obtained; finally, several manipulators pick Agaricus bisporus according to the corresponding picking path, and implement different avoidance strategies according to the movement stage during the picking process of Agaricus bisporus, so as to realize multi-arm dynamic coordination planning and improve work efficiency.

Claims

1. A control method for picking Agaricus bisporus by multiple manipulators, 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 robot base coordinate system to the robot end coordinate system, obtain the conversion matrix from the camera coordinate system to the robot base coordinate system; according to the conversion matrix from the camera coordinate system to the robot base coordinate system, align the camera acquisition data and the movement data of the manipulator at the end of the robot arm, so that the manipulator at the end of the robot arm 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; According to the position information of the Agaricus bisporus to be picked, as well as the number and grasping range of the manipulators, 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, graph structures are 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 manipulators between the nodes; S3, based on a plurality of graph structures of the areas to be picked, obtaining a picking path for each area to be picked; Specifically, the 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; 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 whose path cost is less than the cost threshold among the several optimized paths; if so, the optimized path is used as the picking path; if not, several optimized paths perform several cross processing, mutation processing and path cost judgment operations until a path whose path cost is less than the cost threshold appears 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, and the sum of all fitness 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 the tree structure of S3, the operations of obtaining several optimization paths are specifically: Sort several initial paths by path cost from large to small, select the initial paths ranked in the first percentile, exchange some paths between adjacent sorted initial paths, and obtain several cross paths; The cross paths whose path costs are less than the cross path cost threshold are taken as learning paths, and all learning paths form a learning path set; the remaining cross paths select one learning path from the learning path set as the corresponding target learning path, and replace the intermediate tree nodes of the corresponding cross paths with the intermediate tree nodes of the target learning path to obtain several variant paths as several optimized paths.

5. 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: The camera is fixed on the outside of the robotic arm and is stationary relative to the robotic arm base. The robot at the end of the robotic arm holds the calibration plate to change the position and posture 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. The camera also records the position and posture changes of the calibration points on the calibration plate in the camera coordinate system observed by the camera when the robotic arm end is in different positions, as well as the position and posture changes of the robotic arm end with respect 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 the conversion matrices from the calibration plate coordinate system to the camera coordinate system and the conversion matrices from the robotic arm base coordinate system to the robotic arm end coordinate system, a linear equation group is constructed, and the conversion matrix from the camera coordinate system to the robotic arm base coordinate system is obtained by solving the equation group based on the singular value decomposition method.

6. The method for controlling the picking of Agaricus bisporus by multiple manipulators 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 value of the sum of 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 is taken as the optimal picking path for the current area to be picked, and the operation in S4 is executed.

7. The method for controlling the picking of Agaricus bisporus by multiple manipulators according to claim 6, 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.

8. The method for controlling the picking of Agaricus bisporus by multiple manipulators according to claim 1, characterized in that: 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 and / or pre-picking stage, and the manipulator in the picking stage avoids the manipulator in the pre-picking stage.

9. 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 mechanical arms on its base, and a mechanical hand is provided at the end of each mechanical arm. The picking robot performs the actions of moving, picking Agaricus bisporus, and placing Agaricus bisporus on the Agaricus bisporus collection track according to the picking path action execution signal of each to-be-picked area transmitted by the control unit; A calculation unit, used for obtaining a transformation matrix from the camera coordinate system to the robot base coordinate system based on a transformation matrix from the calibration plate coordinate system to the camera coordinate system and a transformation matrix from the robot base coordinate system to the robot 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 the plurality of areas to be picked, respectively, a graph structure is constructed to obtain the graph structures of the plurality of areas to be picked; and for obtaining a picking path for each area to be picked based on a plurality of graph structures of the areas to be picked; A control unit, 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.

10. The multi-manipulator picking Agaricus bisporus control system according to claim 9, 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