JP5544464B2 - 対象物の３次元位置・姿勢認識装置及びその方法 - Google Patents

Description

この発明は、位置・姿勢を認識しようとする同一形状の対象物（３次元形状の物体でワーク等）がばら積み等された状況下において、各対象物の位置と姿勢を認識する対象物の３次元位置・姿勢認識装置及びその方法に関する。

従来から、例えば、ばら積みされた同一形状の対象物の位置・姿勢を３次元位置・姿勢認識装置により認識し、認識した前記対象物をロボットにより把持し所定の位置に移動する対象物認識把持搬送システムが知られている。

この対象物認識把持搬送システムは、対象物の位置・姿勢を認識する３次元位置・姿勢認識装置と、対象物を把持して移動するマニプレータを備えるロボットとから構成される。

特許文献１には、対象物のエッジを抽出するカメラと、前記対象物の高さを抽出する超音波距離センサを用いて対象物の位置と高さを認識し、認識した対象物をロボットにより把持して所定位置まで移動するロボット装置が開示されているが、対象物の位置と高さを２種類のセンサ（カメラと超音波センサ）により認識するので装置構成が複雑である。

特許文献２、３には、２台のビデオカメラを用いて、ばら積みされた対象物の位置・姿勢を、いわゆるステレオ視方式の画像処理により認識する３次元位置・姿勢認識装置が開示されている。

特開平７−８８７９１号公報 特許第３５２５８９６号公報 特許第４００４８９９号公報

しかしながら、上記特許文献２、３に係るステレオ視方式の３次元位置・姿勢認識装置では、対象物の濃淡に基づき対象物の３次元位置・姿勢を認識するために、対象物が限定され、さらに、光源変化等の外光の変動によっても対象物の濃淡が変化するので、２つの画像のマッチングの際のあいまいさが残ってしまうという問題がある。しかも２台のビデオカメラの撮像特性を合致させるキャリブレーション操作も繁雑である。

この発明はこのような課題を考慮してなされたものであり、基準面上に置かれた同一形状の複数の対象物の３次元位置・姿勢を正確かつ容易に認識することを可能とする対象物の３次元位置・姿勢認識装置及びその方法を提供することを目的とする。

この発明に係る対象物の３次元位置・姿勢認識装置は、基準面上に置かれた同一形状の複数の対象物の集合から各対象物の位置・姿勢をコンピュータにより認識する対象物の３次元位置・姿勢認識装置において、以下の特徴（１）〜（５）を有する。

（１）前記対象物の集合の３次元位置・姿勢データを取得する３次元形状取得部と、前記対象物の３次元モデルの位置と姿勢を変化させる３次元モデル位置・姿勢変化部と、前記３次元形状取得部により得られた前記対象物の集合の３次元位置・姿勢データと、位置と姿勢を変化させた前記３次元モデルの位置・姿勢データとを比較して前記対象物を認識し、認識した前記対象物の集合から、前記３次元モデルの位置・姿勢に一致度の高い対象物を決定する比較部と、を備えることを特徴とする。

この特徴（１）を有する発明によれば、３次元形状取得部により得られた対象物の集合の３次元位置・姿勢データと、前記対象物の３次元モデルであって位置と姿勢を変化させた当該３次元モデルの位置・姿勢データとを比較して前記対象物を認識し、認識した前記対象物の集合中、前記３次元モデルの位置・姿勢に一致度の高い対象物を決定するようにしているので、前記対象物の集合中、前記３次元モデルの位置・姿勢に一致度の高い対象物を正確に決定することができる。

この結果、基準面上に、例えば、ばら積みされた同一形状の対象物の３次元位置・姿勢を正確かつ容易に認識することができる。

（２）特徴（１）を有する発明において、前記３次元モデル位置・姿勢変化部と前記比較部は、集団的降下法を用いて、前記対象物の集合の３次元位置・姿勢データを構成するポリゴンと、前記３次元モデルの位置・姿勢データを構成するポリゴンとを比較し、ｚ方向の位置が一致しているポリゴンの数に基づき前記対象物を決定することで、対象物をより正確かつ短時間に決定することができる。

（３）特徴（１）を有する発明において、前記３次元モデル位置・姿勢変化部と前記比較部は、集団的降下法を用いて、前記対象物の集合の３次元位置・姿勢データを構成する輪郭と、前記３次元モデルの位置・姿勢データを構成する輪郭とを比較し、輪郭の共通領域部分の数に基づき前記対象物を決定することで、対象物を正確かつ短時間に決定することができる。

（４）特徴（２）に記載の対象物の３次元位置・姿勢認識装置により求めた前記一致度をＡとし、特徴（３）に記載の対象物の３次元位置・姿勢認識装置により求めた前記一致度をＢとするとき、
評価関数Ｆ^evaluation

ここで、ｐ，ｑ，ｓ，ｔは、任意の整数、を用いて、前記対象物の一致度を求めることで、基準面上の対象物の集合の状態に応じた最適な対象物の３次元・位置姿勢認識装置を構築することができる。

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

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

上記対象物の３次元位置・姿勢認識装置の技術思想は、３次元位置・姿勢認識方法（請求項６記載の発明）に適用することができる。

この発明によれば、基準面上に置かれた同一形状の複数の対象物の３次元位置と姿勢を正確かつ容易に認識することができる。

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

以下、この発明に係る対象物の３次元位置・姿勢認識方法が適用された対象物の３次元位置・姿勢認識装置の実施形態について図面を参照して説明する。

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

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

３次元位置・姿勢認識装置１２は、載置台２４上に積載された対象物２０の集合２１の３次元位置・姿勢データを取得する３次元形状取得部２６と、コンピュータ２８と、から構成される。

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

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

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

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

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

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

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

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

