JP5476225B2 - Interference determination apparatus and interference determination method - Google Patents

Description

  The present invention relates to an interference determination device and an interference determination method.

  A double-arm robot that has two horizontal articulated arms with lifting axes at the tip like human arms, has a high degree of freedom for the space required for installation, and at least a part of the work area of both arms. Since there is an overlap, there is an advantage that it is possible to perform combined work such as screwing while holding the workpiece. However, each arm has a danger of colliding in the movement path, and it is difficult for the operator to predict the movement. Therefore, when each arm of a double-arm robot moves in a work area that partially overlaps the work area of the other arm, it is necessary to determine whether or not each other's arm overlaps the movement trajectory of the opponent. Become. In addition, when the dual-arm robot actually performs work, there is a possibility of interference with obstacles such as jigs and safety fences in addition to the left and right arms. Judgment may also be necessary.

  There are several methods for performing interference determination. For example, in order to perform an interference check with a numerical control device that controls a machine tool, “define each interference area of a plurality of machine structures, move the interference area, and each of the interference areas of each machine structure For a numerical control device having a function of checking whether to interfere, a method of defining a rectangular solid, cylinder, or planar solid figure as the interference area, and checking for interference depending on whether each solid figure intersects or does not intersect. (See Patent Document 1). As another method for performing interference determination, for a horizontal articulated robot, “transform a 3D robot model into a 2D robot model projected onto a 2D plane” and “convert a 3D obstacle model into a 2D There is also a method of performing interference determination with what is divided as an obstacle model (see Patent Document 2).

JP 2009-054043 A JP 2004-202627 A

  As in the above-described Patent Document 1, interference determination for a three-dimensional object that moves in a three-dimensional direction is often performed by converting it into a three-dimensional simple approximation model that is easy to define. However, when considering only the two-dimensional motion of the double-arm robot (not considering the movement of the robot arm's lifting axis and the jig's up / down), handling this with a three-dimensional model can reduce processing time. It is not useful in terms of securing memory.

  In the invention of Patent Document 2, the obstacle model is divided for each height of the three-dimensional obstacle model, and the height from the installation surface of the horizontal articulated robot to the arm (the height is constant). Only obstacle models that match (with a certain thickness) are subject to interference determination. Therefore, it is possible to obtain in a short time a result of excluding a portion where three-dimensional intersection is possible from the interference determination in spite of using the two-dimensional robot model. The advantage of this method is that it is only necessary to determine a split surface that matches the height of the horizontal articulated robot. However, for example, when a horizontal articulated robot has to be divided into a plurality of division planes, it is necessary to perform interference determination on each division plane. This is also the case with a double-arm robot, since each part (the first link, the second link, the lifting shaft at the tip, etc.) constituting the robot arm has a different height, there are a large number of split surfaces on which interference determination should be performed. In such a case, there is a problem that the amount of calculation increases and the processing takes a long time. Further, in the invention of Patent Document 2, it is assumed that interference determination is performed only by overlapping the occupied areas of the two-dimensional robot model projected on the two-dimensional plane, and there is no solid intersection without interference. That is, in the case of Patent Document 2, a solid intersection without interference is not taken into consideration. However, it is expected that the accuracy of interference determination will be further improved by further taking into consideration whether there is a solid intersection without interference.

  Therefore, the present invention has been made in view of the above, and an object thereof is to provide an interference determination device and an interference determination method capable of performing interference determination in a short time while considering a solid intersection without interference. To do.

In order to solve the above-described problem, an interference determination apparatus according to the present invention is an interference determination apparatus that performs interference determination on each part of a robot and an obstacle operating in three dimensions, and involves movement among the parts. Interference condition information indicating whether or not there is interference when three-dimensionally intersecting each other part for each moving part that is a part, and indicates whether or not interference is involved when three-dimensionally intersecting Interference condition information storage means for storing the interference condition information set based on interference information, and projecting the space in which the robot operates on a two-dimensional plane, and then dividing the two-dimensional plane into a predetermined number of regions and Rumasu th management unit, and the area occupied by the sites, the information specifying the sites occupied, the occupation information recording means for recording the occupation information that associates, with the occupation information interference the region With condition information Zui it, characterized in that it comprises a determining means for determining whether or not accompanied by solid two or more different said portions with each other the interference cross each other.
Also, the interference determination device of the present invention, the identification number assigning means for assigning an identification number to each of the sites, and the interference information storage means for storing the interference information, further wherein the grid management means, each of said areas The occupation information recording means may record the identification number of each part as the occupation information in a memory area corresponding to the area occupied by each part.
The interference determination apparatus according to the present invention is an interference determination apparatus that performs interference determination on each part of a robot and an obstacle that operate in three dimensions, and has a plurality of digits and other identification numbers. An identification number assigning means for assigning an identification number set so that all the digits become “0” when calculating the logical product for each digit, and a part of the part with movement. Information indicating “0” is stored as interference information when the moving part does not interfere with the other part, and “1” is stored when there is interference when the moving part intersects. Interference information storage means, and information that is set based on the interference information, has a plurality of digits, and indicates whether or not interference occurs when the moving part intersects with another part. The interference condition information is stored for each moving part. An information storage means and a space in which the robot operates is projected onto a two-dimensional plane, and then the two-dimensional plane is divided into n1 × n2 cells and memory areas corresponding to the n1 × n2 cells, respectively. A grid managing means for securing the grid, a corresponding grid that is a grid corresponding to each part of the n1 × n2 grids, and information based on the identification number of each part, Occupancy information recording means for recording occupancy information having the number of digits in the memory area corresponding to the corresponding cell, and obtaining a logical product for each digit in the occupancy information and the interference condition information for each corresponding cell If the result of logical product is “0” in all digits, it is determined that the parts do not interfere with each other. If the result of logical product is “1” in at least one digit, the parts are Determining means for determining to Wataru may comprise.

The interference determination method of the present invention is an interference determination method for performing interference determination on each part of a robot and an obstacle operating in three dimensions, and for each moving part that is a part that involves movement among the above parts, Interference condition information indicating whether or not there is interference when crossing each other part, and is set based on interference information indicating whether or not interference occurs when crossing wherein the interference condition information storing step of storing the interference condition information, and separated Rumasu th managing step of the two-dimensional plane in a predetermined number of regions operate the space on projected onto a two-dimensional plane of the robot, the said area occupied by the sites, based on the information specifying the sites occupied, the occupation information recording step of occupation information is recorded that associates the occupation information and the interference condition information said area, Mutual Wherein the two or more different said portions to each other to grade contains and a determination step it is determined whether or not accompanied by the interference.
Also, the interference determination method of the present invention, the identification number assigning step that the identification number is assigned for each of the sites, and the interference information storing step of the interference information is stored, further comprising a, in the grid management step, the A memory area is secured for each area, and in the occupation information recording step, the area occupied by each part is specified, and the identification number of each part is recorded as the occupation information in a memory area corresponding to the area. May be.
The interference determination method of the present invention is an interference determination method for performing interference determination on each part of a robot operating in three dimensions and an obstacle, and has a plurality of digits and is connected to other identification numbers. When the logical product for each digit is calculated, an identification number set so that all digits are “0” is assigned to each part, and a moving part that is a part with movement among the parts, Information indicating “0” is stored as interference information when there is no interference at the time of a three-dimensional intersection with another part, and “1” is stored as interference information when there is an interference at a three-dimensional intersection, Interference condition information, which is set based on interference information, has a plurality of digits, and indicates whether interference occurs when the moving part intersects with the other part, It is stored for each moving part, and the space where the robot operates is 2D flat. In addition, the two-dimensional plane is divided into n1 × n2 cells and a memory area corresponding to each of the n1 × n2 cells is secured, and among the n1 × n2 cells, Corresponding cells that are cells corresponding to the respective parts are identified, and the occupancy information that is information based on the identification number of each part and has the plurality of digits is stored in the memory area corresponding to the corresponding cells. Recorded, the logical product for each digit in the occupation information and the interference condition information is obtained for each corresponding cell, and if the result of the logical product is “0” in all digits, the respective parts do not interfere with each other. If the result of the logical product is “1” in at least one digit, it may be determined that the parts interfere with each other.

  In such an interference determination apparatus and interference determination method of the present invention, the identification number, interference information, interference condition information, and occupancy information are used as essential information for determining interference between the parts. Among these, the interference information and the interference condition information are information based on a value when no interference occurs and a value when interference occurs when the moving part intersects with another part. Therefore, by using these interference information and interference condition information, the result of the interference determination according to the present invention takes into consideration the presence or absence of interference at the time of a three-dimensional intersection between the parts. Thereby, the accuracy of interference determination is improved.

  Further, in the present invention, the interference determination is performed after the space in which the robot operates is projected onto a two-dimensional plane, so that the interference determination process can be performed in a shorter time than, for example, three-dimensional interference determination. It becomes. Also, the interference information used in the present invention is information representing “0” or “1”, and since the data amount is small, the calculation amount and calculation time in the entire apparatus can be shortened.

  Further, since the identification number, interference condition information, and occupation information used in the present invention are information having the same number of digits as the number of each part of the robot and the obstacle, for example, the number increases when the number of parts increases. In addition, the interference condition in the present invention can be determined by setting the number of digits of the identification number, the interference condition information, and the occupancy information by the number of the increased parts after setting the interference conditions for the different parts. As described above, according to the present invention, when the number of parts increases, a flexible response is possible. In addition, since the calculation is performed with a small data amount of “0” or “1”, it is possible to prevent the calculation amount and calculation time of the entire apparatus from being extremely increased as the number of parts increases.

