JP6973444B2 - Control system, information processing device and control method - Google Patents

Description

The present technology relates to control systems, information processing devices and control methods.

Conventionally, a robot having an end effector that picks an object from a plurality of objects is known. In such picking, the end effector may interfere with obstacles (such as other workpieces and containers) existing around the target workpiece. In order to avoid such interference, a technique is known in which the position of the end effector at the time of picking is calculated to determine the interference with surrounding objects.

For example, Japanese Patent Application Laid-Open No. 2018-144166 (Patent Document 1) discloses an image processing apparatus including an end effector model registration unit, an additional model creation unit, and an interference determination unit. The end effector model registration unit registers an end effector model that is three-dimensional CAD (Computer Aided Design) data. The additional model creation unit creates an additional model in which an additional area represented by one or more predetermined three-dimensional figures is added to the surface of the end effector model. The interference determination unit determines whether the end effector model registered in the end effector model registration unit and the additional model created in the additional model creation unit interfere with other objects that may interfere with the gripping motion of the work model. Is determined.

Japanese Unexamined Patent Publication No. 2018-144166

Generally, the end effector has a movable part for picking the target work. However, the 3D CAD data shows an end effector model when the moving part is in place. In the technique described in Patent Document 1, although an additional model can be added to the end effector model, the end effector model itself cannot be changed. Therefore, if the position of the movable portion when picking the target work and the position of the portion corresponding to the movable portion in the end effector model are different, the interference determination cannot be performed accurately.

By incorporating 3D CAD data editing software into the image processing device, it is possible to edit the end effector model according to the position of the movable portion when picking the target work. However, 3D CAD data is information that directly affects quality, and workers at the manufacturing site usually do not have the authority to edit the 3D CAD model. Therefore, it is not realistic to incorporate editing software into an image processing device.

The present invention has been made in view of the above problems, and an object of the present invention is to provide a control system, an information processing device, and a control method capable of accurately determining interference between an end effector and an object. be.

According to one example of the present disclosure, the control system controls a robot having an end effector that picks one object among a plurality of objects as an object. The end effector includes a movable portion and a support portion that movably supports the movable portion. The control system includes an acquisition unit, a user interface, a first setting unit, a measurement unit, a selection unit, and a determination unit. The acquisition unit acquires first shape data indicating the three-dimensional shape of the end effector. The first setting unit displays the end effector model indicated by the first shape data on the user interface, and based on the information input to the user interface, the movable part model and the movable part corresponding to the movable part of the end effector model. Set the movable direction of the model. The measuring unit measures the three-dimensional shape of the visual field region including a plurality of objects. The selection unit selects the position / orientation candidate of the end effector when picking the object based on the measurement result of the measurement unit. The determination unit determines interference between the end effector model of the position / orientation candidate selected by the selection unit and an object other than the object. In the end effector model, the determination unit makes an interference determination in a state where the movable unit model is moved in the movable direction by a set amount of movement.

According to this disclosure, the shape of the end effector model can be changed so as to match the shape of the end effector when actually picking the object by appropriately setting the movement amount according to the size of the object. Therefore, by using the modified end effector model, it is possible to accurately determine the interference between the end effector and an object other than the object.

In the above disclosure, the acquisition unit further acquires the second shape data indicating the three-dimensional shape of the object. The control system further includes a second setting unit that sets the amount of movement of the movable part model required for the end effector model to pick the object model indicated by the second shape data. The determination unit moves the movable unit model by the amount of movement set by the second setting unit.

According to this disclosure, by using the object model and the end effector model, the amount of movement of the movable part model required for picking can be appropriately set.

In the above disclosure, the plurality of objects include a plurality of types of objects having at least one of different shapes and sizes from each other. The acquisition unit further acquires the second shape data indicating the three-dimensional shape of the object for each of the plurality of types of objects. The control system further provides a second setting unit for each of the plurality of types of objects to set the amount of movement of the movable part model required for the end effector model to pick the object model indicated by the second shape data. Be prepared. The selection unit identifies the type of the object based on the measurement result of the measurement unit, and selects a position / posture candidate according to the identification result. The determination unit moves the movable unit model by the amount of movement set by the second setting unit with respect to the type identified by the selection unit.

According to this disclosure, even if a plurality of types of objects exist, the amount of movement is set according to the type of the object. By using the end effector model changed according to the movement amount, it is possible to more accurately determine the interference between the end effector and an object other than the object.

In the above disclosure, the control system further comprises a second setting unit that sets the amount of movement of the movable unit model required to pick the object based on the three-dimensional shape of the visual field region. The determination unit moves the movable unit model by the amount of movement set by the second setting unit.

According to this disclosure, even when there is no data indicating the three-dimensional shape of the object, the amount of movement of the movable part model required for picking the object is set. By using the end effector model changed according to the movement amount, it is possible to more accurately determine the interference between the end effector and an object other than the object.

In the above disclosure, the support supports the movable part so that at least one of translation and rotational movement is possible. According to this disclosure, it can be applied to various types of end effectors.

In the above disclosure, the movable portion has a plurality of claw portions, and the support portion may support the plurality of claw portions so as to be able to move in parallel so that the distance between the plurality of claw portions is variable.

According to this disclosure, since the number of parts constituting the end effector model is small, the operator can easily set the movable part model by using the user interface.

In the above disclosure, the movable portion has a plurality of claw portions. The second setting unit extracts the highest position from the three-dimensional shape of the visual field region. The second setting unit labels the area of the visual field area where the altitude difference from the extracted position is equal to or less than the specified value. The second setting unit is a movement of each of the plurality of claws so that the plurality of claws are located around one labeled area and the plurality of claws do not overlap with the other labeled areas. Set the amount.

According to this disclosure, the labeled region corresponds to a convex part in the measured three-dimensional shape. The object is located on the convex portion. Therefore, even if there is no data indicating the three-dimensional shape of the object, the movable part is located in the concave portion of the measured three-dimensional shape and when a plurality of claws are located around the object. The amount of movement of the model is set. By using the end effector model changed according to the movement amount, it is possible to more accurately determine the interference between the end effector and an object other than the object.

According to an example of the present disclosure, the information processing apparatus uses a user interface to process information on shape data indicating a three-dimensional shape of an end effector that picks an object. The end effector includes a movable portion and a support portion that movably supports the movable portion. The information processing device displays an acquisition unit that acquires shape data indicating the three-dimensional shape of the end effector and an end effector model indicated by the shape data on the user interface, and based on the information input to the user interface, the end effector. Among the models, a movable part model corresponding to the movable part and a setting part for setting the movable direction of the movable part model are provided.

According to one example of the present disclosure, the control method is a method of controlling a robot having an end effector that picks one object from a plurality of objects as an object. The end effector includes a movable portion and a support portion that movably supports the movable portion. The control method includes the first to fifth steps. The first step is a step of acquiring shape data indicating the three-dimensional shape of the end effector. The second step is to display the end effector model indicated by the shape data on the user interface, and based on the information input to the user interface, the movable part model and the movable part model corresponding to the movable part of the end effector model. It is a step to set the movable direction. The third step is to measure the three-dimensional shape of the visual field region including a plurality of objects. The fourth step is a step of selecting a position / orientation candidate of the end effector when picking an object based on the measurement result. The fifth step is a step of determining interference between the selected position / orientation candidate end effector model and an object other than the object. The step of determining the interference includes a step of moving the movable portion model in the movable direction by a set amount of movement in the end effector model.

Even with these disclosures, it is possible to accurately determine the interference between the end effector and the object.

According to the present disclosure, it is possible to accurately determine the interference between the end effector and the object.

It is a schematic diagram which shows the whole structure of the control system which concerns on embodiment. It is a figure which shows an example of the end effector model shown by 3D CAD data, and the end effector when picking a target work. It is a schematic diagram which shows an example of the hardware configuration of the image processing apparatus shown in FIG. It is a block diagram which shows an example of the functional structure of the image processing apparatus shown in FIG. It is a figure which shows the example of the internal representation of the 1st CAD data. It is a figure which shows an example of the UI screen displayed in the user interface which concerns on embodiment. It is a figure which shows an example of the UI screen when the movement amount is input in the movement amount input field. It is a figure which shows an example of the UI screen when a work display button is operated. It is a figure which shows an example of the coordinate transformation matrix which shows the relative position and orientation of a work model with respect to a support part model. It is a figure which shows the method of determining the position | posture which the support part of an end effector should take when gripping a target work. It is a figure which shows an example of the end effector model which coordinate-transformed by a model change part. It is a figure which shows the example which the end effector and the work other than the target work interfere with each other. It is a figure which shows the example which the end effector and a container interfere with each other. It is a flowchart which shows the flow of the preset process which is executed by the image processing apparatus which concerns on embodiment. It is a flowchart which shows the flow of the search process of the picking position executed by the image processing apparatus which concerns on embodiment. It is a flowchart which shows the flow of the search process of the picking position executed by the image processing apparatus which concerns on modification 1. FIG. It is a figure which shows the end effector when gripping a workpiece which varies in shape and size. It is a block diagram which shows the functional structure of the image processing apparatus which concerns on modification 3. It is a figure which shows an example of the UI screen displayed in the user interface which concerns on modification 3. FIG. It is a figure which shows the processing of the selection part of the image processing apparatus which concerns on modification 3. It is a flowchart which shows the flow of the preset process executed by the image processing apparatus which concerns on modification 3. FIG. It is a flowchart which shows the flow of the picking position search processing executed by the image processing apparatus which concerns on modification 3. FIG. It is a flowchart which shows an example of the processing flow of the subroutine of step S52 of FIG. It is a block diagram which shows an example of the functional structure of the image processing apparatus which concerns on modification 4.

Embodiments of the present invention will be described in detail with reference to the drawings. The same or corresponding parts in the drawings are designated by the same reference numerals and the description thereof will not be repeated.

§1 Application example First, an example of a situation in which the present invention is applied will be described with reference to FIG. FIG. 1 is a schematic diagram showing an overall configuration of a control system according to an embodiment. The control system SYS exemplified in FIG. 1 is incorporated in a production line or the like, and controls such that work 1 which is an object bulkly piled up in a container 5 is picked one by one. Although FIG. 1 illustrates a case where only two works 1 exist in the container 5 for easy understanding, a large number of works 1 may be arranged in an overlapping state.

The control system SYS exemplified in FIG. 1 includes an image processing device 100, a robot controller 200, and a robot 300. A projector (lighting device) 7, an image pickup device 8, and a user interface 9 are connected to the image processing device 100.

The robot 300 performs an operation of picking the work 1. The robot 300 includes an end effector 30 that grips the work 1, an articulated arm 33 that supports the end effector 30 at one end, and a base 34 that supports the other end of the articulated arm 33. The operation of the robot 300 is controlled by the robot controller 200.

The robot controller 200 receives position / orientation information indicating the position / orientation to be taken by the end effector 30 from the image processing device 100. The robot controller 200 generates a command for positioning the end effector 30 of the robot 300 in an appropriate position / posture based on the position / posture information, and outputs the command to the robot 300. In response to this command, the robot 300 starts a picking operation (grasping operation in the example shown in FIG. 1) of the work 1.

The projector 7 functions as a kind of surface light source, and can project illumination light of an arbitrary shading pattern to a predetermined position (a position where the container 5 including the work 1 is arranged) according to an instruction from the image processing device 100. .. This arbitrary shading pattern includes a pattern in which the brightness is uniform in the irradiation surface and a pattern in which the brightness changes periodically along a predetermined direction in the irradiation surface.

The projector 7 includes, as main components, a light source such as an LED (Light Emitting Diode) or a halogen lamp, and a filter arranged on the side of the irradiation surface of the projector 7. Further, the filter generates a shading pattern necessary for measuring a three-dimensional shape as described later, and can arbitrarily change the in-plane light transmittance according to a command from the image processing apparatus 100. Instead of the filter, a liquid crystal display or a digital mirror device may be used to generate a shading pattern. Any shade pattern can be generated by using a liquid crystal display or a digital mirror device.

The image pickup apparatus 8 is installed so that the container 5 is included in the visual field region, and the illumination light emitted by the projector 7 captures the light reflected on the surface of the work 1. The image pickup device 8 outputs the image obtained by the image pickup to the image processing device 100.

The image pickup device 8 includes an optical system such as a lens and an image pickup element such as a CCD (Coupled Charged Device) or a CMOS (Complementary Metal Oxide Semiconductor) sensor as main components.

The image processing device 100 selects one of the plurality of works 1 in the container 5 as the target work 2 based on the image received from the image pickup device 8, and the end effector 30 for gripping the target work 2. Position Position Determine the posture. The image processing device 100 generates position / posture information indicating the determined position / posture, and outputs the generated position / posture information to the robot controller 200. The robot controller 200 controls the robot 300 according to the position / orientation information received from the image processing device 100.

Specifically, the image processing device 100 measures the three-dimensional shape of the visual field region of the image pickup device 8 based on the image received from the image pickup device 8. Since the field of view region includes the container 5, the three-dimensional shapes of the plurality of works 1 bulkly stacked in the container 5 are measured.

The image processing apparatus 100 selects one of the plurality of works 1 as the target work 2 to be gripped based on the measured three-dimensional shape, and the end effector 30 when gripping the target work 2. Select a position / posture candidate.

The end effector 30 in the selected position / orientation candidate state may interfere with an object other than the target work 2 (including the other work 1 and the container 5). When such interference occurs, the end effector 30 or an object other than the target work 2 may be damaged. Therefore, in order to avoid such interference, the image processing apparatus 100 uses the three-dimensional CAD data indicating the three-dimensional shape of the end effector 30 to determine the interference between the end effector 30 and an object other than the target work 2. Specifically, the end effector model shown by the three-dimensional CAD data and existing in the selected position / orientation candidate is regarded as the end effector 30, and the interference determination between the end effector model and the object other than the target work 2 is performed. ..

FIG. 2 is a diagram showing an example of an end effector model shown by three-dimensional CAD data and an end effector when picking a target work. As illustrated on the right side of FIG. 2, the end effector 30 includes a right claw portion 31R and a left claw portion 31L, which are movable portions, and a support portion 32 that movably supports the right claw portion 31R and the left claw portion 31L. including. Specifically, the support portion 32 supports the right claw portion 31R and the left claw portion 31L so as to be able to translate so that the distance (interval) between the right claw portion 31R and the left claw portion 31L is variable. ..

In order to grip the target work 2, it is necessary to widen the distance between the right claw portion 31R and the left claw portion 31L by the opening width W according to the size of the target work 2. However, the three-dimensional shape data showing the three-dimensional shape of the end effector 30 is the three-dimensional shape of the entire end effector 30 when the right claw portion 31R and the left claw portion 31L are in a constant state (closed state in the example shown in FIG. 2). Created based on shape. Therefore, as illustrated on the left side of FIG. 2, the shape of the end effector model 35 does not match the shape of the end effector 30 when the target work 2 is actually picked.

Further, the three-dimensional CAD data usually shows only the three-dimensional shape of the entire end effector 30, and does not include attribute information indicating which portion the right claw portion 31R, the left claw portion 31L, and the support portion 32 correspond to. Therefore, it is not possible to change the shape of the end effector model 35 so as to match the shape of the end effector 30 when picking the target work 2 using only the three-dimensional CAD data.

From the above, when the end effector model 35 is used as it is, it is not possible to accurately determine the interference between the end effector 30 and an object other than the target work 2. Therefore, the image processing device 100 performs the interference determination as follows.

The image processing device 100 causes the user interface 9 to display the end effector model 35 represented by the three-dimensional CAD data. Then, the image processing device 100 sets the movable portion model corresponding to the movable portion of the end effector model 35 and the movable direction of the movable portion model based on the information input to the user interface 9. In the example shown in FIG. 2, the right claw portion model 36R and the left claw portion model 36L corresponding to the right claw portion 31R and the left claw portion 31L are set in the end effector model 35, respectively. Further, the movable directions of the right claw model 36R and the left claw model 36L are set to the + Y direction and the −Y direction, respectively.

The image processing device 100 makes an interference determination in the end effector model 35 in a state where the right claw model 36R and the left claw model 36L are moved in the movable direction by a set amount of movement.

According to the present embodiment, by appropriately setting the movement amount according to the size of the target work 2, the shape of the end effector model 35 so as to match the shape of the end effector 30 when actually picking the target work 2. Can be changed. In the example shown in FIG. 2, 1/2 of the opening width W may be set as the movement amount. Then, by using the changed end effector model 35, it is possible to accurately determine the interference between the end effector 30 and an object other than the target work 2.

§2 Specific example Next, a specific example of the control system according to the present embodiment will be described.

<A. Image processing device hardware configuration example>
The image processing device 100 is typically a computer having a general-purpose architecture, and executes image processing according to the present embodiment by executing a pre-installed program (instruction code). .. Such a program is typically distributed in a state of being stored in various recording media or the like, or is installed in the image processing apparatus 100 via a network or the like.

When using such a general-purpose computer, an OS (Operating System) for executing basic computer processing is installed in addition to the application for executing image processing according to the present embodiment. It may have been done. In this case, the program according to the present embodiment may call the necessary modules in a predetermined array at a predetermined timing among the program modules provided as a part of the OS to execute the process. good. That is, the program itself according to the present embodiment does not include the module as described above, and the process may be executed in cooperation with the OS. The program according to the present embodiment may be in a form that does not include such a part of the modules.

Further, the program according to the present embodiment may be provided by being incorporated into a part of another program. Even in that case, the program itself does not include the modules included in the other programs to be combined as described above, and the processing is executed in cooperation with the other programs. That is, the program according to the present embodiment may be a form incorporated in such another program. Note that some or all of the functions provided by executing the program may be implemented as a dedicated hardware circuit.

FIG. 3 is a schematic view showing an example of the hardware configuration of the image processing apparatus shown in FIG. As shown in FIG. 3, the image processing device 100 includes a CPU (Central Processing Unit) 110 which is an arithmetic processing unit, a main memory 112 and a hard disk 114 as a storage unit, a camera interface 116, an input interface 118, and the like. It includes a display controller 120, a projector interface 122, a communication interface 124, and a data reader / writer 126. Each of these parts is connected to each other via a bus 128 so as to be capable of data communication.

The CPU 110 expands the programs (codes) installed in the hard disk 114 into the main memory 112 and executes them in a predetermined order to perform various calculations. The main memory 112 is typically a volatile storage device such as a DRAM (Dynamic Random Access Memory), and in addition to the program read from the hard disk 114, the image acquired by the image pickup device 8 and the calibration. Holds data, information about measurement results, etc. Further, various setting values and the like may be stored in the hard disk 114. In addition to the hard disk 114, or instead of the hard disk 114, a semiconductor storage device such as a flash memory may be adopted.

The camera interface 116 mediates data transmission between the CPU 110 and the image pickup apparatus 8. That is, the camera interface 116 is connected to the image pickup device 8. The camera interface 116 includes an image buffer 116a for temporarily storing an image from the image pickup apparatus 8. Then, when the image of a predetermined number of frames is accumulated in the image buffer 116a, the camera interface 116 transfers the accumulated image to the main memory 112. Further, the camera interface 116 gives an image pickup command to the image pickup apparatus 8 according to an internal command generated by the CPU 110.

The input interface 118 mediates data transmission between the CPU 110 and an input unit such as a touch panel 90, a mouse, and a keyboard constituting the user interface 9. That is, the input interface 118 receives an operation command given by the user operating the input unit.

The display controller 120 is connected to the display 92 constituting the user interface 9 and notifies the user of the processing result of the CPU 110 and the like. That is, the display controller 120 is connected to the display 92 and controls the display on the display 92.

The projector interface 122 mediates data transmission between the CPU 110 and the projector 7. More specifically, the projector interface 122 gives a lighting command to the projector 7 according to an internal command generated by the CPU 110.

The communication interface 124 mediates data transmission between the CPU 110 and an external device such as the robot controller 200. The communication interface 124 typically comprises Ethernet (registered trademark), USB (Universal Serial Bus), or the like.

The data reader / writer 126 mediates data transmission between the CPU 110 and the memory card 106, which is a recording medium. That is, the memory card 106 is distributed in a state in which a program or the like executed by the image processing device 100 is stored, and the data reader / writer 126 reads the program from the memory card 106. Further, the data reader / writer 126 writes the input image acquired by the image pickup apparatus 8 and / or the processing result in the image processing apparatus 100 to the memory card 106 in response to the internal command of the CPU 110. The memory card 106 may be a general-purpose semiconductor storage device such as SD (Secure Digital), a magnetic storage medium such as a flexible disk, or an optical storage medium such as a CD-ROM (Compact Disk Read Only Memory). Etc.

<B. Function configuration example of image processing device>
FIG. 4 is a block diagram showing an example of the functional configuration of the image processing apparatus shown in FIG. As shown in FIG. 4, the image processing device 100 includes a first storage unit 10, a second storage unit 11, a CAD reading unit 12, a parts setting unit 13, a movement amount setting unit 14, and a relative position setting. It includes a unit 15, a 3D measurement unit 20, a container detection unit 21, a selection unit 22, a model change unit 26, an interference determination unit 27, and a grip position output unit 28. The first storage unit 10 and the second storage unit 11 are realized by the hard disk 114 shown in FIG. CAD reading unit 12, parts setting unit 13, movement amount setting unit 14, relative position setting unit 15, 3D measurement unit 20, container detection unit 21, selection unit 22, model change unit 26, interference determination unit 27, and grip position output unit. 28 is realized by the CPU 110 executing a program.

The CAD reading unit 12, the parts setting unit 13, the movement amount setting unit 14, and the relative position setting unit 15 are blocks that execute preset processing before picking the work 1. The 3D measurement unit 20, the container detection unit 21, the selection unit 22, the model change unit 26, the interference determination unit 27, and the grip position output unit 28 are blocks that execute a search process for searching the picking position of the work 1.

The first storage unit 10 stores the first CAD data 10a showing the three-dimensional shape of the end effector 30. The first CAD data 10a shows only the three-dimensional shape of the entire end effector 30, and does not include attribute information indicating which portion the right claw portion 31R, the left claw portion 31L, and the support portion 32 correspond to. The second storage unit 11 stores the second CAD data 11a showing the three-dimensional shape of the work 1.

The CAD reading unit 12 acquires the first CAD data 10a by reading the first CAD data 10a from the first storage unit 10. Further, the CAD reading unit 12 acquires the second CAD data 11a by reading the second CAD data 11a from the second storage unit 11.

The CAD reading unit 12 converts the read CAD data into an internal representation (expression for processing). FIG. 5 is a diagram showing an example of the internal representation of the first CAD data. The first CAD data 10a is converted as a set of a plurality of elements. That is, the end effector model 35 represented by the first CAD data 10a is composed of a set of a plurality of elements. As shown in FIG. 5, the element is, for example, a box, a cross section, a triangular mesh, or a point.

Returning to FIG. 4, the parts setting unit 13 causes the user interface 9 to display the end effector model 35 represented by the first CAD data 10a. The parts setting unit 13 sets the right claw portion model 36R and the left claw portion model 36L (see FIG. 2) based on the information input to the user interface 9. Further, the parts setting unit 13 sets the movable directions of the right claw portion model 36R and the left claw portion model 36L based on the information input to the user interface 9. Further, the parts setting unit 13 sets the support unit model 37 (see FIG. 2), which is a portion corresponding to the support unit 32 in the end effector model 35, based on the information input to the user interface 9.

The parts setting unit 13 has a right claw model 36R, a left claw model 36L, or a support model 37 for each element (box, cross section, triangular mesh, or point shown in FIG. 5) constituting the end effector model 35. Attribute information indicating whether it belongs to is given.

The movement amount setting unit 14 has a right claw model 36R and a left claw model necessary for the end effector model 35 to grip the work model shown by the second CAD data 11a based on the information input to the user interface 9. Each movement amount of 36L is set.

The relative position setting unit 15 sets the relative position / orientation of the work model with respect to the support model 37 when the end effector model 35 grips the work model based on the information input to the user interface 9.

The 3D measurement unit 20 measures the three-dimensional shape of the visual field region of the image pickup device 8 based on the image acquired from the image pickup device 8. The measured three-dimensional shape is represented by, for example, three-dimensional point cloud data. The three-dimensional point cloud data shows the three-dimensional coordinates of each of the plurality of points constituting the surface of the object existing in the visual field region on the image pickup device 8 side.

The container detection unit 21 stores information indicating the three-dimensional shape of the container 5 in advance, and determines the position and orientation of the container 5 based on the information and the three-dimensional point cloud data which is the measurement result of the 3D measurement unit 20. To detect. Since a plurality of works 1 are stacked in the container 5, the upper end surface of the side wall of the container 5 is observed when viewed from the image pickup apparatus 8. Therefore, the container detection unit 21 extracts the point data corresponding to the upper end surface of the side wall of the container 5 from the three-dimensional point cloud data, and from the extracted data and the information indicating the three-dimensional shape of the container 5, the container The position and orientation of 5 may be calculated.

The selection unit 22 selects one or a plurality of position / orientation candidates of the end effector 30 when picking the target work 2 based on the measurement result of the 3D measurement unit 20. As shown in FIG. 4, the selection unit 22 includes a work detection unit 23, a target work selection unit 24, and a position / orientation determination unit 25.

The work detection unit 23 searches for the work 1 from the visual field area of the image pickup apparatus 8 by using template matching, and detects the position and orientation of the searched work 1.

The target work selection unit 24 selects the target work 2 to be picked based on the position and orientation of the work 1 detected by the work detection unit 23. For example, the target work selection unit 24 selects the work 1 having a predetermined number of heights higher than the work 1 searched by the work detection unit 23 as the target work 2.

The position / posture determining unit 25 determines the position / posture to be taken by the support portion 32 of the end effector 30 when gripping the target work 2. As described above, the relative position / orientation of the work model with respect to the support model 37 when the end effector model 35 grips the work model is set by the relative position setting unit 15. Therefore, the position / posture determining unit 25 determines the position / posture to be taken by the support unit 32 so that the relative position / posture of the target work 2 with respect to the support unit 32 matches the relative position / posture set by the relative position setting unit 15. Just do it.

The model changing unit 26 changes the end effector model 35 so as to match the state of the end effector 30 when gripping the target work 2. The model change unit 26 changes the end effector model 35 by converting the coordinates of each of the plurality of elements constituting the end effector model 35.

Specifically, the model changing unit 26 converts the coordinates of each element constituting the end effector model 35 so that the position / orientation of the support unit model 37 matches the position / orientation determined by the position / orientation determining unit 25. .. Further, the model changing unit 26 moves the right claw unit model 36R and the left claw unit model 36L in the movable direction by the amount of movement set by the movement amount setting unit 14, respectively, so that the right claw unit model 36R and the left claw unit 26 move. The coordinates of each element constituting the model 36L are converted.

The interference determination unit 27 determines interference between the end effector model 35 changed by the model change unit 26 and an object other than the target work 2 (work 1 other than the target work 2 and the container 5).

The gripping position output unit 28 determines the position / posture of the end effector 30 based on the determination result of the interference determination unit 27, and generates position / posture information indicating the determined position / posture. The gripping position output unit 28 outputs the generated position / posture information to the robot controller 200.

<C. Setting method by parts setting part, movement amount setting part and relative position setting part>
With reference to FIGS. 6 to 9, various information set by the parts setting unit 13, the movement amount setting unit 14, and the relative position setting unit 15 will be described in detail.

FIG. 6 is a diagram showing an example of a UI screen displayed on the user interface according to the embodiment. The UI screen 40 shown in FIG. 6 is displayed on the display 92 of the user interface 9 according to the processing result of the CPU 110 (see FIG. 3).

The UI screen 40 has a display area 41, a support portion selection button 45, a right claw portion selection button 46, a left claw portion selection button 47, a movable direction input field 48 for the right claw, and a movable direction for the left claw. It includes an input field 49, a movement amount input field 50, a work display button 51, an OK button 52, and a cancel button 53.

The end effector model 35 represented by the first CAD data 10a is displayed in the display area 41. In the display area 41, the coordinate axes corresponding to the coordinate system of the first CAD data 10a (hereinafter, referred to as “CAD coordinate system”) are also displayed.

The parts setting unit 13 receives an operation input for rotating the end effector model 35 displayed in the display area 41 in any of the yaw, roll, and pitch directions via the user interface 9. In addition, the parts setting unit 13 also accepts translation and enlargement / reduction operation inputs of the end effector model 35 via the user interface 9. The parts setting unit 13 changes the position / orientation and size of the end effector model 35 displayed in the display area 41 according to the operation input to the user interface 9. As a result, the operator can change the posture of the end effector model 35 so that the portions corresponding to the support portion 32, the right claw portion 31R, and the left claw portion 31L of the end effector model 35 do not overlap each other.

In the display area 41, a frame line 42 for setting the support portion model 37, which is a portion of the end effector model 35 corresponding to the support portion 32, is displayed. The operator changes the position / posture and size of the frame line 42 by operating the touch panel 90 of the user interface 9. The parts setting unit 13 indicates attribute information indicating the support unit model 37 with respect to the element located in the frame line 42 when the support unit selection button 45 is operated among the plurality of elements constituting the end effector model 35. Is given. As a result, the operator appropriately changes the position, posture, and size of the frame line 42 so that the portion corresponding to the support portion 32 is included in the frame line 42 while viewing the image of the display area 41. The model 37 can be set.

In the display area 41, a frame line 43 for setting the right claw portion model 36R, which is a portion of the end effector model 35 corresponding to the right claw portion 31R, is displayed. The operator changes the position / posture and size of the frame line 43 by operating the touch panel 90. The parts setting unit 13 indicates the right claw portion model 36R with respect to the element located within the frame line 43 when the right claw portion selection button 46 is operated among the plurality of elements constituting the end effector model 35. Add attribute information. As a result, the operator can appropriately change the position and orientation and size of the frame line 43 so that the portion corresponding to the right claw portion 31R is included in the frame line 43 while viewing the image of the display area 41. The claw model 36R can be set.

In the display area 41, a frame line 44 for setting the left claw portion model 36L, which is a portion of the end effector model 35 corresponding to the left claw portion 31L, is displayed. The operator changes the position / posture and size of the frame line 44 by operating the touch panel 90. The parts setting unit 13 indicates the left claw portion model 36L with respect to the element located within the frame line 44 when the left claw portion selection button 47 is operated among the plurality of elements constituting the end effector model 35. Add attribute information. As a result, the operator appropriately changes the position, posture, and size of the frame line 44 so that the portion corresponding to the left claw portion 31L is included in the frame line 44 while looking at the image of the display area 41. The claw model 36L can be set.

The operator can input the movable direction of the right claw portion model 36R set by operating the right claw portion selection button 46 in the movable direction input field 48. Similarly, the operator inputs the movable direction of the left claw portion model 36L set by operating the left claw portion selection button 47 in the movable direction input field 49. Vectors indicating the movable direction are input to the movable direction input fields 48 and 49. In the example shown in FIG. 6, a vector (0, 1, 0) indicating the + Y direction in the CAD coordinate system is input as the movable direction of the right claw portion model 36R. Further, as the movable direction of the left claw portion model 36L, a vector (0, -1, 0) indicating the −Y direction is input. The parts setting unit 13 sets the movable direction of the right claw model 36R according to the vector input in the movable direction input field 48, and the movable direction of the left claw model 36L according to the vector input in the movable direction input field 49. To set.

The operator can input the movement amount of the right claw portion model 36R and the left claw portion model 36L in the movement amount input field 50. The amount of movement of the right claw model 36R indicates the amount of movement of the right claw model 36R in the movable direction from the reference position of the right claw model 36R. The reference position of the right claw portion model 36R is a position specified by the coordinates of the element to which the attribute information indicating the right claw portion model 36R is given in the first CAD data 10a. Similarly, the amount of movement of the left claw model 36L indicates the amount of movement of the left claw model 36L in the movable direction from the reference position of the left claw model 36L. The reference position of the left claw model 36L is a position specified by the coordinates of the element to which the attribute information indicating the left claw model 36L is given in the first CAD data 10a.

When the movement amount was input to the movement amount input field 50, the movement amount setting unit 14 input the right claw part model 36R and the left claw part model 36L in the end effector model 35 displayed in the display area 41. Move each by the amount of movement in the movable direction. Thereby, the operator can grasp the state of the end effector model 35 after the movement of the right claw portion model 36R and the left claw portion model 36L by confirming the display area 41.

FIG. 7 is a diagram showing an example of a UI screen when a movement amount is input in the movement amount input field. As shown in FIG. 7, in the end effector model 35 displayed in the display area 41, the right claw model 36R and the left claw model 36L are input to the movement amount input field 50 from the state shown in FIG. It is moving by the amount of movement.

When the work display button 51 is operated, the relative position setting unit 15 causes the display area 41 to display the work model indicated by the second CAD data 11a read by the CAD reading unit 12. At this time, the relative position setting unit 15 appropriately changes the display position of the work model so that the work model and the end effector model are displayed in the display area 41.

FIG. 8 is a diagram showing an example of a UI screen when the work display button is operated. As shown in FIG. 8, the work model 3 is displayed in the display area 41.

The relative position setting unit 15 receives an operation input for translating or rotating the work model 3 displayed in the display area 41 via the user interface 9. The relative position setting unit 15 performs coordinate conversion of the work model 3 in response to an operation input to the user interface 9, and changes the position and orientation of the work model 3 displayed in the display area 41. Further, as described above, the parts setting unit 13 changes the posture of the end effector model 35 displayed in the display area 41 in response to the operation of rotating and moving the end effector model 35.

The operator operates the user interface 9 so that the work model 3 can be gripped by the end effector model 35, the position and posture of the work model 3, the posture of the end effector model 35, and the right claw model 36R and the left. The amount of movement of the claw model 36L is appropriately changed.

The relative position setting unit 15 has a work model 3 of the position and orientation displayed in the display area 41 and a posture displayed in the display area 41, and the right claw unit moved by the amount of movement currently input. Interference determination with the end effector model 35 having the model 36R and the left claw model 36L may be performed. Then, the relative position setting unit 15 may display the interference determination result in the display area 41.

In the example shown in FIG. 8, in the work model 3, the portion that interferes with the end effector model 35 is indicated by a thick line 54. The thick line 54 may be displayed in red for easy viewing. As a result, the operator can check the display of the thick line 54 so that the work model 3 is located with a slight gap between the right claw model 36R and the left claw model 36L, while checking the display of the work model 3. The position posture, the posture of the end effector model 35, and the amount of movement of the right claw model 36R and the left claw model 36L can be appropriately adjusted.

The operator can grip the work model 3 by the end effector model 35 in terms of the position and posture of the work model 3, the posture of the end effector model 35, and the amount of movement of the right claw model 36R and the left claw model 36L. When the adjustment is completed, the OK button 52 is operated.

The movement amount setting unit 14 is a right claw model 36R required when the end effector model 35 grips the movement amount input to the movement amount input field 50 when the OK button 52 is operated. And it is set as the movement amount of the left claw model 36L.

The movement amount setting unit 14 calculates _{the coordinate transformation matrix H MR} for translating the right claw portion model 36R by the set movement amount in the movable direction. Further, the movement amount setting unit 14 calculates a _{coordinate transformation matrix H ML} for translating the left claw portion model 36L by the set movement amount in the movable direction.

The relative position setting unit 15 is the relative position of the work model 3 with respect to the support model 37 based on the position and orientation of the work model 3 and the support model 37 displayed in the display area 41 when the OK button 52 is operated. Set the posture. The relative position / orientation corresponds to the state when the end effector model 35 grips the work model 3.

The relative position and orientation of the work model 3 with respect to the support model 37 is such that the coordinate system based on the support model 37 (hereinafter referred to as “support model coordinate system”) is used as the coordinate system based on the work model 3 (hereinafter referred to as “support model coordinate system”). Hereinafter, it is referred to as a “work model coordinate system”) and is shown by ^{a coordinate conversion matrix tM} H _wM. The support model coordinate system is set to match the tool coordinate system (coordinate system set on the flange surface) of the robot 300. Therefore, the relative position setting unit 15 calculates the ^{coordinate transformation matrix tM} H _{w M} as information indicating the relative position / orientation of the work model 3 with respect to the support unit model 37.

FIG. 9 is a diagram showing an example of the ^{coordinate transformation matrix tM} H _{w M} showing the relative position and orientation of the work model with respect to the support portion model. FIG. 9 shows a rectangular parallelepiped support model 37 and a work model 3 which is a T-shaped tube.

In the example shown in FIG. 9, the support part model coordinate system is a coordinate system in which the reference point 37o of the support part model 37 is the origin and the three specific directions 37v1 to 37v3 in the support part model 37 are the base vectors. The relative position setting unit 15 may select any one point (for example, the center of the upper surface of the support model 37) included in the plurality of elements constituting the support model 37 as the reference position of the support model 37. Further, the relative position setting unit 15 may select any three directions in the support unit model 37 as specific directions 37v1 to 37v3. The specific direction 37v1 is, for example, a direction from the upper surface to the lower surface of the support model 37. The specific direction 37v2 is, for example, a direction orthogonal to a pair of side surfaces facing each other in the support model 37. The specific direction 37v3 is, for example, a direction orthogonal to the remaining pair of side surfaces in the support model 37.

The work model coordinate system is a coordinate system in which the reference point 3o of the work model 3 is the origin and the three specific directions 3v1 to 3v3 in the work model 3 are the basis vectors. The reference points 3o and the specific directions 3v1 to 3v3 of the work model 3 are predetermined. The reference point 3o of the work model 3 is predetermined, for example, at a branch point in the T-shaped officer. The specific direction 3v1 is defined as, for example, one of the flow path directions in the T-shaped tube. The specific direction 3v2 is defined, for example, in another flow path direction in the T-shaped tube. The specific direction 3v3 is defined, for example, in a direction orthogonal to the specific directions 3v1 and 3v2.

The relative position setting unit 15 calculates a ^{coordinate transformation matrix tM} H _{w M} that transforms the support model coordinate system into the work model coordinate system. The coordinate transformation matrix ^tM H _wM represents the components of the basis vectors 3v1 to 3v3 of the work model coordinate system and the positions of the origin 3o in the support model coordinate system.

<D. 3D measurement>
Next, the details of the three-dimensional shape measurement process by the 3D measurement unit 20 will be described. The 3D measurement unit 20 uses a known three-dimensional measurement process to generate three-dimensional point cloud data indicating the three-dimensional coordinates of each point on the surface of an object existing in the visual field region of the image pickup apparatus 8.

As a specific example of the three-dimensional measurement process, for example, a phase shift method is adopted. This phase shift method is a method using an image (sinusoidal wave projection image) taken in a state of being irradiated with illumination light having a pattern in which the shading is changed in a sinusoidal shape in the irradiation surface. A plurality of patterns having different shade change cycles and phases in the irradiation surface are prepared, and the projector 7 sequentially irradiates the visual field region of the image pickup apparatus 8 with the plurality of patterns. The 3D measurement unit 20 acquires images captured when a plurality of patterns are irradiated as a sine wave projection image group. Then, the 3D measuring unit 20 calculates the three-dimensional coordinates based on the change in the brightness (brightness and brightness) of the corresponding portion between these sinusoidal projected images.

<E. Work detection method>
Next, a method of detecting the work 1 by the work detection unit 23 will be described. The work detection unit 23 stores in advance templates for each posture when the work model shown in the second CAD data 11a is viewed from various viewpoints. The work detection unit 23 collates each template with the three-dimensional point cloud data which is the measurement result of the 3D measurement unit 20, and extracts data similar to the template from the three-dimensional point cloud data. The work detection unit 23 detects the position and orientation of the work 1 existing in the visual field region of the image pickup apparatus 8 based on the extracted data.

The three-dimensional point cloud data, which is the measurement result of the 3D measurement unit 20, is represented by the coordinate system of the image obtained by the image pickup of the image pickup apparatus 8 (hereinafter, referred to as “camera coordinate system”). Therefore, the position and orientation of the work 1 (including the target work 2) detected by the work detection unit 23 is indicated by the camera coordinate system.

<F. Processing method of position / orientation determination part and model change part>
Next, with reference to FIGS. 10 and 11, details of the processing method of the position / orientation determination unit and the model change unit will be described.

FIG. 10 is a diagram showing a method of determining the position and posture that the support portion of the end effector should take when gripping the target work.

In FIG. 10, the coordinate transformation matrix ^C H _w is a matrix for transforming the camera coordinate system into a coordinate system based on the target work 2 (hereinafter, referred to as “target work coordinate system”). The target work coordinate system is a coordinate system in which the reference point 2o of the target work 2 is the origin and the three specific directions 2v1 to 2v3 in the target work 2 are the base vectors. The reference point 2o and the specific direction 2v1 to 2v3 of the target work 2 are defined to correspond to the reference point 3o and the specific direction 3v1 to 3v3 (see FIG. 9) of the work model 3 described above, respectively. The coordinate transformation matrix ^C H _w represents the components of the basis vectors 2v1 to 2v3 of the target work coordinate system and the position of the origin 2o in the camera coordinate system. The components of the basis vectors 2v1 to 2v3 and the position of the origin 2o in the target work coordinate system indicate the position and orientation of the target work 2. Therefore, the position and orientation of the target work 2 in the camera coordinate system is indicated by the ^{coordinate transformation matrix C} H _w.

As described above, the relative position and orientation of the work model 3 with respect to the support model 37 when the end effector model 35 grips the work model 3 is indicated by the ^{coordinate transformation matrix tM} H _{w M.} Therefore, the position / orientation determination unit 25 regards the target work 2 as the work model 3, and according to the following equation (1), coordinates indicating the position / orientation of the support unit model 37 when gripping the target work 2 in the camera coordinate system. Calculate the transformation matrix ^C H _{t M.}
^C H _tM = ^C H _w・ ( ^tM H _wM ) ^-1・ ・ ・ Equation (1)
The coordinate transformation matrix ^C H _tM indicates the position and orientation that the support portion 32 of the end effector 30 should take when gripping the target work 2 in the camera coordinate system.

The model changing unit 26 changes the end effector model 35 so as to match the shape of the end effector 30 when gripping the target work 2. Specifically, the model change unit 26 converts the coordinates of each element constituting the end effector model 35.

First, the model changing unit 26 converts the coordinates of each element constituting the support unit model 37 so that the position / orientation of the support unit model 37 matches the position / orientation determined by the position / attitude determination unit 25. That is, the model change unit 26 converts the coordinates to which the attribute information indicating the support unit model 37 is added in the first CAD data 10a.

The first CAD data 10a shows coordinate values in the CAD coordinate system. Further, the position / orientation determined by the position / orientation determination unit 25 is the position / orientation to be taken by the support unit 32 in the camera coordinate system, and is shown by the coordinate transformation matrix ^C H _tM described above. Therefore, the model change unit 26 converts the coordinates to which the attribute information indicating the support unit model 37 is given in the first CAD data 10a by using the coordinate transformation matrix ^C H _tM = ^C H _w · ( ^tM H _wM ) ^-1. do. As a result, the position and orientation of the support portion model 37 coincides with the position and orientation that the support portion 32 of the end effector 30 should take when gripping the target work 2 in the camera coordinate system.

^{Here, the same coordinate transformation matrix C} H _tM = ^C H _w · ( ^tM H _wM ) ^-1 for the coordinates to which the attribute information indicating the right claw model 36R and the left claw model 36L is given in the first CAD data 10a. It is assumed that the conversion is performed using. In this case, the right claw model 36R and the left claw model 36L are in a constant state (for example, a closed state). Therefore, as shown in FIG. 10, the positions and postures of the right claw model 36R and the left claw model 36L coincide with the positions and postures that the right claw portion 31R and the left claw portion 31L should take when gripping the target work 2. do not.

Therefore, the model changing unit 26 moves the right claw portion model 36R and the left claw portion model 36L in the movable direction by the amount of movement set by the movement amount setting unit 14. Specifically, the model change unit 26 includes the coordinate transformation matrix ^C H _tM = ^C H _w · ( ^tM H _wM ) ^-1 _{, and the coordinate transformation matrix H MR,} H _M L calculated by the movement amount setting unit 14. Is used. That is, the model changing unit 26 converts the coordinates to which the attribute information indicating the right claw part model 36R is added in the first CAD data 10a by using the coordinate transformation matrix ^C H _w · ( ^tM H _wM ) ^-1 · H _MR. do. The model changing unit 26 converts the coordinates to which the attribute information indicating the left claw portion model 36L is added in the first CAD data 10a by using the coordinate transformation matrix ^C H _w · ( ^tM H _wM ) ^-1 · H _ML .

FIG. 11 is a diagram showing an example of an end effector model whose coordinates have been transformed by the model change unit. As shown in FIG. 11, by using the coordinate transformation matrix H _MR, the position and orientation of the right claw portions model 36R includes a position and orientation to be taken by the right claw portion 31R of the end effector 30 when grasping the target work 2 Match. Similarly, by using the coordinate transformation matrix H _ML , the position and orientation of the left claw portion model 36L coincide with the position and orientation that the left claw portion 31L of the end effector 30 should take when gripping the target work 2. As a result, by using the end effector model 35 changed by the model changing unit 26, it is possible to accurately determine the interference between the end effector 30 and an object other than the target work 2.

<G. Interference judgment processing method>
Next, the details of the interference determination processing method by the interference determination unit 27 will be described. As described above, the position / posture determination unit 25 determines the position / posture candidate to be taken by the support unit 32 when gripping the target work 2. Therefore, when the support portion 32 is arranged in the position / posture candidate, the end effector 30 may interfere with the work 1 or the container other than the target work 2.

FIG. 12 is a diagram showing an example in which an end effector and a work other than the target work interfere with each other. FIG. 13 is a diagram showing an example in which the end effector and the container interfere with each other.

In order to avoid such interference, the interference determination unit 27 determines interference with an object other than the end effector model 35 and the target work 2 changed by the model change unit 26. The three-dimensional point cloud data, which is the measurement result of the 3D measurement unit 20, is shown in the camera coordinate system. The end effector model 35 modified by the model modification unit 26 according to the above method is also shown in the camera coordinate system. Therefore, the interference determination unit 27 may determine interference between the end effector model 35 and an object other than the target work 2 in the camera coordinate system. The interference determination unit 27 may perform interference determination in a coordinate system based on the base 34 of the robot 300 (hereinafter, referred to as “base coordinate system”). In this case, in order to convert the appropriate coordinates using the coordinate transformation matrix ^B H _C to convert the base coordinate system to the camera coordinate system, it may be performed interference determination.

The interference determination unit 27 deletes the point cloud data corresponding to the target work 2 from the three-dimensional point cloud data. The interference determination unit 27 compares the coordinates of the elements constituting the end effector model 35 changed by the model change unit 26 with the coordinates of the remaining three-dimensional point cloud data, and thereby other than the end effector model 35 and the target work 2. Interference with the work 1 is determined.

Further, the interference determination unit 27 and the end effector model 35 by comparing the coordinates of the elements constituting the modified end effector model 35 with the coordinates indicating the position and orientation of the container 5 detected by the container detection unit 21. The interference with the container 5 is determined.

For example, when the element constituting the end effector model 35 is a box, the interference determination unit 27 determines that there is interference when the measurement point is included inside any of the boxes constituting the end effector model 35.

When the element constituting the end effector model 35 is a cross section, the interference determination unit 27 is used when a measurement point is included on any of the cross sections constituting the end effector model 35, or between two adjacent cross sections. When the measurement point is included, it is judged that there is interference.

