JP2010207989A - 対象物の把持システム及び同システムにおける干渉検出方法 - Google Patents

Abstract

【課題】マニプレータが対象物を把持する前に、当該マニプレータと把持対象物以外の物体（他の対象物を含む）との干渉の有無を検出する。
【解決手段】３次元形状取得部２６により取得した対象物２０の３次元位置・姿勢データと対象物２０の３次元モデルとの比較結果に基づき対象物２０の３次元位置・姿勢を認識し、認識した対象物２０に対しｚ方向からマニプレータ１４を降下させ、該マニプレータ１４により対象物２０を把持しようとする際、マニプレータ１４のｚ方向の位置が最も低くなる計算位置にマニプレータ１４の平面視の大きさを考慮した干渉判定用仮想平板を設け、当該干渉判定用仮想平板のｚ方向上方の空間内に前記３次元位置・姿勢データが存在している場合には干渉有りと判定する。
【選択図】図１

Description

この発明は、対象物（３次元形状の物体でワーク等）がばら積み等された状況下において、マニプレータにより特定の対象物を把持しようとする際に、当該マニプレータの他の物体（例えば、前記特定の対象物と同一の物であって、後に把持するための他の対象物を含む。）への干渉を検出する機能を有する対象物の把持システム及び同システムにおける干渉検出方法に関する。

従来から、ワークを把持したマニプレータが、決められた移動範囲内を通過するとき、前記移動範囲を３次元のマトリクス状のエリアに仮想的に分割し、前記マニプレータが前記移動範囲内の特定のエリアで物体（障害物）との干渉を発生したとき、その干渉が発生したエリア（干渉発生エリア）を記憶し、次に、マニプレータがワークを把持して前記移動範囲内を通過するとき、前記干渉発生エリアを避けて前記移動範囲内を通過するように構成した産業用ロボットの制御装置が提案されている（特許文献１）。

また、蓋のない箱にランダムに入れられた複数の対象物の各位置をステレオカメラを利用して計測し画像処理を行って３次元位置を認識し、認識した対象物をマニプレータにより把持し前記箱の中から移動するビンピッキングシステムが提案されている（特許文献２）。この特許文献２では、ステレオカメラにより複数の対象物の各位置を計測した後、把持しようとする対象物以外の対象物とマニプレータとの間で干渉が発生しないかどうか、及びマニプレータと箱との間で干渉が発生したかどうかの干渉確認を行う。干渉発生の可能性があると判断した場合には、マニプレータにより前記箱の中の対象物をかき混ぜ対象物の位置を移動し（変化させ）、再度、把持しようとする対象物以外の対象物及び箱と、マニプレータとの間の干渉を確認して把持搬送作業を継続するようにしている。

特開２００３−１３６４６６号公報 特開２００１−１７９６７７号公報（［００２１］）

しかしながら、上記特許文献１に係る産業用ロボットの制御装置では、マニプレータに負荷検出機構を設け、前記負荷検出機構により実際に負荷を検出したときに、マニプレータと物体（障害物）との干渉が発生したものと検出するので、マニプレータが負荷を受けるまでは干渉の有無が分からず、結果としてマニプレータに無駄な動きをさせる事態が生じ、ひいては対象物移動工程のサイクルタイムが長くなるという問題がある。

また、特許文献２に係るステレオ視方式のビンピッキングシステムにおいては、対象物の濃淡に基づき対象物の３次元位置・姿勢を認識するために、対象物が限定され、さらに、光源変化等の外光の変動によっても対象物の濃淡が変化するので、２つの画像のマッチングの際のあいまいさが残ってしまうという問題がある。しかも２台のビデオカメラの撮像特性を合致させるキャリブレーション操作も繁雑である。また、特許文献２には、マニプレータと障害物との干渉計算方法について、「視覚認識装置が計測した対象物を把持すると仮定したときのマニプレータの姿勢を計算し、その姿勢におけるマニプレータの各部と箱との干渉の有無をチェックし、」とのみ記載されており、干渉確認処理の具体性に欠ける。

この発明はこのような課題を考慮してなされたものであり、マニプレータが対象物を把持する前に、当該マニプレータと把持対象物以外の物体（他の対象物を含む）との干渉の有無を好適に検出することを可能とする対象物の把持システム及び同システムにおける干渉検出方法を提供することを目的とする。

この発明に係る対象物の把持システムは、以下の特徴（１）〜（５）を備える。

（１）基準位置からの対象物の３次元位置・姿勢データを取得する３次元形状取得部と、前記３次元位置・姿勢データと前記対象物の３次元モデルとの比較結果に基づき前記３次元位置・姿勢データから前記対象物の３次元位置・姿勢を認識する３次元位置・姿勢比較部と、３次元位置・姿勢が認識された前記対象物に対しｚ方向からマニプレータを降下させ、該マニプレータにより前記対象物を把持しようとする際、前記マニプレータのｚ方向の位置が最も低くなる計算位置に前記マニプレータの平面視の大きさを考慮した干渉判定用仮想平板を設け、当該干渉判定用仮想平板のｚ方向上方の空間内に前記３次元位置・姿勢データが存在している場合には干渉有りと判定し、存在していない場合には干渉無しと判定する干渉判定部と、を備えることを特徴とする。

この特徴（１）を有する発明によれば、３次元位置・姿勢が認識された前記対象物に対しｚ方向からマニプレータを降下させ、該マニプレータにより対象物を把持しようとする際、前記マニプレータのｚ方向の位置が最も低くなる計算位置に前記マニプレータの平面視の大きさを考慮した干渉判定用仮想平板を設け、当該干渉判定用仮想平板のｚ方向上方の位置で前記３次元位置・姿勢データが存在している場合には干渉有りと判定し、存在していない場合には干渉無しと判定するようにしたので、マニプレータが対象物を把持する前に、当該マニプレータと把持対象物以外の物体（他の対象物を含む）との干渉の有無を検出することができる。

（２）上記の特徴（１）を有する発明において、前記対象物が同一形状の複数の対象物の集合から構成されるとき、前記３次元位置・姿勢比較部では、集団的降下法を用いて、前記複数の対象物の前記３次元モデルとの一致度を算出し、一致度のより大きい前記対象物を次に把持しようとする対象物に仮に決定して前記干渉判定部による干渉判定を行い、干渉有りと判定された場合には、一致度の次に大きい対象物を把持しようとする対象物に仮に決定して前記干渉判定部による干渉判定を行うことを特徴とする。

この特徴（２）を有する発明によれば、一致度の大きい順に対象物を把持する前に、干渉判定の結果、干渉有りと判定した場合には、その対象物の把持を後回しにして、次に一致度の大きい対象物を把持対象にし干渉判定するように制御するので、対象物の集合から対象物を効率的に把持することができる。

（３）上記の特徴（２）を有する発明において、全ての対象物に干渉有りとの判定がなされた場合、前記対象物の集合の配置を変更することで、３次元形状取得部により再度基準位置からの対象物の集合の３次元位置・姿勢データを取得し、３次元位置・姿勢比較部により再び対象物の位置・姿勢を認識し、干渉判定部での干渉判定を再び行うようにすることで、対象物の集合の把持処理を継続することができる。

（４）上記の特徴（２）又は（３）を有する発明において、前記対象物の集合から前記マニプレータにより、前記一致度の大きい前記対象物を前記対象物の集合から把持して取り去ったのち、前記３次元形状取得部により前記対象物を取り去ったエリア部分のみの３次元位置・姿勢データを得、既に取得してある前記対象物の集合の全体の前記３次元位置・姿勢データに貼り付けることを特徴とする。

この特徴（４）に係る発明によれば、例えば、ばら積みされた同一形状の対象物の位置・姿勢を認識し、認識した前記対象物をマニプレータにより把持し所定の位置に移動する作業を短時間で行うことができる。

上記対象物の把持システムの技術思想は、対象物の把持システムにおける干渉検出方法（請求項５記載の発明）に適用することができる。

この発明によれば、マニプレータが対象物を把持する前に、当該マニプレータと把持対象物以外の物体（他の対象物を含む）との干渉の有無を検出することができることから、マニプレータに無駄な動作をさせることがなくなる。結果としてマニプレータによる対象物の把持処理を短時間で行うことができる。

この実施形態に係る対象物の把持システムの構成図である。 対象物の把持システムの動作説明に供されるフローチャートである。 ３次元形状取得部による対象物の集合の３次元位置・姿勢データの取得の仕方の説明図である。 取得された３次元位置・姿勢データの模式図である。 図５Ａは、対象物の集合の実際の状態を表す模式図、図５Ｂは、センシングポリゴンデータの模式図、図５Ｃは、センシングエッジデータの模式図である。 ３次元モデルのポリゴンと、検出した対象物の集合のポリゴンとを集団的降下法を用いて比較し一致度を計算する第１の手法に係るフローチャートである。 集団的降下法の説明図である。 図８Ａは、一致度が低い例の説明図、図８Ｂは、一致度が高い例の説明図である。 図９Ａは、一致度が相対的に低い例の説明図、図９Ｂは、一致度が相対的に高い例の説明図である。 ３次元モデルのエッジと、検出した対象物の集合の対象物のエッジ抽出画像とを集団的降下法を用いて比較し一致度を計算する第２の手法に係るフローチャートである。 図１１Ａは、移動・回転したモデルのエッジ模式図、図１１Ｂは、センシングエッジデータのエッジ模式図、図１１Ｃは、モデルとセンシングデータとのエッジの共通領域の説明図である。 図１２Ａは、一致度が低い例の説明図、図１２Ｂは、一致度が高い例の説明図である。 把持対象候補優先順位表の説明図である。 把持条件の優先順位の説明図である。 干渉判定処理の説明に供されるフローチャートである。 干渉判定処理の説明図である。 干渉判定用仮想平板を説明するための平面視模式図である。

以下、この発明に係る対象物の把持システムにおける干渉検出方法が適用された対象物の把持システムの実施形態について図面を参照して説明する。

図１は、この実施形態に係る３次元位置・姿勢認識装置１２を備える対象物の把持システム１０の構成を示している。

この対象物の把持システム１０は、基本的には、３次元位置・姿勢認識装置１２と、マニプレータ１４を有するロボット（対象物把持移動用ロボット）１６と、ワーク等の同一形状の対象物２０をばら積みする底面を有する収容箱２３と、収容箱２３を置く載置台２４と、から構成される。なお、収容箱２３の底面は、後述するように対象物２０の集合の位置・姿勢の基準面２２とされる。

３次元位置・姿勢認識装置１２は、収容箱２３内に収容された対象物２０の集合２１の３次元位置・姿勢データを取得する３次元形状取得部（２次元レーザ変位センサ）２６と、コンピュータ２８と、から構成される。

３次元形状取得部２６は、公知の光切断法（スリット光Ｌをワークに照射し、ワーク上の曲がった帯状の光をカメラで撮影し、画像内の結像位置から、点列のｘ、ｙ、ｚ値を三角測量の原理に基づいて求める光切断法）等を用いるものであり、２次元レーザ変位センサ等を使用可能であるが、この３次元形状取得部２６が、図示しない搬送手段によりｙ方向に搬送されることで、基準面２２からの対象物２０の集合２１の３次元位置・姿勢データ（ｘ，ｙ，ｚ，α，β，γ）を取得する。なお、スリット光Ｌの死角となる位置が問題となる場合には、センサヘッドをｘ方向に所定間隔離して２つ用いｙ方向に走査することでお互いの死角を補うことができることが慣用されているので、センサヘッドを２つ用いることもこの発明の範囲に含まれる。

基準面２２は、平面であることが好ましいが、３次元形状取得部２６により基準面２２からの高さデータを取得するので、基準面２２に凹凸、うねり等があってもこの発明を適用することができる。なお、対象物２０が床等にばら積みされている場合、当該床が基準面にされる。