ただし、ｈは前記各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の部位の前記干渉条件情報であり、ＩＤｊはｊ番目の部位の前記識別番号であり、ｔｉｊはｉ番目の部位とｊ番目の部位における前記干渉情報である。 In the present invention, the interference condition information storage means may store information Ci calculated by the following formula (1) as the interference condition information.

Where h is the number of each part, i and j are natural numbers of 1 to h, Ci is the interference condition information of the i-th part, and IDj is the identification number of the j-th part. Yes, tij is the interference information in the i-th part and the j-th part.

  According to the present invention, a specific method for calculating interference condition information is provided.

  In the present invention, when the memory area is initialized, the occupation information recording means records the identification number as the occupation information in the memory area, and the memory area is not initialized. In this case, a logical sum for each digit between the existing occupation information and the identification number may be recorded in the memory area as the occupation information.

  According to the present invention, a specific method for recording occupation information is provided.

  Further, in the present invention, the movement start time and the movement end time in the movement of the robot are divided at regular time intervals, and the occupancy information is obtained with respect to the position of each part at each divided time. The recording means identifies the corresponding squares at each time, records the occupation information in the memory area corresponding to the corresponding squares, and the determination means is for each digit in the occupation information and the interference condition information. For each corresponding square, and if the result of the logical product is “0” in all the digits, it is determined that the parts do not interfere with each other, and the result of the logical product is in at least one digit. If it is “1”, it may be determined that the parts interfere with each other.

  According to the present invention, the interference determination is performed for each time between the operation start time and the operation end time in the operation of the robot. Thus, the robot can be operated safely after determining that no interference occurs at all times during operation.

  In the present invention, the robot may be a double-arm robot having a horizontal articulated arm.

  The present invention is particularly useful when the robot is a double-arm robot having a horizontal articulated arm.

  In the present invention, the identification number, the interference condition information, and the occupation information may be information having the same number of digits as the number of each part.

  In the present invention, since the identification number, the interference condition information, and the occupation information have the same number of digits as the number of each part of the robot and the obstacle, each information has the minimum necessary number of digits. Therefore, the calculation can be performed using the minimum necessary memory amount.

In addition, the present invention is an interference determination apparatus that performs interference determination on each part of a robot and an obstacle operating in three dimensions, and after projecting a space in which the robot operates on a two-dimensional plane, A cell management unit that divides the two-dimensional plane into a predetermined number of cells and secures a memory area corresponding to each of the predetermined cells, and corresponds to each part of the predetermined number of cells. A corresponding cell that is a cell to be identified is specified for each region, and occupancy information indicating whether each region occupies the corresponding cell is sequentially stored in the memory region corresponding to the corresponding cell. Occupation information recording means for recording, and when recording the occupancy information in the memory area, if the occupancy information of another part is already recorded in the memory area, it is determined that the parts interfere with each other Size Characterized in that it comprises a means.

  Such an interference determination apparatus of the present invention has a simple configuration in which only the grid management unit, the occupation information recording unit, and the determination unit are essential components, and such a configuration makes it possible to perform a simple interference determination process. It becomes possible. This simple interference determination process is particularly effective for robots and obstacles in which there are few interference determination target parts and only an operation on a two-dimensional plane (for example, an arm turning operation) needs to be considered. In other words, since interference is determined on a two-dimensional plane using squares, calculation can be performed with a very small amount of data compared to the case where interference is determined using a three-dimensional space or model, and interference determination is performed at high speed. This is particularly effective when considering only the two-dimensional movement of the robot (no movement of the lifting / lowering axis of the robot arm or up / down of the jig).

  ADVANTAGE OF THE INVENTION According to this invention, the interference determination apparatus and the interference determination method which can perform interference determination in a short time can be provided, considering the solid intersection without interference.

1 is a schematic configuration diagram of a double-arm robot system 100. FIG. 2 is a schematic side view of a double-arm robot 3. FIG. 3 is a schematic top view of a double-arm robot 3. FIG. 3 is a block diagram showing functional components of an interference determination unit 4. FIG. It is a figure which shows an example of identification number IDi which the identification number allocation part 421 allocated to each site | part of the double arm robot 3 and the obstruction 5. FIG. It is a figure which shows an example of the table which the interference information generation part 422 created. It is a figure which shows an example of the interference condition information produced | generated by the interference condition information generation part 423. It is a figure which shows an example of the interference condition information produced | generated by the interference condition information generation part 423. It is a figure which shows an example of the determination area | region 6 produced by the square management part 424. FIG. 3 is a flowchart showing main processing performed in the dual-arm robot system 100. It is a flowchart which shows the interference determination process by the interference determination part 4. FIG. It is a figure which shows an example of the angle table memorize | stored in the specification storage part 411 in step S205 of FIG. FIG. 12 is a diagram illustrating a specific example of processing from step S208 to step S210 at a certain time in the loop 1 of FIG. 11 (when i = 1). It is a figure which shows the specific example of the process of step S208 to step S210 at a certain time in the loop 1 of FIG. 11 (when i = 2). It is a figure which shows the specific example of the process of step S208 to step S210 at a certain time in the loop 1 of FIG. 11 (when i = 6). It is a figure for demonstrating the identification method of the occupation area by the occupation information recording part 425. FIG. It is a figure for demonstrating the identification method of the occupation area by the occupation information recording part 425 (procedure 1). It is a figure for demonstrating the identification method of the occupation area by the occupation information recording part 425 (procedure 2). It is a figure for demonstrating the identification method of the occupation area by the occupation information recording part 425 (procedure 3).

  DESCRIPTION OF EMBODIMENTS Hereinafter, preferred embodiments of an interference determination device and an interference determination method according to the present invention will be described in detail with reference to the accompanying drawings. In the description of the drawings, the same elements are denoted by the same reference numerals, and redundant description is omitted.

[Overall configuration of dual-arm robot system 100]
First, an overall configuration of a double-arm robot system 100 according to an embodiment of the present invention will be described with reference to FIGS. FIG. 1 is a schematic configuration diagram of a double-arm robot system 100. As shown in FIG. 1, the dual-arm robot system 100 includes a trajectory generator 1, servo drivers 2 a to 2 d, a dual-arm robot 3, and an interference determination unit 4. The communication means represented by the arrows are connected so as to communicate with each other. FIG. 2 is a schematic side view of the double-arm robot 3, and FIG. 3 is a schematic top view of the double-arm robot 3.

  For example, as shown in FIGS. 2 and 3, the double-arm robot 3 has two horizontal articulated arms 31 and 32 having elevating shafts 313 and 323 at their tips. The horizontal articulated arms 31 and 32 are arranged, for example, with the workpiece 5 sandwiched therebetween at intervals where at least a part of the work area overlaps like both human arms. Servo motors 3a to 3d are provided at the roots of the first axes 311 and 321 of each arm (shoulder portion of the double-arm robot 3) and the roots of the second axes 312 and 322 (elbow portions of the double-arm robot 3), respectively. Is provided. With this configuration, each axis can turn in the horizontal direction.

  The trajectory generator 1 shown in FIG. 1 calculates the movement amount and movement speed of each joint so that each part of the double-arm robot 3 can move from the current position to the target position, and sends a command to each servo driver. Part. The trajectory generation unit 1 includes a storage unit 11 and a calculation unit 12. The storage unit 11 controls the dimensions of the two-arm robot 3 necessary for trajectory generation, such as interpolation information and speed information regarding movement, and the robot operation sequence described by teaching or programming in a robot language. It is a part for storing the program and interference conditions necessary for interference determination. The calculation unit 12 is a part that calculates the movement amount and movement speed of each joint so that each part of the double-arm robot 3 can move from the current position to the target position. Before the movement of the double-arm robot 3, the calculation unit 12 of the trajectory generation unit 1 inquires of the interference determination unit 4 about the presence or absence of interference during and after movement when moving from the position before movement to the position after movement.

  The servo drivers 2 a to 2 d receive the command from the trajectory generator 1, respectively drive the servo motors 3 a to 3 d provided at each joint of the double-arm robot 3, and set the current position after driving to the trajectory generator 1. This is the part that feeds back.

[Configuration of Interference Determination Unit 4]
Hereinafter, the configuration of the interference determination unit 4 (corresponding to the “interference determination device” in the claims), which is a component of the dual-arm robot system 100, will be described in detail with further reference to FIGS. The interference determination unit 4 receives an inquiry from the trajectory generation unit 1 and applies each part (moving part) of the dual-arm robot 3 operating in three dimensions and each part (fixed part) of the workpiece or jig as an obstacle. This is the part that performs interference determination. FIG. 4 is a block diagram illustrating functional components of the interference determination unit 4. As shown in FIG. 4, the interference determination unit 4 includes a storage unit 41 and an interference determination calculation unit 42. In the following description, “moving part” refers to a part of the two-arm robot 3 and the obstacle 5 that accompanies movement, and “fixed part” refers to the double-arm robot 3 and the obstacle 5. Of these parts, a part that does not move is referred to, and “part” is a general term for a moving part and a fixed part.

  The storage unit 41 of the interference determination unit 4 includes a specification etc. storage unit 411, an interference information storage unit 412 (corresponding to “interference information storage means” in the claims), and an interference condition information storage unit 413 (claims). The “interference condition information storage means” in FIG. The specification storage unit 411 is a part that stores the specifications of the double-arm robot 3 received after power-on and the angles of the moving parts before and after the movement received for each inquiry from the trajectory generation unit 1. The interference information storage unit 412 is a part that stores interference information on the moving parts of the double-arm robot 3 and the obstacle 5. As will be described in detail later, the interference information is “0” when there is no interference when the moving part intersects with another part, and “1” when there is interference. Is information. The interference condition information storage unit 413 is a part that stores interference condition information for each moving part. As will be described later in detail, the interference condition information is set based on the interference information, has a plurality of digits equal to the number of each part of the double-arm robot 3 and the obstacle 5, and the moving part has another number. This is information indicating whether or not interference occurs when a three-dimensional intersection is made with a part.

  The interference determination calculation unit 42 of the interference determination unit 4 is a part that performs interference determination based on each piece of information stored in the storage unit 41. The interference determination calculation unit 42 performs interference determination with respect to the angles of the joints before and after the movement of the dual-arm robot 3 and with respect to the angles of the joints in the movement process divided every predetermined time. As shown in FIG. 4, the interference determination calculation unit 42 includes an identification number assigning unit 421 (corresponding to “identification number assigning means” in the claims), an interference information generating unit 422 (“interference information in the claims”). Storage condition ”), interference condition information generation unit 423 (corresponding to“ interference condition information storage means ”in the claims), and grid management unit 424 (corresponding to“ cell management means ”in the claims) And an occupancy information recording unit 425 (corresponding to “occupation information recording means” in claims) and a determination unit 426 (corresponding to “determination means” in claims). Hereinafter, each component which comprises the interference determination calculating part 42 is demonstrated in detail.

[Identification number assignment unit 421]
The identification number assigning unit 421 is a part that assigns an identification number to each part of the double-arm robot 3 and the obstacle 5. Each identification number has a plurality of digits equal to the number of each part of the double-arm robot 3 and the obstacle 5, and all the digits are calculated by calculating the logical product for every digit with all other identification numbers. Is set to be “0”. Specifically, FIG. 5 shows an example of the identification number IDi assigned to each part of the double-arm robot 3 and the obstacle 5 shown in FIG. 2 and FIG. In the example of FIG. 5, the identification number assigning unit 421 assigns “1000000” as the identification number ID1 to the first axis (first link) 311 of the arm 31. Further, the identification number assigning unit 421 assigns “0000010” as the identification number ID6 to the lifting shaft 323 of the arm 32. Further, the identification number assigning unit 421 assigns “0000001” as the identification number ID 7 to the jig 5. In the example given here, the number of digits of each identification number IDi is “7” because there are “seven” parts of the double-arm robot 3 and the obstacle 5. Further, when the logical product for each digit from ID1 to ID7 is calculated, it becomes “0” in all the digits.

[Interference Information Generation Unit 422]
The interference information generation unit 422 sets “1” when there is no solid intersection without interference between the moving parts of the double-arm robot 3 and other parts (that is, when interference occurs at the three-dimensional intersection). This is a part for creating a table in which the three-dimensional intersection without interference is possible (that is, when there is no interference at the time of three-dimensional intersection) or the relationship with itself is “0”.

  Specifically, an example of a table created by the interference information generation unit 422 is shown in FIG. In the example of FIG. 6, for example, the value of the interference information t12 indicating the presence or absence of interference in the case of i = 1 and j = 2 is “0”, which is the same as the first axis 311 of the arm 31 corresponding to i = 1. This indicates that there is a solid intersection with no interference with the second axis 312 of the arm 31 corresponding to j = 2, that is, no interference is involved in the solid intersection. Further, for example, the value of the interference information t43 indicating the presence or absence of interference in the case of i = 4 and j = 3 is “1”, which corresponds to the first axis 321 of the arm 32 corresponding to i = 4 and j = 3. This indicates that there is no solid crossing without interference between the corresponding lifting and lowering shafts 313 of the arm 31, that is, there is interference when a solid crossing occurs. Further, for example, the value of the interference information t55 indicating the presence or absence of interference in the case of i = 5 and j = 5 is “0”, which means that the second axis 322 of the arm 32 corresponding to i = 5 It indicates that there is no possibility of interfering with each other.

  The interference information generation unit 422 stores the table created in this way in the interference information storage unit 412 of the storage unit 41. The interference information generation unit 422 does not need to obtain the interference information t71, t72, etc. for the fixed part that is a part that does not move, that is, the jig 5 with the identification number ID7. This is because the presence or absence of interference with the jig 5 has already been taken into account in the interference information t17, t27, etc. of other moving parts. In this sense, since the value of the interference information tij and the value of the interference information tji are always the same, the example in FIG. 6 may be a table in which only one of tij and tji is clarified.

ただし、ｈは双腕ロボット３および障害物５の各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の移動部位の干渉条件情報であり、ＩＤｊはｊ番目の部位の識別番号であり、ｔｉｊはｉ番目の移動部位とｊ番目の部位における干渉情報である。 [Interference Condition Information Generation Unit 423]
The interference condition information generation unit 423 is a part that generates interference condition information. The interference condition information is set based on the interference information, has a plurality of digits equal to the number of each part of the double-arm robot 3 and the obstacle 5, and the moving part is three-dimensionally crossed with another part. This is information indicating whether or not there is interference. The interference condition information generation unit 423 generates information Ci calculated by the following mathematical formula (1) as interference condition information.

