JP6415026B2 - 干渉判定装置、干渉判定方法、コンピュータプログラム - Google Patents

Description

本発明は、ワークを把持する際の干渉判定の技術に関するものである。

工場などの生産ラインにおいて、山積みされたワークの中から一個体をビジョンシステムを用いて特定し、三次元位置姿勢を認識することで、ロボットに取り付けたハンドにより把持を行う、山積みピッキング技術が近年開発されている。この技術において認識したワークを把持する際に、ワークの収容箱、あるいは把持対象以外のワークなどの周辺物体とハンドとの衝突を避けるためには、衝突の有無を事前に判定する処理が必要となる。

特許文献１には、上方に取り付けたセンサからワークまでの距離を画像様式で取得し、取得した情報からワークの位置姿勢を認識する。そして、ワークを把持するハンドの形状と位置姿勢、および取得した距離情報に基づき、事前に干渉判定を行う方法が記載されている。具体的には、認識したワークのz方向上方の基準高さ位置にハンドを仮想的に持っていく。そして、基準高さ位置からワークの把持位置までハンドをz方向に仮想的に下降させる。そして、下降させた把持位置で第１指部と第２指部との間を少し開き、第１指部と第２指部の各指先のz方向下方に指先から適当な余裕Δを持たせ、かつワークからも適当な余裕Δを持たせ、干渉判定用の仮想平板を仮想的に設定する。そして、センサで取得した画像様式の距離情報において仮想平板と重なる画素の距離値に、仮想平板よりも大きなz値を持つものが存在すれば干渉が発生すると判定する。これにより、認識したワークをハンドが把持する際の周辺物体との干渉の有無を高速に事前判定可能となる。

特開２０１０−２０７９８９号公報

しかしながら、特許文献１の方法では、取得した画像様式の距離情報の中に、把持の対象となるワークの形状を表す距離情報も含まれる。特許文献１では、把持対象ワーク自身と干渉があると判定されるのを防ぐため、各指をワークから適当な余裕Δだけ離間させている。そのため、実際に把持対象ワークを把持する際に干渉しない周囲の物体とも干渉ありと判定されてしまう場合がある。

本発明は以上の課題に鑑みてなされたものであり、把持対象ワーク以外の周辺物体とハンドとの干渉を事前に正しく判定することを目的とする。

本発明にかかる干渉判定装置は、例えば、複数のワークを含んだ空間を撮像する撮像装置を用いて前記空間内の複数の３次元計測点までの距離を取得する取得手段と、前記ワークの３次元モデルの情報を保持する保持手段と、前記取得手段の取得結果と前記保持手段が保持する３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定手段と、前記複数の３次元計測点のうち、前記撮像装置によって撮像された場合に、前記把持対象ワークを前記撮像装置によって撮像した場合に得られる領域と重なる点を除外した３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定手段と、を備える。

本発明によれば、把持対象ワーク以外の物体とハンドとの干渉を事前に正しく判定することが可能となる。

第１の実施形態における山積みピッキングシステムの構成を示す図である。 第１の実施形態における干渉判定装置１の構成例を示すブロック図である。 第１の実施形態における干渉判定装置１を備えたワークの山積みピッキングシステムによるピッキング処理のフローチャートである。 第１の実施形態における、把持対象ワーク周辺の物体とハンドとの干渉判定を説明する図である。 第２の実施形態における、把持対象ワーク周辺の物体とハンドとの干渉判定を説明する図である。

（第１の実施形態）
第１の実施形態では、検出したワークを把持する際に、把持対象ワークの位置姿勢を利用して、２Ｄ画像上で把持対象ワーク領域を特定し、周辺物体とハンドとの干渉の有無を事前判定する方法について述べる。

具体的には、まず検出したワークの位置姿勢に基づき、計測装置と同様のカメラパラメータを持つ仮想カメラの観測画像平面上にワークの三次元形状モデルを投影する。そして、ワークの投影が行われた画素に距離画像上で対応する三次元点は把持対象ワーク領域と判定する。そして、把持対象ワーク領域の三次元点を除いた三次元形状データとハンドモデルとを用いて、三次元空間的な重なりを検出することで干渉の有無を判定する。

これにより、ハンドを把持対象ワークに極めて近づけた把持動作に対しても、把持対象ワーク部分の計測点による誤検出なく、周辺（周囲）物体とハンドとの干渉を正しく判定することが可能となる。

図１は、この実施形態に係る干渉判定装置１を備える対象ワークの山積みピッキングシステム１０の構成を示している。山積みピッキングシステム１０は、干渉判定装置１と、ロボット作業指示部１８と、ロボット制御部１９と、対象とするワーク２２を把持可能なハンド２０を有するロボットアーム２１と、同一形状のワークを山積みで収容する（複数含んでいる）ワーク収容箱２３と、から構成される。まず、各構成物について説明する。

ロボット作業指示部１８は、干渉判定装置１の出力結果に基づき、ハンドと周辺物体との間に把持動作時の干渉が発生しないと判断されたワークを把持するよう、ロボット制御部１９に指示を行う。