When the element constituting the end effector model 35 is a triangular mesh, the interference determination unit 27 calculates the distance between each measurement point belonging to the three-dimensional point cloud data and each triangular mesh. The distance has a positive value when the measurement point is located on the normal side of the triangular mesh, and a negative value when the measurement point is located on the opposite side of the normal direction of the triangular mesh. The interference determination unit 27 determines that there is interference when the distance between the measurement point and the triangular mesh is negative.

When the element constituting the end effector model 35 is a point, the interference determination unit 27 determines that there is interference when there is a point cloud surrounding the measurement point among the plurality of points constituting the end effector model 35.

<H. Method of generating position / posture information by gripping position output unit>
Next, the details of the method of generating the position / posture information by the gripping position output unit 28 will be described. The gripping position output unit 28 generates position / posture information indicating one of the position / posture candidates determined to have no interference by the interference determination unit 27.

When a plurality of target works 2 are selected by the target work selection unit 24, the position / posture determination unit 25 selects a position / posture candidate for each of the plurality of target works 2. Therefore, the selection unit 22 selects a plurality of position / posture candidates. The interference determination unit 27 makes an interference determination for each of the plurality of position / posture candidates. In this case, it may be determined that there is no interference for a plurality of position / orientation candidates. When there are a plurality of position / posture candidates determined to have no interference, the gripping position output unit 28 extracts one of the plurality of position / posture candidates according to a predetermined extraction method. For example, the gripping position output unit 28 extracts the closest position / posture candidate from the current position of the end effector 30. The gripping position output unit 28 generates position / orientation information indicating one extracted position / orientation candidate.

As mentioned above, the position and orientation candidates are shown in the camera coordinate system. On the other hand, the robot controller 200 controls the robot 300 in a different coordinate system. For example, the robot controller 200 controls the robot 300 in a coordinate system based on the base 34 of the robot 300 (hereinafter, referred to as “base coordinate system”). In such a case, the gripping position output unit 28 preferably generates the position / orientation information indicated by the base coordinate system. Therefore, the grip position output unit 28, using the coordinate transformation matrix ^B H _C to convert the base coordinate system to the camera coordinate system, and generates the position and orientation information represented by the base coordinate system. Specifically, the grip position output section 28 generates a position and orientation information by coordinate converting the position and orientation candidates with ^(B H _C) ^-1.

<I. Flow of preset processing ＞
The flow of the preset processing by the image processing apparatus 100 will be described with reference to FIG. FIG. 14 is a flowchart showing the flow of the preset processing executed by the image processing apparatus according to the embodiment.

First, the CPU 110 of the image processing apparatus 100 reads the first CAD data 10a showing the three-dimensional shape of the end effector 30 (step S1). The CPU 110 converts the read first CAD data 10a into an internal representation (a set of elements such as a box, a cross section, a triangular mesh, and a point) (step S2).

The CPU 110 displays a UI screen 40 (see FIG. 6) including an image of the end effector model 35 shown in the first CAD data 10a on the user interface 9. Then, the CPU 110 sets the movable portion model (right claw portion model 36R, left claw portion model 36L) and the support portion model 37 based on the information input to the user interface 9 (step S3). The CPU 110 has an attribute indicating any one of the right claw model 36R, the left claw model 36L, and the support model 37 with respect to the elements set in the right claw model 36R, the left claw model 36L, and the support model 37. Each of the information is saved (step S4).

The CPU 110 sets the movable directions of the right claw model 36R and the left claw model 36L based on the information input to the user interface 9 (step S5).

When the work display button 51 is operated on the UI screen 40, the CPU 110 reads the second CAD data 11a showing the three-dimensional shape of the work 1 and displays the image of the work model 3 shown by the second CAD data 11a on the UI screen 40. (Step S6).

When the movement amount is input to the movement amount input field 50, the CPU 110 moves the right claw part model 36R and the left claw part model 36L by the input movement amount in the end effector model 35 displayed on the UI screen 40. They are moved in each direction (step S7).

When an operation for rotating the end effector model 35 is input to the user interface 9, the CPU 110 changes the posture of the end effector model 35 displayed on the UI screen 40 in response to the operation input. Further, when an operation for translating or rotating the work model 3 is input to the user interface 9, the CPU 110 changes the position and orientation of the work model 3 displayed on the UI screen 40 in response to the operation input. (Step S8).

Next, the CPU 110 determines the interference between the end effector model 35 and the work model 3 displayed on the UI screen 40, and displays the determination result (step S9).

Next, the CPU 110 determines whether or not the OK button 52 has been operated on the UI screen 40 (step S10). If the OK button is not operated (NO in step S10), the preset processing returns to step S7.

When the OK button is operated (YES in step S10), the CPU 110 uses the right claw model 36R and the left claw required to grip the work model 3 for the movement amount currently input to the movement amount input field 50. It is set as the movement amount of the unit model 36L (step S11). _{Specifically, the coordinate transformation matrix H MR} for moving in parallel by the amount of movement set in the movable direction of the right claw model 36R and the coordinates for moving in parallel by the amount of movement set in the movable direction of the left claw model 36L. The transformation matrix H _ML is set.

Next, the CPU 110 sets the relative position and orientation of the work model 3 with respect to the support model 37 when the end effector model 35 grips the work model 3 (step S12). Specifically, a coordinate transformation matrix ^tM H _{w M} indicating the relative position / orientation is set.

<J. Flow of picking position search process>
Next, with reference to FIG. 15, the flow of the search process for the picking position of the end effector 30 by the image processing device 100 will be described. FIG. 15 is a flowchart showing the flow of the picking position search process executed by the image processing apparatus according to the embodiment.

First, the CPU 110 reads the first CAD data 10a showing the three-dimensional shape of the end effector 30 (step S21). The CPU 110 reads the attribute information saved in step S4 (step S22). The CPU 110 reads a plurality of templates corresponding to the plurality of postures of the work 1 (step S23). _{Further, the CPU 110 reads the coordinate transformation matrices H MR} and H _ML corresponding to the movement amount set in step S11 (step S24).

Next, the CPU 110 measures the three-dimensional shape of the surface of the object (work 1 and container 5) existing in the visual field region of the image pickup device 8 based on the image received from the image pickup device 8 (step S25).

Next, the CPU 110 collates the three-dimensional point cloud data, which is the measurement result, with the plurality of templates read in step S23, and detects the position and orientation of the work 1 (step S26). ^{The position and orientation of the work 1 are indicated by a coordinate transformation matrix C} H _W indicating the reference point and the specific direction of the work 1 in the camera coordinate system.

Further, the CPU 110 collates the three-dimensional point cloud data, which is the measurement result, with the shape information of the container 5 stored in advance, and detects the position and orientation of the container 5 (step S27).

When the CPU 110 selects a predetermined number of work 1s as the target work 2 from the work 1 whose position and posture are detected, and grips the target work 2 for each of the selected predetermined number of target work 2. The position / posture candidate to be taken by the support portion 32 is determined. The CPU 110 creates a list of a predetermined number of determined position / posture candidates (step S28). The position / orientation candidate is a coordinate transformation matrix that is the product of the coordinate transformation matrix ^C H _W indicating the position / orientation of the target work 2 and the inverse matrix ( ^tM H _wM ) ^{-1 of the coordinate transformation matrix tM} H _wM set in step S12. ^C H _W · ( ^tM H _wM ) ^-1 .

The CPU 110 selects one position / posture candidate from the list of position / posture candidates (step S29).

The CPU 110 converts the coordinates of each element constituting the support model 37 so as to match the position / orientation indicated by the selected position / orientation candidate (step S30). Specifically, the CPU 110 converts the coordinates to which the attribute information indicating the support portion model 37 is added in the first CAD data 10a by using the coordinate transformation matrix ^C H _W · ( ^tM H _wM ) ^-1 .

Further, the CPU 110 converts the coordinates of each element constituting the right claw portion model 36R and the left claw portion model 36L by using the _{coordinate transformation matrices H MR} and H _{ML read in step S24 (step S31).} Specifically, the CPU 110 converts the coordinates to which the attribute information indicating the right claw model 36R is added in the first CAD data 10a by using the coordinate transformation matrix ^C H _W · ( ^tM H _wM ) ^-1 · H _MR. do. The CPU 110 converts the coordinates to which the attribute information indicating the left claw portion model 36L is added in the first CAD data 10a by using the coordinate transformation matrix ^C H _W · ( ^tM H _wM ) ^-1 · H _ML .

Next, the CPU 110 has the end effector model 35 whose coordinates have been converted in steps S29 and S30, and the work 1 of the position and orientation detected in step S26 (however, the target work 2 corresponding to the position and orientation candidate selected in step S29). (Excluding)) and the interference determination (step S32).

Further, the CPU 110 determines the interference between the end effector model 35 whose coordinates have been transformed in steps S29 and S30 and the container 5 in the position and orientation detected in step S27 (step S33).

Next, the CPU 110 determines whether or not there is an unselected position / posture candidate in the list of position / posture candidates created in step S28 (step S34). If there is an unselected position / orientation candidate (YES in step S34), the search process returns to step S29. As a result, the interference determination is performed for each position / posture candidate.

When there is no unselected position / orientation candidate (NO in step S34), the CPU 110 sorts the position / orientation candidates determined to have no interference according to a predetermined condition (step S35). The predetermined conditions are, for example, in the order close to the position and orientation of the support portion 32 of the current end effector 30. In addition, even if the predetermined conditions are in descending order of goodness of fit at the time of workpiece detection, in order of decreasing amount of rotation of the end effector, in order of approach angle closer to the vertical direction, and in order of higher degree of exposure (order of lower degree of overlap). good.

The CPU 110 generates position / posture information indicating the highest position / posture candidate, and outputs the generated position / posture information to the robot controller 200 (step S36). As a result, the search process for the picking position of the end effector 30 is completed.

<K. Action / effect>
As described above, the control system SYS according to the present embodiment includes the CAD reading unit 12, the user interface 9, the parts setting unit 13, the 3D measurement unit 20, the selection unit 22, the model change unit 26, and the interference. A determination unit 27 is provided. The CAD reading unit 12 acquires the first CAD data 10a showing the three-dimensional shape of the end effector 30. The parts setting unit 13 causes the user interface 9 to display the end effector model 35 represented by the first CAD data 10a. Based on the information input to the user interface 9, the parts setting unit 13 includes the right claw model 36R and the left claw model 36L corresponding to the right claw and the left claw of the end effector model 35, and the right claw. The movable direction of the portion model 36R and the left claw portion model 36L is set. The 3D measurement unit 20 measures the three-dimensional shape of the visual field region including the plurality of workpieces 1. The selection unit 22 selects a position / posture candidate of the end effector 30 (specifically, the support unit 32) when picking the target work 2 based on the measurement result of the 3D measurement unit 20. The interference determination unit 27 determines interference between the end effector model 35 of the position / orientation candidate selected by the selection unit 22 and an object other than the target work 2. However, the interference determination unit 27 performs interference determination in the end effector model 35 in a state where the right claw model 36R and the left claw model 36L are each moved in the movable direction by a set amount of movement.

According to the above configuration, by appropriately setting the movement amount according to the size of the target work 2, the shape of the end effector model 35 is adjusted to match the shape of the end effector 30 when actually picking the target work 2. Can be changed. Therefore, by using the modified end effector model 35, it is possible to accurately determine the interference between the end effector 30 and an object other than the target work 2.

The CAD reading unit 12 further acquires the second CAD data 11a showing the three-dimensional shape of the target work 2. The control system SYS has a movement amount setting unit 14 that sets the movement amount of the right claw part model 36R and the left claw part model 36L necessary for the end effector model 35 to pick the work model 3 shown by the second CAD data 11a. Be prepared. The model changing unit 26 moves the right claw portion model 36R and the left claw portion model 36L by the movement amount set by the movement amount setting unit 14.

According to the above configuration, by using the work model 3 and the end effector model 35, the movement amounts of the right claw portion model 36R and the left claw portion model 36L required for picking can be appropriately set.

The support portion 32 supports the right claw portion 31R and the left claw portion 31L so as to be able to translate, respectively, so that the distance (interval) between the right claw portion 31R and the left claw portion 31L is variable. In this case, since the number of parts constituting the end effector model 35 is small, the operator can easily set the movable part model (right claw part model 36R and left claw part model 36L) by using the user interface 9. Can be done.

<L. Modification 1>
In the above description, it is assumed that one type of work 1 having the same shape and size is bulk-loaded in the container 5. However, in the container 5, a plurality of types of workpieces having different shapes and sizes may be stacked in bulk.

When a plurality of types of workpieces are stacked in bulk in the container 5, the CAD reading unit 12 reads CAD data indicating the three-dimensional shape of each of the plurality of types of workpieces from the second storage unit 11.

The movement amount setting unit 14 sets the movement amount of the right claw portion model 36R and the left claw portion model 36L necessary for gripping the work for each of the plurality of types of workpieces. That is, the movement amount setting unit 14 calculates _{the coordinate transformation matrix H MR} for translating each of the plurality of types of workpieces by the movement amount set in the movable direction of the right claw portion model 36R. Further, the movement amount setting unit 14 calculates a _{coordinate transformation matrix H ML} for translating each of the plurality of types of workpieces by the movement amount set in the movable direction of the left claw portion model 36L.

^{The relative position setting unit 15 sets a coordinate transformation matrix tM} H _{w M} indicating the relative position / orientation of the work model with respect to the support model 37 when gripping the work for each of the plurality of types of works.

The preset processing shown in FIG. 14 is executed for each of the plurality of types of workpieces. As a result, the coordinate transformation matrices H _MR , H _ML , and ^tM H _wM are set for each of a plurality of types of works.

The selection unit 22 identifies the type of the work existing in the visual field region of the image pickup apparatus 8 based on the measurement result of the 3D measurement unit 20, and the support unit 32 when gripping the work should be taken according to the identification result. Select a position / posture candidate.

The model changing unit 26 moves the right claw portion model 36R and the left claw portion model 36L by a set amount of movement according to the type of work corresponding to the position / posture candidate.

FIG. 16 is a flowchart showing the flow of the picking position search process executed by the image processing apparatus according to the first modification. The flowchart shown in FIG. 16 is different from the flowchart shown in FIG. 15 in that steps S37 to S40 are included instead of steps S23, S24, and S26.

In step S37, the CPU 110 reads a plurality of templates corresponding to the plurality of postures for each of the plurality of types of workpieces.

In step S38, the CPU 110 collates the three-dimensional point cloud data, which is the measurement result of step S25, with the plurality of templates read in step S37, and identifies the type of work existing in the visual field region of the image pickup apparatus 8. , Detects the position and orientation of the work.

In step S39, the CPU 110 selects a predetermined number of works as the target work 2 from the works whose position and posture are detected, and grips the target work 2 for each of the selected predetermined number of target works 2. Occasionally, the support unit 32 determines a position / posture candidate to be taken. The CPU 110 determines a position / orientation candidate by using ^{a coordinate transformation matrix tM} H _wM according to the type of the work. The CPU 110 creates a list of a predetermined number of determined position / posture candidates.

In step S40, the CPU 110 reads _{the coordinate transformation matrices H MR} and H _ML for moving the work by the set amount of movement for the type of work corresponding to each position / orientation candidate.

After step S40, the same steps S29 to S36 as in FIG. 15 are executed. However, in step S30, the coordinates of each element constituting the support model 37 are converted by using ^{the coordinate transformation matrix tM} H _wM set according to the type of the work corresponding to the selected position / orientation candidate. Further, in step S31, the right claw portion model 36R and the left claw portion model 36L are configured by using _{the coordinate transformation matrices H MR} and H _ML set according to the type of the work corresponding to the selected position / orientation candidate. The coordinates of each element are transformed.

<M. Modification 2>
In the above description, the interference determination unit 27 determines the interference between the container 5 and the end effector model 35 detected based on the three-dimensional point cloud data which is the measurement result of the 3D measurement unit 20. However, the container 5 is hidden in the work piled up in bulk and may not be included in the image obtained by the image pickup of the image pickup apparatus 8. In this case, the container detection unit 21 cannot detect the position and orientation of the container 5 from the three-dimensional point cloud data.

The container 5 is usually placed in place. Therefore, the image processing apparatus 100 may store in advance container information indicating the coordinates of the space indicated by the container 5 arranged at a fixed position. The interference determination unit 27 may determine interference between the end effector model 35 and the container 5 based on the container information.

The container information may be shown in the camera coordinate system or in the base coordinate system. If the container information is indicated by the base coordinate system, the interference determination unit 27 converts the container information using the coordinate transformation matrix ^B H _C to convert the base coordinate system to the camera coordinate system, the container information after conversion It may be used to make an interference determination. Or, the interference determination unit 27 converts the coordinates of the end effector model 35 after the change according to the coordinate transformation matrix ^(B H _C) ^-1, performs a collision determination of the end effector model 35 and the container 5 in the base coordinate system May be good.

<N. Modification 3>
In the above description, it is assumed that the work 1 having a predetermined shape and size is bulk-loaded in the container 5. However, the workpieces 1 having variations in shape and size may be bulk-loaded in the container 5.

FIG. 17 is a diagram showing an end effector when gripping a work 1 having variations in shape and size. Even if the shape and size of the work 1 vary, the target work 2 can be gripped if the following conditions a and b are satisfied.
Condition a: The distance between the plane passing through both lower ends of the right claw portion 31R and the left claw portion 31L and the highest point of the target work 2 is equal to or larger than a predetermined threshold value dh in consideration of variations in the shape and size of the work 1. Is.
Condition b: The right claw portion 31R and the left claw portion 31L do not interfere with an object other than the target work 2.

The image processing device according to the third modification selects the position / orientation candidate of the support portion 32 so as to satisfy the conditions a and b based on the three-dimensional shape of the visual field region of the image pickup device 8, and also selects the position / orientation candidate of the support portion 32 and the right claw portion model 36R and the right claw portion model 36R. Set the movement amount of the left claw model 36L.

(N-1. Functional configuration)
FIG. 18 is a block diagram showing a functional configuration of the image processing apparatus according to the modified example 3. As shown in FIG. 18, the image processing device 100A according to the modification 3 is different from the image processing device 100 shown in FIG. 4 in that it does not include the second storage unit 11 and the relative position setting unit 15. .. Further, the image processing device 100A sets a range instead of the movement amount setting unit 14, the container detection unit 21, the selection unit 22, the model change unit 26, and the interference determination unit 27, as compared with the image processing device 100 shown in FIG. The difference is that a unit 16, a container information storage unit 21A, a selection unit 22A, and an interference determination unit 27A are provided. Compared with the selection unit 22 shown in FIG. 4, the selection unit 22A has a recess detection unit 29, a position / orientation determination unit 25A, and a movement amount instead of the work detection unit 23, the target work selection unit 24, and the position / orientation determination unit 25. The difference is that the setting unit 14A is included. In the modified example 3, the container 5 is arranged at a fixed position as in the modified example 2.

The range setting unit 16 sets the settable range and the number of steps of the movement amount of the right claw portion model 36R and the left claw portion model 36L based on the information input to the user interface 9.

FIG. 19 is a diagram showing an example of a UI screen displayed on the user interface according to the modified example 3. Compared to the UI screen 40 shown in FIG. 6, the UI screen 40A shown in FIG. 19 includes range input fields 55 and 56 and a step number input field 57 instead of the work display button 51 and the movement amount input field 50. It's different.

The range setting unit 16 sets a range from the value (minimum value) input in the range input field 55 to the value (maximum value) input in the range input field 56 as the range in which the movement amount can be set. The range setting unit 16 sets the value input in the step number input field 57 as the number of steps of the movement amount.

Returning to FIG. 18, the container information storage unit 21A stores container information indicating the three-dimensional coordinates of the container 5 arranged at a fixed position.

The selection unit 22A selects the position / orientation candidate of the support portion when picking the work based on the three-dimensional shape of the visual field region of the image pickup device 8, and also selects the right claw model 36R and the right claw part model 36R necessary for picking the work. Set the movement amount of the left claw model 36L.

With reference to FIG. 20, the processing of the recess detection unit 29, the movement amount setting unit 14A, and the position / orientation determination unit 25A of the selection unit 22A will be described. FIG. 20 is a diagram showing processing of a selection unit of the image processing apparatus according to the modified example 3. FIG. 20 shows a view of the three-dimensional shape measured by the 3D measuring unit 20 when viewed from above.

The recess detection unit 29 detects a recess in the three-dimensional shape of the visual field region of the image pickup apparatus 8 based on the three-dimensional point cloud data which is the measurement result of the 3D measurement unit 20. Specifically, the recess detection unit 29 extracts the highest point data from the three-dimensional point cloud data. Let z_max be the Z coordinate value of the highest point. The recess detection unit 29 masks the point cloud data (z <z_max−dh) whose Z coordinate value is less than z_max−dh in the three-dimensional point cloud data. The recess detection unit 29 labels the remaining region on a plane having a Z coordinate of z_max−dh. In FIG. 20, regions 60 and 61 indicate labeled regions. The region 60 is a region including a point projected onto a plane whose Z coordinate is z_max−dh from the highest point in the three-dimensional shape. Regions other than the regions 60 and 61 correspond to the recesses in the three-dimensional shape.

The movement amount setting unit 14A selects the movement amount in order from the minimum value to the maximum value of the movement amount range set by the range setting unit 16. The movement amount setting unit 14A sets the amount obtained by dividing the difference between the maximum value and the minimum value of the movement amount range by (number of steps-1) as the step width, and sequentially selects the movement amount at intervals of the step width.

The movement amount setting unit 14A determines whether or not both of the following conditions (i) and (ii) are satisfied when the right claw portion model 36R and the left claw portion model 36L are moved in the movable direction by the selected movement amount. judge.
Condition (i): The right claw model 36R and the left claw model 36L are located around the region 60 (positioned so as to sandwich the region 60).
Condition (ii): The right claw model 36R and the left claw model 36L are located in the recess.
When both the conditions (i) and (ii) are satisfied, the movement amount setting unit 14A sets the selected movement amount as the movement amount required when gripping the target work 2 existing in the area 60.

For example, when the movable directions of the right claw model 36R and the left claw model 36L are set to be opposite to each other, and the right claw model 36R and the left claw model 36L are arranged side by side along the movable direction. , The movement amount is set as follows.

As shown in FIG. 20, the movement amount setting unit 14A calculates the coordinate value of the center of gravity G of the region 60. Further, the movement amount setting unit 14A moves the distance between the right claw part model 36R and the left claw part model 36L when the right claw part model 36R and the left claw part model 36L are moved by the movement amount selected in the movable direction, respectively. (Opening width W) is calculated. The movement amount setting unit 14A specifies a ring-shaped region 64 centered on the center of gravity G and having the calculated opening width W as the inner diameter and the value obtained by adding α to the opening width W as the outer diameter. α provides a predetermined margin to the total length of the end faces (hereinafter referred to as “lower end faces”) on the opposite side of the support model 37 in the right claw model 36R and the left claw model 36L along the movable direction. The added value.

The movement amount setting unit 14A of the right claw portion model 36R so that the straight line coincides with the movable direction at two places where the straight line and the ring-shaped region 64 intersect for each of a predetermined number of straight lines passing through the center of gravity G. A simulation is performed in which the lower end surface 63R and the lower end surface 63L of the left claw model 36L are arranged. The angle formed by each of a predetermined number of straight lines and the reference direction (for example, the X direction) is predetermined. FIG. 20 shows an example when a straight line 65 is selected from a predetermined number of straight lines. The movement amount setting unit 14A determines whether or not the lower end surfaces 63R and 63L overlap with the areas 60 and 61. When the lower end surfaces 63R and 63L do not overlap with the areas 60 and 61, the movement amount setting unit 14A may set the selected movement amount as the movement amount that can grip the target work 2 existing in the area 60.

Similar to the above-mentioned movement amount setting unit 14, the movement amount setting unit 14A calculates _{the coordinate transformation matrix H MR for translating the movement amount set in the movable direction of the right claw portion model 36R.} Further, the movement amount setting unit 14A calculates a _{coordinate transformation matrix H ML} for parallel movement by the movement amount set in the movable direction of the left claw portion model 36L.

When the movement amount setting unit 14A sets the movement amount capable of gripping the target work 2 existing in the area 60, the direction of the straight line and the coordinates of the center of gravity G determined that the lower end surfaces 63R and 63L do not overlap with the areas 60 and 61. Is output to the position / orientation determination unit 25A.

When there are a plurality of straight lines determined that the lower end surfaces 63R and 63L do not overlap with the regions 60 and 61, the movement amount setting unit 14A outputs all the directions of the plurality of straight lines to the position / posture determination unit 25A.

The position / posture determining unit 25A determines the position / posture to be taken by the support unit 32 when gripping the target work 2 existing in the region 60. Specifically, the position / orientation determination unit 25A calculates the position / orientation of the support unit model 37 in the camera coordinate system when the end effector model 35 is arranged so as to satisfy the following conditions (1) to (3). .. The position and orientation are shown ^{by the coordinate transformation matrix C} H _tM that transforms the camera coordinate system into the support model coordinate system. The coordinate transformation matrix ^C H _tM represents the components of the basis vectors 37v1 to 37v3 of the support model coordinate system and the position of the origin 37o in the camera coordinate system (see FIG. 10). The position / posture determination unit 25A calculates the coordinate transformation matrix ^C H _tM as information indicating a position / posture candidate to be taken by the support unit 32 when gripping the target work 2.
Condition (1): The lower end surface 63R of the right claw model 36R and the lower end surface 63L of the left claw model 36L are located on a plane whose Z coordinate is z_max-dh.
Condition (2): The midpoint between the center of the lower end surface 63R of the right claw model 36R and the center of the lower end surface 63L of the left claw model 36L coincides with the center of gravity G received from the movement amount setting unit 14A.
Condition (3): The movable direction of the right claw portion model 36R and the left claw portion model 36L coincides with the direction of the straight line received from the movement amount setting unit 14A.

The interference determination unit 27A determines the interference between the end effector model 35 and the container 5 based on the container information stored in the container information storage unit 21A.

(N-2. Flow of preset processing)
FIG. 21 is a flowchart showing the flow of the preset processing executed by the image processing apparatus according to the modification 3. The flowchart shown in FIG. 21 is different from the flowchart shown in FIG. 14 in that steps S7 to S9 are omitted and steps S41 and S42 are included instead of steps S11 and S12.

When the OK button is operated (YES in step S10), in step S41, the CPU 110 sets the values currently input in the range input fields 55 and 56 as the minimum value and the maximum value of the settable range of the movement amount, respectively. do. Further, in step S42, the CPU 110 sets the value currently input in the step number input field 57 as the number of steps.

(N-3. Flow of picking position search process)
Next, with reference to FIGS. 22 and 23, the flow of the search process for the picking position of the end effector 30 by the image processing apparatus 100A according to the modification 3 will be described. FIG. 22 is a flowchart showing the flow of the picking position search process executed by the image processing apparatus according to the modified example 3. FIG. 23 is a flowchart showing an example of the processing flow of the subroutine in step S52 of FIG. The flowchart shown in FIG. 22 is a point in which steps S23 and S24 are omitted, a point in which steps S51 to S55 are included instead of steps S26 to S27, and a point in which steps S32 and S33 are substituted, as compared with the flowchart shown in FIG. Is different in that it includes step S56.

In step S51, the CPU 110 selects the minimum movement amount in the settable range set in step S41.

Next, in step S52, the CPU 110 extracts a position / posture candidate of the support portion 32 capable of gripping the target work 2 when the right claw portion 31R and the left claw portion 31L are moved in the movable direction by the selected movement amount. do.

For example, in step S52, steps S521 to S527 shown in FIG. 23 are executed.

In step S521, the CPU 110 extracts the highest point data (x, y, z_max) from the three-dimensional point cloud data measured by the 3D measuring unit 20. In step S522, the CPU 110 masks the point cloud data (z <z_max-dh) whose Z coordinate value is less than z_max-dh in the three-dimensional point cloud data. In step S523, the CPU 110 labels the remaining regions on the plane whose Z coordinate is z_max-dh, and calculates the coordinates of the center of gravity of each region.

In step S524, the CPU 110 has a ring-shaped region whose inner diameter is the opening width W corresponding to the selected movement amount and whose outer diameter is the value obtained by adding α to the opening width W, centered on the center of gravity of the region corresponding to the highest point. 64 is specified (see FIG. 20).

In step S525, the CPU 110 extracts a position / posture candidate to be taken by the support portion 32 when gripping the target work 2 based on the specified ring-shaped region 64 and the labeled regions 60 and 61. Specifically, the CPU 110 is a support portion model 37 when the lower end surface 63R of the right claw portion model 36R and the lower end surface 63L of the left claw portion model 36L do not overlap with the regions 60 and 61 in the ring-shaped region 64. The position / posture is determined as a position / posture candidate to be taken by the support portion 32. As described above, the position and orientation candidates to be taken by the support portion 32 are indicated by the ^{coordinate transformation matrix C} H _tM.

Returning to FIG. 24, after step S52, in step S53, the CPU 110 determines whether or not at least one position / posture candidate has been extracted. When at least one position / posture candidate is not extracted (NO in step S53), in step S54, the CPU 110 is calculated from the currently selected movement amount according to the number of steps set in step S41. Select a movement amount that is larger by the width. After step S54, the search process returns to step S52, and the position / posture candidate extraction process is executed.

When at least one position / posture candidate is extracted (YES in step S53), in step S55, the CPU 110 sets the selected movement amount as the movement amount that can grip the target work 2. _{Specifically, the coordinate transformation matrix H MR} for moving in parallel by the amount of movement set in the movable direction of the right claw model 36R and the coordinates for moving in parallel by the amount of movement set in the movable direction of the left claw model 36L. The transformation matrix H _ML is set.

After that, steps S28 to S31 described with reference to FIG. 15 are executed. In step S30, the coordinates to which the attribute information indicating the support portion model 37 is added are transformed using the ^{coordinate transformation matrix C} H _tM. However, the coordinate transformation matrix ^C H _tM is determined in step S525. Further, in step S31, the coordinates of each element constituting the right claw portion model 36R and the left claw portion model 36L are converted using _{the coordinate transformation matrices H MR} and H _ML. However, the coordinate transformation matrices H _MR and H _ML are set in step S55.

Next, in step S56, the CPU 110 determines the interference between the coordinate-converted end effector model 35 and the container 5 having the position and orientation indicated by the container information stored in the container information storage unit 21A. After that, steps S34 to S36 described with reference to FIG. 15 are executed.

<O. Modification 4>
In the above description, the operator specifies the parts corresponding to the right claw portion 31R, the left claw portion 31L, and the support portion 32, respectively, by using the frame lines 42 to 44 in the end effector model 35 displayed on the user interface 9. do. At this time, the image processing device estimates the portions corresponding to the right claw portion 31R, the left claw portion 31L, and the support portion 32 from the shape of the end effector model 35, and the default frame lines 42 to 44 are estimated according to the estimation result. Positional orientation and size may be determined. As a result, the operator can simplify the operation for the frame lines 42 to 44.

FIG. 24 is a block diagram showing an example of the functional configuration of the image processing apparatus according to the modified example 4. Compared with the image processing device 100 shown in FIG. 4, the image processing device 100B according to the modified example 4 includes a first CAD reading unit 12A and a second CAD reading unit 12B instead of the CAD reading unit 12, and has a learning database 17 and a learning database 17. , The learning machine 18 and the discriminating machine 19 are further provided.

The first CAD reading unit 12A reads the first CAD data 10a from the first storage unit 10. The first CAD reading unit 12A converts the read first CAD data 10a into an internal representation (a set of elements such as a box, a cross section, a triangular mesh, and a point). The second CAD reading unit 12B reads the second CAD data 11a from the second storage unit 11.

The learning database 17 stores the learning data set 17a. In the training data set 17a, each of a plurality of images obtained by projecting end effectors of various shapes from various directions contains training data. The training data is training data in which attribute information indicating which of the support portion and the movable portion corresponds to each pixel of the image is added.

The learning machine 18 executes training of the trained model using the training data set 17a. The learning machine 18 outputs the generated trained model to the identification machine 19. The learning machine 18 generates a trained model by using a segmentation method such as Segnet (http://mi.eng.cam.ac.uk/projects/segnet).

The trained model receives the input of the image data of the end effector, and for each pixel constituting the image shown by the image data, the score indicating the possibility that the pixel corresponds to the support portion (hereinafter, "" It is referred to as "first score"). Further, the trained model calculates a score (hereinafter, referred to as “second score”) indicating the possibility that the element corresponds to a movable portion for each pixel.

The trained model includes, for example, a preprocessing network classified as CNN (Convolutional Neural Network), an intermediate layer, an activation function corresponding to an output layer, and a Softmax function.

The discriminator 19 calculates the first score and the second score for each pixel of the image displayed in the display area 41 of the UI screen 40 by using the trained model. The discriminator 19 identifies pixels having a first score larger than the second score and having a first score equal to or higher than a predetermined threshold value as pixels corresponding to the support unit model 37. The discriminator 19 identifies pixels having a second score larger than the first score and having a second score equal to or higher than a predetermined threshold value as pixels corresponding to the movable portion model. The discriminator 19 identifies pixels whose first score and second score are less than a specified threshold value as background pixels. When a plurality of pixels corresponding to the movable portion model are labeled in two sets, the discriminator 19 identifies the pixel belonging to one of the two sets as the pixel corresponding to the right claw portion model 36R. , The pixel belonging to the other is identified as the pixel corresponding to the left claw model 36L.

The parts setting unit 13 determines the default position / orientation and size of the frame lines 42 to 44 according to the identification result by the identification machine 19. As a result, if the identification result of the identification machine 19 is correct, the operator can omit the operation for the frame lines 42 to 44. If there is an error in the identification result of the identification machine 19, the operator may appropriately correct the position / orientation and size of the frame lines 42 to 44.

<P. Modification 5>
In the above-described embodiments and modifications, an example in which the end effector 30 has two claws that move in parallel as movable portions has been described. However, the end effector 30 may have three or more claws that move in parallel as movable portions. For example, it may be a multi-finger hand in which the N claws move along N straight lines passing through the center points of the N claws. In the multi-fingered hand, the target work 2 can be gripped by moving the N claws so as to approach each other. With such a multi-fingered hand, even if the work 1 has a predetermined shape and size, the amount of movement of the claw portion can be set by the same method as in the above-mentioned modification 3.

Alternatively, the end effector 30 may have a movable portion that rotates and moves. When having a movable portion that rotates and moves, the parts setting unit 13 may set the rotation center axis and the rotation direction as the movable direction for the movable portion model corresponding to the movable portion.

The picking of the target work 2 by the end effector 30 is not limited to the above-mentioned gripping operation. For example, picking may be performed by adsorbing the target work 2.

<Q. Modification 6>
In the above embodiment, the image processing apparatus 100 performs both the preset processing and the search processing for searching the picking position. However, the preset processing may be executed by an information processing device different from the image processing device 100. The information processing device is composed of a general-purpose computer and has a hardware configuration (FIG. 3) similar to that of the image processing device. If the information processing device includes a first storage unit 10, a second storage unit 11, a CAD reading unit 12, a parts setting unit 13, a movement amount setting unit 14, and a relative position setting unit 15. good.

<R. Addendum>
As described above, the present embodiment and modifications include the following disclosures.

(Structure 1)
A control system (SYS) that controls a robot (300) having an end effector (30) that picks one object among a plurality of objects (1) as an object (2).
The end effector (30) includes a movable portion (31R, 31L) and a support portion (32) that movably supports the movable portion (31R, 31L).
The control system (SYS))
The acquisition unit (12, 12A) for acquiring the first shape data indicating the three-dimensional shape of the end effector (30), and
User interface (9) and
The end effector model (35) shown in the first shape data is displayed on the user interface (9), and the end effector model (35) of the end effector model (35) is described based on the information input to the user interface (9). A first setting unit (13) that sets the movable portion model (36R, 36L) corresponding to the movable portion (31R, 31L) and the movable direction of the movable portion model (36R, 36L), and
The measuring unit (20) that measures the three-dimensional shape of the visual field region including the plurality of objects, and
A selection unit (22, 22A) that selects a position / posture candidate of the end effector (30) when picking the object based on the measurement result of the measurement unit (20).
The end effector model (35) of the position / posture candidate selected by the selection unit (22, 22A) is provided with a determination unit (27, 27A) for determining interference with an object other than the object.
The determination unit (26, 27, 27A) determines the interference in the end effector model (35) in a state where the movable unit model (36R, 36L) is moved in the movable direction by a set amount of movement. Control system (SYS) to perform.