Here, h is the number of each part of the double arm robot 3 and the obstacle 5, i and j are natural numbers of 1 to h, Ci is interference condition information of the i-th moving part, and IDj is j This is the identification number of the th part, and tij is the interference information in the i th moving part and the j th part.

  Hereinafter, a method for generating interference condition information by the interference condition information generation unit 423 will be described in detail. First, as a simple example, consider a case where there are only two interference determination sites. That is, consider a case where interference determination is performed only on the right arm (ID1 = 01) and the left arm (ID2 = 10). FIG. 7 shows the interference condition information generated by the interference condition information generation unit 423 in this case. In the example of FIG. 7, interference information is set on the assumption that interference occurs when the right arm and the left arm are three-dimensionally crossed. That is, t12 = 1 and t21 = 1.

In the example of FIG. 7, when the interference condition information C1 is obtained for the right arm that is the part of the identification number ID1 with i = 1, according to the above equation (1),
C1 = ID1 · t11 + ID2 · t12 (2)
It is. In the example of FIG. 7, since ID1 = 01, t11 = 0, ID2 = 10, and t12 = 1, when the respective values are substituted into the above equation (2),
C1 = 0.01 + 10.1 = 10 (3)
Thus, the value of the interference condition information C1 of the right arm with the identification number ID1 is obtained as 10.

Next, as a more complicated example, consider a case where there are seven interference determination sites shown in FIGS. For convenience of explanation, the same identification number and interference information as in FIGS. 5 and 6 will be considered. FIG. 8 shows the interference condition information generated by the interference condition information generation unit 423 in this case. In this case, when obtaining the interference condition information C1 with respect to the first axis 311 of the arm 31 that is the part of the identification number ID1 of i = 1, according to the above equation (1),
C1 = ID1 · t11 + ID2 · t12 + ID3 · t13 + ID4 · t14 + ID5 · t15 + ID6 · t16 + ID7 · t17 (4)
It is. ID1 = 1000000, t11 = 0, ID2 = 000000, t12 = 0, ID3 = 0010000, t13 = 1, ID4 = 0001000, t14 = 1, ID5 = 0000100, t15 = 0, ID6 = 0000010, t16 = 1, Since ID7 = 0000001 and t17 = 0, if each value is substituted into the above equation (4),
C1 = 1000000 ・ 0
+0100000 ・ 0
+ 0010000 · 1
+0001000 ・ 1
+0000100.0
+0000010/1
+00000001
= 0011010 (5)
Thus, the value of the interference condition information C1 of the first axis 311 of the arm 31 with the identification number ID1 is obtained as 0011010.

  Similarly, interference condition information can be obtained for other parts, and the result is shown as values C2 to C6 in FIG. The interference condition information generation unit 423 stores the interference condition information generated in this way in the interference condition information storage unit 413 of the storage unit 41. The interference condition information generation unit 423 does not need to obtain the interference condition information for the jig 5 having the identification number ID7 that is a fixed part. This is because the presence or absence of interference with the jig 5 of the fixed part is already reflected in the values of C1 to C6 that are the interference condition information for the moving part.

[Cross section management unit 424]
The square management unit 424 projects a space in which the double-arm robot 3 operates onto a two-dimensional plane, creates a determination region that divides the two-dimensional plane into n1 × n2 squares, and also creates the n1 × This is a portion for securing a memory area corresponding to each cell of n2. FIG. 9 shows an example of the determination area 6 created by the grid management unit 424. The determination area 6 includes a range in which the dual-arm robot 3 operates based on the value of the specification storage unit 411 that stores specifications required to calculate the posture of the dual-arm robot 3 and the position of the obstacle 5. Or a part of the range in which the two-arm robot 3 operates, and is represented by a grid on an n1 × n2 two-dimensional plane obtained by projecting the space to be subjected to interference determination onto the two-dimensional plane. .

[Occupancy information recording unit 425]
The occupancy information recording unit 425 identifies the corresponding cell that is the cell corresponding to each part of the double-arm robot 3 and the obstacle 5 among the n1 × n2 cells by the cell management unit 424, and occupies the information Is recorded in a memory area corresponding to the corresponding cell. FIG. 9 shows an example of the corresponding grid specified by the occupation information recording unit 425. In the example of FIG. 9, a correspondence corresponding to a region (hereinafter, also referred to as “occupied region”) in which each part of the double-arm robot 3 and the obstacle 5 having the posture to be determined projected on the same plane exists. The cells are identified as indicated by reference numerals 311m, 312m, 313m, 321m, 322m, 323m, and 5m. Note that the method of specifying the corresponding cell will be described in detail in the section of “Occupied area specifying method” described later.

  The occupancy information recording unit 425 records the occupancy information in the memory area corresponding to the corresponding cell after specifying the corresponding cell. The occupation information is information based on the identification numbers of the parts of the double-arm robot 3 and the obstacle 5, and is information having a plurality of digits equal to the number of parts of the double-arm robot 3 and the obstacle 5. The occupation information recording unit 425 records, in the memory area, the identification number of the part that occupies the corresponding cell as the occupation information when the memory area to be recorded with the occupation information is initialized. On the other hand, the occupancy information recording unit 425, when the memory area to which occupancy information is to be recorded has not been initialized (when the occupancy information has already been recorded), A logical sum for each digit between the identification numbers of the occupied parts is recorded in the memory area as occupation information. By recording the occupation information by the occupation information recording unit 425, a bit corresponding to each part of the double-arm robot 3 and the obstacle 5 that occupies each square is set. Details of the operation by the occupation information recording unit 425 will be described later.

[Judgment Unit 426]
The determination unit 426 calculates a logical product for each digit in the occupation information and the interference condition information for each corresponding square, and performs interference determination based on the result of the logical product. If the result of the logical product is “0” in all digits, the determination unit 426 determines that the parts of the dual-arm robot 3 and the obstacle 5 do not interfere with each other. On the other hand, if the result of the logical product is “1” in at least one digit, the determination unit 426 determines that the parts of the dual-arm robot 3 and the obstacle 5 interfere with each other. The determination unit 426 outputs information indicating the interference determination result to the trajectory generation unit 1. When the determination unit 426 outputs the result of the interference determination, the determination unit 426 may output together information for specifying a site where the interference occurs.

[Operation of the dual-arm robot system 100]
Next, operations performed by the double-arm robot system 100 will be described with reference to FIGS. The following description is broadly divided into main processing in the dual-arm robot system 100 and interference determination processing by the interference determination unit 4.

[Main processing]
First, main processing in the dual-arm robot system 100 will be described with reference to FIG. FIG. 10 is a flowchart showing main processing performed in the dual-arm robot system 100.

  First, the calculation unit 12 of the trajectory generation unit 1 transmits the parameters stored in the storage unit 11 to the interference determination calculation unit 42 of the interference determination unit 4. This parameter is set for each target region for interference determination, and includes the above-described interference information tij (step S101).

  Next, the calculation unit 12 of the trajectory generation unit 1 reads the current angle of each part of the dual-arm robot 3 from each of the servo drivers 2a to 2d (step S102).

  Next, the calculation unit 12 of the trajectory generation unit 1 analyzes the operation program stored in the storage unit 11, and obtains the movement destination angle (target angle) of each part of the dual-arm robot 3 (step S103).

  Next, the calculation unit 12 of the trajectory generation unit 1 transmits the current angle and the target angle of each part of the double-arm robot 3 to the interference determination calculation unit 42 of the interference determination unit 4 (step S104).

  Next, the calculation unit 12 of the trajectory generation unit 1 waits for a determination result signal from the interference determination calculation unit 42 of the interference determination unit 4 (step S105). During this time, the interference determination unit 4 performs an interference determination process, which will be described in detail later, and determines whether or not there is interference between the parts during the entire movement of the arms 31 and 32 of the dual-arm robot 3. When the determination result signal from the interference determination calculation unit 42 of the interference determination unit 4 is “no interference” (step S106: YES), the calculation unit 12 of the trajectory generation unit 1 displays “ “No interference” is displayed (step S107), the operation program stored in the storage unit 11 is executed, and the operation control of the dual-arm robot 3 is performed (step S108).

  On the other hand, when the determination result signal from the interference determination calculation unit 42 of the interference determination unit 4 is “with interference” (step S106: NO), the calculation unit 12 of the trajectory generation unit 1 displays a display unit (not shown). ) Is displayed (step S109), and the process is terminated. As described above, the trajectory generation unit 1 waits without sending a command to the servo drivers 2a to 2d until interference is generated until a determination result is obtained that does not cause interference between parts throughout the movement of the dual-arm robot 3. The movement of each moving part of the double-arm robot 3 is not started.