ロボット制御部１９は、ロボット作業指示部１８からの指示情報を受けて、ロボットアーム２１およびハンド２０の動作制御を行い、把持対象ワークの把持、搬送動作を行う。

ハンド（把持部）２０は、ロボットアーム先端であるフランジに取り付けられ、ロボット制御部１９からの動作制御によりワークの把持動作を行うエンドエフェクタである。たとえば、ワークの平面部に押しつけることで把持を行う磁石式あるいは吸着式ハンドでもよいし、二指、三指といった多数の指の開閉により物体を内側あるいは外側から挟み込むことで把持を行うグリッパー式ハンドでもよい。その他、ロボットアームに取り付け可能で把持機構を有するエンドエフェクタであればいかなるものであってもよい。

ロボットアーム２１は、ロボット制御部の動作制御により対象ワークを把持可能な位置姿勢までハンドを動作可能な多自由度のロボットである。

距離センサ２は、各画素に奥行きの情報を保持する距離画像を取得するセンサである。距離センサ２としては、たとえば対象物体に照射したレーザ光やスリット光・ランダムドットパターン光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。アクティブ式の距離計測方法としては，空間符号化法、位相シフト法などのさまざまな方式を用いてよい。さらに、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。この他、対象物体の表面上の三次元位置を距離画像から取得できる方法であれば、いかなるものであっても本発明の本質を損なうものではない。

次に、干渉判定装置１の構成について、干渉判定装置の機能構成例を示すブロック図である図２を用いて説明する。

干渉判定装置１は、取得部１１、ワーク位置姿勢導出部１２、ワークモデル保持部１３、対象領域特定部１４、ハンド位置姿勢導出部１５、ハンドモデル保持部１６、干渉判定部１７とから構成される。以下、各処理部について説明する。

３次元形状データ取得部１１は、対象ワークの山積み状態の表面形状を表す三次元形状データを取得する。より具体的には、距離センサにより距離画像を撮影し、各画素の座標と奥行き値から三次元の座標を算出した三次元点群を三次元形状データとして取得する。

３次元形状データ取得部１１が取得した計測情報はワーク位置姿勢導出部１２に入力される。ワーク位置姿勢導出部１２に設定された座標系を以後、センサ座標系と呼ぶ。なお、本実施形態において、距離センサとロボットとの幾何関係は固定であるものとし、両者間の相対位置姿勢は事前にロボット・ビジョンキャリブレーションを実施して求めておくことにより既知であるものとする。また、本実施形態では、センサは、ワーク収容箱の上部に固定されたものとして説明を行ったが、センサはロボットアームに固定してもかまわない。

ワーク位置姿勢導出部１２は、３次元形状データ取得部１１により入力された情報を手がかりに、ワーク収容箱２３内に存在するワークの一個体を検出するとともに、センサ座標系における、検出ワークの位置姿勢を表す６パラメータを算出する。

本実施形態ではまず、多数の位置姿勢からワークを観測した距離画像情報をそれぞれパターンとしてデータベースに保持しておき、距離センサ２で計測した距離画像とこれらのパターンとをマッチングすることで一個体を検出し、ワークのおおまかな位置姿勢を算出する。次に、おおまかな位置姿勢からワークの位置姿勢を再度算出し直す。まず、ワークを距離画像として取得した際に観測されるであろう三次元点群を後段の処理で用いるワークの三次元形状モデルから抽出しておく。そして、おおまかな位置姿勢に基づいて、その三次元点群と、距離センサ２で取得した距離画像上から導出された三次元点群との対応関係を求め、両者がフィッティングするように位置姿勢を導出する。ただし、取得部１１で取得した距離画像を用いて、山積みワークの中から把持対象となる個体を発見し、その三次元位置姿勢を導出できる方法であれば、ここで述べた以外のいかなる方法であってもかまわない。

ワークモデル保持部１３は、山積みピッキングの対象となるワークの三次元形状モデルを保持する。三次元形状モデル（３次元モデル情報）には、たとえばワークの三次元形状を近似表現したポリゴンモデルを用いることができる。ポリゴンモデルは、対象ワーク形状の表面点の３次元座標と、形状を近似する三角形の面（ポリゴン）を構成するための各表面点の連結情報によって構成される。なお、ポリゴンは三角形で構成されるのが一般的であるが、四角形や五角形であってもよい。その他、表面点の座標とその連結情報によりワーク形状を近似表現できるポリゴンモデルであればいかなるものであってもかまわない。ただし、三次元形状モデルはポリゴンモデルに限らない。たとえばＣＡＤデータ（ＣＡＤモデル）のように、Ｂ−Ｒｅｐ（Ｂｏｕｎｄａｒｙ−Ｒｅｐｒｅｓｅｎｔａｔｉｏｎ）表現と呼ばれる、区分されたパラメータ曲面の集合で形状を表現したモデルであってもよい。その他、ワークの三次元形状として、面形状を表現可能なモデルであればいかなるものであってもかまわない。三次元形状モデルは、干渉判定部１７において、距離画像から得られる三次元点群から、把持対象ワーク領域に該当する三次元点を特定する際に用いられる。