３次元形状取得部２６により取得した３次元位置・姿勢データ（ｘ，ｙ，ｚ，α，β，γ）中、位置データ（ｘ，ｙ，ｚ）は、基準位置｛例えば、基準面２２の四頂点のうちの一頂点位置を（０，０，０）｝からの３次元位置データであり、姿勢データ（α，β，γ）は、その位置（ｘ，ｙ，ｚ）におけるｘ軸回りのロール角、ｙ軸回りのピッチ角、ｚ軸回りのヨー角であるが、オイラー角等で代替することもできる。

ロボット１６は、６軸（ｘ，ｙ，ｚ，α，β，γ）の自由度を有するマニプレータ１４を有し、マニプレータ１４は、ハンド部１１４とこのハンド部１１４から延びる第１指部１１１と第２指部１１２とから構成され、３次元位置・姿勢認識装置１２により認識された対象物２０を第１指部１１１と第２指部１１２により把持し所定の位置まで移動する。

ロボット１６は、ロボットコントローラ５４により動作が制御される。

コンピュータ２８は、ＣＰＵがプログラムを実行することにより行う各種処理部と、前記プログラムを格納するメモリ等から構成される。この実施形態では、コンピュータ２８は、３次元形状取得部２６により取得され入力処理部４１で処理（加工）された対象物２０の集合の３次元位置・姿勢データ（ｘ，ｙ，ｚ，α，β，γ）を格納する３次元位置・姿勢データ格納部４２と、対象物２０の３次元モデルデータを予め格納する３次元モデルデータ格納部４４と、３次元モデルデータ格納部４４から読み出した対象物２０の３次元モデルの位置と姿勢を変化させる３次元モデル位置・姿勢変化部４６と、次に説明する比較部４８と、優先順位表格納部５０と、マニプレータ１４と対象物２０との干渉を判定する干渉判定部５１と、通信インタフェース５２と、を備える。

上記の比較部４８は、３次元形状取得部２６により取得された対象物２０の集合２１の３次元位置・姿勢データ（ｘ，ｙ，ｚ，α，β，γ）と、位置と姿勢を変化させた前記３次元モデルの位置・姿勢データ（ｘ，ｙ，ｚ，α，β，γ）とを比較して対象物２０の位置と姿勢を認識し、認識した前記対象物２０の集合２１中、変化させた前記３次元モデルの位置・姿勢に一致度の高い対象物２０を決定する。上記の優先順位表格納部５０は、決定された対象物２０の一致度の高い順に把持候補としての対象物２０の位置・姿勢データを格納する。

ロボットコントローラ５４は、コンピュータ２８による対象物２０の一致度算出・認識処理結果に基づきロボット１６を介して対象物２０がばら積みされている載置台２４から認識された対象物２０を１個ずつ把持し、所定の位置まで搬送（移動）させる。

次に、基本的には以上のように構成される対象物の把持システム１０の動作について、図２のフローチャートに基づき説明する。

ステップＳ１において、コンピュータ２８は、図示しないＣＡＤ（Ｃｏｍｐｕｔｅｒ Ａｉｄｅｄ Ｄｅｓｉｇｎ）システム等により作成した対象物２０の３次元モデルデータ（３次元形状データ）を取得し、３次元モデルデータ格納部４４に格納する。この実施形態において、対象物２０の３次元モデルデータは、４辺形のポリゴンにより形状表現されたデータ｛３次元モデルの基準座標に対する各ポリゴンの頂点データの集合からなるデータ｝としている。

ステップＳ２において、コンピュータ２８は、図示しない搬送手段を通じて３次元形状取得部２６をｙ方向に移動させて、収容箱２３の底面である基準面２２上にばら積みされた対象物２０の集合２１の３次元位置・姿勢データを取得し、入力処理部４１による処理後に３次元位置・姿勢データ格納部４２の所定範囲に格納する。なお、入力処理部４１による処理は、比較部４８で行うこともできる。

この場合、図３に模式的に示すように、３次元形状取得部２６がｙ方向に等速度で搬送されることで、３次元形状取得部２６のスリット光Ｌの走査に基づき基準面２２からの対象物２０の集合２１（模式的に直方体で描いているが、実際には、図１に示すようにばら積みされた対象物２０の集合２１）の３次元位置・姿勢データ（形状データ、高さデータ）を取得することができる。

この場合、検出された（計測された）対象物２０の集合２１の高さｓｚｉｊ（ここで、ｓは検出された対象物２０、ｚは高さ、ｉｊはｘｙ座標を意味する。）は、図４に示すように、模式的に表すことができる。

図４において、座標ｘｉは、３次元形状取得部２６のｘ方向の解像度で決まり、座標ｙｊは、走査回数（スキャンサイクル）と３次元形状取得部２６の移動速度で決まる離散値である。

入力処理部４１により３次元形状取得部２６から得られたデータを処理し、基準面２２の座標（ｘｉ，ｙｊ）からの高さｓｚｉｊ、換言すれば、対象物２０の集合２１を、例えば、４辺形のポリゴン１００に分割した各頂点の各座標（ｘｉ，ｙｊ，ｓｚｉｊ）からなる３次元位置・姿勢データを得ることができる。各ポリゴン１００の姿勢（α，β，γ）は、入力処理部４１において、ポリゴン１００の４頂点の座標より計算することができる。このようにして３次元形状取得部２６により取得した対象物２０の集合２１を表すポリゴン１００の全体をセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）という。センシングポリゴンデータはセンシング形状データともいう。センシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）中、位置データ（ｘｉ，ｙｊ，ｓｚｉｊ）は、例えば、４辺形のポリゴン１００の中心位置とする。

ここで、図１に示す対象物２０の集合２１の実際の状態（鳥瞰図）が、例えば図５Ａの模式図のように表されると仮定する（理解の妨げとなるので、対象物２０同士の重なり合いを描いていない）。

この場合、図５Ａの模式図に対応する図５Ｂに示すようなセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）が、３次元位置・姿勢データ格納部４２に格納される。基準面２２のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ、ただし、この実施形態では、ｓｚｉｊ＝０，α＝０，β＝０，γ＝０と仮定している。）は、描いていない。

ポリゴン１００は、３次元形状取得部２６により分割された画素単位としてもよく、入力処理部４１による画像処理により画素の整数倍単位としてもよい。

３次元位置・姿勢データ格納部４２には、センシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）が格納される他、センシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）を入力処理部４１で作成する際に用いたポリゴン１００（４辺形）を形成する各頂点データ（ｘｉ，ｙｊ，ｓｚｉｊ）も格納される。

図５Ｃは、３次元位置・姿勢データ格納部４２に格納されている４辺形を形成するポリゴン１００の各頂点データ（ｘｉ，ｙｊ，ｓｚｉｊ）またはセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）から微分処理等により抽出した各対象物２０の輪郭を表すエッジ抽出データを模式的に示している。対象物２０に開口が明いている場合には、１つの対象物２０から２つの輪郭が得られる場合があるが、対象物２０が同一形状であるので、内側の開口の輪郭は画像処理により除去することができる。

エッジは、例えば、１画素｛画素が四辺形であれば、その１辺であり、画素と区別するために辺素（ワイヤフレームモデルの単位ワイヤ）ともいう。｝の位置・姿勢データ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）の結合で表される。３次元形状取得部２６により取得した各座標を元に得られるので、対象物２０の集合２１のセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）という。センシングエッジデータは、センシング輪郭データともいう。

このようにして、図５Ｂに示す対象物２０の集合２１のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ）の他、図５Ｃに示す対象物２０の集合２１のセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）も、３次元位置・姿勢データ格納部４２に格納される。

次いで、ステップＳ３において、比較部４８において、対象物２０がばら積みされた集合２１のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ）、又はセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）、あるいはセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ）及びセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）と、３次元モデル位置・姿勢変化部４６からの３次元モデルデータの加工データとを用いて、対象物２０の集合２１を構成する各対象物２０の３次元位置・姿勢を計算する。このステップＳ３の詳細な処理については後述する。

上述したように、対象物２０は、ばら積みされているので、ｚ方向に重なっている対象物２０も存在する。そこで、この実施形態では、比較部４８での処理結果に基づいて位置と姿勢が計算された各対象物２０を、ステップＳ４において、ロボット１６のマニプレータ１４により把持するために、ｚ方向のより高い位置にある対象物２０を、把持優先順位の高い対象物２０に決定（指定）する把持候補優先順位表１２４（図１３等）を出力し、優先順位表格納部５０に格納する。

次いで、ステップ５において、詳細を後述する、対象物２０の優先順位ｕ（把持対象優先順位ｕともいう。）と対象物２０の把持条件の優先順位ｖ（把持条件優先順位ｖともいう。）と、を考慮した優先順位の高い対象物２０から事前干渉確認処理を行う。

事前干渉確認処理の結果、把持対象優先順位ｕの対象物２０が把持条件優先順位ｖの把持条件で干渉しないと判断した場合、ステップＳ６において、把持対象優先順位ｕの対象物２０の３次元位置・姿勢データと、把持条件優先順位ｖの把持条件とを通信インタフェース５２を通じてロボットコントローラ５４に送る。

次に、ステップＳ６において、ロボットコントローラ５４からの指令によりロボット１６は、マニプレータ１４により該当する対象物２０を把持し、所定の位置に移動する。

次に、ステップＳ３の対象物２０の３次元位置・姿勢計算手法について、２つの手法｛第１の手法：３次元モデルのポリゴンデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）によるポリゴン１００ｍと、検出した対象物２０の集合２１のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）によるポリゴン１００とを集団的降下法を用いて比較し一致度Ａを計算する手法、第２の手法：３次元モデルのエッジデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）の辺素（１画素）と、検出した対象物２０の集合２１の対象物２０のエッジデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）の辺素（１画素）と、を集団的降下法を用いて比較し一致度Ｂを計算する手法｝について説明する。

まず、差分進化(Ｄｉｆｆｅｒｅｎｔｉａｌ Ｅｖｏｌｕｔｉｏｎ)ＤＥや粒子群最適化(Ｐａｒｔｉｃｌｅ Ｓｗａｒｍ Ｏｐｔｉｍｉｚａｔｉｏｎ)ＰＳＯ等のように解集団による最適化の際に降下法を利用した最適化法である既知の集団的降下法（Ｐｏｐｕｌａｔｉｏｎ−ｂａｓｅｄ ｄｅｓｃｅｎｔ ｍｅｔｈｏｄ）について一般的に説明する。

１．初期化:集団に属する解をランダムに発生する。
２．評価:全ての解を評価する。
３．終了判定:終了条件を満足すれば終了する。
４．各解に対して、（ａ）生成:各解と集団の情報に基づき新しい解を生成し、（ｂ）評価:新しい解を評価し、（ｃ）更新:新しい解が古い解より良ければ、古い解を新しい解に置換する。
５．３．へ戻る。

以上の説明が、既知の集団的降下法の一般的な説明である。

次に、３次元モデルのポリゴンデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）によるポリゴン１００ｍと、検出した対象物２０の集合２１のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）によるポリゴン１００とを集団的降下法を用いて比較し一致度Ａを計算する第１の手法について、図６のフローチャートを参照して説明する。

ステップＳ３ａにおいて、３次元モデル位置・姿勢変化部４６は、３次元モデルデータ格納部４４から対象物２０の３次元モデルを読み出し、３次元モデルを構成している４辺形の各ポリゴン１００ｍの各頂点データ（基準位置からのｘ，ｙ，ｚ位置データ）を抽出する。

