JP2826138B2 - Mobile body interference check device - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION 【産業上の利用分野】[Industrial applications]

本発明は、移動物体の干渉を高速且つ高精度で判断す
る干渉チェック装置に関し、例えば、教示したロボット
の動作の安全性をグラフィックシミュレータ等により確
認する装置やロボット動作における物体との干渉をリア
ルタイムで判定したり、無人車におけるリアルタイムで
の干渉判定装置等に応用できる。The present invention relates to an interference check device that determines the interference of a moving object at high speed and with high accuracy. For example, a device that checks the safety of the operation of a taught robot by a graphic simulator or the like and interference with an object in a robot operation in real time. It can be applied to a real-time interference determination device for an unmanned vehicle or the like.

【従来技術】[Prior art]

従来、例えば、ロボットの干渉判定においては、画面
上で動作径路の各位置でロボットの姿勢を表示してロボ
ットの連続動作を教示したり、プログラムされたロボッ
トの動作を画面上に連続的に表示するグラフィックシミ
ュレータが知られている。この装置では、ロボットの連
続動作が実際の移動速度で表示されてその動作がシミュ
レートされるのであるが、移動するロボットアーム間や
ロボットアームと静止物体との干渉の有無を高精度且つ
高速度で検出することが必要となっている。 この干渉判定を行う手法は、各種のものが知られてい
る。 例えば、第１の方法として、特開昭62−43706号公報
に見られるように、ロボット及び作業環境の各部材を多
面体で被覆して、その多面体の射影図形から干渉の有無
を判定する方法、第２の方法として、特開昭60−99591
号公報に見られるように、ロボット及び作業環境の各部
材を線分で表現し、それらの線分間の距離を演算して干
渉の有無を判定する方法、第３の方法として、ロボット
学会誌６巻１号35頁に見られるように、ロボット又は環
境物体の移動位置を時間関数として表現し、その関数か
ら頂点が面と衝突したり、辺と辺とが衝突した時に干渉
と判定する方法が知られている。 又、第４の方法として、ロボット学会誌５巻３号21頁
に見られるように、ロボット及び環境物体を大きさの異
なる立方体で階層的に近似して、２つの物体間の各立方
体の包含関係を調べることにより干渉判定を行ったり、
第５の方法として、第６回日本ロボット学会学術講演会
予稿集105頁に見られるように、ロボット及び環境物体
を半径の異なる球体で階層的に近似して、その半径の異
なる球体の包含関係を調べることにより、物体の干渉判
定を行う方法が知られている。Conventionally, for example, in robot interference determination, the robot's posture is displayed at each position on the operation path on the screen to teach the robot's continuous operation, or the programmed robot's operation is displayed continuously on the screen Graphic simulators are known. In this device, the continuous motion of the robot is displayed at the actual moving speed and the motion is simulated. However, the presence or absence of interference between the moving robot arms or between the robot arm and the stationary object is determined with high accuracy and high speed. It is necessary to detect by. Various methods are known for performing this interference determination. For example, as a first method, as shown in JP-A-62-43706, a method in which each member of a robot and a work environment is covered with a polyhedron and the presence or absence of interference is determined from a projected figure of the polyhedron, As a second method, JP-A-60-99591
As disclosed in Japanese Unexamined Patent Application Publication No. H10-163, a method of expressing each member of the robot and the working environment by a line segment, calculating the distance between these line segments to determine the presence or absence of interference, and a third method include: As shown in Vol. 1, p. 35, there is a method of expressing the movement position of a robot or an environmental object as a time function, and determining from the function that when a vertex collides with a surface or when a side collides, it is determined that there is interference. Are known. As a fourth method, as shown in the Journal of the Robotics Society of Japan, Vol. 5, No. 3, page 21, robots and environmental objects are hierarchically approximated by cubes having different sizes, and each cube is included between two objects. Interference judgment is performed by examining the relationship,
As a fifth method, the robot and environmental objects are hierarchically approximated by spheres with different radii and the inclusion relation of spheres with different radii as shown in page 105 of the 6th Annual Conference of the Robotics Society of Japan. Is known to determine the interference of an object by examining.

【発明が解決しようとする課題】[Problems to be solved by the invention]

上記第１の方法では、例えば部材が凹立体であり形状
を単なる円柱又は直方体で近似することが困難となるよ
うな場合に精度の高い干渉チェックができないし、第２
の方法では、高速判定が可能であるが、モデル化が単純
すぎて干渉判定の精度が良くないし、第３の方法では、
面の数が多くなると計算量が増大し、処理速度が非常に
遅くなるという問題がある。 又、第４の方法は、所謂、オクトツリーの階層的構造
による手法であるが、立方体近似であるため、連続する
移動体を表現するための座標変換が複雑になり時間がか
かるという問題がある。 更に、第５の方法は、この座標変換を単純化するため
に、ロボット及び環境物体を半径の異なる球体で階層的
に近似しているのであるが、下位階層の球体を発生させ
る方法が上位階層の球体に内接する正多面体を求め、そ
の正多面体を構成する各面と外接し、その中心が多面体
の重心と一致する球体として求めているため、下位階層
の球体の発生機構が複雑となりモデル化に時間がかかる
という問題がある。又、球体と球体との重複部分が多く
なったり、上位階層の球体の外部を内部に包含する下位
階層の球体が発生したり、同一球体が上位階層の異なる
２つの球体に属したり、１階層上位の球体には属するが
２階層以上上位の球体には属さない球体が発生されたり
するため、モデル化効率が良くなくひいてはモデル化に
時間がかかるという欠点がある。更に、モデル化する対
象物体の形状を表現するのに不必要な物体の内部及び外
部に対して、一様に階層構造の球体が発生されるため球
体の数が多くなり、同様にモデル化効率が良くないとい
う問題がある。又、精度は最小球の半径に依存し、干渉
判定の演算速度は球体間の干渉判定回数、即ち、球体の
階層化構造に依存する。しかし、各階層の球体の半径比
が用いられる多面体の面数により固定的に決定されるこ
とから、半径比を要求される干渉判定精度に基づいて任
意に設定することができない。従って、階層化構造と最
小球の半径とを任意に変化させることが困難であり、精
度と演算速度との調整が困難となる。又、ある階層に属
する多面体は重なりを生じるが、次の下位の階層の球体
を求める時に、多面体の重なりに関しては一切配慮され
ていないので、無駄に球体が発生されてしまい、探索速
度が低下するという問題がある。 本発明は、上記の課題を解決するために成されたもの
であり、その目的とするところは、物体のモデル化速度
を向上させると共に球体の階層化構造を簡略化すること
により球体間の干渉判定回数を減少させ、物体の干渉判
定を高速且つ精度良く実行することにある。In the first method, for example, when the member is concave and it is difficult to approximate the shape with a simple cylinder or a rectangular parallelepiped, highly accurate interference check cannot be performed.
In the method of (1), high-speed determination is possible, but modeling is too simple and the accuracy of interference determination is not good. In the third method,
As the number of faces increases, the amount of calculation increases, and there is a problem that the processing speed becomes extremely slow. Further, the fourth method is a method based on a so-called octree hierarchical structure. However, since the method is a cubic approximation, there is a problem that coordinate transformation for expressing a continuous moving object is complicated and takes time. . Further, in the fifth method, in order to simplify the coordinate transformation, the robot and the environmental object are hierarchically approximated by spheres having different radii. The regular polyhedron inscribed in the sphere is obtained, and the sphere that circumscribes each face constituting the regular polyhedron and whose center coincides with the center of gravity of the polyhedron is determined. There is a problem that it takes time. In addition, the number of overlapping portions between spheres increases, a sphere in a lower hierarchy including the outside of a sphere in an upper hierarchy occurs, the same sphere belongs to two spheres in different upper hierarchies, or a sphere in one hierarchy. Since a sphere that belongs to a higher-order sphere but does not belong to a sphere higher than two or more layers is generated, there is a drawback that modeling efficiency is not good, and that modeling takes time. Furthermore, since spheres having a hierarchical structure are uniformly generated inside and outside the object unnecessary to represent the shape of the object to be modeled, the number of spheres increases, and similarly, the modeling efficiency increases. There is a problem that is not good. The accuracy depends on the radius of the smallest sphere, and the calculation speed of the interference determination depends on the number of times of interference determination between the spheres, that is, the hierarchical structure of the spheres. However, since the radius ratio of the sphere in each hierarchy is fixedly determined by the number of faces of the polyhedron used, the radius ratio cannot be set arbitrarily based on the required interference determination accuracy. Therefore, it is difficult to arbitrarily change the hierarchical structure and the radius of the smallest sphere, and it is difficult to adjust accuracy and calculation speed. In addition, polyhedrons belonging to a certain hierarchy cause overlap, but when obtaining a sphere of the next lower hierarchy, no consideration is given to the overlap of polyhedrons, so spheres are generated uselessly, and the search speed decreases. There is a problem. SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems, and an object of the present invention is to improve the modeling speed of an object and simplify the hierarchical structure of the spheres to thereby reduce the interference between the spheres. An object of the present invention is to reduce the number of determinations and execute the interference determination of an object at high speed and with high accuracy.

【課題を解決するための手段及び作用】Means and Action for Solving the Problems

上記課題を解決するための発明の構成は、第１図に示
すように、各物体の表面を表す面情報を記憶した面情報
記憶手段Ａと、ある階層の任意の１つの球体に対して、
その球体に内包されるとともに１つ下位階層に属する複
数の球体とで構成されるような内包関係よりも少なくと
も２階層以上に階層化された球体を用い、面情報に基づ
いて各物体の表面を各々複数種類の半径からなる球体に
より階層的に被覆してモデル化する階層球体モデル生成
手段Ｂと、生成されたモデル化球体の位置を記憶する球
体モデル記憶手段Ｃと、移動体に関する複数種類の半径
のモデル化球体の位置を物体の移動に応じて更新して球
体モデル記憶手段に出力する球***置更新手段Ｄと、球
体モデル記憶手段に記憶された任意時刻の２つの物体に
関するモデル化球体における一方の物体に属する球体と
他方の物体に属する球体との２つの球体において、それ
らの２つの球体の中心位置とそれらの球体の半径とに基
づいて、中心間距離が半径の和以下の場合にその２つの
球体が干渉し、中心間距離が半径の和よりも大きい場合
にそれらの球体は非干渉として、相互に干渉する球体を
検出し、その干渉すると判断された球体同士のみについ
てその相互に干渉する球体に属する下位階層の球体の中
からさらに干渉する球体を検出するという手順を半径の
大きい球体から順に半径の小さい球体へと階層的に実行
し、干渉球が存在しない時点で干渉演算を中止すると共
に、最終的に相互に干渉する最小半径の球体が存在する
ときに２つの物体は干渉すると判断しその相互に干渉す
る最小半径の球体の位置から２つの物体の干渉位置を求
め、これらの演算結果を出力する球体干渉演算手段Ｅと
から成る。 上記面情報記憶手段Ａは物体の表面を形成している平
面、曲面に関する情報を記憶する手段であり、例えば、
構成平面や構成曲面の関数を記憶した装置である。 階層球体モデル生成手段Ｂは、上記面情報に基づいて
決定される物体の表面を被覆する球体を発生し、その球
体の位置を生成する手段であり、指定された階層数及び
指定半径の球体にて、上記物体の表面を階層的に被覆し
て、物体の表面を階層球体によりモデル化する手段であ
る。階層的とは、各物体を内包する各大球、その球に内
包される各中球、さらにその中球に内包される各小球の
ように内包関係にあることを意味する。 球体モデル記憶手段Ｃは生成された各階層の球体の位
置、例えば、中心位置や半径を記憶する手段である。 球***置更新手段Ｄは物体の移動に応じて各階層の球
体の位置を更新して、球体モデル記憶手段Ｃに記憶する
手段である。例えば、物体の位置及び姿勢は物体に固定
された座標系の平行移動及び回転移動で表されるが、こ
の平行移動と回転移動とを同時に１つの行列で表すこと
ができる。従って、移動後の各球体の絶対座標系から見
た中心位置は、各物体に固定された座標系で見た中心位
置に上記の変換行列をかけて求めることができる。 球体干渉演算手段Ｅは物体の移動に際し、物体間の干
渉の有無及びその位置を球体モデル記憶手段Ｃに記憶さ
れた各球体の位置から演算する手段である。より具体的
には、先ず、２つの物体間で最大半径の球体、即ち、最
上位階層の球体が干渉するか否が判定される。この干渉
判定は、例えば、２つの球体の中心間の距離が各球体の
半径の和より小さいか否かにより判定される。そして、
干渉しないと判定された場合には、他の物体間の干渉判
定が行われる。一方、干渉有りと判定されると、その球
体に属する次に半径の小さな球体、即ち、下位階層の球
体に対して、同様に干渉有無の判定が実行される。干渉
有りと判定されると更に、その干渉する球体に属する更
に下位階層の球体に対して干渉判定が順次実行される。
一方、ある階層の球体で干渉する球体が存在しない場合
には、１つ上位の階層に遡り、その階層で他に干渉する
球体を検出する演算を実行し、干渉する球体が有れば、
上記した手順により更に下位階層の球体について干渉判
定を行い、干渉球が無ければ、更に、上位階層に遡り、
同様に干渉する球体を検出するという演算が実行され
る。そして、最終的に相互に干渉する最小半径の球体が
存在するときに２つの物体は干渉すると判断しその相互
に干渉する最小半径の球体の位置から２つの物体の干渉
位置が求められる。又、干渉球の階層的な検索におい
て、ある階層での球体について相互に干渉する球体が存
在しなくなれば、当然に、下位階層の球体の干渉はあり
えないので、その時点で干渉演算は停止される。As shown in FIG. 1, the configuration of the invention for solving the above-mentioned problem is as follows: surface information storage means A storing surface information representing the surface of each object;
Using a sphere that is included in the sphere and is hierarchized at least two or more levels higher than the inclusive relation constituted by a plurality of spheres belonging to one lower hierarchy, the surface of each object is determined based on the surface information. Hierarchical sphere model generating means B for hierarchically covering and modeling spheres each having a plurality of types of radii, sphere model storing means C for storing the positions of the generated modeled spheres, and a plurality of types of moving bodies A sphere position update unit D that updates the position of the radius modeled sphere according to the movement of the object and outputs the updated sphere position to the sphere model storage unit, and a modeled sphere related to two objects at arbitrary times stored in the sphere model storage unit In two spheres, a sphere belonging to one object and a sphere belonging to the other object, a center distance is determined based on the center positions of the two spheres and the radii of the spheres. If is less than or equal to the sum of the radii, the two spheres interfere, and if the center-to-center distance is greater than the sum of the radii, the spheres are determined to be non-interfering, detect spheres that interfere with each other, and are determined to interfere. The procedure of detecting further interfering spheres from among the lower-level spheres belonging to the mutually interfering spheres for only the interfering spheres is performed hierarchically in order from the sphere with the larger radius to the sphere with the smaller radius. When there is no sphere, the interference calculation is stopped, and finally, when there is a sphere having the minimum radius that interferes with each other, it is determined that the two objects interfere with each other, and two spheres having the minimum radius interfering with each other are determined. And a spherical interference calculating means E for calculating the interference position of the object and outputting the calculation results. The surface information storage means A is a means for storing information relating to a plane forming a surface of an object and a curved surface.
It is a device that stores functions of constituent planes and constituent curved surfaces. The hierarchical sphere model generating means B is a means for generating a sphere covering the surface of the object determined based on the surface information and generating the position of the sphere. Means for hierarchically covering the surface of the object and modeling the surface of the object with a hierarchical sphere. Hierarchical means that there is an inclusive relation such as each large sphere containing each object, each medium sphere included in the sphere, and each small sphere included in the medium sphere. The sphere model storage means C is means for storing the position of the generated sphere in each layer, for example, the center position and radius. The sphere position updating means D is means for updating the position of the sphere in each layer according to the movement of the object and storing the updated sphere position in the sphere model storage means C. For example, the position and orientation of the object are represented by translation and rotation in a coordinate system fixed to the object, and the translation and rotation can be simultaneously represented by one matrix. Accordingly, the center position of each sphere after moving as viewed from the absolute coordinate system can be obtained by multiplying the center position as viewed from the coordinate system fixed to each object by the above-described conversion matrix. The sphere interference calculating means E is means for calculating the presence or absence of interference between the objects and the position of each sphere from the position of each sphere stored in the sphere model storage means C when the objects move. More specifically, first, it is determined whether or not a sphere having the maximum radius, that is, a sphere of the highest hierarchy, interferes between the two objects. This interference determination is made based on, for example, whether or not the distance between the centers of the two spheres is smaller than the sum of the radii of the respective spheres. And
When it is determined that there is no interference, interference between other objects is determined. On the other hand, when it is determined that there is interference, the determination of the presence or absence of interference is similarly performed on a sphere having the next smallest radius belonging to the sphere, that is, a sphere of a lower hierarchy. When it is determined that there is interference, the interference determination is sequentially performed on the spheres in the lower hierarchy belonging to the interfering sphere.
On the other hand, if there is no sphere that interferes with a sphere in a certain hierarchy, the calculation goes back to the next higher hierarchy and performs an operation to detect another sphere that interferes in that hierarchy.
The interference determination is performed for the spheres in the lower hierarchy according to the above-described procedure.
Similarly, an operation of detecting an interfering sphere is executed. Finally, when there is a sphere having a minimum radius that interferes with each other, it is determined that the two objects interfere with each other, and an interference position of the two objects is obtained from the position of the sphere having the minimum radius that interferes with each other. Also, in the hierarchical search for interference spheres, if there are no spheres that interfere with each other with respect to spheres in a certain hierarchy, there is naturally no possibility that the spheres in the lower hierarchy will interfere, and the interference calculation is stopped at that point. .

【実施例】【Example】