対象領域特定部１４は、３次元形状データ取得部１１により取得される把持対象ワークを含んだ空間の３次元形状データから、把持対象ワークの領域を除いた領域を特定する。そして、対象領域特定部１４は、干渉判定部１７に送出する。

ハンド位置姿勢導出部１５は、ワーク位置姿勢導出部１２により導出されたワークの位置姿勢に基づいて、該把持対象ワークを把持するハンドの位置姿勢を導出する。具体的な処理は後述する。ハンド位置姿勢導出部１５は、導出されたハンドの位置姿勢を干渉判定部１７に送出する。

ハンドモデル保持部１６は、把持対象ワークを把持するハンドの３次元形状モデルを保持する。ハンドモデルの３次元形状モデルに関しても、ワークの３次元形状モデルと同様なので説明を省略する。ハンドモデル保持部１６は、保持するハンドの３次元形状モデルを上述のハンド位置姿勢導出部１５と干渉判定部１７に送出する。

干渉判定部１７は、３次元形状データ取得部１１が取得した三次元形状データのうち、把持対象ワーク領域特定部（対象領域特定部１４）が特定した把持対象ワークの領域を除いた領域に対して、干渉判定を行う。干渉判定は、ハンド位置姿勢導出部１６により導出されるハンドの位置姿勢でのハンドモデルと、把持対象ワーク周辺物体との干渉の有無を判定する。これには、ハンドモデル保持部１５に保持されるハンドの三次元形状を表現したポリゴンモデルを用いて、ゲームプログラミングなどで一般的に利用される衝突判定技術を利用する。具体的には、ビジョンシステムにより山積みワークを撮影した距離画像から得られる三次元形状データと、検出したワークを把持する際の位置姿勢におけるハンド形状との、三次元空間上での重なりを検出することで、干渉の発生を検出する。干渉判定部１７は、上記の干渉判定処理を行い、干渉がないと判定された場合に、把持対象ワークをロボット作業指示部１８に出力する。もちろん、干渉があると判定された場合にも、干渉判定の結果を出力してもかまわない。

図３は、本実施形態において、干渉判定装置１を利用して把持動作時の干渉を事前判定しながら山積みワークのピッキングを行う処理手順を示すフローチャートである。

（ステップＳ２０１）
ステップＳ２０１では、ワークモデル保持部１３が、保持しているワークの三次元形状モデルをワーク位置姿勢導出部１２および干渉判定部１７に入力する。

（ステップＳ２０２）
ステップＳ２０２では、ハンドモデル保持部１６が、保持しているハンドの三次元形状を表すモデルをハンド位置姿勢導出部１５および干渉判定部１７に入力する。ここでは、三次元形状をポリゴンの集合により近似したポリゴンモデルを入力するものとする。

（ステップＳ２０３）
ステップＳ２０３では、３次元形状データ取得部１１が、山積みされた対象ワークの距離画像を撮影し、各画素の座標と奥行き値から三次元点群の座標データを算出して、ワーク位置姿勢導出部１２に入力する。三次元点群の点の総数をＮとする。なお、得られる三次元点群の各点の座標は、基準座標系で表現されているものとする。これ以後、基準座標系で表現された三次元点群の各点にＩＤをつけ、ｉ番目の三次元点をｋｃ＿ｉと呼ぶことにする。ここで、本ステップで生成した三次元点の座標と、対応する距離画像の各画素の座標には、互いに参照できるようにリンク情報を記録しておく。

（ステップＳ２０４）
ステップＳ２０４では、ワーク位置姿勢導出部１２が、山積み中のワークの中から把持対象となる個体を一つ検出し、センサ座標系における検出ワークの位置姿勢を表す６つのパラメータを算出する。個体の検出および位置姿勢の算出にはさまざまな方法を利用できる。前述したように、本ステップではまず、ワークを多数の位置姿勢から観測したときの各距離画像のパターンと、ステップＳ２０１で得られた距離画像とのマッチングを行い、もっともスコアの高かったデータに対応する観測位置姿勢を把持対象ワークの概略位置姿勢として求める。具体的には、特開２００１−１４３０７号公報明細書に記載されている方法を利用し、距離画像上の特徴点間の位置関係と、データベース上に存在する、各観測位置姿勢における特徴点間の位置関係とを比較して、最も一致する特徴点群を選別することで概略位置姿勢を算出する。次に、得られた概略位置姿勢を初期値として、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法と呼ばれる手法を用いて、距離画像から得られた三次元点群に対して、ワークの三次元形状モデルが三次元空間中で当てはまるような位置姿勢を算出する。

まず、ワークの三次元形状モデル表面上から抽出した点群をＰとする。

Ｐ ＝ ｛ ｐ_ｍ＿１， ｐ_ｍ＿２， …， ｐ_ｍ＿Ｎ ｝ （１）
次に、距離画像から得られる三次元点群をＫとする。