次いで、ステップＳ３ｂにおいて、上述した対象物２０の集合２１のセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）、すなわち一致度Ａを評価（算出）したいデータを元に、図７に示すように、対象物２０の３次元モデル１２０（３次元ポリゴン表現モデル）の各頂点データから計算したモデルポリゴンデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）からなる３次元モデル１２０を、各格子点位置（ｘｉ，ｙｊ）を基準位置座標として回転させ（＋α，−α）、（＋β，−β）、（＋γ，−γ）、ｚ方向に推定最大高さであるセンシング高さ閾値ｓｚｔｈから基準面２２の高さ０［ｍｍ］まで降下させる（−ｚ）。

ステップＳ３ｃにおいて、移動（降下）・回転させた頂点データ群であるモデルポリゴンデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）から、図３の格子点座標（ｘｉ，ｙｊ）上での基準面２２（０［ｍｍ］）からのモデル高さデータｍｚｉｊを補間により求める。

次いでステップＳ３ｄにおいて、基準面２２の全ての格子点座標（ｘｉ，ｙｊ）上でパラメータνｉｊを定義し、モデルポリゴンデータ及びセンシングポリゴンデータが共に存在する格子点座標（ｘｉ，ｙｊ）上で、かつモデルポリゴン１００ｍの高さｍｚｉｊとセンシングポリゴン１００の高さｓｚｉｊの絶対値の差ｅｉｊが、誤差の閾値Ｅｔｈ以内で、センシングポリゴン１００の高さｓｚｉｊがセンシング高さ閾値ｓｚｔｈを下回る場合に、パラメータνｉｊをνｉｊ＝１とし、それ以外の場合には、パラメータνｉｊ＝０にする。

さらに、ステップＳ３ｅにおいて、モデルポリゴンデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）及びセンシングポリゴンデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）が共に存在する格子点座標（ｘｉ，ｙｊ）のペアの総数をＳとするとき、一致度Ａを次式（［数１］）で計算する。

一致度Ａの値の大きさについて図８Ａ、図８Ｂ、図９Ａ、図９Ｂを参照して、模式的に説明すると、図８Ａと図８Ｂにおいて、３次元モデル１２０のモデルポリゴンデータと対象物２０のセンシングポリゴンデータの一致度Ａは、図８Ｂの状態が図８Ａの状態よりも大きいことが分かる。また、図９Ａと図９Ｂにおいて、３次元モデル１２０のモデルポリゴンデータと対象物２０のセンシングポリゴンデータとの一致度Ａは、図９Ａの状態が図９Ｂの状態よりも大きい。

したがって、一致度Ａは、３次元モデル１２０のモデルポリゴンデータと対象物２０のセンシングポリゴンデータが一致しているほど、Ａ＝１に近い値となる。

このようにして、対象物２０が存在する３次元位置・姿勢を把握することができる。ただし、対象物２０がｚ方向で重なっている場合には、重なって上側にある対象物２０は、一致度ＡがＡ≒１となって検出できるが、重なって下側にある対象物２０は、平面視で重なっている部分のセンシングポリゴンデータがないので、一致度Ａが平面視で重なっている部分に対応して１より小さくなる。すなわち、一致度ＡがＡ≒１となっている対象物２０は、ロボット１６のマニプレータ１４による把持対象とされるが、一致度Ａが１より明らかに小さい場合（Ａ＜１）には把持対象とされない。この結果、一致度ＡがＡ＝１に近いほど、把持候補の優先順位を高くすることができる。

次に、３次元モデルのエッジデータ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）の辺素と、検出した対象物２０の集合２１の対象物２０のセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）の辺素と、を集団的降下法を用いて比較し一致度Ｂを計算する第２の手法について、図１０のフローチャートを参照して説明する。

ステップＳ３ｕにおいて、３次元モデル位置・姿勢変化部４６は、３次元モデルデータ格納部４４から対象物２０の３次元モデル１２０を読み出し、３次元モデル１２０からエッジを抽出する。この場合、抽出した３次元エッジモデルは、例えば、１辺素（単位辺素）が連結されたワイヤフレームモデルとなる。

この場合、図７の図面と同一の図面を描いて説明すると繁雑になるので、図７の図面において、対象物２０の３次元モデル１２０は、３次元ポリゴンモデルではなく３次元エッジモデル（３次元ワイヤフレームモデル）であるものとする。

次いで、ステップＳ３ｖにおいて、３次元位置・姿勢データ格納部４２に格納されている対象物２０の集合２１のセンシングエッジデータ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）を読み出し、図７に示すように、対象物２０の３次元エッジモデルを、基準位置座標で回転させ（＋α，−α）、（＋β，−β）、（＋γ，−γ）、Ｚ方向に推定最大高さであるセンシング高さ閾値ｓｚｔｈから基準面２２の高さ０［ｍｍ］まで降下させる（−ｚ）。

ステップＳ３ｗにおいて、格子点座標（ｘｉ，ｙｊ）上、パラメータｗｉｊを定義し、回転・降下させたエッジデータ群であるモデルエッジデータｍｄｉｊ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）で表される辺素（１画素）と、センシングエッジデータｓｄｉｊ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）で表される辺素（１画素）の位置・姿勢を比較し、図３の格子点座標（ｘｉ，ｙｊ）上での領域を共通する辺素（１画素）を検出することで両方の共通領域部分がある格子点座標（ｘｉ，ｙｊ）のパラメータｗｉｊ＝１とし、それ以外の場合には、パラメータｗｉｊ＝０にする。

ただし、パラメータｗｉｊをｗｉｊ＝１とする場合、ノイズの混入を回避するため（検出精度を上げるため）、モデルエッジデータｍｄｉｊ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）のエッジ強度ｍｄｉｊ（微分値）がモデルエッジ強度の閾値ｍＤｔｈ（シミュレーションあるいは実験的に定めることができる。）より大きいこと、センシングエッジデータｓｄｉｊ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）のエッジ強度ｓｄｉｊ（微分値）がセンシングエッジ強度の閾値ｓＤｔｈより大きいことを条件とする。