以下、本発明を具体的な実施例に基づいて説明する。 第１実施例 本第１実施例は、ロボットや作業環境に関する形状や
位置のデータを入力して、ロボット及び作業環境をグラ
フィックディスプレイ上に表示し、ロボットの動作のシ
ミュレーションを行う場合において、ロボット間又はロ
ボットと物体間の干渉チェックを行う装置である。 第２図に示すように、本装置はコンピュータ装置で構
成されており、論理演算等を実行するCPU1とその処理手
順を示すプログラムを記憶したROM2と各種のデータを記
憶するRAM3とロボットや作業環境の位置及び形状データ
を入力したりシミュレート開始指令等の各種の指令を与
えるデータ入力装置４と経時変化するロボットの姿勢や
作業環境を微小サンプリングタイム毎に表示するグラフ
ィックディスプレイ５とデータ入力装置４及びグラフィ
ックディスプレイ５をCPU1と接続するための入出力イン
タフェース6,7とから構成されている。又、RAM3にはロ
ボットアームや作業環境の各構成部材毎にそれらの形状
を表現する面や頂点、稜線の情報を記憶する形状データ
メモリと、各構成部材の表面を階層化したときの各球体
の中心位置と半径及び階層に関する情報を記憶する球体
モデルメモリ32と、ロボットの現在のジョイント座標を
記憶する関節角メモリ33と、ロボットや作業環境の各構
成部材毎にそられの現在の位置を記憶する部材位置メモ
リ34とが形成されている。 次に、本装置の作用をCPU1の処理手順を示した第３図
のフローチャートに従って説明する。 ステップ100では、まずロボットや作業環境の各構成
部材毎にそれぞれの形状を生成するためのデータがデー
タ入力装置４から入力される。この入力により、各構成
部材の形状を表す頂点、稜線、面等のデータが演算され
る。これらの形状データは形状データメモリ31に記憶さ
れる。 次に、ステップ102では、ロボットや作業環境の各構
成部材に関して、それらの拘束関係や絶対座標での位置
のデータがデータ入力装置４から入力される。この入力
データが入力されるに連れてグラフィックディスプレイ
５にロボットや作業環境を表示するようにしており、入
力データの誤りの検出やデータ入力が容易に実行される
ように構成されている。そして、ロボット及び作業環境
の拘束関係や位置のデータは部材位置メモリ34に記憶さ
れる。 ステップ102での位置データの入力が完了すると、ス
テップ104でデータ入力装置４により干渉の対象となる
ロボット及び作業環境を指定するデータが入力される。 次に、ステップ106において、ステップ104で指定され
たロボットあるいは作業環境について各構成部材毎にそ
の構成部材の各表面を大，中，小の３種類の球体により
階層的に被覆して、表面の階層的な球体モデル化が行わ
れる。この表面の球体モデル化は次のように実行され
る。 第４図に示すように、１つの構成部材11を内包する直
方体形状の外包体12をその構成部材11の形状データメモ
リ31から参照した面情報データより発生する。更に、そ
の外包体12に外接する球体を考え、その球体を大球14と
する。このように、最上位階層の大球14は、構成部材11
を内包する球体として発生される。そして、構成部材11
にとられた基準座標から見た大球14の中心位置とその半
径が球体モデルメモリ32に記憶される。 次に、第５図（ａ）に示すように、外包体12を中球15
の半径rmに対して、１辺が の立法体の複数のセル17に分割する。外包体12の各辺の
長さは必ずしもセル17の辺の長さの倍数とはならないの
で、各セル17はオーバーラップさせて配置される。次
に、第５図（ｂ）に示すように、各セル17に外接する球
体20を発生させ、その球体20のうち、構成部材11の表面
13と交差する球体のみを抽出し、その抽出された球体を
中球15とする。そして、構成部材11にとられた基準座標
から見た各中球15の中心位置が球体モデルメモリ32に記
憶される。 次に、第６図（ａ）に示すように、中球15に内接する
セル17に注目し、その１つのセル17に対して、更に、小
球の半径をrsとして、一辺を とする立方体のセル18に分割する。このとき各セル18は
中球の発生時と同様にオーバーラップするように配置さ
れている。次に、この各セル18に外接する各球体21を発
生させ、構成部材11の表面13と交差する球体のみを抽出
し、その抽出された球体を小球16とする。そして、構成
部材11にとられた基準座標から見た各小球16の中心位置
が球体モデルメモリ32に記憶される。 このようにして、第７図に示すように、構成部材11
は、全体を外包する大球14と構成部材11の表面13を含む
それより小さい中球15,更に小さい小球16の半径の異な
る３種類の球体により階層的にモデル化される。即ち、
第８図によると、全ての小球は何れかの１つの中球にの
み属し、中球の全ては大球に属するというようにツリー
状に階層化されることが分る。 尚、小球16の半径rsは、干渉判定の精度を考慮して任
意に設定される。次に中球15の半径rmは、大球の半径rl
と小球の半径rsとから次式 により自動的に求める。本式によれば、小球と中球、中
球と大球との体積比を等しくできる。このことにより干
渉判定時の探索に効率の良い階層構造が得られる。 又、例外として、構成部材11の外寸が小さいため、大
球14の径が与えられた小球16の径より小さくなる場合は
小球16の生成は行わず、大球14のみのモデルとする。 以上のように各構成部材の階層的球体モデル化が完了
すると、処理はステップ108へ移行して、ロボットの動
作指令をデータ入力装置４から入力する。次に、ステッ
プ110へ移行して、補間点に対する微小移動動作に対応
して、微小移動後のロボットのジョイント座標、即ち、
関節角の演算を行い、その関節角データは関節角メモリ
33に記憶される。次に、関節角の演算が完了すると、ス
テップ112へ移行して、各構成部材にとられた基準座標
系と絶対座標系とを関係づける変換行列Ｔが、その時の
関節角から演算される。上記の変換行列Ｔは、ロボット
或いは作業環境を構成する各構成部材にとられた基準座
標の絶対座標から見た位置及び姿勢を３×４の行列で表
したものである。そして、各構成部材にとられた基準座
標から見た各球体の中心位置を表すベクトルをPrelとす
ると、絶対座標から見た各球体の中心位置を表すベクト
ルPabsは次式 により求められる。ロボットなど各構成部材が動く物体
については、動作するたびに各関節角から変換行列Ｔを
計算し、構成部材をモデル化する全ての球体の中心位置
の絶対座標を更新する必要がある。但し、この絶対座標
の更新については、計算の効率化を図るため、干渉チェ
ックの対象となる球体についてのみ行っても良い。 このように、球体モデルメモリ32には、各構成部材に
とられた基準座標から見た各球体の中心位置Prelと、現
在時刻における各球体の絶対座標から見た中心位置Pabs
とが記憶されていることになる。 次に、ステップ114へ移行して、以下の手順により干
渉チェックの演算が実行される。 ステップ114にて、大球，中球，小球の順で球体同士
の干渉チェックを階層的に行い、ステップ116にて、最
下位階層の球体（小球）同士で干渉有と判定された場合
に、ステップ118へ移行して、干渉の発生及び干渉部位
をその相互に干渉する小球の中心位置から求めて表示す
ると共に、ステップ120にて、干渉状態にあるロボット
と作業環境との表示が行われ、本プログラムを終了す
る。 次に、ステップ114における、大球，中球，小球によ
る階層的なチェックの手順を以下に示す。 step1.第９図（ａ）に示すように、大球14a,14b同士
のチェックを行い、干渉するものが有ればstep2.を実行
する。なければチェック終了とする。従って、まず、各
構成部材がある程度接近するまでは、大球同士の干渉判
定となり、演算時間は極めて短い。 step2.第９図（ｂ）に示すように、１つの大球14a
と、相手の大球14bに含まれる中球15b1〜15bnとの干渉
チェックを実行し、干渉する中球15b1,15b2,15b3を抽出
する。又、逆に、大球14bと相手の大球14aに含まれる中
球15a1〜15anとの干渉チェックを実行し、干渉する中球
15a1,15a2を抽出する。干渉する中球が１つもなけれ
ば、２つの構成部材は干渉しないと判断し、演算を中断
する。 step3.第９図（ｃ）に示すように、抽出された中球15
b1,15b2,15b3,15a1,15a2同士の干渉チェックを行い、相
互に干渉する中球15b1,15b2,15a1を抽出し、step4を実
行する。又、相互に干渉する中球が存在しない場合に
は、２つの構成部材は干渉しないと判断して、チェック
演算を中断する。 step4.第９図（ｄ）に示すように、干渉する１つの中
球15b2と、相手の中球15a1に含まれる小球16a11〜16a1n
₁との干渉チェックを行い、干渉する小球16a11,16a12を
抽出する。そして、逆に、干渉する１つの中球15a1と、
相手の中球15b2に含まれる小球16b21〜16b2n₂のなかか
ら干渉する小球が抽出され、次のstep5を実行する。ま
た、干渉する小球が存在しない場合には、中球15b1と、
相手の中球15a1に属する小球16a11〜16a1n₁との干渉判
定が同様に実行される。干渉する小球が検出されない場
合には、相互に干渉する他の中球について同様に実行
し、相互に干渉する中球が存在しなくなると、２つの構
成部材は干渉しないと判定される。 step5.第９図（ｅ）に示すように、抽出された小球16
a11,16a12,16b21,16b22,16b23同士のチェックを行い、
相互に干渉する小球16a11,16b21を抽出する。そして、
この場合には、２つの構成部材は小球16a11,16b21の部
位で干渉すると判断する。又、相互に干渉する小球が存
在しない場合には、相互に干渉する他の中球の抽出step
へ戻ることになる。 結局、最終的に、最下位階層の小球同士の干渉が検出
された時に、干渉有りと判定し、その小球の位置を干渉
部位と判断する。小球同士の干渉が存在しない場合に
は、上位階層の他の相互に干渉する中球に対する処理が
実行され、その相互に干渉する中球が存在しない場合に
は、最終的に、干渉無しと判定される。従って、第８図
に示すツリー構造を干渉の可能性のある枝路に沿って、
探索していくことになり、一様に全ての小球同士の干渉
を同時に判定するより、数段、干渉判定の時間が短くな
る。 尚、球体の干渉判定は、二つの球の半径をそれぞれ
r₁，r₂，中心位置をP₁，P₂とするとき、次式 r₁＋r₂≧‖P₁−P₂‖ を満足するか否かによって判定される。上式を満足すれ
ば、その２つの球体は干渉するとする。 以上のような、大球，中球，小球の階層的なチェック
により、干渉発生部を効率良く絞り込み、簡易且つ高速
の干渉チェックを行うことができる。 尚、ステップ116にて、最下位階層の球体で干渉が発
生しないと判定されると、その構成部材間での干渉が発
生してないのであるから、ステップ122へ移行して、全
ての構成部材間の組合せに対して干渉チェックの演算が
実行されたか否かが判定され、全ての部材の組合せにつ
いて演算が完了していない場合には、ステップ114へ戻
り、その構成部材間での干渉チェックが同様に実行され
る。又、全ての構成部材の組合せについて干渉チェック
演算が完了した場合には、ステップ124へ移行して全て
の指定された物体について干渉チェック演算が完了した
か否が判定され、完了していない場合には、他の物体間
での干渉チェック判定が同様に実行される。 ステップ124にて、全ての指定された物体の組合せに
ついて干渉チェック演算が完了したと判定された場合に
は、ステップ126へ移行して、関節角メモリ33に記憶さ
れいるその時の関節角からロボットの姿勢が演算され、
その姿勢でロボットを表示すると共に作業環境の表示が
行われる。次に、ステップ128へ移行して、全ての移動
動作が完了したか否が判定される。移動未完了の場合に
は、ステップ110へ戻り次の移動点に対し上記の干渉判
定処理が繰り返し実行される。又、移動完了と判定され
ると、本プログラムは終了する。 以上説明したように、本発明に係る装置は、要請され
る精度と演算速度との両条件を容易に満足させることが
できるので、例えばロボットのラフ・モーション時にお
ける高速干渉チェックを行う場合に特に有効である。 又、本発明の装置はいかなる形状の部材も同じ精度で
モデル化できる他、実ロボットにもオフラインシミュレ
ータにも適用可能な汎用性の高い装置である。 第２実施例 本第２実施例は、物体の大きさや最小球の半径等を考
慮し、干渉が発生する部位の探索効率が最適となるよう
に球体モデルの階層数および各階層の球の半径を自動的
に設定するものである。以下に前記第１実施例との相違
点のみについて説明する。 階層球体モデル生成手段Ｂにおいて、物体を内包する
最大球の半径r₁、干渉チェックの精度を決定する最小球
の半径r_nおよび下位の球との半径比ρ₁を与えることに
より、階層の深さ（階層数）ｎと各階層での球の半径r_i
を、夫々次の（１）、（２）式で算出する。 但し、ｎは四捨五入し整数とする。 但し、ｉ＝1,2,……ｎ ここで、（１）式の導出過程を示す。 各階層の球の半径をr_iとすると、下位の球（半径r_i+1）
との体積比ρ_vは（３）式で表される。 ρ_vが各階層で一定とすると、最大球の半径r₁と最小球
の半径r_nとの関係は（４）式で表される。 ここで、上位・下位の球の半径比をρ_rとすると、
（３）式よりρ_v＝ρ_r ³であるので、（４）式は（５）
式のように表される。 r₁ ³＝r_n ³・ρ_r ^3(n-1) ……（５） （５）式の両辺の対数をとることにより、（１）式が
得られる。 また、前記（３）式は（５）式よりr_i ³＝r_n ³・ρ_r
^3(n-1)の関係があるから両式より導出される。 上述においては、上位・下位の球の体積比ρ_vを一
定、すなわち上位の球内に一定数の下位の球を含むとい
う関係を規定することにより、各階層において各球が有
する下位の球の数を全体にわたって均一化できるので、
最大球から最小球に至る階層数および各階層における球
の数を探索効率上最適に設定できる。 さらに、上位・下位の球の面積比 ρ_s＝（r_i／r_i+1）²を一定としても、同様に上位・下位
の球の半径比ρ_r一定の関係に帰着されることはいうま
でもない。 なお、半径比ρ_rは物体の大きさ、形状に応じて設定
されるものであるが、本実施例では約２としている。 上述の如く決定された階層数ｎおよび各階層での球半
径r_iを用いて、物体の球体モデルを次のように生成す
る。 ステップ1:物体を内包する直方体Rec1とその直方体Re
c1を内包する球体Sphを生成する。（階層レベル:k＝
１） ステップ2:階層レベルをｋ＝ｋ＋１とし、直方体Rec
k−１を半径r_kの球に内接する立方体セル（すなわち一
辺が の立方体）で分割する。ここで、各立方体セルは、直方
体Rec k−１の各辺と各立方体の辺とを一致するように
配置するので、第10図（ａ）においてハッチング部で示
すように、多くの場合各立方体セルは夫々隣接セルとオ
ーバラップする。このオーバラップ部は、後述するよう
に削除して直方体Rec k,jとして再構成し、全てのセル
（ｊ＝1,2……）についてステップ３を実行する。 ステップ3:直方体Rec k,jを包む球が物体の表面と交
わっているかどうかを調べる。交わっている場合はその
球を球体モデルのデータに加え、ｋ＜ｎ（すなわち、階
層レベルｋが最下層レベルでない）ならば、直方体Rec
k＋１について更にステップ２を実行する。 ここで、立方体セルのオーバラップ部の削除について
説明する。 上述のように各階層レベルにおいて、下位球（すなわ
ち下位セル）が生成される順に優先的にオーバラップ部
を加えていき、順序が後になるセルについてはオーバラ
ップ部を除いた直方体セルとして定義するものである。
すなわち、第10図（ｂ）において立方体セルRec k−１
を半径r_kの９つの球Sph k,1〜Sph k,9により各直方体Re
c k,1〜Rec k,9に分割する場合、最初の球Sph k,1に内
接するように立方体セルRec k,1を決め、次の球Sph k,2
に対してはSph k,2に内接する立方体において値球体Rec
k,1とのオーバラップ部を除く部分を直方体セルRec k,
2と定義する。以下、Sph k,3〜Sph k,9に対し、順次同
様の操作を行って、夫々オーバラップ部のない各直方体
セルRec k,1〜Rec k,9を定義する。次に、下位の球Sph
k＋1,iを生成する場合は、前記各直方体セルRec k,1〜R
ec k,9を前述と同様に分割するので、オーバラップして
いた部分で生成される球Sph k＋1,iの重複はなくなり、
球データ数を必要最小限に減らすことができる。こうし
て、物体の表面を表現する（モデル化）するに際し、各
階層での球の数を必要最小限にすることができるので、
干渉チェックの際の探索効率が低下することがない。 次に、上記のように生成された物体X,Yの夫々の階層
数がn_X，n_Yである階層球対モデルに対し、球体干渉演算
手段Ｂにおいて階層的に干渉チェックする再帰的アルゴ
リズムを以下に示す。このアルゴリズムは、前記階層数
が等しくない（n_X≠n_Y）場合にも適用できるが、基本的
には前記第１実施例の場合と同じである。なお、以下の
各ステップにおいて「true/falseを返す」とは、一つ前
のステップにtrue/falseを返すことを意味する。 ステップ1:二つの物体X,Yの各最上位（階層レベルｋ
＝１）の球について、夫々の球データに干渉の可能性を
表すマークを付け（フラグに１を立てる）ステップ２を
実行する。ステップ２で真（true）という結果が得られ
た場合は、物体ＸとＹは互いに干渉しているものとす
る。そうでない場合（偽（false）という結果が得られ
た場合）は、物体ＸとＹは互いに干渉していないものと
する。 ステップ2:物体X,Yに夫々属するチェック対象となる
球の全ての組合せについてステップ３を実行する。ステ
ップ３がtrueならばtrueを返す。また、ステップ３でtr
ueとなる組合せが一つも存在しない場合はfalseを返
す。 ステップ3:一つの組合せの夫々物体X,Yに属す各球デ
ータのどちらにも干渉可能性を示すマークが付いている
ならば、それら球同士の干渉チェックを行う。そうでな
ければfalseを返す。上記チェックの結果、球同士が干
渉している場合は、その二つの球についてステップ４を
実行する。干渉していなければfalseを返す。また、ス
テップ４の結果がtrueならばtrueを、falseならばfalse
を返す。 ステップ4:二つの球が共に最下位階層（n_X，n_Y）であ
れば、物体ＸとＹは互いに干渉しているものと判定しtr
ueを返す。そうでない場合、物体X,Yのどちらか一方に
属する球のみが最下位階層ならばステップ５を、また、
二つの球が共に最下位階層でない場合は、ステップ６を
夫々実行する。ステップ５およびステップ６の夫々の結
果がtrueならばtrueを、falseならばfalseを返す。 ステップ5:物体X,Yの一方に属する最下位階層の球
と、他方の物体に属する最下位でない階層の球の１階層
下の階層の球との全ての組合せについて球同士の干渉チ
ェックを行う。干渉が有る場合は、ステップ４を実行す
る。干渉有の球が一つもなければfalseを返す。ステッ
プ４の結果がtrueならばtrueを、falseならばfalseを返
す。なお、ここでマークを全てクリア（フラグを０にす
る）することによりセットし、次にまた物体間の干渉チ
ェックをする際に、ステップ１から直ちに始められるよ
うにしておく。 ステップ6:物体Ｘに属する一つの球と、物体Ｙに属す
る一つの球の１階層下の階層の複数の球との全ての組合
せについて、球同士の干渉チェックを行う。干渉が発生
している場合は、前記物体Ｙに属する一つの球の１階層
下の階層の球データにマークを付ける。マークの付いた
球が一つも存在しない場合はfalseを返す。同様の処理
を、物体Ｙに属する一つの球と、物体Ｘに属する一つの
球の１階層下の階層の複数の球との全ての組合せについ
ても行う。このようにして、物体Ｘ側の球および物体Ｙ
側の球夫々について、階層のレベルを徐々に下げて上位
階層の球との干渉の可能性のある球を選び出す。そし
て、マークの付いた球があれば、ステップ２を全ての組
合せについて実行する。ステップ２の結果がtrueならば
trueを、falseならばfalseを返す。なお、ここでステッ
プ５と同様に、マークを全てクリヤ（フラグを０にす
る）する。 次に、上記アルゴリズムに従って干渉判定を行うため
のCPU1の処理手順を第12図（ａ），（ｂ）に基づいて説
明する。 （ａ）階層化されたＸ球とＹ球との階層数が同一である
場合。 ステップ200において、物体Ｘの現探索階層ｉが１
に、物体Ｙの現探索階層ｊが１の最上位階層に初期設定
され、最上位球の干渉フラグがオンとされる。最上位球
の干渉フラグをオンとすることで、下位階層への球対の
探索が開始される。次に、ステップ202において、現探
索階層（i,j）において、干渉フラグが共にオンの全球
対の探索が終了したか否かが判定される。判定結果がNO
の場合には、ステップ204以下のステップが実行され、
判定結果がYESの場合には、ステップ250以下のステップ
が実行される。最上位球に関する最初の実行時には、全
球対の探索が終了していないのであるから、ステップ20
2の判定結果は、当然に、NOとなる。 ステップ204では、干渉フラグがオンの球であって、
階層ｉに属する１つのＸ球と、階層ｊに属する１つのＹ
球とが選択され、未探索の１つの球対（iX,jY）が発生
される。次のステップ206では、ステップ204で発生され
た球対（iX,jY）の干渉演算が実行され、次のステップ2
08で２つの球iXとjYは干渉するか否かが判定される。
尚、干渉演算は、第１実施例で示したように、２つの球
の中心間の距離がその２つの球の半径の和よりも小さい
か否かによって判定される。 次に、ステップ210で、球対（iX,jY）は共に最下位階
層に存在するか否かが判定され、判定結果がNOの場合に
は、ステップ212へ移行する。ステップ212では、球対
（iX,jY）の一方の球は最下位階層に存在するか否かが
判定され、判定結果がNOの場合には、ステップ214へ移
行する。 ステップ214では、球iXと、球jYに属する１階層下の
下位球の全てに対して干渉判定が行われる。そして、干
渉する場合には、その球jYの下位球の干渉フラグがオン
とされる。次に、ステップ216へ移行して、ステップ214
の干渉判定の結果、球jYの下位球のうち少なくとも１球
が、球iXと干渉しているか否かが判定され、判定結果が
YESの場合には、ステップ218が実行される。 ステップ218では、ステップ214と同様にして、球jY
と、球iXに属する１階層下の下位球に全てに対して干渉
判定が行われる。そして、干渉する場合には、その球iX
の下位球の干渉フラグがオンとされる。次に、ステップ
220へ移行して、ステップ218の干渉判定の結果、球iXの
下位球のうち少なくとも１球が、球jYと干渉しているか
否かが判定され、判定結果がYESの場合には、ステップ2
22が実行される。 次に、ステップ222において、現探索階層（i,j）にお
ける次の探索球対の選択が可能なように、現探索球対
（iX,jY）が領域Ｄ（i,j）に記憶される。又、現探索階
層（i,j）へ復帰後の処理の継続に必要な他のパラメー
タもスタックされる。 次に、ステップ224において、現探索階層（i,j）が、
それぞれ、１階層下げられる。 尚、現探索階層（i,j）は、注目している球対（iX,j
Y）の存在する階層として定義される。即ち、親球iX
と、親球jYに属する１階層下の下位球との干渉演算が実
行されている場合の現探索階層（i,j）は、親球iX,jYの
属する階層として定義される。又、現探索球対（iX,j
Y）とは、注目されている親球iXと親球jYとの球対とし
て定義される。 このように、ステップ222,224にて、復帰のためのス
タック処理と現探索階層の更新処理が完了すると、ステ
ップ202へ戻り、現探索階層が次の下位階層となった状
態で、上述の処理が繰り返される。 上記のステップ202からステップ224が分岐することな
く繰り返されると、干渉球対を探索する探索枝は、Ｘ
球、Ｙ球共に、最短経路で下位階層へ伸びて行く。 尚、上記のステップ202からステップ224のルーチンの
中に、判断結果により処理ステップを分岐するステップ
が含まれているが、分岐されることにより、探索枝は、
上位階層に戻りその上位階層で水平方向に伸びて又下位
階層に伸びるというようにして進行して行く。 上記の分岐ステップのうち、ステップ208で、球対（i
X,jY）が干渉しないと判定されると、ステップ202へ戻
り、ステップ204において、干渉フラグがオンに設定さ
れた球であって、現探索階層（i,j）に属する他の球対
（iX,jY）が発生される。そして、ステップ206以下にお
いて、上述したように、同様にその球対（iX,jY）の干
渉が判定され、干渉する場合には、親球iXと、親球jYに
属する下位の子球との干渉判定と、親球jYと、親球iXに
属する下位の子球との干渉判定とが実行され、その干渉
する子球の干渉フラグがオンとされる。また、ステップ
216で、親球jYに属する親球iXと干渉する子球が存在し
ないと判定された場合、又は、ステップ220で、親球iX
に属する親球iYと干渉する子球が存在しないと判定され
た場合には、その親球iXと親球jYとは本来干渉しないの
であるから、その球対（iX,jY）から下位階層へ向け
て、探索枝を伸ばす必要がない。したがって、この場合
には、ステップ202へ戻り、続くステップ204で現探索階
層（i,j）に属する他の球対（iX,jY）が生成され、ステ
ップ206以下の処理により、その球対（iX,jY）から探索
枝が伸ばされる。 又、ステップ202で現探索階層（i,j）に属する探索フ
ラグオンの球対の全てが探索されたと判定された場合に
は、ステップ250へ移行する。 ステップ250では、その現探索階層（i,j）が最上位階
層（1,1）であるか否かが判定される。判定結果がNOの
場合には、ステップ252へ移行して、現探索階層（i,j）
に属する探索の対象となった全球対は干渉しないのであ
るから、その全球対を構成する球の干渉フラグがオフと
される。 そして、ステップ254へ移行して、現探索階層（i,j）
が１階層上にあげられる。これにより探索枝は上記階層
に戻ることになる。 探索枝がある上位階層から下位階層へ伸びるときに、
ステップ222でその上位階層の現探索球対等のデータが
記憶されたが、ステップ256では、下位階層からその上
位階層へ戻る時に、その上位階層での現探索球対等のデ
ータが復元される。従って、ステップ202へ戻ると、そ
の上位階層において、次の未探索の球対から、干渉球の
探索が継続されることになる。 一方、ステップ250において、現探索階層（i,j）が最
上位階層と判定された場合には、最上位球が干渉しない
ことを意味している。従って、この場合は、ステップ25
8へ移行して、物体Ｘと物体Ｙとは干渉しないという結
論を出すことになる。 これに対し、干渉球の探索が進行する過程において、
ステップ210において、ステップ208で干渉すると判定さ
れた現探索球対（iX,jY）の球iXと球jYとが共に最下位
階層であると判定された場合には、物体Ｘと物体Ｙとは
その球対（iX,jY）の存在する部分で干渉していると判
定することができる。よって、この場合には、ステップ
226へ移行して、２物体は干渉するという結論を出すこ
とができる。 （ｂ）階層化されたＸ球とＹ球との階層数が異なる場
合。 現探索球対（iX,jY）のいずれか一方の球が、最下位
階層になるまでは、上記したステップ200〜224及びステ
ップ250〜258が実行される。 上記の処理過程において、ステップ212で、現探索球
対（iX,jY）の一方の球が最下位階層であると判定され
ると、ステップ280以下が実行される。 ステップ280では、現探索球対（iX,jY）のうち、最下
位階層の球をＬ、相手球をＭとするとき、球Ｌと、相手
球Ｍに属する１階層下の下位球、即ち、子球のうち１つ
の球kM（ｋは子球によって変化する変数）が選択され、
未探索球対（L,kM）が発生される。そして、次のステッ
プ282で、その球対（L,kM）の干渉演算が実行され、次
のステップ284で、その球対（L,kM）は干渉するか否か
が判定される。判定結果がYESの場合には、ステップ286
へ移行して、将来、親球Ｌと親球Ｍの存在する階層とし
て定義される現探索階層（i,j）に復帰する場合に、継
続してその階層の干渉球の探索が実行されるように、親
球Ｌと親球Ｍとの球対で定義される現探索球対（iX,j
Y）（＝（L,M））が領域Ｄ（i,j）に記憶される。又、
その他の復帰に必要なパラメータがスタックされる。 次に、ステップ288において、親球の存在する現探索
階層（i,j）が１階層下げられる。この時、球iXが最下
位階層に存在する場合には、Ｙ球の階層ｊが１階層下げ
られる。これに対し、球iYが最下位階層に存在する場合
には、Ｘ球の階層ｉが１階層下げられる。 次に、ステップ290へ移行して、現在の球Ｌと子球kM
が、次の干渉球の探索過程における新しい親球とみなさ
れる。このために、親球の球対で定義される現探索球対
（iX,jY）が球対（L,kM）として再定義される。そし
て、ステップ210へ戻り、次の干渉球の探索が進行され
る。 このようにして、一方の球が最下位階層に属する場合
には、他方の球だけが、下位階層に向かって探索され
る。 又、ステップ284で、球対（L,kM）は非干渉と判定さ
れると、ステップ292へ移行して、子球kMは親球Ｍに属
する最後の球か否かが判定される。判定結果がNOの場合
には、ステップ280へ戻り、親球Ｍに属する他の子球kM
が選択される。そして、ステップ282以下で、上述と同
様に、球対（L,kM）の干渉判定が実行される。 ステップ292で、子球kMは親球Ｍに属する最後の球で
あると判定された場合には、結果的に、親球Ｌと親球Ｍ
とは非干渉であることを意味するので、現探索球対（i
X,jY）を変更する必要がある。ステップ294はそのため
に存在する。 ステップ294では、現探索階層（i,j）において、Ｘ球
の階層ｉとＹ球の階層ｊとが等しいか否かが判定され
る。判定結果がYESの場合には、同一階層に属する他の
球対を発生すればよいので、ステップ202へ戻る。 ステップ294の判定結果がNOの場合は、現探索球対（i
X,jY）が、親球Ｌと親球Ｍの属する１階層上の親球との
球対に戻され、干渉球の探索はその状態から継続される
必要がある。このために、次のステップ296で、現探索
階層（i,j）が１階層上げられる。尚、この場合、球iX
が最下位階層に存在する場合には、Ｙ球の階層ｊが１階
層上げられる。これに対し、球iYが最下位階層に存在す
る場合には、Ｘ球の階層ｉが１階層上げられる。又、ス
テップ298で、更新された現探索階層（i,j）において、
干渉球の探索を行っていた最後の探索球対が現探索球対
（iX,jY）として復元される。尚、その球対は、以前に
実行されたステップ286において、領域Ｄ（i,j）に記憶
されている値である。 そして、ステップ210へ戻り、ステップ212でYESと判
定され、ステップ280に移行する。そして、ステップ280
で、現探索球対（iX,jY）を親球として、次の新しい未
探索の球対（L,kM）が発生され、その球対に対して干渉
判定が行われる。 以上、説明したようにして、干渉する可能性のある球
についてのみ、下位階層に向けて、順次探索されていく
ので、探索速度が向上する。 次に、上述の干渉球の探索アルゴリズム及びCPU1によ
り実行される探索処理手順により、どの様に干渉球が求
められるかを、第11図に示す例に基づき説明する。 第11図には、物体Ｘが５階層、物体Ｙが３階層の階層
球体モデルで表される場合について第８図と同様に、最
上位の球（物体Ｙでは球x,物体Ｙでは球ｙ）から最下位
の球（物体Ｘでは球x₁₁₁₁，……，x_mmmm、物体Ｙでは球
y₁₁，……，y_nn）に至る各階層の関係が示されている。
なお、図中の〜は、物体間の球同士の干渉チェック
の手順を示すもので、以下の記載に対応している。さら
に、以下の記載において「（x,y）の干渉チェック」と
は、球ｘと球ｙとの間の干渉チェックを行うことを意味
する。まず、物体Ｘと物体Ｙとの間の干渉チェックを実
行するにあたり、x,yに夫々干渉可能性を示すマーク付
けをする（干渉フラグをオン）。 （x,y）の干渉チェックにより干渉有のため、x,yを夫
々干渉有とする。 干渉有とされたｘとｙの下位階層のy₁，y₂，……，y_n
との全ての組合せ（x,y₁），（x,y₂）……（x,y_n）の干
渉チェックにより、ｘとy₁およびy₂との間で干渉有のた
め、y₁，y₂に夫々マーク付けをする。 同様に、ｙとｘの下位階層のx₁，x₂，…，x_mとの全て
の組合せの干渉チェックにより、干渉有のx₁，x₂に夫々
マーク付けをする。 マークを付けられた球の組合せ全部（x₁，y₁），
（x₁，y₂），（x₂，y₁），（x₂，y₂）の干渉チェックよ
り、x₁とy₁との間で干渉有のため、x₁，y₁を夫々干渉有
とする。すなわち、マークを付けられていないx₃，…，
x_mとy₃，…，y_nとの間の干渉チェックは行わない。 と同様に、干渉有とされたx₁，y₁に対し、x₁とy₁
の下位階層のy₁₁，y₁₂，…，y_1nとの全ての組合せの干
渉チェックにより、干渉有のy₁₁，y₁₂に夫々マーク付け
をする。 と同様に、干渉有とされたx₁，y₁に対し、y₁とx₁
の下位階層のx₁₁，x₁₂，…，x_1mとの全ての組合せの干
渉チェックにより、干渉有のx₁₁，x₁₂に夫々マーク付け
をする。 マークを付けられた球の組合せ全部（x₁₁，y₁₁），
（x₁₁，y₁₂），（x₁₂，y₁₁），（x₁₂，y₁₂）の干渉チェ
ックより、x₁₁とy₁₁との間で干渉有のため、x₁₁，y₁₁を
夫々干渉有とする。なお、y₁₁は物体Ｙの最下位階層の
球であるため、これ以後は物体Ｙのチェック対象球は常
にy₁₁となる。 干渉有とされたx₁₁，y₁₁に対し、y₁₁とx₁₁の下位階層
のx₁₁₁，x₁₁₂，…，x_11mとの全ての組合せに対し順次、
干渉チェックを行う。x₁₁₁とy₁₁とは干渉が無いので次
に（x₁₁₂，y₁₁）の干渉チェックを行う。その結果、x
₁₁₂とy₁₁の間で干渉が発生しているためx₁₁₂を干渉有と
するとともに、チェック対象をx₁₁₂からx₁₁₂の下位階層
の球へ変更する。 y₁₁とx₁₁₂の下位階層のx₁₁₂₁との干渉チェックにより
干渉が無いため、次に（x₁₁₂₂，y₁₁）の干渉チェックを
行う。この結果、x₁₁₂₂とy₁₁との間で干渉が発生してい
るためx₁₁₂₂を干渉有とする。x₁₁₂₂は最下位階層の球で
あるので、ここで処理を終了し、次のx₁₁₂₃に関しては
干渉の有無に関係なく干渉チェックを行わない。 こうして、物体Ｘと物体Ｙとは球x₁₁₂₂と球y₁₁の存在
する部位で干渉していると直ちに判断される。 但し、物体の全部位について干渉の有無を知りたい場
合は、においてチェック対象から外したx₁₁₃，…，x
_11m全てについても、y₁₁との干渉チェックを行えば良
い。 以上説明したように、他の物体の上位階層の球との間
でのチェックにより、干渉の可能性ありとしてマークを
付けられた球同士に付いてのみ干渉チェックを行うた
め、チェック対象となる球の組合せの数を極めて少なく
することができるので、物体間の干渉チェックの効率を
非常に高めることができる。 Hereinafter, the present invention will be described based on specific examples. First Embodiment This first embodiment is based on the configuration of the robot and the working environment.
Enter the position data to graph the robot and work environment.
Display on the physical display to show the robot operation
When performing a simulation,
This is a device that checks for interference between a bot and an object. As shown in FIG. 2, the present device is a computer device.
CPU 1 that executes logical operations and the processing
ROM2 that stores the program indicating the order and various data
RAM3 and robot and work environment position and shape data
Input or give various commands such as a simulation start command.
Data input device 4 and the posture of the robot that changes over time
Graph that displays the working environment at each minute sampling time
Display 5, data input device 4, and graphics
Input / output interface for connecting the
Interfaces 6 and 7. Also, RAM3
The shape of each component of the bot arm and working environment
Shape data that stores information on faces, vertices, and edges that represent
Memory and each sphere when the surface of each component is hierarchized
A sphere that stores information about the center position, radius, and hierarchy of objects
Model memory 32 and the current joint coordinates of the robot
The joint angle memory 33 that stores
A member position memo that stores the current position of each component
A rib 34 is formed. Next, the operation of this apparatus is shown in FIG.
This will be described according to the flowchart of FIG. In step 100, first of all, each configuration of the robot and the work environment
Data for generating each shape for each member is
Input from the data input device 4. By this input, each configuration
Data such as vertices, ridges, and faces representing the shape of the member are calculated.
You. These shape data are stored in the shape data memory 31.
It is. Next, in step 102, each component of the robot and the working environment is
Regarding components, their relations and positions in absolute coordinates
Is input from the data input device 4. This input
Graphic display as data is entered
5 displays the robot and the work environment.
Easy detection of input data errors and data entry
It is configured as follows. And robots and work environment
The relationship and position data are stored in the member position memory 34.
It is. When the input of the position data in step 102 is completed,
At step 104, the data input device 4 becomes an object of interference.
Data specifying a robot and a work environment is input. Next, in step 106, the
Robot or work environment for each component
The surface of each component is composed of three types of spheres, large, medium and small.
Hierarchical coverage and hierarchical sphere modeling of the surface
It is. The sphere modeling of this surface is performed as follows
You. As shown in FIG.
The outer envelope 12 having the shape of a rectangular parallelepiped is stored in the shape data
It is generated from the surface information data referenced from the file 31. Furthermore,
Consider a sphere circumscribing the outer envelope 12 of the
I do. In this manner, the large sphere 14 at the top level is
Is generated as a sphere containing Then, the constituent member 11
Of the center of the large sphere 14 and its half as viewed from the reference coordinates taken
The diameter is stored in the spherical model memory 32. Next, as shown in FIG.
For a radius rm ofIs divided into a plurality of cells 17 of the cube. Of each side of the envelope 12
The length is not necessarily a multiple of the length of the side of cell 17
Thus, the cells 17 are arranged so as to overlap each other. Next
5B, spheres circumscribing each cell 17
Generates a body 20 and, among the spheres 20, the surface of the constituent member 11
Extract only spheres that intersect with 13, and extract the extracted spheres
The middle sphere is set to 15. Then, the reference coordinates taken for the constituent member 11
The center position of each sphere 15 as viewed from the
Remembered. Next, as shown in FIG.
Attention is paid to the cell 17, and for one cell 17,
Let the radius of the sphere be rs and one sideIs divided into cubic cells 18. At this time, each cell 18
Arrange so that they overlap as in the case of a sphere
Have been. Next, each sphere 21 circumscribing each cell 18 is emitted.
And extract only spheres that intersect with the surface 13 of the component 11
Then, the extracted sphere is referred to as a small sphere 16. And the composition
The center position of each small ball 16 viewed from the reference coordinates taken on the member 11
Is stored in the sphere model memory 32. In this way, as shown in FIG.
Includes the large sphere 14 enclosing the whole and the surface 13 of the component 11
The smaller spheres 15 and smaller spheres 16
Are modeled hierarchically by three types of spheres. That is,
According to FIG. 8, all small balls are attached to any one middle ball.
Tree, and all of the middle spheres belong to the large sphere
You can see that they are hierarchized in a shape. The radius rs of the small ball 16 is determined in consideration of the accuracy of the interference determination.
Is set to Next, the radius rm of the medium sphere 15 is the radius rl of the large sphere
From the radius rs of the globule andAutomatically asked by According to this formula, small and medium spheres, medium
The volume ratio between a sphere and a large sphere can be made equal. This allows
An efficient hierarchical structure is obtained for the search at the time of negotiation determination. Also, as an exception, since the outer dimensions of the constituent member 11 are small,
If the diameter of the ball 14 is smaller than the diameter of the given small ball 16
The small ball 16 is not generated, and a model of only the large ball 14 is used. Hierarchical sphere modeling of each component is completed as described above
Then, the process proceeds to step 108, where the robot moves.
An operation command is input from the data input device 4. Next,
Move to step 110 to support small movements relative to the interpolation point
Then, the joint coordinates of the robot after the minute movement, that is,
The joint angle is calculated and the joint angle data is stored in the joint angle memory.
It is stored in 33. Next, when the calculation of the joint angle is completed,
Move to step 112, and reference coordinates taken for each component
The transformation matrix T relating the system and the absolute coordinate system is
It is calculated from the joint angle. The above transformation matrix T is
Or a reference seat for each component of the working environment
The position and orientation viewed from the absolute coordinates of the target are represented by a 3 × 4 matrix.
It was done. And the reference seat taken for each component
Let Prel be the vector representing the center position of each sphere as viewed from the target.
Is a vector that represents the center position of each sphere viewed from the absolute coordinates
Le Pabs isRequired by Objects with moving components such as robots
For each operation, a transformation matrix T is calculated from each joint angle
Center position of all spheres to calculate and model components
Need to update the absolute coordinates of. However, this absolute coordinate
In order to improve the calculation efficiency, the interference check
It may be performed only for the sphere to be locked. In this way, the spherical model memory 32 stores
The center position Prel of each sphere viewed from the taken reference coordinates and the current position
Center position Pabs from the absolute coordinates of each sphere at the time of day
Is stored. Next, the process proceeds to step 114, and
The calculation of the negotiation check is executed. In step 114, the spheres are in the order of large sphere, medium sphere, and small sphere
Are performed hierarchically, and in step 116, the
When it is determined that there is interference between spheres (small balls) in the lower hierarchy
Then, the process proceeds to step 118, where the occurrence of interference and the interference portion
From the center of the small spheres that interfere with each other
And the robot in the interference state in step 120
And the work environment are displayed, and the program ends.
You. Next, in step 114, the large ball, the medium ball, and the small ball are used.
The hierarchical check procedure is shown below. step1. As shown in Fig. 9 (a), the large balls 14a and 14b
Check, and if there is something that interferes, execute step 2.
I do. If not, the check ends. Therefore, first, each
Until the components come close to some degree, interference
Calculation time is extremely short. step2. As shown in FIG. 9 (b), one large ball 14a
Interference with the middle spheres 15b1 to 15bn contained in the opponent's large sphere 14b
Performs checks and extracts interfering middle spheres 15b1, 15b2, 15b3
I do. On the other hand, on the contrary, the large ball 14b and the large ball 14a
Performs a collision check with the spheres 15a1 to 15an and interferes with the middle sphere
Extract 15a1 and 15a2. There must be no middle sphere to interfere
If the two components do not interfere, the calculation is interrupted
I do. step3. As shown in Fig. 9 (c), the extracted
b1, 15b2, 15b3, 15a1, 15a2
Extract the spheres 15b1, 15b2, and 15a1 that interfere with each other, and execute step 4.
Run. Also, when there are no spheres that interfere with each other
Determines that the two components do not interfere and checks
Interrupt the operation. step4. As shown in Fig. 9 (d), one of the interfering
The ball 15b2 and the small balls 16a11 to 16a1n included in the opponent's middle ball 15a1
₁Check for interference with the spheres 16a11 and 16a12
Extract. And, conversely, one interfering sphere 15a1,
Small balls 16b21 to 16b2n included in the opponent's medium ball 15b2_TwoInside
Then, the interfering small ball is extracted, and the next step 5 is executed. Ma
If there are no interfering small spheres, the middle sphere 15b1,
Small balls 16a11-16a1n belonging to the opponent's medium ball 15a1₁Interference with
Is performed similarly. Where no interfering globules are detected
If so, do the same for the other spheres that interfere with each other
When there are no more spheres that interfere with each other,
It is determined that the component does not interfere. step5. As shown in FIG. 9 (e), the extracted small balls 16
a11,16a12,16b21,16b22,16b23
The small spheres 16a11 and 16b21 that interfere with each other are extracted. And
In this case, the two components are the parts of the small balls 16a11 and 16b21.
It is determined that interference occurs at the position. Also, there are small balls that interfere with each other.
If not, extract the other spheres that interfere with each other
Will return to. Eventually, the interference between small balls in the lowest hierarchy is finally detected
Is judged as having interference, and the position of the small ball interferes
Judge as a part. When there is no interference between small balls
Means that the processing of the other middle spheres
Run and there are no interfering spheres
Is finally determined to have no interference. Therefore, FIG.
Along the possible branches of the tree structure shown in
As a result of the search, the interference of all small balls uniformly
Are several steps faster than simultaneous determination.
You. In addition, when determining the interference of spheres, the radius of two
r₁, R_Two, Center position P₁, P_TwoWhere r₁+ R_Two≧ ‖P₁−P_TwoIt is determined by whether or not ‖ is satisfied. Satisfy the above formula
Suppose the two spheres interfere. Hierarchical check of large, medium and small spheres as described above
Simplifies and speeds up the interference generating section
Can be checked. In step 116, interference occurs at the lowest-level sphere.
If it is determined that no interference will occur, interference between the components will occur.
Since it has not been produced, go to step 122
Calculation of interference check for all combinations between components
It is determined whether or not execution has been performed.
Return to step 114 if the calculation has not been completed.
Interference check between the components
You. In addition, interference check for all combinations of components
When the calculation is completed, the process proceeds to step 124 and all
Check operation completed for the specified object
It is determined whether or not it is not completed.
Is similarly performed. In step 124, a combination of all specified objects
If it is determined that the interference check calculation has been completed
Moves to step 126, and is stored in the joint angle memory 33.
The posture of the robot is calculated from the joint angle at that time,
The robot is displayed in that posture and the work environment is displayed.
Done. Next, go to step 128 and move all
It is determined whether the operation has been completed. If the move is not completed
Returns to step 110 and returns to the next
The regular processing is repeatedly executed. Also, it is determined that the movement is complete
Then, this program ends. As explained above, the device according to the present invention is
Easily satisfy both conditions of accuracy and calculation speed.
Can be used, for example, during rough motion of a robot.
This is particularly effective when performing a high-speed interference check. In addition, the device of the present invention can be used to form members of any shape with the same accuracy.
In addition to modeling, offline simulation is also possible for real robots
It is a highly versatile device that can be applied to data. Second Embodiment The second embodiment considers the size of an object, the radius of the smallest sphere, and the like.
To optimize the search efficiency for the site where interference occurs.
Automatically determines the number of layers in the spherical model and the radius of the sphere in each layer.
Is set to The differences from the first embodiment will be described below.
Only the points will be described. In the hierarchical sphere model generating means B, the object is included
Radius of the largest sphere r₁, The smallest sphere that determines the accuracy of the interference check
Radius of r_nAnd the radius ratio ρ to the lower sphere₁To give
Thus, the depth of the layer (the number of layers) n and the radius r of the sphere at each layer_i
Is calculated by the following equations (1) and (2), respectively. Here, n is an integer rounded off. Here, i = 1, 2,... N Here, the derivation process of the equation (1) is shown. The radius of the sphere at each level is r_iThen the lower sphere (radius r_{i + 1})
Volume ratio ρ_vIs represented by equation (3).ρ_vIs constant at each level, the radius r of the largest sphere₁And the smallest sphere
Radius of r_nIs expressed by equation (4). Here, the radius ratio of the upper and lower spheres is ρ_rThen
From equation (3), ρ_v= Ρ_r ^ThreeTherefore, equation (4) becomes
It is expressed like a formula. r₁ ^Three= R_n ^Three・ Ρ_r ^{3 (n-1)} ... (5) By taking the logarithm of both sides of equation (5), equation (1) becomes
can get. In addition, the above equation (3) is given by r_i ^Three= R_n ^Three・ Ρ_r
^{3 (n-1)}Is derived from both equations. In the above, the volume ratio ρ of the upper and lower spheres_vOne
That is, the upper sphere contains a certain number of lower spheres
By defining the relationship, each sphere has
The number of lower spheres to be made can be made uniform throughout,
Number of hierarchies from the largest sphere to the smallest sphere and spheres in each hierarchy
Can be set optimally for search efficiency. Furthermore, the area ratio ρ of the upper and lower spheres_s= (R_i/ R_{i + 1})^TwoIs the same,
Radius ratio ρ of the sphere_rNeedless to say, a certain relationship
not. Note that the radius ratio ρ_rIs set according to the size and shape of the object
In this embodiment, it is set to about 2. The number of hierarchies n determined as described above and the ball half in each hierarchy
Diameter r_iIs used to generate a spherical model of the object as follows:
You. Step 1: rectangular parallelepiped Rec1 containing the object and its rectangular parallelepiped Re
Generate a sphere Sph containing c1. (Hierarchical level: k =
1) Step 2: Set the hierarchical level to k = k + 1 and set the rectangular parallelepiped Rec
k-1 to radius r_kCube cell inscribed in the sphere of
Side). Where each cubic cell is a rectangular
Make each side of the body Rec k-1 coincide with the side of each cube
Since they are arranged, they are shown by hatching in FIG.
In most cases, each cubic cell is
Overlap. This overlap portion is described below.
And reconstructed as a rectangular parallelepiped Rec k, j
Step 3 is executed for (j = 1, 2,...). Step 3: The sphere enclosing the rectangular parallelepiped Rec k, j intersects the surface of the object
Find out if you are If you do,
A sphere is added to the data of the sphere model and k <n (ie,
If the layer level k is not the lowest level), the rectangular parallelepiped Rec
Step 2 is further executed for k + 1. Here, about the deletion of the overlap part of the cubic cell
explain. As described above, at each hierarchical level, the lower sphere (
(Overlying cells)
And for cells that come later,
It is defined as a rectangular parallelepiped cell excluding the gap.
That is, in FIG. 10 (b), the cubic cell Rec k−1
The radius r_kEach cuboid Re by the 9 spheres Sph k, 1 to Sph k, 9
When dividing into c k, 1 to Rec k, 9, the first sphere Sph k, 1
Decide the cubic cell Rec k, 1 so that it touches, and the next sphere Sph k, 2
, The value sphere Rec in the cube inscribed in Sph k, 2
 The part excluding the overlap with k, 1 is the rectangular parallelepiped cell Rec k,
Defined as 2. Hereinafter, the same applies to Sph k, 3 to Sph k, 9.
Perform the same operation to make each rectangular parallelepiped
The cells Rec k, 1 to Rec k, 9 are defined. Next, the lower sphere Sph
When generating k + 1, i, each of the rectangular parallelepiped cells Rec k, 1 to R
Since ec k, 9 is divided in the same manner as above,
The overlap of the spheres Sph k + 1 and i generated in the
The number of sphere data can be reduced to a necessary minimum. Like this
When expressing (modeling) the surface of an object,
Since the number of spheres in the hierarchy can be minimized,
The search efficiency at the time of interference check does not decrease. Next, each layer of the objects X and Y generated as described above
The number is n_X, N_YSphere interference model
Recursive algo for hierarchical interference checking in means B
The rhythm is shown below. This algorithm is based on the number of layers
Are not equal (n_X≠ n_Y) Can also be applied, but basic
Are the same as in the first embodiment. In addition, the following
"Return true / false" in each step is one step before
Means return true / false to the step. Step 1: Top level of each of two objects X and Y (hierarchical level k
= 1) For each sphere data, the possibility of interference
Put a mark to represent (set the flag to 1) Step 2
Execute. Step 2 gives a true result
, It is assumed that objects X and Y are interfering with each other.
You. Otherwise (a false result is obtained
), Objects X and Y do not interfere with each other
I do. Step 2: check objects belonging to objects X and Y respectively
Perform step 3 for all combinations of spheres. Stay
If step 3 is true, return true. Also, in step 3
Returns false if there is no ue combination
You. Step 3: Each sphere data belonging to each object X and Y in one combination
Are marked with potential interference
If so, check for interference between the spheres. That's right
If it returns false. As a result of the above check, the balls
If so, go to step 4 for the two balls
Execute. Returns false if not interfering. Also,
True if the result of step 4 is true, false if false
return it. Step 4: Both spheres are at the lowest level (n_X, N_Y)
If it is determined that the objects X and Y are interfering with each other,
Returns ue. Otherwise, either object X or Y
If only the sphere to which the sphere belongs is the lowest level, go to step 5,
If both spheres are not at the lowest level, skip to step 6.
Execute each one. Step 5 and Step 6
Returns true if the result is true, false if false. Step 5: Sphere of lowest hierarchy belonging to one of objects X and Y
And one level of a sphere of a non-lowest level belonging to the other object
For all combinations with spheres in the lower hierarchy, interference between spheres
Check. If there is interference, execute step 4.
You. Returns false if there are no interfering spheres. Step
True if the result of step 4 is true, false if false
You. Here, all marks are cleared (the flag is set to 0).
), Then set the interference between objects again.
You can start immediately from step 1
Keep it. Step 6: One sphere belonging to object X and one sphere belonging to object Y
All unions with a plurality of spheres one level below one sphere
Check for interference between balls. Interference occurs
If it is, one layer of one sphere belonging to the object Y
Mark the sphere data in the lower hierarchy. Marked
Returns false if no sphere exists. Similar processing
To one sphere belonging to the object Y and one sphere belonging to the object X
All combinations with multiple spheres one level below the sphere
Even do. Thus, the sphere on the object X side and the object Y
For each ball on the side, gradually lower the level of the hierarchy and rank higher
Select a sphere that may interfere with the sphere in the hierarchy. Soshi
If there is a marked ball, repeat step 2 for all pairs
Execute for matching. If the result of step 2 is true
Returns true, false if false. Note that here
Clear all marks (set the flag to 0) as in step 5.
To). Next, to perform interference determination according to the above algorithm
The processing procedure of CPU 1 is explained based on FIGS. 12 (a) and 12 (b).
I will tell. (A) The number of layers of the hierarchized X and Y spheres is the same
Case. In step 200, the current search hierarchy i of the object X is 1
, The current search hierarchy j of the object Y is initially set to the highest hierarchy of 1.
Then, the interference flag of the highest sphere is turned on. Top sphere
By turning on the interference flag of
The search starts. Next, in step 202, the current search
In the search hierarchy (i, j), the global flag for which both interference flags are on
It is determined whether the pair search has been completed. Judgment result is NO
In the case of, the steps after step 204 are executed,
If the judgment result is YES, the steps from step 250 onwards
Is executed. During the first run for the top sphere,
Since the search for the ball pair has not been completed, step 20
The determination result of 2 is naturally NO. In step 204, the sphere with the interference flag turned on,
One X sphere belonging to hierarchy i and one Y belonging to hierarchy j
A sphere is selected and one unsearched sphere pair (iX, jY) is generated
Is done. In the next step 206, the
The collision calculation of the pair of spheres (iX, jY) is executed, and the next step 2
At 08, it is determined whether or not the two balls iX and jY interfere.
Incidentally, the interference calculation is performed by using two spheres as shown in the first embodiment.
Is less than the sum of the radii of the two spheres
It is determined by whether or not. Next, in step 210, the ball pair (iX, jY) is
It is determined whether or not it exists in the layer, and if the determination result is NO,
Moves to step 212. In step 212, the ball pair
It is determined whether one sphere of (iX, jY) exists at the lowest level.
If the determination is NO and the determination result is NO, proceed to Step 214.
Run. In step 214, the sphere iX and the sphere jY
Interference determination is performed for all lower spheres. And dried
In the case of interference, the interference flag of the lower sphere of that sphere jY is turned on.
It is said. Next, the process proceeds to step 216, and step 214
As a result of the interference determination, at least one of the lower spheres of the sphere jY
Is determined whether or not interferes with the ball iX, the determination result is
If YES, step 218 is executed. In step 218, similarly to step 214, the sphere jY
And all lower-level spheres belonging to sphere iX
A determination is made. And if it interferes, the ball iX
Of the lower sphere is turned on. Then, step
The process proceeds to 220, and as a result of the interference determination in step 218, the ball iX
Whether at least one of the lower spheres is interfering with sphere jY
Is determined, and if the determination result is YES, step 2
22 is executed. Next, in step 222, the current search hierarchy (i, j) is displayed.
Current search sphere pair so that the next search sphere pair
(IX, jY) is stored in the area D (i, j). Also, the current search floor
Other parameters required to continue processing after returning to layer (i, j)
Are also stacked. Next, in step 224, the current search hierarchy (i, j) is:
Each is lowered by one level. Note that the current search hierarchy (i, j) is a sphere pair (iX, j
It is defined as the hierarchy where Y) exists. That is, parent ball iX
And interference calculation with the lower sphere one level lower belonging to the parent sphere jY
The current search hierarchy (i, j) when the line is executed is the parent sphere iX, jY
Defined as the hierarchy to which it belongs. In addition, the current search sphere pair (iX, j
Y) is the ball pair of the parent ball iX and the parent ball jY that are receiving attention.
Is defined as Thus, in steps 222 and 224, the return
When the tack process and the update process of the current search hierarchy are completed,
Returning to step 202, the state where the current search hierarchy is the next lower hierarchy
In this state, the above processing is repeated. Step 224 does not branch from step 202 above.
Repeatedly, the search branch searching for the pair of interference spheres is X
Both the sphere and the Y sphere extend to the lower hierarchy by the shortest path. Incidentally, in the routine of steps 202 to 224 described above,
Branching the processing step according to the judgment result
However, by branching, the search branch becomes
Return to the upper layer and extend horizontally at the upper layer and lower
It progresses by extending to the hierarchy. In step 208 of the above branching steps, the ball pair (i
X, jY), if it is determined that there is no interference, return to step 202
In step 204, the interference flag is set to ON.
Other sphere pairs that belong to the current search hierarchy (i, j)
(IX, jY) is generated. Then, after step 206
And, as mentioned above, the ball pair (iX, jY)
If interference is determined and interference occurs, the parent ball iX and the parent ball jY
Judgment of interference with the lower child ball to which it belongs, parent ball jY and parent ball iX
The collision determination with the child sphere belonging to the lower rank is executed, and the interference is determined.
The interference flag of the child sphere is turned on. Also step
At 216, there is a child ball that interferes with the parent ball iX belonging to the parent ball jY.
If it is determined that there is no, or in step 220, the parent ball iX
Is determined that there is no child sphere that interferes with the parent sphere iY belonging to
The parent sphere iX and the parent sphere jY do not interfere
From the ball pair (iX, jY) to the lower hierarchy
Therefore, there is no need to extend the search branches. So in this case
Return to step 202, and in step 204, the current search floor
Another sphere pair (iX, jY) belonging to layer (i, j) is generated and
Search from the sphere pair (iX, jY) by the processing below 206
The branches are stretched. Also, in step 202, a search file belonging to the current search layer (i, j) is searched.
If it is determined that all of the lag-on ball pairs have been searched,
Moves to step 250. In step 250, the current search hierarchy (i, j) is
It is determined whether or not the layer is (1,1). Judgment result is NO
In this case, the process proceeds to step 252, where the current search hierarchy (i, j)
The global pairs that were searched for belong to
Therefore, the interference flags of the spheres that make up the global pair are turned off.
Is done. Then, the process proceeds to step 254, where the current search hierarchy (i, j)
Is raised one level. As a result, the search branch is in the above hierarchy
Will return to. When a search branch extends from an upper hierarchy to a lower hierarchy,
In step 222, the data of the current search sphere pair on the higher hierarchy is
However, in step 256,
When returning to the upper hierarchy, the data of the current search
Data is restored. Therefore, when returning to step 202,
In the upper layer of, the next unsearched ball pair
The search will continue. On the other hand, in step 250, the current search hierarchy (i, j) is
If it is determined to be the upper layer, the highest sphere does not interfere
Means that. Therefore, in this case, step 25
8 and concludes that object X and object Y do not interfere.
Will argue. On the other hand, in the process of searching for interference spheres,
In step 210, it is determined that interference occurs in step 208.
Sphere iX and sphere jY of the current search sphere pair (iX, jY)
When it is determined that the object is in the hierarchy, the object X and the object Y
It is determined that there is interference at the part where the ball pair (iX, jY) exists.
Can be specified. So, in this case, step
Go to 226 and conclude that the two objects interfere
Can be. (B) When the number of layers of the stratified X sphere and Y sphere are different
Go. Either sphere of the current search sphere pair (iX, jY) is the lowest
Steps 200 to 224 and steps
Steps 250 to 258 are performed. In the above process, in step 212, the current search sphere
One sphere of the pair (iX, jY) is determined to be the lowest hierarchy
Then, step 280 and subsequent steps are executed. In step 280, the current search sphere pair (iX, jY)
When the ball in the rank hierarchy is L and the opponent ball is M, the ball L and the opponent ball
The lower sphere one level below the sphere M, that is, one of the child spheres
Sphere kM (k is a variable that changes with the child sphere) is selected,
An unsearched sphere pair (L, kM) is generated. And the next step
In step 282, an interference calculation of the ball pair (L, kM) is performed, and
In step 284, whether the ball pair (L, kM) interferes
Is determined. If the determination is YES, step 286
To the hierarchy where the parent ball L and the parent ball M exist in the future.
When returning to the current search hierarchy (i, j) defined by
Next, the search for the interference sphere at that level is performed so that the parent
The current search sphere pair (iX, j) defined by the sphere pair of the sphere L and the parent sphere M
Y) (= (L, M)) is stored in the area D (i, j). or,
Other parameters required for return are stacked. Next, in step 288, the current search where the parent ball exists
The hierarchy (i, j) is lowered by one hierarchy. At this time, the ball iX is at the bottom
If it exists in the rank hierarchy, the hierarchy j of the Y sphere is lowered by one hierarchy
Can be On the other hand, if sphere iY is at the lowest level
, The hierarchy i of the X sphere is lowered by one hierarchy. Next, the process proceeds to step 290, where the current ball L and the child ball kM
Is regarded as a new parent sphere in the process of searching for the next interference sphere
It is. For this purpose, the current search ball pair defined by the parent ball pair
(IX, jY) is redefined as a sphere pair (L, kM). Soshi
Return to step 210 to search for the next interference sphere.
You. Thus, if one sphere belongs to the lowest hierarchy
Only the other sphere is searched for
You. In step 284, the ball pair (L, kM) is determined to be non-interfering.
Then, the process proceeds to step 292, where the child ball kM belongs to the parent ball M.
It is determined whether it is the last ball to play. When the judgment result is NO
Returns to step 280 to obtain another child ball kM belonging to the parent ball M.
Is selected. Then, after step 282, the same as above is performed.
As described above, the collision determination of the ball pair (L, kM) is performed. In step 292, the child ball kM is the last ball belonging to the parent ball M
If it is determined that there is, the parent ball L and the parent ball M
Means that the current search sphere pair (i
X, jY) must be changed. Step 294 therefore
Exists. In step 294, in the current search hierarchy (i, j), the X sphere
It is determined whether or not the layer i of layer Y is equal to the layer j of layer Y.
You. If the determination result is YES, the other
Since it is sufficient to generate a ball pair, the process returns to step 202. If the decision result in the step 294 is NO, the current search sphere pair (i
X, jY) is the relationship between the parent sphere L and the parent sphere on the next hierarchical level to which the parent sphere M belongs.
Returned to ball pair, search for interfering sphere continues from that state
There is a need. To this end, in the next step 296, the current search
The hierarchy (i, j) is moved up one hierarchy. In this case, the ball iX
Exists in the lowest hierarchy, the hierarchy j of the Y sphere is the first floor
Can be raised. In contrast, the sphere iY exists at the lowest level
In this case, the level i of the X sphere is increased by one level. Also,
At step 298, in the updated current search hierarchy (i, j),
The last search sphere pair that was searching for interference spheres is the current search sphere pair
It is restored as (iX, jY). The ball pair was previously
In the executed step 286, the data is stored in the area D (i, j).
Value. Then, the process returns to step 210, and YES is determined in step 212.
The process proceeds to step 280. And step 280
Then, with the current search sphere pair (iX, jY) as the parent sphere,
A search sphere pair (L, kM) is generated and interferes with the sphere pair
A determination is made. As explained above, a sphere that may interfere
Is searched in order toward the lower hierarchy only for
Therefore, the search speed is improved. Next, the above-described interference sphere search algorithm and the CPU 1
How the interfering sphere is found
Will be described with reference to an example shown in FIG. In FIG. 11, the object X has five layers and the object Y has three layers.
As in FIG. 8, the case of the sphere model
From the top sphere (sphere x for object Y, sphere y for object Y)
Sphere (for object X, sphere x₁₁₁₁, ……, x_mmmm, The sphere in object Y
y₁₁, ……, y_nn) Are shown.
In the figure, ~ indicates interference between spheres between objects.
It corresponds to the following description. Further
In the following description, "(x, y) interference check"
Means to perform interference check between sphere x and sphere y
I do. First, an interference check between the object X and the object Y is performed.
At the time of execution, with a mark indicating the possibility of interference for x and y respectively
(The interference flag is turned on). (X, y) interference check
It is assumed that there is interference. Y in the lower layer of x and y that were determined to have interference₁, Y_Two, ……, y_n
And all combinations (x, y₁), (X, y_Two) …… (x, y_n) Dried
X and y₁And y_TwoInterference with
, Y₁, Y_TwoTo each of them. Similarly, x in the lower hierarchy of y and x₁, X_Two,…, X_mAll with
The interference check of the combination of₁, X_TwoTo each
Make a mark. All combinations of marked spheres (x₁, Y₁),
(X₁, Y_Two), (X_Two, Y₁), (X_Two, Y_Two) Interference check
X₁And y₁X due to interference with₁, Y₁Each with interference
And I.e. unmarked x_Three,…,
x_mAnd y_Three,…, Y_nNo interference check is performed. As in x₁, Y₁For x₁And y₁
Y in the lower hierarchy of₁₁, Y₁₂,…, Y_1nAnd all combinations of dried
Interference check indicates that there is interference₁₁, Y₁₂Mark each
do. As in x₁, Y₁For y₁And x₁
X in the lower hierarchy of₁₁, X₁₂,…, X_1mAnd all combinations of dried
Interference check indicates that there is interference x₁₁, X₁₂Mark each
do. All combinations of marked spheres (x₁₁, Y₁₁),
(X₁₁, Y₁₂), (X₁₂, Y₁₁), (X₁₂, Y₁₂) Interference che
X₁₁And y₁₁X due to interference with₁₁, Y₁₁To
Each is assumed to have interference. Note that y₁₁Is the lowest hierarchy of object Y
Since this is a sphere, the sphere to be checked for object Y is always
To y₁₁Becomes X with interference₁₁, Y₁₁For y₁₁And x₁₁Lower hierarchy of
X₁₁₁, X₁₁₂,…, X_11mFor all combinations with
Perform interference check. x₁₁₁And y₁₁There is no interference so next
To (x₁₁₂, Y₁₁2) Check for interference. As a result, x
₁₁₂And y₁₁X because there is interference between₁₁₂With interference
And the check target is x₁₁₂To x₁₁₂Lower hierarchy of
Change to a ball. y₁₁And x₁₁₂X in the lower hierarchy of₁₁₂₁Check for interference with
Since there is no interference, (x₁₁₂₂, Y₁₁) Interference check
Do. As a result, x₁₁₂₂And y₁₁Interference has occurred between
For x₁₁₂₂Is assumed to have interference. x₁₁₂₂Is the lowest level sphere
Because there is, processing ends here, the next x₁₁₂₃about
Do not check for interference regardless of the presence or absence of interference. Thus, object X and object Y are sphere x₁₁₂₂And sphere y₁₁The presence of
It is immediately determined that there is interference at the part where the light is emitted. However, if you want to know the presence or absence of interference for all parts of the object
If x is excluded from the check₁₁₃,…, X
_11mFor all, y₁₁It is good to check interference with
No. As explained above, between the sphere of the higher hierarchy of other objects
Check that there is a possibility of interference
Perform interference check only on attached spheres
The number of ball combinations to be checked is extremely small
The efficiency of interference checking between objects
Can be greatly enhanced.

