対象物把持装置、対象物把持装置の制御方法、およびプログラム
【課題】対象物の姿勢を推定して把持を行う処理を高速化すると共に、把持動作の失敗確率を低減する。
【解決手段】対象物の姿勢を姿勢推定パラメータに基づいて推定する推定部と、推定部により推定された対象物の姿勢に基づいて対象物を把持する把持部と、把持部による把持の失敗を検知する検知部と、検知部により把持の失敗が検知された際の対象物の姿勢に基づいて姿勢推定パラメータを修正する修正部と、を備える。
【解決手段】対象物の姿勢を姿勢推定パラメータに基づいて推定する推定部と、推定部により推定された対象物の姿勢に基づいて対象物を把持する把持部と、把持部による把持の失敗を検知する検知部と、検知部により把持の失敗が検知された際の対象物の姿勢に基づいて姿勢推定パラメータを修正する修正部と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物の姿勢を推定して把持を行う対象物把持装置、対象物把持装置の制御方法、およびプログラムに関する。
【背景技術】
【０００２】
山積みされた部品等の対象物の状態をカメラ等のセンサで撮像して情報を取得し、取得した画像等の情報から各対象物の位置や姿勢を推定して、ロボットに把持させて順次対象物を取り出す技術が広く開発されている。特許文献１および特許文献２は、対象物を把持する処理の高速化を実現する手法を開示している
特許文献１に開示される技術は、対象物（部品）の複数の特定部位が基準姿勢を取るときの形状を特定する複数の照合モデルを持ち、その照合優先順位を装置の使用時に実際に認識された回数の多い順とすることにより、認識処理の高速化を図っている。
【０００３】
特許文献２に開示される技術は、粗位置が認識された対象物（ワーク）の集合から、３次元位置の測定に失敗した対象物又は取出しに失敗した対象物を除外することにより処理の高速化を図ると共に、次に選択される対象物の把持失敗確率を低減している。
【０００４】
また、特許文献３に開示される技術は、６軸力角センサの加重の変化に基づき把持の失敗等の異常動作を検知し、その後のリカバリを行っている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３２２５７４０号公報
【特許文献２】特許第４２２６６２３号公報
【特許文献３】特開２０００−２６３４８１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
山積みされた対象物の集合から順次対象物を取り出すためには、個々の対象物の位置や姿勢を推定する必要がある。作業効率の面から、位置姿勢推定に要する時間を短縮することが求められている。
【０００７】
また、部品等の対象物について、ロボットのアームの先端に備えられたマニュプレータ（ハンド）で把持すべき部位は、通常、特定の部位に定められている。そのため、対象物の姿勢によっては特定部位にハンドがアクセスできずに把持に失敗する場合がある。また、対象物の姿勢によっては、センサより得られる情報が他の姿勢と似ている場合があり、誤った姿勢推定を行い易い場合もある。
【０００８】
特許文献１に開示される技術は、複数の特定部位が基準姿勢を取ったことを認識した回数を考慮して高速化を図っているが、誤った姿勢を認識したり把持に失敗したりした場合のことは考慮していない。
【０００９】
特許文献２に開示される技術は、精密な３次元位置の測定に失敗した場合や対象物の取出しに失敗した場合の粗位置を考慮して高速化を図っているが、失敗した場合の当該対象物の姿勢については考慮していない。
【００１０】
特許文献３に開示される技術は、把持動作の異常検知とその後のリカバリを、オペレータを呼ばずに行うもので、高速化を目的とするものではない。
【００１１】
すなわち従来技術には、対象物の把持に失敗した際の当該対象物の姿勢を考慮していないという課題がある。
【００１２】
上記の課題に鑑み、本発明は、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することを目的とする。
【課題を解決するための手段】
【００１３】
上記の目的を達成する本発明に係る対象物把持装置は、
対象物の姿勢を姿勢推定パラメータに基づいて推定する推定手段と、
前記推定手段により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持手段と、
前記把持手段による把持の失敗を検知する検知手段と、
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正手段と、
を備えることを特徴とする。
【発明の効果】
【００１４】
本発明によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【図面の簡単な説明】
【００１５】
【図１】対象物把持装置の構成を示す図。
【図２】測地ドームの考え方を説明する図。
【図３】対象物の代表姿勢の例を示す図。
【図４】（ａ）撮像装置で撮像された山積み状態の対象物の画像例を示す図、（ｂ）テンプレート・マッチングによる姿勢推定処理のためのウィンドウ・スキャンの説明図。
【図５】テンプレート・マッチングによる姿勢推定処理の手順を示すフローチャート。
【図６】対象物把持処理の手順を示すフローチャート。
【図７】ツリー型姿勢推定処理の処理構造を説明する図。
【図８】ツリー型姿勢推定処理における切り出し局所矩形領域のスキャンおよび投票処理を説明する図。
【図９】ツリー型姿勢推定処理の手順を示すフローチャート。
【図１０】ツリー型姿勢推定処理における失敗姿勢に対応する照合データの削除の様子を示す図。
【図１１】撮像装置で撮像される領域の分割の一例を示す図。
【図１２】失敗姿勢の周辺姿勢を説明する図。
【図１３】把持の失敗をユーザが対象物把持装置へ通知する機構を備えた対象物把持装置の構成を示す図。
【図１４】対象物の姿勢は変動するが位置が略一定である場合の対象物把持装置の一例を示す図。
【発明を実施するための形態】
【００１６】
（第１実施形態）
図１を参照して、第１実施形態に係る対象物把持装置の構成を説明する。対象物把持装置は、ロボット・アーム１０１と、ハンド機構１０２と、撮像装置１０３と、コントローラ１０６とを備える。
【００１７】
ハンド機構１０２は、ロボット・アーム１０１のエンドエフェクタとしてのハンド機構である。撮像装置１０３は、トレイ１０５上の部品１０４の状態を取得するセンサであり、山積み状態のトレイ１０５の画像を取得する。なお撮像装置１０３は、二次元輝度画像情報を得るための単眼カメラであってもよく、または距離情報を得るためのステレオカメラやＴＯＦセンサ、レーザーレンジファインダーのような測距センサであってもよく、本発明において限定されるものではない。部品１０４は、把持する対象物である部品の一例である。トレイ１０５は、複数の部品１０４が山積み状態で積載されたトレイである。
【００１８】
コントローラ１０６は、ロボット・アーム１０１、ハンド機構１０２、撮像装置１０３を制御する。コントローラ１０６は、カメラ制御部１０７と、位置姿勢推定処理部１０８と、姿勢推定辞書格納部１０９と、取出し部品設定部１１０と、ロボット制御部１１１と、把持失敗検知部１１２と、推定処理修正部１１３と、を備える。
【００１９】
カメラ制御部１０７は、撮像装置１０３を制御すると共に、撮像装置１０３が撮像したトレイ１０５の画像情報を取得する。位置姿勢推定処理部１０８は、カメラ制御部１０７により取得された画像情報を、姿勢推定辞書格納部１０９に格納されている辞書データを用いて分析処理し、画像中に含まれる部品１０４の位置および姿勢を推定する。姿勢推定辞書格納部１０９は、部品１０４の位置および姿勢を推定するための辞書データ等のパラメータを格納する。この姿勢推定パラメータには、姿勢別の照合優先順位や重み係数を含むようにしても良い。
【００２０】
取出し部品設定部１１０は、位置および姿勢を推定した多数の部品の中から、順次把持すべき部品を選択し、ハンド気候１０２により把持される部品上の被把持部の情報をロボット制御部１１１へ通知する。把持すべき部品の選択方法として、例えば最も上部にあって被把持部が隠されておらず把持するための制御が容易である部品や、位置姿勢推定処理部１０８における推定の信頼度が高い部品を選択する等の方法が考えられる。
【００２１】
ロボット制御部１１１は、取出し部品設定部１１０により選択された部品１０４の被把持部の情報に基づいて、部品１０４の把持動作および後処理動作を行うための命令を生成し、ロボット・アーム１０１およびハンド１０２を制御して動作させる。
【００２２】
把持失敗検知部１１２は、ハンド機構１０２に備えられた不図示の異常検知用センサの出力を、ロボット制御部１１１を介して受け取る。そして把持失敗検知部１１２は、当該センサの出力を分析することにより、部品１０４の把持に成功したか否かを判別する。異常検知用センサには、例えばハンド機構１０２に備えた不図示の力覚センサを用いてもよく、例えば特許文献３に開示される技術を用いて把持の失敗を検知することが可能である。尚、撮像装置１０３により撮像される画像を分析して把持の失敗を検知する構成であってもよい。例えば、撮像された画像中において、部品１０４が把持部から離れた位置にあることを認識して把持の失敗を検知してもよい。
【００２３】
推定処理修正部１１３は、把持失敗検知部１１２により把持の失敗が検知されると、その際の対象物（部品１０４）の姿勢に基づいて姿勢推定辞書格納部１０９に保持されている姿勢推定パラメータを修正する。
【００２４】
ここで、前述の位置姿勢推定処理部１０８は、対象物の位置を検出すると共に姿勢をクラス分けするクラス分類器である。各クラスに対応する姿勢を代表姿勢と呼び、測地ドームと面内回転を組み合わせて定義する。ここで測地ドームとは、正多面体の各頂点や面素の中心点、或いは、各三角形面素を再帰的に同一面積の三角形に分割してそれらの頂点を用いることで、球面を均一に離散化表現する公知の手法である。
【００２５】
図２（ａ）は、正二十面体を基本とする測地ドームの例を示している。正二十面体の中心点に、所定姿勢の対象物の重心点が一致するように配置し、例えば、各頂点と各三角形面素の中心点とを視点として観察した際の対象物の見え方を代表姿勢として定義とする。各視点からの視線は、対象物の中心点すなわち正二十面体の重心点で交差する。正二十面体においては頂点の数は１６、面素の数は２０であるので、正二十面体をそのまま用いた測地ドームでは、計３６の視点からの代表姿勢が定義可能である。
【００２６】
しかしながら、通常これだけの粒度では、把持を行うのに十分な精度とはならないため、本実施形態では、各面素の辺をさらに２分割している。すると正二十面体の各正三角形面素は、図２（ｂ）に示すように４つの同一面積の小正三角形で構成されることになる。ここで視点２０１は、元の正二十面体の頂点と面素中央点とで定義された計４点の視点を表しており、視点２０２は小三角形に分割することにより増えた計６点の視点を表している。面中心点の数は分割前の元の三角形面素の数（２０面）の４倍、頂点の数は元の正二十面体の辺の数分（＝３４本）増えることになるので、この分割により１６＋３４＋２０×４＝１３０通りの視点からの代表姿勢を定義することができる。
【００２７】
図２（ｃ）は、面内回転の例を示している。画像２０５は、図２（ａ）における視点２０４より取得した部品の画像であって、これを所定角度ずつ回転させることにより、図に示すような複数の代表姿勢を得ることができる。本実施形態においては、面内回転を１０度ずつで定義するものとする。すなわち、上述の１３０通りの各視点に対し３６通りの面内回転パターンが存在することになるので、１３０×３６＝４６８０通りの代表姿勢が定義される。
【００２８】
ここで視点とは撮像装置１０３の観察位置であるので、代表姿勢とは撮像装置１０３で取得される個々の対象物の画像のバリエーションに対応する。姿勢推定辞書格納部１０９は、予め部品の姿勢を変化させて撮像した全ての代表姿勢の画像データから、機械学習等の技術により生成した照合データを保持している。各代表姿勢の画像データの例を５つ、図３（ａ）−（ｅ）に示す。
【００２９】
本実施形態においては、位置姿勢推定処理部１０８は、テンプレート・マッチング処理により、撮像装置１０３を基準とする座標系であるカメラ座標系における各対象物の位置の推定処理と、当該対象物の代表姿勢のクラス分け処理とを行う。
【００３０】
従って、姿勢推定辞書格納部１０９には、４６８０通りの代表姿勢クラスに対応する照合データ（テンプレート）を全て保持している。しかしながら、各視点から０°回転の１３０通りのテンプレートのみを保持しておき、面内回転分については照合の都度、回転処理により生成するようにすることで、保持するデータ量を削減するようにする構成であってもよい。
【００３１】
ここで、カメラ座標系における対象物の基準点（物体中心等）の位置を、カメラ座標系の原点からの平行移動ベクトルPwとして表す。これが推定される対象物の位置に相当する。カメラ座標系から見た奥行き方向の位置は、検出された部品の画像中の大きさに基づいて推定してもよく、若しくは、レンジファインダーのような測距センサを用いてより正確に推定してもよい。
【００３２】
そして、いずれの代表姿勢クラスのテンプレートに合致したかに基づいて、検出された対象物の姿勢を推定する。ここで対象物の姿勢は、カメラ座標系における部品座標系３軸の方向ベクトルの組みによる姿勢行列ＥＷ＝［ｅＷＸ、ｅＷＹ、ｅＷＺ］で表すことができる。ｅＷＸ、ｅＷＹ、ｅＷＺは、部品の基準位置からそれぞれ３方向に直交して固定された、長さ１の単位列ベクトルである。すなわち、各代表姿勢には、それぞれ固有の姿勢行列ＥＷが対応付けられている。
【００３３】
また、ハンド機構１０２で把持すべき部品上の把持位置（被把持部）は、通常、特定の部位に定められている。この被把持部を正しく把持した状態は、ハンド機構１０２を基準とするハンド座標系における同様の対象物位置Phおよび姿勢行列Ｅｈにより、予め定義される。従って、ロボット制御部１１１は、把持しようとする部品のカメラ座標系における位置Pwおよび姿勢ＥＷを、ハンド座標系における位置Phおよび姿勢Ｅｈと一致させるように、ロボット・アーム１０１の各関節やハンド機構１０２に対する把持動作計画および制御を行う。
【００３４】
＜位置姿勢推定処理＞
ここで、図４（ａ）、図４（ｂ）および図５を参照して、位置姿勢推定処理部１０８で行われるテンプレート・マッチング処理による位置姿勢推定手法を説明する。図４（ａ）は、撮像装置１０３により撮像された画像データの一例である。この画像データには、トレイ１０５上に多数積載された山積み状態の部品１０４が撮像されている。
【００３５】
図４（ｂ）は、位置姿勢推定処理部１０８が、カメラ制御部１０７を介して送られてきた図４（ａ）の画像をスキャン処理して、テンプレート・マッチングによる位置姿勢推定処理を行う様子を示している。ここで、図４（ａ）の画像に相当する画像データ４００上に設定された、テンプレートと同サイズの切り出しウィンドウ４０１により画像全域（若しくは、部品の存在することが予め分かっている範囲）を、隈なくスキャンする。
【００３６】
切り出しウィンドウ４０１により切り出された矩形領域の部分画像は、各代表姿勢に対応する照合データ（すなわち、テンプレート）と相関演算され、当該位置に代表姿勢の部品が有るかどうかが判別される。ここで用いられるテンプレートは、各代表姿勢で所定の距離から撮像された（すなわち切り出しウィンドウ４０１と等しい大きさの）部品画像、若しくは部品画像に対し所定のエッジ抽出等の前処理を行って生成された画像データである。尚、所定の前処理を行ったテンプレートを用いる場合は、切り出し処理を行う前に、予め画像データ４００の全域に対して同じ前処理を掛けておく。
【００３７】
以下、図５のフローチャートを参照して、本実施形態に係る位置姿勢推定処理部１０８により行われる部品の位置姿勢推定処理の手順を説明する。
【００３８】
Ｓ５０１において、位置姿勢推定処理部１０８は、画像データ４００上に設定する切り出しウィンドウ４０１の所定の位置を初期位置とする。本実施形態では画像データ４００の全域を部品の存在する可能性のある領域としているので、画像左上を切り出しウィンドウ４０１の初期位置とする。
【００３９】
Ｓ５０２において、位置姿勢推定処理部１０８は、画像データ４００から現在の切り出しウィンドウ位置の矩形領域の部分画像を切り出して取得する。
【００４０】
Ｓ５０３おいて、位置姿勢推定処理部１０８は、姿勢推定辞書格納部１０９に格納されている４６８０通りの各代表姿勢に対応するテンプレートの中から、最初に任意の代表姿勢に対応するテンプレートを選択する。なお、ここでは４６８０通りのテンプレートの場合を例に挙げて説明するが、他の数のテンプレートを用いてもよい。
【００４１】
Ｓ５０４において、位置姿勢推定処理部１０８は、切り出された部分画像について、Ｓ５０３で選択された代表姿勢テンプレートとの相関値を算出する（相関値算出処理）。
【００４２】
Ｓ５０５において、位置姿勢推定処理部１０８は、Ｓ５０４で計算された相関値が所定値以上であるか否かを判別する。相関値が所定値以上であると判別された場合（Ｓ５０５；ＹＥＳ）、Ｓ５０６へ進む。一方、相関値が所定値より小さいと判別された場合（Ｓ５０５；ＮＯ）、Ｓ５０７へ進む。
【００４３】
Ｓ５０６において、位置姿勢推定処理部１０８は、現在の切り出しウィンドウ位置に、選択された代表姿勢の部品が存在するものと推定して、その位置と代表姿勢とを位置姿勢推定結果リストに保存する。その後、Ｓ５０７へ進む。
【００４４】
Ｓ５０７において、位置姿勢推定処理部１０８は、全ての代表姿勢のテンプレートを用いた照合処理が完了したか否かを判定する。全ての照合処理が完了したと判定された場合（Ｓ５０７；ＹＥＳ）、Ｓ５０９へ進む。一方、全ての照合処理が完了していないと判定された場合（Ｓ５０７；ＮＯ）、Ｓ５０８へ進む。
【００４５】
Ｓ５０８において、位置姿勢推定処理部１０８は、姿勢推定辞書格納部１０９に格納されている４６８０通りの各代表姿勢に対応するテンプレートの中から、次の任意の代表姿勢に対応するテンプレートを選択する。その後、Ｓ５０４へ戻る。
【００４６】
Ｓ５０９において、位置姿勢推定処理部１０８は、切り出しウィンドウ４０１のスキャンが最後の位置まで行われたか否かを判定する。切り出しウィンドウ４０１が最後の位置まで移動したと判定された場合（Ｓ５０９；ＹＥＳ）、Ｓ５１１へ進む。一方、切り出しウィンドウ４０１が最後の位置まで移動していないと判定された場合（Ｓ５０９；ＮＯ）、Ｓ５１０へ進む。
【００４７】
Ｓ５１０において、位置姿勢推定処理部１０８は、全ての姿勢別テンプレートに対する処理が完了したので、切り出しウィンドウ４０１を次の位置に移動させる。その後、Ｓ５０２へ戻る。
【００４８】
なお、このときのスキャンのルールは、図４（ｂ）に示すように、右方向に１画素ずつ切り出しウィンドウ４０１をずらして行き、画像４００の右端に到達したら、１画素下の列の左端に移動し、以後、画像４００の右下端まで同様の処理を繰り返すというものである。ここで、処理を高速化するために、一度に移動する画素数を１画素ではなく数画素とすることも、もちろん可能であるが、その場合は位置検出精度が数画素単位となる。
【００４９】
また、上述の処理の説明では、分かりやすくするために省略しているが、実際には、画像を所定範囲で変倍させるか、若しくはテンプレートを変倍させることにより、各々の代表姿勢について多少サイズの異なった部品の検出も同時に行っている。そして検出された部品のサイズに基づき、上下左右の二次元方向の位置だけでなく、カメラから見た奥行き方向の位置推定も行う。もちろん測距センサを用いて、より正確に奥行き方向の位置を検出するようにしても構わない。
【００５０】
Ｓ５１１において、位置姿勢推定処理部１０８は、位置姿勢推定結果リストに保存されている複数の位置姿勢データを統合する。一般にこのような位置姿勢推定処理においては、部品の存在する位置や姿勢そのものだけでなく、その近傍位置や類似姿勢の相関値も高くなる傾向があるため、統合処理を行う必要がある。本発明では、統合処理の手法は問わないので、例えば、相関値の高い領域や姿勢範囲をまとめてそれらの平均を取るといった、既知の手法を用いることができる。もちろんこのとき、相関値の高い姿勢を優先するようにしても良い。
【００５１】
以上により図５のフローチャートの処理が終了し、画像データ４００中に含まれる複数の部品の位置及び姿勢が推定される。
【００５２】
＜辞書更新付の把持処理＞
次に図６のフローチャートを参照して、本実施形態に係る把持処理の手順を説明する。把持動作の失敗は、部品の山積み状態とハンド機構系、把持動作計画・制御系、位置姿勢推定処理の各要素の誤差要因等の様々な要因が複雑に絡んで生じる。本実施形態では、これらの要因を区別することなく把持動作の失敗の発生に基づいて姿勢推定辞書格納部１０９に格納されている照合データを修正することにより、位置姿勢推定処理部１０８で行われる推定処理を修正する。これにより、姿勢推定処理は、徐々に失敗確率の高い姿勢を推定しなくなり高速化してゆくと共に、推定された姿勢の部品は把持失敗が起こりにくいものとなっていく。
【００５３】
Ｓ６０１において、カメラ制御部１０７は、図４（ｂ）で示した画像データ４００のような、山積み状態の多数の部品１０４を撮像した画像データを取得する。この撮像は前述のように、カメラ制御部１０７の制御により撮像装置１０３を用いて行われる。
【００５４】
Ｓ６０２において、位置姿勢推定処理部１０８は、取得された画像データに含まれる部品の位置および姿勢を推定する処理を行う。本実施形態において、この処理は、上述した通り図５のフローチャートにおける処理である。これにより、複数の部品の位置姿勢推定結果の一覧を得ることができる。
【００５５】
Ｓ６０３においては、位置姿勢推定処理部１０８は、位置および姿勢が推定できた部品があるか否かを判定する。位置および姿勢が推定できた部品があると判定された場合（Ｓ６０３；ＹＥＳ）、Ｓ６０４へ進む。一方、位置および姿勢が推定できた部品がないと判定された場合（Ｓ６０３；ＮＯ）、処理は完了となり、例えば次のトレイが送られてくるまで、把持装置は停止する。
【００５６】
Ｓ６０４において、取出し部品設定部１１０は、位置および姿勢が推定できた部品のうちの一つを選択する。取出し部品設定部１１０は、上述したように、例えば最も上部にある（カメラに近い）部品が選択されてもよく、或いは位置姿勢推定結果の信頼度の高い部品が選択されてもよい。
【００５７】
Ｓ６０５において、ロボット制御部１１１は、Ｓ６０４で選択された部品の位置姿勢情報に基づいて把持動作計画を立てて、ロボット・アーム１０１およびハンド機構１０２を制御し、部品の把持動作を行う。
【００５８】
Ｓ６０６において、把持失敗検知部１１２は、把持動作が成功したか否かを判定する。把持動作が成功したと判定された場合（Ｓ６０６；ＹＥＳ）、Ｓ６０７へ進む。一方、把持動作が失敗したと判定された場合（Ｓ６０６；ＮＯ）、Ｓ６０８へ進む。
【００５９】
Ｓ６０７において、ロボット制御部１１１は、所定の後工程（例えば、組み付けや再配置）を行うべく、ロボット・アーム１０１とハンド機構１０２とを制御する。そして後工程が完了したらＳ６０３に戻り、位置姿勢推定できた部品がまだ残っていれば、同様の処理を繰り返す。
【００６０】
Ｓ６０８において、推定処理修正部１１３は、当該把持に失敗した部品に対して推定された姿勢を確認する。そして、その姿勢に対する過去の把持成功・失敗履歴から、姿勢別把持失敗確率を算出する。ここで失敗履歴としては、多様な山積み状態からの履歴データを取ってもよい。また、推定処理修正部１１３は、周囲の部品の位置・姿勢に応じた当該部品の推定姿勢に対する把持失敗確率を算出するようにしてもよい。
【００６１】
Ｓ６０９において、推定処理修正部１１３は、算出された姿勢別把持失敗確率が所定の基準値を超えたか否かを判定する。姿勢別把持失敗確率が所定の基準値を超えたと判定された場合（Ｓ６０９；ＹＥＳ）、Ｓ６１０へ進む。一方、姿勢別把持失敗確率が所定の基準値以下であると判定された場合（Ｓ６０９；ＮＯ）、Ｓ６０１へ戻る。
【００６２】
Ｓ６１０において、推定処理修正部１１３は、当該姿勢は失敗しやすいものであると判断して、姿勢推定辞書格納部１０９に格納されている当該姿勢に対応する照合データを削除（若しくは無効化）する。その後、トレイ１０５の山積み状態は、ハンド機構１０２が接触したことにより変化してしまっていると考えられるため、Ｓ６０１に戻って撮像からやり直す。
【００６３】
Ｓ６０２における位置及び姿勢推定処理は、照合データが失敗確率に基づいてオンラインで減少していくため、徐々に高速化されていく。また、失敗し易い姿勢の部品はそもそも検出されなくなっていくため、失敗自体が起こりにくくなり、全体の把持処理が高速化される。
【００６４】
尚、把持が失敗し易くなる要因は、いくつか考えられる。例えば姿勢推定は正しくできるものの、被把持部が下向きになるか或いは他の部品に埋もれやすくなる等、その姿勢自体が被把持部にアクセスし難いものである場合、あるいは、姿勢推定を誤りやすい姿勢である場合、等が考えられる。いずれの場合も本発明では区別せずに対応可能である。
【００６５】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【００６６】
（第２実施形態）
＜ツリー型位置姿勢推定器の説明＞
本発明は、第１実施形態とは異なる位置姿勢推定処理方式に対しても適用することが可能である。本実施形態では、一例として、ツリー型位置姿勢推定処理を用いる場合について説明する。
【００６７】
なお、本実施形態において、対象物把持装置の構成は図１と同様であり、位置姿勢推定処理は、位置姿勢推定処理部１０８により行われる。また、このとき用いる辞書情報は同様に推定辞書保持部１０９に保持されている。さらに、把持処理の基本的な流れは図６のフローチャートと同様であるので、これらについてはその差分のみを説明する。また、代表姿勢についても第１実施形態と同様４６８０通りである。
【００６８】
図７（ａ）および図７（ｂ）を参照して、本実施形態で用いられるツリー型位置姿勢推定処理の処理構造の一例を説明する。図７（ａ）において、画像７００は、辞書情報を作成するための代表姿勢の画像の一つである。局所領域７０１は、代表姿勢画像７００の特徴的な個所に設定された局所領域であり、代表姿勢毎に１０個所ずつの局所領域７０１が設定されている。なお、各代表姿勢で局所領域７０１が設定される位置は異なっている。代表姿勢は上述の通り４６８０個存在するので、局所領域は４６８００通り存在する。各局所領域７０１は、当該代表姿勢における二次元的な物体中心７０２との相対位置が対応付けられている。本実施形態では説明を簡単にするため、局所領域は全て同一サイズであるものとするが、実際にはそれぞれ異なったサイズであっても構わない。
【００６９】
図７（ｂ）は、本実施形態のツリー型位置姿勢推定処理で用いられるツリー型クラス分類器の構造を示している。このツリーは二分木を組み合わせた構造をしている。各ノード７０４は、例えば矩形画像内の所定の２点の大小関係や、所定領域内画素値合計が閾値以上であるか否か等の、簡便な特徴判別処理を行い、入力された矩形画像が、二分木の右のグループに属するか左のグループに属するかを判定する。各ノード７０４に対応する辞書データとして特徴判別処理パラメータが保持されている。
【００７０】
ツリーの最終段であるリーフ７０５の数は４６８００個であり、それぞれ上述の４６８００通りの局所領域クラスに対応する。各リーフ７０５に対応する辞書データには、クラス判別された局所領域が４６８０通りの代表姿勢種別のいずれに属するかの情報と、対応する物体中心７０２との相対位置を示す情報が保持されている。
【００７１】
図８を参照して、ツリー型位置姿勢推定処理における切り出し局所矩形のスキャンと、リーフ７０５まで辿り着きクラス分類された後に行う投票処理の様子を説明する。図８（ａ）の画像データ８００は、撮像装置１０３により取得された画像データを示す。切り出し局所矩形８０１は、画像データ８００上の初期位置に設定された、局所領域と同サイズの切り出し局所矩形である。図４の切り出しウィンドウ４０１のスキャンと同様、切り出し局所矩形８０１は、画像全域（若しくは、部品の存在することが予め分かっている範囲）を、隈なくスキャンする。
【００７２】
切り出し局所矩形８０１により切り出された矩形画像は、図７（ｂ）の構造のツリー型クラス分類器に入力され、最終的にリーフ７０５に辿り着くことで、４６８００個のいずれの局所領域に最も近いかがクラス判別される。
【００７３】
ここで、４６８０個の各代表姿勢には、それぞれ異なった投票平面が用意されている。投票平面とは、処理中の撮像画像全域を、対象物の位置を特定したい粒度でグリッド分割し、各マスに１ビンを割り当てて、二次元ヒストグラムとしたものである。
【００７４】
位置姿勢推定処理部１０８は、切り出し局所矩形８０１が分類されたリーフ７０５に対応する辞書情報を参照し、クラス判別された局所領域が属する代表姿勢に対応する投票平面を選択する。そして、投票平面の物体中心７０２の位置に相当するビンに投票する。
【００７５】
図８（ｂ）、（ｃ）および（ｄ）は、投票平面の例を示している。図８（ｂ）は、図３（ｅ）に相当する代表姿勢の投票平面を示す。図８（ｃ）は、同一視点からの見えを右に１０°回転させた代表姿勢に対応する投票平面を、図８（ｄ）は、同一視点からの見えを右に２０°回転させた代表姿勢に対応する投票平面をそれぞれ示している。各黒点は、物体中心７０２として投票された位置を表す。画像データ８００には、位置８０２で示される位置に図８（ｃ）の代表姿勢に近い姿勢の部品が存在する。そのため、図８（ｃ）の投票平面において位置８０２に相当するビン８０３には、複数の局所領域のクラス判別結果に基づく投票が集中している。一方で、画像中に存在しない姿勢の投票平面には、たまたま誤ってクラス判別された結果に基づく投票結果が、例えばビン８０４のように単発的に存在するのみである。本実施形態においては、各代表姿勢に対応する局所領域は１０個ずつであるので、各ビンの値を例えば５で閾値処理することにより、残ったビンの位置に当該投票平面に対応する代表姿勢の部品が存在するものと推定することができる。もちろん１０に近い値であるほど、その推定信頼度は高いものと判定できる。
【００７６】
次に図９のフローチャートを参照して、本実施形態に係るツリー型位置姿勢推定器における位置姿勢推定処理の手順を説明する。
【００７７】
Ｓ９０１において、位置姿勢推定処理部１０８は、全ての代表姿勢に対応する４６８０個の投票平面の全ビンを０にクリアすることにより初期化する。また、位置姿勢推定処理部１０８は、切り出し局所矩形の位置を、図８（ａ）の切り出し局所矩形８０１に示すように、画像データ８００の左上の位置（若しくは、部品の存在することが予め分かっている範囲の先頭位置）に初期化する。
【００７８】
Ｓ９０２において、位置姿勢推定処理部１０８は、切り出し局所矩形の位置の画像を切り出す。そして切り出した局所矩形画像をツリーに投入する処理が以下実行される。Ｓ９０３において、位置姿勢推定処理部１０８は、図７（ｂ）に示したツリーの先頭ノードを選択する。
【００７９】
Ｓ９０４において、位置姿勢推定処理部１０８は、Ｓ９０３で選択されたノードに対応する辞書情報の特徴判別パラメータを読み込み、入力された矩形画像の特徴を分析して、次に選択すべきノードがツリー上における右か左かを判別する。
【００８０】
Ｓ９０５において、位置姿勢推定処理部１０８は、次ノードが終端であるか否かすなわち次ノードがリーフまで辿り着いたか否かを判別する。リーフまで辿り着いた、とはすなわち、入力された局所矩形画像が、４６８００通りのいずれの局所領域であるかがクラス判別されたことに相当する。次ノードがリーフまで辿り着いたと判定された場合（Ｓ９０５；ＹＥＳ）、Ｓ９０７へ進む。一方、次ノードがリーフまで辿り着いていないと判定された場合（Ｓ９０５；ＮＯ）、Ｓ９０６へ進む。
【００８１】
Ｓ９０６において、位置姿勢推定処理部１０８は、次ノードを選択して、Ｓ９０４へ戻って同様の処理を繰り返す。Ｓ９０７において、位置姿勢推定処理部１０８は、当該リーフの局所領域が属する代表姿勢の種別と投票先の相対位置情報を読み込む。
【００８２】
Ｓ９０８において、位置姿勢推定処理部１０８は、代表姿勢種別に対応する投票平面を選択する。さらに位置姿勢推定処理部１０８は、投票先相対位置より、現在の切り出し局所矩形の位置に基づいて、投票平面上の投票すべきビンを選択し、インクリメントする。
【００８３】
Ｓ９０９において、位置姿勢推定処理部１０８は、切り出し矩形位置が画像の終端（若しくは、部品の存在することが予め分かっている範囲の終端）まで到達したか否かを判定する。切り出し矩形位置が画像の終端まで到達したと判定された場合（Ｓ９０９；ＹＥＳ）、Ｓ９１１へ進む。切り出し矩形位置が画像の終端まで到達していないと判定された場合（Ｓ９０９；ＹＥＳ）、Ｓ９１０へ進む。
【００８４】
Ｓ９１０において、位置姿勢推定処理部１０８は、切り出し局所矩形位置を次の位置へ移動させる。このスキャン方式は、図４で説明した第１実施形態の切り出しウィンドウの移動と同様である。
【００８５】
Ｓ９０９において、画像終端までのスキャンおよび各局所矩形画像の分類に基づく投票処理が一通り完了した場合、Ｓ９１１からの投票結果を確認する処理へと移る。Ｓ９１１において、位置姿勢推定処理部１０８は、最初の代表姿勢を選択する。
【００８６】
Ｓ９１２において、位置姿勢推定処理部１０８は、Ｓ９１１で選択された代表姿勢に対応する投票平面の各ビンの値をチェックし、所定の閾値以上のビンのみを残す処理を行う。
【００８７】
Ｓ９１３において、位置姿勢推定処理部１０８は、残ったビンの位置から、対応する入力画像中の位置を計算し、選択された代表姿勢種別の情報と共に、それを位置姿勢推定結果リストに保存する。
【００８８】
Ｓ９１４において、位置姿勢推定処理部１０８は、最後の代表姿勢までの処理が完了したか否かを判定する。最後の代表姿勢までの処理が完了したと判定された場合（Ｓ９１４；ＹＥＳ）、Ｓ９１６へ進む。最後の代表姿勢までの投票平面の確認処理が完了すると、位置姿勢推定結果リストには、入力画像中に存在すると推定された各代表姿勢の部品の位置が、一覧となって保存されている。一方、最後の代表姿勢までの処理が完了していないと判定された場合（Ｓ９１４；ＮＯ）、Ｓ９１５へ進む。
【００８９】
Ｓ９１５において、位置姿勢推定処理部１０８は、次の代表姿勢を選択して、Ｓ９１２へ戻り、同様の処理を繰り返す。Ｓ９１６においては、位置姿勢推定処理部１０８は、Ｓ５１１と同様に、これらの位置姿勢データを統合する処理を行う。以上で処理が終了する。
【００９０】
なお、第１実施形態と同様に、分かり易くするためにフローチャートからは省略しているが、実際には部品の大きさ、若しくは測距センサの出力に基づき、奥行き方向の位置推定も同時に行っている。
【００９１】
＜ツリー型の場合の辞書削除処理＞
本実施形態においても、図６のフローチャートのＳ６０８における姿勢別把持失敗確率の算出処理と、Ｓ６０９における基準値を超えたか否かの判別処理とは、第１実施形態と同様に実行される。Ｓ６１０における推定処理の修正処理は、以下に説明するようなノードやリーフの削除処理となる。
【００９２】
図１０は、図７（ｂ）のツリー型クラス分類器の一部を示している。ここで×印のリーフは、リーフの局所領域クラスが属する代表姿勢の姿勢別把持失敗確率が所定値以上となって、対応する辞書情報が削除されたことを表す。
【００９３】
ある一つの代表姿勢が削除されると、当該姿勢に属する局所領域クラスは１０個あるので、１０個のリーフの辞書情報が削除されるが、これらは必ずしもツリー上で隣接する位置に存在する訳ではない。従って、図１０のリーフ１００１の部分以外にも×印が付けられたリーフがあるように、通常は連続しない削除状態となる。
【００９４】
いくつかの把持失敗確率の高い代表姿勢が削除された結果、リーフ１００１のように一部の連続するリーフが削除されると、分岐先の無くなった上位のノードも削除可能となる。
【００９５】
一つのリーフが削除されると、代表姿勢の種別を示す情報と投票先相対位置を示す情報とが削減される。一つのノードが削除されると、特徴抽出のための情報と分岐先のノードの情報とが削除される。これによって徐々に辞書サイズは低減していくことになり、メモリ占有容量が低減される。メモリ占有容量が下がれば、大容量メモリと高速メモリのスワップ操作等の頻度が下がり、推定処理が高速化する可能性が高くなる。
【００９６】
また図９のＳ９０４において、分岐先として判別されたノードやリーフが削除されている場合、位置姿勢推定処理部１０８は、処理中の矩形画像に対する処理をそこで打ち切ってもよい。つまり、Ｓ９０７およびＳ９０８の投票処理はスキップして、Ｓ９０９の処理を行ってもよい。打ち切られた時点で矩形画像は失敗確率が高い姿勢のものであることが確定するので、投票を行わなくとも問題なく、従って推定処理が高速化する。
【００９７】
また、削除された代表姿勢については、姿勢推定する必要がないのであるから、投票平面を持つ必要もなくなるので、この点でもメモリ占有率を下げることが可能となる。
【００９８】
失敗姿勢に基づく推定処理の修正処理が進むにつれて、把持の失敗自体が起こりにくくなり、全体の把持処理フローが徐々に高速化されていくのは、第１実施形態と同様である。
【００９９】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【０１００】
（第３実施形態）
＜優先順位変更による修正＞
上述の実施形態では、推定処理の修正処理として、失敗確率の高い代表姿勢に係る照合データを削除する例を説明したが、本発明の修正処理はこれに限るものではない。例えば、失敗確率の高い代表姿勢に係る当該照合データの照合優先順位を低くするという修正を行ってもよい。すなわち、失敗確率の高い代表姿勢に係る当該照合データの照合順序を後にする。第１実施形態で用いたようなテンプレート・マッチングタイプの位置姿勢推定処理で、切り出しウィンドウのスキャン位置の数が、姿勢別テンプレートの数よりも少ない場合には、このような優先順位の変更による修正が有効である。
【０１０１】
この場合、図５のフローチャートにおいて、スキャン処理のループ（Ｓ５０１→Ｓ５０９、Ｓ５１０）と、姿勢別テンプレート選択のループ（Ｓ５０３→Ｓ５０７、Ｓ５０８）の順序を入れ替える。さらに、Ｓ５０３、Ｓ５０８で選択されるテンプレートは、姿勢別優先順位の順とする。そして、Ｓ５０５において相関値が所定値以上となり、選択中の代表姿勢の部品が検出された時点で、同位置に関する処理は打ち切り、次のスキャン位置に移動する。
【０１０２】
このようにすることで、失敗確率の高い姿勢が推定される優先順位は徐々に下がっていくので、結果として把持の失敗頻度が下がり、把持処理フローが効率化するという効果がある。
【０１０３】
＜重み付け変更による修正＞
或いは、各代表姿勢に対応する重み係数を設定し、姿勢別把持失敗確率に基づき、この重み係数を修正するようにすることも可能である。例えば図５において、Ｓ５０４で算出される相関値は姿勢別のスコアであると見なせる。或いは、図９において、Ｓ９１２の閾値処理で残ったビンの投票値はやはり姿勢別スコアであると見なせる。
【０１０４】
そして、Ｓ５０６或いはＳ９１３において、姿勢別のスコアに対して対応する重み係数を乗じた値を、最終的な姿勢別の信頼度スコアとし、位置・姿勢と合わせて保存するようにする（信頼度算出処理）。或いは、図９のような投票処理を伴う姿勢推定処理では、重み係数を乗ずる代わりに、Ｓ９０８における投票処理で選択されたビンにインクリメントする値として、この重み係数を用いるようにすることもできる。この場合はビンの値がそのまま姿勢別の重みを考慮した信頼度スコアとなる。
【０１０５】
ここで保存された姿勢別の信頼度スコアは、Ｓ５１１或いはＳ９１６の統合処理で参照されることにより、重み係数の大きい代表姿勢であるほど、優先的に統合結果として残りやすくなるように作用する。
【０１０６】
このような代表姿勢別の重み係数を用いる場合、図６のＳ６１０で行われる姿勢推定処理の修正処理は、次のようになる。まず代表姿勢θに対応する重み係数をＷθとする。システムが稼働を開始した時点でのＷθの初期値はＷθ＝Ｗθ0とする。Ｗθ0は、全姿勢において共通とするか、若しくは事前に分かっている部品の形状に基づく把持のし易さや姿勢の起こり易さを加味して設定しても良い。そしてＳ６０８で算出される姿勢別失敗確率をＰθとする。失敗が起こりにくい姿勢であるほど失敗確率Ｐθは０に近い値となるので、Ｓ６１０においては、例えばＷθ＝（１−Ｐθ）Ｗθ0として、重み係数Ｗθを更新する。ここで、Ｓ６０９を省略して、姿勢別失敗が基準値を超えたかどうかを判別すること無しに、失敗を検知した場合には毎回更新するようにしても構わない。
【０１０７】
或いは、各姿勢に近い周辺姿勢からの影響を加味することもできる。例えば、各代表姿勢のＰθに対して、測地ドーム上の視点間の距離や面内回転角度に応じて減衰するガウシアンカーネルにより、離散的なコンボリューション演算を行って計算した値を用いて、Ｗθを修正してもよい。
【０１０８】
このように重み係数を修正する場合も、やはり失敗確率の高い姿勢は徐々に検出されにくくなっていき、把持処理フローは効率化する効果がある。
【０１０９】
＜分割領域別確率＞
また、上述の実施形態では、入力画像全域に渡って同一の位置姿勢推定処理を行う例を示したが、入力画像を領域分割して、それぞれの領域で異なった位置姿勢推定処理を行うようにすることも可能である。
【０１１０】
図１１は、対象物把持装置が図１において不図示の領域分割部をさらに備え、撮像装置１０３により撮像される画像の領域を、ＣとＮ，Ｓ，Ｅ，Ｗの５つの領域に分割する例を示している。なお、Ｎ，Ｓ，Ｅ，Ｗのそれぞれは、東西南北に対応するここで中心部Ｃの領域以外の４つの領域は、それぞれトレイ１０５のいずれかの壁面に近いため、把持動作を行う際には、いずれかの壁面の影響を受けやすい。従って各領域で失敗しやすい代表姿勢は、少なくとも一部は異なったものとなる可能性が高い。
【０１１１】
そこで、各領域に対する位置姿勢推定処理を行う際に、異なった辞書データを用いるようにする。但し、これら５つの辞書データは初期状態では同じものとする。そして、図６のＳ６０８で算出する姿勢別把持失敗確率は、分割した領域別の姿勢別把持失敗確率とし、Ｓ６１０ではそれぞれの領域に対応する辞書データを修正する。
【０１１２】
その他、領域分割は、部品が存在し得る空間を三次元的に分割して、上記と同様に辞書データの修正を行っても良い。このようにすることで、把持の失敗確率を下げつつも、失敗確率が高い姿勢として除外される部品の数が減り、トレイ上のより多くの部品を把持することが可能となる。
【０１１３】
＜周辺姿勢も考慮＞
また、上述の実施形態では、失敗した際の代表姿勢に対する修正のみを行う例を示してきたが、失敗姿勢に近い周辺姿勢に関する修正も同時に行うようにすることも可能である。ここで周辺姿勢の例を図１２に示す。視点１２０１を、失敗した代表姿勢を定義する測地ドーム状の視点とする場合、隣接する視点１２０２乃至視点１２０６の各視点で定義される代表姿勢が周辺姿勢となる。或いは、隣接視点に限らず、失敗した代表姿勢の視点を中心として、測地ドーム状で所定の距離範囲内に存在する視点としてもよい。面内回転についても同様に、把持に失敗した姿勢の面内回転角度を中心として、所定範囲内を周辺姿勢としてもよい。これら周辺姿勢の範囲を失敗確率の空間分布に基づいて決定してもよい。
【０１１４】
推定処理の修正処理は、周辺姿勢をまとめて削除するものとしても良いし、上述した重み係数による修正を失敗姿勢からの距離に応じて行う等、失敗姿勢との類似性に基づいて修正の影響度を変えるようにしてもよい。或いは失敗確率を算出する際には、周辺姿勢の失敗確率も同時にいくらか上げるというようにすることもできる。
【０１１５】
一般に、把持に失敗し易い姿勢の周辺姿勢は、やはり失敗し易い確率が高いと考えられるため、このような周辺姿勢も含めた修正とすることにより、より効率的に姿勢推定辞書を更新することが可能となる。
【０１１６】
＜ユーザによる把持失敗通知＞
また、上述の実施形態では、把持の失敗の検知を、ハンド機構１０２に備えた不図示の力覚センサの出力に基づき行うものとして説明したが、本発明はこれに限るものではない。例えばユーザが認知した把持の失敗の通知を受け付けるようにすることももちろん可能である（通知受付処理）。
【０１１７】
例えば、図１３は、オペレータによりロボットの動作が監視されることを想定して、緊急停止ボタン１３０１をロボット制御部１１１に接続した対象物把持装置を示している。オペレータは、部品１０４の把持動作が失敗した際には、緊急停止ボタン１３０１を押してロボットを停止させる。把持失敗検知部１１２は、ロボット制御部１１１を介して緊急停止を知ることにより、把持の失敗を検知することができる。
【０１１８】
＜姿勢推定のみの場合＞
また、上述の実施形態では、対象物の位置および姿勢の両方を推定する例を示してきたが、本発明は、位置推定が必要無く、姿勢の推定処理のみを行う場合にも適用できる。図１４は、対象物の位置は毎回ほぼ一定である例を示している。
【０１１９】
コンベア１４０１は、対象物である様々な姿勢であり得る部品１０４を、図１４において下方向に運搬する。下方向に運搬された部品１０４は、ガイド機構１４０２に接触してガイドされ、ある程度向きが揃えられた状態で、把持位置１４０３までさらに運搬される。把持位置１４０３に一つの部品１０４が入ると不図示の遮光センサ等によりそのことが検知され、コンベア４０１は一旦停止する。
【０１２０】
そして把持位置１４０３における部品１０４の姿勢推定処理が行われる。この処理には、基本的には上述の実施形態の位置姿勢推定処理とほぼ同じ処理を適用できるが、部品の存在する位置はほぼ決まっているので、１個所についてのみ、若しくは極めて限定された範囲のスキャンのみで、姿勢推定処理を行えば良い。また、ガイド機構１４０２により、部品の姿勢はある程度揃えられているので、最初からより少ない代表姿勢の照合データの姿勢推定辞書を用いることが可能である。
【０１２１】
姿勢推定結果に基づき、ロボット・アーム１０１およびハンド機構１０２による把持処理を行う。把持の失敗を検知した際は、上述の実施形態と同様に推定処理の修正を行う。把持動作および後処理が完了したら、コンベア１４０１による部品１０４の運搬を再開する。以後これを繰り返す。
【０１２２】
把持失敗時、若しくは失敗確率の高い姿勢であるため姿勢が推定されなくなった場合は、把持位置１４０３の部品は、そのままコンベア１４０１で図の下方に運搬され、再利用篭１４０４に投入される。再利用篭１４０４に投入された部品は、オペレータ等により再度コンベア１４０１に投入される。
【０１２３】
上述の実施形態では、対象物は一種類であるものとして説明しているが、複数種類の対象物に対応することももちろん可能である。この場合、代表姿勢に対応する照合データを、複数の対象物それぞれについて保持するようにし、これらを同時に使用するようにすれば良い。
【０１２４】
以上、本発明の好ましい実施形態について詳述したが、本発明は係る特定の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内において、種々の変形・変更が可能である。例えば、生産用ロボット・システムに限らず、家庭用ロボット等の種々のロボット・システムに対しても適用可能であることは言うまでもない。
【０１２５】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【０１２６】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、対象物の姿勢を推定して把持を行う対象物把持装置、対象物把持装置の制御方法、およびプログラムに関する。
【背景技術】
【０００２】
山積みされた部品等の対象物の状態をカメラ等のセンサで撮像して情報を取得し、取得した画像等の情報から各対象物の位置や姿勢を推定して、ロボットに把持させて順次対象物を取り出す技術が広く開発されている。特許文献１および特許文献２は、対象物を把持する処理の高速化を実現する手法を開示している
特許文献１に開示される技術は、対象物（部品）の複数の特定部位が基準姿勢を取るときの形状を特定する複数の照合モデルを持ち、その照合優先順位を装置の使用時に実際に認識された回数の多い順とすることにより、認識処理の高速化を図っている。
【０００３】
特許文献２に開示される技術は、粗位置が認識された対象物（ワーク）の集合から、３次元位置の測定に失敗した対象物又は取出しに失敗した対象物を除外することにより処理の高速化を図ると共に、次に選択される対象物の把持失敗確率を低減している。
【０００４】
また、特許文献３に開示される技術は、６軸力角センサの加重の変化に基づき把持の失敗等の異常動作を検知し、その後のリカバリを行っている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３２２５７４０号公報
【特許文献２】特許第４２２６６２３号公報
【特許文献３】特開２０００−２６３４８１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
山積みされた対象物の集合から順次対象物を取り出すためには、個々の対象物の位置や姿勢を推定する必要がある。作業効率の面から、位置姿勢推定に要する時間を短縮することが求められている。
【０００７】
また、部品等の対象物について、ロボットのアームの先端に備えられたマニュプレータ（ハンド）で把持すべき部位は、通常、特定の部位に定められている。そのため、対象物の姿勢によっては特定部位にハンドがアクセスできずに把持に失敗する場合がある。また、対象物の姿勢によっては、センサより得られる情報が他の姿勢と似ている場合があり、誤った姿勢推定を行い易い場合もある。
【０００８】
特許文献１に開示される技術は、複数の特定部位が基準姿勢を取ったことを認識した回数を考慮して高速化を図っているが、誤った姿勢を認識したり把持に失敗したりした場合のことは考慮していない。
【０００９】
特許文献２に開示される技術は、精密な３次元位置の測定に失敗した場合や対象物の取出しに失敗した場合の粗位置を考慮して高速化を図っているが、失敗した場合の当該対象物の姿勢については考慮していない。
【００１０】
特許文献３に開示される技術は、把持動作の異常検知とその後のリカバリを、オペレータを呼ばずに行うもので、高速化を目的とするものではない。
【００１１】
すなわち従来技術には、対象物の把持に失敗した際の当該対象物の姿勢を考慮していないという課題がある。
【００１２】
上記の課題に鑑み、本発明は、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することを目的とする。
【課題を解決するための手段】
【００１３】
上記の目的を達成する本発明に係る対象物把持装置は、
対象物の姿勢を姿勢推定パラメータに基づいて推定する推定手段と、
前記推定手段により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持手段と、
前記把持手段による把持の失敗を検知する検知手段と、
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正手段と、
を備えることを特徴とする。
【発明の効果】
【００１４】
本発明によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【図面の簡単な説明】
【００１５】
【図１】対象物把持装置の構成を示す図。
【図２】測地ドームの考え方を説明する図。
【図３】対象物の代表姿勢の例を示す図。
【図４】（ａ）撮像装置で撮像された山積み状態の対象物の画像例を示す図、（ｂ）テンプレート・マッチングによる姿勢推定処理のためのウィンドウ・スキャンの説明図。
【図５】テンプレート・マッチングによる姿勢推定処理の手順を示すフローチャート。
【図６】対象物把持処理の手順を示すフローチャート。
【図７】ツリー型姿勢推定処理の処理構造を説明する図。
【図８】ツリー型姿勢推定処理における切り出し局所矩形領域のスキャンおよび投票処理を説明する図。
【図９】ツリー型姿勢推定処理の手順を示すフローチャート。
【図１０】ツリー型姿勢推定処理における失敗姿勢に対応する照合データの削除の様子を示す図。
【図１１】撮像装置で撮像される領域の分割の一例を示す図。
【図１２】失敗姿勢の周辺姿勢を説明する図。
【図１３】把持の失敗をユーザが対象物把持装置へ通知する機構を備えた対象物把持装置の構成を示す図。
【図１４】対象物の姿勢は変動するが位置が略一定である場合の対象物把持装置の一例を示す図。
【発明を実施するための形態】
【００１６】
（第１実施形態）
図１を参照して、第１実施形態に係る対象物把持装置の構成を説明する。対象物把持装置は、ロボット・アーム１０１と、ハンド機構１０２と、撮像装置１０３と、コントローラ１０６とを備える。
【００１７】
ハンド機構１０２は、ロボット・アーム１０１のエンドエフェクタとしてのハンド機構である。撮像装置１０３は、トレイ１０５上の部品１０４の状態を取得するセンサであり、山積み状態のトレイ１０５の画像を取得する。なお撮像装置１０３は、二次元輝度画像情報を得るための単眼カメラであってもよく、または距離情報を得るためのステレオカメラやＴＯＦセンサ、レーザーレンジファインダーのような測距センサであってもよく、本発明において限定されるものではない。部品１０４は、把持する対象物である部品の一例である。トレイ１０５は、複数の部品１０４が山積み状態で積載されたトレイである。
【００１８】
コントローラ１０６は、ロボット・アーム１０１、ハンド機構１０２、撮像装置１０３を制御する。コントローラ１０６は、カメラ制御部１０７と、位置姿勢推定処理部１０８と、姿勢推定辞書格納部１０９と、取出し部品設定部１１０と、ロボット制御部１１１と、把持失敗検知部１１２と、推定処理修正部１１３と、を備える。
【００１９】
カメラ制御部１０７は、撮像装置１０３を制御すると共に、撮像装置１０３が撮像したトレイ１０５の画像情報を取得する。位置姿勢推定処理部１０８は、カメラ制御部１０７により取得された画像情報を、姿勢推定辞書格納部１０９に格納されている辞書データを用いて分析処理し、画像中に含まれる部品１０４の位置および姿勢を推定する。姿勢推定辞書格納部１０９は、部品１０４の位置および姿勢を推定するための辞書データ等のパラメータを格納する。この姿勢推定パラメータには、姿勢別の照合優先順位や重み係数を含むようにしても良い。
【００２０】
取出し部品設定部１１０は、位置および姿勢を推定した多数の部品の中から、順次把持すべき部品を選択し、ハンド気候１０２により把持される部品上の被把持部の情報をロボット制御部１１１へ通知する。把持すべき部品の選択方法として、例えば最も上部にあって被把持部が隠されておらず把持するための制御が容易である部品や、位置姿勢推定処理部１０８における推定の信頼度が高い部品を選択する等の方法が考えられる。
【００２１】
ロボット制御部１１１は、取出し部品設定部１１０により選択された部品１０４の被把持部の情報に基づいて、部品１０４の把持動作および後処理動作を行うための命令を生成し、ロボット・アーム１０１およびハンド１０２を制御して動作させる。
【００２２】
把持失敗検知部１１２は、ハンド機構１０２に備えられた不図示の異常検知用センサの出力を、ロボット制御部１１１を介して受け取る。そして把持失敗検知部１１２は、当該センサの出力を分析することにより、部品１０４の把持に成功したか否かを判別する。異常検知用センサには、例えばハンド機構１０２に備えた不図示の力覚センサを用いてもよく、例えば特許文献３に開示される技術を用いて把持の失敗を検知することが可能である。尚、撮像装置１０３により撮像される画像を分析して把持の失敗を検知する構成であってもよい。例えば、撮像された画像中において、部品１０４が把持部から離れた位置にあることを認識して把持の失敗を検知してもよい。
【００２３】
推定処理修正部１１３は、把持失敗検知部１１２により把持の失敗が検知されると、その際の対象物（部品１０４）の姿勢に基づいて姿勢推定辞書格納部１０９に保持されている姿勢推定パラメータを修正する。
【００２４】
ここで、前述の位置姿勢推定処理部１０８は、対象物の位置を検出すると共に姿勢をクラス分けするクラス分類器である。各クラスに対応する姿勢を代表姿勢と呼び、測地ドームと面内回転を組み合わせて定義する。ここで測地ドームとは、正多面体の各頂点や面素の中心点、或いは、各三角形面素を再帰的に同一面積の三角形に分割してそれらの頂点を用いることで、球面を均一に離散化表現する公知の手法である。
【００２５】
図２（ａ）は、正二十面体を基本とする測地ドームの例を示している。正二十面体の中心点に、所定姿勢の対象物の重心点が一致するように配置し、例えば、各頂点と各三角形面素の中心点とを視点として観察した際の対象物の見え方を代表姿勢として定義とする。各視点からの視線は、対象物の中心点すなわち正二十面体の重心点で交差する。正二十面体においては頂点の数は１６、面素の数は２０であるので、正二十面体をそのまま用いた測地ドームでは、計３６の視点からの代表姿勢が定義可能である。
【００２６】
しかしながら、通常これだけの粒度では、把持を行うのに十分な精度とはならないため、本実施形態では、各面素の辺をさらに２分割している。すると正二十面体の各正三角形面素は、図２（ｂ）に示すように４つの同一面積の小正三角形で構成されることになる。ここで視点２０１は、元の正二十面体の頂点と面素中央点とで定義された計４点の視点を表しており、視点２０２は小三角形に分割することにより増えた計６点の視点を表している。面中心点の数は分割前の元の三角形面素の数（２０面）の４倍、頂点の数は元の正二十面体の辺の数分（＝３４本）増えることになるので、この分割により１６＋３４＋２０×４＝１３０通りの視点からの代表姿勢を定義することができる。
【００２７】
図２（ｃ）は、面内回転の例を示している。画像２０５は、図２（ａ）における視点２０４より取得した部品の画像であって、これを所定角度ずつ回転させることにより、図に示すような複数の代表姿勢を得ることができる。本実施形態においては、面内回転を１０度ずつで定義するものとする。すなわち、上述の１３０通りの各視点に対し３６通りの面内回転パターンが存在することになるので、１３０×３６＝４６８０通りの代表姿勢が定義される。
【００２８】
ここで視点とは撮像装置１０３の観察位置であるので、代表姿勢とは撮像装置１０３で取得される個々の対象物の画像のバリエーションに対応する。姿勢推定辞書格納部１０９は、予め部品の姿勢を変化させて撮像した全ての代表姿勢の画像データから、機械学習等の技術により生成した照合データを保持している。各代表姿勢の画像データの例を５つ、図３（ａ）−（ｅ）に示す。
【００２９】
本実施形態においては、位置姿勢推定処理部１０８は、テンプレート・マッチング処理により、撮像装置１０３を基準とする座標系であるカメラ座標系における各対象物の位置の推定処理と、当該対象物の代表姿勢のクラス分け処理とを行う。
【００３０】
従って、姿勢推定辞書格納部１０９には、４６８０通りの代表姿勢クラスに対応する照合データ（テンプレート）を全て保持している。しかしながら、各視点から０°回転の１３０通りのテンプレートのみを保持しておき、面内回転分については照合の都度、回転処理により生成するようにすることで、保持するデータ量を削減するようにする構成であってもよい。
【００３１】
ここで、カメラ座標系における対象物の基準点（物体中心等）の位置を、カメラ座標系の原点からの平行移動ベクトルPwとして表す。これが推定される対象物の位置に相当する。カメラ座標系から見た奥行き方向の位置は、検出された部品の画像中の大きさに基づいて推定してもよく、若しくは、レンジファインダーのような測距センサを用いてより正確に推定してもよい。
【００３２】
そして、いずれの代表姿勢クラスのテンプレートに合致したかに基づいて、検出された対象物の姿勢を推定する。ここで対象物の姿勢は、カメラ座標系における部品座標系３軸の方向ベクトルの組みによる姿勢行列ＥＷ＝［ｅＷＸ、ｅＷＹ、ｅＷＺ］で表すことができる。ｅＷＸ、ｅＷＹ、ｅＷＺは、部品の基準位置からそれぞれ３方向に直交して固定された、長さ１の単位列ベクトルである。すなわち、各代表姿勢には、それぞれ固有の姿勢行列ＥＷが対応付けられている。
【００３３】
また、ハンド機構１０２で把持すべき部品上の把持位置（被把持部）は、通常、特定の部位に定められている。この被把持部を正しく把持した状態は、ハンド機構１０２を基準とするハンド座標系における同様の対象物位置Phおよび姿勢行列Ｅｈにより、予め定義される。従って、ロボット制御部１１１は、把持しようとする部品のカメラ座標系における位置Pwおよび姿勢ＥＷを、ハンド座標系における位置Phおよび姿勢Ｅｈと一致させるように、ロボット・アーム１０１の各関節やハンド機構１０２に対する把持動作計画および制御を行う。
【００３４】
＜位置姿勢推定処理＞
ここで、図４（ａ）、図４（ｂ）および図５を参照して、位置姿勢推定処理部１０８で行われるテンプレート・マッチング処理による位置姿勢推定手法を説明する。図４（ａ）は、撮像装置１０３により撮像された画像データの一例である。この画像データには、トレイ１０５上に多数積載された山積み状態の部品１０４が撮像されている。
【００３５】
図４（ｂ）は、位置姿勢推定処理部１０８が、カメラ制御部１０７を介して送られてきた図４（ａ）の画像をスキャン処理して、テンプレート・マッチングによる位置姿勢推定処理を行う様子を示している。ここで、図４（ａ）の画像に相当する画像データ４００上に設定された、テンプレートと同サイズの切り出しウィンドウ４０１により画像全域（若しくは、部品の存在することが予め分かっている範囲）を、隈なくスキャンする。
【００３６】
切り出しウィンドウ４０１により切り出された矩形領域の部分画像は、各代表姿勢に対応する照合データ（すなわち、テンプレート）と相関演算され、当該位置に代表姿勢の部品が有るかどうかが判別される。ここで用いられるテンプレートは、各代表姿勢で所定の距離から撮像された（すなわち切り出しウィンドウ４０１と等しい大きさの）部品画像、若しくは部品画像に対し所定のエッジ抽出等の前処理を行って生成された画像データである。尚、所定の前処理を行ったテンプレートを用いる場合は、切り出し処理を行う前に、予め画像データ４００の全域に対して同じ前処理を掛けておく。
【００３７】
以下、図５のフローチャートを参照して、本実施形態に係る位置姿勢推定処理部１０８により行われる部品の位置姿勢推定処理の手順を説明する。
【００３８】
Ｓ５０１において、位置姿勢推定処理部１０８は、画像データ４００上に設定する切り出しウィンドウ４０１の所定の位置を初期位置とする。本実施形態では画像データ４００の全域を部品の存在する可能性のある領域としているので、画像左上を切り出しウィンドウ４０１の初期位置とする。
【００３９】
Ｓ５０２において、位置姿勢推定処理部１０８は、画像データ４００から現在の切り出しウィンドウ位置の矩形領域の部分画像を切り出して取得する。
【００４０】
Ｓ５０３おいて、位置姿勢推定処理部１０８は、姿勢推定辞書格納部１０９に格納されている４６８０通りの各代表姿勢に対応するテンプレートの中から、最初に任意の代表姿勢に対応するテンプレートを選択する。なお、ここでは４６８０通りのテンプレートの場合を例に挙げて説明するが、他の数のテンプレートを用いてもよい。
【００４１】
Ｓ５０４において、位置姿勢推定処理部１０８は、切り出された部分画像について、Ｓ５０３で選択された代表姿勢テンプレートとの相関値を算出する（相関値算出処理）。
【００４２】
Ｓ５０５において、位置姿勢推定処理部１０８は、Ｓ５０４で計算された相関値が所定値以上であるか否かを判別する。相関値が所定値以上であると判別された場合（Ｓ５０５；ＹＥＳ）、Ｓ５０６へ進む。一方、相関値が所定値より小さいと判別された場合（Ｓ５０５；ＮＯ）、Ｓ５０７へ進む。
【００４３】
Ｓ５０６において、位置姿勢推定処理部１０８は、現在の切り出しウィンドウ位置に、選択された代表姿勢の部品が存在するものと推定して、その位置と代表姿勢とを位置姿勢推定結果リストに保存する。その後、Ｓ５０７へ進む。
【００４４】
Ｓ５０７において、位置姿勢推定処理部１０８は、全ての代表姿勢のテンプレートを用いた照合処理が完了したか否かを判定する。全ての照合処理が完了したと判定された場合（Ｓ５０７；ＹＥＳ）、Ｓ５０９へ進む。一方、全ての照合処理が完了していないと判定された場合（Ｓ５０７；ＮＯ）、Ｓ５０８へ進む。
【００４５】
Ｓ５０８において、位置姿勢推定処理部１０８は、姿勢推定辞書格納部１０９に格納されている４６８０通りの各代表姿勢に対応するテンプレートの中から、次の任意の代表姿勢に対応するテンプレートを選択する。その後、Ｓ５０４へ戻る。
【００４６】
Ｓ５０９において、位置姿勢推定処理部１０８は、切り出しウィンドウ４０１のスキャンが最後の位置まで行われたか否かを判定する。切り出しウィンドウ４０１が最後の位置まで移動したと判定された場合（Ｓ５０９；ＹＥＳ）、Ｓ５１１へ進む。一方、切り出しウィンドウ４０１が最後の位置まで移動していないと判定された場合（Ｓ５０９；ＮＯ）、Ｓ５１０へ進む。
【００４７】
Ｓ５１０において、位置姿勢推定処理部１０８は、全ての姿勢別テンプレートに対する処理が完了したので、切り出しウィンドウ４０１を次の位置に移動させる。その後、Ｓ５０２へ戻る。
【００４８】
なお、このときのスキャンのルールは、図４（ｂ）に示すように、右方向に１画素ずつ切り出しウィンドウ４０１をずらして行き、画像４００の右端に到達したら、１画素下の列の左端に移動し、以後、画像４００の右下端まで同様の処理を繰り返すというものである。ここで、処理を高速化するために、一度に移動する画素数を１画素ではなく数画素とすることも、もちろん可能であるが、その場合は位置検出精度が数画素単位となる。
【００４９】
また、上述の処理の説明では、分かりやすくするために省略しているが、実際には、画像を所定範囲で変倍させるか、若しくはテンプレートを変倍させることにより、各々の代表姿勢について多少サイズの異なった部品の検出も同時に行っている。そして検出された部品のサイズに基づき、上下左右の二次元方向の位置だけでなく、カメラから見た奥行き方向の位置推定も行う。もちろん測距センサを用いて、より正確に奥行き方向の位置を検出するようにしても構わない。
【００５０】
Ｓ５１１において、位置姿勢推定処理部１０８は、位置姿勢推定結果リストに保存されている複数の位置姿勢データを統合する。一般にこのような位置姿勢推定処理においては、部品の存在する位置や姿勢そのものだけでなく、その近傍位置や類似姿勢の相関値も高くなる傾向があるため、統合処理を行う必要がある。本発明では、統合処理の手法は問わないので、例えば、相関値の高い領域や姿勢範囲をまとめてそれらの平均を取るといった、既知の手法を用いることができる。もちろんこのとき、相関値の高い姿勢を優先するようにしても良い。
【００５１】
以上により図５のフローチャートの処理が終了し、画像データ４００中に含まれる複数の部品の位置及び姿勢が推定される。
【００５２】
＜辞書更新付の把持処理＞
次に図６のフローチャートを参照して、本実施形態に係る把持処理の手順を説明する。把持動作の失敗は、部品の山積み状態とハンド機構系、把持動作計画・制御系、位置姿勢推定処理の各要素の誤差要因等の様々な要因が複雑に絡んで生じる。本実施形態では、これらの要因を区別することなく把持動作の失敗の発生に基づいて姿勢推定辞書格納部１０９に格納されている照合データを修正することにより、位置姿勢推定処理部１０８で行われる推定処理を修正する。これにより、姿勢推定処理は、徐々に失敗確率の高い姿勢を推定しなくなり高速化してゆくと共に、推定された姿勢の部品は把持失敗が起こりにくいものとなっていく。
【００５３】
Ｓ６０１において、カメラ制御部１０７は、図４（ｂ）で示した画像データ４００のような、山積み状態の多数の部品１０４を撮像した画像データを取得する。この撮像は前述のように、カメラ制御部１０７の制御により撮像装置１０３を用いて行われる。
【００５４】
Ｓ６０２において、位置姿勢推定処理部１０８は、取得された画像データに含まれる部品の位置および姿勢を推定する処理を行う。本実施形態において、この処理は、上述した通り図５のフローチャートにおける処理である。これにより、複数の部品の位置姿勢推定結果の一覧を得ることができる。
【００５５】
Ｓ６０３においては、位置姿勢推定処理部１０８は、位置および姿勢が推定できた部品があるか否かを判定する。位置および姿勢が推定できた部品があると判定された場合（Ｓ６０３；ＹＥＳ）、Ｓ６０４へ進む。一方、位置および姿勢が推定できた部品がないと判定された場合（Ｓ６０３；ＮＯ）、処理は完了となり、例えば次のトレイが送られてくるまで、把持装置は停止する。
【００５６】
Ｓ６０４において、取出し部品設定部１１０は、位置および姿勢が推定できた部品のうちの一つを選択する。取出し部品設定部１１０は、上述したように、例えば最も上部にある（カメラに近い）部品が選択されてもよく、或いは位置姿勢推定結果の信頼度の高い部品が選択されてもよい。
【００５７】
Ｓ６０５において、ロボット制御部１１１は、Ｓ６０４で選択された部品の位置姿勢情報に基づいて把持動作計画を立てて、ロボット・アーム１０１およびハンド機構１０２を制御し、部品の把持動作を行う。
【００５８】
Ｓ６０６において、把持失敗検知部１１２は、把持動作が成功したか否かを判定する。把持動作が成功したと判定された場合（Ｓ６０６；ＹＥＳ）、Ｓ６０７へ進む。一方、把持動作が失敗したと判定された場合（Ｓ６０６；ＮＯ）、Ｓ６０８へ進む。
【００５９】
Ｓ６０７において、ロボット制御部１１１は、所定の後工程（例えば、組み付けや再配置）を行うべく、ロボット・アーム１０１とハンド機構１０２とを制御する。そして後工程が完了したらＳ６０３に戻り、位置姿勢推定できた部品がまだ残っていれば、同様の処理を繰り返す。
【００６０】
Ｓ６０８において、推定処理修正部１１３は、当該把持に失敗した部品に対して推定された姿勢を確認する。そして、その姿勢に対する過去の把持成功・失敗履歴から、姿勢別把持失敗確率を算出する。ここで失敗履歴としては、多様な山積み状態からの履歴データを取ってもよい。また、推定処理修正部１１３は、周囲の部品の位置・姿勢に応じた当該部品の推定姿勢に対する把持失敗確率を算出するようにしてもよい。
【００６１】
Ｓ６０９において、推定処理修正部１１３は、算出された姿勢別把持失敗確率が所定の基準値を超えたか否かを判定する。姿勢別把持失敗確率が所定の基準値を超えたと判定された場合（Ｓ６０９；ＹＥＳ）、Ｓ６１０へ進む。一方、姿勢別把持失敗確率が所定の基準値以下であると判定された場合（Ｓ６０９；ＮＯ）、Ｓ６０１へ戻る。
【００６２】
Ｓ６１０において、推定処理修正部１１３は、当該姿勢は失敗しやすいものであると判断して、姿勢推定辞書格納部１０９に格納されている当該姿勢に対応する照合データを削除（若しくは無効化）する。その後、トレイ１０５の山積み状態は、ハンド機構１０２が接触したことにより変化してしまっていると考えられるため、Ｓ６０１に戻って撮像からやり直す。
【００６３】
Ｓ６０２における位置及び姿勢推定処理は、照合データが失敗確率に基づいてオンラインで減少していくため、徐々に高速化されていく。また、失敗し易い姿勢の部品はそもそも検出されなくなっていくため、失敗自体が起こりにくくなり、全体の把持処理が高速化される。
【００６４】
尚、把持が失敗し易くなる要因は、いくつか考えられる。例えば姿勢推定は正しくできるものの、被把持部が下向きになるか或いは他の部品に埋もれやすくなる等、その姿勢自体が被把持部にアクセスし難いものである場合、あるいは、姿勢推定を誤りやすい姿勢である場合、等が考えられる。いずれの場合も本発明では区別せずに対応可能である。
【００６５】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【００６６】
（第２実施形態）
＜ツリー型位置姿勢推定器の説明＞
本発明は、第１実施形態とは異なる位置姿勢推定処理方式に対しても適用することが可能である。本実施形態では、一例として、ツリー型位置姿勢推定処理を用いる場合について説明する。
【００６７】
なお、本実施形態において、対象物把持装置の構成は図１と同様であり、位置姿勢推定処理は、位置姿勢推定処理部１０８により行われる。また、このとき用いる辞書情報は同様に推定辞書保持部１０９に保持されている。さらに、把持処理の基本的な流れは図６のフローチャートと同様であるので、これらについてはその差分のみを説明する。また、代表姿勢についても第１実施形態と同様４６８０通りである。
【００６８】
図７（ａ）および図７（ｂ）を参照して、本実施形態で用いられるツリー型位置姿勢推定処理の処理構造の一例を説明する。図７（ａ）において、画像７００は、辞書情報を作成するための代表姿勢の画像の一つである。局所領域７０１は、代表姿勢画像７００の特徴的な個所に設定された局所領域であり、代表姿勢毎に１０個所ずつの局所領域７０１が設定されている。なお、各代表姿勢で局所領域７０１が設定される位置は異なっている。代表姿勢は上述の通り４６８０個存在するので、局所領域は４６８００通り存在する。各局所領域７０１は、当該代表姿勢における二次元的な物体中心７０２との相対位置が対応付けられている。本実施形態では説明を簡単にするため、局所領域は全て同一サイズであるものとするが、実際にはそれぞれ異なったサイズであっても構わない。
【００６９】
図７（ｂ）は、本実施形態のツリー型位置姿勢推定処理で用いられるツリー型クラス分類器の構造を示している。このツリーは二分木を組み合わせた構造をしている。各ノード７０４は、例えば矩形画像内の所定の２点の大小関係や、所定領域内画素値合計が閾値以上であるか否か等の、簡便な特徴判別処理を行い、入力された矩形画像が、二分木の右のグループに属するか左のグループに属するかを判定する。各ノード７０４に対応する辞書データとして特徴判別処理パラメータが保持されている。
【００７０】
ツリーの最終段であるリーフ７０５の数は４６８００個であり、それぞれ上述の４６８００通りの局所領域クラスに対応する。各リーフ７０５に対応する辞書データには、クラス判別された局所領域が４６８０通りの代表姿勢種別のいずれに属するかの情報と、対応する物体中心７０２との相対位置を示す情報が保持されている。
【００７１】
図８を参照して、ツリー型位置姿勢推定処理における切り出し局所矩形のスキャンと、リーフ７０５まで辿り着きクラス分類された後に行う投票処理の様子を説明する。図８（ａ）の画像データ８００は、撮像装置１０３により取得された画像データを示す。切り出し局所矩形８０１は、画像データ８００上の初期位置に設定された、局所領域と同サイズの切り出し局所矩形である。図４の切り出しウィンドウ４０１のスキャンと同様、切り出し局所矩形８０１は、画像全域（若しくは、部品の存在することが予め分かっている範囲）を、隈なくスキャンする。
【００７２】
切り出し局所矩形８０１により切り出された矩形画像は、図７（ｂ）の構造のツリー型クラス分類器に入力され、最終的にリーフ７０５に辿り着くことで、４６８００個のいずれの局所領域に最も近いかがクラス判別される。
【００７３】
ここで、４６８０個の各代表姿勢には、それぞれ異なった投票平面が用意されている。投票平面とは、処理中の撮像画像全域を、対象物の位置を特定したい粒度でグリッド分割し、各マスに１ビンを割り当てて、二次元ヒストグラムとしたものである。
【００７４】
位置姿勢推定処理部１０８は、切り出し局所矩形８０１が分類されたリーフ７０５に対応する辞書情報を参照し、クラス判別された局所領域が属する代表姿勢に対応する投票平面を選択する。そして、投票平面の物体中心７０２の位置に相当するビンに投票する。
【００７５】
図８（ｂ）、（ｃ）および（ｄ）は、投票平面の例を示している。図８（ｂ）は、図３（ｅ）に相当する代表姿勢の投票平面を示す。図８（ｃ）は、同一視点からの見えを右に１０°回転させた代表姿勢に対応する投票平面を、図８（ｄ）は、同一視点からの見えを右に２０°回転させた代表姿勢に対応する投票平面をそれぞれ示している。各黒点は、物体中心７０２として投票された位置を表す。画像データ８００には、位置８０２で示される位置に図８（ｃ）の代表姿勢に近い姿勢の部品が存在する。そのため、図８（ｃ）の投票平面において位置８０２に相当するビン８０３には、複数の局所領域のクラス判別結果に基づく投票が集中している。一方で、画像中に存在しない姿勢の投票平面には、たまたま誤ってクラス判別された結果に基づく投票結果が、例えばビン８０４のように単発的に存在するのみである。本実施形態においては、各代表姿勢に対応する局所領域は１０個ずつであるので、各ビンの値を例えば５で閾値処理することにより、残ったビンの位置に当該投票平面に対応する代表姿勢の部品が存在するものと推定することができる。もちろん１０に近い値であるほど、その推定信頼度は高いものと判定できる。
【００７６】
次に図９のフローチャートを参照して、本実施形態に係るツリー型位置姿勢推定器における位置姿勢推定処理の手順を説明する。
【００７７】
Ｓ９０１において、位置姿勢推定処理部１０８は、全ての代表姿勢に対応する４６８０個の投票平面の全ビンを０にクリアすることにより初期化する。また、位置姿勢推定処理部１０８は、切り出し局所矩形の位置を、図８（ａ）の切り出し局所矩形８０１に示すように、画像データ８００の左上の位置（若しくは、部品の存在することが予め分かっている範囲の先頭位置）に初期化する。
【００７８】
Ｓ９０２において、位置姿勢推定処理部１０８は、切り出し局所矩形の位置の画像を切り出す。そして切り出した局所矩形画像をツリーに投入する処理が以下実行される。Ｓ９０３において、位置姿勢推定処理部１０８は、図７（ｂ）に示したツリーの先頭ノードを選択する。
【００７９】
Ｓ９０４において、位置姿勢推定処理部１０８は、Ｓ９０３で選択されたノードに対応する辞書情報の特徴判別パラメータを読み込み、入力された矩形画像の特徴を分析して、次に選択すべきノードがツリー上における右か左かを判別する。
【００８０】
Ｓ９０５において、位置姿勢推定処理部１０８は、次ノードが終端であるか否かすなわち次ノードがリーフまで辿り着いたか否かを判別する。リーフまで辿り着いた、とはすなわち、入力された局所矩形画像が、４６８００通りのいずれの局所領域であるかがクラス判別されたことに相当する。次ノードがリーフまで辿り着いたと判定された場合（Ｓ９０５；ＹＥＳ）、Ｓ９０７へ進む。一方、次ノードがリーフまで辿り着いていないと判定された場合（Ｓ９０５；ＮＯ）、Ｓ９０６へ進む。
【００８１】
Ｓ９０６において、位置姿勢推定処理部１０８は、次ノードを選択して、Ｓ９０４へ戻って同様の処理を繰り返す。Ｓ９０７において、位置姿勢推定処理部１０８は、当該リーフの局所領域が属する代表姿勢の種別と投票先の相対位置情報を読み込む。
【００８２】
Ｓ９０８において、位置姿勢推定処理部１０８は、代表姿勢種別に対応する投票平面を選択する。さらに位置姿勢推定処理部１０８は、投票先相対位置より、現在の切り出し局所矩形の位置に基づいて、投票平面上の投票すべきビンを選択し、インクリメントする。
【００８３】
Ｓ９０９において、位置姿勢推定処理部１０８は、切り出し矩形位置が画像の終端（若しくは、部品の存在することが予め分かっている範囲の終端）まで到達したか否かを判定する。切り出し矩形位置が画像の終端まで到達したと判定された場合（Ｓ９０９；ＹＥＳ）、Ｓ９１１へ進む。切り出し矩形位置が画像の終端まで到達していないと判定された場合（Ｓ９０９；ＹＥＳ）、Ｓ９１０へ進む。
【００８４】
Ｓ９１０において、位置姿勢推定処理部１０８は、切り出し局所矩形位置を次の位置へ移動させる。このスキャン方式は、図４で説明した第１実施形態の切り出しウィンドウの移動と同様である。
【００８５】
Ｓ９０９において、画像終端までのスキャンおよび各局所矩形画像の分類に基づく投票処理が一通り完了した場合、Ｓ９１１からの投票結果を確認する処理へと移る。Ｓ９１１において、位置姿勢推定処理部１０８は、最初の代表姿勢を選択する。
【００８６】
Ｓ９１２において、位置姿勢推定処理部１０８は、Ｓ９１１で選択された代表姿勢に対応する投票平面の各ビンの値をチェックし、所定の閾値以上のビンのみを残す処理を行う。
【００８７】
Ｓ９１３において、位置姿勢推定処理部１０８は、残ったビンの位置から、対応する入力画像中の位置を計算し、選択された代表姿勢種別の情報と共に、それを位置姿勢推定結果リストに保存する。
【００８８】
Ｓ９１４において、位置姿勢推定処理部１０８は、最後の代表姿勢までの処理が完了したか否かを判定する。最後の代表姿勢までの処理が完了したと判定された場合（Ｓ９１４；ＹＥＳ）、Ｓ９１６へ進む。最後の代表姿勢までの投票平面の確認処理が完了すると、位置姿勢推定結果リストには、入力画像中に存在すると推定された各代表姿勢の部品の位置が、一覧となって保存されている。一方、最後の代表姿勢までの処理が完了していないと判定された場合（Ｓ９１４；ＮＯ）、Ｓ９１５へ進む。
【００８９】
Ｓ９１５において、位置姿勢推定処理部１０８は、次の代表姿勢を選択して、Ｓ９１２へ戻り、同様の処理を繰り返す。Ｓ９１６においては、位置姿勢推定処理部１０８は、Ｓ５１１と同様に、これらの位置姿勢データを統合する処理を行う。以上で処理が終了する。
【００９０】
なお、第１実施形態と同様に、分かり易くするためにフローチャートからは省略しているが、実際には部品の大きさ、若しくは測距センサの出力に基づき、奥行き方向の位置推定も同時に行っている。
【００９１】
＜ツリー型の場合の辞書削除処理＞
本実施形態においても、図６のフローチャートのＳ６０８における姿勢別把持失敗確率の算出処理と、Ｓ６０９における基準値を超えたか否かの判別処理とは、第１実施形態と同様に実行される。Ｓ６１０における推定処理の修正処理は、以下に説明するようなノードやリーフの削除処理となる。
【００９２】
図１０は、図７（ｂ）のツリー型クラス分類器の一部を示している。ここで×印のリーフは、リーフの局所領域クラスが属する代表姿勢の姿勢別把持失敗確率が所定値以上となって、対応する辞書情報が削除されたことを表す。
【００９３】
ある一つの代表姿勢が削除されると、当該姿勢に属する局所領域クラスは１０個あるので、１０個のリーフの辞書情報が削除されるが、これらは必ずしもツリー上で隣接する位置に存在する訳ではない。従って、図１０のリーフ１００１の部分以外にも×印が付けられたリーフがあるように、通常は連続しない削除状態となる。
【００９４】
いくつかの把持失敗確率の高い代表姿勢が削除された結果、リーフ１００１のように一部の連続するリーフが削除されると、分岐先の無くなった上位のノードも削除可能となる。
【００９５】
一つのリーフが削除されると、代表姿勢の種別を示す情報と投票先相対位置を示す情報とが削減される。一つのノードが削除されると、特徴抽出のための情報と分岐先のノードの情報とが削除される。これによって徐々に辞書サイズは低減していくことになり、メモリ占有容量が低減される。メモリ占有容量が下がれば、大容量メモリと高速メモリのスワップ操作等の頻度が下がり、推定処理が高速化する可能性が高くなる。
【００９６】
また図９のＳ９０４において、分岐先として判別されたノードやリーフが削除されている場合、位置姿勢推定処理部１０８は、処理中の矩形画像に対する処理をそこで打ち切ってもよい。つまり、Ｓ９０７およびＳ９０８の投票処理はスキップして、Ｓ９０９の処理を行ってもよい。打ち切られた時点で矩形画像は失敗確率が高い姿勢のものであることが確定するので、投票を行わなくとも問題なく、従って推定処理が高速化する。
【００９７】
また、削除された代表姿勢については、姿勢推定する必要がないのであるから、投票平面を持つ必要もなくなるので、この点でもメモリ占有率を下げることが可能となる。
【００９８】
失敗姿勢に基づく推定処理の修正処理が進むにつれて、把持の失敗自体が起こりにくくなり、全体の把持処理フローが徐々に高速化されていくのは、第１実施形態と同様である。
【００９９】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【０１００】
（第３実施形態）
＜優先順位変更による修正＞
上述の実施形態では、推定処理の修正処理として、失敗確率の高い代表姿勢に係る照合データを削除する例を説明したが、本発明の修正処理はこれに限るものではない。例えば、失敗確率の高い代表姿勢に係る当該照合データの照合優先順位を低くするという修正を行ってもよい。すなわち、失敗確率の高い代表姿勢に係る当該照合データの照合順序を後にする。第１実施形態で用いたようなテンプレート・マッチングタイプの位置姿勢推定処理で、切り出しウィンドウのスキャン位置の数が、姿勢別テンプレートの数よりも少ない場合には、このような優先順位の変更による修正が有効である。
【０１０１】
この場合、図５のフローチャートにおいて、スキャン処理のループ（Ｓ５０１→Ｓ５０９、Ｓ５１０）と、姿勢別テンプレート選択のループ（Ｓ５０３→Ｓ５０７、Ｓ５０８）の順序を入れ替える。さらに、Ｓ５０３、Ｓ５０８で選択されるテンプレートは、姿勢別優先順位の順とする。そして、Ｓ５０５において相関値が所定値以上となり、選択中の代表姿勢の部品が検出された時点で、同位置に関する処理は打ち切り、次のスキャン位置に移動する。
【０１０２】
このようにすることで、失敗確率の高い姿勢が推定される優先順位は徐々に下がっていくので、結果として把持の失敗頻度が下がり、把持処理フローが効率化するという効果がある。
【０１０３】
＜重み付け変更による修正＞
或いは、各代表姿勢に対応する重み係数を設定し、姿勢別把持失敗確率に基づき、この重み係数を修正するようにすることも可能である。例えば図５において、Ｓ５０４で算出される相関値は姿勢別のスコアであると見なせる。或いは、図９において、Ｓ９１２の閾値処理で残ったビンの投票値はやはり姿勢別スコアであると見なせる。
【０１０４】
そして、Ｓ５０６或いはＳ９１３において、姿勢別のスコアに対して対応する重み係数を乗じた値を、最終的な姿勢別の信頼度スコアとし、位置・姿勢と合わせて保存するようにする（信頼度算出処理）。或いは、図９のような投票処理を伴う姿勢推定処理では、重み係数を乗ずる代わりに、Ｓ９０８における投票処理で選択されたビンにインクリメントする値として、この重み係数を用いるようにすることもできる。この場合はビンの値がそのまま姿勢別の重みを考慮した信頼度スコアとなる。
【０１０５】
ここで保存された姿勢別の信頼度スコアは、Ｓ５１１或いはＳ９１６の統合処理で参照されることにより、重み係数の大きい代表姿勢であるほど、優先的に統合結果として残りやすくなるように作用する。
【０１０６】
このような代表姿勢別の重み係数を用いる場合、図６のＳ６１０で行われる姿勢推定処理の修正処理は、次のようになる。まず代表姿勢θに対応する重み係数をＷθとする。システムが稼働を開始した時点でのＷθの初期値はＷθ＝Ｗθ0とする。Ｗθ0は、全姿勢において共通とするか、若しくは事前に分かっている部品の形状に基づく把持のし易さや姿勢の起こり易さを加味して設定しても良い。そしてＳ６０８で算出される姿勢別失敗確率をＰθとする。失敗が起こりにくい姿勢であるほど失敗確率Ｐθは０に近い値となるので、Ｓ６１０においては、例えばＷθ＝（１−Ｐθ）Ｗθ0として、重み係数Ｗθを更新する。ここで、Ｓ６０９を省略して、姿勢別失敗が基準値を超えたかどうかを判別すること無しに、失敗を検知した場合には毎回更新するようにしても構わない。
【０１０７】
或いは、各姿勢に近い周辺姿勢からの影響を加味することもできる。例えば、各代表姿勢のＰθに対して、測地ドーム上の視点間の距離や面内回転角度に応じて減衰するガウシアンカーネルにより、離散的なコンボリューション演算を行って計算した値を用いて、Ｗθを修正してもよい。
【０１０８】
このように重み係数を修正する場合も、やはり失敗確率の高い姿勢は徐々に検出されにくくなっていき、把持処理フローは効率化する効果がある。
【０１０９】
＜分割領域別確率＞
また、上述の実施形態では、入力画像全域に渡って同一の位置姿勢推定処理を行う例を示したが、入力画像を領域分割して、それぞれの領域で異なった位置姿勢推定処理を行うようにすることも可能である。
【０１１０】
図１１は、対象物把持装置が図１において不図示の領域分割部をさらに備え、撮像装置１０３により撮像される画像の領域を、ＣとＮ，Ｓ，Ｅ，Ｗの５つの領域に分割する例を示している。なお、Ｎ，Ｓ，Ｅ，Ｗのそれぞれは、東西南北に対応するここで中心部Ｃの領域以外の４つの領域は、それぞれトレイ１０５のいずれかの壁面に近いため、把持動作を行う際には、いずれかの壁面の影響を受けやすい。従って各領域で失敗しやすい代表姿勢は、少なくとも一部は異なったものとなる可能性が高い。
【０１１１】
そこで、各領域に対する位置姿勢推定処理を行う際に、異なった辞書データを用いるようにする。但し、これら５つの辞書データは初期状態では同じものとする。そして、図６のＳ６０８で算出する姿勢別把持失敗確率は、分割した領域別の姿勢別把持失敗確率とし、Ｓ６１０ではそれぞれの領域に対応する辞書データを修正する。
【０１１２】
その他、領域分割は、部品が存在し得る空間を三次元的に分割して、上記と同様に辞書データの修正を行っても良い。このようにすることで、把持の失敗確率を下げつつも、失敗確率が高い姿勢として除外される部品の数が減り、トレイ上のより多くの部品を把持することが可能となる。
【０１１３】
＜周辺姿勢も考慮＞
また、上述の実施形態では、失敗した際の代表姿勢に対する修正のみを行う例を示してきたが、失敗姿勢に近い周辺姿勢に関する修正も同時に行うようにすることも可能である。ここで周辺姿勢の例を図１２に示す。視点１２０１を、失敗した代表姿勢を定義する測地ドーム状の視点とする場合、隣接する視点１２０２乃至視点１２０６の各視点で定義される代表姿勢が周辺姿勢となる。或いは、隣接視点に限らず、失敗した代表姿勢の視点を中心として、測地ドーム状で所定の距離範囲内に存在する視点としてもよい。面内回転についても同様に、把持に失敗した姿勢の面内回転角度を中心として、所定範囲内を周辺姿勢としてもよい。これら周辺姿勢の範囲を失敗確率の空間分布に基づいて決定してもよい。
【０１１４】
推定処理の修正処理は、周辺姿勢をまとめて削除するものとしても良いし、上述した重み係数による修正を失敗姿勢からの距離に応じて行う等、失敗姿勢との類似性に基づいて修正の影響度を変えるようにしてもよい。或いは失敗確率を算出する際には、周辺姿勢の失敗確率も同時にいくらか上げるというようにすることもできる。
【０１１５】
一般に、把持に失敗し易い姿勢の周辺姿勢は、やはり失敗し易い確率が高いと考えられるため、このような周辺姿勢も含めた修正とすることにより、より効率的に姿勢推定辞書を更新することが可能となる。
【０１１６】
＜ユーザによる把持失敗通知＞
また、上述の実施形態では、把持の失敗の検知を、ハンド機構１０２に備えた不図示の力覚センサの出力に基づき行うものとして説明したが、本発明はこれに限るものではない。例えばユーザが認知した把持の失敗の通知を受け付けるようにすることももちろん可能である（通知受付処理）。
【０１１７】
例えば、図１３は、オペレータによりロボットの動作が監視されることを想定して、緊急停止ボタン１３０１をロボット制御部１１１に接続した対象物把持装置を示している。オペレータは、部品１０４の把持動作が失敗した際には、緊急停止ボタン１３０１を押してロボットを停止させる。把持失敗検知部１１２は、ロボット制御部１１１を介して緊急停止を知ることにより、把持の失敗を検知することができる。
【０１１８】
＜姿勢推定のみの場合＞
また、上述の実施形態では、対象物の位置および姿勢の両方を推定する例を示してきたが、本発明は、位置推定が必要無く、姿勢の推定処理のみを行う場合にも適用できる。図１４は、対象物の位置は毎回ほぼ一定である例を示している。
【０１１９】
コンベア１４０１は、対象物である様々な姿勢であり得る部品１０４を、図１４において下方向に運搬する。下方向に運搬された部品１０４は、ガイド機構１４０２に接触してガイドされ、ある程度向きが揃えられた状態で、把持位置１４０３までさらに運搬される。把持位置１４０３に一つの部品１０４が入ると不図示の遮光センサ等によりそのことが検知され、コンベア４０１は一旦停止する。
【０１２０】
そして把持位置１４０３における部品１０４の姿勢推定処理が行われる。この処理には、基本的には上述の実施形態の位置姿勢推定処理とほぼ同じ処理を適用できるが、部品の存在する位置はほぼ決まっているので、１個所についてのみ、若しくは極めて限定された範囲のスキャンのみで、姿勢推定処理を行えば良い。また、ガイド機構１４０２により、部品の姿勢はある程度揃えられているので、最初からより少ない代表姿勢の照合データの姿勢推定辞書を用いることが可能である。
【０１２１】
姿勢推定結果に基づき、ロボット・アーム１０１およびハンド機構１０２による把持処理を行う。把持の失敗を検知した際は、上述の実施形態と同様に推定処理の修正を行う。把持動作および後処理が完了したら、コンベア１４０１による部品１０４の運搬を再開する。以後これを繰り返す。
【０１２２】
把持失敗時、若しくは失敗確率の高い姿勢であるため姿勢が推定されなくなった場合は、把持位置１４０３の部品は、そのままコンベア１４０１で図の下方に運搬され、再利用篭１４０４に投入される。再利用篭１４０４に投入された部品は、オペレータ等により再度コンベア１４０１に投入される。
【０１２３】
上述の実施形態では、対象物は一種類であるものとして説明しているが、複数種類の対象物に対応することももちろん可能である。この場合、代表姿勢に対応する照合データを、複数の対象物それぞれについて保持するようにし、これらを同時に使用するようにすれば良い。
【０１２４】
以上、本発明の好ましい実施形態について詳述したが、本発明は係る特定の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の要旨の範囲内において、種々の変形・変更が可能である。例えば、生産用ロボット・システムに限らず、家庭用ロボット等の種々のロボット・システムに対しても適用可能であることは言うまでもない。
【０１２５】
本実施形態によれば、対象物の把持動作の失敗確率を低減しつつ、対象物の姿勢を推定して把持を行う処理を高速化することが可能となる。
【０１２６】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
対象物の姿勢を姿勢推定パラメータに基づいて推定する推定手段と、
前記推定手段により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持手段と、
前記把持手段による把持の失敗を検知する検知手段と、
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正手段と、
を備えることを特徴とする対象物把持装置。
【請求項２】
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて姿勢別の把持失敗確率を算出する算出手段をさらに備え、
前記修正手段は、当該姿勢別の把持失敗確率に基づいて前記姿勢推定パラメータを修正することを特徴とする請求項１に記載の対象物把持装置。
【請求項３】
前記対象物の存在し得る領域を複数の領域に分割する領域分割手段をさらに備え、
前記算出手段は、前記検知手段により把持の失敗が検知された際の前記対象物の位置および姿勢に基づいて、前記領域分割手段により分割された領域ごとに姿勢別の把持失敗確率を算出し、
前記修正手段は、前記分割された領域ごとの姿勢別の把持失敗確率に基づいて、それぞれの領域に対応する姿勢推定パラメータを修正することを特徴とする請求項２に記載の対象物把持装置。
【請求項４】
前記姿勢推定パラメータは、前記対象物の各姿勢に対応する照合データを含み、
前記修正手段は、前記姿勢別の把持失敗確率が閾値以上の姿勢に対応する照合データに対する修正を行うことを特徴とする請求項２または３に記載の対象物把持装置。
【請求項５】
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する照合データを前記姿勢推定パラメータから削除することを特徴とする請求項４に記載の対象物把持装置。
【請求項６】
前記姿勢推定パラメータは、前記対象物の各姿勢に対応する照合データの照合順序を示す照合優先順位を含み、
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する照合データの照合順序を、他の姿勢の照合データを用いた対象物の照合順序よりも後にするように、前記照合優先順位を修正することを特徴とする請求項１に記載の対象物把持装置。
【請求項７】
前記推定手段は、前記対象物の姿勢と前記照合データとの相関値を姿勢ごとに算出する相関値算出手段を備え、
前記推定手段は、前記相関値に基づき前記対象物の姿勢を推定することを特徴とする請求項４乃至６の何れか１項に記載の対象物把持装置。
【請求項８】
前記姿勢推定パラメータには、前記対象物の各姿勢に対応する所定の重み係数を含み、
前記推定手段は、前記相関値に対して各姿勢に対応する所定の前記重み係数を乗じて姿勢別の信頼度を算出する信頼度算出手段を備え、
前記推定手段は、前記信頼度に基づき前記対象物の姿勢を推定し、
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する前記重み係数をより低い値に修正することを特徴とする請求項７に記載の対象物把持装置。
【請求項９】
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する前記姿勢推定パラメータの修正に加えて、当該姿勢と所定の範囲内にある周辺姿勢に対応する前記姿勢推定パラメータも修正することを特徴とする請求項１乃至８の何れか１項に記載の対象物把持装置。
【請求項１０】
前記検知手段は、前記把持手段に備えられたセンサの出力に基づき把持の失敗を検知することを特徴とする請求項１に記載の対象物把持装置。
【請求項１１】
ユーザが認知した前記対象物の把持の失敗通知を受け付ける受付手段をさらに備え、
前記検知手段は、前記通知に基づいて把持の失敗を検知することを特徴とする請求項１に記載の対象物把持装置。
【請求項１２】
前記推定手段は、撮像手段により撮像された画像に基づいて前記対象物の姿勢を推定することを特徴とする請求項１に記載の対象物把持装置。
【請求項１３】
前記推定手段は、測距センサから取得された距離情報に基づいて前記対象物の姿勢を推定することを特徴とする請求項１に記載の対象物把持装置。
【請求項１４】
推定手段と、把持手段と、検知手段と、修正手段と、を備える対象物把持装置の制御方法であって、
前記推定手段が、対象物の姿勢を姿勢推定パラメータに基づいて推定する推定工程と、
前記把持手段が、前記推定工程により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持工程と、
前記検知手段が、前記把持工程による把持の失敗を検知する検知工程と、
前記修正手段が、前記検知工程により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正工程と、
を備えることを特徴とする対象物把持装置の制御方法。
【請求項１５】
請求項１４に記載の対象物把持装置の制御方法の各工程をコンピュータに実行させるためのプログラム。
【請求項１】
対象物の姿勢を姿勢推定パラメータに基づいて推定する推定手段と、
前記推定手段により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持手段と、
前記把持手段による把持の失敗を検知する検知手段と、
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正手段と、
を備えることを特徴とする対象物把持装置。
【請求項２】
前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に基づいて姿勢別の把持失敗確率を算出する算出手段をさらに備え、
前記修正手段は、当該姿勢別の把持失敗確率に基づいて前記姿勢推定パラメータを修正することを特徴とする請求項１に記載の対象物把持装置。
【請求項３】
前記対象物の存在し得る領域を複数の領域に分割する領域分割手段をさらに備え、
前記算出手段は、前記検知手段により把持の失敗が検知された際の前記対象物の位置および姿勢に基づいて、前記領域分割手段により分割された領域ごとに姿勢別の把持失敗確率を算出し、
前記修正手段は、前記分割された領域ごとの姿勢別の把持失敗確率に基づいて、それぞれの領域に対応する姿勢推定パラメータを修正することを特徴とする請求項２に記載の対象物把持装置。
【請求項４】
前記姿勢推定パラメータは、前記対象物の各姿勢に対応する照合データを含み、
前記修正手段は、前記姿勢別の把持失敗確率が閾値以上の姿勢に対応する照合データに対する修正を行うことを特徴とする請求項２または３に記載の対象物把持装置。
【請求項５】
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する照合データを前記姿勢推定パラメータから削除することを特徴とする請求項４に記載の対象物把持装置。
【請求項６】
前記姿勢推定パラメータは、前記対象物の各姿勢に対応する照合データの照合順序を示す照合優先順位を含み、
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する照合データの照合順序を、他の姿勢の照合データを用いた対象物の照合順序よりも後にするように、前記照合優先順位を修正することを特徴とする請求項１に記載の対象物把持装置。
【請求項７】
前記推定手段は、前記対象物の姿勢と前記照合データとの相関値を姿勢ごとに算出する相関値算出手段を備え、
前記推定手段は、前記相関値に基づき前記対象物の姿勢を推定することを特徴とする請求項４乃至６の何れか１項に記載の対象物把持装置。
【請求項８】
前記姿勢推定パラメータには、前記対象物の各姿勢に対応する所定の重み係数を含み、
前記推定手段は、前記相関値に対して各姿勢に対応する所定の前記重み係数を乗じて姿勢別の信頼度を算出する信頼度算出手段を備え、
前記推定手段は、前記信頼度に基づき前記対象物の姿勢を推定し、
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する前記重み係数をより低い値に修正することを特徴とする請求項７に記載の対象物把持装置。
【請求項９】
前記修正手段は、前記検知手段により把持の失敗が検知された際の前記対象物の姿勢に対応する前記姿勢推定パラメータの修正に加えて、当該姿勢と所定の範囲内にある周辺姿勢に対応する前記姿勢推定パラメータも修正することを特徴とする請求項１乃至８の何れか１項に記載の対象物把持装置。
【請求項１０】
前記検知手段は、前記把持手段に備えられたセンサの出力に基づき把持の失敗を検知することを特徴とする請求項１に記載の対象物把持装置。
【請求項１１】
ユーザが認知した前記対象物の把持の失敗通知を受け付ける受付手段をさらに備え、
前記検知手段は、前記通知に基づいて把持の失敗を検知することを特徴とする請求項１に記載の対象物把持装置。
【請求項１２】
前記推定手段は、撮像手段により撮像された画像に基づいて前記対象物の姿勢を推定することを特徴とする請求項１に記載の対象物把持装置。
【請求項１３】
前記推定手段は、測距センサから取得された距離情報に基づいて前記対象物の姿勢を推定することを特徴とする請求項１に記載の対象物把持装置。
【請求項１４】
推定手段と、把持手段と、検知手段と、修正手段と、を備える対象物把持装置の制御方法であって、
前記推定手段が、対象物の姿勢を姿勢推定パラメータに基づいて推定する推定工程と、
前記把持手段が、前記推定工程により推定された前記対象物の姿勢に基づいて前記対象物を把持する把持工程と、
前記検知手段が、前記把持工程による把持の失敗を検知する検知工程と、
前記修正手段が、前記検知工程により把持の失敗が検知された際の前記対象物の姿勢に基づいて前記姿勢推定パラメータを修正する修正工程と、
を備えることを特徴とする対象物把持装置の制御方法。
【請求項１５】
請求項１４に記載の対象物把持装置の制御方法の各工程をコンピュータに実行させるためのプログラム。
【図１】
【図５】
【図６】
【図９】
【図１３】
【図２】
【図３】
【図４】
【図７】
【図８】
【図１０】
【図１１】
【図１２】
【図１４】
【図５】
【図６】
【図９】
【図１３】
【図２】
【図３】
【図４】
【図７】
【図８】
【図１０】
【図１１】
【図１２】
【図１４】
【公開番号】特開２０１３−１０１５５（Ｐ２０１３−１０１５５Ａ）
【公開日】平成２５年１月１７日（２０１３．１．１７）
【国際特許分類】
【出願番号】特願２０１１−１４３３８９（Ｐ２０１１−１４３３８９）
【出願日】平成２３年６月２８日（２０１１．６．２８）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２５年１月１７日（２０１３．１．１７）
【国際特許分類】
【出願日】平成２３年６月２８日（２０１１．６．２８）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]