JP3402645B2 - ロボットの干渉判定方法 - Google Patents

Description

【発明の詳細な説明】 【０００１】 【産業上の利用分野】本発明は、ロボットと、このロボ
ットが作業する作業環境に存在する物体との干渉を判定
するためのロボットの干渉判定方法に関する。 【０００２】 【従来の技術】工場の中で組み立てや加工作業を行う産
業用ロボットを中心として、人間が作業を行うのに適し
ていない環境で人間の代わりに作業を行う極限作業用ロ
ボット等、ロボットはさまざまな分野で用いられるよう
になってきている。 【０００３】上記産業用ロボットや極限作業用ロボット
に動作を教示するために、産業用ロボットの場合には主
にティーチングプレイバック方式が採用され、極限作業
用ロボットの場合には人間による遠隔操作が採用されて
いる。 【０００４】ところが、産業用ロボットに動作の教示を
行うティーチングプレイバック方式では、人間がロボッ
トを実際に動かしてみて、そのときの動きをセンサ情報
として記憶、再生するわけであるから、ロボットに動作
の教示を行っている間は、ロボットに作業をさせること
ができない。このような教示にともなう作業停止期間
は、今後、多品種少量生産が要求されるようになるにつ
れ、工場の生産性に大きく影響する可能性がある。 【０００５】そこで、ロボット及びロボットの周囲の環
境を計算機内部に形状モデルとして作成しておき、これ
らの形状モデルをもとにロボットの動作を計算機上で生
成してしまおうというオフラインティーチングシステム
の開発が行われている。 【０００６】オフラインティーチングシステムでは、オ
ペレータがロボットの動作軌道をプログラミングし、こ
のプログラミングされた軌道が適当であり周囲の物体と
衝突しないかどうかを、ロボットの動作を画面上に表示
させてみることにより確認することができる。 【０００７】ところが、２次元の画面からは２次元的な
動きをしているロボットと周囲の物体との干渉を判定す
るのが難しい場合もあるので、計算機によって内部の形
状モデルをもとに計算により干渉の有無を判定されるこ
とが望ましい。 【０００８】また、極限作業用ロボットを遠隔操作によ
り操縦している際に、人間の判定ミスによってまわりの
境界の物体とロボットが衝突してしまうような操作をす
る可能性がある。このような操作ミスによる事故を防ぐ
ためには、ロボットの近傍に物体が存在しないかどうか
をセンサで計測することが考えられる。 【０００９】ところが、ロボット近傍に物体が存在しな
いかどうかを計測するためには、センサをロボット表面
全体に取り付けなければならず、容易ではない。 【００１０】そこで、ロボットの周囲の環境が予めわか
っている場合には、計算機内部に形状モデルを作成して
おき、ロボットを構成する各リンクと周囲の環境の物体
との干渉チェックを計算により行う方法が考えられる。 【００１１】このように、ロボット及び周囲の環境を構
成する物体の形状モデルを計算機の内部に作成してお
き、ロボットと周囲の環境が衝突しないかどうか、ある
いは衝突しそうなほど接近していないかどうかを判定す
ることが望まれている。 【００１２】しかし、上記のロボットと周囲の環境との
干渉チェック計算は、通常ロボットを構成する全てのリ
ンクと周囲の環境に存在する全ての物体との間の干渉の
有無の判定を行う。このために、周囲の環境が複雑なも
のになるにつれて、干渉判定のための計算量が非常に多
くなり、計算能力の優れた高価な計算機を必要とする上
に、さらに場合によっては長い計算時間を要することが
あった。 【００１３】このような問題点を解決するために、さま
ざまな研究、開発が行われている。例えば、特開平１−
２２４８１１号公報、特開平１−１７３２０５号公報で
開示されているように、ロボットの周囲の環境を構成す
る物体の形状を球などで近似して、まずこの近似球とロ
ボットとの干渉判定計算を行い、近似球とロボットとが
干渉しているときだけ実際の物体の詳細な形状とロボッ
トとの干渉判定計算を行うものである。これにより、大
部分の干渉判定計算を、ロボットを構成する多面体ある
いは円柱と近似球との計算量の少ない干渉判定計算とし
て行い、全体の計算量を低減する方法である。 【００１４】しかしながら、上記のような近似形状によ
る計算量の低減方法は、物体の形状により実際の形状よ
りかなり大きく近似された近似形状となる場合があるた
めに、かならずしも効果的に働かない場合がある。ま
た、ロボットリンクの姿勢により計算量がかなり低減さ
れる場合もあるが、姿勢によってはほとんど効果がない
場合も存在する。 【００１５】一方、日本ロボット学会誌Vol.5. No.3 に
記載の「オクトツリーを用いた高速干渉チェック法」で
は、ロボットの作業境界を格子状に区切り、それぞれの
格子内部に物体があるがどうかを予め計算して求めてお
き、この格子状に区切られた作業空間を階層構造表現と
しておくことにより、ロボットとその周囲の作業境界と
の干渉判定を高速化しようと試みている。 【００１６】しかし、この方法では、作業境界全体を格
子状に表現するための多量のメモリが必要となる。さら
に、作業途中にロボットが部品を移動した等の理由によ
り作業境界が変化した場合、予め計算しておいた格子状
に区切った環境のモデルを再び求め直したり、あるいは
修正したりする必要が生じるが、これに要する計算量は
かなり多くなるために、作業境界が変化する場合には不
向きであった。 【００１７】また、日本ロボット学会誌Vol.5. No.4 に
記載の「マニピュレータの障害物回避動作計画を考慮し
た凸多面体間の干渉チェック法」では、ロボット及び作
業境界を構成する物体を凸多面体の組み合わせで表現す
ることにより、任意の形状をほぼ正確に表現でき、また
凹な形状を含む場合と比較するとかなり少ない計算量で
干渉判定を行うことが可能となる。物体の位置が移動し
た場合のデータ変更も容易である。 【００１８】この方法では、 （１）一方の頂点と他方の面との干渉判定（頂点・面判
定） （２）両方の稜線同士での干渉判定（稜線判定） の組み合わせにより凸多面体間の干渉判定を行ってい
る。頂点と面との干渉判定は、一方の凸多面体の全ての
頂点が他方の凸多面体のある一つの平面を無限に広げた
平面の外側にあれば干渉していないという凸多面体の性
質を利用したものである。しかし、頂点と面との干渉判
定において、一方の凸多面体の全ての頂点が外側に位置
する面を探索するのに、凸多面体を構成する面を順番に
探索しているために、物体間の位置関係によっては、１
回の頂点と面との干渉判定で判明する場合もあれば、最
後の面で判明することもあり、計算量にばらつきが生じ
る可能性がある。また、ふたつの物体が干渉している場
合には、全ての頂点・面判定及び稜線判定を行うことに
より初めて干渉していることが判定されるために、かな
り計算量が増大した。 【００１９】 【発明が解決しようとする課題】このように従来の干渉
判定方法では、実用的な複雑さをもつ環境において干渉
判定を行うにはいまだ多くの計算を必要とするため、計
算機による干渉判定がさまざまな用途になかなか用いら
れることがなかった。 【００２０】そこで、本発明の目的は、実用的な複雑さ
をもつ動的な環境においても、実用的な計算を利用して
ロボットと周囲の環境との干渉判定を行うことができる
ロボットの干渉判定方法を提供することである。 【００２１】 【課題を解決するための手段】上記課題を解決するため
に、本発明によるロボットの干渉判定方法は、ロボット
およびロボットの作業環境内にある物体をソリッドとし
て扱いこれらのソリッドの位置および形状を計算機内部
の形状モデルのデータに反映させ、この形状モデルのデ
−タに基づきロボットとロボットの作業環境内にある物
体との干渉の有無を判定するロボットの干渉判定方法に
おいて、前記形状モデルのデータは、各ソリッドに関す
るデータと、各ソリッドを構成する面およびこれらの面
の境界要素に関するデータとを含み、各ソリッドに関す
るデータは、各ソリッドに固定されたローカル座標系の
座標原点およびローカル座標系の姿勢を基準となる基準
座標系で表したデータと、これらのデータの設定時刻と
を含み、各ソリッドを構成する面およびこれらの面の境
界要素に関するデータは、面およびこれらの面の境界要
素を特定するのに必要な位置、姿勢または形状を各ロー
カル座標系および基準座標系で表したデータと、これら
のデータを設定した時刻とを含み、各ソリッドの間の干
渉の有無の判定は、まず、各ソリッドに関するデータに
ついては、判定しようとする最新の時刻におけるデータ
を計算して設定し、各ソリッドの間の干渉の有無を判定
し、次に、各ソリッドを構成する面およびこれらの面の
境界要素に関するデータについては、干渉の有無を判定
する際に干渉が有ると判定されたソリッドに対しての
み、必要となるソリッドを構成する面およびこれらの面
の境界要素に対して、これらのデータの設定時刻が判定
しようとする最新の時刻より古いか否かを判断するとと
もに、古い場合には判定しようとする最新の時刻におけ
るこれらのデータを計算して設定し、行われることを特
徴とする。 【００２２】また、本発明は、ロボットリンクと作業環
境内の物体との干渉が生じている場合に、干渉している
ロボットリンクを構成する頂点の中で作業境界内の物体
の内部に含まれるもの、あるいは作業境界内の物体を構
成する頂点の中でロボットリンクの内部に含まれるもの
を求め記憶しておき、それぞれのリンク物体間の干渉判
定を行う際に、まず記憶してある頂点がないかを調べ、
もしある場合にはまずその頂点が他方の物体あるいはリ
ンクの内部に含まれるかどうかの判定計算を行うことを
特徴とする。 【００２３】また、本発明では、ソリッド間の干渉判定
を行う際に、一方のソリッドを構成する全ての頂点が他
方のソリッドを構成するひとつの平面を無限に広げた平
面の外側に位置するかどうかの面・頂点判定を含む干渉
判定方法において、干渉判定計算の際に、一方のソリッ
ドを構成する全ての頂点が他方のソリッドを構成するひ
とつの平面の外側に位置することが判定した場合に、そ
の平面を記憶しておき、面・頂点判定計算の際には、ま
ず記憶してある面がないかどうかを調べ、もしある場合
にはまずその平面と相手ソリッドの頂点との面・頂点判
定を行うことを特徴とする。 【００２４】また、本発明は、各ソリッド間の干渉判定
を行う際に、一方のソリッドを構成する稜線と他方のソ
リッドを構成する稜線との交差判定を含む干渉判定方法
において、干渉判定計算の際に、ある稜線が他方のソリ
ッドのすべての稜線と交差しないことが判明した場合
に、その稜線を記憶しておき、干渉判定計算の際には、
まず記憶してある稜線がないかどうかを調べ、もしある
場合にはまずその稜線と相手ソリッドの稜線との稜線・
稜線判定を行うことを特徴とする。 【００２５】 【作用】形状モデルをソリッドモデルとし、頂点座標値
と面の方程式の値として各ソリッドに固定されたローカ
ルな座標系での値とともに基準座標系での値も持たせ、
さらにソリッド・面・頂点のデータに時間の項目を与
え、干渉判定の際には、前回の干渉判定の際の位置から
移動したソリッドの座標原点の位置・姿勢及び基準座標
系からの変換行列の計算、時刻を設定し、頂点座標値お
よび面の方程式の基準座標系での値は、干渉判定計算の
それぞれの頂点・面判定の際に、それぞれの判定計算で
必要となる頂点あるいは面の時刻とその頂点・面が含ま
れるソリッドの時刻を比較し、ソリッドの時刻の方が新
しい場合にのみその頂点あるいは面の基準座標系での座
標値あるいは面方程式を再計算、時刻をソリッドの時刻
に設定し、ソリッドと頂点・面の時刻が等しい場合には
すでに計算されている基準座標系での値を干渉判定計算
に利用することにより、ロボットリンクや他の物体が移
動した際の頂点座標値や面の方程式の再計算を、実際の
干渉判定に利用する必要最小限の頂点・面だけに限定す
ることが可能となる。従って、干渉判定に要する計算時
間も短縮することができる。 【００２６】また、本発明によれば、ロボットリンクと
作業環境内の物体との干渉が生じている場合に、干渉し
ているロボットリンクを構成する頂点の中で作業環境内
の物体の内部に含まれるもの、あるいは作業環境内の物
体を構成する頂点の中でロボットリンクの内部に含まれ
るものを求め記憶しておき、それぞれのリンク物体間の
干渉判定を行う際に、まず記憶してある頂点がないかを
調べ、もしある場合にはまずその頂点が他方の物体ある
いはリンクの内部に含まれるかどうかの判定計算を行う
ことにより、オフラインティーチングシステムでの生成
したマニピュレータ軌道で作業境界内の物体との干渉が
生じないかどうかの判定を行う場合や、障害物回避動作
計画により作業境界内の物体と干渉を生じずに目標の姿
勢に到達するマニピュレータ軌道を探索により求める場
合や、極限作業用のマニピュレータを遠隔操作で操縦し
ている際にまわりの物体とマニピュレータが衝突しそう
かどうかを一定のサンプリング周期ごとに干渉判定計算
することにより監視する場合などのようにマニピュレー
タの姿勢を少しずつ変化させながら繰り返し干渉判定計
算を行う場合には、リンクと作業境界内の物体が干渉し
ている場合にすべての頂点・面判定、稜線・稜線判定を
行わずに記憶してある頂点が他方の物体に含まれるかど
うかの判定計算を行うだけで干渉していることを判定す
ることができ、従って計算時間を短縮することができ
る。 【００２７】また、本発明によれば、干渉判定計算を行
う際に、一方のソリッドを構成する全ての頂点が他方の
ソリッドを構成するひとつの平面の外側に位置すること
が判定した場合に、その平面を記憶しておき、面・頂点
判定計算の際には、まず記憶してある面がないかどうか
を調べ、もしある場合にはまずその平面と相手ソリッド
の頂点との面・頂点判定を行うことにより、オフライン
ティーチングシステムでの生成したマニピュレータ軌道
で作業境界内の物体との干渉が生じないかどうかの判定
を行う場合や、障害物回避動作計画により作業境界内の
物体と干渉を生じずに目標の姿勢に到達するマニピュレ
ータ軌道を探索により求める場合や、極限作業用のマニ
ピュレータを遠隔操作で操縦している際にまわりの物体
とマニピュレータが衝突しそうかどうかを一定のサンプ
リング周期ごとに干渉判定計算することにより監視する
場合などのようにマニピュレータの姿勢にを少しずつ変
化させながら繰り返し干渉判定計算を行う場合には、記
憶しておいた平面と他方の物体の頂点との面・頂点判定
計算のみでほとんどの場合には干渉しないことが判定す
ることができ、従って計算時間を短縮することができ
る。 【００２８】また、本発明によれば、各ソリッド間の干
渉判定を行う際に、一方のソリッドを構成する稜線と他
方のソリッドを構成する稜線との交差判定を含む干渉判
定方法において、干渉判定計算の際に、ある稜線が他方
のソリッドのすべての稜線と交差しないことが判明した
場合に、その稜線を記憶しておき、干渉判定計算の際に
は、まず記憶してある稜線がないかどうかを調べ、もし
ある場合にはまずその稜線と相手ソリッドの稜線との稜
線・稜線判定を行うことにより、オフラインティーチン
グシステムでの生成したマニピュレータ軌道で作業境界
内の物体との干渉が生じないかどうかの判定を行う場合
や、障害物回避動作計画により作業境界内の物体と干渉
を生じずに目標の姿勢に到達するマニピュレータ軌道を
探索により求める場合や、極限作業用のマニピュレータ
を遠隔操作で操縦している際にまわりの物体とマニピュ
レータが衝突しそうかどうかを一定のサンプリング周期
ごとに干渉判定計算することにより監視する場合などの
ようにマニピュレータの姿勢を少しずつ変化させながら
繰り返し干渉判定計算を行う場合には、ほとんどの場合
には記憶しておいた稜線と他方の物体との稜線・稜線判
定のみで干渉していないことが判定できるので、計算時
間を短縮することができる。 【００２９】 【実施例】次に本発明に係わるロボットの干渉判定方法
の実施例について説明する。第１実施例 ロボットとロボットの作業環境内にある物体との干渉の
有無を判定するために、ロボットおよびロボットの作業
環境内にある物体をソリッドモデルによりソリッドとし
て取り扱われる。これらのソリッドの位置および形状は
計算機内部の形状モデルのデータに反映されており、こ
の形状モデルのデ−タに基づきソリッド間の干渉の有無
が判定される。 【００３０】形状モデルのデータには、各ソリッドに関
するデータと、各ソリッドを構成する面およびこれらの
面の境界要素に関するデータとが含まれている。ここで
面の境界要素とは頂点や稜線等をさす。 【００３１】これらの各ソリッドに関するデータには、
各ソリッドに固定されたローカル座標系の座標原点およ
びローカル座標系の姿勢を基準となる基準座標系で表し
たデータと、これらのデータの設定時刻とが含まれてい
る。 【００３２】また、各ソリッドを構成する面およびこれ
らの面の境界要素に関するデータには、面およびこれら
の面の境界要素を特定するのに必要十分な位置、姿勢ま
たは形状を各ローカル座標系および基準座標系で表した
データと、これらのデータを設定した時刻とが含まれて
いる。 【００３３】各ソリッドの間の干渉の有無の判定は、次
のようにして行われる。すなわち、一方で、各ソリッド
に関するデータについては、判定しようとする最新の時
刻におけるデータが計算され設定される。 【００３４】また、他方で、各ソリッドを構成する面お
よびこれらの面の境界要素に関するデータについては、
干渉の有無を判定する際に必要となるソリッドを構成す
る面およびこれらの面の境界要素に対してのみ、これら
のデータの設定時刻が判定しようとする最新の時刻より
古いか否かが判断されるとともに、古い場合には判定し
ようとする最新の時刻におけるこれらのデータが計算さ
れて設定される。 【００３５】図１に本実施例の概略的フローチャートを
示す。まずＳＴ１で、全てのソリッドのデータを最新の
値に更新する。各ソリッドに関するデータは、各ソリッ
ドに固定されたローカル座標系の座標原点およびローカ
ル座標系の姿勢を基準となる基準座標系で表したデータ
と、これらのデータの設定時刻とを含む。次に、ＳＴ２
で、ソリッド間の干渉の有無を判定するために必要な面
や頂点のデータを形状モデルから求める。ＳＴ２は詳細
には、ＳＴ１０、ＳＴ１１およびＳＴ１２から構成され
ている。ＳＴ１０では、現データの値は最新の値か否か
が判断される。ＳＴ１０で現データの値が最新の値でな
いと判断された場合には、ＳＴ１１において、判定しよ
うとする最新の時刻における面や頂点のデータを計算す
る。この計算は、最新の時刻における各ソリッドに関す
るデータを用いて行われる。ＳＴ１０で現データの値が
最新の値であると判断された場合には、ＳＴ１２におい
て、そのまま現データが出力される。 【００３６】ＳＴ２の結果を用いて、ＳＴ３においてソ
リッド間の干渉の有無を判定するための計算が行われ
る。さらに、ＳＴ４において、全てのソリッドについて
干渉の有無を判定したかを判断し、未だ全てのソリッド
について判定していないと判断された場合には、ＳＴ２
へ戻る。 【００３７】図２に示されるように、本実施例で用いる
干渉判定装置１は、ロボット７（図３参照）が作業する
環境の環境形状モデル２と、ロボット形状モデル３と、
ロボットとこのロボットの周囲の作業境界との干渉を演
算する干渉計算装置４と、ロボット７の関節角度信号か
ら各リンクの座標変換行列の値を変更するロボット形状
モデル変更装置５を備えている。 【００３８】以下、上記構成のロボットの干渉判定装置
１を用いて行うロボットとその周囲の環境との干渉判定
について説明する。 ＜環境形状モデル２＞図３には、ロボットの計算機モデ
ル６が示されている。このモデル６は、ロボット７及び
作業環境が含まれている。本モデルにおけるロボット７
は、６自由度多関節型であり、周囲の環境を構成する物
体の数は９２個である。なお、環境を構成する物体及び
ロボット７のリンクはすべて凸多面体及びその組み合わ
せで表現されている。 【００３９】環境形状モデル２及びロボット形状モデル
３は、周囲の環境を構成する物体を位置的に近いもので
グループ化して階層構造化されている。図４に、環境形
状モデル２を階層構造化した例を示す。形状データとし
ては以下の項目を保持している。 １．各ソリッドのデータ ・ソリッド情報の時刻 ・実体ソリッド／上位ソリッドの識別番号 ・上位階層のソリッド番号 ・下位階層のソリッドの数 ・下位階層のソリッド番号リストへのポインタ ・拘束されているソリッドの数 ・拘束されているソリッド番号リストへのポインタ ・拘束されている各ソリッドへのこのソリッドからの座
標変換行列リストへのポインタ ・直接拘束されているソリッドの数 ・直接拘束されているソリッド番号リストへのポインタ ・ソリッドの基準座標系での位置および姿勢（位置・姿
勢） ・ソリッドの基準座標系での座標変換行列 ・ソリッドを構成する面の数 ・ソリッドを構成する面の番号リストへのポインタ ・ソリッドを構成する稜線の数 ・ソリッドを構成する稜線の番号リストへのポインタ ・ソリッドを構成する頂点の数 ・ソリッドを構成する頂点の番号リストへのポインタ ２．各面のデータ ・面情報の時刻 ・平面方程式の係数（面が含まれるソリッド座標系での
値） ・平面方程式の係数（基準座標系での値） ・面に含まれる稜線の数 ・面に含まれる稜線の番号リストへのポインタ ３．各稜線のデータ ・稜線の両端点の頂点番号 ・稜線の両側に位置する面の面番号 ４．各頂点のデータ ・頂点情報の時刻 ・頂点の座標値（頂点が含まれるソリッド座標系での
値） ・頂点の座標値（基準座標系での値） また、ソリッドの数、面の総数、稜線の総数、頂点の総
数をデータとして保持している。実体ソリッドとは階層
構造の環境形状モデルにおいて一番下の階層に位置する
ソリッドのことで、実際のそれぞれの物体に対応したソ
リッドである。一方、上位ソリッドとは、実体ソリッド
以外のソリッドであり、環境形状モデルにおいて下の階
層にソリッドを有するソリッドである。形状としては、
下の階層に含まれる全てのソリッドを含む形状を有して
いる。このように環境形状モデル３を階層構造とし、特
開平２−３２５７４２「ロボットの干渉判定方法」にあ
るように上位階層のソリッドから順に干渉判定を行って
ゆき、上位階層のソリッドと干渉している場合に限り、
下位の階層のソリッドとの干渉判定を行う。これによ
り、環境形状モデルが実用的な複雑なモデルとなりソリ
ッドの数が多くなった場合にも、ソリッド間の干渉判定
回数をあまり増大させないで判定することが可能とな
る。 【００４０】また、ソリッドを構成する面の番号リスト
などの構成要素の番号リストは、動的にピープ領域に必
要なメモリを確保し、番号リストを書き込み、各ソリッ
ド・面のデータにはそれらのピープ領域の先頭アドレス
を格納している。 ＜干渉判定装置４＞干渉判定装置４は、ロボット形状モ
デル変更装置５により変更されたロボット形状モデル３
及び環境形状モデル２を用いてロボット６と周囲の環境
を構成する物体が干渉していないかどうかを判定する。 ＜干渉判定手順＞以下、図５乃至図１１を参照して、干
渉判定の手順について説明する。図５は全体の干渉判定
手順を示しており、図６は各リンクと作業境界内の各物
体との干渉判定の手順を示している。図８および図９は
各凸多面体間の干渉判定アルゴリズムを示しており、図
１０および図１１は稜線・稜線判定の手順を示してい
る。 【００４１】手順１０１［すべてのリンクとすべてのソ
リッドとの干渉判定」は、各リンクと作業境界内の各ソ
リッドとの干渉判定である手順１０２「ｉ番目のリンク
とｊ番目のソリッドとの干渉判定」をすべてのリンクと
すべての最上位階層に位置するソリッドとの間で行う。
ただし途中で干渉が見つかれば、干渉しているので判明
するのでそこで処理を中止する。最上位階層に位置する
ソリッドとは図４の環境形状モデルにおいて階層構造化
された形状データの最も上の階層に位置するソリッドの
ことである。 【００４２】手順１０２「ｉ番目のリンクとｊ番目のソ
リッドとの干渉判定」の詳細は図６に記載されている。
ソリッドｊが下位のソリッドを有する上方階層ソリッド
の場合には、まず手順１０３「リンクｉ形状とソリッド
ｊ形状との干渉判定」を行う。もし干渉していない場合
にはリンクｉとソリッドｊが干渉していないことが判明
するので終了する。もし、干渉している場合には、ソリ
ッドｊのひとつ下の階層のすべてのソリッドとリンクｉ
との間で手順１０４「リンクｉとひとつ下の階層のｋ番
目のソリッドとの干渉判定」を行う。手順１０４は図６
のソリッドの番号をｊからソリッドｊのひとつの下の階
層のｋ番目のソリッドに変更しただけのものであり、処
理内容としては図６と同じであり、再帰的に同じ手続き
が呼び出される形になっている。 【００４３】また、ソリッドｊが実体ソリッドの場合に
は、手順１０５「リンクｉ形状とソリッドｊ形状との干
渉判定」を行う。もし干渉していない場合にはリンクｉ
とソリッドｊが干渉していないことが判明するので終了
する。干渉している場合には、まずリンクｉが実体ソリ
ッドかどうかを調べ、実体ソリッドならばリンクｉとソ
リッドｊが干渉していることが判明するので終了する。
もし、リンクｉが上方階層ソリッドの場合には、手順１
０６「ひとつ下の階層のｋ番目のリンクとソリッドｊと
の干渉判定」を行う。手順１０６はやはり図６と処理内
容が同じものであり、再帰的に同じ手続きが呼び出され
る形になっている。 【００４４】上記のアルゴリズムにより、リンクが作業
境界内のどの物体とも接近してない場合には最上位のソ
リッドとリンクとの干渉判定のみで処理が終了する。ま
た、リンクがある物体に接近している状態でも他の物体
から離れたリンクは最上位のソリッドとの干渉判定のみ
で終了し、その物体と接近しているリンクと、リンクが
接近している物体が含まれる物体のグループとの干渉判
定が増えるだけですむ。 【００４５】一方、手順１０３や手順１０５は、すべて
の物体が凸多面体で表現されているので凸多面体間の干
渉判定アルゴリズムとなり図８にその詳細を示す。手順
１０７「多面体Ａの面と多面体Ｂの点判定」では、多面
体Ａのすべての面について、その面を無限に広げた平面
の外側に多面体Ｂのすべての頂点が位置するかどうかの
判定計算である手順１０８「多面体Ａのｉ番目の面と多
面体Ｂのｊ番目の点の外側内側判定」を行う。多面体Ｂ
のすべての頂点が外側に位置する平面がひとつでも存在
すれば、ふたつの多面体間の干渉は存在しないからそこ
で処理を終了する。また、手順１０８は平面の方程式と
頂点の座標値（ともに基準座標系での値）から以下の計
算を行い、その符号を判定することにより行うことがで
きる。 ｆ＝ａ・ｘ０＋ｂ・ｙ０＋ｃ・ｚ０＋ｄ （１） （ａ，ｂ，ｃ，ｄ） ；平面の方程式の係数(ax+by+cz+
d=0) （ｘ０，ｙ０，ｚ０）；頂点の座標値 もし、多面体Ａのすべての面について面・頂点判定計算
を行い、多面体Ｂの頂点がすべて外側に位置する面が存
在しなかった場合には、手順１０９「多面体Ｂの面と多
面体Ａの点の外側内側判定」を行う。 【００４６】多面体Ａのすべての点が外側に位置する多
面体Ｂの面もみつからない場合には、手順１１０「多面
体Ａの稜線と多面体Ｂの稜線の交差判定」を行う。手順
１１０は、多面体Ａのすべての稜線に着いて、まず手順
１１１「多面体Ａのｉ番目の稜線が多面体Ｂに向いてい
るかどうかの判定」を行う。稜線ｅ１が多面体Ｂに向い
ているとは、多面体Ｂのすべての頂点が稜線ｅ１の両側
に位置するふたつの平面のいずれかの外側に位置するこ
とを意味する。つまり、 ｆ＝min ｛max ×｛ｆｉ１・ｖｋ，ｊｉ２・ｖｋ｝｝ （２） ｋ＝１，ｎ ｆｉ１，ｆｉ２；稜線ｅ１の両側に位置する平面のベク
トル ｖｋ ；多面体Ｂの頂点の座標ベクトル ｎ ；多面体Ｂの頂点の数 の符号により判定を行うことができる。もし、多面体Ａ
のｉ番目の稜線が多面体Ｂに向いている場合には、多面
体Ａのｉ番目の稜線と多面体Ｂのすべての稜線との間で
手順１１２「多面体Ａのｉ番目の稜線と多面体Ｂのｊ番
目の稜線の交差判定」を行う。交差判定の結果、多面体
Ｂのすべての稜線と交差しない多面体が存在する場合に
は、その多面体Ａと多面体Ｂは干渉していないことが判
明するので処理を終了する。また、そのような稜線がな
かった場合には、手順１１３「多面体Ｂの稜線と多面体
Ａの稜線の交差判定」を行い、それでも干渉していない
ことが判定されなければふたつの多面体Ａ，Ｂは干渉し
ており、処理が終了する。なお、手順１１２「多面体Ａ
のｉ番目の稜線と多面体Ｂのｊ番目の稜線の交差判定」
の詳細は図１０に示す。また、凸多面体間の干渉判定ア
ルゴリズムについては、日本ロボット学会誌Vol.5, No.
4 「マニピュレータの障害物回避動作計画を考慮した凸
多面体間の干渉チェック法」に詳細が述べられている。
＜ロボット形状モデル変更装置５＞ロボット形状モデル
変更装置５には、ロボット関節角度信号が一定周期ごと
に入力され、ロボット６の各リンクの座標系と基準座標
系との間の座標変更行列の値を計算し、ロボット形状モ
デル３の各リンクに対応するソリッドの基準座標系での
座標変換行列の値及びソリッド情報の時刻の値を変更す
る。 【００４７】また、干渉判定計算を行っている際に、面
の方程式の係数の値が必要となると干渉計算装置４はロ
ボット形状モデル３あるいは環境形状モデル２に面方程
式の係数要求を発する。環境形状モデル２あるいはロボ
ット形状モデル３は、まず面情報の時刻がその面が含ま
れるソリッド情報の時刻と等しいかどうかを調べる。等
しければすでにその面の基準座標系での平面方程式の係
数の値は最新の値であるので、その値を干渉計算装置４
に返す。もし、面情報の時刻の方が古い場合には、その
面方程式の基準座標系での値はその面が含まれるソリッ
ドの位置が変化したのに対応して更新されていないの
で、環境形状モデル２あるいはロボット形状モデル３は
ロボット形状モデル変更装置５に面情報の更新依頼を発
し、ロボット形状モデル変更装置５はその面が含まれる
ソリッドの基準座標系での座標変換行列の値及び面のソ
リッド座標系での係数の値から基準座標系での面方程式
の係数を計算し、環境形状モデル２あるいはロボット形
状モデル３の面方程式の係数の値及び面情報の時刻を更
新する。そして環境形状モデル２あるいはロボット形状
モデル３は干渉計算装置４にその更新された面方程式の
係数の値を返す。 【００４８】頂点の座標値が必要になった場合にも同様
にして環境形状モデル２あるいはロボット形状モデル３
が必要に応じてロボット形状モデル変更装置５に頂点情
報の更新依頼を発する。 【００４９】干渉判定計算では、物体を構成する頂点の
座標値及び面の方程式の値を随所に用いるが、上記に示
したようにソリッド・面・頂点のデータに時刻の項目を
加え、ソリッドと面あるいは頂点のデータの時刻を比較
することにより、干渉判定に必要な頂点・面の座標値あ
るいは平面方程式を重複することなく一回だけ計算する
ことが可能となり、ロボット関節の動きなどに伴う頂点
座標値・平面方程式の再計算を必要最小限に抑えること
が可能となる。第２の実施例 次に第２の実施例について図１２および図１３を用いて
説明する。図１２および図１３は第２の実施例における
凸多面体間の干渉判定アルゴリズムである。なお頂点番
号は配列変数ｉｎ＿ｖｘ〔〕［］に記憶する。ｉｎ
〔ｉ〕［ｊ］はＮｏ．ｉソリッドの内側に含まれるＮ
ｏ．ｊソリッドの頂点番号とする。変数ｉｎ＿ｖｘ〔〕
［］はあらかじめすべての要素が０に初期化されてお
り、その値が０であるかどうかにより頂点番号が設定さ
れているかどうかがわかるようになっている。 【００５０】まず多面体Ａの内側に含まれる多面体Ｂの
頂点が記憶されているかどうかを調べ、記憶されている
場合には、手順１１４「記憶されている頂点ｖ０と多面
体Ａのすべての面の外側内側判定」を行う。記憶されて
いる頂点ｖ０が多面体Ａのすべての面の内側に存在すれ
ば多面体Ａの内側に含まれることになるので、多面体Ａ
と多面体Ｂは干渉していることになり、処理は終了す
る。もし頂点ｖ０が外側に位置する面が存在する場合に
は、頂点ｖ０は多面体Ａの外側に位置するので変数ｉｎ
＿ｖｘの値を０に戻し頂点番号を無効にする。 【００５１】手順１１４で処理が終了しない場合には、
多面体Ｂの内側に含まれる多面体Ａの頂点が記憶されて
いるかどうかをしらべ、もし記憶されている場合にはそ
の記憶されている頂点ｖ１と多面体Ｂのすべての面との
外側内側判定を行い、頂点ｖ１が多面体Ｂのすべての面
の内側に位置すれば多面体Ａと多面体Ｂは干渉している
ことになるので処理は終了する。もし頂点ｖ１が外側に
位置する面が存在する場合には、頂点ｖ１は多面体Ｂの
外側に位置するので変数ｉｎ＿ｖｘの値を０に戻して頂
点番号を無効にする。 【００５２】以上のふたつの処理で多面体Ａと多面体Ｂ
の干渉が判明しない場合には、第１の実施例で説明した
ように多面体Ａの面と多面体Ｂの点判定、多面体Ｂの面
と多面体Ａの点判定、多面体Ａの稜線と多面体Ｂの稜線
の交差判定、多面体Ｂの稜線と多面体Ａの稜線の交差判
定を順に行ってゆき、それでも干渉しないことが判明し
ない場合には、ふたつの多面体Ａ，Ｂは干渉しているこ
とになる。 【００５３】多面体Ａ，Ｂが干渉していることがこの段
階で判明した場合には、多面体Ｂの内部に含まれる多面
体Ａの頂点を求める処理である手順１１５「多面体Ａの
内部に存在する多面体Ｂの頂点の探索」及び多面体Ａの
内部に含まれる多面体Ｂの頂点を求める処理である手順
１１６「多面体Ｂの内部に存在する多面体Ａの頂点の探
索」を行う。ただし、一方の多面体の内部に含まれる頂
点がみつかった段階で処理は終了する。 【００５４】ふたつの多面体が干渉している場合の大部
分の場合には、一方の多面体のある頂点が他方の多面体
の内部に含まれた状態となる。従って、上記のような処
理とすることにより、オフラインティーチングシステム
での生成したマニピュレータ軌道で作業境界内の物体と
の干渉が生じないかどうかの判定を行う場合や、障害物
回避動作作業境界内の物体と干渉を生じずに目標の姿勢
に到達するマニピュレータ軌道を探索により求める場合
や、極限作業用のマニピュレータを遠隔操作で操縦して
いる際にまわりの物体とマニピュレータが衝突しそうか
どうかを一定のサンプリング周期ごとに干渉判定計算す
ることにより監視する場合などのようにマニピュレータ
の姿勢を少しずつ変化させながら繰り返し干渉判定計算
を行う場合には、マニピュレータの姿勢は少しずつ変化
してゆくので、リンクと作業境界内の物体との干渉が生
じている場合には、記憶しておいた頂点がしばらくの期
間、多面体の内側に位置し続けるので、すべての点・面
判定、稜線・判定を行わずとも記憶しておいた頂点と他
方の多面体の内側外側判定のみで干渉していることが判
明し、計算時間の短縮がはかれる。 【００５５】特に、第１の実施例のように環境形状モデ
ルを階層構造とする場合には、マニピュレータのリンク
が物体に接近すると上位回押すのソリッドの形状モデル
とは干渉しているが下位階層のソリッドの形状モデルと
は干渉していない状態になり、上位階層のソリッドの形
状モデルと干渉しているかことを少ない計算量で判定で
きることが重要になり、本発明の干渉判定方法が有効に
働く。第３の実施例 次に第３の実施例について図１４を用いて説明する。図
１４は第３の実施例における凸多面体間の干渉判定アル
ゴリズムである。なお面番号は配列変数ｓｕｃ＿ｆａｃ
ｅ〔〕［］に記憶する。ｓｕｃ＿ｆａｃｅ〔ｉ〕［ｊ］
はＮｏ．ｉソリッドのすべての頂点が外側に位置するＮ
ｏ．ｊソリッドの面番号とする。変数ｓｕｃ＿ｆａｃｅ
〔〕［］はあらかじめすべての要素が０に初期化されて
おり、その値が０であるかどうかにより面番号が設定さ
れているかどうかがわかるようになっている。 【００５６】凸多面体間の干渉判定アルゴリズム全体の
流れは第１の実施例と同じである。多面体間の点・面判
定の中身が変化している。手順１１７「多面体Ａの面と
多面体Ｂの点判定」についてみてみると、まず手順１１
８「記憶されている面があるか？」で変数ｓｕｃ＿ｆａ
ｃｅ〔〕［］の値を参照することにより、多面体Ｂのす
べての頂点が外側に位置していた多面体Ａの面が記憶さ
れているかどうか調べる。もし、変数の値が０でなく、
記憶されていれば、手順１９「記憶されている面と多面
体Ｂのすべての点の外側内側判定」でその記憶されてい
る面と多面体Ｂのすべての点との外側内側判定を行い、
もし多面体Ｂのすべての頂点がその記憶されている面の
外側に位置すれば、多面体Ａと多面体Ｂが干渉しないこ
とが判明するので処理を終了する。 【００５７】もし、処理が終了しない場合には、多面体
Ａのすべての面に対して、多面体Ｂのすべての頂点が外
側に位置するかどうかの判定を行う。ただし、多面体Ｂ
の頂点がすべて外側に位置する多面体Ａの面が見つかっ
た場合には、そのときの多面体Ａの面を記憶しておき、
以降の干渉判定に利用できるようにする。 【００５８】これまでも説明してきたように、干渉判定
を通常利用する場合には、マニピュレータの姿勢を少し
ずつ変化させて繰り返し干渉判定を行うために、このよ
うに点・面判定が成功した面を記憶しておくことによ
り、大部分の場合に手順１１９の外側内側判定のみで多
面体Ａと多面体Ｂが干渉しないことが判明し処理が終了
し、計算時間の短縮をはかることができる。第４の実施例 次に第４の実施例について図１５および図１６を用いて
説明する。図１５および図１６は第４の実施例における
凸多面体間の干渉判定アルゴリズムの中の手順１１１
「多面体Ａの稜線と多面体Ｂの稜線の交差判定」であ
る。なお稜線番号は配列変数ｓｕｃ＿ｅｄ〔〕［］に記
憶する。ｓｕｃ＿ｅｄ〔ｉ〕［ｊ］はＮｏ．ｉソリッド
のすべての稜線と交差しないＮｏ．ｊソリッドの稜線番
号とする。変数ｓｕｃ＿ｅｄ〔〕［］はあらかじめすべ
ての要素が０に初期化されており、その値が０であるか
どうかにより稜線番号が設定されているかどうかがわか
るようになっている。 【００５９】凸多面体間の干渉判定アルゴリズム全体の
流れは第１の実施例と同じである。手順１１１「多面体
Ａの稜線と多面体Ｂの稜線の交差判定」及び手順１１３
「多面体Ｂの稜線と多面体Ａの稜線の交差判定」のみが
変化している。手順１１３は処理内容としては手順１１
１と同じであるので、構成４記載の発明における手順１
１１の内容を図１５および図１６に従って説明する。 【００６０】まず、手順１２０「記憶されている多面体
Ａの稜線があるか？」で変数ｓｕｃ＿ｅｄ〔〕［］の値
を参照することにより、記憶されている多面体Ａの稜線
があるかどうかを調べる。もしある場合には、その記憶
されている稜線が多面体Ｂに向いているかどうかの判定
を行い、もし向いているようならば、多面体Ｂのすべて
の稜線と交差しないかどうかの判定を行う。もし、交差
していなければ、多面体Ａと多面体Ｂが干渉しないこと
が判明したので処理を終了する。 【００６１】もし、処理が終了しない場合には第１の実
施例と同様に多面体Ａのすべての稜線について、多面体
Ｂのすべての稜線と交差しないかどうかの交差判定を行
う。ただし、多面体Ｂのすべての稜線と交差しない多面
体Ａの稜線がみつかったときに、以降の干渉判定のため
にその多面体Ａの稜線番号を変数ｓｕｃ＿ｅｄ〔〕［］
に記憶する。 【００６２】これまでも説明してきたように、干渉判定
を通常利用する場合には、マニピュレータの姿勢を少し
ずつ変化させて繰り返し干渉判定を行うために、このよ
うに点・面判定が成功した面を記憶しておくことによ
り、大部分の場合には手順１１９の外側内側判定のみで
多面体Ａと多面体Ｂが干渉しないことが判明し処理が終
了し、計算時間の短縮をはかることができる。 【００６３】なお、第１の実施例乃至第４の実施例まで
を適時組み合わせることによりさらなる計算時間の短縮
をはかることが可能である。 【００６４】 【発明の効果】以上説明したように本発明に係わるロボ
ットの干渉判定装置によれば、以前の干渉判定結果を記
憶し、その内容を利用しているので、オフラインティー
チングシステムでの生成したマニピュレータ軌道で作業
境界内の物体との干渉が生じないかどうかの判定を行う
場合や、障害物回避動作計画により作業境界内の物体と
干渉を生じずに目標の姿勢に到達するマニピュレータ軌
道を探索により求める場合や、極限作業用のマニピュレ
ータを遠隔操作で操縦している際にまわりの物体とマニ
ピュレータが衝突しそうかどうかを一定のサンプリング
周期ごとに干渉判定計算することにより監視する場合な
どのようにマニピュレータの姿勢を少しずつ変化させな
がら繰り返し干渉判定計算を行う場合には、干渉判定に
要する点・面の判定回数や稜線・稜線の判定回数などを
削減することができ、従って干渉判定に要する計算時間
を短縮し実用的な複雑さをもったモデルでも比較的短時
間の計算で干渉判定を行うことを可能とする。