【発明の効果】【The invention's effect】

本発明は、上記したように、各物体の表面を被覆する
球体でモデル化しており、物体内部及び外部はモデル化
から除外しているので、球体の数が少なく階層構造が単
純化されると共にモデル化速度が向上する。従って、干
渉チェックの演算時間が短くなるという効果を有してい
る。 又、球体でモデル化しているので、物体の移動に伴う
球体の移動は、その球体の中心位置の移動演算だけ行え
ば良く、移動に伴う球体の更新演算が簡略化される。 更に、干渉判定の精度は最小球の半径に依存し、その
最小球の半径を任意に設定できるので、干渉判定精度を
任意に変化させることができる。 更に、階層的にモデル化しているので、階層を深くす
ることにより演算時間の短縮化を図ることもできる。 更に、球体の干渉判定を干渉の可能性の高い上位階層
の球体から順次その干渉球体に属する下位階層の球体に
対して階層的に実行しているので、干渉判定速度が向上
するという効果がある。即ち、粗いチェックから詳細な
チェックへ移る過程において干渉の可能性のない部分を
除外していくことにより、干渉発生部を効率良く絞り込
むことができる。The present invention, as described above, is modeled by spheres covering the surface of each object, and the inside and outside of the object are excluded from the modeling, so that the number of spheres is small and the hierarchical structure is simplified and Modeling speed is improved. Therefore, there is an effect that the calculation time of the interference check is shortened. In addition, since the sphere is modeled, the movement of the sphere accompanying the movement of the object only needs to be performed for the movement of the center position of the sphere, and the calculation for updating the sphere accompanying the movement is simplified. Further, the accuracy of the interference determination depends on the radius of the minimum sphere, and the radius of the minimum sphere can be set arbitrarily. Therefore, the accuracy of the interference determination can be changed arbitrarily. Furthermore, since the modeling is performed in a hierarchical manner, the calculation time can be reduced by making the hierarchy deeper. Furthermore, since the collision determination of a sphere is performed hierarchically on spheres of a lower hierarchy belonging to the interference sphere sequentially from a sphere of a higher hierarchy having a high possibility of interference, there is an effect that the interference determination speed is improved. . That is, in the process of shifting from a coarse check to a detailed check, a portion where there is no possibility of interference is eliminated, so that an interference generating unit can be efficiently narrowed down.

【図面の簡単な説明】[Brief description of the drawings]

第１図は本発明の全体の構成を示したブロックダイヤグ
ラム、第２図は本発明の具体的な実施例に係る装置の構
成を示したブロックダイヤグラム、第３図は同実施例の
装置に使用されたCPUの処理手順を示したフローチャー
ト、第４図、第５図、第６図は、それぞれ、大球、中
球、小球の発生方法を示した説明図、第７図、第８図は
球体モデルの階層構造を示した説明図、第９図は干渉判
定の論理を示した説明図、第10図はオーバラップのある
立方体セル及びオーバラップ部を削除した直方体セルと
それらを内包する球の関係を示した模式図、第11図は階
層数の異なる球体モデル間の干渉チェックを示した説明
図、第12図は干渉球の探索を行うCPUの処理手順を示し
たフローチャートである。 １…CPU、３…RAM 31…形状データメモリ、32…球体モデルメモリ 33…関節角メモリ、34…部材位置メモリFIG. 1 is a block diagram showing the overall configuration of the present invention, FIG. 2 is a block diagram showing the configuration of an apparatus according to a specific embodiment of the present invention, and FIG. 4, 5, and 6 are flowcharts showing the processing procedure of the selected CPU, and are explanatory diagrams showing the method of generating a large sphere, a medium sphere, and a small sphere, respectively, and FIGS. Is an explanatory diagram showing the hierarchical structure of the spherical model, FIG. 9 is an explanatory diagram showing the logic of interference determination, and FIG. 10 is a cubic cell having an overlap and a cuboid cell with the overlap portion removed and including them FIG. 11 is a schematic diagram showing a relationship between spheres, FIG. 11 is an explanatory diagram showing an interference check between sphere models having different numbers of layers, and FIG. 12 is a flowchart showing a processing procedure of a CPU for searching for an interfering sphere. 1 CPU, 3 RAM 31 Shape data memory, 32 Spherical model memory 33 Joint angle memory, 34 Member position memory

