WO2022107683A1 - 自己干渉判定装置 - Google Patents

自己干渉判定装置 Download PDF

Info

Publication number
WO2022107683A1
WO2022107683A1 PCT/JP2021/041614 JP2021041614W WO2022107683A1 WO 2022107683 A1 WO2022107683 A1 WO 2022107683A1 JP 2021041614 W JP2021041614 W JP 2021041614W WO 2022107683 A1 WO2022107683 A1 WO 2022107683A1
Authority
WO
WIPO (PCT)
Prior art keywords
link
self
interference
shape
robot
Prior art date
Application number
PCT/JP2021/041614
Other languages
English (en)
French (fr)
Inventor
裕樹 木本
Original Assignee
ファナック株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by ファナック株式会社 filed Critical ファナック株式会社
Priority to JP2022563718A priority Critical patent/JPWO2022107683A1/ja
Priority to DE112021004791.4T priority patent/DE112021004791T5/de
Priority to CN202180068149.1A priority patent/CN116261505A/zh
Priority to US18/042,219 priority patent/US20230321832A1/en
Publication of WO2022107683A1 publication Critical patent/WO2022107683A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39096—Self-collision, internal collison, collision between links of one robot

Definitions

  • the present invention relates to a robot control technique, and particularly to a self-interference determination device for determining self-interference of a robot having a complicated shape or shaft structure.
  • self-interference may occur in which one link collides with another when the link is operated.
  • the robot is controlled so that self-interference does not occur, and the position or angle of the link at the time of self-interference can be determined by a predetermined self-interferometry.
  • the conventional self-interferometry that determines the presence or absence of self-interference only by the position or angle of a specific link regardless of the position or angle of a link other than the specific link is all.
  • the conditions cannot be easily met.
  • the range of motion of the robot may be significantly limited when trying to deal with it using a self-interferometry formula.
  • it may not be realistic because there are too many conditional branches in the formula.
  • the following documents are known as prior arts for determining the self-interference of a robot.
  • Patent Document 1 discloses an interference determination device for an articulated robot.
  • the interference determination device has an interference region that always self-interferes regardless of the rotation angle of joints other than the specific joint in the configuration space whose coordinate axis is the rotation angle of two or three specific joints of the articulated robot.
  • the non-interference area that is always non-interfering regardless of the rotation angle of joints other than a specific joint, the acquisition unit that acquires area information representing the area, and the coordinates that represent the posture determined by the rotation angle of a specific joint are non-interfering areas and non-interference areas. It is provided with a determination unit for determining self-interference by determining which of the interference regions it belongs to. When the coordinates belong to the detailed determination region that does not belong to either the interference region or the non-interference region, the determination unit determines whether or not self-interference occurs based on the shapes of the plurality of links.
  • Patent Document 2 discloses a robot interference check device.
  • the interference check device checks for the possibility of interference between a plurality of components of the robot.
  • the interference check device is a first model expressed as a first-order approximation element including each of a plurality of components, and has a first-order interference determination unit for determining whether or not there is a possibility of interference between the first-order approximation elements and a first-order approximation unit. It is provided with a second-order interference determination unit for determining whether or not there is a possibility of interference between the second-order approximation elements in the second model expressed as a second-order approximation element equal to or smaller than the first-order approximation element.
  • the present invention is a robot having a complicated shape or shaft structure in which the presence or absence of self-interference cannot be determined only by the position or angle of a specific link regardless of the position or angle of a link other than the specific link in view of the conventional problems.
  • the purpose is to provide a technique that can determine the presence or absence of self-interference even if it exists.
  • One aspect of the present disclosure is a link shape correction unit that corrects the link shape based on a specified distance between the link shapes that are considered to be self-interfering between the links of a robot having a plurality of links, and self-interference based on the link shape.
  • a self-interference determination device including a shape interference determination unit for determining the presence or absence of.
  • Another aspect of the present disclosure is an undeterminable link in which the presence or absence of self-interference cannot be determined only by the position or angle of a specific link regardless of the position or angle of the link other than the specific link of the robot having a plurality of links.
  • a self-interference determination device including a shape interference determination unit that determines the presence or absence of self-interference based on whether or not the link shapes are close to each other to a specified distance or less.
  • a self-interference type definition unit that defines a self-interference formula that determines the possibility of self-interference by a robot having a plurality of links, and a self-interference type determination unit that determines the possibility of self-interference based on the self-interference formula.
  • a self-interference type determination unit is provided, and the self-interference type definition unit cannot determine whether or not there is self-interference only by the position or angle of the specific link regardless of the position or angle of the link other than the specific link.
  • a self-interference determination device that defines a self-interference expression based on a shape including a movable area of all links from the undecidable link on the root side of the robot to the undecidable link on the tip side of the robot is provided.
  • Yet another aspect of the present disclosure is a self-interference determination device that determines self-interference between the first link and the second link, and when the first link and the second link are considered to be self-interfering, the first link and the second link are considered to be self-interfering.
  • An undecidable link determination unit that determines whether or not one of the second links is an undecidable link that cannot determine the presence or absence of self-interference based only on the position or angle of one link, and an undecidable link based on the determination result.
  • a self-interference determination device including an interference determination unit for determining self-interference.
  • the term "link” means a connecting member in a single-axis robot or a multi-axis robot.
  • the “link” includes not only the arm portion of the robot, but also the tool portion such as, for example, a hand, a screwdriver, a welding gun, etc.
  • the term "self-interference" in this document means that links interfere with each other.
  • FIG. 1 is a schematic configuration diagram of the robot system 1.
  • the robot system 1 includes a robot 10 having a plurality of links and a self-interference determination device 20 for determining the presence or absence of self-interference.
  • the robot 10 is, for example, a collaborative robot having a complicated shape or shaft structure.
  • the robot 10 includes a first link 11, a second link 12, a third link 13, a fourth link 14, a fifth link 15, a sixth link 16, and a seventh link 17.
  • the first link 11 to the sixth link 16 are arm portions of the robot 10, and the seventh link 17 is a tool portion (for example, a hand).
  • These plurality of links are connected in series.
  • the first link 11 to the sixth link 16 are connected to each other by gears such as a speed reducer.
  • the sixth link 16 and the seventh link 17 are fastened with screws such as bolts.
  • the first link 11 rotates around the axis J1, the second link 12 rotates around the axis J2, the third link 13 rotates around the axis J3, and the fourth link 14 rotates around the axis J4.
  • the fifth link 15 rotates around the axis J5, and the sixth link 16 and the seventh link 17 rotate together around the axis J6.
  • the first link 11 to the seventh link 17 are driven by a drive source (not shown) such as a servomotor.
  • This robot 10 has a complicated shape in which the rotation center of another link (for example, the third link 13 to the seventh link 17) does not exist on a virtual plane (for example, an XZ plane or a YZ plane) including the axis J1 of the first link 11.
  • a virtual plane for example, an XZ plane or a YZ plane
  • the seventh link 17 may self-interfere with the first link 11 or the second link 12.
  • a typical articulated robot has a simple shape or axial structure in which the center of rotation of another link is on the virtual plane containing the axis J1 of the first link, so that the link is not a specific link. It can be easily determined by the self-interference formula that determines the presence or absence of self-interference only by the position or angle of a specific link regardless of the position or angle of.
  • the fifth link 15, the sixth link 16, and the seventh link 17 are the first link 11 or only at a specific link position or angle. It cannot be determined whether or not it self-interferes with the second link 12.
  • the self-interference determination device 20 can determine the presence or absence of self-interference even with the robot 10 having such a complicated shape or shaft structure.
  • the self-interference determination device 20 is, for example, a robot control device.
  • the self-interference determination device 20 may be a computer device including, for example, a processor, a memory, an input / output unit, and the like.
  • the processor is equipped with, for example, a CPU (central processing unit).
  • the memory includes, for example, RAM (random access memory), ROM (read only memory), and the like.
  • the memory stores, for example, a program executed by the processor and various data used or generated by the processor.
  • the input / output unit inputs or outputs various data used or generated by the processor.
  • the self-interference determination device 20 is a link capable of determining the presence or absence of self-interference only by the position or angle of a specific link regardless of the position or angle of a link other than the specific link (hereinafter, referred to as "determinable link").
  • determinable link a link capable of determining the presence or absence of self-interference only by the position or angle of a specific link regardless of the position or angle of a link other than the specific link.
  • the self-interference determination device 20 cannot determine the presence or absence of self-interference only by the position or angle of the specific link regardless of the position or angle of the link other than the specific link (hereinafter, referred to as "undeterminable link”. ) Is based on a shape that includes the movable area of all the links from the undecidable link on the root side of the robot 10 (for example, the fifth link 15) to the undecidable link on the tip side of the robot 10 (for example, the seventh link 17). Define a self-interferometry formula that determines the possibility of self-interference. For example, in the robot 10 shown in FIG.
  • the fifth link 15 to the seventh link 17 are undeterminable links, and all the links from the fifth link 15 on the root side of the robot to the seventh link 17 on the tip side of the robot.
  • the self-interferometry is defined based on the shape including the operating range. Then, the self-interferometry determination device 20 roughly determines the possibility of self-interference based on the self-interferometry (hereinafter, referred to as "self-interferometry determination" or "rough determination").
  • the self-interference determination device 20 determines the presence or absence of self-interference for the determineable link by the self-interferometry method, but for the undeterminable link, the self-interference occurs based on whether or not the link shapes approach each other to a specified distance or less.
  • the presence or absence is determined in detail (hereinafter, referred to as "shape interference determination” or “detailed determination”).
  • shape interference determination or “detailed determination”
  • the self-interference determination device 20 has a hybrid determination function of rough determination based on self-interferometry determination and detailed determination based on shape interference determination.
  • FIG. 2 is a functional block diagram of the robot system 1. It should be noted that the "-part" of the self-interference determination device 20 is, for example, a functional module realized by a program executed by a processor.
  • the self-interferometry determination device 20 includes a self-interferometry type definition unit 21, a self-interferometry type determination unit 22, and a robot control unit 29.
  • the self-interference type definition unit 21 defines a self-interference type for determining the possibility of the robot 10 self-interfering, and the self-interference type determination unit 22 determines the possibility of self-interference based on the defined self-interference type.
  • the robot control unit 29 controls the drive source 18 of the robot 10, and the drive source 18 provides the robot control unit 29 with the current state of the link (that is, the current position or angle, speed, etc.).
  • the robot control unit 29 provides the self-interferometry determination unit 22 with the current state of the link.
  • the self-interferometry determination unit 22 substitutes the current position or angle of the provided link into the self-interferometry to determine the possibility of self-interference.
  • FIG. 3 is a perspective view of the robot 10 showing an example of the self-interferometry definition method.
  • the self-interferometry definition unit 21 first starts from the fourth link 14 on the root side of the robot 10 with respect to the fifth link 15. All the links up to the 7th link 17 on the tip side of the robot 10 are operated, and the movable area of all the links from the 5th link 15 on the root side of the robot 10 to the 7th link 17 on the tip side of the robot 10 is included.
  • the cylinder 15-17 to be used is specified.
  • the self-interferometry definition unit 21 may acquire the link shapes of the fifth link 15 to the seventh link 17 from the link shape acquisition unit 25 described later.
  • FIG. 4 is a geometric diagram of the robot 10 showing an example of the self-interferometry definition method. Note that for ease of understanding, only the second link 12, the third fourth link 13-14, and the cylinder 15-17 are simplified and drawn in FIG.
  • the base point of the second link 12 is P1
  • the rotation center of the third link 13 is P2
  • the center of the cylinder 15-17 is P3, and the nearest vicinity point of the cylinder 15-17 to the second link 12 is P4.
  • P5 be the intersection of the perpendicular line drawn from the center P3 of the cylinder 15-17 to the center line of the second link 12 and the center line of the second link 12.
  • the length of the line segment P2P3 is L1
  • the length of the line segment P3P4 is L2
  • the current angle of the third link 13 is ⁇ 1
  • the current angle of the fifth link 15 (the line segment P2P3 and the line segment P3P4).
  • the angle formed) is ⁇ 2.
  • the size of the cylinder 15-17 toward the second link 12 can be expressed as L2cos ( ⁇ 1 + ⁇ 2-90 °). Further, the size of the second link 12 toward the cylinder 15-17 is defined as R1. On the other hand, the distance between the cylinder 15-17 and the second link 12 can be expressed as L1sin ⁇ 1. Therefore, the distance L1sin ⁇ 1 between the cylinder 15-17 and the second link 12 has a magnitude L2cos ( ⁇ 1 + ⁇ 2-90 °) toward the second link 12 of the cylinder 15-17 and toward the cylinder 15-17 of the second link 12.
  • the self-interferometry formula for determining the possibility that the undeterminable link (fifth link 15 to seventh link 17) may self-interfere with the second link 12 can be expressed as follows.
  • the self-interferometry definition unit 21 defines a self-interferometry based on the same principle for combinations of other links that may self-interfere (for example, the fifth link 15 to the seventh link 17 and the first link 11).
  • the robot control unit 29 provides the current state of the link (that is, the current position or angle) to the self-interferometry determination unit 22.
  • the self-interferometry determination unit 22 substitutes the current state of the link into the self-interferometry and determines whether or not there is a possibility of self-interference. For example, the self-interferometry determination unit 22 substitutes the current angle ⁇ 1 of the third link 13 and the current angle ⁇ 2 of the fifth link 15 into the self-interferometry of Equation 1 to perform self-interferometry determination (rough determination). conduct.
  • the self-interference determination device 20 may further include an undecidable link definition unit 23 and an undecidable link determination unit 24.
  • the undecidable link definition unit 23 defines an undecidable link that may self-interfere, and the undecidable link determination unit 24 determines whether or not the undecidable link may actually self-interfere.
  • the undecidable link definition unit 23 defines, for example, the fifth link 15 to the seventh link 17 as undecidable links. That is, the fifth link 15 to the seventh link 17 may self-interfere with the first link 11 or the second link 12, and the presence or absence of self-interference cannot be determined only by the position or angle of these links. Is. In other words, the fifth link 15 to the seventh link 17 are links having no center of rotation on the virtual plane including the axis J1 of the first link 11.
  • the undecidable link determination unit 24 determines, for example, whether or not the fifth link 15 to the seventh link 17, which are undeterminable links, may actually interfere with the first link 11 or the second link 12.
  • the undecidable link determination unit 24 is a non-determinable link (fifth link 15 to seventh link 17) in which a link determined by the self-interferometry determination unit 22 to have a possibility of self-interference is defined. Determine if it exists.
  • the undeterminable link determination unit 24 sends a stop command for the robot 10 to the robot control unit 29.
  • the robot control unit 29 stops the drive source 18 of the robot 10 based on the stop command.
  • the impossible link determination unit 24 sends a shape interference determination command to the shape interference determination unit 28.
  • the self-interference determination device 20 further includes a link shape acquisition unit 25, a designated distance acquisition unit 26, a link shape correction unit 27, and a shape interference determination unit 28.
  • the link shape acquisition unit 25 acquires the link shape
  • the designated distance acquisition unit 26 acquires the specified distance between the link shapes deemed to have interfered with each other
  • the link shape correction unit 27 corrects the link shape based on the specified distance.
  • the shape interference determination unit 28 makes a detailed determination of the presence or absence of self-interference based on whether or not the link shapes are close to each other to a specified distance or less.
  • the robot control unit 29 provides the shape interference determination unit 28 with the current state of the link (that is, the current position or angle, speed, etc.).
  • the shape interference determination unit 28 calculates the current position and orientation of the link shape from the current position or angle of the provided link.
  • the link shape acquisition unit 25 acquires the link shapes of the first link 11, the second link 12, the fifth link 15, the sixth link 16, and the seventh link 17, which may interfere with each other, for example.
  • the link shape acquisition unit 25 may acquire all the link shapes of the first link 11 to the seventh link 17.
  • the link shape may be three-dimensional data such as three-dimensional CAD (computer-aided design) data and three-dimensional polygon data.
  • the link shape acquisition unit 25 acquires the link shape from, for example, a memory or an external device.
  • the designated distance acquisition unit 26 acquires, for example, a designated distance of 50 mm between the link shapes deemed to have self-interfered between the second link 12 and the fifth link 15. As a result, when the second link 12 and the fifth link 15 approach each other to a designated distance of 50 mm or less, it is considered that self-interference has occurred, and the robot 10 can be stopped.
  • the designated distance may be designated by a range, and in this case, the designated distance acquisition unit 26 acquires, for example, a range of 50 mm to 70 mm between the link shapes deemed to have self-interfered between the second link 12 and the fifth link 15.
  • the designated distance acquisition unit 26 acquires a designated distance from, for example, a memory or an external device.
  • the link shape correction unit 27 expands the link shape by a specified distance, for example.
  • FIG. 5A is a perspective view of the robot 10 showing the link shape 15a enlarged by a designated distance.
  • the link shape 15a is an enlargement of the actual link shape of the fifth link 15 by a designated distance of 50 mm.
  • the link shape correction unit 27 may be simplified so that the link shape is within the designated range, for example.
  • FIG. 5B is a perspective view of the robot 10 showing a link shape 15b simplified so as to fit within a designated range.
  • the link shape 15b is a simplification of the actual link shape of the fifth link 15 within a specified range of 50 mm to 70 mm.
  • the link shape 15b is simplified by two cylinders 30 and 31 and one sphere 32.
  • the link shape may be simplified with one cylinder, one sphere, or the like as long as it is within the specified range of 50 mm to 70 mm.
  • link shape correction unit 27 similarly expands the other undeterminable links such as the sixth link 16 and the seventh link 17 by a specified distance, or simplifies them so that they fall within the specified range.
  • the shape interference determination unit 28 determines, for example, whether or not the expanded link shape 15a of the fifth link 15 and the link shape of the second link 12 interfere (contact or intersect). For example, when using three-dimensional polygon data, it is advisable to determine contact or intersection between polygon faces by roundabout. For example, if one or more vertices of one polygon surface are on the back side of the other polygon surface and the remaining vertices are on the front side of the other polygon surface, it can be determined that the link shapes are interfering with each other. Such shape interference determination is superior in terms of calculation cost to determining whether or not the distance between polygon faces is equal to or less than a specified distance by round-robin.
  • the shape interference determination unit 28 may determine, for example, whether or not the simplified link shape 15b of the fifth link 15 and the link shape of the second link 12 interfere (contact or intersect). For example, when three-dimensional polygon data is used, the simplified link shape 15b has an overwhelmingly smaller number of polygon faces than the enlarged link shape 15a, so that the shape interference determination based on the simplified link shape 15b is an enlarged link shape. It is superior to the shape interference determination based on 15a in terms of calculation cost.
  • the shape interference determination unit 28 When it is determined that there is self-interference based on such shape interference determination (detailed determination), the shape interference determination unit 28 sends a stop command for the robot 10 to the robot control unit 29. The robot control unit 29 stops the drive source 18 of the robot 10 based on the stop command. Alternatively, when it is determined that there is no self-interference based on the shape interference determination (detailed determination), the shape interference determination unit 28 sends a determination command to the self-interferometry determination unit 22 and returns to the self-interferometry determination (rough determination). Or, the shape interference determination (detailed determination) is repeated.
  • the self-interference determination device 20 calculates the current shortest distance between the link shapes and the current relative speed between the link shapes, and whether or not the robot 10 can be safely (softly) stopped based on the shortest distance and the relative speed. If it is determined that the robot 10 can be stopped safely, a robot stop command unit (not shown) that sends a safe stop command to the robot control unit 29 may be further provided. When the robot stop command unit determines that the robot 10 cannot be stopped safely, the robot stop command unit may send an emergency stop command to the robot control unit 29.
  • FIG. 6 is a flowchart showing an example of preprocessing of the self-interference determination device 20. This flowchart is executed, for example, by the processor of the self-interference determination device 20.
  • step S10 an undeterminable link in which the presence or absence of self-interference cannot be determined only by the position or angle of a specific link is defined.
  • the fifth link 15 to the seventh link 17, which are undeterminable links are input by a user operation via a graphical user interface (GUI).
  • GUI graphical user interface
  • a self-interferometry formula for determining the possibility of self-interfering between links is defined.
  • the conventional self-interferometry is defined for the determineable link, and the fifth link 15 to the seventh link 17, which are undeterminable links, are from the fifth link 15 on the root side of the robot 10 to the seventh on the tip side of the robot 10.
  • the self-interferometry is defined based on the shape including the movable area of all the links up to the link 17.
  • step S12 the link shape is acquired.
  • the link shape is input from a memory or an external device.
  • step S13 the designated distance between the link shapes deemed to have interfered with each other is acquired.
  • the designated distance of 50 mm is input by user operation via the GUI.
  • step S14 the link shape is corrected based on the designated distance.
  • the link shape of the fifth link 15 is expanded by a specified distance of 50 mm, or when the specified distance is specified in a range, the link shape of the fifth link 15 is simplified so that it fits within the specified range of 50 mm to 70 mm. do.
  • FIG. 7 is a flowchart showing an example of the determination process of the self-interference determination device 20. This flowchart is executed, for example, by the processor of the self-interference determination device 20. Also note that this flowchart is executed during the operation of the robot 10.
  • step S20 the possibility of self-interference is determined based on the self-interferometry (that is, the self-interferometry determination (rough determination) is performed). If it is determined by the self-interferometry that there is no possibility of self-interference (NO in step S20), the self-interferometry determination (rough determination) in step S20 is repeated.
  • step S20 If it is determined by the self-interferometry that there is a possibility of self-interference (YES in step S20), is the link determined by the self-interferometry that there is a possibility of self-interference in step S21 a non-determinable link? Judge whether or not. If the link determined by the self-interferometry that there is a possibility of self-interference is a determineable link (NO in step S21), the operation of the robot 10 is stopped in step S23 because there is self-interference. At this time, the current shortest distance between the link shapes and the current relative speed between the link shapes may be calculated, and the robot 10 may be safely stopped based on the shortest distance and the relative speed.
  • the link determined by the self-interference type to have the possibility of self-interference is an undeterminable link (YES in step S21), it is based on whether or not the link shapes approach each other to a specified distance or less in step S22. Judgment of the presence or absence of self-interference (that is, shape interference judgment (detailed judgment) is performed). For example, it is determined whether or not the link shape expanded by a specified distance and another link shape interfere (contact or intersect). Alternatively, when the specified distance is specified in the range, it is determined whether or not the simplified link shape and another link shape interfere (contact or intersect) within the specified range.
  • step S22 If it is determined that the link shapes do not interfere with each other (NO in step S22), the process returns to the self-interferometry determination in step S20. Alternatively, the shape interference determination in step S22 may be repeated. If it is determined that the link shapes are interfering with each other (YES in step S22), the operation of the robot 10 is stopped in step S23 because there is self-interference. At this time, the current shortest distance between the link shapes and the current relative speed between the link shapes may be calculated, and the robot 10 may be safely stopped based on the shortest distance and the relative speed.
  • the self-interference determination device 20 described above may not be configured as a robot control device, but may be configured as a host computer device that sends various commands to the robot control device.
  • FIG. 8 is a functional block diagram of the robot system 1 of another embodiment.
  • the robot system 1 includes one self-interference determination device 20, a plurality of robot control devices 40, and a plurality of robots 10.
  • the current state of the link of each robot 10 (for example, the current position or angle, speed, etc.) is aggregated in the self-interference determination device 20 via each robot control device 40.
  • the self-interference determination device 20 performs self-interference type determination (rough determination) and shape interference determination (detailed determination) based on the current state (current position or angle, speed, etc.) of the link of each robot 10.
  • the self-interference determination device 20 determines that there is self-interference
  • the self-interference determination device 20 sends a stop command for each robot 10 to each robot control device 40.
  • Each robot control device 40 stops the drive source 18 of each robot 10 based on the stop command.
  • the configuration of the robot system 1 and the self-interference determination device 20 described above is an example, and other configurations may be adopted.
  • the robot 10 is not an industrial robot but may be a robot of another form such as a humanoid.
  • the robot 10 may apply the shape interference determination to a robot having a plurality of parallel links (for example, a dual-arm robot) instead of having a plurality of links in series.
  • the seventh link 17 of the robot 10 may be a tool portion of another form such as a screwdriver, a welding tool, a sealing tool, etc., instead of the hand.
  • the self-interference determination device 20 is not provided with a processor that executes a program, but is provided with an integrated circuit (for example, FPGA (field-programmable gate array), ASIC (application specific integrated circuit), etc.) that does not execute the program.
  • the circuit may include the "-part" shown in FIG.
  • the flowcharts shown in FIGS. 6 and 7 may be executed by the integrated circuit instead of the processor.
  • the self-interference determination device 20 may not have a hybrid determination function of self-interference type determination (rough determination) and shape interference determination (detailed determination), but may have only a shape interference determination function.
  • the link shape correction unit 27 does not expand the link shape of the fifth link 15, which is an undeterminable link, by a specified distance, but the first link 11 or the second link may self-interfere with the fifth link 15.
  • the link shape of the link 12 may be enlarged.
  • the link shape of the fifth link 15 and the link shape of the first link 11 or the second link 12 may be expanded as long as they are within the designated distance.
  • the link shape correction unit 27 does not simplify the link shape of the fifth link 15, which is an undeterminable link, so that it fits within the specified range.
  • the link shape of the first link 11 or the second link 12 which may self-interfere with the fifth link 15 may be simplified, or if it is within the specified range, the link shape of the fifth link 15 may be simplified.
  • the link shape of the first link 11 or the second link 12 may be simplified respectively.
  • the self-interference determination device 20 when the self-interference determination device 20 is considered to self-interfere with each other based on the self-interferometry, it is determined that one of these links cannot determine the presence or absence of self-interference only by the position or angle of the link. Even if it is an impossible link, if self-interference can be determined by the position or angle of the other link, the self-interference of the undecidable link is simply determined by the position or angle of the other link instead of performing the shape interference determination.
  • An interference determination unit for determination may be provided.
  • a program executed by the above-mentioned processor, another integrated circuit, or the like, or a program executing the above-mentioned flowchart may be provided by recording on a computer-readable non-temporary recording medium such as a CD-ROM.
  • the program may be distributed and provided from a server device on a WAN (wide area network) or LAN (local area network) via a wired or wireless system.
  • Robot system 10 Robot 11 1st link 12 2nd link 13 3rd link 14 4th link 15 5th link 15a Enlarged link shape 15b Simplified link shape 16 6th link 17 7th link 18 Drive source 13-14 Third and fourth links 15-17 Cylinder 20 Self-interference judgment device 21 Self-interference type definition unit 22 Self-interference type judgment unit 23 Undecidable link definition unit 24 Undecidable link determination unit 25 Link shape acquisition unit 26 Designated distance acquisition unit 27 Link Shape correction part 28 Shape interference judgment part 29 Robot control part 30, 31 Cylinder 32 Sphere J1 to J6 Axis line

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

自己干渉判定装置は、複数のリンクを備えたロボットのリンク同士が自己干渉したとみなすリンク形状間の指定距離に基づいてリンク形状を修正するリンク形状修正部と、リンク形状に基づいて自己干渉の有無を判定する形状干渉判定部と、を備える。

Description

自己干渉判定装置
　本発明は、ロボットの制御技術に関し、特に複雑な形状又は軸構造を持つロボットの自己干渉を判定する自己干渉判定装置に関する。
　複数のリンクを備えるロボットでは、リンクを動作させた場合に或るリンクが他のリンクに衝突する自己干渉が発生することがある。ロボットは自己干渉が発生しないように制御されており、自己干渉する際のリンクの位置又は角度は予め定めた自己干渉式で判定可能である。しかし、複雑な形状又は軸構造を持つロボットでは、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみで自己干渉の有無を判定する従来の自己干渉式では全ての条件に簡単に対応できない。また自己干渉式の数式で対応しようとするとロボットの動作範囲が著しく制限されることがある。さらに数式の条件分岐が多くなりすぎて現実的ではないこともある。ロボットの自己干渉を判定する先行技術としては後述の文献が公知である。
　特許文献１には、多関節ロボットの干渉判定装置が開示されている。干渉判定装置は、多関節ロボットの二つ又は三つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に、特定の関節以外の関節の回転角度に関係なく必ず自己干渉する干渉領域と、特定の関節以外の関節の回転角度に関係なく必ず非干渉となる非干渉領域と、を表す領域情報を取得する取得部と、特定の関節の回転角度によって定まる姿勢を表す座標が干渉領域と非干渉領域の何れに属するかを判定することで自己干渉を判定する判定部と、を備えている。当該座標が干渉領域と非干渉領域の何れにも属さない詳細判定領域に属する場合、判定部は複数のリンクの形状に基づいて自己干渉が発生するか否かを判定している。
　特許文献２には、ロボットの干渉チェック装置が開示されている。干渉チェック装置は、ロボットの複数の構成要素間の干渉可能性の有無をチェックする。干渉チェック装置は、複数の構成要素の夫々を包含する１次近似要素として表現される第１モデルで１次近似要素間の干渉可能性の有無を判定する１次干渉判定部と、１次近似要素と同等又は１次近似要素よりも小さい２次近似要素として表現される第２モデルで２次近似要素間の干渉可能性の有無を判定する２次干渉判定部と、を備えている。
特開２０１９－１９８９１４号公報 特開２０１１－１３１３０３号公報
　本発明は、従来の問題点に鑑み、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみでは自己干渉の有無を判定できない複雑な形状又は軸構造を持つロボットであっても自己干渉の有無を判定可能な技術を提供することを目的とする。
　本開示の一態様は、複数のリンクを備えたロボットのリンク同士が自己干渉したとみなすリンク形状間の指定距離に基づいてリンク形状を修正するリンク形状修正部と、リンク形状に基づいて自己干渉の有無を判定する形状干渉判定部と、を備える、自己干渉判定装置を提供する。
　本開示の他の態様は、複数のリンクを備えたロボットの特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクについてはリンク形状同士が指定距離以下まで接近したか否かに基づいて自己干渉の有無を判定する形状干渉判定部を備える、自己干渉判定装置を提供する。
　本開示の別の態様は、複数のリンクを備えたロボットが自己干渉する可能性を判定する自己干渉式を定義する自己干渉式定義部と、自己干渉式に基づいて自己干渉の可能性を判定する自己干渉式判定部と、を備え、自己干渉式定義部は、特定のリンク以外のリンクの位置又は角度に拘わらず前記特定のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクについてはロボットの根元側の判定不能リンクからロボットの先端側の判定不能リンクまでの全てのリンクの可動領域を包含する形状に基づいて自己干渉式を定義する、自己干渉判定装置を提供する。
　本開示のさらに別の態様は、第一リンクと第二リンクの自己干渉を判定する自己干渉判定装置であって、第一リンクと第二リンクが自己干渉するとみなされた場合に第一リンクと第二リンクのいずれか一方が一方のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクであるか否かを判定する判定不能リンク判定部と、判定結果に基づいて判定不能リンクの自己干渉を判定する干渉判定部と、を備える自己干渉判定装置を提供する。
　本開示の態様によれば、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみでは自己干渉の有無を判定できない複雑な形状又は軸構造を持つロボットであっても自己干渉の有無を判定できる。
一実施形態のロボットシステムの概略構成図である。 一実施形態のロボットシステムの機能ブロック図である。 自己干渉式定義方法の一例を示すロボットの斜視図である。 自己干渉式定義方法の一例を示すロボットの幾何学図である。 指定距離分拡大したリンク形状を示すロボットの斜視図である。 指定された範囲内に収まるように簡略化したリンク形状を示すロボットの斜視図である。 自己干渉判定装置の前処理の一例を示すフローチャートである。 自己干渉判定装置の判定処理の一例を示すフローチャートである。 他の実施形態のロボットシステムの機能ブロック図である。
　以下、添付図面を参照して本開示の実施形態を詳細に説明する。各図面において、同一又は類似の構成要素には同一又は類似の符号が付与されている。また、以下に記載する実施形態は特許請求の範囲に記載される発明の技術的範囲及び用語の意義を限定するものではない。なお、本書において用語「リンク」とは、単軸ロボット又は多軸ロボットにおける連結部材を意味する。例えば「リンク」は、ロボットのアーム部分だけでなく、例えばハンド、ドライバ、溶接ガン等のツール部分も含むことに留意されたい。また本書において用語「自己干渉」とは、リンク同士が干渉することを意味する。さらに本書において用語「干渉」とは、リンク形状に基づく形状干渉判定の計算上、物体と物体の接触だけではなく、物体と物体の交差も含む意味であることに留意されたい。また本書において用語「回動」とは、正回転と逆回転の双方を含む意味であることに留意されたい。
　先ず、本実施形態のロボットシステムの構成について説明する。図１はロボットシステム１の概略構成図である。ロボットシステム１は、複数のリンクを備えたロボット１０と、自己干渉の有無を判定する自己干渉判定装置２０と、を備えている。ロボット１０は例えば複雑な形状又は軸構造を持つ協働ロボットである。ロボット１０は、第一リンク１１、第二リンク１２、第三リンク１３、第四リンク１４、第五リンク１５、第六リンク１６、及び第七リンク１７を備えている。例えば第一リンク１１～第六リンク１６はロボット１０のアーム部分であり、第七リンク１７はツール部分（例えばハンド）である。これら複数のリンクは直列して連結されている。
　第一リンク１１～第六リンク１６は例えば減速機等の歯車で互いに連結される。第六リンク１６と第七リンク１７は例えばボルト等のねじで締結される。第一リンク１１は軸線Ｊ１回りに回動し、第二リンク１２は軸線Ｊ２回りに回動し、第三リンク１３は軸線Ｊ３回りに回動し、第四リンク１４は軸線Ｊ４回りに回動し、第五リンク１５は軸線Ｊ５回りに回動し、第六リンク１６と第七リンク１７は軸線Ｊ６回りに一緒に回動する。第一リンク１１～第七リンク１７は例えばサーボモータ等の駆動源（図示せず）によって駆動される。
　このロボット１０は第一リンク１１の軸線Ｊ１を含む仮想平面（例えばＸＺ平面やＹＺ平面）上に他のリンク（例えば第三リンク１３～第七リンク１７）の回転中心が存在しない複雑な形状又は軸構造を持つ。また、第三リンク１３は軸線Ｊ３回りに３６０°回動し、第四リンク１４は軸線Ｊ４回りに３６０°回動するため、第四リンク１４よりもロボット１０の先端側にある第五リンク１５～第七リンク１７が第一リンク１１又は第二リンク１２に自己干渉する可能性がある。
　対照的に、例えば一般的な垂直多関節ロボットは第一リンクの軸線Ｊ１を含む仮想平面上に他のリンクの回転中心が存在する単純な形状又は軸構造を持つため、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみで自己干渉の有無を判定する自己干渉式で簡単に判定できる。しかし、図１に示すような複雑な形状又は軸構造を持つロボット１０では、第五リンク１５、第六リンク１６、及び第七リンク１７が特定のリンクの位置又は角度のみで第一リンク１１又は第二リンク１２に自己干渉するか否かを判定できない。自己干渉判定装置２０はこのように複雑な形状又は軸構造を持つロボット１０であっても自己干渉の有無を判定できる。
　自己干渉判定装置２０は例えばロボット制御装置である。自己干渉判定装置２０は、図示しないが、例えばプロセッサ、メモリ、入出力部等を備えたコンピュータ装置でよい。プロセッサは例えばＣＰＵ（central processing unit）を備えている。メモリは例えばＲＡＭ（random access memory）、ＲＯＭ（read only memory）等を備えている。メモリは例えばプロセッサで実行されるプログラムやプロセッサで利用又は生成される各種データを記憶する。入出力部はプロセッサで利用又は生成される各種データを入力又は出力する。
　先ず自己干渉判定装置２０は、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみで自己干渉の有無を判定できるリンク（以下、「判定可能リンク」と称する。）については特定のリンクの位置又は角度のみで自己干渉の有無を判定する従来の自己干渉式を定義する。但し、図１に示すロボット１０では、自己干渉する可能性がある判定可能リンクはないことに留意されたい。
　一方、自己干渉判定装置２０は、特定のリンク以外のリンクの位置又は角度に拘わらず特定のリンクの位置又は角度のみで自己干渉の有無を判定できないリンク（以下、「判定不能リンク」と称する。）についてはロボット１０の根元側の判定不能リンク（例えば第五リンク１５）からロボット１０の先端側の判定不能リンク（例えば第七リンク１７）までの全てのリンクの可動領域を包含する形状に基づいて自己干渉の可能性を判定する自己干渉式を定義する。例えば図１に示すロボット１０では、第五リンク１５～第七リンク１７が判定不能リンクであり、ロボットの根元側の第五リンク１５からロボットの先端側の第七リンク１７までの全てのリンクの動作範囲を包含する形状に基づいて自己干渉式を定義する。そして、自己干渉判定装置２０は自己干渉式に基づいて自己干渉の可能性を概略判定する（以下、「自己干渉式判定」又は「概略判定」と称する。）。
　また自己干渉判定装置２０は、判定可能リンクについては自己干渉式で自己干渉の有無を判定するが、判定不能リンクについてはリンク形状同士が指定距離以下まで接近したか否かに基づいて自己干渉の有無を詳細判定する（以下、「形状干渉判定」又は「詳細判定」と称する。）。例えば図１に示すロボット１０では、判定不能リンクである第五リンク１５～第七リンク１７については形状干渉判定に基づく詳細判定を行う。これにより、ロボット１０の動作範囲が制限されてしまうのを防止できる。つまり自己干渉判定装置２０は自己干渉式判定に基づく概略判定と形状干渉判定に基づく詳細判定というハイブリッド判定機能を備えている。
　図２はロボットシステム１の機能ブロック図である。自己干渉判定装置２０が有する「～部」とは、例えばプロセッサで実行されるプログラムによって実現される機能モジュールであることに留意されたい。自己干渉判定装置２０は、自己干渉式定義部２１と、自己干渉式判定部２２と、ロボット制御部２９と、を備えている。自己干渉式定義部２１はロボット１０が自己干渉する可能性を判定する自己干渉式を定義し、自己干渉式判定部２２は定義された自己干渉式に基づいて自己干渉の可能性を判定する。ロボット制御部２９はロボット１０の駆動源１８を制御し、駆動源１８はリンクの現在の状態（つまり現在の位置又は角度、速度等）をロボット制御部２９に提供する。ロボット制御部２９は自己干渉式判定部２２に対してリンクの現在の状態を提供する。自己干渉式判定部２２は提供されたリンクの現在の位置又は角度を自己干渉式に代入して自己干渉の可能性を判定する。
　図３は自己干渉式定義方法の一例を示すロボット１０の斜視図である。例えば判定不能リンクである第五リンク１５～第七リンク１７の自己干渉式を定義する場合、自己干渉式定義部２１は、先ず第五リンク１５よりもロボット１０の根元側の第四リンク１４からロボット１０の先端側の第七リンク１７までの全てのリンクを動作させ、ロボット１０の根元側の第五リンク１５からロボット１０の先端側の第七リンク１７までの全てのリンクの可動領域を包含する円柱１５－１７を規定する。なお、自己干渉式定義部２１は第五リンク１５～第七リンク１７のリンク形状を後述のリンク形状取得部２５から取得してもよい。
　次いで、自己干渉式定義部２１は判定不能リンクである第五リンク１５～第七リンク１７の可動領域を包含する円柱１５－１７が例えば第二リンク１２に干渉する可能性を判定する自己干渉式を定義する。図４は自己干渉式定義方法の一例を示すロボット１０の幾何学図である。理解を容易にするため、図４には、第二リンク１２、第三第四リンク１３－１４、及び円柱１５－１７のみが簡略化して描かれていることに留意されたい。
　ここで、第二リンク１２の基点をＰ１とし、第三リンク１３の回転中心をＰ２とし、円柱１５－１７の中心をＰ３とし、円柱１５－１７の第二リンク１２に最も近い近傍点をＰ４とし、円柱１５－１７の中心Ｐ３から第二リンク１２の中心線に下した垂線と第二リンク１２の中心線との交点をＰ５とする。また線分Ｐ２Ｐ３の長さをＬ１とし、線分Ｐ３Ｐ４の長さをＬ２とし、第三リンク１３の現在の角度をθ１とし、第五リンク１５の現在の角度（線分Ｐ２Ｐ３と線分Ｐ３Ｐ４の成す角度）をθ２とする。
　このとき、垂線Ｐ３Ｐ５と線分Ｐ３Ｐ４の成す角度はθ１＋θ２－９０°となるため、円柱１５－１７の第二リンク１２に向かう大きさはＬ２ｃｏｓ（θ１＋θ２－９０°）と表せる。また第二リンク１２の円柱１５－１７に向かう大きさをＲ１とする。一方、円柱１５－１７と第二リンク１２との間の距離はＬ１ｓｉｎθ１として表せる。従って、円柱１５－１７と第二リンク１２との間の距離Ｌ１ｓｉｎθ１が円柱１５－１７の第二リンク１２に向かう大きさＬ２ｃｏｓ（θ１＋θ２－９０°）と第二リンク１２の円柱１５－１７に向かう大きさＲ１との合計以下なると、円柱１５－１７（つまり判定不能リンクである第五リンク１５～第七リンク１７）が第二リンク１２に干渉した可能性があることになる。つまり判定不能リンク（第五リンク１５～第七リンク１７）が第二リンク１２に自己干渉する可能性を判定する自己干渉式は以下のように表せる。
Figure JPOXMLDOC01-appb-M000001
　ここで、Ｌ１、Ｌ２、及びＲ１は定数であるため、第三リンク１３の現在の角度θ１と、第五リンク１５の現在の角度θ２を式１の自己干渉式に代入すれば、判定不能リンク（第五リンク１５～第七リンク１７）が第二リンク１２に自己干渉する可能性を判定できることになる。自己干渉式定義部２１は自己干渉する可能性がある他のリンクの組み合せ（例えば第五リンク１５～第七リンク１７と第一リンク１１）についても同様の原理で自己干渉式を定義する。
　図２を再び参照すると、ロボット制御部２９はリンクの現在の状態（つまり現在の位置又は角度）を自己干渉式判定部２２に提供する。自己干渉式判定部２２はリンクの現在の状態を自己干渉式に代入して自己干渉の可能性があるか否か判定する。例えば自己干渉式判定部２２は、第三リンク１３の現在の角度θ１と、第五リンク１５の現在の角度θ２とを式１の自己干渉式に代入して自己干渉式判定（概略判定）を行う。
　自己干渉判定装置２０は、判定不能リンク定義部２３と、判定不能リンク判定部２４と、をさらに備えているとよい。判定不能リンク定義部２３は自己干渉する可能性がある判定不能リンクを定義し、判定不能リンク判定部２４は判定不能リンクが実際に自己干渉する可能性があるか否かを判定する。
　判定不能リンク定義部２３は例えば第五リンク１５～第七リンク１７を判定不能リンクとして定義する。つまり第五リンク１５～第七リンク１７は、第一リンク１１又は第二リンク１２に自己干渉する可能性があり、また、これらリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクである。換言すれば、第五リンク１５～第七リンク１７は第一リンク１１の軸線Ｊ１を含む仮想平面上に回転中心が存在しないリンクである。
　判定不能リンク判定部２４は例えば判定不能リンクである第五リンク１５～第七リンク１７が実際に第一リンク１１又は第二リンク１２に自己干渉する可能性があるか否かを判定する。具体的には、判定不能リンク判定部２４は自己干渉式判定部２２で自己干渉の可能性があると判定されたリンクが定義された判定不能リンク（第五リンク１５～第七リンク１７）であるか否かを判定する。
　自己干渉式で自己干渉の可能性があると判定されたリンクが、定義された判定不能リンクではなく、判定可能リンクであった場合は、自己干渉式で自己干渉の有無を正確に判定できるリンクであるため、判定不能リンク判定部２４はロボット制御部２９に対してロボット１０の停止指令を送出する。ロボット制御部２９は停止指令に基づいてロボット１０の駆動源１８を停止させる。
　或いは、自己干渉式で自己干渉の可能性があると判定されたリンクが定義された判定不能リンクであった場合は、自己干渉式で自己干渉の有無を正確に判定できないリンクであるため、判定不能リンク判定部２４は形状干渉判定部２８に対して形状干渉判定指令を送出する。自己干渉判定装置２０は、リンク形状取得部２５と、指定距離取得部２６と、リンク形状修正部２７と、形状干渉判定部２８と、をさらに備えている。
　リンク形状取得部２５はリンク形状を取得し、指定距離取得部２６はリンク同士が干渉したとみなすリンク形状間の指定距離を取得し、リンク形状修正部２７はリンク形状を指定距離に基づき修正し、形状干渉判定部２８はリンク形状同士が指定距離以下まで接近したか否かに基づいて自己干渉の有無を詳細判定する。ロボット制御部２９は形状干渉判定部２８に対してリンクの現在の状態（つまり現在の位置又は角度、速度等）を提供する。形状干渉判定部２８は提供されたリンクの現在の位置又は角度からリンク形状の現在の位置及び姿勢を算出する。
　リンク形状取得部２５は、例えば自己干渉する可能性がある第一リンク１１、第二リンク１２、第五リンク１５、第六リンク１６、及び第七リンク１７のリンク形状を取得する。或いは、リンク形状取得部２５は第一リンク１１～第七リンク１７の全てのリンク形状を取得してもよい。例えばリンク形状は、三次元ＣＡＤ（computer-aided design）データ、三次元ポリゴンデータ等の三次元データでよい。リンク形状取得部２５は例えばメモリ又は外部装置からリンク形状を取得する。
　指定距離取得部２６は例えば第二リンク１２と第五リンク１５が自己干渉したとみなすリンク形状間の指定距離５０ｍｍを取得する。これにより、第二リンク１２と第五リンク１５が指定距離５０ｍｍ以下まで接近したときに自己干渉が発生したとみなし、ロボット１０を停止できる。或いは、指定距離は範囲で指定されてもよく、この場合、指定距離取得部２６は例えば第二リンク１２と第五リンク１５が自己干渉したとみなすリンク形状間の範囲５０ｍｍ～７０ｍｍを取得する。これにより、第二リンク１２と第五リンク１５が指定された範囲５０ｍｍ～７０ｍｍ内まで接近したときに自己干渉が発生したとみなし、ロボット１０を停止できる。指定距離取得部２６は例えばメモリ又は外部装置から指定距離を取得する。
　リンク形状修正部２７は例えばリンク形状を指定距離分拡大する。図５Ａは指定距離分拡大したリンク形状１５ａを示すロボット１０の斜視図である。リンク形状１５ａは第五リンク１５の実際のリンク形状を指定距離５０ｍｍ分拡大したものである。或いは、指定距離が範囲で指定される場合、リンク形状修正部２７は例えばリンク形状を指定された範囲内に収まるように簡略化してもよい。図５Ｂは指定された範囲内に収まるように簡略化したリンク形状１５ｂを示すロボット１０の斜視図である。リンク形状１５ｂは第五リンク１５の実際のリンク形状を指定された範囲５０ｍｍ～７０ｍｍ内で簡略化したものである。リンク形状１５ｂは二つの円柱３０、３１と一つの球体３２で簡略化されている。なお、リンク形状は、指定された範囲５０ｍｍ～７０ｍｍ内に収まる限り、一つの円柱や一つの球体等で簡略化してもよい。
　またリンク形状修正部２７は、他の判定不能リンクである第六リンク１６、第七リンク１７等についても同様に指定距離分拡大するか又は指定された範囲内に収まるように簡略化する。
　図２を再び参照すると、形状干渉判定部２８は例えば第五リンク１５の拡大したリンク形状１５ａと第二リンク１２のリンク形状が干渉（接触又は交差）しているか否かを判定する。例えば三次元ポリゴンデータを用いる場合、ポリゴン面同士の接触又は交差を総当たりで判定するとよい。例えば一方のポリゴン面の一つ又は複数の頂点が他方のポリゴン面の裏側にあり、残りの頂点が他方のポリゴン面の表側にあれば、リンク形状同士が干渉していると判定できる。このような形状干渉判定は、ポリゴン面同士の距離が指定距離以下であるか否かを総当たりで判定するよりも計算コストの面で優れている。
　或いは、形状干渉判定部２８は例えば第五リンク１５の簡略化したリンク形状１５ｂと第二リンク１２のリンク形状が干渉（接触又は交差）しているか否かを判定してもよい。例えば三次元ポリゴンデータを用いる場合、簡略化したリンク形状１５ｂは拡大したリンク形状１５ａよりもポリゴン面数が圧倒的に低減するため、簡略化したリンク形状１５ｂに基づく形状干渉判定は拡大したリンク形状１５ａに基づく形状干渉判定よりも計算コストの面で優れている。
　このような形状干渉判定（詳細判定）に基づき自己干渉が有ると判定された場合、形状干渉判定部２８はロボット制御部２９に対してロボット１０の停止指令を送出する。ロボット制御部２９は停止指令に基づいてロボット１０の駆動源１８を停止させる。或いは、形状干渉判定（詳細判定）に基づき自己干渉が無いと判定された場合、形状干渉判定部２８は自己干渉式判定部２２に判定指令を送出して自己干渉式判定（概略判定）に戻るか、又は形状干渉判定（詳細判定）を繰り返す。
　なお、自己干渉判定装置２０は、リンク形状同士の現在の最短距離とリンク形状同士の現在の相対速度を計算し、最短距離と相対速度に基づいてロボット１０を安全に（柔らかく）停止できるか否かを判定し、ロボット１０を安全に停止できると判定した場合はロボット制御部２９に対して安全停止指令を送出するロボット停止指令部（図示せず）をさらに備えていてもよい。ロボット停止指令部は、ロボット１０を安全に停止できないと判定した場合はロボット制御部２９に対して緊急停止指令を送出してもよい。
　図６は自己干渉判定装置２０の前処理の一例を示すフローチャートである。このフローチャートは例えば自己干渉判定装置２０のプロセッサで実行される。ステップＳ１０では、特定のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクを定義する。例えば判定不能リンクである第五リンク１５～第七リンク１７がグラフィカルユーザインタフェース（ＧＵＩ）を介してユーザ操作で入力される。
　ステップＳ１１では、リンク同士が自己干渉する可能性を判定する自己干渉式を定義する。例えば判定可能リンクについては従来の自己干渉式を定義し、判定不能リンクである第五リンク１５～第七リンク１７についてはロボット１０の根元側の第五リンク１５からロボット１０の先端側の第七リンク１７までの全てのリンクの可動領域を包含する形状に基づいて自己干渉式を定義する。
　ステップＳ１２ではリンク形状を取得する。例えばリンク形状はメモリ又は外部装置から入力される。ステップＳ１３ではリンク同士が干渉したとみなすリンク形状間の指定距離を取得する。例えば指定距離５０ｍｍはＧＵＩを介してユーザ操作で入力される。ステップＳ１４ではリンク形状を指定距離に基づき修正する。例えば第五リンク１５のリンク形状を指定距離５０ｍｍ分拡大するか、又は指定距離が範囲で指定される場合は第五リンク１５のリンク形状を指定された範囲５０ｍｍ～７０ｍｍ内に収まるように簡略化する。
　なお、図６に示すフローチャートにおいて、ステップＳ１０～Ｓ１１とステップＳ１２～Ｓ１４の順序やステップＳ１２とステップＳ１３の順序は逆でもよいことに留意されたい。
　図７は自己干渉判定装置２０の判定処理の一例を示すフローチャートである。このフローチャートは例えば自己干渉判定装置２０のプロセッサによって実行される。またこのフローチャートはロボット１０の動作中に実行されることに留意されたい。ステップＳ２０では、自己干渉式に基づいて自己干渉の可能性を判定する（つまり自己干渉式判定（概略判定）を行う）。自己干渉式で自己干渉の可能性が無いと判定された場合は（ステップＳ２０のＮＯ）、ステップＳ２０の自己干渉式判定（概略判定）を繰り返す。
　自己干渉式で自己干渉の可能性が有ると判定された場合は（ステップＳ２０のＹＥＳ）、ステップＳ２１において自己干渉式で自己干渉の可能性が有ると判定されたリンクが判定不能リンクであるか否かを判定する。自己干渉式で自己干渉の可能性が有ると判定されたリンクが判定可能リンクである場合は（ステップＳ２１のＮＯ）、自己干渉有りのため、ステップＳ２３においてロボット１０の動作を停止させる。このとき、リンク形状同士の現在の最短距離とリンク形状同士の現在の相対速度を計算し、最短距離と相対速度に基づいてロボット１０を安全停止させてもよい。
　自己干渉式で自己干渉の可能性が有ると判定されたリンクが判定不能リンクである場合は（ステップＳ２１のＹＥＳ）、ステップＳ２２においてリンク形状同士が指定距離以下まで接近したか否かに基づいて自己干渉の有無を判定する（つまり形状干渉判定（詳細判定）を行う）。例えば指定距離分拡大したリンク形状と他のリンク形状が干渉（接触又は交差）しているか否かを判定する。或いは、指定距離が範囲で指定された場合は、指定された範囲内で簡略化したリンク形状と他のリンク形状が干渉（接触又は交差）しているか否かを判定する。
　リンク形状同士が干渉していないと判定された場合は（ステップＳ２２のＮＯ）、ステップＳ２０の自己干渉式判定に戻る。或いは、ステップＳ２２の形状干渉判定を繰り返してもよい。リンク形状同士が干渉していると判定された場合は（ステップＳ２２のＹＥＳ）、自己干渉有りのため、ステップＳ２３においてロボット１０の動作を停止させる。このとき、リンク形状同士の現在の最短距離とリンク形状同士の現在の相対速度を計算し、最短距離と相対速度に基づいてロボット１０を安全停止させてもよい。
　前述した自己干渉判定装置２０は、ロボット制御装置として構成するのではなく、ロボット制御装置に対して種々の指令を送出する上位コンピュータ装置として構成してもよい。図８は他の実施形態のロボットシステム１の機能ブロック図である。ロボットシステム１は、一台の自己干渉判定装置２０と、複数台のロボット制御装置４０と、複数台のロボット１０と、を備えている。各ロボット１０のリンクの現在の状態（例えば現在の位置又は角度、速度等）は各ロボット制御装置４０を介して自己干渉判定装置２０に集約される。
　自己干渉判定装置２０は、各ロボット１０のリンクの現在の状態（現在の位置又は角度、速度等）に基づき、自己干渉式判定（概略判定）と形状干渉判定（詳細判定）を行う。自己干渉判定装置２０は、自己干渉有りと判定した場合は各ロボット制御装置４０に対して各ロボット１０の停止指令を送出する。各ロボット制御装置４０は停止指令に基づいて各ロボット１０の駆動源１８を停止する。
　前述したロボットシステム１や自己干渉判定装置２０の構成は一例であり、他の構成も採用され得ることに留意されたい。例えばロボット１０は、産業用ロボットではなく、ヒューマノイド等の他の形態のロボットでもよい。またロボット１０は、直列した複数のリンクを備えるのではなく、並列した複数のリンクを備えたロボット（例えば双腕ロボット等）について形状干渉判定を適用してもよい。またロボット１０の第七リンク１７は、ハンドではなく、ドライバ、溶接ツール、シーリングツール等の他の形態のツール部分でもよい。
　また自己干渉判定装置２０は、プログラムを実行するプロセッサを備えるのではなく、プログラムを実行しない集積回路（例えばＦＰＧＡ（field-programmable gate array）、ＡＳＩＣ（application specific integrated circuit）等）を備え、当該集積回路が図２に示す「～部」を備えていてもよい。また図６及び図７に示すフローチャートは、プロセッサではなく、当該集積回路で実行されてもよい。また自己干渉判定装置２０は、自己干渉式判定（概略判定）と形状干渉判定（詳細判定）というハイブリッド判定機能を備えるのではなく、形状干渉判定機能のみを備えていてもよい。
　またリンク形状修正部２７は、例えば判定不能リンクである第五リンク１５のリンク形状を指定距離分拡大するのではなく、第五リンク１５と自己干渉する可能性がある第一リンク１１又は第二リンク１２のリンク形状を拡大してもよい。或いは、指定距離内であれば第五リンク１５のリンク形状と第一リンク１１又は第二リンク１２のリンク形状を夫々拡大してもよい。同様に、リンク形状修正部２７は、指定距離が範囲で指定される場合は、例えば判定不能リンクである第五リンク１５のリンク形状を指定された範囲内に収まるように簡略化するのではなく、第五リンク１５と自己干渉する可能性がある第一リンク１１又は第二リンク１２のリンク形状を簡略化してもよいし、又は指定された範囲内であれば第五リンク１５のリンク形状と第一リンク１１又は第二リンク１２のリンク形状を夫々簡略化してもよい。
　なお、自己干渉判定装置２０は、リンク同士が自己干渉式に基づいて自己干渉するとみなされた場合に、これらリンクのいずれか一方がそのリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクであっても、他方のリンクの位置又は角度等によって自己干渉を判定できる場合は、形状干渉判定を行うのではなく、単に他方のリンクの位置又は角度等によって判定不能リンクの自己干渉を判定する干渉判定部を備えていてもよい。
　前述したプロセッサ、他の集積回路等で実行されるプログラム、又は前述したフローチャートを実行するプログラムは、コンピュータ読取り可能な非一時的記録媒体、例えばＣＤ－ＲＯＭ等に記録して提供してもよいし、或いは有線又は無線を介してＷＡＮ（wide area network）又はＬＡＮ（local area network）上のサーバ装置から配信して提供してもよい。
　本明細書において種々の実施形態について説明したが、本発明は、前述の実施形態に限定されるものではなく、特許請求の範囲に記載された範囲内において種々の変更を行えることを認識されたい。
　１　ロボットシステム
　１０　ロボット
　１１　第一リンク
　１２　第二リンク
　１３　第三リンク
　１４　第四リンク
　１５　第五リンク
　１５ａ　拡大したリンク形状
　１５ｂ　簡略化したリンク形状
　１６　第六リンク
　１７　第七リンク
　１８　駆動源
　１３－１４　第三第四リンク
　１５－１７　円柱
　２０　自己干渉判定装置
　２１　自己干渉式定義部
　２２　自己干渉式判定部
　２３　判定不能リンク定義部
　２４　判定不能リンク判定部
　２５　リンク形状取得部
　２６　指定距離取得部
　２７　リンク形状修正部
　２８　形状干渉判定部
　２９　ロボット制御部
　３０、３１　円柱
　３２　球体
　Ｊ１～Ｊ６　軸線

Claims (10)

  1. 　複数のリンクを備えたロボットのリンク同士が自己干渉したとみなすリンク形状間の指定距離に基づいてリンク形状を修正するリンク形状修正部と、
    　前記リンク形状に基づいて前記自己干渉の有無を判定する形状干渉判定部と、
    　を備える、自己干渉判定装置。
  2. 　前記リンク形状修正部は前記リンク形状を前記指定距離分拡大する、請求項１に記載の自己干渉判定装置。
  3. 　前記指定距離が範囲で指定された場合、前記リンク形状修正部は前記リンク形状が指定された前記範囲内に収まるように前記リンク形状を簡略化する、請求項１又は２に記載の自己干渉判定装置。
  4. 　複数のリンクを備えたロボットの特定のリンク以外のリンクの位置又は角度に拘わらず前記特定のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクについてはリンク形状同士が指定距離以下まで接近したか否かに基づいて前記自己干渉の有無を判定する形状干渉判定部を備える、自己干渉判定装置。
  5. 　前記指定距離を取得する指定距離取得部をさらに備える、請求項１から４のいずれか一項に記載の自己干渉判定装置。
  6. 　前記リンク形状を取得するリンク形状取得部をさらに備える、請求項１から５のいずれか一項に記載の自己干渉判定装置。
  7. 　前記リンク形状同士の現在の最短距離とリンク形状同士の現在の相対速度を計算し、前記最短距離と前記相対速度に基づいて前記ロボットを安全停止させる安全停止指令を送出するロボット停止指令部をさらに備える、請求項１から６のいずれか一項に記載の自己干渉判定装置。
  8. 　複数のリンクを備えたロボットが自己干渉する可能性を判定する自己干渉式を定義する自己干渉式定義部と、
    　前記自己干渉式に基づいて前記自己干渉の可能性を判定する自己干渉式判定部と、
    　を備え、
    　前記自己干渉式定義部は、特定のリンク以外のリンクの位置又は角度に拘わらず前記特定のリンクの位置又は角度のみで前記自己干渉の有無を判定できない判定不能リンクについては前記ロボットの根元側の前記判定不能リンクから前記ロボットの先端側の前記判定不能リンクまでの全てのリンクの可動領域を包含する形状に基づいて前記自己干渉式を定義する、自己干渉判定装置。
  9. 　自己干渉する可能性がある前記判定不能リンクを定義する判定不能リンク定義部と、
    　前記判定不能リンクが実際に自己干渉する可能性が有るか否かを判定する判定不能リンク判定部と、
    　をさらに備える、請求項８に記載の自己干渉判定装置。
  10. 　第一リンクと第二リンクの自己干渉を判定する自己干渉判定装置であって、
    　前記第一リンクと前記第二リンクが自己干渉するとみなされた場合に前記第一リンクと前記第二リンクのいずれか一方が前記一方のリンクの位置又は角度のみで自己干渉の有無を判定できない判定不能リンクであるか否かを判定する判定不能リンク判定部と、
    　前記判定結果に基づいて前記判定不能リンクの自己干渉を判定する干渉判定部と、
    　を備える自己干渉判定装置。
PCT/JP2021/041614 2020-11-18 2021-11-11 自己干渉判定装置 WO2022107683A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2022563718A JPWO2022107683A1 (ja) 2020-11-18 2021-11-11
DE112021004791.4T DE112021004791T5 (de) 2020-11-18 2021-11-11 Eigenstörungsbestimmungsvorrichtung
CN202180068149.1A CN116261505A (zh) 2020-11-18 2021-11-11 自干扰判定装置
US18/042,219 US20230321832A1 (en) 2020-11-18 2021-11-11 Self-interference determination device

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020191881 2020-11-18
JP2020-191881 2020-11-18

Publications (1)

Publication Number Publication Date
WO2022107683A1 true WO2022107683A1 (ja) 2022-05-27

Family

ID=81708862

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/041614 WO2022107683A1 (ja) 2020-11-18 2021-11-11 自己干渉判定装置

Country Status (6)

Country Link
US (1) US20230321832A1 (ja)
JP (1) JPWO2022107683A1 (ja)
CN (1) CN116261505A (ja)
DE (1) DE112021004791T5 (ja)
TW (1) TW202221433A (ja)
WO (1) WO2022107683A1 (ja)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH01224811A (ja) * 1988-03-04 1989-09-07 Fanuc Ltd ロボットの干渉チェック方法
WO2008031664A1 (en) * 2006-09-14 2008-03-20 Abb Research Ltd. A method and a device for avoiding collisions between an industrial robot and an object
JP2008279524A (ja) * 2007-05-08 2008-11-20 Ihi Corp 複数ロボットの衝突検知方法及びロボット装置
JP2010052114A (ja) * 2008-08-29 2010-03-11 Mitsubishi Electric Corp 干渉チェック制御装置および干渉チェック制御方法
JP2011131303A (ja) * 2009-12-22 2011-07-07 Honda Motor Co Ltd 干渉チェック装置
JP2018134703A (ja) * 2017-02-21 2018-08-30 株式会社安川電機 ロボットシミュレータ、ロボットシステム及びシミュレーション方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH01224811A (ja) * 1988-03-04 1989-09-07 Fanuc Ltd ロボットの干渉チェック方法
WO2008031664A1 (en) * 2006-09-14 2008-03-20 Abb Research Ltd. A method and a device for avoiding collisions between an industrial robot and an object
JP2008279524A (ja) * 2007-05-08 2008-11-20 Ihi Corp 複数ロボットの衝突検知方法及びロボット装置
JP2010052114A (ja) * 2008-08-29 2010-03-11 Mitsubishi Electric Corp 干渉チェック制御装置および干渉チェック制御方法
JP2011131303A (ja) * 2009-12-22 2011-07-07 Honda Motor Co Ltd 干渉チェック装置
JP2018134703A (ja) * 2017-02-21 2018-08-30 株式会社安川電機 ロボットシミュレータ、ロボットシステム及びシミュレーション方法

Also Published As

Publication number Publication date
TW202221433A (zh) 2022-06-01
CN116261505A (zh) 2023-06-13
US20230321832A1 (en) 2023-10-12
DE112021004791T5 (de) 2023-07-27
JPWO2022107683A1 (ja) 2022-05-27

Similar Documents

Publication Publication Date Title
Huo et al. The joint‐limits and singularity avoidance in robotic welding
Ju et al. Kinematics modeling and experimental verification of baxter robot
JP5025598B2 (ja) 干渉チェック制御装置および干渉チェック制御方法
KR102261469B1 (ko) 로봇 제어 장치, 로봇의 관절의 각도를 구하는 방법, 프로그램
Xu et al. Dual arm-angle parameterisation and its applications for analytical inverse kinematics of redundant manipulators
Bounab Multi-objective optimal design based kineto-elastostatic performance for the delta parallel mechanism
CN110480624B (zh) 多关节机器人的干扰判定装置、干扰判定方法、记录介质
WO2020149021A1 (ja) 経路生成装置、経路生成方法、及び経路生成プログラム
JP2009274148A (ja) シミュレーション装置
Zhao et al. Vibration error-based trajectory planning of a 5-dof hybrid machine tool
Nicolescu et al. Forward and inverse kinematics study of industrial robots taking into account constructive and functional parameter's modeling
WO2022107683A1 (ja) 自己干渉判定装置
US20220105627A1 (en) Singularity-free kinematic parameterization of soft robot manipulators
WO2020149020A1 (ja) ロボット制御装置、ロボット制御方法、及びロボット制御プログラム
Ghaffari et al. Kinematic design of a novel 4-DOF parallel mechanism for turbine blade machining
JP2017131990A (ja) 干渉回避方法
Simas et al. Extended jacobian for redundant robots obtained from the kinematics constraints
Seyboldt et al. Sampling-based path planning to cartesian goal positions for a mobile manipulator exploiting kinematic redundancy
JP2597979B2 (ja) ロボツト制御データの干渉チェック方法
Briot et al. On the optimal design of parallel robots taking into account their deformations and natural frequencies
US11697206B2 (en) Acceleration adjustment apparatus and non-transitory computer-readable storage medium storing an acceleration adjustment program
WO2021144911A1 (ja) プログラム生成装置、プログラム生成方法、及び生成プログラム
Filiposka et al. Complexity analysis for calculating the Jacobian matrix of 6DOF reconfigurable machines
JP2021186929A (ja) 多軸ロボットの制御方法
Somasundar et al. Singularity analysis of Kuka 6 DOF robot for motion simulation

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21894557

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022563718

Country of ref document: JP

Kind code of ref document: A

122 Ep: pct application non-entry in european phase

Ref document number: 21894557

Country of ref document: EP

Kind code of ref document: A1