【図面の簡単な説明】 【図１】本発明の一実施例の概略を示すフローチャート
図。 【図２】本発明の一実施例の概略構成を示すブロック
図。 【図３】ロボットと周囲の環境のモデルの一例を示す斜
視図。 【図４】環境形状モデルの階層構造を示す図。 【図５】階層構造表現された環境とロボットリンクの干
渉判定手順を示す図。 【図６】図５における手順１０２を詳細に示す図。 【図７】図６に続く図。 【図８】凸多面体間の干渉判定アルゴリズムを示す図。 【図９】図８に続く図。 【図１０】稜線ｉと稜線ｊとの交差判定を説明する図。 【図１１】図１０に続く図。 【図１２】多面体の内側に含まれる頂点番号を記憶して
おく凸多面体同士の干渉判定手順を示す図。 【図１３】図１２に続く図。 【図１４】点・面判定が成功した面の番号を記憶してお
く凸多面体同士の干渉判定手順を示す図。 【図１５】稜線・稜線判定が成功した稜線番号を記憶し
ておくふたつの多面体の稜線の交差判定手順を示す図。 【図１６】図１５に続く図。 【符号の説明】 １ ロボット干渉判定装置 ２ 環境形状モデル ３ ロボット形状モデル ４ 干渉計算装置 ５ ロボット形状モデル変更装置 ６ ロボット ７ リンク

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 3/00 - 3/04 B05J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06 G05B 19/18 - 19/46 G06F 17/50