フロントページの続き (72)発明者 久野 敏孝 愛知県愛知郡長久手町大字長湫字横道41 番地の１ 株式会社豊田中央研究所内 (56)参考文献 特開 昭58−171615（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁶，ＤＢ名) G05D 1/02Continuation of the front page (72) Inventor Toshitaka Kuno 41-1 Okucho, Yokomichi, Nagakute-cho, Aichi-gun, Aichi Prefecture Inside Toyota Central Research Laboratory Co., Ltd. (56) References JP-A-58-171615 (JP, A) (58) Field surveyed (Int.Cl. ⁶ , DB name) G05D 1/02

Claims (1)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】移動する物体を含む複数の物体に対し、物
体相互の干渉をチェックする装置において、 各物体の表面を表す面情報を記憶した面情報記憶手段
と、 ある階層の任意の１つの球体に対して、その球体に内包
されるとともに１つ下位階層に属する複数の球体とで構
成されるような内包関係により少なくとも２階層以上に
階層化された球体を用い、前記面情報に基づいて前記各
物体の表面を各々複数種類の半径からなる球体により階
層的に被覆してモデル化する階層球体モデル生成手段
と、 前記生成されたモデル化球体の位置を記憶する球体モデ
ル記憶手段と、 移動体に関する前記複数種類の半径のモデル化球体の位
置を物体の移動に応じて更新して前記球体モデル記憶手
段に出力する球***置更新手段と、 前記球体モデル記憶手段に記憶された任意時刻の２つの
物体に関するモデル化球体における一方の物体に属する
球体と他方の物体に属する球体との２つの球体におい
て、それらの２つの球体の中心位置とそれらの球体の半
径とに基づいて、中心間距離が半径の和以下の場合にそ
の２つの球体が干渉し、中心間距離が半径の和よりも大
きい場合にそれらの球体は非干渉として、相互に干渉す
る球体を検出し、その干渉すると判断された球体同士の
みについてその相互に干渉する球体に属する下位階層の
球体の中からさらに干渉する球体を検出するという手順
を半径の大きい球体から順に半径の小さい球体へと階層
的に実行し、干渉球が存在しない時点で干渉演算を中止
すると共に、最終的に相互に干渉する最小半径の球体が
存在するときに２つの物体は干渉すると判断しその相互
に干渉する最小半径の球体の位置から２つの物体の干渉
位置を求め、これらの演算結果を出力する球体干渉演算
手段と を有することを特徴とする物体の干渉チェック装置。An apparatus for checking interference between a plurality of objects including a moving object, comprising: surface information storage means for storing surface information representing a surface of each object; For a sphere, a sphere that is included in the sphere and is hierarchized into at least two or more layers by an inclusion relation such that the sphere is composed of a plurality of spheres belonging to one lower layer is used, based on the surface information. Hierarchical sphere model generating means for hierarchically covering and modeling the surface of each of the objects with spheres each having a plurality of radii; sphere model storing means for storing the positions of the generated modeled spheres; Sphere position updating means for updating the positions of the plurality of types of modeled spheres with respect to the body in accordance with the movement of the object and outputting the updated sphere positions to the sphere model storage means; and the sphere model storage means. Of the two spheres belonging to one object and the sphere belonging to the other object in the modeled spheres of the two objects at the stored arbitrary times, the center position of the two spheres and the radius of the spheres If the center-to-center distance is less than or equal to the sum of the radii, the two spheres will interfere with each other, and if the center-to-center distance is greater than the sum of the radii, the two spheres will be deemed non-interfering and the spheres that interfere with each other will be detected. The procedure of detecting the spheres that interfere further from among the spheres in the lower hierarchy belonging to the spheres that interfere with each other only for the spheres that are determined to interfere with each other is performed hierarchically in order from the sphere with the larger radius to the sphere with the smaller radius. And stop the interference calculation when there is no interfering sphere, and finally judge that the two objects interfere when there is a sphere of minimum radius that interferes with each other. And a sphere interference calculating means for calculating the interference position of the two objects from the positions of the spheres having the minimum radius of mutual interference and outputting the calculation results thereof.