(Structure 2)
The acquisition unit (12, 12B) further acquires the second shape data indicating the three-dimensional shape of the object, and further acquires the second shape data.
The control system (SYS) further
The second setting unit (14) that sets the movement amount of the movable part model (36R, 36L) required for the end effector model (35) to pick the object model (3) shown by the second shape data. )
The control system (SYS) according to configuration 1, wherein the determination unit (26, 27) moves the movable unit model (36R, 36L) by a movement amount set by the second setting unit (14).

(Structure 3)
The plurality of objects include a plurality of types of objects having at least one different shape and size from each other.
The acquisition unit (12, 12B) further acquires second shape data indicating the three-dimensional shape of the object for each of the plurality of types of objects.
The control system (SYS) further
For each of the plurality of types of objects, the second movement amount of the movable part model (36R, 36L) required for the end effector model to pick the object model indicated by the second shape data is set. Equipped with a setting unit (14)
The selection unit (22) identifies the type of the object based on the measurement result of the measurement unit (20), selects the position / posture candidate according to the identification result, and selects the position / posture candidate.
The determination unit (26, 27) moves the movable unit model (36R, 36L) by the amount of movement set by the second setting unit (14) with respect to the type identified by the selection unit (22). The control system (SYS) according to the configuration 1.

(Structure 4)
A second setting unit (14A) for setting the amount of movement of the movable unit model (36R, 36L) required for picking the object based on the three-dimensional shape of the visual field region is further provided.
The control system (SYS) according to configuration 1, wherein the determination unit (26, 27A) moves the movable unit model (36R, 36L) by a movement amount set by the second setting unit (14A).

(Structure 5)
The control system (SYS) according to any one of configurations 1 to 4, wherein the support portion (32) supports the movable portion (31R, 31L) so that at least one of translation and rotational movement is possible.

(Structure 6)
The movable portion (31R, 31L) has a plurality of claw portions (31R, 31L).
From configuration 1, the support portion (32) supports the plurality of claw portions (31R, 31L) so as to be able to translate, respectively, so that the distance between the plurality of claw portions (31R, 31L) is variable. 4. The control system (SYS) according to any one of 4.

(Structure 7)
The movable portion (31R, 31L) has a plurality of claw portions (31R, 31L).
The second setting unit (14A) is
The highest position is extracted from the three-dimensional shape of the visual field region, and
Of the visual field areas, the area where the altitude difference from the extracted position is equal to or less than the specified value is labeled.
The plurality of claws (31R, 31L) are located around one labeled region, and the plurality of claws (31R, 31L) do not overlap with the other labeled regions. The control system (SYS) according to the configuration 4, which sets the movement amount of each of the claw portions (31R, 31L).

(Structure 8)
An information processing device (100) that processes information on shape data indicating a three-dimensional shape of an end effector (30) that picks an object (2) using a user interface (9).
The end effector (30) includes a movable portion (31R, 31L) and a support portion (32) that movably supports the movable portion (31R, 31L).
The information processing device (100) is
The acquisition unit (12, 12A) for acquiring shape data indicating the three-dimensional shape of the end effector (30), and
The end effector model (35) indicated by the shape data is displayed on the user interface (9), and the movable portion of the end effector model (35) is based on the information input to the user interface (9). An information processing apparatus (100) including a movable portion model (36R, 36L) corresponding to (31R, 31L) and a setting unit (14, 14A) for setting a movable direction of the movable portion model (36R, 36L). ..

(Structure 9)
It is a control method for controlling a robot (300) having an end effector (30) that picks one object from a plurality of objects (1) as an object (2).
The end effector (30) includes a movable portion (31R, 31L) and a support portion (32) that movably supports the movable portion (31R, 31L).
The control method is
A step of acquiring shape data indicating the three-dimensional shape of the end effector (30), and
The end effector model (35) indicated by the shape data is displayed on the user interface (9), and based on the information input to the user interface (9), the movable part (35) of the end effector model (35) is displayed. A step of setting the movable part model (36R, 36L) corresponding to 31R, 31L) and the movable direction of the movable part model (36R, 36L), and
The step of measuring the three-dimensional shape of the visual field region including the plurality of objects, and
A step of selecting a position / orientation candidate of the end effector (30) when picking the object (2) based on the measurement result, and a step of selecting the position / orientation candidate.
A step of determining interference between the end effector model (35) of the selected position / orientation candidate and an object other than the object (2) is provided.
The step of performing the interference determination is a control method including a step of moving the movable portion model (36R, 36L in the movable direction) by a set movement amount in the end effector model (35).

Although embodiments of the present invention have been described, the embodiments disclosed herein should be considered to be exemplary and not restrictive in all respects. The scope of the present invention is indicated by the scope of claims and is intended to include all modifications within the meaning and scope equivalent to the scope of claims.

1 work, 2 target work, 2o, 3o, 37o reference point (origin), 2v1, 2v3, 3v1, 3v3, 3v2, 37v3, 37v2, 37v1 specific direction (base vector), 3 work model, 5 container, 7 projector, 8 Image pickup device, 9 User interface, 10 1st storage unit, 10a 1st CAD data, 11 2nd storage unit, 11a 2nd CAD data, 12 CAD reading unit, 12A 1st CAD reading unit, 12B 2nd CAD reading unit, 13 parts setting Unit, 14, 14A Movement amount setting unit, 15 Relative position setting unit, 16 Range setting unit, 17 Learning database, 17a Learning data set, 18 Learning machine, 19 Identification machine, 20 3D measurement unit, 21 Container detection unit, 21A container information storage unit, 22,22A selection unit, 23 work detection unit, 24 target work selection unit, 25,25A position / orientation determination unit, 26 model change unit, 27,27A interference determination unit, 28 grip position output unit, 29. Recess detection part, 30 end effector, 31L left claw part, 31R right claw part, 32 support part, 33 articulated arm, 34 base, 35 end effector model, 36L left claw part model, 36R right claw part model, 37 support part Model, 40,40A UI screen, 41 display area, 42,43,44 frame line, 45 support part selection button, 46 right claw part selection button, 47 left claw part selection button, 48,49 movable direction input field, 50 movement Amount input field, 51 work display button, 52 OK button, 53 cancel button, 54 thick line, 55,56 range input field, 57 step number input field, 60,61 area, 63L, 63R bottom surface, 64 ring-shaped area, 65 Straight line, 90 touch panel, 92 display, 100, 100A, 100B image processing device, 106 memory card, 110 CPU, 112 main memory, 114 hard disk, 116 camera interface, 116a image buffer, 118 input interface, 120 display controller, 122 projector interface , 124 communication interface, 126 data reader / writer, 128 bus, 200 robot controller, 300 Robot, SYS control system.

Claims (9)

A control system that controls a robot having an end effector that picks one object among a plurality of objects as an object.
The end effector includes a movable portion and a support portion that movably supports the movable portion.
The control system is
An acquisition unit that acquires first shape data indicating the three-dimensional shape of the end effector, and
With the user interface
The end effector model represented by the first shape data is displayed on the user interface, and based on the information input to the user interface, the movable part model corresponding to the movable part and the movable part of the end effector model are displayed. The first setting unit that sets the movable direction of the model,
A measuring unit that measures the three-dimensional shape of the visual field region containing a plurality of objects,
A selection unit that selects a position / orientation candidate for the end effector when picking the object based on the measurement result of the measurement unit.
A determination unit for determining interference between the end effector model of the position / orientation candidate selected by the selection unit and an object other than the object is provided.
The determination unit is a control system that performs the interference determination in a state where the movable unit model is moved in the movable direction by a set amount of movement in the end effector model. The acquisition unit further acquires the second shape data indicating the three-dimensional shape of the object, and further acquires the second shape data.
The control system further
A second setting unit for setting the movement amount of the movable unit model required for the end effector model to pick the object model indicated by the second shape data is provided.
The control system according to claim 1, wherein the determination unit moves the movable unit model by a movement amount set by the second setting unit. The plurality of objects include a plurality of types of objects having at least one different shape and size from each other.
The acquisition unit further acquires second shape data indicating the three-dimensional shape of the object for each of the plurality of types of objects.
The control system further
For each of the plurality of types of objects, a second setting unit for setting the movement amount of the movable portion model required for the end effector model to pick the object model indicated by the second shape data is provided.
The selection unit identifies the type of the object based on the measurement result of the measurement unit, selects the position / posture candidate according to the identification result, and selects the position / posture candidate.
The control system according to claim 1, wherein the determination unit moves the movable unit model by a movement amount set by the second setting unit with respect to the type identified by the selection unit. Further provided with a second setting unit for setting the amount of movement of the movable unit model required for picking the object based on the three-dimensional shape of the visual field region.
The control system according to claim 1, wherein the determination unit moves the movable unit model by a movement amount set by the second setting unit. The control system according to any one of claims 1 to 4, wherein the support portion supports the movable portion so that at least one of translation and rotational movement is possible. The movable portion has a plurality of claw portions and has a plurality of claw portions.
The control system according to any one of claims 1 to 4, wherein the support portion supports the plurality of claw portions so that the distance between the plurality of claw portions is variable so that the plurality of claw portions can be translated. The movable portion has a plurality of claw portions and has a plurality of claw portions.
The second setting unit is
The highest position is extracted from the three-dimensional shape of the visual field region, and
Of the visual field areas, the area where the altitude difference from the extracted position is equal to or less than the specified value is labeled.
The movement amount of each of the plurality of claws is set so that the plurality of claws are located around one labeled region and the plurality of claws do not overlap with the other labeled regions. The control system according to claim 4. An information processing device that uses a user interface to process information about shape data that indicates the three-dimensional shape of an end effector that picks an object.
The end effector includes a movable portion and a support portion that movably supports the movable portion.
The information processing device is
An acquisition unit that acquires shape data indicating the three-dimensional shape of the end effector, and
The end effector model indicated by the shape data is displayed on the user interface, and based on the information input to the user interface, the movable part model corresponding to the movable part and the movable part model of the end effector model An information processing device provided with a setting unit for setting the movable direction. It is a control method for controlling a robot having an end effector that picks one object from a plurality of objects as an object.
The end effector includes a movable portion and a support portion that movably supports the movable portion.
The control method is
A step of acquiring shape data indicating the three-dimensional shape of the end effector, and
The end effector model indicated by the shape data is displayed on the user interface, and the movable part model corresponding to the movable part and the movable part model of the movable part of the end effector model are movable based on the information input to the user interface. Steps to set the direction and
The step of measuring the three-dimensional shape of the visual field region including the plurality of objects, and
Based on the measurement results, a step of selecting a position / orientation candidate of the end effector when picking the object, and a step of selecting the position / orientation candidate.
A step of determining interference between the end effector model of the selected position / orientation candidate and an object other than the object is provided.
The step of performing the interference determination is a control method including a step of moving the movable portion model in the movable direction by a set amount of movement in the end effector model.

Images