[Interference judgment processing]
Next, interference determination processing by the interference determination unit 4 will be described with reference to FIGS. 11 and 12. FIG. 11 is a flowchart showing interference determination processing by the interference determination unit 4, and FIG. 12 is a diagram referred to in the description of step S205.

  First, the identification number assigning unit 421 assigns an identification number IDi (i = 1 to h) for each target site for interference determination (see step S201, FIG. 5).

  Next, the interference condition information generation unit 423 is described in the identification number IDi assigned in step S201 and the table created by the interference information generation unit 422 and stored in the interference information storage unit 412 (see FIG. 6). Based on the interference information tij, the interference condition information Ci is generated according to the above-described equation (1) (see step S202, FIG. 7 and FIG. 8).

  Next, the grid management unit 424 secures a memory area corresponding to the determination area (n1 × n2 grid area, see FIG. 9) (step S203).

  Next, the interference determination calculation unit 42 waits for the current angle and the target angle of each part of the dual arm robot 3 to be input from the trajectory generation unit 1 in step S104 (see FIG. 10) (step S204).

  When the current angle and target angle of each part of the dual arm robot 3 are input from the trajectory generation unit 1 (step S204: YES), the interference determination calculation unit 42 determines the target angle of each part of the dual arm robot 3 and the target angle. The angle of each part at a constant time interval is obtained from the current angle difference Δθ and the speed parameter of each part, and this is stored in the angle table of the specification storage unit 411. Although not shown, the interference determination calculation unit 42 may include a separate processing unit for this processing. FIG. 12 shows an example of an angle table stored in the specification storage unit 411. As shown in FIG. 12, the angle table includes a field for each part in a record (record number: m) at regular time intervals (step S205).

  Thereafter, loop 1 from step S206 to step S213 is started. After step S205, the square management unit 424 sets the memory area corresponding to the determination area (n1 × n2 square area, see FIG. 9) to 0 as the initial value of the occupancy information. Initialization is performed (step S206).

  Next, the occupation information recording unit 425 reads the angle data of the k-th (k = 1 to m) record from the angle table illustrated in FIG. 12 (step S207).

  Thereafter, loop 2 from step S208 to step S210 is started. After step S207, the occupation information recording unit 425 uses the angle data and the dimension parameters of the interference determination target part i, and the cells occupied by the interference determination target part i in the determination region (two-dimensional plane), that is, the corresponding cells. Is identified. Note that the process of specifying the corresponding cell will be described in detail in the section of “Occupied area specifying method” described later. Further, the occupation information recording unit 425 acquires the occupation information recorded in the memory area corresponding to the identified corresponding cell (step S208).

  Next, the occupancy information recording unit 425 writes the identification number IDi as occupancy information in the memory area where the existing occupancy information is 0 in the memory area occupied by the interference determination target site i, and the existing occupancy information is 0. In other memory areas, the logical sum of the existing occupancy information and the identification number IDi is written as occupancy information (step S209).

  Next, the determination unit 426 obtains a logical product with the interference condition Ci for all the memory areas in which the occupation information is written in the process of the previous step S209 (step S210).

  The processing from step S208 to step S210 is repeated until i = h, that is, all the target portions for interference determination. Thereby, the loop 2 is completed, and the interference determination with respect to the posture of the double-arm robot 3 at a certain time point k is completed. Note that a specific example of the processing of the loop 2 will be described later in the section “Specific Example of Processing of the Loop 2” below.

  When the loop 2 ends, the determination unit 426 determines whether or not each logical product obtained in step S210 is 0 in all the digits (step S211). If it is determined in step S211 that all logical products are 0 in all digits (step S211: YES), the determination unit 426 stores information indicating “no interference” in the storage unit 41 (step S212). On the other hand, if it is determined in step S211 that any one of the logical products is not 0 in all digits (step S211: NO), the determination unit 426 stores information indicating “with interference” in the storage unit 41. (Step S213). The storage unit 41 may further include a separate storage unit for the storage process, and the specification storage unit 411 may perform the storage process.

  The processing from step S206 to step S213 is repeated until k = m, that is, the target angle of each part. Thereby, the loop 1 is finished, and the interference determination process in all postures until each arm of the double-arm robot 3 shifts to the target posture is finished.

  When the loop 1 is completed, the determination unit 426 obtains a final determination result from the determination result information stored in the storage unit 41 in step S212 or step S213 for each predetermined time interval, and uses this as a determination result signal. It is sent to the trajectory generator 1. The determination result is “no interference” if all of the determination result information at regular time intervals has no interference and “interference” if there is even one interference (step S214). By such interference determination processing, the presence or absence of interference between parts is confirmed for each posture at regular time intervals while the arms 31 and 32 of the double-arm robot 3 operate from the initial posture to the target posture. Can do.

[Specific example of loop 2 processing]
Subsequently, a specific example of the processing of the loop 2 will be described with reference to FIGS. 13 to 15 show specific examples of processing from step S208 to step S210 at a certain time in the loop 1 (for example, the k-th time). FIG. 13 shows a specific example of processing when i = 1, FIG. 14 shows a specific example of processing when i = 2, and FIG. 15 shows a specific example of processing when i = 6.

In the case of i = 1 shown in FIG. 13, the processing from step S <b> 208 to step S <b> 210 is performed on the first axis 311 of the arm 31. That is, first, the occupation information recording unit 425 specifies the occupation area of the first axis 311 in the determination area 6 (the grid corresponding to the area H1 marked with dots in FIG. 13) (step S208). Next, the occupation information recording unit 425 writes the occupation information to the memory area corresponding to the corresponding cell identified in step S208. At this time, since all the memory areas are in the state initialized in step S206 (the state in which “0” is written as the initial value of the occupation information), there is no memory area corresponding to the square of the area H1. The identification number ID1 = “1000000” of the first axis 311 is written (step S209). Next, the occupation information recording unit 425 obtains a logical product of each of the occupation information written in the previous step S209 and the interference condition C1 (step S210). That is, the determination unit 426 calculates a logical product represented by the following equation (6).
Logical product related to squares in region H1 =
1 million
× 0011010
= 0000000 (6)

In the case of i = 2 shown in FIG. 14, the processing from step S <b> 208 to step S <b> 210 is performed on the second axis 312 of the arm 31. That is, first, the occupation information recording unit 425 specifies the occupation area of the second axis 312 in the determination area 6 (the grid corresponding to the area H2 with dots and the area H3 with hatches in FIG. 14) ( Step S208). The region H2 is a region where only the second axis 312 exists, and the region H3 is a region where the second axis 312 and the first axis 311 overlap. Next, the occupation information recording unit 425 writes the occupation information to the memory area corresponding to the corresponding cell identified in step S208. The identification number ID2 = “0100000” of the second axis 312 is written in the memory area corresponding to the square of the area H2 with the memory area being initialized. On the other hand, the existing occupation information “1000000” and the identification number ID2 of the second axis 312 are included in the memory area corresponding to the square of the area H3 that has not been initialized by writing with i = 1. A logical sum of “0100000” is written as new occupation information. That is, the occupancy information recording unit 425 records the occupancy information represented by the following formula (7) in a memory area corresponding to the square of the area H3.
Occupancy information written in the memory area corresponding to the square of the area H3 =
1 million
+0100000
= 1100000 (7)

Next, the determination unit 426 calculates the logical product of the occupancy information and the interference condition C2 represented by the following equations (8) and (9) for each cell corresponding to each of the region H2 and the region H3. Ask for.
Logical product related to squares in region H2 =
0100000
× 0000110
= 0000000 (8)
Logical product related to the grid of area H3 =
1100000
× 0000110
= 0000000 (9)

  As described above, the process proceeds for each part to be determined, i = 3, elevating shaft 313, i = 4, first axis 321, i = 5, second axis 322, and i shown in FIG. In the case of = 6, the process from step S208 to step S210 is performed on the lifting shaft 323 of the arm 32. That is, first, the occupancy information recording unit 425 displays the blocks corresponding to the occupying area of the lifting shaft 323 in the determination area 6 (the area H4 with dots in FIG. 15, the area H5 with hatching, the area H6, and the area H7). Eye) is specified (step S208). Region H4 is a region where only the lifting shaft 323 exists, region H5 is a region where the lifting shaft 323 and the second shaft 322 of the arm 32 overlap, and region H6 overlaps the lifting shaft 323 and the second shaft 312 of the arm 31. The region H7 is a region where the elevating shaft 323 and the first shaft 311 and the second shaft 312 of the arm 31 overlap.

Next, the occupation information recording unit 425 writes the occupation information to the memory area corresponding to the corresponding cell identified in step S208. The identification number ID6 = “0000010” of the lifting shaft 323 is written in the memory area corresponding to the square of the area H4 where the memory is initialized. On the other hand, the identification number of the second axis 322 of the arm 32 as the existing occupancy information is stored in the memory area corresponding to the corresponding cell of the area H5 where the memory is not initialized by writing at i = 5. A logical sum of ID5 = “0000100” and identification number ID6 = “0000010” of the lifting shaft 323 is written as new occupation information. That is, the occupancy information recording unit 425 records the occupancy information represented by the following formula (10) in a memory area corresponding to the square of the area H5.
Occupancy information written in the memory area corresponding to the square of the area H5 =
0000100
+0000010
= 0000110 (10)

Further, the identification number ID2 of the second axis 321 of the arm 1 as the existing occupancy information is stored in the memory area corresponding to the square of the area H6 in which the memory is not initialized by writing at i = 2. = “0100000” and the logical sum of the identification number ID6 = “0000010” of the lifting shaft 323 are written as new occupation information. That is, the occupation information recording unit 425 records the occupation information represented by the following formula (11) in the memory area corresponding to the square of the area H6.
0100000
+0000010
= 0100010 (11)

Furthermore, the first axis 311 of the arm 1 as the existing occupancy information is stored in the memory area corresponding to the square of the area H7 where the memory is not initialized by writing with i = 1 and i = 2. The logical sum of the identification number ID1 = “1000000”, the identification number ID2 = “0100000” of the second axis 312 and the identification number ID6 = “0000010” of the lifting shaft 323 is written as new occupation information. That is, the occupancy information recording unit 425 records the occupancy information represented by the following formula (12) in a memory area corresponding to the square of the area H6.
1 million
+0100000
+0000010
= 1100010 (12)

Next, the determination unit 426 calculates the logical product of the occupation information indicated by the following equations (13) to (16) and the interference condition C6 for each cell corresponding to each of the regions H4 to H7. Ask for.
Logical product related to squares in region H4 =
0000010
× 1111001
= 0000000 (13)
Logical product related to square in area H5 =
0000110
× 1111001
= 0000000 (14)
Logical product related to grid in area H6 =
0100010
× 1111001
= 0100000 (15)
Logical product related to square in area H7 =
1100010
× 1111001
= 1100000 (16)

  The specific example of the processing of the loop 2 has been described above, but when the loop 2 ends in this way (when the logical product of the occupation information and the interference condition is obtained for all the determination target parts), the determination unit 426 It is determined whether or not each logical product is 0 (step S211). In this case, the determination unit 426 determines that interference does not occur if the result of the logical product is 0 in all the digits as in the case of i = 1 and i = 2 in the specific example described above. In fact, when i = 1 in the specific example, the first axis 311 cannot interfere with itself. In addition, when i = 2, the first axis 311 and the second axis 312 are connected by a joint and are three-dimensionally crossed and cannot interfere with each other. On the other hand, when the result of the logical product is not 0 (at least one digit is 1) as in the case of i = 6 in the above specific example, it is determined that interference occurs. The place where the interference occurs is the part of the identification number having the same digit as the digit having the value “1” and the value “1” in the equations (15) and (16). That is, in the above formula (15), the digit having the value “1” is the second digit from the left, and the part of the identification number having the value “1” in this digit is the second axis 312 ( Identification number “0100000”). Therefore, it can be seen that the lifting shaft 323 interferes with the second shaft 312 of the arm 31. Further, in the equation (16), the digit having the value “1” is the first and second digits from the left, and the part of the identification number having the value “1” in these digits is the first of the arm 31. The axis 311 (identification number “100000”) and the second axis 312 (identification number “0100000”). Therefore, it can be seen that the lifting shaft 323 causes interference also at the position where the first shaft 311 and the second shaft 312 of the arm 31 overlap, that is, at the right elbow.

  In FIG. 13 to FIG. 15, for convenience of explanation, the grid (memory area) is set large. However, the actual grid is set to be smaller than the examples of FIGS.

[Occupied area identification method]
As described above, the occupancy information recording unit 425 includes each of the two-arm robot 3 and the obstacle 5 at a certain point in time among the n1 × n2 cells (determination region, see FIG. 9) created by the cell management unit 424. Corresponding cells corresponding to the region where the part exists (occupied region) are specified. Hereinafter, a method for specifying the occupied area by the occupied information recording unit 425 will be described in detail with reference to FIGS. 16 to 19 and specific examples.

  FIG. 16 is a diagram for explaining a method of specifying the occupied area by the occupied information recording unit 425. The example shown in FIG. 16 is an example of specifying a region where the arm CC having the joint AA and the joint BB before and after exists on the XY plane (the occupied region of the arm CC, the region K displayed in gray in FIG. 16). is there. Further, the example shown in FIG. 16 can be understood as an example for specifying the corresponding square with respect to the first axis 311 of the arm 31 described above, for example, and the first axis 311 in this case is shown in FIG. This corresponds to the arm CC shown in FIG. In this case, the first shaft 311 has, for example, an oval shape. Further, in the actual XY plane, a plane area in which all of the n1 × n2 grids related to the determination area shown in FIG. 9 can be secured, but in FIG. 16, only the arm CC portion is enlarged for convenience of explanation. As shown. As the specifying method, the following steps 1 to 3 are performed on the points (four vertices constituting the grid) existing on the XY plane of FIG. 16, and each point is included in the occupied area of the arm CC. By determining whether or not the arm CC is occupied, the occupied area of the arm CC on the XY plane can be specified.

In this example, the center coordinates of the joint AA, the center coordinates of the joint BB, and the width of the arm CC are known information. These pieces of known information are stored in the storage unit 11 of the trajectory generation unit 1 and transmitted to the interference determination calculation unit 42. In FIG. 16, the center coordinate of the joint AA is the point A (x ₁ , y ₁ ), the center coordinate of the joint BB is the point B (x ₂ , y ₂ ), and the width of the arm CC is w. The radius R of the joint AA and the joint BB is R = w / 2. That means
Point A (x ₁ , y ₁ ): Center coordinates of joint AA Point B (x ₂ , y ₂ ): Center coordinates of joint BB w: Arm width (R = w / 2)
It is. Further, in this example, the point D is a point to be determined as to whether or not it is in the occupied area of the arm CC. That means
Point D (x ₀ , y ₀ ): Any vertex of any square. The definition of each auxiliary line and auxiliary point is as follows.
l ₁ : Straight line passing through points A and B ₁₁ 1: Tangent line (lower side) of a circle approximating joint AA and joint BB (circle R having a center on points A and B)
l ₁₂ : Tangent line (upper side) of a circle approximating the joint AA and the joint BB (circle R having a center at the points A and B)
l _2: through the point D, the straight line intersecting perpendicularly with the _{l 1} (i.e. _{l 1} and _{l 2} vertical relationship)
Point C (x ₃ , y ₃ ): intersection of l ₁ and l ₂ Point A _i , B _i : Point of contact with tangent l _1i of circle with radius R centered on points A and B Point E ′: From point A A point where the Y coordinate is the same as A ₁ when a perpendicular is drawn on the X axis Point B ′: A point where it intersects with l ₁ when a perpendicular is drawn from the point D to the X axis The radius of the joints AA and BB is The same. Therefore, l ₁ , l ₁₁ , and l ₁₂ are parallel to each other. Further, a range (region K) surrounded by the approximate circles and the line segments l ₁₁ and l ₁₂ of the joint AA and the joint BB corresponds to the arm CC.

(Procedure 1)
First, according to the rule shown in FIG. 17, the coordinate system where the determination target part (arm CC) is located is specified, and the grid area that may be occupied is limited. The coordinates (x ₀ , y ₀ ) of point D are
_{(X min <x 0 <x} max) ... (17)
And (y _min <y ₀ <y _max ) (18)
If the condition is satisfied, the point D may be included in the occupied region of the determination target part. The squares outside this range are included in a circular portion described later in the procedure 3 below, or are not included in the occupied region of the determination target part.

となる。 Here, x _12w and y _12w are based on the similarity between the triangle ABE and the triangle AA ₁ E ′,
L _ab : a ₁₂ : b ₁₂ = R: y _12w : x _{12w
(However, a 12 = y 1 -y 2} , b 12 = x 1 -x 2)
Therefore, taking into account the direction,
_{x 12w = -R b 12 / L} ab
y _12w = -R a ₁₂ / _Lab
As given. L _ab is from the three-square theorem,

It becomes.

(Procedure 2)
Next, the points D of the cells satisfying the above equations (17) and (18), that is, (x _min <x ₀ <x _max ) and (y _min <y ₀ <y _max ) are shown in FIG. It is determined whether or not the region K1 displayed in gray is entered. This includes whether or not the length L _cd of the perpendicular from point D to line segment l ₁ is less than or equal to R, ie
L _cd ≦ R (19)
What is necessary is just to determine whether it becomes. If this is established, the grid having the point D is included in the occupied area.

である。 Here, L _cd can be obtained from a known condition value given from parameters stored in the storage unit 11 of the trajectory generation unit 1 as follows. First, the equation of the line segment l ₁ is expressed as follows, where the slope is a _{1 and the} intercept is b ₁ .
a ₁ x + b ₁ = y
It is. Accordingly, the coordinate of the point B ′ in FIG. 18 is (x ₀ , a ₁ x ₀ + b ₁ ) because the X coordinate is x ₀ which is the same as the point D. Therefore, the distance L _{db ′} between the two points DB _′ is
L _{db ′} = y ₀ − (a ₁ x ₀ + b ₁ )
It is. Here, since the triangle ABE and the triangle DB′C are similar,
L _{db ′} : L _cd = L _ab : (x ₂ −x ₁ )
Holds. Solving for the distance L _cd between the two points CD,
L _cd = (x ₂ −x ₁ ) L _{db ′} / L _ab
= (X ₂ -x ₁ ) {y _0- (a ₁ x ₀ + b ₁ )} / _Lab
= − (X ₁ −x ₂ ) [y ₀ − {(y ₁ −y ₂ ) / (x ₁ −x ₂ )} x ₀ − (y ₂ x ₁ −x ₂ y ₁ ) / (x ₁ −x ₂ )] / _{Lab
= {(Y 1 - y 2} ) x 0 - (x 1 - x 2) y 0 + (y 2 x 1 - x 2 y 1)} / L ab
However, as above

It is.

(Procedure 3)
Finally, it is determined whether or not the point D enters the circular area K2 displayed in gray in FIG. The equation for a circle of radius R centered at coordinates (x _i , y _i ) is
(X−x _i ) ² + (y−y _i ) ² = R ²
Therefore, the inside including the boundary of the circle is
(X−x _i ) ² + (y−y _i ) ² ≦ R ²
Can be expressed as Here, if two circles of the joint AA and the joint BB are considered with respect to the point D (x ₀ , y ₀ ),
(X ₀ −x ₁ ) ² + (y ₀ −y ₁ ) ² ≦ R ² (20)
(X ₀ −x ₂ ) ² + (y ₀ −y ₂ ) ² ≦ R ² (21)
If any of the above holds, the square having the point D (x ₀ , y ₀ ) is included in the occupied area.

(Summary)
As described above, it is determined whether or not the four vertex coordinates of each square existing in the region specified in the procedure 1 above meet the condition of the procedure 2 (equation (19)), and all It is determined whether the four vertex coordinates of the square correspond to the condition of the procedure 3 (Formula (20) or Formula (21)). If any vertex meets any condition, it is determined that the square is included in the occupied area of the arm CC, that is, the determination target part i.

[Operation and effect of this embodiment]
Next, operations and effects of the dual arm robot system 100 according to the present embodiment will be described. In the two-arm robot system 100 of the present embodiment, the identification number, interference information, interference condition information, and occupancy information are used as essential information for determining interference between the parts. Among these, the interference information and the interference condition information are information based on a value when no interference occurs and a value when interference occurs when the moving part intersects with another part. Accordingly, by using these interference information and interference condition information, the result of the interference determination according to the present embodiment takes into consideration the presence or absence of interference at the time of a three-dimensional intersection between the parts. Thereby, the accuracy of interference determination is improved.

  Further, in the present embodiment, the interference determination is performed after projecting the space in which the double-arm robot 3 operates on a two-dimensional plane, so that the interference determination process is performed in a shorter time than, for example, three-dimensional interference determination. Can be done. Also, the interference information used in the present embodiment is information representing “0” or “1”, and since the data amount is small, the calculation amount and calculation time in the entire apparatus can be shortened.

  Moreover, since the identification number, interference condition information, and occupation information used in the present embodiment are information having the same number of digits as the number of each part of the double-arm robot 3 and the obstacle 5, for example, the number of parts has increased. In this case, after setting the interference condition regarding the increased part, the number of digits of the identification number, the interference condition information, and the occupation information can be increased by the number of the increased part, and the interference determination in this embodiment can be performed. . Thus, according to this embodiment, when the number of parts increases, a flexible response is possible. In addition, since the calculation is performed with a small data amount of “0” or “1”, according to the present embodiment, as the number of parts increases, the calculation amount and the calculation time of the entire apparatus become extremely large. It can be prevented from increasing.

  In the present embodiment, the identification number, the interference condition information, and the occupation information have the same number of digits as the number of each part of the double-arm robot 3 and the obstacle 5. In this case, there is no waste in the calculation, and the calculation can be performed using the minimum amount of memory.

  Further, according to the present embodiment, the interference determination is performed for each time between the motion start time and the motion end time in the motion of the double-arm robot 3. Thereby, the dual-arm robot 3 can be operated safely after determining that no interference occurs at all times during operation.

  The trajectory generation unit 1 and the interference determination unit 4 described above can also be used as a simulation device that performs interference determination in advance before actually operating the dual-arm robot 3. That is, even if the dual-arm robot 3 and the servo drivers 2a to 2d shown in FIG. 1 do not actually exist, these mechanical parts are configured on a personal computer, and the dual-arm robot 3 and each part of the obstacle are configured. It is possible to simulate the interference determination between the two.