Claims (1)

1. (57)【特許請求の範囲】 【請求項１】ロボットおよびロボットの作業環境内にあ
   る物体をソリッドとして扱いこれらのソリッドの位置お
   よび形状を計算機内部の形状モデルのデータに反映さ
   せ、この形状モデルのデ−タに基づきロボットとロボッ
   トの作業環境内にある物体との干渉の有無を判定するロ
   ボットの干渉判定方法において、 前記形状モデルのデータは、各ソリッドに関するデータ
   と、各ソリッドを構成する面およびこれらの面の境界要
   素に関するデータとを含み、 各ソリッドに関するデータは、各ソリッドに固定された
   ローカル座標系の座標原点およびローカル座標系の姿勢
   を基準となる基準座標系で表したデータと、これらのデ
   ータの設定時刻とを含み、 各ソリッドを構成する面およびこれらの面の境界要素に
   関するデータは、面およびこれらの面の境界要素を特定
   するのに必要な位置、姿勢または形状を各ローカル座標
   系および基準座標系で表したデータと、これらのデータ
   を設定した時刻とを含み、 各ソリッドの間の干渉の有無の判定は、 まず、各ソリッドに関するデータについては、判定しよ
   うとする最新の時刻におけるデータを計算して設定し、
   各ソリッドの間の干渉の有無を判定し、 次に、各ソリッドを構成する面およびこれらの面の境界
   要素に関するデータについては、干渉の有無を判定する
   際に干渉が有ると判定されたソリッドに対してのみ、必
   要となるソリッドを構成する面およびこれらの面の境界
   要素に対して、これらのデータの設定時刻が判定しよう
   とする最新の時刻より古いか否かを判断するとともに、
   古い場合には判定しようとする最新の時刻におけるこれ
   らのデータを計算して設定し、 行われることを特徴とするロボットの干渉判定方法。