次に、基本的には以上のように構成される３次元位置・姿勢認識システム１０の動作について、図２のフローチャートに基づき説明する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次いで、ステップＳ５において、把持候補優先順位表１２４を参照して優先順位の高い対象物２０の３次元位置・姿勢データを通信インタフェース５２を通じてロボットコントローラ５４に送る。次に、ロボットコントローラ５４からの指令によりロボット１６は、マニプレータ１４で優先順位の高い対象物２０を把持し、所定の位置に移動する。

なお、対象物２０がｚ方向に重なっていない場合には、上記の把持候補優先順位表１２４の順位は、例えば、ロボット１６が最も短い時間で全ての対象物２０を所定の位置に移動できる順位とする。また、対象物２０がｚ方向に重なっている場合には、一般には、ｚ方向の高さが対象物２０の１個の最大高さ以上の高さを有する最も高い位置にある対象物２０を第１優先順位とし、この第１優先順位の対象物２０を把持・移動した後、再度ステップＳ２〜Ｓ５を繰り返す。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上説明したように、上述した実施形態に係る対象物の３次元位置・姿勢認識装置１２は、基準面２２上に置かれた同一形状の複数の対象物２０の集合２１から各対象物２０の位置・姿勢をコンピュータ２８により認識する対象物２０の３次元位置・姿勢認識装置１２であり、対象物２０の集合の３次元位置・姿勢データを取得する３次元形状取得部２６と、対象物２０の３次元モデルの位置と姿勢を変化させる３次元モデル位置・姿勢変化部４６と、３次元形状取得部２６により得られた対象物２０の集合２１の３次元位置・姿勢データに対応するセンシングポリゴンデータ、あるいはセンシングエッジデータ（又はセンシングポリゴンデータ及びセンシングエッジデータ）と、位置と姿勢を変化させた３次元モデルの位置・姿勢データであるモデルポリゴンデータ、あるいはモデルエッジデータ（、又はモデルポリゴンデータ及びモデルエッジデータ）とを比較し、対象物２０の集合２１から、ポリゴン｛この実施形態では４辺形（４角形）であるが３辺形（３角形）でもよい。｝からなる３次元モデル又はエッジからなる３次元モデルの位置・姿勢に一致度の高い対象物２０を決定する比較部４８と、を備える。

このように処理することで、３次元モデルの位置・姿勢に一致度の高い各対象物２０を正確に決定することができる。

この結果、基準面２２上に、例えば、ばら積みされた同一形状の対象物２０の３次元位置・姿勢を正確・容易かつ短時間に認識することができる。

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

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

Claims (3)

1. 基準面上に置かれた同一形状の複数の対象物の集合から各対象物の位置・姿勢をコンピュータにより認識する対象物の３次元位置・姿勢認識装置において、
   撮像方向から前記基準面を撮像することで、前記対象物の集合の３次元形状データを取得する３次元形状取得部と、
   前記対象物の形状に対応する３次元モデルの位置・姿勢を変化させた状態を示す位置・姿勢データを取得する３次元モデル位置・姿勢変化部と、
   前記３次元モデル位置・姿勢変化部により取得された前記位置・姿勢データと、前記３次元形状取得部により取得された前記３次元形状データとを、前記３次元モデルとの位置・姿勢の一致度を定量化した評価関数に基づいて比較し、前記対象物の集合の中から前記対象物を１つずつ認識する比較部と、を備え、
   前記３次元形状データ及び前記位置・姿勢データをそれぞれ３次元ポリゴンで表現する場合に前記撮像方向の位置が一致するポリゴンの数の割合をＡとし、
   前記３次元形状データの輪郭を構成する辺素及び前記位置・姿勢データの輪郭を構成する辺素の間にて位置・姿勢が一致する部分領域の数の総和をＢとするとき、
   前記評価関数は、指数が整数であるＡの累乗と、指数が整数であるＢの累乗との積の一次結合で表される
   ことを特徴とする対象物の３次元位置・姿勢認識装置。
2. 請求項１記載の対象物の３次元位置・姿勢認識装置において、
   前記対象物の集合から把持するマニプレータをさらに設け、該マニプレータにより、前記一致度の大きい前記対象物を前記対象物の集合から把持して取り去ったのち、前記３次元形状取得部により前記対象物を取り去ったエリア部分のみの３次元形状データを得、既に取得してある前記対象物の集合の全体の前記３次元形状データに貼り付ける
   ことを特徴とする対象物の３次元位置・姿勢認識装置。
3. 基準面上に置かれた同一形状の複数の対象物の集合から各対象物の位置・姿勢をコンピュータにより認識する対象物の３次元位置・姿勢認識方法において、
   撮像方向から前記基準面を撮像することで、前記対象物の集合の３次元形状データを取得するステップと、
   前記対象物の形状に対応する３次元モデルの位置・姿勢を変化させた状態を示す位置・姿勢データを取得するステップと、
   それぞれ取得された前記位置・姿勢データと前記３次元形状データとを、前記３次元モデルとの位置・姿勢の一致度を定量化した評価関数に基づいて比較し、前記対象物の集合の中から前記対象物を１つずつ認識するステップと、を備え、
   前記３次元形状データ及び前記位置・姿勢データをそれぞれ３次元ポリゴンで表現する場合に前記撮像方向の位置が一致するポリゴンの数の割合をＡとし、
   前記３次元形状データの輪郭を構成する辺素及び前記位置・姿勢データの輪郭を構成する辺素の間にて位置・姿勢が一致する部分領域の数の総和をＢとするとき、
   前記評価関数は、指数が整数であるＡの累乗と、指数が整数であるＢの累乗との積の一次結合で表される
   ことを特徴とする対象物の３次元位置・姿勢認識方法。

Images