[Other Embodiments]
As mentioned above, although preferred embodiment of this invention was described, it cannot be overemphasized that this invention is not limited to the said embodiment. For example, although not shown, the interference determination calculation unit 42 may have a simple configuration in which only the grid management unit 424, the occupation information recording unit 425, and the determination unit 426 are essential components.

  In this case, as described above, the grid management unit 424 projects a space in which the dual-arm robot 3 operates onto a two-dimensional plane, and then determines a determination area obtained by dividing the two-dimensional plane into n1 × n2 grids. A memory area corresponding to each of the n1 × n2 cells is secured while being created. In addition, the occupation information recording unit 425 specifies corresponding squares corresponding to the parts of the double-arm robot 3 and the obstacle 5 among the n1 × n2 squares by the square management unit 424, Occupancy information is sequentially recorded in the memory area corresponding to the corresponding cell for each part. Here, the occupation information in this case is information indicating whether each part of the double-arm robot 3 and the obstacle 5 occupies the corresponding square. In addition, when the occupation information is recorded in the memory area, the determination unit 426 determines that these parts interfere with each other if the occupation information of another part is already recorded in the memory area.

  With the above configuration, a simple interference determination process as described below is possible. For convenience of explanation, consider a case where there are only two interference determination sites. For example, the case where the interference determination of only the right arm (identification number ID = 01) and the left arm (identification number ID = 10) is performed is given as an example. In this case, first, the grid management unit 424 projects a motion space of the dual-arm robot 3 onto a two-dimensional plane, and then creates a determination region that divides the two-dimensional plane into n1 × n2 grids. In addition, a memory area corresponding to each of the n1 × n2 cells is secured. Next, the occupancy information recording unit 425 identifies the cell occupied by the part with ID = 01, and writes “information indicating ID = 01” as the occupancy information in the memory area. Next, the occupancy information recording unit 425 identifies the cell occupied by the part with ID = 10, and writes “information indicating ID = 10” as the occupancy information in the memory area. During the writing process of “information indicating ID = 10”, the occupation information recording unit 425 has already written “information indicating ID = 01” and “information indicating ID = 10” in the memory area. When it is necessary to write the information, the determination unit 426 determines that the parts “interfere” between the part with ID = 01 and the part with ID = 10. On the other hand, when “information indicating ID = 01” is not written in any of the memory areas to which “information indicating ID = 10” is written, it is determined that the parts do not interfere with each other. .

  The simple interference determination process as described above is particularly effective for robots and obstacles in which there are few interference determination target portions and only an operation on a two-dimensional plane (for example, an arm turning operation) needs to be considered. In other words, since interference is determined on a two-dimensional plane using squares, calculation can be performed with a very small amount of data compared to the case where interference is determined using a three-dimensional space or model, and interference determination is performed at high speed. This is particularly effective when considering only the two-dimensional movement of the dual-arm robot 3 (no movement of the lifting / lowering axis of the robot arm or raising / lowering of the jig).

  DESCRIPTION OF SYMBOLS 100 ... Double-arm robot system, 1 ... Trajectory generation part, 11 ... Memory | storage part, 12 ... Calculation part, 2a-2d ... Servo driver, 3a-3d ... Servo motor, 3 ... Double-arm robot, 31, 32 ... Horizontal articulated Arm 311, first axis of arm 31, 312 ... second axis of arm 31, 313 ... lifting axis of arm 31, 321 ... first axis of arm 32, 322 ... second axis of arm 32, 323 ... arm 32 4... Interference determination unit, 41... Storage unit, 411... Specification storage unit, 412... Interference information storage unit, 413 ... interference condition information storage unit, 42. 422 ... Interference condition information generation unit, 424 ... Grid management unit, 425 ... Occupancy information recording unit, 426 ... Judgment unit, 5 ... Obstacle, work, jig, 6 ... Determination region.

Claims (11)

An interference determination apparatus that performs interference determination on each part of a robot and an obstacle that operates in three dimensions,
Interference condition information indicating whether or not there is interference when three-dimensionally crossing each other part for each moving part that is a part that involves movement among the parts, and when the three-dimensionally intersecting Interference condition information storage means for storing the interference condition information set based on interference information indicating whether or not interference is involved;
And separated Rumasu th management means to the two-dimensional plane a predetermined number of regions operate the space on projected onto a two-dimensional plane of the robot,
Said area occupied by said portion, and information for specifying a region occupying the area, and the occupation information recording means for recording the occupation information that associates,
Determining means for determining whether or not two or more different parts that are three-dimensionally crossed with each other are based on the occupation information and the interference condition information;
An interference determination apparatus comprising: An identification number assigning means for assigning an identification number for each part;
Interference information storing means for storing the interference information ;
Further comprising a,
The grid management means secures a memory area for each area,
The occupation information recording means records the identification number of each part as the occupation information in a memory area corresponding to an area occupied by each part.
The interference determination apparatus according to claim 1. The identification number assigning means has an identification number that has a plurality of digits and is set so that all digits are "0" when calculating the logical product for each digit with other identification numbers. Assigned to each part,
The interference information storage means is “0” when the moving part does not cause the interference when the three-dimensional intersection with the other part is performed, and “0” when the movement part causes the interference when the three-dimensional intersection occurs. 1 ”is stored as the interference information,
The interference condition information storage means, the set based on the interference information, and storing the interference condition information chromatic said number plurality of digits,
The grid management means divides the two-dimensional plane into n1 × n2 grids, and secures memory areas corresponding to the n1 × n2 grids,
The occupancy information recording means specifies a corresponding cell that is a cell corresponding to each of the n1 × n2 cells, and is information based on the identification number of each of the plurality of digits. Is recorded in the memory area corresponding to the corresponding cell,
The determination means obtains a logical product for each digit in the occupancy information and the interference condition information for each corresponding square, and if the result of the logical product is “0” in all digits, the respective parts interfere with each other. If the result of the logical product is “1” in at least one digit, it is determined that the parts interfere with each other.
The interference determination apparatus according to claim 2. The interference condition information storage means stores information Ci calculated by the following mathematical formula (1) as the interference condition information.
The interference determination apparatus according to claim 3.

Where h is the number of each part, i and j are natural numbers of 1 to h, Ci is the interference condition information of the i-th part, and IDj is the identification number of the j-th part. Yes, tij is the interference information in the i-th part and the j-th part. The occupation information recording means is
If the memory area is initialized, the identification number is recorded in the memory area as the occupation information,
If the memory area is not initialized, record a logical OR for each digit between the existing occupation information and the identification number as the occupation information in the memory area.
The interference determination apparatus according to claim 3 or 4, wherein Between the operation start time and the operation end time in the operation of the robot is divided at regular time intervals, with respect to the position of each part at each of the divided times,
The occupancy information recording means identifies the corresponding cell at each time, records the occupancy information in the memory area corresponding to the corresponding cell,
The determination means obtains a logical product for each digit in the occupancy information and the interference condition information for each corresponding square, and if the result of the logical product is “0” in all digits, the respective parts interfere with each other. If the result of the logical product is “1” in at least one digit, it is determined that the parts interfere with each other.
The interference determination apparatus according to any one of claims 3 to 5, wherein: The identification number, the interference condition information, and the occupation information are information having the same number of digits as the number of each part.
The interference determination apparatus according to any one of claims 3 to 6, wherein The robot is a double-arm robot having a horizontal articulated arm.
The interference determination apparatus according to any one of claims 1 to 7, wherein: An interference determination method for performing interference determination on each part of a robot and an obstacle operating in three dimensions,
Interference condition information indicating whether or not there is interference when three-dimensionally crossing each other part for each moving part that is a part that involves movement among the parts, and when the three-dimensionally intersecting An interference condition information storing step for storing the interference condition information set based on interference information indicating whether or not interference is involved;
And separated Rumasu th managing step of the two-dimensional plane in a predetermined number of regions on the projection of the space for movement of the robot on a two-dimensional plane,
Said area occupied by said portion, and information for specifying a region occupying the area, and the occupation information recording step of occupation information is recorded that associates,
A determination step of determining whether or not two or more different parts that are three-dimensionally crossed with each other are based on the occupation information and the interference condition information;
The interference determination method characterized by including. An identification number assigning step in which an identification number is assigned to each part;
An interference information storing step in which the interference information is stored ;
Further comprising a,
In the grid management step, a memory area is secured for each area,
In the occupation information recording step, the area occupied by each part is specified, and the identification number of each part is recorded as the occupation information in a memory area corresponding to the area.
The interference determination method according to claim 9 . In the identification number assigning step, an identification number having a plurality of digits and set so that all the digits become “0” when calculating the logical product for each digit with other identification numbers, Assigned to each part,
In the interference information storing step, the moving part is “0” when there is no interference when three-dimensionally intersecting with the other part, and “1” when there is interference when three-dimensionally intersecting. Is stored as interference information,
In the interference condition information storing step, it is set based on the interference information, has the plurality of digits, and represents whether or not interference occurs when the moving part intersects with the other part. Interference condition information that is information is stored for each moving part,
In the grid management step, the space in which the robot operates is projected onto a two-dimensional plane, and then the two-dimensional plane is divided into n1 × n2 grids, and each of the n1 × n2 grids is divided. The corresponding memory area is reserved,
In the occupation information recording step, a corresponding cell that is a cell corresponding to each part of the n1 × n2 cells is specified, and the information is based on the identification number of each part, and the plurality of digits Is recorded in the memory area corresponding to the corresponding cell,
In the determining step, a logical product for each digit in the occupancy information and the interference condition information is obtained for each corresponding cell, and if the result of the logical product is “0” in all digits, the respective parts are It is determined that there is no interference, and if the result of the logical product is “1” in at least one digit, it is determined that the parts interfere with each other.
The interference determination method according to claim 10 .

Images