Ｋ ＝ ｛ ｋ_ｍ＿１， ｋ_ｍ＿２， …， ｋ_{ｍ＿Ｎ ｝} （２）
三次元形状モデルからサンプリングした表面点群Ｐを変換して、距離画像から得られる三次元点群Ｋに一致するような位置姿勢パラメータを算出する。概略位置姿勢に基づき、点群Ｐの各点ｐ_ｉをセンサ座標系に変換した時に点群Ｋの中で距離が最も近い点をｂ_ｉ∈Ｋとしたとき、（３）のように誤差関数を定義できる。ここで、Ｒとｔはそれぞれ、求める姿勢パラメータと移動ベクトルである。

この誤差関数Ｅを小さくするＲとｔを求めることで、位置姿勢を表す６パラメータを算出する。なお、誤差関数Ｅを小さくするＲとｔを求める方法については、以下の文献に載っている。Ｋ．Ｓ．Ａｒｕｎ， Ｔ．Ｓ．Ｈｕａｎｇ， ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｌｎ，”Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ３−Ｄ Ｐｏｉｎｔ Ｓｅｔｓ，” ＰＡＭＩ，ｖｏｌ．９，ｎｏ．５，１９８７
なお、本ステップでは上記のようにＲ，ｔを直接求めるのではなく、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法など非線形最適化手法を用いてワークの位置姿勢パラメータの補正を繰り返すことで算出しても良い。その他、山積み中のワークの中から一個体を検出し、センサ座標系におけるワークの位置姿勢を表す６つのパラメータを算出可能な方法であればいかなる方法であってもよい。

（ステップＳ２０５）
ステップＳ２０５では、対象領域特定部１４が、ステップＳ２０４で導出した把持対象ワークの位置姿勢に基づき、三次元形状データ上で把持対象ワーク以外に該当する三次元点を特定する。本ステップの処理について図４を用いて説明する。

図４（ａ）は、収容箱に山積みされたワークを撮影した距離画像、および、その距離画像からステップＳ２０２において検出、位置姿勢が算出された把持対象ワークを示している。ここで、ワークモデル保持部１３の保持するワークの三次元形状モデルと、把持対象ワークの位置姿勢とを用いて、以下の方法により、把持対象ワーク以外の三次元形状データを特定する。

まず、仮想カメラの観測画像平面を用意し、その観測画像平面上に、算出した位置姿勢に基づいてワークの三次元形状モデルをレンダリングする。なお、仮想カメラのカメラパラメータには、計測部（取得部１１）で実際に用いる撮像装置と同一のものを取得して利用するのがよい。同一のカメラパラメータを利用することで、計測部で撮影した距離画像の各画素（ｘ，ｙ）が、仮想カメラの観測画像平面上の（ｘ，ｙ）に対応する。ただし、撮影した距離画像と、仮想カメラの観測画像平面との画素の対応関係が特定できれば、必ずしも同一のカメラパラメータである必要はない。なお、レンダリング処理に先立って、まず、画像平面全体をたとえば（Ｒ，Ｇ，Ｂ）＝（０，０，０）などの色で初期化しておく。

次に、ワークがレンダリングされた画素を特定できるよう、三次元形状モデルを構成する各面に対して、初期化に利用した色以外で固有の色を割り当て、割り当てた色で各面をレンダリングする。たとえば、ポリゴンモデルを利用するのであれば、ポリゴンモデルを構成する全ポリゴンに（Ｒ，Ｇ，Ｂ）＝（２５５，０，０）の色を割り当ててレンダリングを行う。あるいはＢ−Ｒｅｐ表現の区分パラメータ曲面のモデルを利用する場合においても、各曲面に対して固有の色を割り当ててレンダリングを行う。なお、ワークがレンダリングされている領域の画素を特定できれば、カラー画像、濃淡画像を用いてもかまわないし、各面ごとに異なる色を割り当てても同一の色を割り当ててもかまわない。あるいは、二値化画像を利用してもかまわない。図４（ａ）の把持対象ワークに対して、上記処理により生成された画像を図４（ｂ）に示す。

次に、距離画像上の各画素（ｘ，ｙ）に対して、その画素に対応する観測画像平面上の各画素の輝度値（Ｒ，Ｇ，Ｂ）を順次参照することで、その画素から算出した三次元点が把持対象ワークに該当する領域であるか順次判定する。たとえば距離画像の注目画素（ｘ，ｙ）に対応する観測画像平面上の画素の輝度値が（Ｒ，Ｇ，Ｂ）＝（２５５，０，０）であったとする。この場合、その画素から算出した三次元点は把持対象ワークに該当する形状を表すデータであるものと判定して、その画素に対応付けて０の値を記録しておく。あるいは注目画素（ｘ，ｙ）に対応する観測画像平面上の画素の輝度値が（Ｒ，Ｇ，Ｂ）＝（２５５，０，０）であったとする。この場合、その画素から算出した三次元点は、ワーク収容箱あるいは把持対象以外のワークなど周辺物体の形状を表すデータであるものと判定して、その画素に対応付けて１の値を記録しておく。この処理を全画素に行うことで、各画素から算出した三次元点について、把持対象ワークの形状を表すもの、把持対象ワーク以外の周辺物体の形状を表すもの、のいずれに該当するかを特定可能となる。

なお、ここでは、レンダリングの結果画像を格納するフレームバッファのうち、カラーバッファの輝度値を参照することで把持対象ワーク領域を特定したが、カラーバッファの代わりに、デプスバッファやステンシルバッファの値を参照して特定を行ってもよい。あるいはレンダリングを行わずに、仮想カメラ座標系の原点から各画素に対して向かう仮想の光線を計算し、その光線と対象ワークとが交わるか否かを判定することで把持対象ワーク領域か否かを判定してもよい。

ここで、図４（ｃ）は、図４（ａ）の距離画像に対する太線部の断面図を示したものである。計測装置により取得した距離画像から算出された各三次元点を白丸で示す。また、その中で、上記処理により把持対象ワークに該当するものを破線部で示す。破線部で示された把持対象ワークに該当する三次元点は除き（除外して）、実線の白丸で表示された三次元点を用いて干渉判定を行うことで、把持対象ワーク以外の物体との干渉判定が可能となる。

なお、把持対象ワークの領域の特定は、以下の方法によっても可能である。たとえば、各種類が固有の色をもつような、異なる複数種のワークが混合で山積みされた中から、１種類のワークを把持する場合には、距離画像との対応が特定可能な輝度画像を取得し、輝度画像の画素の色情報に基づいて、把持対象ワークの領域を特定してもよい。すなわち、把持対象ワークが赤色なのであれば、輝度画像上で赤色の画素に対応する距離画像上の画素の計測点は無効、赤色以外の画素に対応する距離画像上の計測点は有効という情報を付与することで、把持対象ワークの特定処理が可能である。

（ステップＳ２０６）
ステップＳ２０６では、ハンド位置姿勢導出部１５が、ステップＳ２０４で導出したワークの位置姿勢に基づき、センサ座標系におけるワークの把持動作を行うハンドの位置姿勢を導出する。具体的には、把持を行う際のワークとハンドとの相対位置姿勢を、把持教示情報として記録させる作業を事前に行っておく。ワークとハンドとの相対位置姿勢の設定は、ワークのモデルとハンドのモデルとを仮想環境で操作することで行う。あるいは、センサにより計測したデータに基づき、センサ座標系における位置姿勢を求めたワークに対して、センサとハンド間の相対位置姿勢が取得可能なハンドにより、そのワークを把持できるような位置姿勢へハンドを動かし、両者の位置姿勢から算出してもよい。そして算出したワークの位置姿勢を表す座標変換行列に対して、記録しておいた把持教示情報に基づき、ワーク・ハンド間の相対位置姿勢を表す座標変換行列を掛け合わせることでセンサ座標系におけるハンドの座標変換行列を求め、両者間の位置姿勢を算出する。

図４（ｄ）に、図４（ｃ）の把持対象ワークに対して、本ステップにより把持を行うハンドの位置姿勢を導出して配置した様子を示す。なお、ステップＳ２０５により把持対象ワークに該当すると判定された三次元点は取り除いている。

（ステップＳ２０７）
ステップＳ２０７では、干渉判定部１７が、ステップＳ２０５で特定した、把持対象ワーク以外に該当する三次元形状データと、ステップＳ２０６で算出した位置姿勢におけるハンドとの干渉判定を行う。

まず、ステップＳ２０６で導出された位置姿勢にハンドの形状を表現するポリゴンモデルを配置する。さらに、把持対象ワーク以外に該当する三次元形状データの各三次元点の位置に半径ｒの微小な球を配置する。そして、各ポリゴンと球との三次元空間的な重なりを検出することで干渉判定を行う。球とポリゴンとに重なりがあれば干渉あり、なければ干渉なしと判定する。ポリゴンと球との干渉判定方法については、「Ｃｈｒｉｓｔｅｒ Ｅｒｉｃｓｏｎ， ”ゲームプログラミングのためのリアルタイム衝突判定”，（ボーンデジタル，２００５）」に記載されている方法を用いることができる。図４（ｄ）中において、把持対象ワーク以外の三次元点のうち、球とポリゴンとの干渉判定により、ハンドとの干渉ありと判定されたものを黒丸で示す。

ただし、干渉判定はこのほかの方法で行ってもよい。たとえば、距離画像上で隣接した画素間の三次元点を連結することで山積みワークの三次元形状データを表現するポリゴンを生成し、把持位置姿勢におけるハンドのポリゴンモデルとの、ポリゴン間の交差を検出することで干渉判定を実施する。この場合、ポリゴン間に交差があれば干渉あり、なければ干渉なしと判定し、結果を出力する。ポリゴン間の干渉判定方法については、「Ｔｏｍａｓ Ｍoｌｌｅｒ， ”Ａ Ｆａｓｔ Ｔｒｉａｎｇｌｅ−Ｔｒｉａｎｇｌｅ Ｉｎｔｅｒｓｅｃｔｉｏｎ Ｔｅｓｔ”， １９９７」に記載されている方法を用いることができる。なお、ハンドの形状モデルにはポリゴンモデルを利用したが、円柱や球、直方体などの簡単な幾何形状の組み合わせで三次元形状を表現したモデルを用いてもかまわない。このほか、山積みワークの三次元形状データと、ワークを把持するハンドの形状モデルとの間の三次元空間的な重なりを検出できる方法であればいかなる方法であってもかまわない。また、ハンドの形状モデルを把持対象ワークと反対側の方向に拡大してもよい。拡大したハンドの形状モデルを用いて干渉判定を行うことにより、実際に行われる把持動作に余裕を持たせることができる。

（ステップＳ２０８）
ステップＳ２０８では、ステップＳ２０７の結果、干渉がないと判定されればステップＳ２０９へ進む。干渉があると判定されればステップＳ２０２へ進み、別の個体の検出、位置姿勢導出を行う。

（ステップＳ２０９）
ステップＳ２０９では、ロボットアームを動作させて、把持対象ワークを把持できる位置姿勢へハンドを移動し、ハンドによるワークの把持、所定位置への搬送動作を行う。

（ステップＳ２１０）
ステップＳ２１０では、ワーク収容箱が空であるか判定する。空でなければ、ステップＳ２０３へ進む。空であれば処理を終了する。

以上の方法により、干渉判定装置１を利用して把持動作時の干渉を事前判定し、干渉がないと判定された把持対象ワークに対してピッキング（把持）を行う。

第１の実施形態では、検出したワークを把持する際に、把持対象ワークの位置姿勢を利用して、２Ｄ画像上で把持対象ワーク領域を特定し、周辺物体とハンドとの干渉の有無を事前判定する方法について説明した。具体的には、検出したワークの位置姿勢に基づき、計測装置と同様のカメラパラメータを持つ仮想カメラの観測画像平面上にワークの三次元形状モデルを投影し、ワークの投影が行われている画素に対応する三次元点は把持対象ワーク領域と判定する。そして、把持対象ワーク領域の三次元点を除いた三次元形状データとハンドモデルとを用いて、三次元空間的な重なりを検出することで干渉の有無を判定する。これにより、ハンドを把持対象ワークに極めて近づけた把持動作に対しても、把持対象ワーク部分の計測点による誤検出なく、周辺物体とハンドとの干渉を正しく判定することが可能である。この方法は主に、ワークの検出方法が、他の個体に遮蔽されていない山積み表層の個体を検出する性質を持つ場合に有効である。

なお、本実施形態では、把持教示情報は一つだけ記録してハンドの把持位置姿勢を算出したが、ワークが把持可能な相対位置姿勢を複数記録しておき、ワークの姿勢などに応じて把持教示情報を選択してハンドの位置姿勢を算出してもよい。あるいは、複数の把持教示情報に対して優先順位を設定しておき、優先順位の高い把持教示情報を選択して位置姿勢を算出してもよい。複数の把持教示情報を記録しておく場合、干渉判定処理により干渉が発生すると判定された場合には、次点の把持教示情報に基づく把持動作を選択して位置姿勢を算出し、再度干渉判定を行う。

（第２の実施形態）
第２の実施形態（以下、本実施形態）では、検出したワークを把持する際に、把持対象ワークの位置姿勢を利用して、３Ｄ空間上で把持対象ワーク領域を特定し、周辺物体とハンドとの干渉の有無を事前判定する方法について述べる。本実施例では、算出した位置姿勢における把持対象ワークの三次元形状モデルと、三次元形状データを構成する各三次元点との距離に基づき、距離が小さいものを把持対象ワークに該当するものと判定することで、周辺物体に該当する三次元点群を特定する。これにより、他の個体により遮蔽された個体を検出した場合においても、把持対象ワーク部分の計測点による誤検出なく、周辺物体とハンドとの干渉を正しく判定することが可能となる。

なお、本実施形態における装置構成および各処理部の処理内容は基本的には第１の実施形態において図２で示した干渉判定装置１と同様のため、説明を省略する。また、持動作時の干渉を事前判定しながら山積みワークのピッキングを行う処理手順についても、基本的には第１の実施形態において図３に示したフローチャートと同様のため、説明を省略する。ただし、本実施形態では、ステップＳ２０５、Ｓ２０６、Ｓ２０７の処理内容が第１の実施形態と異なる。したがって、そのほかのステップＳ２０１、Ｓ２０２、Ｓ２０３、Ｓ２０４、Ｓ２０８、Ｓ２０９、Ｓ２１０については説明を省略し、以下ではステップＳ２０５、Ｓ２０６、Ｓ２０７についてのみ説明する。

（ステップＳ２０５）
ステップＳ２０５は、対象領域特定部１４が、ステップＳ２０４で導出したワークの位置姿勢に基づき、三次元形状データ上で把持対象ワーク以外に該当する三次元点を特定する。本ステップの処理について図５を用いて説明する。

図５（ａ）は、収容箱に山積みされたワークを撮影した距離画像、および、その距離画像からステップＳ２０２において検出され、位置姿勢が算出された把持対象ワークを示している。センサにより取得した距離画像上で、他の個体に遮蔽されずに観察された把持対象ワーク表面上の三次元点は、算出された位置姿勢に基づいて配置された把持対象ワークの表面との距離が十分に小さくなることが期待される。そこで、ワークモデル保持部１３の保持するワークの三次元形状モデルと、把持対象ワークの位置姿勢とを用いて以下の方法により、把持対象ワーク以外の三次元形状データを特定する。

まず、各三次元点ｋｃ＿ｉ（ｉ＝１〜Ｎ）に対して、算出した位置姿勢におけるワークの三次元形状モデルとの距離ｄ ｉを算出する。具体的には、センサ座標系の原点から、三次元点ｋｃ＿ｉに対応する距離画像上の画素へ向かう視線ベクトルを求め、算出した位置姿勢におけるワークの三次元形状モデルと視線ベクトルとが最短距離で交わる点を求める。この交点と三次元点ｋｃ＿ｉとの距離をｄ ｉとして、３次元点と対応付けて記録する。なお、交点が存在しなかった場合には、想定される距離値に対して極端に大きい値を距離値として記録する。たとえば、ｄ ｉ＜１０ が想定できるのであれば、交点が存在しなかった場合には、ｄ ｉ＝１００００００を記録しておく。ここで、図５（ｂ）は、図５（ａ）の断面図を示している。この図において、破線で示された直線は視線ベクトルを、太線で示された直線は求めた交点と三次元点との距離を示している。次に、記録された距離値ｄ ｉを参照して、三次元点ｋｃ＿ｉが把持対象ワークに該当するか否かを判定する。具体的には、小さい距離値を判定するための閾値θを定めておき、ｄ ｉ＜θのものは把持対象ワークに該当する三次元点、θ≦ｄ ｉであれば把持対象ワーク以外の別の物体に該当する三次元点、と判定する。この処理により、図５（ｂ）において破線で示された白丸は、ワークの三次元形状モデルとの距離値が小さいため、把持対象ワークに該当する三次元点と判定される。

（ステップＳ２０６）
ステップＳ２０５で導出したワークの位置姿勢に基づき、センサ座標系における、ワークの把持動作を行うハンドの位置姿勢を導出する。具体的には、把持を行う際のワークとハンドとの相対位置姿勢を、把持教示情報として記録させる作業を事前に行っておく。そして導出したワークの位置姿勢を表す座標変換行列に対して、記録しておいた把持教示情報に基づき、ワーク・ハンド間の相対位置姿勢を表す座標変換行列を掛け合わせることでセンサ座標系におけるハンドの座標変換行列を求め、両者間の位置姿勢を算出する。

図５（ｃ）に、図５（ｂ）の把持対象ワークに対して、本ステップにより把持を行うハンドの位置姿勢を算出して配置した様子を示す。なお、ステップＳ２０５により把持対象ワークに該当すると判定された三次元点は取り除いている。

（ステップＳ２０７）
ステップＳ２０５で特定した、把持対象ワーク以外に該当する三次元形状データと、ステップＳ２０６で算出した位置姿勢におけるハンドとの干渉判定を実施する。なお、干渉判定には、第１の実施形態と同様の方法を用いればよい。ここで図５（ｃ）中において、把持対象ワーク以外の三次元点のうち、ハンドとの干渉ありと判定されたものを黒丸で示す。

第２の実施形態では、検出したワークを把持する際に、把持対象ワークの位置姿勢を利用して、３Ｄ空間上で把持対象ワーク領域を特定し、周辺物体とハンドとの干渉の有無を事前判定する方法について述べた。具体的には、算出した位置姿勢における把持対象ワークの三次元形状モデルと、三次元形状データを構成する各三次元点との距離に基づき、距離が小さいものを把持対象ワークに該当するものと判定することで、周辺物体に該当する三次元点群を特定する。これにより、他の個体により遮蔽された個体を検出した場合においても、把持対象ワーク部分の計測点による誤検出なく、周辺物体とハンドとの干渉を正しく判定することが可能となる。この方法は主に、ワークの検出方法が他の個体に一部遮蔽されているような個体を検出する性質を持つ場合に有効である。

なお、本実施形態では、各三次元点に対応する距離画像上の画素へ向かう視線ベクトルと、三次元形状モデルとの交点を求めることで、三次元点との距離を算出した。しかし、ワークの三次元形状モデルと各三次元点との距離の算出方法はこの他の方法であってもかまわない。たとえば、算出した位置姿勢におけるワークの三次元形状モデルの表面との最短距離を求め、この値を代わりに利用してもよい。

（第３の実施形態）
図２に示した干渉判定装置１内の各部はハードウェアで構成しても良い。また、ワークモデル保持部１３をメモリで構成し、その他の各部をコンピュータプログラムで構成しても良い。

その場合、一般のＰＣ（パーソナルコンピュータ）等を情報処理装置１〜３に適用することができる。例えば、ワークモデル保持部１３をハードディスク内に構成し、且つモデル保持部以外の各部の機能をＣＰＵに実行させるためのコンピュータプログラムをこのハードディスクに保存しておく。そしてＰＣのＣＰＵがハードディスクに保存されているコンピュータプログラムやデータをＲＡＭに読み出し、読み出したコンピュータプログラムやデータを用いて処理を実行することで、このＰＣは、干渉判定装置１として機能することになる。また、上記の各実施形態や変形例は、適宜組み合わせて用いても良い。

（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

Claims (12)

1. 複数のワークを含んだ空間を撮像する撮像装置を用いて前記空間内の複数の３次元計測点までの距離を取得する取得手段と、
   前記ワークの３次元モデルの情報を保持する保持手段と、
   前記取得手段の取得結果と前記保持手段が保持する３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定手段と、
   前記複数の３次元計測点のうち、前記撮像装置によって撮像された場合に、前記把持対象ワークを前記撮像装置によって撮像した場合に得られる領域と重なる３次元計測点を除外した３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定手段と、を備えることを特徴とする干渉判定装置。
2. 複数のワークを含んだ空間内の複数の３次元計測点までの距離を取得する取得手段と、前記ワークの３次元モデルの情報を保持する保持手段と、
   前記取得手段の取得結果と前記保持手段が保持する３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定手段と、
   前記複数の３次元計測点のうち、前記複数のワークを含んだ空間を上方から撮像した場合に、前記把持対象ワークを上方から撮像した場合の前記把持対象ワークと重なる３次元計測点を除外した３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定手段と、を備えることを特徴とする干渉判定装置。
3. 複数のワークを含んだ空間内の複数の３次元計測点までの距離を取得する取得手段と、前記ワークの３次元モデルの情報を保持する保持手段と、
   前記取得手段の取得結果と前記保持手段が保持する３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定手段と、
   前記複数の３次元計測点のうち、前記特定手段によって位置姿勢が特定された前記把持対象ワークの位置姿勢に配置した前記３次元モデルとの距離が所定の値以上の３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定手段と、を備えることを特徴とする干渉判定装置。
4. 前記特定手段は、色情報に基づいて、前記複数のワークを含んだ空間を撮像した場合に得られる画像の中の前記把持対象ワークの存在する領域を特定することを特徴とする請求項１乃至３のいずれか１項に記載の干渉判定装置。
5. 前記３次元モデルの情報は、ＣＡＤデータであることを特徴とする請求項１乃至４のいずれか１項に記載の干渉判定装置。
6. 更に、前記判定手段による干渉判定の結果、干渉がないと判定された場合、前記把持対象ワークを把持するように前記把持手段を制御する制御手段を備えることを特徴とする請求項１乃至５の何れか１項に記載の干渉判定装置。
7. 前記取得手段は、前記複数のワークを含んだ空間に投影されたパターンを撮像した画像に基づいて、前記複数の３次元計測点までの距離を取得することを特徴とする請求項１乃至６のいずれか１項に記載の干渉判定装置。
8. 複数のワークを含んだ空間を撮像する撮像装置を用いて前記空間内の複数の３次元計測点までの距離、および、前記ワークの３次元モデルの情報を取得する取得工程と、前記取得工程で取得した前記複数の３次元計測点までの距離と前記３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定工程と、
   前記複数の３次元計測点のうち、前記撮像装置によって撮像された場合に、前記把持対象ワークを前記撮像装置によって撮像した場合に得られる領域と重なる３次元計測点を除外した３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定工程と、
   を備えることを特徴とする干渉判定方法。
9. 複数のワークを含んだ空間内の複数の３次元計測点までの距離、および、前記ワークの３次元モデルの情報を取得する取得工程と、
   前記取得工程で取得した前記複数の３次元計測点までの距離と前記３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定工程と、
   前記複数の３次元計測点のうち、前記複数のワークを含んだ空間を上方から撮像した場合に、前記把持対象ワークを上方から撮像した場合の前記把持対象ワークと重なる３次元計測点を除外した３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定工程と、
   を備えることを特徴とする干渉判定方法。
10. 複数のワークを含んだ空間内の複数の３次元計測点までの距離、および、前記ワークの３次元モデルの情報を取得する取得工程と、
    前記取得工程で取得した前記複数の３次元計測点までの距離と前記３次元モデルの情報とに基づいて、前記複数のワークのうちの把持対象ワークの位置姿勢を特定する特定工程と、
    前記複数の３次元計測点のうち、前記特定工程で位置姿勢が特定された前記把持対象ワークの位置姿勢に配置した前記３次元モデルとの距離が所定の値以上の３次元計測点と、前記把持対象ワークを把持する把持手段との干渉判定を行う判定工程と、
    を備えることを特徴とする干渉判定方法。
11. コンピュータに、請求項８乃至１０のいずれか１項の干渉判定方法の各工程を実行させることを特徴とするプログラム。
12. 請求項１乃至７のいずれか１項の干渉判定装置と
    前記干渉判定装置の前記判定手段による干渉判定の結果、干渉がないと判定された場合、把持対象ワークを把持する把持手段を備えることを特徴とするシステム。