モデルエッジデータｍｄｉｊ（ｘｉ，ｙｊ，ｍｚｉｊ，α，β，γ）とセンシングエッジデータｓｄｉｊ（ｘｉ，ｙｊ，ｓｚｉｊ，α，β，γ）の共通部分を算出する処理について、図１１Ａ〜図１１Ｃを参照して説明すると、移動・回転した辺素（画素、輪郭要素）が連結された構成の３次元モデルのモデルエッジデータｍｄｉｊ（図１１Ａ）と、対象物２０のセンシングエッジデータｓｄｉｊ（図１１Ｂ）とを対比すると、パラメータｗｉｊ＝１となる共通領域１２２は、図１１Ｃに示すように、両辺素の位置・姿勢が一致している領域となる。

そして、ステップＳ３ｘにおいて、３次元モデル１２０のモデルエッジデータｍｄｉｊと対象物２０のセンシングエッジデータｓｄｉｊとが一致し、かつ共通領域１２２が閉ループを形成しているパラメータｗｉｊの数を、一致度Ｂとして次式（［数２］）で表す。なお、対象物２０のセンシングエッジデータは、閉ループのワイヤフレームを構成する。

この場合、一致度Ｂの値は、一般には、対象物２０の平面視の輪郭が大きいほど（輪郭の長さが長いほど）大きい値になる。

なお、図１２Ａ（図１１Ｃ）に示すように、共通領域１２２が閉ループを形成していない場合には、対象物２０の存在位置・姿勢を検出することができないので、パラメータｗｉｊの総和である一致度Ｂを算出せず、図１２Ｂに示すように、共通領域１２２が所定の誤差範囲で閉ループを形成している場合にパラメータｗｉｊの総和である一致度Ｂを算出する。

このようにして、対象物２０の存在位置・姿勢を把握できる。ただし、対象物２０がｚ方向で（平面視で）重なっている場合、重なって上側にある対象物２０は、共通領域１２２を連結した画像は閉ループとなり対象物２０の輪郭となって把持対象となる。しかし、重なって下側にある対象物２０は、平面視で重なっている部分のセンシングエッジデータｓｄｉｊがないので、共通領域１２２を連結した画像は開ループとならず対象物２０の輪郭にならないので、把持対象とされない。

上述したステップＳ３ｂ、Ｓ３ｃの計算処理、及びステップＳ３ｖ、Ｓ３ｗの計算は処理負荷が大きいのでＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いた並列高速計算処理で行うことが好ましい。

上述した第１及び第２の２つの手法により算出した一致度Ａ、又は一致度Ｂを対象物２０の集合２１からロボット１６のマニプレータ１４によって把持しようとする対象物２０の把持候補の優先順位を付けるための評価関数Ｆ^evaluationとする。

さらに好ましくは、求めた一致度Ａ、Ｂを組み合わせて、対象物２０の集合２１から対象物２０の把持候補の優先順位を付けるための次式（［数３］）に示す評価関数Ｆ^evaluationとしてもよい。

ここで、ｇは重み係数である。値は、実験的あるいはシミュレーションにより求めることができる。

なお、評価関数Ｆ^evaluationは、上式を含むより一般的な次式（［数４］で記述することができる。

ここで、ｐ，ｑ，ｓ，ｔは、任意の整数であるが、ｇ０，１＝１、ｇ１，１＝ｇ、ｐ＝０、ｑ＝１、ｓ＝−１、ｔ＝１の場合、上式（［数３］）になる。

評価関数Ｆ^evaluationの値を大きな順に並べると、図１３に示すような、対象物２０の把持対象の候補に優先順位ｕ（把持対象優先順位ｕ、ｕ＝１，２，３，…）を付けた表（把持対象候補優先順位表１２４、図１３参照）を得ることができる。

次に、ステップＳ２の処理に先立ち実行されるオフライン処理でのステップＳ１１（図２参照）の対象物２０に対する把持条件優先順位ｖの算出アルゴリズムについて説明する。

なお、把持条件優先順位ｖの算出アルゴリズムは、認識された対象物２０の３次元形状、収容箱２３の形状、ロボット１６及びマニプレータ１４の形状・仕様等により変化し、一律に決定することはできないが、基本的には、対象物２０の把持移動のサイクルタイムを短縮化する、あるいは対象物２０を確実に把持する（対象物２０の持ちやすい方向）等の条件により把持条件の優先順位ｖが決定される。

例えば、認識された対象物２０の形状が、円柱形状であって、図１４に示すように、基準面２２上に横たわっている場合、第１指部１１１と第２指部１１２を備えるハンド部１１４により把持しようとするとき、第１指部１１１と第２指部１１２とをなるべく離間させないで対象物２０を把持できる指部位置の優先順位ｖを高いものとする。したがって、図１４例の場合には、第１指部１１１と第２指部１１２による対象物２０の側面からの把持が把持条件の優先順位ｖ＝１になり、対象物２０の上面・底面からの把持が把持条件の優先順位ｖ＝２になる。

このようにして対象物２０を把持する優先順位である把持条件優先順位ｖ＝１、２、…が決定される。

ここで、評価関数Ｆ^evaluationに基づき決定される把持対象優先順位をｕ（ｕ＝１，２，３，…）で表し、最適把持条件による優先順位をｖ（ｖ＝１，２，３，…）と表す。

ついで、ステップＳ５の干渉判定処理について、図１５のフローチャートに基づき説明する。

なお、図１５のフローチャートにおいて、ステップＳ２、Ｓ３、Ｓ４、Ｓ６の各処理は、図２の同じステップの処理を表している。

ステップＳ５ａにおいて、干渉判定処理の容易化のために、認識された対象物２０の優先順位付けの初期化処理を行う。この初期化処理では、対象物２０の把持対象優先順位ｕをｕ＝１とし、把持条件優先順位ｖをｖ＝１とする（ｕ＝ｖ＝１）。

次いで、ステップＳ５ｂにおいて、優先順位ｕ番目（最初は、ｕ＝１）の把持対象を干渉判定対象とする。

次に、ステップＳ５ｃにおいて、把持対象優先順位ｕと認識した対象物２０について、優先順位ｖ番目（最初は、ｖ＝１）の把持条件での事前干渉確認（事前干渉チェック）を行う。

このステップＳ５ｃでの干渉計算アルゴリズムについて、図１６、図１７を参照して説明する。

先ず、把持対象優先順位ｕと認識した対象物２０のｚ方向上方の基準高さ位置（例えば、認識した対象物２０の集合２１中、最も高い位置）にマニプレータ１４を仮想的に持って行く。次に、基準高さ位置から把持対象優先順位ｕの対象物２０の把持位置まで、マニプレータ１４をｚ方向に仮想的に下降させる（図１６参照）。

次いで、仮想的に下降させた把持位置で第１指部１１１と第２指部１１２との間隔を少し開き、第１指部１１１と第２指部１１２の各指先のｚ方向下方に指先から適当な余裕代Δを持たせ、かつ対象物２０からも適当な余裕代Δを持たせ、適当な形状寸法の干渉判定用仮想平板Ｉｖ（この実施形態では、ｖは、把持条件優先順位に対応し、最初は、Ｉｖ＝１）を仮想的に設定する。

このとき、干渉判定用仮想平板Ｉ１の領域（ｘｙ面）で干渉判定用仮想平板Ｉ１よりも大きなｚ座標値を持つセンシングデータが存在すれば、マニプレータ１４（第１指部１１１、１１２）は優先順位ｕ番目の対象物２０以外の物と干渉有りと判断する（ステップＳ５ｃ：ＹＥＳ）。例えば、図１６例では、右側の干渉判定用仮想平板Ｉｖ＝Ｉ１よりもｚ方向の高い位置に障害物２００（センシングデータ）が存在するので、対象物２０を把持する際に障害物２００との干渉が発生すると事前にチェックされる。

干渉有りと判断した場合には、ステップＳ５ｄにおいて、最適把持条件の優先順位ｖをｖ＝ｖ＋１とし、ステップＳ５ｅにおいて、全ての把持条件が確認されたかどうかを判断し、把持条件が残っている場合には、再び、ステップＳ５ｃにおいて、優先順位ｖがｖ＝２の把持位置で干渉判定用仮想平板Ｉｖ＝Ｉ２を設定し、再度同様に事前干渉チェックを行う。

なお、この実施形態では、図１７に示すように、対象物２０の短手方向に把持条件優先順位ｖ＝１の干渉判定用仮想平板Ｉｖ＝Ｉ１の対が設定され、対象物２０の長手方向に把持条件優先順位ｖ＝２の干渉判定用仮想平板Ｉｖ＝Ｉ２の対が設定される。

ステップＳ５ｅにおいて、把持条件ｖが残っていない場合には、優先順位ｕ番目の把持対象物２０を把持することを諦める。

次に、ステップＳ５ｆにおいて、把持対象の優先順位ｕをｕ＝ｕ＋１とし、ステップＳ５ｇにおいて優先順位ｕの付けられた全ての把持対象物２０（把持候補）が干渉確認の対象とされたかどうかを判断し、優先順位ｕの付けられた把持対象物２０が残っている場合には、ステップＳ５ｂ以下の干渉判定処理を継続する。

ステップＳ５ｇの判断において、もし優先順位ｕの付けられた全ての把持対象物２０を干渉確認してもマニプレータ１４に対して干渉のない把持対象物２０が存在しなかった場合には、ステップＳ５ｈにおいて、ロボットコントローラ５４に対してかき混ぜ指令を送出する。

この指令によりロボットコントローラ５４は、マニプレータ１４により対象物２０の集合２１をかき混ぜて対象物２０の集合２１の形状を変更させ、再度、ステップＳ２以降の対象物２０のセンシング処理を行う。

なお、かき混ぜ処理は、コンピュータ２８からの警告等に基づき、作業者が行ってもよく、載置台２４に振動機構を設けて収容箱２３を揺らすようにしてもよい。

なお、対象物２０がｚ方向に重なっている場合には、優先順位ｕの高い対象物２０を把持・移動した後に、ステップＳ２における対象物２０の集合２１の位置・姿勢データを再取得する際、優先順位ｕの高い対象物２０を取り去ったエリア部分のみを３次元形状取得部２６により再走査して元の対象物２０の集合２１の３次元位置・姿勢データに貼り付けることにより、対象物２０の集合２１の位置・姿勢データの取得時間を大幅に短縮することができる。もちろん、ステップＳ６における把持・移動作業後に、ステップＳ２で取得した３次元位置・姿勢データから対象物２０の重なり部分がないと判断した後は、３次元形状取得部２６による再走査を行う必要がない。

以上説明したように、上述した実施形態に係る対象物２０の把持システム１０よれば、基準位置からの対象物２０の３次元位置・姿勢データを取得する３次元形状取得部２６と、前記３次元位置・姿勢データと対象物２０の３次元モデルとの比較結果に基づき前記３次元位置・姿勢データから対象物２０の３次元位置・姿勢を認識する３次元位置・姿勢比較部４８と、３次元位置・姿勢が認識された対象物２０に対しｚ方向からマニプレータ１４を降下させ、該マニプレータ１４により対象物２０を把持しようとする際、マニプレータ１４のｚ方向の位置が最も低くなる計算位置にマニプレータ１４の平面視の大きさを考慮した干渉判定用仮想平板Ｉｖを設け、当該干渉判定用仮想平板Ｉｖのｚ方向上方の空間内に前記３次元位置・姿勢データが存在している場合には干渉有りと判定し、存在していない場合には干渉無しと判定する干渉判定部５１と、を備える。

このように処理することで、マニプレータ１４が対象物２０を把持する前に、当該マニプレータ１４と把持対象物２０以外の物体（他の対象物を含む）との干渉の有無を検出することができる。

この場合、対象物２０が同一形状の複数の対象物２０の集合２１から構成されるとき、３次元位置・姿勢比較部４８では、集団的降下法を用いて、複数の対象物２０の３次元モデルとの一致度を算出し、一致度のより大きい対象物２０を次に把持しようとする対象物２０に仮に決定して干渉判定部５１による干渉判定を行い、干渉有りと判定された場合には、一致度の次に大きい対象物２０を把持しようとする対象物２０に仮に決定して干渉判定部５１による干渉判定を行うようにすることが好ましい。

すなわち、一致度の大きい順に対象物２０を把持しようとする前に、干渉判定の結果、干渉有りと判定した場合には、その対象物２０の把持を後回しにして、次に一致度の大きい対象物２０を把持対象にするように制御するので、対象物２０の集合２１から対象物２０を効率的に把持することができる。

なお、全ての対象物２０に干渉有りとの判定がなされた場合、対象物２０の集合２１の配置を、例えば、マニプレータ１４を作動させて強制的に形状を変更することで、３次元形状取得部２６により再度基準位置からの対象物２０の集合２１の３次元位置・姿勢データを取得し、３次元位置・姿勢比較部４８により再び対象物２０の位置・姿勢を認識し、干渉判定部５１での干渉判定を再び行うようにして、対象物２０の集合２１の把持処理を継続することができる。

上述したように、対象物２０の集合２１からマニプレータ１４により、一致度の大きい対象物２０を対象物２０の集合２１から把持して取り去ったのち、３次元形状取得部２６により対象物２０を取り去ったエリア部分のみの３次元位置・姿勢データを得、既に取得してある対象物２０の集合２１の全体の前記３次元位置・姿勢データに貼り付けることで、例えば、ばら積みされた同一形状の対象物２０の位置・姿勢を認識し、認識した対象物２０をマニプレータ１４により把持し所定の位置に移動する作業を短時間で行うことができる。

なお、この発明は、上述した実施形態に限らず、この明細書の記載内容に基づき、種々の構成を採り得ることはもちろんである。

１０…対象物の把持システム １２…３次元位置・姿勢認識装置
１４…マニプレータ １６…ロボット
２０…対象物 ２１…集合
２２…基準面 ２３…収容箱
２４…載置台 ２６…３次元形状取得部
２８…コンピュータ ４２…３次元位置・姿勢データ格納部
４４…３次元モデルデータ格納部 ４６…３次元モデル位置・姿勢変化部
４８…比較部 ５０…優先順位表格納部
５１…干渉判定部 ５２…通信インタフェース
５４…ロボットコントローラ １００…ポリゴン（センシングポリゴン）
１００ｍ…ポリゴン（モデルポリゴン） １１１…第１指部
１１２…第２指部 １１４…ハンド部
１２０…３次元モデル １２２…共通領域

Claims (5)

1. 基準位置からの対象物の３次元位置・姿勢データを取得する３次元形状取得部と、
   前記３次元位置・姿勢データと前記対象物の３次元モデルとの比較結果に基づき前記３次元位置・姿勢データから前記対象物の３次元位置・姿勢を認識する３次元位置・姿勢比較部と、
   ３次元位置・姿勢が認識された前記対象物に対しｚ方向からマニプレータを降下させ、該マニプレータにより前記対象物を把持しようとする際、前記マニプレータのｚ方向の位置が最も低くなる計算位置に前記マニプレータの平面視の大きさを考慮した干渉判定用仮想平板を設け、当該干渉判定用仮想平板のｚ方向上方の空間内に前記３次元位置・姿勢データが存在している場合には干渉有りと判定し、存在していない場合には干渉無しと判定する干渉判定部と、
   を備えることを特徴とする対象物の把持システム。
2. 請求項１記載の対象物の把持システムにおいて、
   前記対象物が同一形状の複数の対象物の集合から構成されるとき、前記３次元位置・姿勢比較部では、集団的降下法を用いて、前記複数の対象物の前記３次元モデルとの一致度を算出し、一致度のより大きい前記対象物を次に把持しようとする対象物に仮に決定して前記干渉判定部による干渉判定を行い、干渉有りと判定された場合には、一致度の次に大きい前記対象物を把持しようとする対象物に仮に決定して前記干渉判定部による干渉判定を行う
   ことを特徴とする対象物の把持システム。
3. 請求項２記載の対象物の把持システムにおいて、
   全ての対象物に干渉有りとの判定がなされた場合、前記対象物の集合の配置を、変更する
   ことを特徴とする対象物の把持システム。
4. 請求項２又は３記載の対象物の把持システムにおいて、
   前記対象物の集合から前記マニプレータにより、前記一致度の大きい前記対象物を前記対象物の集合から把持して取り去ったのち、前記３次元形状取得部により前記対象物を取り去ったエリア部分のみの３次元位置・姿勢データを得、既に取得してある前記対象物の集合の全体の前記３次元位置・姿勢データに貼り付ける
   ことを特徴とする対象物の把持システム。
5. ３次元形状取得部により基準位置からの対象物の３次元位置・姿勢データを取得するステップと、
   前記３次元位置・姿勢データと前記対象物の３次元モデルとの比較結果に基づき前記３次元位置・姿勢データから前記対象物の３次元位置・姿勢を認識する３次元位置・姿勢比較ステップと、
   ３次元位置・姿勢が認識された前記対象物に対しｚ方向からマニプレータを降下させ、該マニプレータにより前記対象物を把持しようとする際、前記マニプレータのｚ方向の位置が最も低くなる計算位置に前記マニプレータの平面視の大きさを考慮した干渉判定用仮想平板を設け、当該干渉判定用仮想平板の方向上方の位置で前記３次元位置・姿勢データが存在している場合には干渉有りと判定し、存在していない場合には干渉無しと判定する干渉判定ステップと、
   を備えることを特徴とする対象物の把持システムにおける干渉検出方法。

Images