JP5857803B2 - Industrial machine interference determination device, interference determination method, computer program, and recording medium - Google Patents
Industrial machine interference determination device, interference determination method, computer program, and recording medium Download PDFInfo
- Publication number
- JP5857803B2 JP5857803B2 JP2012050236A JP2012050236A JP5857803B2 JP 5857803 B2 JP5857803 B2 JP 5857803B2 JP 2012050236 A JP2012050236 A JP 2012050236A JP 2012050236 A JP2012050236 A JP 2012050236A JP 5857803 B2 JP5857803 B2 JP 5857803B2
- Authority
- JP
- Japan
- Prior art keywords
- model
- interference
- separation axis
- rectangular parallelepiped
- plane
- Prior art date
- Legal status (The legal status 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 status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 176
- 238000004590 computer program Methods 0.000 title claims description 6
- 238000000926 separation method Methods 0.000 claims description 234
- 239000013598 vector Substances 0.000 claims description 213
- 230000029777 axis specification Effects 0.000 claims description 35
- 238000010586 diagram Methods 0.000 description 14
- 230000006870 function Effects 0.000 description 6
- 230000012447 hatching Effects 0.000 description 3
- 210000000707 wrist Anatomy 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 239000012636 effector Substances 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
本発明は、産業用機械と障害物との干渉を判定する技術に関する。 The present invention relates to a technique for determining interference between an industrial machine and an obstacle.
ロボットの実働時や教示時あるいは動作シミュレーション時に、ロボットと障害物(設備)とが干渉することを防止するため、種々の技術が提案されている。例えば、特許文献1には、水平多関節ロボットの3次元モデルと障害物の3次元モデルとをそれぞれ水平面に射影し、その水平面上でロボットと障害物との干渉を判定する技術が記載されている。このように、物体間(ロボットと障害物間)の干渉を判定するためのアルゴリズムとして、その他、例えば、物体間の距離に基づいて干渉を判定する方法や、物体を直方体に分割してその直方体同士で干渉の判定を行う方法、物体表面をポリゴンで表し、ポリゴン同士で干渉を判定する方法、物体と物体とを分離軸と呼ばれる直線上に射影してその分離軸上での像の重なりに応じて干渉の判定を行う方法などがある。
Various techniques have been proposed to prevent the robot and an obstacle (equipment) from interfering during actual operation, teaching, or motion simulation of the robot. For example,
しかし、特許文献1に記載の技術では、ロボットの形態が水平多関節型に限定されてしまうため、複雑な構成のロボットに適用することができないという問題があった。また、物体間の距離に基づいて干渉を判定する方法や、直方体同士で干渉を判定する方法、ポリゴン同士で干渉を判定する方法では、ロボットや設備の形状が複雑になるほど処理負担が増大し、干渉の判定に時間がかかるという問題があった。また、分離軸による判定では、物体の形状によっては、最適な分離軸を決定することが困難な場合があった。これらの問題は、ロボットに限らず、可動部を有する産業用機械と障害物との干渉を判定する際に共通した問題であった。
However, the technique described in
これらの問題を考慮し、本発明が解決しようとする課題は、産業用機械の可動部と障害物との干渉を、迅速かつ高精度に判定可能な技術を提供することである。 In view of these problems, the problem to be solved by the present invention is to provide a technique capable of quickly and accurately determining interference between a movable part of an industrial machine and an obstacle.
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態又は適用例として実現することが可能である。 SUMMARY An advantage of some aspects of the invention is to solve at least a part of the problems described above, and the invention can be implemented as the following forms or application examples.
[適用例1]可動部を有する産業用機械と障害物との干渉を判定する干渉判定装置であって、前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、が記憶された記憶部と、前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得部と、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルとを含む第1の平面を求め、該第1の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第1の分離軸として決定する分離軸決定部と、前記第1の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第1の平面上の前記第1の分離軸に、前記第1の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第1の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定部と、を備える干渉判定装置。 Application Example 1 An interference determination device that determines interference between an industrial machine having a movable part and an obstacle, and includes at least a part of the movable part and at least a part of the obstacle. A storage unit storing data of a cylindrical model in which one shape is represented by a rectangular cylinder and data of a rectangular parallelepiped model in which the other shape is represented by a rectangular parallelepiped, a position of the shape represented by the cylindrical model, and the rectangular parallelepiped A position acquisition unit that acquires a position of a shape represented by the model, a normal vector of an upper surface or a lower surface of the cylindrical model, and a vector of any one of a surface normal vector or an edge vector of the rectangular parallelepiped model A separation axis determination unit that obtains a first plane including a straight line parallel to a normal vector of the side surface of the cylindrical model on the first plane as a first separation axis; and The cylindrical model and the rectangular parallelepiped model are projected, and the cylindrical model image projected onto the first plane and the rectangular parallelepiped model image onto the first separation axis on the first plane. An interference determination unit that determines the interference between the movable unit and the obstacle based on whether or not the cylindrical model image projected onto the first separation axis and the rectangular parallelepiped model image overlap each other An interference determination device comprising:
このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルと直方体モデルの面法線ベクトル(または辺ベクトル)のうちのいずれか1のベクトルとを含む第1の平面を求め、この第1の平面上における円柱モデルの側面の法線ベクトルに平行な直線を、第1の分離軸として求める。そのため、360°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができ、これにより効率的に直方体モデルと円柱モデルの側面との干渉を判定することができる。このように、上記構成の干渉判定装置では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、産業用機械の可動部と障害物とのリアルタイムの干渉判定が容易となる。また、上記構成によれば、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、産業用機械の可動部の外形形状を、直方体モデルなどの単純なモデルだけではなく、円柱モデルを用いて精度良くモデリングすることができる。よって、実際の産業用機械の形状に即した干渉の判定が可能となるので、狭小空間での干渉の判定など、精度が求められる干渉の判定も容易に行うことが可能になる。また、上記構成の干渉判定装置では、第1の平面に対して円柱モデルと直方体モデルとを射影し、更に、第1の平面上の第1の分離軸に第1の平面に対して射影された円柱モデルの像と直方体モデルの像とを射影し、その射影の結果に基づいて、産業用機械の可動部と障害物との干渉を判定する。つまり、3次元のモデルを、2次元の平面を介して1次元の分離軸に射影するため、分離軸への各モデルの射影を精度良く行うことが可能になる。 In the interference determination apparatus having such a configuration, a first plane including a normal vector of the upper surface or the lower surface of the cylindrical model and any one of the surface normal vectors (or side vectors) of the rectangular parallelepiped model is obtained. Then, a straight line parallel to the normal vector of the side surface of the cylindrical model on the first plane is obtained as the first separation axis. Therefore, it is possible to reliably specify the normal vector used as the separation axis from the normal vectors of the side surfaces of the cylindrical model that exist innumerably over 360 °, and thereby, the rectangular parallelepiped model and the cylindrical model can be efficiently identified. Interference with the side surface can be determined. As described above, the interference determination apparatus having the above configuration can quickly determine the interference even when the cylindrical model is used, so the real-time interference determination between the movable part of the industrial machine and the obstacle is possible. Becomes easy. In addition, according to the above configuration, even when a cylindrical model is used, interference can be determined quickly, so that the outer shape of the movable part of the industrial machine is only a simple model such as a rectangular parallelepiped model. Instead, it can be accurately modeled using a cylindrical model. Therefore, since it is possible to determine interference according to the shape of an actual industrial machine, it is possible to easily perform determination of interference that requires accuracy, such as determination of interference in a narrow space. In the interference determination apparatus having the above configuration, the cylindrical model and the rectangular parallelepiped model are projected onto the first plane, and further projected onto the first plane on the first separation axis on the first plane. The cylindrical model image and the rectangular parallelepiped model image are projected, and interference between the movable part of the industrial machine and the obstacle is determined based on the projection result. That is, since a three-dimensional model is projected onto a one-dimensional separation axis via a two-dimensional plane, each model can be projected onto the separation axis with high accuracy.
[適用例2]適用例1に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか1のベクトルとの外積ベクトルを求め、該外積ベクトルと前記円柱モデルの上面または下面の法線ベクトルとを含む第2の平面を求め、該第2の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第2の分離軸として決定し、前記干渉判定部は、更に、前記第2の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第2の平面上の前記第2の分離軸に、前記第2の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第2の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 Application Example 2 In the interference determination apparatus according to Application Example 1, the separation axis determination unit may further include a normal vector of an upper surface or a lower surface of the cylindrical model and an edge vector or a surface normal of the rectangular parallelepiped model. An outer product vector with any one of the vectors is obtained, a second plane including the outer product vector and a normal vector of the upper surface or the lower surface of the cylinder model is obtained, and the cylinder on the second plane is obtained. A straight line parallel to the normal vector of the side surface of the model is determined as a second separation axis, and the interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the second plane, and The cylindrical model image and the rectangular parallelepiped model image projected onto the second plane are projected onto the second separation axis on the second plane, and projected onto the second separation axis. The cylindrical model image and the Based on the presence or absence of an overlap between the image of the rectangular parallelepiped model is determined interference with the obstacle and the movable portion, the interference determination unit.
このような構成の干渉判定装置によれば、円柱モデルの上面または下面の法線ベクトルと、直方体モデルの辺ベクトル(または面法線ベクトル)のうちのいずれか1のベクトルとの外積ベクトルを求めるとともに、この外積ベクトルと円柱モデルの上面または下面の法線ベクトルとを含む第2の平面を求め、この第2の平面上における円柱モデルの側面の法線ベクトルに平行な第2の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、第1の分離軸とは異なる観点から、円柱モデルによって表された可動部の側面と直方体モデルによって表された障害物との干渉を判定することができる。 According to the interference determination apparatus having such a configuration, an outer product vector of the normal vector of the upper surface or the lower surface of the cylindrical model and any one of the side vectors (or surface normal vectors) of the rectangular parallelepiped model is obtained. In addition, a second plane including the outer product vector and the normal vector of the upper or lower surface of the cylinder model is obtained, and a second separation axis parallel to the normal vector of the side surface of the cylinder model on the second plane is obtained. On the other hand, a cylindrical model and a rectangular parallelepiped model are projected. Therefore, it is possible to determine the interference between the side surface of the movable part represented by the cylindrical model and the obstacle represented by the rectangular parallelepiped model from a viewpoint different from the first separation axis.
[適用例3]適用例1または適用例2に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか1のベクトルとの外積ベクトルを求め、該外積ベクトルに平行な直線を、第3の分離軸として決定し、前記干渉判定部は、更に、前記第3の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第3の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 Application Example 3 In the interference determination apparatus according to Application Example 1 or Application Example 2, the separation axis determination unit further includes a normal vector of the upper surface or the lower surface of the cylindrical model and an edge vector of the rectangular parallelepiped model. Alternatively, an outer product vector with any one of the surface normal vectors is obtained, a straight line parallel to the outer product vector is determined as a third separation axis, and the interference determination unit further includes the third Projecting the cylindrical model and the cuboid model on a separation axis, and based on whether or not the image of the cylindrical model projected on the third separation axis and the image of the cuboid model overlap, the movable portion and the An interference determination device that determines interference with an obstacle.
このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルと、直方体モデルの辺ベクトル(または面法線ベクトル)のうちのいずれか1のベクトルとの外積ベクトルを求め、この外積ベクトルに平行な第3の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部の外形と直方体モデルによって表された障害物の外形との干渉を判定することができる。 In the interference determination device having such a configuration, an outer product vector of a normal vector of the upper surface or the lower surface of the cylindrical model and any one of the side vectors (or surface normal vectors) of the rectangular parallelepiped model is obtained. Project a cylindrical model and a rectangular parallelepiped model onto a third separation axis parallel to the outer product vector. Therefore, it is possible to determine the interference between the outer shape of the movable part represented by the cylindrical model and the outer shape of the obstacle represented by the rectangular parallelepiped model.
[適用例4]適用例1から適用例3までのいずれか一項に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルに平行な直線を、第4の分離軸として決定し、前記干渉判定部は、更に、前記第4の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第4の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 [Application Example 4] The interference determination apparatus according to any one of Application Example 1 to Application Example 3, wherein the separation axis determination unit further includes a surface normal vector or an edge vector of the rectangular parallelepiped model. A straight line parallel to any one of the vectors is determined as a fourth separation axis, and the interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the fourth separation axis, and An interference determination device that determines interference between the movable portion and the obstacle based on whether or not the image of the cylindrical model projected onto the four separation axes overlaps the image of the rectangular parallelepiped model.
このような構成の干渉判定装置では、直方体モデルの面法線ベクトル(または辺ベクトル)のうちのいずれか1のベクトルに平行な第4の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部と直方体モデルによって表された障害物の面との干渉を判定することができる。 In the interference determination apparatus having such a configuration, the cylindrical model and the rectangular parallelepiped model are projected onto the fourth separation axis parallel to any one of the surface normal vectors (or side vectors) of the rectangular parallelepiped model. . Therefore, it is possible to determine the interference between the movable part represented by the cylindrical model and the surface of the obstacle represented by the rectangular parallelepiped model.
[適用例5]適用例1から適用例4までのいずれか一項に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルに平行な直線を第5の分離軸として決定し、前記干渉判定部は、更に、前記第5の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第5の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 [Application Example 5] The interference determination apparatus according to any one of Application Example 1 to Application Example 4, wherein the separation axis determination unit is further parallel to a normal vector on an upper surface or a lower surface of the cylindrical model. A straight line is determined as a fifth separation axis, and the interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the fifth separation axis, and projects the projection onto the fifth separation axis. An interference determination device that determines interference between the movable portion and the obstacle based on whether or not an image of a cylindrical model and an image of the rectangular parallelepiped model overlap.
このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルに平行な第5の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部の上面や下面と直方体モデルによって表された障害物の外形との干渉を判定することができる。 In the interference determination apparatus having such a configuration, the cylindrical model and the rectangular parallelepiped model are projected onto the fifth separation axis parallel to the normal vector of the upper surface or the lower surface of the cylindrical model. Therefore, it is possible to determine the interference between the upper and lower surfaces of the movable part represented by the cylindrical model and the outer shape of the obstacle represented by the rectangular parallelepiped model.
[適用例6]適用例1を引用する適用例2、を引用する適用例3、を引用する適用例4、を引用する適用例5に記載の干渉判定装置であって、前記分離軸決定部は、前記第5の分離軸、前記第4の分離軸、前記第1の分離軸、前記第2の分離軸、前記第3の分離軸、の順序で分離軸の決定を行い、前記干渉判定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸を用いた干渉の判定を行わず、前記分離軸決定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸の決定を行わない、干渉判定装置。 [Application Example 6] The interference determination apparatus according to Application Example 5 that cites Application Example 3 that cites Application Example 2 that cites Application Example 1 and that cites Application Example 4 that cites Application Example 3, and that separates the separation axis determination unit. Determining the separation axis in the order of the fifth separation axis, the fourth separation axis, the first separation axis, the second separation axis, and the third separation axis, and determining the interference The unit does not perform interference determination using the separation axis in the order after the separation axis determined that the movable part and the obstacle do not interfere with each other, and the separation axis determination unit includes the movable part and the obstacle. An interference determination apparatus that does not determine a separation axis in an order subsequent to a separation axis that is determined not to interfere with an obstacle.
このような構成の干渉判定装置によれば、分離軸を決定するための計算量の少ない順に、干渉の判定に用いられる分離軸が決定され、しかも、可動部と障害物とが干渉していないと一旦判定された場合には、それ以降の分離軸の決定と干渉の判定が行われない。そのため、高速に干渉の判定を行うことができる。 According to the interference determination apparatus having such a configuration, the separation axis used for the determination of interference is determined in the order from the smallest calculation amount for determining the separation axis, and the movable part and the obstacle do not interfere with each other. Is once determined, the subsequent determination of the separation axis and the determination of interference are not performed. Therefore, it is possible to determine interference at high speed.
本発明は、上述した干渉判定装置としての構成のほか、干渉判定方法や、コンピュータプログラムとしても構成することができる。かかるコンピュータプログラムは、コンピュータが読取可能な記録媒体に記録されていてもよい。 In addition to the configuration as the interference determination device described above, the present invention can also be configured as an interference determination method or a computer program. Such a computer program may be recorded on a computer-readable recording medium.
A.装置構成:
図1は、本発明の一実施形態としての干渉判定装置400を含むロボットシステム10の概略構成を示す説明図である。ロボットシステム10は、多関節型の産業用ロボット100と、ロボット100の動作を制御する制御装置200と、ロボット100の動作を制御装置200に教示する教示装置300と、ロボット100と障害物500との干渉を判定する干渉判定装置400と、を含んでいる。ロボット100と教示装置300と干渉判定装置400とは、それぞれ、所定のケーブルによって制御装置200に接続されている。
A. Device configuration:
FIG. 1 is an explanatory diagram showing a schematic configuration of a
ロボット100は、工場内の設備に固定されるベース部101を備えている。ベース部101には、水平方向に旋回可能な関節軸によってベース部101に支持されたショルダ部102と、鉛直方向に旋回可能な関節軸によってショルダ部102に下端が支持された下アーム103と、鉛直方向に旋回可能な関節軸によって下アーム103の先端に略中央部が支持された上アーム104と、鉛直方向に旋回可能な関節軸によって上アーム104の先端に支持された手首105と、手首105の先端に設けられた回転可能なフランジ部106と、フランジ部106に取り付けられたエンドエフェクタ107と、が可動部として備えられている。
The
図2は、干渉判定装置400によって行われる干渉判定処理の概要を示す説明図である。本実施形態の干渉判定装置400は、分離軸判定という手法で干渉の判定を行う。図2(A)に示されているように、分離軸判定では、2つの物体が干渉しているか否かを判定するために、まず、ある分離軸が決定され、その分離軸に2つの物体が射影される。そして、その射影によって分離軸上に表れる2つの像が重ならなければ、2つの物体は干渉していないと判定される。一方、図2(B)に示されているように、2つの像が重なれば、その分離軸では、2つの物体が干渉していないと判定することができない。そこで、他の分離軸が選択され、選択された他の分離軸への射影の結果に基づいて、再度、干渉の判定が行われる。分離軸判定では、選択された分離軸うち、2つの像が重ならない分離軸が1つでも存在すれば、2つの物体は干渉していないと判定される。これに対して、選択されたすべての分離軸で2つの像が重なれば、2つの物体は干渉していると判定される。
FIG. 2 is an explanatory diagram illustrating an outline of the interference determination process performed by the
図3は、干渉判定装置400の概略構成を示す説明図である。干渉判定装置400は、CPU410と、RAM420と、HDD(ハードディスクドライブ)430と、モニタ440と、インタフェース450と、を備えている。これらは、バス460を介してそれぞれ相互に接続されている。インタフェース450には、制御装置200が接続される。干渉判定装置400は、例えば、パーソナルコンピュータによって構成することができる。
FIG. 3 is an explanatory diagram illustrating a schematic configuration of the
HDD430には、ロボット100および障害物500の外形形状を表すモデルデータ432が記憶されている。本実施形態では、ロボット100の可動部の少なくとも一部の外形形状が、直円柱形状を表す円柱モデルによって表されている。また、障害物500の外形形状は、直方体の形状を表す直方体モデルによって表されている。また、HDD430には、ロボット100と障害物500との干渉の判定を実現するための干渉判定プログラム434が記憶されている。なお、これらのモデルデータ432および干渉判定プログラム434は、HDD430に限られず、フレキシブルディスクやCD−ROM、DVD−ROM、光磁気ディスク、フラッシュメモリ等の記録媒体に記録されていてもよい。
The
CPU410は、HDD430に記憶された干渉判定プログラム434をRAM420にロードして実行することで、位置取得部412、分離軸決定部414、干渉判定部416として機能する。
The
位置取得部412は、制御装置200からロボット100の各可動部の位置を取得するとともに、予めHDD430に設定された障害物500の位置を取得する機能を有する。
The
分離軸決定部414は、ロボット100の可動部と障害物500との干渉を検出するための分離軸の決定を行う機能を有する。具体的な分離軸の決定方法については後述する。
The separation
干渉判定部416は、分離軸決定部414によって決定された分離軸にロボット100のモデル(円柱モデル)と障害物のモデル(直方体モデル)とを射影して、ロボット100の可動部と障害物500との干渉を判定する機能を有する。具体的な干渉の判定方法については後述する。
The
なお、本実施形態では、干渉判定装置400と制御装置200とをそれぞれ異なる装置として構成としたが、干渉判定装置400の機能をすべて制御装置200に組み込むこととしてもよい。
In the present embodiment, the
B.干渉判定処理:
図4は、CPU410によって干渉判定プログラム434が実行されることによって繰り返し実行される干渉判定処理のフローチャートである。CPU410によって干渉判定プログラム434が実行されると、まず、位置取得部412が、ロボット100の各軸の回転角度を制御装置200から取得する(ステップS10)。位置取得部412は、ロボット100の各軸の回転角度を取得すると、その回転角度から、各リンク座標系、ツール座標系におけるロボット100の各可動部の位置および姿勢を計算する(ステップS12)。ロボット100の位置および姿勢を計算すると、位置取得部412は、干渉判定の対象となる可動部および障害物を特定する(ステップS14)。判定対象を特定すると、位置取得部412は、特定された判定対象の中心座標系における判定対象の位置および姿勢を計算する(ステップS16)。
B. Interference judgment processing:
FIG. 4 is a flowchart of an interference determination process that is repeatedly executed by the
判定対象の位置および姿勢を計算すると、位置取得部412は、判定対象の可動部が円柱モデルであり、かつ、障害物が直方体モデルであるかを判断する(ステップS18)。判定対象が円柱モデルおよび直方体モデルでなければ(ステップS18:NO)、CPU410は、判定対象の種類に応じた判定処理を行う(ステップS20)。例えば、判定対象が、点モデルと角柱モデルとであれば、角柱に点が内包されるか否かに基づき干渉の判定が行われ、判定対象が、直方体モデルと角柱モデルとであれば、直方体が角柱の内側に存在するか否かに基づき干渉の判定が行われる。
When the position and orientation of the determination target are calculated, the
判定対象の可動部が円柱モデルであり、障害物が直方体モデルであると判断された場合には(ステップS18:YES)、分離軸決定部414および干渉判定部416が、第1サブ判定処理(ステップS22)と、第2サブ判定処理(ステップS24)と、第3サブ判定処理(ステップS26)と、第4サブ判定処理(ステップS28)と、第5サブ判定処理(ステップS30)と、をこの順で順番に実行する。第1サブ判定処理から第5サブ判定処理までのすべての判定処理において、「干渉なし」と判定されなければ、干渉判定部416は、「干渉あり」と判定し、モニタ440にその判定結果(干渉あり)を表示する(ステップS32)。これに対して、詳細は後述するが、第1サブ判定処理から第5サブ判定処理までの判定処理のうち、いずれかの判定処理で「干渉なし」と判定されれば、それ以降の判定処理は実行されることなく、干渉判定部416により、モニタ440にその判定結果(干渉なし)が表示され、当該干渉判定処理は終了する。
When it is determined that the movable part to be determined is a cylindrical model and the obstacle is a rectangular parallelepiped model (step S18: YES), the separation
C.第1サブ判定処理:
図5は、第1サブ判定処理の詳細なフローチャートである。第1サブ判定処理は、円柱モデルの上面および下面と、直方体の外形(辺)との干渉を判定するための処理である。第1サブ判定処理が開始されると、分離軸決定部414は、円柱モデルの上面の法線ベクトルに平行な直線を分離軸として決定する(ステップS100。図6(D)参照)。第1サブ判定処理によって決定された分離軸は、特許請求の範囲における「第5の分離軸」に相当する。
C. First sub-determination process:
FIG. 5 is a detailed flowchart of the first sub determination process. The first sub determination process is a process for determining interference between the upper and lower surfaces of the cylindrical model and the outer shape (side) of the rectangular parallelepiped. When the first sub-determination process is started, the separation
分離軸が決定されると、干渉判定部416は、決定された分離軸に、円柱モデルと直方体モデルとを射影し(ステップS102)、分離軸上に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する(ステップS104)。
When the separation axis is determined, the
分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には(ステップS104:YES)、円柱モデルの上面または下面と、直方体の外形とが干渉している可能性があるため、更に詳しく干渉の判定を行うために、干渉判定部416は、当該第1サブ判定処理を終了させ、図4に示した第2サブ判定処理(ステップS24)に処理を移行させる。これに対して、分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には(ステップS104:NO)、図4に示した第2〜5サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、干渉判定部416は、そのまま、「干渉なし」と判断して、モニタ440にその判定結果(干渉なし)を表示させる(ステップS106)。モニタ440に判定結果を表示させると、干渉判定部416は、図4に示した一連の干渉判定処理を終了させる。
If the image of the cylinder model and the image of the cuboid model overlap on the separation axis (step S104: YES), the upper or lower surface of the cylinder model and the outer shape of the cuboid may interfere with each other. In order to perform interference determination in detail, the
図6は、上述した第1サブ判定処理によって決定される分離軸の例を示す図である。(図6(A)〜(D)には、円柱モデルと直方体モデルの位置関係を異なる視点から観察した様子が示されている。図6(A)には直方体モデルを+X方向側から見た図が、図6(B)には直方体モデルを+Y方向から見た図が、図6(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図6(D)には、円柱モデルの上面の法線ベクトルに平行な分離軸AX1に、円柱モデルと直方体モデルとが射影された様子が示されている。図6(D)に示された例では、分離軸AX1上に射影された像同士が重なっていないため、「干渉なし」と判定される。 FIG. 6 is a diagram illustrating an example of the separation axis determined by the first sub determination process described above. (FIGS. 6A to 6D show a state in which the positional relationship between the cylindrical model and the rectangular parallelepiped model is observed from different viewpoints. FIG. 6A shows the rectangular parallelepiped model viewed from the + X direction side. 6B shows a view of the cuboid model viewed from the + Y direction, and FIG. 6C shows a view of the cuboid model viewed from the + Z direction. 6 shows the projection of the cylindrical model and the rectangular parallelepiped model on the separation axis AX1 parallel to the normal vector on the upper surface of the cylindrical model.In the example shown in FIG. Since the images projected on AX1 do not overlap, it is determined that there is no interference.
D.第2サブ判定処理:
図7は、第2サブ判定処理の詳細なフローチャートである。第2サブ判定処理は、第1サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、直方体モデルの面と円柱モデルの外形との干渉を判定するための処理である。第2サブ判定処理が開始されると、分離軸決定部414は、直方体モデルの3種類の面法線ベクトルの中から1つの面法線ベクトルを選択し(ステップS200)、選択された面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する(ステップS202)。このような平面を作成すると、分離軸決定部414は、作成された平面上で、選択された面法線ベクトルに平行な直線を分離軸として決定する(ステップS204。図8(B)、図9(A)、図10(B)参照)。第2サブ判定処理で決定された分離軸は、特許請求の範囲における「第4の分離軸」に相当する。なお、直方体モデルの3種類の面法線ベクトルとは、直方体モデルの計6本の面法線ベクトルのうち、逆向きの2本の面法線ベクトルを1種類と捉えたものである。
D. Second sub-determination process:
FIG. 7 is a detailed flowchart of the second sub determination process. The second sub-determination process is a process that is executed when “no interference” is not determined in the first sub-determination process, and is a process for determining interference between the surface of the rectangular parallelepiped model and the outer shape of the cylindrical model. is there. When the second sub determination processing is started, the separation
分離軸が決定されると、干渉判定部416は、ステップS202で作成された平面に、円柱モデルと直方体モデルとを射影する(ステップS206)。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップS204で決定された分離軸に射影する(ステップS208)。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部416は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する(ステップS210)。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には(ステップS210:NO)、図4に示した第3〜5サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ440にその判定結果(干渉なし)を表示させる(ステップS208)。モニタ440に判定結果を表示させると、干渉判定部416は、図4に示した一連の干渉判定処理を終了させる。
When the separation axis is determined, the
分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には(ステップS210:YES)、干渉判定部416は、更に詳しく干渉の判定を行うため、直方体モデルの3種類の全ての面法線ベクトルを用いて当該第2サブ判定処理における干渉の判定を行ったか否かを判断する(ステップS214)。3種類の全ての面法線ベクトルを用いて干渉の判定を行っていない場合には(ステップS214:NO)、干渉判定部416は、処理をステップS200に戻す。そうすると、3種類の面法線ベクトルのうちのまだ選択されていない面法線ベクトルを用いた干渉の判定が行われる。これに対して、3種類の全ての面法線ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には(ステップS214:YES)、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部416は、当該第2判定処理を終了させて、図4に示した第3サブ判定処理(ステップS26)に処理を移行させる。
When the image of the cylindrical model and the image of the cuboid model overlap on the separation axis (step S210: YES), the
なお、本実施形態の第2サブ判定処理では、まず、平面を作成し、その平面に対して各モデルを射影した上で、更に平面上の各モデルの像を分離軸に射影している。これに対して、第2サブ判定処理では、平面を作成することなく、直方体モデルの各面法線ベクトルに平行な分離軸に、直接的に各モデルを射影してもよい。本実施形態のように、平面を作成してその平面上に各モデルを射影すれば、その平面に各モデル(特に、円柱モデル)を精度良く射影することができるので、分離軸に対しても各モデルを精度良く射影することができる。一方、分離軸に対して直接的に各モデルを射影することとすれば、平面を作成する処理が省略されるので、処理時間を短縮することが可能になる。なお、図5に示した第1サブ判定処理において平面を作成しないのは、第1サブ判定処理では、円柱モデルの上面の法線ベクトルに平行な直線、つまり、円柱の中心軸に平行な直線が分離軸として決定されるため、円柱モデルはその分離軸に対して常に精度良く平行に射影することが可能だからである。 In the second sub-determination process of the present embodiment, a plane is first created, each model is projected onto the plane, and an image of each model on the plane is projected onto the separation axis. On the other hand, in the second sub-determination process, each model may be directly projected onto a separation axis parallel to each surface normal vector of the rectangular parallelepiped model without creating a plane. As in this embodiment, if a plane is created and each model is projected onto the plane, each model (especially a cylindrical model) can be projected onto the plane with high accuracy. Each model can be accurately projected. On the other hand, if each model is projected directly onto the separation axis, the process of creating a plane is omitted, and the processing time can be shortened. The reason why the plane is not created in the first sub-determination process shown in FIG. 5 is that, in the first sub-determination process, a straight line parallel to the normal vector on the upper surface of the cylinder model, that is, a straight line parallel to the central axis of the cylinder Because is determined as the separation axis, the cylindrical model can always be accurately projected parallel to the separation axis.
図8は、第2サブ判定処理において、直方体モデルのX面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図8(A)〜(C)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図8(A)には直方体モデルを+X方向側から見た図が、図8(B)には直方体モデルを+Y方向から見た図が、図8(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図8(B)には、直方体モデルのX面の面法線ベクトルに平行な分離軸AX2に、円柱モデルと直方体モデルとが射影された様子が示されている。図8(B)に示された例では、分離軸AX2上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図8(B)には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。 FIG. 8 is a diagram illustrating an example in which a straight line parallel to the normal vector of the X plane of the rectangular parallelepiped model is determined as the separation axis in the second sub determination process. FIGS. 8A to 8C show a state in which the cylindrical model and the rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. 8A shows a view of the cuboid model from the + X direction side, FIG. 8B shows a view of the cuboid model from the + Y direction, and FIG. 8C shows the cuboid model from the + Z direction. Each view is shown. FIG. 8B shows a state in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis AX2 parallel to the surface normal vector of the X plane of the rectangular parallelepiped model. In the example shown in FIG. 8B, since the images projected on the separation axis AX2 do not overlap, it is determined that there is no interference. In FIG. 8B, a plane including the surface normal vector of the rectangular parallelepiped model and the normal vector of the upper surface of the cylindrical model is omitted.
図9は、第2サブ判定処理において、直方体モデルのY面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図9(A)〜(C)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図9(A)には直方体モデルを+X方向側から見た図が、図9(B)には直方体モデルを+Y方向から見た図が、図9(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図9(A)には、直方体モデルのY面の面法線ベクトルに平行な分離軸AX3に、円柱モデルと直方体モデルとが射影された様子が示されている。図9(A)に示された例では、分離軸AX3上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図9(A)には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。 FIG. 9 is a diagram illustrating an example in which a straight line parallel to the normal vector of the Y plane of the rectangular parallelepiped model is determined as the separation axis in the second sub-determination process. 9A to 9C show a state in which the cylindrical model and the rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. 9A shows a cuboid model viewed from the + X direction side, FIG. 9B shows a cuboid model viewed from the + Y direction, and FIG. 9C shows a cuboid model from the + Z direction. Each view is shown. FIG. 9A shows a state in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis AX3 parallel to the surface normal vector of the Y plane of the rectangular parallelepiped model. In the example shown in FIG. 9A, since the images projected on the separation axis AX3 do not overlap, it is determined that there is no interference. In FIG. 9A, a plane including the surface normal vector of the rectangular parallelepiped model and the normal vector of the upper surface of the cylindrical model is omitted.
図10は、第2サブ判定処理において、直方体モデルのZ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図10(A)〜(C)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図10(A)には直方体モデルを+X方向側から見た図が、図10(B)には直方体モデルを+Y方向から見た図が、図10(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図10(B)には、直方体モデルのZ面の面法線ベクトルに平行な分離軸AX4に、円柱モデルと直方体モデルとが射影された様子が示されている。図10(B)に示された例では、分離軸AX4上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図10(B)には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。 FIG. 10 is a diagram illustrating an example in which a straight line parallel to the normal vector of the Z plane of the rectangular parallelepiped model is determined as the separation axis in the second sub determination process. FIGS. 10A to 10C show a state in which a cylindrical model and a rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. 10A shows a cuboid model viewed from the + X direction side, FIG. 10B shows a cuboid model viewed from the + Y direction, and FIG. 10C shows a cuboid model from the + Z direction. Each view is shown. FIG. 10B shows a state in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis AX4 that is parallel to the surface normal vector of the Z plane of the rectangular parallelepiped model. In the example shown in FIG. 10B, since the images projected on the separation axis AX4 do not overlap each other, it is determined that there is no interference. In FIG. 10B, a plane including the surface normal vector of the rectangular parallelepiped model and the normal vector of the upper surface of the cylindrical model is omitted.
E.第3サブ判定処理:
図11は、第3サブ判定処理の詳細なフローチャートである。第3サブ判定処理は、第2サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、円柱モデルの側面と直方体モデルの外形(辺)との干渉を判定するための処理である。第3サブ判定処理が開始されると、分離軸決定部414は、直方体モデルの3種類の面法線ベクトルの中から1つの面法線ベクトルを選択し(ステップS300)、選択された面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する(ステップS302。図15の平面PF1参照)。このような平面を作成すると、分離軸決定部414は、作成された平面上で、円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する(ステップS304。図15の分離軸AX3参照)。第3サブ判定処理で決定された分離軸は、特許請求の範囲における「第1の分離軸」に相当し、第3サブ判定処理で生成された平面は、特許請求の範囲における「第1の平面」に相当する。
E. Third sub-determination process:
FIG. 11 is a detailed flowchart of the third sub determination process. The third sub-determination process is a process that is executed when “no interference” is not determined in the second sub-determination process, in order to determine the interference between the side surface of the cylindrical model and the outer shape (side) of the rectangular parallelepiped model. It is processing of. When the third sub determination process is started, the separation
分離軸が決定されると、干渉判定部416は、ステップS302で作成された平面に、円柱モデルと直方体モデルとを射影する(ステップS306)。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップS304で決定された分離軸に射影する(ステップS308)。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部416は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する(ステップS310)。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には(ステップS310:NO)、図4に示した第4〜5サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ440にその判定結果(干渉なし)を表示させる(ステップS312)。モニタ440に判定結果を表示させると、干渉判定部416は、図4に示した一連の干渉判定処理を終了させる。
When the separation axis is determined, the
分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には(ステップS310:YES)、干渉判定部416は、更に詳しく干渉の判定を行うため、直方体モデルの3つの全ての面法線ベクトルを用いて当該第3サブ判定処理における干渉の判定を行ったか否かを判断する(ステップS314)。全ての面法線ベクトルを用いて干渉の判定を行っていない場合には(ステップS314:NO)、干渉判定部416は、処理をステップS300に戻す。そうすると、3種類の面法線ベクトルのうちのまだ選択されていない面法線ベクトルを用いた干渉の判定が行われる。これに対して、3種類の全ての面法線ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には(ステップS314:YES)、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部416は、当該第3判定処理を終了させて、図4に示した第4サブ判定処理(ステップS28)に処理を移行させる。
When the image of the cylindrical model and the image of the cuboid model overlap on the separation axis (step S310: YES), the
図12は、第3サブ判定処理のステップS300で直方体モデルのX面の法線ベクトルが選択された場合における分離軸の例を示す図である。図12(A)〜(D)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図12(A)には直方体モデルを+X方向側から見た図が、図12(B)には直方体モデルを+Y方向から見た図が、図12(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図12(D)には、円柱モデルの上面の法線ベクトルと、直方体モデルのX面の法線ベクトルとを含む平面PF1aが示されており、また、この平面PFに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸AX3aが示されている。 FIG. 12 is a diagram illustrating an example of the separation axis when the normal vector of the X plane of the rectangular parallelepiped model is selected in step S300 of the third sub determination process. FIGS. 12A to 12D show a state in which a cylindrical model and a rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. 12A shows a cuboid model viewed from the + X direction, FIG. 12B shows a cuboid model viewed from the + Y direction, and FIG. 12C shows a cuboid model from the + Z direction. Each view is shown. FIG. 12D shows a plane PF1a including the normal vector of the upper surface of the cylindrical model and the normal vector of the X plane of the rectangular parallelepiped model, and the side surface of the cylindrical model included in this plane PF. A separation axis AX3a parallel to the normal vector is shown.
図13は、第3サブ判定処理のステップS300で直方体モデルのY面の法線ベクトルが選択された場合における分離軸の例を示す図である。図13(A)〜(D)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図13(A)には直方体モデルを+X方向側から見た図が、図13(B)には直方体モデルを+Y方向から見た図が、図13(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図13(D)には、円柱モデルの上面の法線ベクトルと、直方体モデルのY面の法線ベクトルとを含む平面PF1bが示されており、また、この平面PFに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸AX3bが示されている。 FIG. 13 is a diagram illustrating an example of the separation axis when the normal vector of the Y plane of the rectangular parallelepiped model is selected in step S300 of the third sub determination process. 13A to 13D show a state in which the cylindrical model and the rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. FIG. 13A shows a cuboid model viewed from the + X direction, FIG. 13B shows a cuboid model viewed from the + Y direction, and FIG. 13C shows a cuboid model from the + Z direction. Each view is shown. FIG. 13D shows a plane PF1b including the normal vector of the upper surface of the cylindrical model and the normal vector of the Y plane of the rectangular parallelepiped model, and the side surface of the cylindrical model included in this plane PF. A separation axis AX3b parallel to the normal vector is shown.
図14は、第3サブ判定処理のステップS300で直方体モデルのZ面の法線ベクトルが選択された場合における分離軸の例を示す図である。図14(A)〜(D)には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図14(A)には直方体モデルを+X方向側から見た図が、図14(B)には直方体モデルを+Y方向から見た図が、図14(C)には直方体モデルを+Z方向から見た図が、それぞれ示されている。図14(D)には、円柱モデルの上面の法線ベクトルと、直方体モデルのZ面の法線ベクトルとを含む平面PF1cが示されており、また、この平面PFに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸AX3cが示されている。 FIG. 14 is a diagram illustrating an example of the separation axis when the normal vector of the Z plane of the rectangular parallelepiped model is selected in step S300 of the third sub determination process. FIGS. 14A to 14D show a state in which a cylindrical model and a rectangular parallelepiped model having the same positional relationship are observed from different viewpoints. 14A shows a cuboid model viewed from the + X direction side, FIG. 14B shows a cuboid model viewed from the + Y direction, and FIG. 14C shows the cuboid model from the + Z direction. Each view is shown. FIG. 14D shows a plane PF1c including the normal vector of the upper surface of the cylindrical model and the normal vector of the Z plane of the rectangular parallelepiped model, and the side surface of the cylindrical model included in this plane PF. A separation axis AX3c parallel to the normal vector is shown.
図15は、第3サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図15には、直方体モデルのZ面の法線ベクトルNV1と、円柱モデルの上面の法線ベクトルNV2と、これらの法線ベクトルNV1,NV2を含む平面PF1とが示されている。また、図15には、平面PF1に含まれる円柱モデルの側面の法線ベクトルNV3に平行な分離軸AX3が示されている。上記ステップS306では、まず、平面PF1上に、円柱モデルと直方体モデルとが射影される。図15には、こうして射影された平面PF1上の二次元の像がハッチングにより表されている。上記ステップS308では、この二次元の像から、平面PF1上の分離軸AX3に対して更に射影が行われる。このように、上記第3サブ判定処理では、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成し、その平面上で円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、図16に示すように、360°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができるので、効率的に、直方体モデルの面と円柱モデルの側面との干渉を判定することができる。なお、図15に示された例では、分離軸AX3上に射影された像同士が重なっていないため、「干渉なし」と判定される。 FIG. 15 is a diagram illustrating a concept in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis in the third sub determination process. FIG. 15 shows a normal vector NV1 on the Z plane of the rectangular parallelepiped model, a normal vector NV2 on the upper surface of the cylindrical model, and a plane PF1 including these normal vectors NV1 and NV2. FIG. 15 also shows a separation axis AX3 parallel to the normal vector NV3 of the side surface of the cylindrical model included in the plane PF1. In step S306, first, a cylindrical model and a rectangular parallelepiped model are projected onto the plane PF1. In FIG. 15, the two-dimensional image on the plane PF1 thus projected is represented by hatching. In step S308, projection is further performed on the separation axis AX3 on the plane PF1 from the two-dimensional image. As described above, in the third sub-determination process, a plane including the surface normal vector of the rectangular parallelepiped model and the normal vector of the upper surface of the cylindrical model is created and parallel to the normal vector of the side surface of the cylindrical model on the plane. A straight line is determined as the separation axis. Therefore, as shown in FIG. 16, the normal vector used as the separation axis can be reliably identified from the normal vectors on the side surfaces of the infinite number of cylinder models extending over 360 °. The interference between the surface of the rectangular parallelepiped model and the side surface of the cylindrical model can be determined. In the example shown in FIG. 15, since the images projected on the separation axis AX3 do not overlap, it is determined that there is no interference.
F.第4サブ判定処理:
図17は、第4サブ判定処理の詳細なフローチャートである。第4サブ判定処理は、第3サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、円柱モデルの側面と直方体モデルの外形(辺)との干渉を第3サブ判定処理とは異なる分離軸で判定するための処理である。第4サブ判定処理が開始されると、分離軸決定部414は、直方体モデルの12本の各辺に平行な3種類のベクトル(辺ベクトル)の中から1つ(1種類)の辺ベクトルを選択し(ステップS400)、選択された辺ベクトルと円柱モデルの上面の法線ベクトルとの外積ベクトルを計算する(ステップS402。図18の外積ベクトルCP1参照)。外積ベクトルを計算すると、分離軸決定部414は、計算された外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する(ステップS404。図18の平面PF2参照)。このような平面を作成すると、分離軸決定部414は、作成された平面上で、円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する(ステップS406。図18の分離軸AX4参照)。第4サブ判定処理で決定された分離軸は、特許請求の範囲における「第2の分離軸」に相当する。
F. Fourth sub determination process:
FIG. 17 is a detailed flowchart of the fourth sub determination process. The fourth sub determination process is a process executed when it is determined that “no interference” is determined in the third sub determination process, and interference between the side surface of the cylindrical model and the outer shape (side) of the rectangular parallelepiped model is determined in the third sub determination process. This is a process for determining with a different separation axis from the determination process. When the fourth sub determination process is started, the separation
分離軸が決定されると、干渉判定部416は、ステップS404で作成された平面に、円柱モデルと直方体モデルとを射影する(ステップS408)。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップS406で決定された分離軸に射影する(ステップS410)。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部416は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する(ステップS412)。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には(ステップS412:NO)、図4に示した第5サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ440にその判定結果(干渉なし)を表示させる(ステップS414)。モニタ440に判定結果を表示させると、干渉判定部416は、図4に示した一連の干渉判定処理を終了させる。
When the separation axis is determined, the
分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には(ステップS412:YES)、干渉判定部416は、更に詳しく干渉の判定を行うため、直方体モデルの3種類の全ての辺ベクトルを用いて当該第4サブ判定処理における干渉の判定を行ったか否かを判断する(ステップS416)。3種類の全ての辺ベクトルを用いて干渉の判定を行っていない場合には(ステップS416:NO)、干渉判定部416は、処理をステップS400に戻す。そうすると、3種類の辺ベクトルのうちのまだ選択されていない辺ベクトルを用いた干渉の判定が行われる。これに対して、3種類の全ての辺ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には(ステップS416:YES)、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部416は、当該第4判定処理を終了させて、図4に示した第5サブ判定処理(ステップS30)に処理を移行させる。
When the image of the cylindrical model and the image of the cuboid model overlap on the separation axis (step S412: YES), the
図18は、第4サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図18には、直方体モデルの辺ベクトルSV1と、円柱モデルの上面の法線ベクトルNV2と、辺ベクトルSV1および法線ベクトルNV2から求まる外積ベクトルCP1とが示されている。また、図18には、外積ベクトルCP1と円柱モデルの上面の法線ベクトルNV2とを含む平面PF2と、この平面PF2に含まれる円柱モデルの側面の法線ベクトルNV3に平行な分離軸AX4が示されている。上記ステップS408では、まず、平面PF2上に、円柱モデルと直方体モデルとが射影される。図18には、こうして射影された平面PF2上の二次元の像がハッチングにより表されている。上記ステップS410では、この二次元の像から、平面PF2上の分離軸AX4に対して更に射影が行われる。このように、上記第4サブ判定処理では、直方体モデルの辺ベクトルと円柱モデルの上面の法線ベクトルとから外積ベクトルを算出し、この外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する。そして、その平面上に存在する円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、第3サブ判定処理と同様に、360°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができ、これにより、効率的に、直方体モデルの外形と円柱モデルの側面との干渉を判定することができる。なお、図18に示された例では、分離軸AX4上に射影された像同士が重なっていないため、「干渉なし」と判定される。 FIG. 18 is a diagram illustrating a concept in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis in the fourth sub determination process. FIG. 18 shows a side vector SV1 of the rectangular parallelepiped model, a normal vector NV2 of the upper surface of the cylindrical model, and an outer product vector CP1 obtained from the side vector SV1 and the normal vector NV2. FIG. 18 also shows a plane PF2 including the outer product vector CP1 and the normal vector NV2 of the upper surface of the cylinder model, and a separation axis AX4 parallel to the normal vector NV3 of the side surface of the cylinder model included in the plane PF2. Has been. In step S408, first, a cylindrical model and a rectangular parallelepiped model are projected on the plane PF2. In FIG. 18, the two-dimensional image on the plane PF2 projected in this way is represented by hatching. In step S410, projection is further performed on the separation axis AX4 on the plane PF2 from the two-dimensional image. As described above, in the fourth sub-determination process, the outer product vector is calculated from the side vector of the rectangular parallelepiped model and the normal vector of the upper surface of the cylindrical model, and a plane including the outer product vector and the normal vector of the upper surface of the cylindrical model. Create Then, a straight line parallel to the normal vector of the side surface of the cylindrical model existing on the plane is determined as the separation axis. Therefore, as in the third sub-determination process, it is possible to reliably specify the normal vector to be used as the separation axis from the normal vectors on the side surfaces of the cylinder model that exist innumerably over 360 °. The interference between the outer shape of the rectangular parallelepiped model and the side surface of the cylindrical model can be determined efficiently. In the example shown in FIG. 18, since the images projected on the separation axis AX4 do not overlap, it is determined that there is no interference.
G.第5サブ判定処理:
図19は、第5サブ判定処理の詳細なフローチャートである。第5サブ判定処理は、第1〜4サブ判定処理のいずれの処理でも干渉していないことが判定できない位置関係にある直方体モデルの外形と円柱モデルの外形との干渉を判定するための処理である。第5サブ判定処理が開始されると、分離軸決定部414は、直方体モデルの3種類の辺ベクトルの中から1つの辺ベクトルを選択し(ステップS500)、選択された辺ベクトルと円柱モデルの上面の法線ベクトルとの外積ベクトルを計算する(ステップS502。図20の外積ベクトルCP2参照)。外積ベクトルを計算すると、分離軸決定部414は、計算された外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する(ステップS504。図20の平面PF3参照)。このような平面を作成すると、分離軸決定部414は、作成された平面上の上記外積ベクトルに平行な直線を分離軸として決定する(ステップS506。図20の分離軸AX5参照)。第5サブ判定処理で決定された分離軸は、特許請求の範囲における「第3の分離軸」に相当し、第5サブ判定処理で生成された平面は、特許請求の範囲における「第3の平面」に相当する。
G. Fifth sub determination process:
FIG. 19 is a detailed flowchart of the fifth sub-determination process. The fifth sub-determination process is a process for determining the interference between the outer shape of the rectangular parallelepiped model and the outer shape of the cylindrical model that are in a positional relationship in which it is not possible to determine that no interference occurs in any of the first to fourth sub-determination processes. is there. When the fifth sub determination process is started, the separation
分離軸が決定されると、干渉判定部416は、ステップS504で作成された平面に、円柱モデルと直方体モデルとを射影する(ステップS508)。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップS506で決定された分離軸に射影する(ステップS510)。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部416は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する(ステップS512)。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には(ステップS512:NO)、干渉判定部416は、「干渉なし」と判断し、モニタ440にその判定結果(干渉なし)を表示させる(ステップS514)。モニタ440に判定結果を表示させると、干渉判定部416は、図4に示した一連の干渉判定処理を終了させる。
When the separation axis is determined, the
分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には(ステップS512:YES)、干渉判定部416は、直方体モデルの3種類の全ての辺ベクトルを用いて当該第5サブ判定処理における干渉の判定を行ったか否かを判断する(ステップS516)。3種類の全ての辺ベクトルを用いて干渉の判定を行っていない場合には(ステップS516:NO)、干渉判定部416は、処理をステップS500に戻す。そうすると、3種類の辺ベクトルのうちのまだ選択されていない辺ベクトルを用いた干渉の判定が行われる。これに対して、3種類の全ての辺ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には(ステップS516:YES)、干渉判定部416は、当該第5サブ判定処理を終了させ、処理を図4に示した干渉判定処理のステップS32に移行させる。そうすると、干渉判定部416は、ここではじめて、「干渉あり」と判断し、モニタ440に判定結果を表示させる。つまり、本実施形態では、第1サブ判定処理から第5サブ判定処理までのすべての判定処理において、「干渉なし」と判断されなかった場合に、円柱モデルと直方体モデルとが干渉している可能性が高いと判断し、「干渉あり」との判定を行う。
When the image of the cylindrical model and the image of the rectangular parallelepiped model overlap on the separation axis (step S512: YES), the
図20は、第5サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図20には、直方体モデルの辺ベクトルSV2と、円柱モデルの上面の法線ベクトルNV2と、辺ベクトルSV1および法線ベクトルNV2から求まる外積ベクトルCP2とが示されている。また、図15には、外積ベクトルCP2と円柱モデルの上面の法線ベクトルNV2とを含む平面PF3と、この平面PF3に含まれる外積ベクトルCP2に平行な分離軸AX5が示されている。上記ステップS508では、まず、平面PF3上に、円柱モデルと直方体モデルとが射影される。図20には、こうして射影された平面PF3上の二次元の像がハッチングにより表されている。上記ステップS510では、この二次元の像から、平面PF3上の分離軸AX5に対して更に射影が行われる。図20に示された例では、分離軸AX5上に射影された像同士が重なっているため、3種類の全ての辺ベクトルを用いて干渉の判定が行われた場合には、「干渉あり」と判定される。ただし、他のベクトルを用いた干渉の判定において干渉なしと判断されれば、「干渉なし」と判定される。 FIG. 20 is a diagram illustrating a concept in which the cylindrical model and the rectangular parallelepiped model are projected onto the separation axis in the fifth sub determination process. FIG. 20 shows a side vector SV2 of the rectangular parallelepiped model, a normal vector NV2 of the upper surface of the cylindrical model, and an outer product vector CP2 obtained from the side vector SV1 and the normal vector NV2. FIG. 15 shows a plane PF3 including the outer product vector CP2 and the normal vector NV2 on the upper surface of the cylindrical model, and a separation axis AX5 parallel to the outer product vector CP2 included in the plane PF3. In step S508, first, the cylindrical model and the rectangular parallelepiped model are projected on the plane PF3. In FIG. 20, the two-dimensional image on the plane PF3 thus projected is represented by hatching. In step S510, projection is further performed on the separation axis AX5 on the plane PF3 from the two-dimensional image. In the example illustrated in FIG. 20, the images projected on the separation axis AX5 overlap each other, and therefore, when interference is determined using all three types of side vectors, “interference exists”. It is determined. However, if it is determined that there is no interference in the determination of interference using another vector, it is determined that there is no interference.
なお、本実施形態の第5サブ判定処理では、まず、平面を作成し、その平面に対して各モデルを射影した上で、更にそれらの像を分離軸に射影している。しかし、第5サブ判定処理では、第2サブ判定処理と同様に、平面を作成することなく、外積ベクトルに平行な分離軸に、直接的に各モデルを射影してもよい。 In the fifth sub-determination process of the present embodiment, a plane is first created, each model is projected onto the plane, and those images are further projected onto the separation axis. However, in the fifth sub determination process, similarly to the second sub determination process, each model may be projected directly onto the separation axis parallel to the outer product vector without creating a plane.
以上で説明した本実施形態の干渉判定装置400は、上述した第1サブ判定処理を実行することにより、円柱モデルの上面または下面と、直方体モデルの外形との干渉を判定することができ、第2サブ判定処理を実行することにより、直方体モデルの各面と円柱モデルの外形との干渉を判定することができる。更に、第3サブ判定処理を実行することにより、円柱モデルの側面と直方体モデルの外形との干渉を判定することができる。また、第4サブ判定処理や第5サブ判定処理において、円柱モデルの法線ベクトルと直方体の辺ベクトルとの外積を求めることで、更に異なる分離軸により、円柱モデルと直方体モデルとの干渉を判定することができる。
The
図21は、第3サブ判定処理以降の処理が必要となる円柱モデルと直方体モデルとの位置関係を示す図である。図21(A)〜(D)には、すべて同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図21(A)を参照すれば、円柱モデルと直方体モデルとは明らかに干渉していないが、この位置関係において、第1サブ判定処理を実行すれば、円柱モデルの上面の法線ベクトルに平行な直線が分離軸となり、その分離軸には、円柱モデルと直方体モデルとが重なって射影される。そのため、第2サブ判定処理の実行が必要となる。第2サブ判定処理を実行すれば、直方体の各面の法線ベクトルに平行な直線が分離軸となるため、例えば、図21(B)に示すように、直方体モデルのZ面の法線ベクトルに平行な直線が分離軸として決定される。しかし、図21(B)に示す例では、やはり、円柱モデルと直方体モデルとは分離軸に重なって射影される。また、図21(C)や図21(D)に示すように、分離軸として直方体モデルのX面の法線ベクトルに平行な直線やY面の法線ベクトルに平行な直線が分離軸として決定されても、やはり、円柱モデルと直方体モデルとはこれらの分離軸に重なって投影される。つまり、図21に示した円柱モデルと直方体モデルとの位置関係では、明らかに、円柱モデルと直方体モデルとは干渉していないが、第1サブ判定処理や第2サブ判定処理においては、干渉なしと判断されることがない。そのため、本実施形態では、第1サブ判定処理や第2サブ判定処理だけではなく、第3サブ判定処理や第4サブ判定処理、第5サブ判定処理を実行することにより、様々な分離軸が決定されるので、精度良く干渉の判定を行うことが可能になる。 FIG. 21 is a diagram illustrating a positional relationship between a cylindrical model and a rectangular parallelepiped model that require processing after the third sub determination processing. FIGS. 21A to 21D show a state in which a cylindrical model and a rectangular parallelepiped model all having the same positional relationship are observed from different viewpoints. Referring to FIG. 21A, the cylinder model and the rectangular parallelepiped model do not clearly interfere with each other. However, if the first sub-determination process is executed in this positional relationship, the cylinder model is parallel to the normal vector on the upper surface of the cylinder model. A straight line becomes a separation axis, and a cylindrical model and a rectangular parallelepiped model are projected onto the separation axis. Therefore, it is necessary to execute the second sub determination process. When the second sub-determination process is executed, a straight line parallel to the normal vector of each surface of the rectangular parallelepiped serves as the separation axis. For example, as shown in FIG. 21B, the normal vector of the Z surface of the rectangular parallelepiped model A straight line parallel to is determined as the separation axis. However, in the example shown in FIG. 21B, the cylindrical model and the rectangular parallelepiped model are projected on the separation axis. Further, as shown in FIGS. 21C and 21D, a straight line parallel to the normal vector of the X plane of the rectangular parallelepiped model and a straight line parallel to the normal vector of the Y plane are determined as the separation axis. Even so, the cylinder model and the rectangular parallelepiped model are projected on these separation axes. That is, in the positional relationship between the cylindrical model and the rectangular parallelepiped model shown in FIG. 21, the cylindrical model and the rectangular parallelepiped model do not interfere with each other, but there is no interference in the first sub-determination process and the second sub-determination process. It is not judged. Therefore, in this embodiment, not only the first sub-determination process and the second sub-determination process, but also the third sub-determination process, the fourth sub-determination process, and the fifth sub-determination process are performed, so that various separation axes can be obtained. Therefore, it is possible to accurately determine the interference.
また、本実施形態の第3サブ判定処理や第4サブ判定処理では、それぞれ、2つのベクトルから平面を作成し、その平面上に存在する円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、360°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができる。この結果、効率的に、直方体モデルと円柱モデルの側面との干渉を判定することができる。このように、本実施形態では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、ロボット100の可動部と障害物500とのリアルタイムの干渉判定が容易となる。そのため、例えば、干渉判定装置400を制御装置200に組み込んだ構成のように、干渉の判定にCPUリソースの多くを割り当てることができないようなシステムであっても十分に高速な干渉判定を行うことができる。また、上記のように、本実施形態では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、ロボット100の各可動部の外形形状を、直方体モデルなどの単純なモデルだけではなく、円柱モデルを用いて精度良くモデリングすることができる。よって、実際のロボット100の形状に即した干渉の判定が可能となるので、狭小空間での干渉の判定など、精度が求められる干渉の判定も容易に行うことが可能になる。
In the third sub determination process and the fourth sub determination process of this embodiment, a plane is created from two vectors, and a straight line parallel to the normal vector of the side surface of the cylindrical model existing on the plane is separated. Determine as axis. Therefore, it is possible to reliably specify the normal vector used as the separation axis from the normal vectors on the side surfaces of the cylindrical model that exist innumerably over 360 °. As a result, the interference between the cuboid model and the side surface of the cylindrical model can be determined efficiently. As described above, in this embodiment, even when a cylindrical model is used, it is possible to quickly determine the interference, so that it is easy to determine the real-time interference between the movable part of the
また、本実施形態では、第1サブ判定処理から第5サブ判定処理までのうち、いずれかの判定処理において、干渉なしと判定されれば、その時点で干渉判定処理全体が終了する。そのため、干渉の判定を高速に行うことができる。特に、本実施形態では、第1サブ判定処理の処理内容が最も単純であり、後に実行されるサブ判定処理ほど、平面の作成や外積の計算など、処理内容が複雑になっている。そのため、早い段階で干渉なしと判定されれば、それだけ高速に全体の処理を終了させることができる。よって、円柱モデルを用いてロボット100の可動部がモデリングされていても、干渉判定の平均的な処理速度を向上させることができる。
Moreover, in this embodiment, if it is determined that there is no interference in any of the determination processes from the first sub determination process to the fifth sub determination process, the entire interference determination process ends at that point. Therefore, it is possible to determine interference at high speed. In particular, in the present embodiment, the processing content of the first sub-determination process is the simplest, and the processing content such as plane creation and outer product calculation is more complicated as the sub-determination processing executed later. For this reason, if it is determined that there is no interference at an early stage, the entire process can be completed at a higher speed. Therefore, even if the movable part of the
以上、本発明の一実施形態について説明したが、本発明はこのような実施形態に限定されず、その趣旨を逸脱しない範囲で種々の構成を採ることができる。例えば、ソフトウェアによって実現した機能を、ハードウェアによって実現するものとしてもよい。その他、以下のような変形が可能である。 As mentioned above, although one Embodiment of this invention was described, this invention is not limited to such Embodiment, A various structure can be taken in the range which does not deviate from the meaning. For example, a function realized by software may be realized by hardware. In addition, the following modifications are possible.
上記実施形態における第1サブ判定処理から第5サブ判定処理までの処理は、少なくとも一部を省略してもよい。また、第1サブ判定処理から第5サブ判定処理実行までの処理の実行順序も適宜変更することが可能である。 The processes from the first sub determination process to the fifth sub determination process in the embodiment may be omitted at least partially. In addition, the execution order of the processes from the first sub determination process to the fifth sub determination process can be changed as appropriate.
上記実施形態では、干渉判定装置400は、ロボット100の可動部と障害物との干渉を判定しているが、ロボット100以外の産業用機械と障害物との干渉を判定してもよい。また、ロボット100の各可動部を円柱モデルと直方体モデルとで表して、可動部同士の干渉の判定を行ってもよい。
In the embodiment described above, the
上記実施形態では、ロボット100の可能部の少なくとも一部の外形形状を円柱モデルによって表し、障害物500の外形形状を直方体モデルによって表している。これに対して、ロボット100の可動部の少なくとも一部を直方体モデルによって表し、障害物500の外形形状の少なくとも一部を円柱モデルによって表してもよい。つまり、円柱モデルによって表される物体と直方体モデルによって表される物体との干渉判定ができればよい。
In the above embodiment, the outer shape of at least a part of the possible portion of the
上記実施形態の各サブ判定処理において、円柱モデルの上面の法線ベクトルを用いた処理では、円柱モデルの下面の法線ベクトルを用いてもよい。また、直方体モデルの面法線ベクトルを用いた処理では、面法線ベクトルに替えて辺ベクトルを用いても良く、逆に、辺ベクトルを用いた処理では、辺ベクトルに替えて面法線ベクトルを用いてもよい。 In each sub-determination process of the above embodiment, in the process using the normal vector on the upper surface of the cylindrical model, the normal vector on the lower surface of the cylindrical model may be used. In addition, in the process using the surface normal vector of the rectangular parallelepiped model, the edge vector may be used instead of the surface normal vector. Conversely, in the process using the edge vector, the surface normal vector is replaced with the edge vector. May be used.
上記実施形態では、干渉判定装置400は、制御装置200からロボット100の可動部の位置を取得している。これに対して、干渉判定装置400は、干渉判定装置400のRAM420あるいはHDD430に記録された可動部の位置を取得してもよい。また、干渉判定装置400は、実際のロボット100の干渉の判定を行ってもよいし、ロボット100の教示時や、ロボット100の動作シミュレーション時において、仮想的に干渉の判定を行ってもよい。
In the embodiment described above, the
10…ロボットシステム
100…ロボット
101…ベース部
102…ショルダ部
103…下アーム
104…上アーム
105…手首
106…フランジ部
107…エンドエフェクタ
200…制御装置
300…教示装置
400…干渉判定装置
410…CPU
412…位置取得部
414…分離軸決定部
416…干渉判定部
420…RAM
430…HDD
432…モデルデータ
434…干渉判定プログラム
440…モニタ
450…インタフェース
460…バス
500…障害物
DESCRIPTION OF
412 ...
430 ... HDD
432 ...
Claims (9)
前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、が記憶された記憶部と、
前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得部と、
前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルとを含む第1の平面を求め、該第1の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第1の分離軸として決定する分離軸決定部と、
前記第1の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第1の平面上の前記第1の分離軸に、前記第1の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第1の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定部と、
を備える干渉判定装置。 An interference determination device for determining interference between an industrial machine having a movable part and an obstacle,
Of the shape of at least a part of the movable part and the shape of at least a part of the obstacle, data of a cylindrical model in which one shape is represented by a rectangular cylinder and data of a cuboid model in which the other shape is represented by a rectangular parallelepiped And a storage unit storing
A position acquisition unit that acquires the position of the shape represented by the cylindrical model and the position of the shape represented by the rectangular parallelepiped model;
A first plane including a normal vector of an upper surface or a lower surface of the cylindrical model and any one of a surface normal vector or an edge vector of the rectangular parallelepiped model is obtained, and the first plane is calculated on the first plane. A separation axis determination unit that determines a straight line parallel to the normal vector of the side surface of the cylindrical model as a first separation axis;
The cylindrical model and the rectangular parallelepiped model are projected onto the first plane, and the cylindrical model image projected onto the first plane on the first separation axis on the first plane; Projecting the image of the rectangular parallelepiped model and interference between the movable part and the obstacle based on whether or not the cylindrical model image projected onto the first separation axis and the image of the rectangular parallelepiped model overlap each other. An interference determination unit for determining
An interference determination device comprising:
前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか1のベクトルとの外積ベクトルを求め、該外積ベクトルと前記円柱モデルの上面または下面の法線ベクトルとを含む第2の平面を求め、該第2の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第2の分離軸として決定し、
前記干渉判定部は、更に、前記第2の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第2の平面上の前記第2の分離軸に、前記第2の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第2の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 The interference determination apparatus according to claim 1,
The separation axis determination unit further obtains an outer product vector of a normal vector of the upper surface or the lower surface of the cylindrical model and any one of an edge vector or a surface normal vector of the rectangular parallelepiped model, and the outer product A second plane including the vector and the normal vector of the upper or lower surface of the cylindrical model is obtained, and a straight line parallel to the normal vector of the side surface of the cylindrical model on the second plane is defined as a second separation axis. Determined as
The interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the second plane, and further projects onto the second plane on the second separation axis on the second plane. And projecting the image of the cylindrical model and the image of the rectangular parallelepiped model, and based on whether or not the cylindrical model image projected onto the second separation axis overlaps the image of the rectangular parallelepiped model. An interference determination device that determines interference between a part and the obstacle.
前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか1のベクトルとの外積ベクトルを求め、該外積ベクトルに平行な直線を、第3の分離軸として決定し、
前記干渉判定部は、更に、前記第3の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第3の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 The interference determination apparatus according to claim 1 or 2, wherein
The separation axis determination unit further obtains an outer product vector of a normal vector of the upper surface or the lower surface of the cylindrical model and any one of an edge vector or a surface normal vector of the rectangular parallelepiped model, and the outer product Determine a straight line parallel to the vector as the third separation axis;
The interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the third separation axis, and the image of the cylindrical model and the image of the rectangular parallelepiped model projected onto the third separation axis. An interference determination device that determines interference between the movable part and the obstacle based on whether or not there is an overlap.
前記分離軸決定部は、更に、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルに平行な直線を、第4の分離軸として決定し、
前記干渉判定部は、更に、前記第4の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第4の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 It is an interference determination apparatus as described in any one of Claim 1- Claim 3, Comprising:
The separation axis determination unit further determines a straight line parallel to any one of the surface normal vector or the edge vector of the rectangular parallelepiped model as a fourth separation axis,
The interference determination unit further projects the cylindrical model and the rectangular parallelepiped model onto the fourth separation axis, and the image of the cylindrical model and the image of the rectangular parallelepiped model projected onto the fourth separation axis. An interference determination device that determines interference between the movable part and the obstacle based on whether or not there is an overlap.
前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルに平行な直線を第5の分離軸として決定し、
前記干渉判定部は、更に、前記第5の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第5の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。 It is an interference determination apparatus as described in any one of Claim 1- Claim 4, Comprising:
The separation axis determination unit further determines a straight line parallel to the normal vector of the upper surface or the lower surface of the cylindrical model as a fifth separation axis,
The interference determination unit further projects the cylindrical model and the rectangular parallelepiped model on the fifth separation axis, and the image of the cylindrical model and the image of the rectangular parallelepiped model projected on the fifth separation axis. An interference determination device that determines interference between the movable part and the obstacle based on whether or not there is an overlap.
前記分離軸決定部は、前記第5の分離軸、前記第4の分離軸、前記第1の分離軸、前記第2の分離軸、前記第3の分離軸、の順序で分離軸の決定を行い、
前記干渉判定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸を用いた干渉の判定を行わず、
前記分離軸決定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸の決定を行わない、干渉判定装置。 An interference determination apparatus according to claim 5, which cites claim 2, which cites claim 2, which cites claim 1, and which cites claim 3, which cites.
The separation axis determination unit determines a separation axis in the order of the fifth separation axis, the fourth separation axis, the first separation axis, the second separation axis, and the third separation axis. Done
The interference determination unit does not perform the interference determination using the separation axis in the order after the separation axis determined that the movable part and the obstacle do not interfere with each other.
The separation determination unit is an interference determination device that does not determine a separation axis in an order subsequent to a separation axis determined that the movable unit and the obstacle do not interfere with each other.
前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、を記憶部に記憶させる記憶工程と、
前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得工程と、
前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルとを含む第1の平面を求め、該第1の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第1の分離軸として決定する分離軸決定工程と、
前記第1の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第1の平面上の前記第1の分離軸に、前記第1の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第1の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定工程と、
を備える干渉判定方法。 An interference determination method for checking interference between an industrial machine having a movable part and an obstacle,
Of the shape of at least a part of the movable part and the shape of at least a part of the obstacle, data of a cylindrical model in which one shape is represented by a rectangular cylinder and data of a cuboid model in which the other shape is represented by a rectangular parallelepiped And a storage step of storing in the storage unit,
A position acquisition step of acquiring the position of the shape represented by the cylindrical model and the position of the shape represented by the rectangular parallelepiped model;
A first plane including a normal vector of an upper surface or a lower surface of the cylindrical model and any one of a surface normal vector or an edge vector of the rectangular parallelepiped model is obtained, and the first plane is calculated on the first plane. A separation axis determination step for determining a straight line parallel to the normal vector of the side surface of the cylindrical model as a first separation axis;
The cylindrical model and the rectangular parallelepiped model are projected onto the first plane, and the cylindrical model image projected onto the first plane on the first separation axis on the first plane; Projecting the image of the rectangular parallelepiped model and interference between the movable part and the obstacle based on whether or not the cylindrical model image projected onto the first separation axis and the image of the rectangular parallelepiped model overlap each other. An interference determination step for determining
An interference determination method comprising:
前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、をコンピュータに記憶させる記憶機能と、
前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得機能と、
前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか1のベクトルとを含む第1の平面を求め、該第1の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第1の分離軸として決定する分離軸決定機能と、
前記第1の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第1の平面上の前記第1の分離軸に、前記第1の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第1の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定機能と、
をコンピュータに実現させるコンピュータプログラム。 A computer program for determining interference between an industrial machine having a movable part and an obstacle,
Of the shape of at least a part of the movable part and the shape of at least a part of the obstacle, data of a cylindrical model in which one shape is represented by a rectangular cylinder and data of a cuboid model in which the other shape is represented by a rectangular parallelepiped And a storage function for storing in a computer,
A position acquisition function for acquiring the position of the shape represented by the cylindrical model and the position of the shape represented by the rectangular parallelepiped model;
A first plane including a normal vector of an upper surface or a lower surface of the cylindrical model and any one of a surface normal vector or an edge vector of the rectangular parallelepiped model is obtained, and the first plane is calculated on the first plane. A separation axis determination function for determining a straight line parallel to the normal vector of the side surface of the cylindrical model as a first separation axis;
The cylindrical model and the rectangular parallelepiped model are projected onto the first plane, and the cylindrical model image projected onto the first plane on the first separation axis on the first plane; Projecting the image of the rectangular parallelepiped model and interference between the movable part and the obstacle based on whether or not the cylindrical model image projected onto the first separation axis and the image of the rectangular parallelepiped model overlap each other. An interference determination function for determining
A computer program that causes a computer to realize
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2012050236A JP5857803B2 (en) | 2012-03-07 | 2012-03-07 | Industrial machine interference determination device, interference determination method, computer program, and recording medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2012050236A JP5857803B2 (en) | 2012-03-07 | 2012-03-07 | Industrial machine interference determination device, interference determination method, computer program, and recording medium |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2013184242A JP2013184242A (en) | 2013-09-19 |
JP5857803B2 true JP5857803B2 (en) | 2016-02-10 |
Family
ID=49386149
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2012050236A Active JP5857803B2 (en) | 2012-03-07 | 2012-03-07 | Industrial machine interference determination device, interference determination method, computer program, and recording medium |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5857803B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022045254A1 (en) * | 2020-08-31 | 2022-03-03 | ファナック株式会社 | Numerical control system |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP7184595B2 (en) | 2018-10-29 | 2022-12-06 | オークマ株式会社 | machine tool system |
CN114185337B (en) * | 2020-09-15 | 2023-08-04 | 宇通客车股份有限公司 | Vehicle, vehicle pre-collision detection method and device |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6243706A (en) * | 1985-08-21 | 1987-02-25 | Hitachi Ltd | Interference checking method in robot |
JPS63300903A (en) * | 1987-06-01 | 1988-12-08 | Hitachi Ltd | System for speeding up interference decision making |
JPS6448107A (en) * | 1987-08-19 | 1989-02-22 | Hitachi Ltd | Discriminating system for interference between objects in space |
JPH01180602A (en) * | 1988-01-12 | 1989-07-18 | Hitachi Ltd | Deciding system for interference between polyhedrons in space |
JPH04210352A (en) * | 1990-11-30 | 1992-07-31 | Enshu Ltd | Interference simulation method for exchanging tool |
US5347459A (en) * | 1993-03-17 | 1994-09-13 | National Research Council Of Canada | Real time collision detection |
JP3415438B2 (en) * | 1998-05-12 | 2003-06-09 | 富士通株式会社 | Interference checking apparatus and method |
US7877210B2 (en) * | 2007-01-11 | 2011-01-25 | Siemens Industry, Inc. | System and method for projecting b-rep outlines to detect collisions along a translational path |
JP5434651B2 (en) * | 2010-02-12 | 2014-03-05 | 富士通株式会社 | Interference determination apparatus, interference determination method, and interference determination program |
-
2012
- 2012-03-07 JP JP2012050236A patent/JP5857803B2/en active Active
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022045254A1 (en) * | 2020-08-31 | 2022-03-03 | ファナック株式会社 | Numerical control system |
JP7448667B2 (en) | 2020-08-31 | 2024-03-12 | ファナック株式会社 | numerical control system |
Also Published As
Publication number | Publication date |
---|---|
JP2013184242A (en) | 2013-09-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4153528B2 (en) | Apparatus, program, recording medium and method for robot simulation | |
JP5949242B2 (en) | Robot system, robot, robot control apparatus, robot control method, and robot control program | |
JP6551184B2 (en) | Simulation apparatus, simulation method, and simulation program | |
JP5850962B2 (en) | Robot system using visual feedback | |
US9727053B2 (en) | Information processing apparatus, control method for information processing apparatus, and recording medium | |
JP5908544B2 (en) | Robot program generation device for generating a robot program for reducing drive shaft jerk | |
JP6137155B2 (en) | Interference avoidance method, control device, and program | |
JP5361004B2 (en) | Smooth path generation device and smooth path generation method | |
JP5775279B2 (en) | Route generator | |
US20110172819A1 (en) | Manipulator and control method thereof | |
JP2012223845A (en) | Method and apparatus for predicting interference between target section of robot and peripheral object | |
JP2015071206A (en) | Control device, robot, teaching data generation method, and program | |
JP2019018272A (en) | Motion generation method, motion generation device, system, and computer program | |
JP2018144144A (en) | Image processing device, image processing method and computer program | |
US10406688B2 (en) | Offline programming apparatus and method having workpiece position detection program generation function using contact sensor | |
JP6863927B2 (en) | Robot simulation device | |
Andersson et al. | AR-enhanced human-robot-interaction-methodologies, algorithms, tools | |
WO2015186508A1 (en) | Teaching data-generating device and teaching data-generating method for work robot | |
JP5857803B2 (en) | Industrial machine interference determination device, interference determination method, computer program, and recording medium | |
JP2019098409A (en) | Robot system and calibration method | |
JP2015054378A (en) | Information processing device, robot, scenario information creation method and program | |
JP7249221B2 (en) | SENSOR POSITION AND POSTURE CALIBRATION DEVICE AND SENSOR POSITION AND POSTURE CALIBRATION METHOD | |
JP2020123042A (en) | Information processor and information processing method and system | |
JP2009166172A (en) | Simulation method and simulator for robot | |
JP7447568B2 (en) | Simulation equipment and programs |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20140902 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20150529 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20150602 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20151117 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20151130 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5857803 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |