JP2013184279A - 情報処理装置、情報処理方法 - Google Patents

Abstract

【課題】 把持対象の候補を減らさずに効率的な作業を行う為の技術を提供すること。
【解決手段】 第１のセンサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める。把持部を有するロボットを制御して、１以上の対象物体のうち１つの対象物体を把持対象物体として把持部に把持させる。把持部が把持している対象物体を計測するための第２のセンサが上記制御後に行った計測の結果から、把持部が把持対象物体の把持に成功しているか否かを判断する。把持部が把持対象物体の把持に失敗している場合、１以上の対象物体のうち把持対象物体に対して相互作用のある１つを選択し、選択した対象物体を把持部に把持させるようにロボットを制御する。
【選択図】図１

Description

本発明は、対象物体の位置姿勢を求める技術に関するものである。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品をピックして組立を行う。従来より、ピックするための部品の供給は、パーツフィーダと呼ばれる部品１つ１つを並べて供給するための装置を利用したり、あるいはパレット（箱）に部品を様々な姿勢で山積みすることによって行われている。パーツフィーダを利用する場合は、部品１つ１つの位置姿勢が予め決まった状態で供給されるため、ロボットによるピックは比較的容易に行われる。しかしパーツフィーダ装置を用意するためのコストが余計にかかる。また、部品の形状に合わせて異なるパーツフィーダを用意しなければならないこともある。一方、部品を山積みにして供給する場合はパレットに部品を置くだけでよいためコスト増を回避することができる。さらに、近年の少量多品種生産の傾向を受けて、様々な部品へ素早く対応することができる山積み供給に注目が集まっている。

従来技術である特許文献１では、山積みからのロボットによる部品の取り出しや把持に失敗した場合、対象部品を一定回数把持対象から除外する。取り出しや把持に失敗した部品を一定回数除外することで、しばらくの間再度失敗を繰り返すことを回避することができる。

特許第０４２２６６２３号

特許文献１による方法では、ロボットによる取り出しに失敗した部品を一定回数把持対象から除外することで、部品の位置姿勢が変わるのを消極的に待っていた。この方法では一定回数作業を行う間、把持対象の候補数が減ることになる。山積みされた部品には限りがあるため候補数が減ると効率的に作業を行うことができない。

本発明は、このような問題に鑑みてなされたものであり、把持対象の候補を減らさずに効率的な作業を行う為の技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は、第１のセンサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める計算手段と、把持部を有するロボットを制御して、前記１以上の対象物体のうち１つの対象物体を把持対象物体として前記把持部に把持させる制御手段と、前記把持部が把持している対象物体を計測するための第２のセンサが前記制御後に行った計測の結果から、前記把持部が把持対象物体の把持に成功しているか否かを判断する判断手段と、前記把持部が前記把持対象物体の把持に失敗している場合、前記１以上の対象物体のうち前記把持対象物体に対して相互作用のある１つを選択する選択手段とを備え、前記制御手段は、前記選択手段が選択した対象物体を前記把持部に把持させるように前記ロボットを制御することを特徴とする。

本発明の構成によれば、把持対象の候補を減らさずに効率的な作業を行うことができる。

システムの構成例を示す図。 ３次元幾何モデルを説明する図。 ＣＡＤモデルとＣＡＤモデルを取り囲むＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅを示す図。 システムの動作を示すフローチャート。 エッジ検出を説明する図。 線分の投影像と検出されたエッジの関係を説明する図。 把持候補物体を示す図。 把持候補物体を示す図。 システムの構成例を示す図。 システムの動作を示すフローチャート。

以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。

［第１の実施形態］
本実施形態では、対象物体に関する２次元情報（２次元画像）と３次元情報（距離画像や３次元点群データを求めるための２次元画像）とを取得する為の第１のセンサ（プロジェクタとカメラ）を用いてパレット内に山積みされた対象物体の位置姿勢を計測する。

また、ロボットに装着され対象物体の３次元情報（距離画像や３次元点群データを求めるための２次元画像）を取得する為の第２のセンサ（プロジェクタとカメラ）を用いてロボットで取出す対象物体の取出し状態計測を行う。

そして、ロボットでの取出しに失敗したと判断した場合、第１のセンサを用いて計測したパレット内の対象物体の位置姿勢に基づいて、取出しに失敗した対象物体に対して相互作用のある対象物体を選択する。本実施形態では、パレット内にある対象物体の位置姿勢に基づき２次元画像上で相互作用のある対象物体を選択する。そしてロボットに対して、選択した対象物体を把持する指示を行う。ロボットは指示に従って対象物体を把持する。 先ず、本実施形態に係るシステムの構成例について、図１を用いて説明する。

ロボット１００は、多関節ロボットであり、ロボットコントローラ部１２０からの制御命令を受けて稼働する。ロボット１００の先端にはエンドエフェクタであるハンド部が装着されており、対象物体に対する作業を行うことが可能である。本実施形態では、エンドエフェクタとして対象物体を把持することが可能なチャック機構を持つハンド部を用いる。エンドエフェクタにはモータ駆動可能なハンド部を用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよい。

なお、センサ部１０１の位置姿勢、ロボット１００やハンド部の位置や軌道、ロボット１００のアームとセンサ部１０２の相対位置姿勢のキャリブレーション作業は、公知の技術によって予め行われているものとする。これによって、位置姿勢計測処理部１１３が計測するパレット１０４内の対象物体の位置姿勢、取り出し状態計測処理部１１５が計測する対象物体の位置姿勢を、パレット１０４が置かれた空間に固定されたワークスペース座標系に変換することが可能になる。また、ワークスペース座標系で指定した位置姿勢にハンド部が移動するようにロボット１００を制御することが可能になる。

センサ部１０１は、プロジェクタと、２次元情報としての２次元画像を撮影するカメラと、を有する。センサ部１０１は、パレット１０４の上方に固定的に配置されており、山積みされた対象物体１０３の画像を撮影し、撮影した画像をセンサ情報取得部１１１に対して出力する。本実施形態では、センサ部１０１で撮影した画像は画像処理部１１０で処理するが、センサ部１０１内に画像処理機構を持たせて画像処理した結果を出力してもよい。センサ部１０１のプロジェクタとカメラとの相対的な位置関係は予めキャリブレーションにより求まっているものとする。

センサ部１０１のプロジェクタは、対象物体１０３に対して規定のパターン像（パターン光）を照射（投影）し、パターン像が投影された対象物体１０３をセンサ部１０１のカメラにより撮影し、撮影した画像をセンサ情報取得部１１１に出力する。規定のパターン像としては、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターンなどの像を利用する。２次元的なパターンやランダムドットのようなパターンでもよい。撮影された画像は、センサ情報取得部１１１を経由して位置姿勢計測処理部１１３にて三角測量の原理により距離を求めるために用いられる。

また、センサ部１０１のプロジェクタは対象物体に対して均一輝度の光を照射することも可能である。センサ部１０１のカメラにより均一輝度で照らされた対象物体の画像を撮影し、センサ情報取得部１１１に対して２次元画像を出力する。

センサ部１０２は、小型のプロジェクタと、２次元画像を撮影する小型のカメラと、を有する。センサ部１０２は、ロボット１００の各関節の角度によってその位置姿勢が制御可能（変更可能）なハンド部近くに固定的に装着されており、ハンド部が把持している対象物体を撮影する。センサ部１０２のプロジェクタとカメラとの相対的な位置関係は予めキャリブレーションにより求まっているものとする。本実施形態では、センサ部１０２で撮影した画像は画像処理部１１０で処理するが、センサ部１０２内に画像処理機構を持たせて画像処理した結果を出力してもよい。

センサ部１０２のプロジェクタは、対象物体１０３に対して規定のパターン像（パターン光）を照射（投影）し、パターン像が投影された対象物体１０３をセンサ部１０２のカメラにより撮影し、撮影した画像をセンサ情報取得部１１４に出力する。規定のパターン像としては、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターンなどの像を利用する。２次元的なパターンやランダムドットのようなパターンでもよい。また、センサ部１０２は、回折格子と照明とカメラとから構成されてもよい。この場合は回折格子と照明により対象物体にパターン光を投射し、カメラによりパターンを撮影する。撮影された画像は、センサ情報取得部１１４を経由して取り出し状態計測処理部１１５にて三角測量の原理により距離を求めるために用いられる。

対象物体１０３は、工業製品を構成する部品である。ロボット１００によりピックされて製品に組み付けられる。素材には、プラスチック、金属、ビニールなど、様々なものを用いることができる。対象物体１０３が様々な姿勢になるようにパレット１０４に山積みされている。

パレット１０４は、対象物体１０３を入れるための箱である。パレットの素材に制約はないが、プラスチックや紙素材を用いることが多い。また形状にも制約はないが、作りやすさの点から立方体あるいは直方体にすることが多い。大きさにも制約はないが、通常はセンサ部１０１で計測可能な範囲内に収まるようにする。

次に、情報処理装置として機能する画像処理部１１０について説明する。

センサ情報取得部１１１は、センサ部１０１が撮影した対象物体１０３の撮影画像を取得し、該取得した撮影画像を位置姿勢計測処理部１１３に対して出力する。センサ部１０１にはパターン光および均一輝度の光を照射するプロジェクタが備えられており、センサ情報取得部１１１はパターン光および均一輝度の光が照射された対象物体１０３の撮影画像を取得する。

モデル情報保持部１１２は、位置姿勢計測処理部１１３および取り出し状態計測処理部１１５で対象物体１０３の位置姿勢を計測するために利用されるモデル情報を保持する。モデル情報の１つとして、３次元ＣＡＤに基づく対象物体１０３の３次元幾何モデルがあげられる。もう１つのモデル情報として、実際の対象物体１０３もしくは対象物体１０３を模した３次元幾何モデルを予め定められた複数の視点から見た参照画像の集合である参照画像モデルがあげられる。

３次元幾何モデルは、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換したものである。本実施形態では、ポリゴン要素で構成される３次元幾何モデルを用いる。ポリゴン要素で構成される３次元幾何モデルについて、図２を用いて説明する。

ポリゴン要素で構成される３次元幾何モデルは、図２に示すような点、線、面といった構成要素からなる。図２（ａ）〜（ｃ）はいずれも同じ３次元幾何モデルを示している。

ポリゴン要素で構成される３次元幾何モデルのモデル情報では、図２（ａ）に例示している３次元幾何モデルの各頂点について、図２（ｄ）に示す如く、該頂点のインデックスと、該頂点の３次元座標値と、を管理している。

また、このモデル情報では、図２（ｂ）に例示している３次元幾何モデルの各辺について、図２（ｅ）に示す如く、該辺のインデックスと、該辺の両端における頂点のインデックスと、を管理している。

また、このモデル情報では、図２（ｃ）に例示している３次元幾何モデルの各面（ポリゴン）について、図２（ｆ）に示す如く、該ポリゴンのインデックスと、該ポリゴンの各辺のインデックスと、該ポリゴンの法線ベクトルと、を管理している。

参照画像モデルは、複数の２次元画像からなるデータである。実写画像に基づく参照画像モデルは、対象物体１０３を中心にして様々な方向からカメラで撮影を行って得た画像から作成する。撮影を行うためにやぐらを組んでカメラを複数配置してもよいし、人が手でカメラを持って撮影してもよいし、ロボットに装着したカメラでロボットを動かしながら撮影を行ってもよい。どの方法で撮影を行ってもよいが、撮影したときのカメラと対象物体１０３との相対位置姿勢を求め、撮影画像と関連付けて記憶しておく。相対位置姿勢は、やぐらに複数のカメラを配置したときは、やぐらの形状から求めることができる。人が手でカメラを持つ場合は、カメラに位置姿勢センサを装着することで、該位置姿勢センサから求めることができる。ロボットに装着したカメラで撮影するときはロボットの制御情報を利用して求めることができる。

対象物体１０３を模した３次元幾何モデルに基づく参照画像モデルは、ＣＡＤモデルの中心から頂点が等しい距離になるＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅを設定し、Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点からＣＡＤモデルの中心を向いたときの画像を用いる。Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点は複数あり、隣り合ったそれぞれの頂点は同じ距離となる。ある頂点を基準位置として、他の頂点との相対的な関係を元にどの方向から見たかを画像と共に記憶しておく。図３にＣＡＤモデルとＣＡＤモデルを取り囲むＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅとを示す。Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの各頂点から中心をみたときの画像の集合を参照画像モデルとする。なお、参照画像モデルは輝度画像でもよいし距離画像でもよい。

対象物体１０３が１種類と予め分かっている場合は、その種類のモデル情報だけをモデル情報保持部１１２に格納しておく。複数種類の対象物体を扱うときは、複数のモデル情報をモデル情報保持部１１２に格納しておき、利用時に切り替える。

位置姿勢計測処理部１１３は、センサ情報取得部１１１からの撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて、パレット１０４内で、ロボット１００のハンド部が把持可能な１以上の対象物体（把持候補物体）の位置姿勢を求める。

モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルの辺である線と、センサ情報取得部１１１から出力された撮影画像から抽出したエッジ成分と、の対応付けを行うことで対象物体１０３の位置姿勢を求める。本実施形態では撮影画像上で、対象物体１０３のエッジと、３次元幾何モデルにおいて該エッジに対応する部分と、が一致するように３次元幾何モデルの位置姿勢の概略値を反復演算により繰り返し補正する。これにより、該補正結果を、対象物体１０３の位置姿勢として求める。

また、３次元幾何モデルから抽出した表面の点群データと、センサ情報取得部１１１から出力された２次元のパターン画像から抽出した距離点群との対応付けを行うことで対象物体の位置姿勢を求めることもできる。パターン画像から距離点群を求めるには、空間符号化法、光切断法などの公知技術を用いればよいため、本実施形態では詳しく説明しない。求めた距離点群とモデル情報とを利用して対象物体の位置姿勢を求めるために、本実施形態ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。対象物体の位置姿勢を反復演算によって繰り返し補正する。さらに、エッジ対応付けと距離点群対応付けとの両方を考慮して対象物体の位置姿勢を求めることもできる。

モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとし、テンプレートマッチングにより撮影画像と最も一致する参照画像を求め、求めた参照画像に関連付けられている相対位置姿勢を元に対象物体１０３の位置姿勢を求める。

そして位置姿勢計測処理部１１３は、１以上の把持候補物体の位置姿勢を、ロボット作業指示部１２１及び対象物体選択部１１７に対して出力する。これによりロボット作業指示部１２１は、この１以上の位置姿勢のうち１つの位置姿勢の把持候補物体をハンド部にて把持させるための指示信号をロボット制御部１２２に対して送出する。

センサ情報取得部１１４は、センサ部１０２が撮影した対象物体１０３の撮影画像を取得し、取得した撮影画像を取り出し状態計測処理部１１５に対して出力する。

取り出し状態計測処理部１１５は、センサ情報取得部１１４から出力された撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて、ロボット１００のハンド部による対象物体の取り出し状態を計測する。本実施形態では、ロボット１００のハンド部により、把持候補物体のうち１つを把持対象物体として取り出すことに成功したか否かを、取り出し状態として計測する。即ち、ハンド部が把持対象物体の取り出しに成功し、該把持対象物体の位置姿勢を求めることができれば、該位置姿勢を取り出し状態とし、該取り出しに失敗したことで該位置姿勢を求めることができなかった場合は、その旨を示す情報を取り出し状態とする。

モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルから抽出した表面の点群データと、センサ情報取得部１１４から出力された２次元のパターン画像から抽出した距離点群との対応付けを行うことで対象物体の位置姿勢を求める。上記のパターン像が投影された対象物体の撮影画像から距離点群を求めるためには、空間符号化法、光切断法などの公知技術を用いればよい。求めた距離点群とモデル情報とを利用して対象物体の位置姿勢を求めるために、本実施形態ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。対象物体の位置姿勢を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための手法はＩＣＰ法に限るものではない。

モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより撮影画像と最も一致する参照画像を求め、その参照画像に関連付けられている相対位置姿勢を元に対象物体の位置姿勢を求める。

そして、取り出し状態計測処理部１１５は、上記のようにして求めた取り出し状態を、判断部１１６に対して送出する。

判断部１１６は、取出し状態計測処理部１１５から出力された取出し状態を用いて、ロボット１００が把持対象物体を取り出すことに成功しているか判断する。なお、取り出しが成功しているというのは、その後の既定の作業（例えば組み付け作業）のためにロボット１００が対象物体を取出して把持できている状態のことを指す。また、取り出しが失敗しているというのは、ロボットにより対象物体を把持し損ねてそのままパレット内に対象物体が残っている状態のことを指す。

そして判断部１１６は、取り出しに成功していると判断した場合は、ロボット作業指示部１２１に対してロボット１００が既定の作業（例えば組み付け作業）を行うよう指示する。一方、判断部１１６は、取り出しに失敗していると判断した場合は、次に把持を行う対象物体を選択するために、対象物体選択部１１７に対して取り出しが失敗したことを示す情報を出力する。

対象物体選択部１１７は、判断部１１６からロボット１００による取り出しが失敗した情報を受け取り、さらに位置姿勢計測処理部１１３から出力された位置姿勢を受け取る。これらの情報を受け取った対象物体選択部１１７は、ロボット１００が次に把持すべき対象物体を選択する。そして対象物体選択部１１７は、次に把持すべき対象物体の位置姿勢を把持指示部１１８に対して出力する。

把持指示部１１８は、対象物体選択部１１７から位置姿勢を受け取ると、ロボット作業指示部１２１に対してロボット１００が次に把持すべき対象物体を把持するよう指示を行う。

次にロボットコントローラ部１２０について説明する。

ロボット作業指示部１２１は、位置姿勢計測処理部１１３、判断部１１６、把持指示部１１８が求めた情報に基づいて、ハンド部を移動させる為の指示信号を生成する。

例えば、ロボット作業指示部１２１は、位置姿勢計測処理部１１３から対象物体の位置姿勢を受けると、該位置姿勢にある対象物体を把持できる位置姿勢にハンド部を移動させて把持を行うことを指示するための指示信号を生成する。

また、ロボット作業指示部１２１は、判断部１１６からの指示に応じて、既定の作業（例えば組み付け作業）を行うよう、ハンド部を移動させるための指示信号を生成する。

また、ロボット作業指示部１２１は、把持指示部１１８からの指示に応じて、次に把持すべき対象物体を把持するよう、ハンド部を移動させるための指示信号を生成する。

なお、ロボットの作業は移動、把持、吸着、組み付けに限られるものではなく、対象物体１０３の外観検査など、他の作業も含むことは言うまでもない。さらに、ロボットも多関節ロボットではなくＮＣ制御可能な可動式の機械でもよいことは言うまでもない。

ロボット制御部１２２は、ロボット作業指示部１２１から指示信号を受けると、この指示信号に従ってロボット１００の動作制御を行う。これによりロボット１００のハンド部の位置姿勢を指示信号に応じて移動させることが出来る。

次に、本実施形態に係るシステムの動作について、図４のフローチャートを用いて説明する。

ステップＳ４０１では、パレット１０４の上方に固定されたセンサ部１０１が対象物体１０３の画像を撮影し、撮影した画像をセンサ情報取得部１１１に出力する。センサ部１０１の位置姿勢は予めキャリブレーションで求まっているものとする。そしてセンサ情報取得部１１１は、センサ部１０１から受けた対象物体１０３の撮影画像を位置姿勢計測処理部１１３に対して送出する。

ステップＳ４０２では、位置姿勢計測処理部１１３は、センサ情報取得部１１１から受けた撮影画像中の複数の対象物体のうち少なくとも１つの対象物体の位置姿勢を求める（計測する）。求めた位置姿勢は、対象物体をロボットに取り出させて把持させる際に利用する。

対象物体の位置姿勢を計測する方法として本実施形態では３通りの方法を説明する。第１の方法として、センサ情報取得部１１１から出力された均一輝度の光を対象物体に照射した２次元画像と、モデル情報保持部１１２が保持しているモデル情報とをマッチングして対象物体１０３の位置姿勢を計測する方法を説明する。第２の方法として、センサ情報取得部１１１から出力された２次元のパターン画像から抽出した距離点群と、３次元幾何モデルから抽出した表面の点群データとのマッチングを行うことで対象物体の位置姿勢を求める方法を説明する。第３の方法として、エッジ対応付けと距離点群対応付けとの両方を考慮して対象物体の位置姿勢を求める方法について説明する。

まず第１の方法である、センサ情報取得部１１１から出力された均一輝度の光を対象物体に照射した２次元画像と、モデル情報保持部１１２が保持しているモデル情報と、をマッチングして対象物体１０３の位置姿勢を計測する方法について説明する。

モデル情報として３次元幾何モデルを利用する場合は、対象物体の３次元幾何モデルの辺である線分と、センサ情報取得部１１１から出力された撮影画像から抽出したエッジ成分との対応付けを行うことで対象物体の位置姿勢を求める。本実施形態では、撮影画像に３次元幾何モデルが当てはまるように、非線形最適化手法の一つであるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて計測対象物体の位置姿勢（６次元ベクトルｓで表す）の概略値を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための最適化手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。

図５はエッジ検出を説明する図である。何らかの方法（例えばテンプレートマッチング）により得られた対象物体の概略位置姿勢とキャリブレーション済みのセンサ部１０１の内部パラメータを用いて、３次元幾何モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。次に、画像上で等間隔になるように、投影された線分５０１上に制御点５０２を設定し、制御点５０２ごとに、投影された線分５０１の法線方向５０３に１次元のエッジ５０４の検出を行う（図５（ａ））。エッジは画素値の濃度勾配５０５の極値として検出されるため、図５（ｂ）に示すように、複数のエッジ５０６が検出されることがある。本実施形態では、検出されるエッジをすべて保持する。

３次元幾何モデルの辺である線分とセンサ情報取得部１１１から出力された撮影画像のエッジ成分との対応付けを行い、対象物体の位置姿勢を求めるには、位置姿勢を算出するための係数行列と誤差ベクトルの算出を行う。ここで係数行列の各要素は、画像上での点と直線の距離を位置姿勢の関数とした時の、位置姿勢の各要素に関する１次の偏微分係数である。誤差ベクトルは、エッジについては投影された線分と検出されたエッジの画像上での符号付き距離である。係数行列の導出について説明する。

図６は、線分の投影像と検出されたエッジの関係を説明する図である。図６では、画像の水平方向、垂直方向をそれぞれｕ軸６０１、ｖ軸６０２としている。ある制御点６０３（投影された各線分を画像上で等間隔に分割した点）の画像上での座標６０４を（ｕ０，ｖ０）、該制御点が所属する線分の画像上での傾きをｕ軸６０１に対する傾きθ６０５と表す。傾きθ６０５は、線分６０６の両端の三次元座標をｓに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該線分６０６の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点６０３の対応点６０７の画像上での座標６０８を（ｕ’，ｖ’）とする。ここで、対応点６０７の座標６０８（ｕ’，ｖ’）を通り、傾きがθ６０５である直線（図６の破線）上の点（ｕ，ｖ）は、以下の式で表すことが出来る。

制御点６０３の画像上での位置は対象物体の位置姿勢により変化する。また、対象物体の位置姿勢の自由度は６自由度である。すなわち、ｓは６次元ベクトルであり、対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す三次元ベクトルなどによって表現される。位置姿勢により変化する点の画像上での座標（ｕ，ｖ）は、座標６０４（ｕ０，ｖ０）の近傍で１次のテイラー展開によって以下の式（２）のように近似できる。但し△ｓｉ（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。

位置姿勢の概略値と実際の位置姿勢との差がそれほどないと仮定すると、正しいｓによって得られる制御点の画像上での位置は（１）が表す直線上にあると仮定できる。（２）によって近似されるｕ、ｖを（１）に代入することにより、（３）が得られる。

ただし、ｒ＝ｕ０ｓｉｎθ−ｖ０ｃｏｓθ（定数）である。（３）は対応付けが行われたすべてのエッジについて立式することができる。なお、すべてのエッジについて立式せずに一部のエッジについてだけ立式してもよい。（３）はｓの各成分の微小変化△ｓｉ（ｉ＝１，２，・・・，６）についての方程式であるため、（４）のような△ｓｉに関する線形連立方程式を立てることができる。

ここで（４）を（５）のように表す。

線形連立方程式の係数行列Ｊを算出するため偏微分係数の算出を行う。（５）をもとに、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１・Ｊ^Ｔを用いて位置姿勢の補正値△ｓを最小二乗基準で求める。しかしながら、エッジには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、（４）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える。重みは例えば（６）に示すようなＴｕｋｅｙの関数により与える。

ｃ１，ｃ２は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える関数であれば、如何なる関数であってもよい。各計測情報（エッジまたは点群データ）に対応する重みをｗｉとする。ここで（７）のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、（５）を（８）のように変形する。

（８）を（９）のように解くことにより補正値△ｓを求める。

算出された位置姿勢の補正値△ｓを用いて以下の式を計算することで、位置姿勢の概略値を補正する。

そして、ｓの収束判定を行い、収束していれば終了し、そうでなければ繰り返し算出を行う。収束判定では、補正値△ｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。このように、収束するまで繰り返すことで、位置姿勢を算出できる。

本実施形態では最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いる方法について説明したが、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより撮影画像と最も一致する参照画像を求め、その参照画像に関連付けられている相対位置姿勢を元に対象物体の位置姿勢を求める。参照画像の輝度をＴ（ｉ，ｊ）、撮影画像の輝度をＩ（ｉ，ｊ）として、参照画像をｍ画素×ｎ画素の画像としたとき、参照画像と撮影画像との一致度Ｒは（１０）で求めることができる。

次に第２の方法である、センサ情報取得部１１１から出力された２次元のパターン画像から抽出した距離点群と、３次元幾何モデルから抽出した表面の点群データとのマッチングを行うことで対象物体の位置姿勢を求める方法について説明する。

パターン画像から距離点群を求めるには、空間符号化法、光切断法などの公知技術を用いればよいため本実施形態では詳しく説明しない。３次元幾何モデルの点群と距離画像点群との対応付けを行うために、本実施例ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。３次元幾何モデルの表面点群をＰとし、以下のように定義する。

また、距離画像点群をＡとし、以下のように定義する。

３次元幾何モデルの表面点群Ｐを変換して距離点群Ａに位置合わせする。点群Ｐの各点ｐｉに距離が最も近い点群Ａの点をｂｉ∈Ａとしたとき、（１３）の誤差関数を定義できる。Ｒとｔはそれぞれ姿勢パラメータと移動ベクトルである。

誤差関数Ｅを小さくするＲとｔを求め、（１４）による補正を行う。

誤差関数Ｅを小さくするＲとｔを求める方法については、以下の文献に載っている。

Ｋ．Ｓ．Ａｒｕｎ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｉｎ，“Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ ３−Ｄ Ｐｏｉｎｔ Ｓｅｔｓ，” ＰＡＭＩ，ｖｏｌ．９，ｎｏ．５，１９８７
そして、Ｐの収束判定を行い、収束していれば終了し、そうでなければ繰り返し補正計算を行う。収束判定では、例えば、Ｐがほとんど変化しないことをもって収束したと判定する。このように収束するまで繰り返すことで、位置姿勢を算出できる。

最後に第３の方法として、エッジ対応付けと距離点群対応付けとの両方を考慮して対象物体の位置姿勢を求めることもできる。この方法は例えば特開２０１１−２７６２３号公報に開示の方法を適用することで実現できる。

このように、ステップＳ４０２において位置姿勢計測処理部１１３が、把持候補物体の位置姿勢を求めると、この求めた位置姿勢をロボット作業指示部１２１および対象物体選択部１１７に対して送出する。なお、位置姿勢計測処理部１１３は、複数個の把持候補物体の位置姿勢を求めたのであれば、求めた全ての位置姿勢をロボット作業指示部１２１及び対象物体選択部１１７に対して送出する。

ステップＳ４０３では、ロボット作業指示部１２１は、位置姿勢計測処理部１１３から受けた位置姿勢にロボット１００のハンド部を移動させるための指示信号を生成し、生成した指示信号をロボット制御部１２２に送出する。なお、ロボット１００が対象物体を吸着するパッドを備えていれば、位置姿勢計測処理部１１３から受けた位置姿勢に基づいて、対象物体１０３の吸着が可能な位置姿勢にロボット１００のパッドを移動させるための指示信号を生成する。

ロボット作業指示部１２１は、位置姿勢計測処理部１１３から複数の把持候補物体の位置姿勢を受け取った場合は、それらのうち１つの把持候補物体の位置姿勢を選択する。例えば山積みの中でピックできる可能性が高いと想定される一番上にある把持候補物体（位置成分のうちパレット１０４が載置されている面の法線方向の座標成分が最も大きい把持候補物体）を選択する。図７にセンサ部１０１が撮影した画像から位置姿勢を求めることができた対象物体（把持候補物体）を点線で囲って示している。図７では４つの把持候補物体についてその位置姿勢が求められていることが分かる。この４つの把持候補物体うち一番上にある把持候補物体１０３’を把持対象物体として選択する。なお、図７の点線はあくまで説明のためのものであり実際の画像には現れない。

なお、本実施形態では、ロボット作業指示部１２１で位置姿勢を計測できた複数の対象物体から１つの対象物体を選択したが、位置姿勢計測処理部１１３で１つの対象物体を選択してからロボット作業指示部１２１に該対象物体の位置姿勢を出力してもよい。

また、位置姿勢計測処理部１１３でも判断部１１６でも把持指示部１１８でも、ロボット作業指示部１２１にその位置姿勢が送出された対象物体については、画像処理部１１０内の不図示のメモリ部内に管理されている把持指示済みリストに登録する。登録する情報については特定情報に限るものではなく、位置姿勢であっても良いし、撮像画像上の位置や領域を示す情報であっても良いし、撮影画像上で割り当てられた識別情報であっても良い。

ステップＳ４０４では、ロボット制御部１２２は、ロボット作業指示部１２１からの指示信号に従ってロボット制御部１２２を制御し、該指示信号が示す位置姿勢にハンド部を移動させる。ロボット１００が把持候補物体１０３’を把持する場合は、ロボット制御部１２２がロボット１００に対して把持候補物体１０３’を把持する制御を行う。ロボット１００が把持候補物体１０３’を吸着する場合は、ロボット制御部１２２がロボット１００に対して把持候補物体１０３’を吸着する制御を行う。

ステップＳ４０５では、ハンド部に装着されているセンサ部１０２は、ハンド部が把持している（必ず把持しているとは限らない）把持対象物体を撮影し、撮影した画像をセンサ情報取得部１１４に対して送出する。センサ部１０２にはパターン光を照射するプロジェクタが備えられており、センサ情報取得部１１４は、パターン光が照射された把持対象物体の画像を取得する。この撮影は、把持地点より上空に所定距離（例えば５ｃｍ）ロボット１００を動かしてから行う。

ステップＳ４０６では、取り出し状態計測処理部１１５は、センサ情報取得部１１４から取得した撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて把持対象物体の位置姿勢を求める。把持対象物体の位置姿勢を求めるために、モデル情報保持部１１２は保持しているモデル情報を出力する。

モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルの表面から抽出した点群と、センサ情報取得部１１４から出力された撮影画像から求めた距離画像点群との対応付けを行うことで対象物体の位置姿勢を求める。パターン画像から距離点群を求めるには、空間符号化法、光切断法などの公知技術を用いればよいため本実施形態では詳しく説明しない。３次元幾何モデルの点群と距離画像点群との対応付けを行うには、ステップＳ４０２の第２の方法で説明したＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いればよい。

モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより撮影画像と最も一致する参照画像を求め、その参照画像に関連付けられている相対位置姿勢を元に対象物体の位置姿勢を求める。具体的な方法は、ステップＳ４０２の第１の方法後半で説明している。

そして取り出し状態計測処理部１１５は、把持対象物体の位置姿勢を求めることができれば、該求めた位置姿勢を判断部１１６に対して送出する。なお、取り出し状態計測処理部１１５は、ロボット１００のハンド部が対象物体の取り出しに失敗するなどが原因で、該対象物体の位置姿勢を求めることができなかった場合には、その旨を示す情報を判断部１１６に出力する。

ステップＳ４０７では、判断部１１６は、取り出し状態計測処理部１１５から位置姿勢を求めることができなかった旨を示す情報を受け取っておらず、且つ位置姿勢を受け取った場合には、ハンド部による対象物体の取り出しに成功したと判断する。一方、判断部１１６は、取り出し状態計測処理部１１５から位置姿勢を求めることができなかった旨を示す情報を受け取った場合には、ハンド部による対象物体の取り出しに失敗したと判断する。なお、判断部１１６が位置姿勢を受け取ったとしても、該位置姿勢が、予めティーチング作業などにより教示した対象物体を把持すべき位置姿勢と比較して大きく異なる場合には、取り出しは失敗したと判断する。

以上のような判断処理により、取り出しが成功したと判断した場合は、処理はステップＳ４０８に進み、取り出しが失敗したと判断した場合は、処理はステップＳ４１０に進む。

ステップＳ４０８では、判断部１１６は、ロボット作業指示部１２１に対してロボット１００が既定の作業（例えば組み付け作業）を行うよう指示する。

ステップＳ４０９では、ステップＳ４０８の指示内容を実行するようロボット作業指示部１２１がロボット制御部１２２に指示を行う。そしてロボット制御部１２２は、ロボット作業指示部１２１からの指示信号に従ってロボット１００を制御し、該指示信号が示す位置姿勢にハンド部を移動させて上記の既定の作業（例えば組み付け作業）を行わせる。

一方、ステップＳ４１０では、次に把持すべき対象物体を選択するため、判断部１１６は、対象物体選択部１１７に対して取り出しが失敗したことを示す情報（取出し失敗情報）を出力する。対象物体選択部１１７は、判断部１１６から出力された取出し失敗情報、位置姿勢計測処理部１１３から出力された把持候補物体の位置姿勢、不図示のメモリ部に登録されている把持指示済みリストを用いて、次に把持すべき対象物体を選択する。

本実施形態では、ステップＳ４０２で位置姿勢を計測できた対象物体のうち、把持指示済みリストに登録されている対象物体を除き（未選択の対象物体）、取り出しに失敗した対象物体に一番近い位置の把持候補物体を次の把持候補とする。一番近い対象物体を把持候補としてロボット１００が把持することで、取り出しに失敗した対象物体の位置姿勢を積極的に変えることができる。

図７に示した、センサ部１０１による撮影画像において、対象物体１０３’はハンド部による取り出しが失敗した対象物体である。対象物体１０３’を除いて位置姿勢を検出できている対象物体は残り３つある。これら３つのうち撮影画像上で対象物体１０３’の位置に一番近い対象物体１０３”を次に把持すべき対象物体として選択する。対象物体の近さは、対象物体の位置姿勢を計測したときのモデル重心位置同士を比較すればよい。比較する際は、モデル重心位置を対象物体の位置姿勢を規定するローカル座標系からワークスペース座標系に変換してから行う。

ステップＳ４１１では、対象物体選択部１１７は、ステップＳ４１０で次に把持すべき対象物体を選択できたかを判断する。例えば、図７では対象物体１０３’以外に３つの対象物体の位置姿勢を計測できていたが、もし対象物体１０３’以外に１つも位置姿勢を計測できていない場合は次に把持すべき対象物体を選択することはできない。もちろんステップＳ４１０での選択条件に合う対象物体がない場合も次に把持すべき対象物体を選択することはできない。これらはステップＳ４０２での位置姿勢計測結果と把持指示済みリストとを参照することで分かる。

次に把持すべき対象物体を選択できない場合は、対象物体選択部１１７は、パレット１０４を揺動する不図示の機構に対して、パレット１０４の揺動指示を出し、その後、処理はステップＳ４０１に戻る。また、対象物体選択部１１７は、把持指示済みリストを初期化する。

一方、ステップＳ４１１で、ステップＳ４１０で次に把持すべき対象物体を選択できた、と判断した場合には、処理はステップＳ４１２に進む。

ステップＳ４１２では、対象物体選択部１１７は、選択した対象物体の位置姿勢（ステップＳ４０２で求めた位置姿勢のうち把持候補の位置姿勢として選択した位置姿勢）を、把持指示部１１８に対して送出する。また、把持指示部１１８はロボット作業指示部１２１に対して選択した対象物体の位置姿勢を送出する。

ロボット作業指示部１２１は、把持指示部１１８から受けた位置姿勢にロボット１００のハンド部を移動させるための指示信号を生成し、生成した指示信号をロボット制御部１２２に送出する。また、把持指示部１１８は、選択した対象物体を把持指示済みリストに登録する。そして処理はステップＳ４０４に進む。

ステップＳ４１３では、終了指示があったかどうかを判定する。終了指示がなければ把持指示済みリストを初期化し、ステップＳ４０１に戻る。終了指示があればフローを終了して全ての動作を停止する。なお、ステップＳ４１３での終了判定を待たずに、ユーザが図示しない緊急停止ボタンを押すことによりフローを終了して全ての動作を停止してもよい。

上記のように、本実施形態では、対象物体の位置姿勢を固定的に配置したセンサを利用して計測し、ロボットによる取出し状態をロボットに装着したセンサを利用して計測する。ロボットによる取出しに失敗したと判断した場合は、パレット内にある対象物体の位置姿勢に基づき撮影画像上で一番近い位置の対象物体を相互作用のある対象物体として選択して把持指示する。このようにすることで取出しに失敗した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。

＜変形例＞
第１の実施形態では、パレット内にある把持候補物体の位置姿勢に基づき撮影画像上で一番近い位置の対象物体を次の把持候補とした。これに対する変形例として、取出しに失敗した対象物体に重なっている対象物体を次の把持候補とすることもできる。対象物体が重なっていることは、図８（ａ）に示すようにステップＳ４０２で対象物体の位置姿勢計測を行った際のモデルを撮影画像に投影配置して外形が干渉していることから判定してもよい。また、図８（ｂ）に示すように対象物体の位置姿勢を元に撮影画像上に対象物体を囲うバウンディングボックスを配置してバウンディングボックス同士が干渉していることから判定してもよい。図８（ａ）、（ｂ）共に対象物体１０３’に対象物体１０３”が干渉していると判定する。対象物体１０３’の取出しに失敗した場合は、次に把持すべき対象物体として対象物体１０３”を選択する。なお、２つの領域の干渉判定については既知の技術であるためここでは説明を省略する。

このように、第１の実施形態の変形例では取り出しに失敗した対象物体に重なった対象物体を次の把持対象にするため、確実に取り出しに失敗した対象物体を動かすことができる。

［第２の実施形態］
本実施形態では、ロボットでの取出しに失敗したと判断した場合、パレット内にある対象物体の位置姿勢に基づき３次元空間的に相互作用のある対象物体を選択する。本実施形態と第１の実施形態との差異は、対象物体選択部１１７の動作（Ｓ４１０）のみである。然るに以下では、本実施形態における対象物体選択部１１７の動作について説明し、以下で特に説明しない限りは、第１の実施形態と同様であるものとする。

対象物体選択部１１７は、判断部１１６からロボット１００による取出しが失敗した旨を示す情報を受け取り、さらに位置姿勢計測処理部１１３から出力された位置姿勢を受け取る。これらの情報を受け取った対象物体選択部１１７は、ロボット１００が次に把持すべき対象物体を選択する。対象物体選択部１１７は、次に把持すべき対象物体の位置姿勢を把持指示部１１８に対して出力する。

ステップＳ４１０では、次に把持すべき対象物体を選択するため、判断部１１６が対象物体選択部１１７に対して取出しが失敗した旨を示す情報を出力する。対象物体選択部１１７は、判断部１１６から出力された取出し失敗情報、位置姿勢計測処理部１１３から出力された対象物体の位置姿勢、把持指示済みリスト、を用いて、次に把持すべき対象物体を選択する。

次に把持すべき対象物体を選択する方法について説明する。本実施形態では、ステップＳ４０２で位置姿勢を計測した対象物体のうち、把持指示済みリストに登録されている対象物体を除き、センサ部１０１から見て取出しに失敗した対象物体より３次元空間的に奥の位置にある対象物体を次に把持すべき対象物体とする。ここで、「センサ部１０１から見て取出しに失敗した対象物体より３次元空間的に奥の位置にある」とは「センサ部１０１から取出しに失敗した対象物体までの距離より長い距離の位置にある」ことを意味する。

取出しに失敗した対象物体よりも下にある対象物体をロボット１００が把持することで、上にある取出しに失敗した対象物体の位置姿勢を積極的に変えることができる。なお、取出しに失敗した対象物体よりも下にあるというのは、重なるように下にある場合も含むし、センサ部１０１による撮影画像的には離れていても他の対象物体を経由して相互作用を与える場合も含むことは言うまでもない。３次元空間中での位置は、対象物体の位置姿勢を計測したときのモデル重心位置とする。モデル重心位置は対象物体の位置姿勢を規定するローカル座標系からワークスペース座標系に変換しておく。

単にセンサ部１０１からの距離に基づいて対象物体を選択してもよいし、パレット内にある対象物体の山積みを山ごとに領域分割して、取出しに失敗した対象物体と同じ山内に存在し、かつ奥にある対象物体を次に把持すべき対象物体として選択してもよい。山ごとの領域分割は、センサ部１０１を利用して撮影したパターン画像から抽出した距離画像を利用して行うことができる。例えば距離画像を１００画素×１００画素の領域に分割し、各領域の平均距離値を各領域の代表距離値とする。代表距離値間を補間することで山積みの等高線を求めることができる。等高線から山の勾配が分かるので山ごとの領域分割を行うことができる。また、距離画像から直接的に等高線を求めて山ごとの領域分割を行ってもよい。

上記のように、本実施形態では、ロボットによる取出しに失敗したと判断した場合は、パレット内にある対象物体の位置姿勢に基づきセンサ部１０１から見て取出しに失敗した対象物体より３次元空間的に奥の位置にある対象物体を選択して把持指示する。このようにすることで、取出しに失敗した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗した対象物体よりも下にある対象物体を次の把持対象にするため山積みの山を崩すことができ、確実に取出しに失敗した対象物体を動かすことができる。

＜変形例＞
第２の実施形態では、パレット内にある対象物体の位置姿勢に基づき取出しに失敗した対象物体よりも下にある対象物体を次の把持候補とした。第２の実施形態の変形例として、取出しに失敗した対象物体に３次元的に干渉している対象物体を次の把持候補にすることもできる。対象物体が干渉していることは、ステップＳ４０２で対象物体の位置姿勢計測を行った際のモデルを３次元空間に配置してモデル表面が干渉している、あるいは極めて近接している位置（例えば1mm以内）にあることから判定してもよい。また、対象物体の位置姿勢を元に３次元空間に対象物体を囲うバウンディングボックスを配置してバウンディングボックス同士が干渉していることから判定してもよい。２つの空間の干渉判定については既知の技術であるためここでは説明を省略する。

上記のように、第２の実施形態の変形例では、取出しに失敗した対象物体に干渉している対象物体を次の把持対象にするため、確実に取出しに失敗した対象物体を動かすことができる。

なお、第１，２の実施形態及びそれらの変形例は、次の構成の一例過ぎず、次の構成に沿った構成であれば、如何なる変形、変更を行っても良い。即ち、第１のセンサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める。そして、把持部を有するロボットを制御して、上記１以上の対象物体のうち１つの対象物体を把持対象物体として把持部に把持させる。そして、把持部が把持している対象物体を計測するための第２のセンサが上記制御後に行った計測の結果から、把持部が把持対象物体の把持に成功しているか否かを判断する。そして、把持部が把持対象物体の把持に失敗している場合、上記１以上の対象物体のうち把持対象物体に対して相互作用のある１つを選択し、選択した対象物体を把持部に把持させるようにロボットを制御する。

［第３の実施形態］
本実施形態では、対象物体に関する２次元情報（２次元画像）と３次元情報（距離画像や３次元点群データを求めるための２次元画像）とを取得する第１のセンサ（プロジェクタとカメラ）を用いてパレット内に山積みされた対象物体の位置姿勢を計測する。

そして、計測した対象物体の位置姿勢に基づき、ロボットで把持する対象物体の取出し状態予測を行う。ロボットでの取出しに失敗すると予測した場合、第１のセンサを用いて計測したパレット内の対象物体の位置姿勢に基づいて、取出しに失敗すると予測した対象物体に対して相互作用のある対象物体を選択する。そしてロボットに対して選択した対象物体を把持する指示を行う。ロボットは指示に従って対象物体を把持する。

本実施形態に係るシステムの構成例について、図９を用いて説明する。

ロボット９００は、多関節ロボットであり、ロボットコントローラ部９２０からの制御命令を受けて稼働する。ロボット９００の先端にはエンドエフェクタであるハンド部が装着されており、対象物体に対する作業を行うことが可能である。本実施形態では、エンドエフェクタとして対象物体を把持することが可能なチャック機構を持つハンド部を用いる。エンドエフェクタにはモータ駆動可能なハンド部を用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよい。

なお、センサ部９０１の位置姿勢、ロボット９００やハンド部の位置や軌道、のキャリブレーション作業は、公知の技術によって予め行われているものとする。これによって、位置姿勢計測処理部９１３が計測するパレット９０４内の対象物体の位置姿勢を、パレット１０４が置かれた空間に固定されたワークスペース座標系に変換することが可能になる。また、ワークスペース座標系で指定した位置姿勢にハンド部が移動するようにロボット９００を制御することが可能になる。

センサ部９０１は、プロジェクタと、２次元情報としての２次元画像を撮影するカメラと、を有する。センサ部９０１は、パレット９０４の上方に固定的に配置されており、山積みされた対象物体９０３の画像を撮影し、撮影した画像をセンサ情報取得部９１１に対して出力する。本実施形態では、センサ部９０１で撮影した画像は画像処理部９１０で処理するが、センサ部９０１内に画像処理機構を持たせて画像処理した結果を出力してもよい。センサ部９０１のプロジェクタとカメラとの相対的な位置関係は予めキャリブレーションにより求まっているものとする。

センサ部９０１のプロジェクタは、対象物体９０３に対して規定のパターン像（パターン光）を照射（投影）し、パターン像が投影された対象物体９０３をセンサ部９０１のカメラにより撮影し、撮影した画像をセンサ情報取得部９１１に出力する。規定のパターン像としては、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターンなどの像を利用する。２次元的なパターンやランダムドットのようなパターンでもよい。撮影された画像は、センサ情報取得部９１１を経由して位置姿勢計測処理部９１３にて三角測量の原理により距離を求めるために用いられる。

また、センサ部９０１のプロジェクタは対象物体に対して均一輝度の光を照射することも可能である。センサ部９０１のカメラにより均一輝度で照らされた対象物体の画像を撮影し、センサ情報取得部９１１に対して２次元画像を出力する。

対象物体９０３は、工業製品を構成する部品である。ロボット９００によりピックされて製品に組み付けられる。素材には、プラスチック、金属、ビニールなど、様々なものを用いることができる。対象物体９０３が様々な姿勢になるようにパレット９０４に山積みされている。

パレット９０４は、対象物体９０３を入れるための箱である。パレットの素材に制約はないが、プラスチックや紙素材を用いることが多い。また形状にも制約はないが、作りやすさの点から立方体あるいは直方体にすることが多い。大きさにも制約はないが、通常はセンサ部９０１で計測可能な範囲内に収まるようにする。

次に、情報処理装置として機能する画像処理部９１０について説明する。

センサ情報取得部９１１は、センサ部９０１が撮影した対象物体９０３の撮影画像を取得し、該取得した撮影画像を位置姿勢計測処理部９１３に対して出力する。センサ部９０１にはパターン光および均一輝度の光を照射するプロジェクタが備えられており、センサ情報取得部９１１はパターン光および均一輝度の光が照射された対象物体９０３の撮影画像を取得する。

モデル情報保持部９１２は、位置姿勢計測処理部９１３で対象物体９０３の位置姿勢を計測するために利用されるモデル情報を保持する。モデル情報については第１の実施形態と同様であるため、これに係る説明は省略する。

位置姿勢計測処理部９１３は、センサ情報取得部９１１からの撮影画像と、モデル情報保持部９１２が保持しているモデル情報と、を用いて、パレット９０４内で、ロボット９００のハンド部が把持可能な１以上の対象物体（把持候補物体）の位置姿勢を求める。位置姿勢計測処理部９１３による位置姿勢計算処理は、上記の位置姿勢計測処理部１１３によるそれと同様であり、これについては第１の実施形態ですでに説明したため、これに係る説明は省略する。そして位置姿勢計測処理部９１３は、１以上の把持候補物体の位置姿勢を、予測部９１４及び対象物体選択部９１５に対して出力する。

予測部９１４は、位置姿勢計測処理部９１３から受けた把持候補物体の位置姿勢を元に、ロボット９００で取出す対象物体の取出し状態を予測する。本実施形態では、対象物体を取出すことに成功するか否かを取出し状態として予測する。なお、取出しが成功するというのは、その後の既定の作業（例えば組み付け作業）のためにロボット９００が対象物体を取出して把持できる状態のことを指す。また、取出しが失敗するというのは、ロボットにより対象物体を把持し損ねてそのままパレット内に対象物体が残る状態のことを指す。

そして予測部９１４は、取出しに成功すると予測した場合は、ロボット作業指示部９２１に対してロボット９００が既定の作業（例えば把持や組み付け作業）を行うよう指示を行う。一方、予測部９１４は、取出しに失敗すると予測した場合は、次に把持を行う対象物体を選択するために、対象物体選択部９１５に対して取出しが失敗することを示す情報を出力する。

対象物体選択部９１５は、予測部９１４からロボット９００による取出しが失敗することを示す情報を受け取り、さらに位置姿勢計測処理部９１３から出力された位置姿勢を受け取る。これらの情報を受け取った対象物体選択部９１５は、ロボット９００が次に把持すべき対象物体を選択する。そして対象物体選択部９１５は、次に把持すべき対象物体の位置姿勢を把持指示部９１６に対して出力する。

把持指示部９１６は、対象物体選択部９１５から位置姿勢を受け取ると、ロボット作業指示部９２１に対してロボット９００が次に把持すべき対象物体を把持するよう指示を行う。

次にロボットコントローラ部９２０について説明する。

ロボット作業指示部９２１は、予測部９１４、把持指示部９１６が求めた情報に基づいて、ハンド部を移動させる為の指示信号を生成する。例えば、ロボット作業指示部９２１は、予測部９１４からの指示に応じて、既定の作業（例えば組み付け作業）を行うよう、ハンド部を移動させるための指示信号を生成する。また、ロボット作業指示部９２１は、把持指示部９１６からの指示に応じて、次に把持すべき対象物体を把持するよう、ハンド部を移動させるための指示信号を生成する。

なお、ロボットの作業は移動、把持、吸着、組み付けに限られるものではなく、対象物体９０３の外観検査など、他の作業も含むことは言うまでもない。さらに、ロボットも多関節ロボットではなくＮＣ制御可能な可動式の機械でもよいことは言うまでもない。

ロボット制御部９２２は、ロボット作業指示部９２１から指示信号を受けると、この指示信号に従ってロボット９００の動作制御を行う。これによりロボット９００のハンド部の位置姿勢を指示信号に応じて移動させることが出来る。

次に、本実施形態に係るシステムの動作について、図１０のフローチャートを用いて説明する。

ステップＳ１００１では、パレット９０４の上方に固定されたセンサ部９０１が対象物体９０３の画像を撮影し、撮影した画像をセンサ情報取得部９１１に出力する。センサ部９０１の位置姿勢は予めキャリブレーションで求まっているものとする。そしてセンサ情報取得部９１１は、センサ部９０１から受けた対象物体９０３の撮影画像を位置姿勢計測処理部９１３に対して送出する。

ステップＳ１００２では、位置姿勢計測処理部９１３は、センサ情報取得部９１１から受けた撮影画像中の複数の対象物体のうち少なくとも１つの対象物体の位置姿勢を求める（計測する）。求めた位置姿勢は、対象物体の取出し成功を予測する際や、対象物体をロボットに把持させる際に利用する。ステップＳ１００２における処理は、上記のステップＳ４０２における処理と同様であるため、これに係る説明は省略する。

そして、位置姿勢計測処理部９１３が、把持候補物体の位置姿勢を求めると、この求めた位置姿勢を予測部９１４及び対象物体選択部９１５に対して送出する。なお、位置姿勢計測処理部９１３は、複数個の把持候補物体の位置姿勢を求めたのであれば、求めた全ての位置姿勢を予測部９１４及び対象物体選択部９１５に対して送出する。

ステップＳ１００３では、予測部９１４は、位置姿勢計測処理部９１３から複数の把持候補物体の位置姿勢を受け取った場合は、それらのうち１つの把持候補物体の位置姿勢を選択する。例えば山積みの中でピックできる可能性が高いと想定される一番上にある把持候補物体（位置成分のうちパレット１０４が載置されている面の法線方向の座標成分が最も大きい把持候補物体）を選択する。なお、位置姿勢計測処理部９１３から１つの対象物体の位置姿勢を受け取った場合はその対象物体の位置姿勢を選択する。

次に、予測部９１４は、選択した対象物体の位置姿勢を元にロボット９００により取出しが成功するか否かを示す取出し状態を予測する。予めロボット９００による取出しが成功する可能性の高い対象物体の位置または姿勢または位置姿勢の範囲を、画像処理部９１０内の不図示のメモリ部に取出し成功範囲情報として保持しておく。この範囲は不図示のユーザインタフェース部をユーザが操作することにより設定して決めてもよいし、過去のロボットによる取出し等の作業履歴から決めてもよい。取出し状態の予測は、上記で選択した対象物体の位置姿勢が取出し成功範囲内に含まれているかどうかで行う。選択した対象物体の位置姿勢が取出し成功範囲に含まれていれば取出しが成功すると予測する。そうでなければ取出しが失敗すると予測する。

また、予測部９１４は、取り出し状態の予測を行った対象物体については、画像処理部１１０内の不図示のメモリ部内に管理されている予測済みリストに登録する。登録する情報については特定情報に限るものではなく、位置姿勢であっても良いし、撮像画像上の位置や領域を示す情報であっても良いし、撮影画像上で割り当てられた識別情報であっても良い。

そして、予測部９１４が取出しに成功すると予測した場合は、処理はステップＳ１００５に進み、予測部９１４が取出しに失敗すると予測した場合は、処理はステップＳ１００７に進む。

ステップＳ１００５では、予測部９１４は、ロボット作業指示部９２１に対してロボット１００が既定の作業（例えば組み付け作業）を行うよう指示する。

ステップＳ１００６では、ステップＳ１００５の指示内容を実行するようロボット作業指示部９２１がロボット制御部９２２に指示を行う。そしてロボット制御部９２２は、ロボット作業指示部９２１からの指示信号に従ってロボット９００を制御し、該指示信号が示す位置姿勢にハンド部を移動させて上記の既定の作業（例えば組み付け作業）を行わせる。

一方、ステップＳ１００７では、次に把持すべき対象物体を選択するため、予測部９１４は、対象物体選択部９１５に対して取り出しが失敗するという予測情報を出力する。対象物体選択部９１５は、予測部９１４から出力された予測情報、位置姿勢計測処理部９１３から出力された把持候補物体の位置姿勢、不図示のメモリ部に登録されている予測済みリストを用いて、次に把持すべき対象物体を選択する。

本実施形態では、ステップＳ１００２で位置姿勢を計測できた対象物体（把持候補物体）のうち、予測済みリストに登録されている対象物体を除き、取り出しに失敗すると予測した対象物体に一番近い位置の対象物体を次の把持候補とする。一番近い対象物体を把持候補としてロボット９００が把持することで、取り出しに失敗すると予測した対象物体の位置姿勢を積極的に変えることができる。

対象物体の近さは、対象物体の位置姿勢を計測したときのモデル重心位置同士を比較すればよい。比較する際は、モデル重心位置を対象物体の位置姿勢を規定するローカル座標系からワークスペース座標系に変換してから行う。

ステップＳ１００８では、対象物体選択部９１５は、ステップＳ１００７で次に把持すべき対象物体を選択できたかを判断する。例えば、取り出しに失敗すると予測した対象物体以外に１つも位置姿勢を計測できていない場合は次に把持すべき対象物体を選択することはできない。もちろんステップＳ１００７での選択条件に合う対象物体がない場合も次に把持すべき対象物体を選択することはできない。これらはステップＳ１００２での位置姿勢計測結果と予測済みリストとを参照することで分かる。

そして対象物体選択部９１５は、次に把持すべき対象物体を選択できた場合には、ステップＳ１００２で求めた位置姿勢のうち、把持候補の位置姿勢として選択した位置姿勢を、把持指示部９１６に対して送出する。そして処理はステップＳ１００３に進む。

次に把持すべき対象物体を選択できない場合は、対象物体選択部９１５は、パレット９０４を揺動する不図示の機構に対して、パレット９０４の揺動指示を出し、その後、処理はステップＳ１００１に戻る。また、対象物体選択部９１５は、予測済みリストを初期化する。

ステップＳ１００９では、終了指示があったかどうかを判定する。終了指示がなければ予測済みリストを初期化し、ステップＳ１００１に戻る。終了指示があればフローを終了して全ての動作を停止する。なお、ステップＳ１００９での終了判定を待たずに、ユーザが図示しない緊急停止ボタンを押すことによりフローを終了して全ての動作を停止してもよい。

このように、本実施形態では、対象物体の位置姿勢を固定的に配置したセンサを利用して計測し、予め用意した取出し成功範囲情報と比較することでロボットによる取出し状態を予測する。ロボットによる取出しに失敗すると予測した場合は、パレット内にある対象物体の位置姿勢に基づき撮影画像上で一番近い位置の対象物体を相互作用のある対象物体として選択して把持指示する。このようにすることで、取出しに失敗すると予測した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗すると予測した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗すると予測した対象物体を把持しないため、作業工程のタクトタイムを短くすることができる。

＜変形例＞
第３の実施形態では、パレット内にある対象物体の位置姿勢に基づき撮影画像上で一番近い位置の対象物体を次の把持候補とした。これに対する変形例として、取出しに失敗すると予測した対象物体に重なっている対象物体を次の把持候補とすることもできる。対象物体が重なっていることは、図８（ａ）に示すようにステップＳ１００２で対象物体の位置姿勢計測を行った際のモデルを撮影画像に投影配置して外形が干渉していることから判定してもよい。また、図８（ｂ）に示すように対象物体の位置姿勢を元に撮影画像上に対象物体を囲うバウンディングボックスを配置してバウンディングボックス同士が干渉していることから判定してもよい。なお、２つの領域の干渉判定については既知の技術であるためここでは説明を省略する。

このように、第３の実施形態の変形例では取り出しに失敗すると予測した対象物体に重なった対象物体を次の把持対象にするため、確実に取り出しに失敗すると予測した対象物体を動かすことができる。

［第４の実施形態］
本実施形態では、ロボットでの取出しに失敗すると予測した場合、パレット内にある対象物体の位置姿勢に基づき３次元空間的に相互作用のある対象物体を選択する。本実施形態と第３の実施形態との差異は、対象物体選択部９１５の動作（Ｓ１００７）のみである。然るに以下では、本実施形態における対象物体選択部９１５の動作について説明し、以下で特に説明しない限りは、第３の実施形態と同様であるものとする。

対象物体選択部９１５は、予測部９１４からロボット９００による取出しが失敗するという予測情報を受け取り、さらに位置姿勢計測処理部９１３から出力された位置姿勢を受け取る。これらの情報を受け取った対象物体選択部９１５は、ロボット９００が次に把持すべき対象物体を選択する。対象物体選択部９１５は、次に把持すべき対象物体の位置姿勢情報を把持指示部９１６に対して出力する。

ステップＳ１００７では、次に把持すべき対象物体を選択するため、予測部９１４が対象物体選択部９１５に対して取出しが失敗するという予測情報を出力する。対象物体選択部９１５は、予測部９１４から出力された予測情報、位置姿勢計測処理部９１３から出力された対象物体の位置姿勢、予測済みリスト、を用いて、次に把持すべき対象物体を選択する。

次に把持すべき対象物体を選択する方法について説明する。本実施形態ではステップＳ１００２で位置姿勢を計測した対象物体のうち予測済みリストに登録されている対象物体を除き、センサ部９０１から見て取出しに失敗すると予測した対象物体より３次元空間的に奥の位置の対象物体を次に把持すべき対象物体とする。ここで、「センサ部９０１から見て取出しに失敗すると予測した対象物体より３次元空間的に奥の位置」とは「センサ部９０１から取出しに失敗すると予測した対象物体までの距離より長い距離の位置」ことを意味する。

取出しに失敗すると予測した対象物体よりも下にある対象物体をロボット９００が把持することで、上にある取出しに失敗すると予測した対象物体の位置姿勢を積極的に変えることができる。なお、取出しに失敗すると予測した対象物体よりも下にあるというのは、重なるように下にある場合も含むし、センサ部９０１による撮影画像的には離れていても他の対象物体を経由して相互作用を与える場合も含むことは言うまでもない。３次元空間中での位置は、対象物体の位置姿勢を計測したときのモデル重心位置とする。モデル重心位置は対象物体の位置姿勢を規定するローカル座標系からワークスペース座標系に変換しておく。

単にセンサ部９０１からの距離に基づいて対象物体を選択してもよいし、パレット内の対象物体の山積みを山ごとに領域分割し、取出しに失敗すると予測した対象物体と同じ山内に存在し、かつ奥にある対象物体を次に把持すべき対象物体として選択してもよい。この技術は、第２の実施形態ですでに説明したため、これに係る説明は省略する。

上記のように本実施形態では、ロボットによる取出しに失敗すると予測した場合、パレット内にある対象物体の位置姿勢に基づきセンサ部９０１から見て取出しに失敗すると予測した対象物体より３次元空間的に奥の位置にある対象物体を選択して把持指示する。このようにすることで、取出しに失敗すると予測した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗すると予測した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗すると予測した対象物体よりも下にある対象物体を次の把持対象にするため山積みの山を崩すことができ、確実に取出しに失敗すると予測した対象物体を動かすことができる。

＜変形例＞
第４の実施形態では、パレット内にある対象物体の位置姿勢に基づき取出しに失敗すると予測した対象物体よりも下にある対象物体を次の把持候補とした。第４の実施形態の変形例として、取出しに失敗すると予測した対象物体に３次元的に干渉している対象物体を次の把持候補にすることもできる。対象物体が干渉していることは、ステップＳ１００２で対象物体の位置姿勢計測を行った際のモデルを３次元空間に配置してモデル表面が干渉している、あるいは極めて近接している位置（例えば1mm以内）にあることから判定してもよい。また、対象物体の位置姿勢を元に３次元空間に対象物体を囲うバウンディングボックスを配置してバウンディングボックス同士が干渉していることから判定してもよい。２つの空間の干渉判定については既知の技術であるためここでは説明を省略する。

上記のように、第４の実施形態の変形例では、取出しに失敗すると予測した対象物体に干渉している対象物体を次の把持対象にするため、確実に取出しに失敗すると予測した対象物体を動かすことができる。

なお、第３，４の実施形態及びそれらの変形例は、次の構成の一例過ぎず、次の構成に沿った構成であれば、如何なる変形、変更を行っても良い。即ち、センサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める。そして、上記１以上の対象物体のうちの１つの対象物体について求めた結果に基づいて、ロボットが有する把持部が該１つの対象物体の把持に成功するか否かを予測する。失敗すると予測した場合には、上記１以上の対象物体のうち上記１つの対象物体に対して相互作用のある１つを選択し、選択した１つの対象物体を把持部に把持させる。

［第５の実施形態］
第１，２の実施形態では、対象物体の位置姿勢を固定されたセンサ部を用いて計測し、対象物体の取出し状態をロボットに装着したセンサ部を用いて計測した。この変形例として、対象物体の位置姿勢をロボットに装着したセンサ部を用いて計測し、対象物体の取出し状態を固定されたセンサ部を用いて計測してもよい。

また、第３，４の実施形態では、対象物体の位置姿勢を固定されたセンサ部を用いて計測したが、ロボットに装着したセンサ部を用いて計測してもよい。

また、第１〜４の実施形態では、位置姿勢計測処理部１１３（９１３）で対象物体の位置姿勢を計測したが、近い対象物体や下にある対象物体を選択するために位置のみを計測してもよい。

また、第１〜４の実施形態では、センサ部１０１（９０１）をパレット１０４（９０４）の上方に固定的に配置していたが、パレット１０４（９０４）に対して斜めあるいは横に近い位置姿勢に配置してもよい。第２，４の実施形態では、取出しに失敗した、あるいは取出しに失敗すると予測した対象物体の下にある対象物体を次に把持すべき対象物体として選択するため、センサ部１０１（９０１）から見て奥にある対象物体を選択した。センサ部１０１（９０１）をパレット１０４（９０４）に対して斜めあるいは横に近い位置姿勢に配置した場合は、３次元空間的な配置から幾何変換を行い対象物体の下にある対象物体を選択する。

また、第１，２の実施形態では、ロボットに装着したセンサ部を利用して取出し状態を計測したが、ロボットの制御パラメータを利用して取出し状態を計測してもよい。例えばロボットハンドが対象物体を取出す時の把持角度（もしくはトルクや吸着圧力）といった制御パラメータと予め定めたロボットの制御パラメータとが異なると判断した場合、取出しが失敗したと判断して次に把持すべき対象物体を選択し、把持を指示する。

また、センサ部１０１とセンサ部１０２とを同じセンサ部で構成してもよい。具体的にはロボットに装着したセンサ部１０２にセンサ部１０１の役割も持たせることで実現する。まず、ロボット１００によりセンサ部１０２をパレット１０４の上方に配置してパレット内の対象物体の位置姿勢を計測する。ロボット１００による対象物体の把持を行ったらセンサ部１０２で把持している対象物体の取出し状態を計測する。取出しに失敗していると判断したら、センサ部１０２を用いて計測した対象物体の位置姿勢に基づいて次に把持すべき対象物体を選択し、把持することを指示する。

また、センサ部１０１、１０２、９０１は対象物体に関する３次元情報（距離画像や３次元点群データ）を取得するセンサ部（距離画像センサや３次元点群計測センサ）であってもよい。距離画像を取得するセンサ部としては、プロジェクタとカメラからなる距離画像センサや、各画素までの奥行きを光伝播時間によって計測するＴＯＦ型の距離画像センサなどを用いることができる。また、２次元配列に並ぶ画像状の距離データになっていなくとも、まばらな点群として計測されている３次元点群データであってもよい。また、１次元ラインセンサを走査して距離画像や３次元点群データといった３次元情報を取得してもよい。

また、上記ではモデル情報保持部１１２に保持されたモデルを利用した位置姿勢計測について説明を行ったが、センサ部１０１、センサ部１０２、センサ部９０１をそれぞれステレオカメラで構成してステレオ計測により対象物体の位置姿勢計測を行ってもよい。

また、上記の説明では対象物体に対する作業を行うロボットを用いたが、ロボットを用いることに限らない。例えば、直動ステージと回転ステージを組み合わせた機構部で構成され、ステージの制御によって位置姿勢を変更しても良い。

また、図１，９に示した画像処理部１１０（９１０）は、一般のＰＣ（パーソナルコンピュータ）を用いて実現することができる。例えば、モデル情報保持部１１２（９１２）をハードディスク装置で構成する。そして、画像処理部１１０（９１０）においてモデル情報保持部１１２（９１２）以外の各部の機能をＰＣのＣＰＵに実行させるためのコンピュータプログラムやデータをこのハードディスク装置に格納しておく。ＣＰＵは、ハードディスク装置に格納されているコンピュータプログラムやデータを適宜ＲＡＭ等のメモリにロードし、該コンピュータプログラムやデータを用いて処理を実行できる。これにより、結果としてＰＣは、画像処理部１１０（９１０）の機能を実現することができる。

＜各実施形態の効果＞
第１の実施形態では、対象物体の位置姿勢を固定的に配置したセンサを利用して計測し、ロボットによる取出し状態をロボットに装着したセンサを利用して計測する。ロボットによる取出しに失敗したと判断した場合は、パレット内にある対象物体の位置姿勢に基づき２次元画像上で一番近い位置の対象物体を相互作用のある対象物体として選択して把持指示する。このようにすることで、取出しに失敗した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。

第１の実施形態の変形例では、取出しに失敗した対象物体に重なった対象物体を次の把持対象にするため、確実に取出しに失敗した対象物体を動かすことができる。

第２の実施形態では、ロボットによる取出しに失敗したと判断した場合は、パレット内にある対象物体の位置姿勢に基づき、センサ部１０１から見て取出しに失敗した対象物体より３次元空間的に奥の位置にある対象物体を選択して把持指示する。このようにすることで、取出しに失敗した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗した対象物体よりも下にある対象物体を次の把持対象にするため、山積みの山を崩すことができ、確実に取出しに失敗した対象物体を動かすことができる。

第２の実施形態の変形例では、取出しに失敗した対象物体に干渉している、もしくは近接している対象物体を次の把持対象にするため、確実に取出しに失敗した対象物体を動かすことができる。

第３の実施形態では、対象物体の位置姿勢を固定的に配置したセンサを利用して計測し、予め用意した取出し成功範囲情報と比較することでロボットによる取出し状態を予測する。ロボットによる取出しに失敗すると予測した場合は、パレット内にある対象物体の位置姿勢に基づき２次元画像上で一番近い位置の対象物体を相互作用のある対象物体として選択して把持指示する。このようにすることで、取出しに失敗すると予測した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗すると予測した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗すると予測した対象物体を把持しないため、作業工程のタクトタイムを短くすることができる。

第３の実施形態の変形例では、取出しに失敗すると予測した対象物体に重なった対象物体を次の把持対象にするため、確実に取出しに失敗すると予測した対象物体を動かすことができる。

第４の実施形態では、ロボットによる取出しに失敗すると予測した場合は、パレット内にある対象物体の位置姿勢に基づきセンサ部９０１から見て取出しに失敗すると予測した対象物体より３次元空間的に奥の位置にある対象物体を選択して把持指示する。このようにすることで、取出しに失敗すると予測した対象物体を積極的に動かすことができ、把持対象の候補を減らさずに効率的な作業を行うことができる。また、次の対象物体を把持することと、取出しに失敗すると予測した対象物体を動かすこととを同時に行うため、作業工程のタクトタイムを短くすることができる。また、取出しに失敗すると予測した対象物体よりも下にある対象物体を次の把持対象にするため、山積みの山を崩すことができ、確実に取出しに失敗すると予測した対象物体を動かすことができる。また、取出しに失敗すると予測した対象物体を把持しないため、作業工程のタクトタイムを短くすることができる。

第４の実施形態の変形例では、取出しに失敗すると予測した対象物体に干渉している、もしくは近接している対象物体を次の把持対象にするため、確実に取出しに失敗すると予測した対象物体を動かすことができる。

＜定義＞
上記の「取出し状態」は、ロボットが対象物体を取出した状態、もしくは取出す状態を示すものであればどのようなものであってもよい。取出し状態の例としては、第１の実施形態、第２の実施形態の対象物体の把持が成功したか否かを示す情報であってもよいし、第３の実施形態、第４の実施形態の対象物体の把持が成功するか否かを示す情報であってもよい。

また、「相互作用のある対象物体」は、取出しに失敗した対象物体、もしくは取出しに失敗すると予測した対象物体を動かすことができる対象物体であればどのようなものであってもよい。相互作用のある対象物体の例としては、第１，３の実施形態の近い位置にある対象物体であってもよいし、第１，３の実施形態の変形例の重なっている対象物体であってもよい。また、第２，４の実施形態の第１のセンサから見て奥にある、即ち下にある対象物体であってもよいし、第２，４の実施形態の変形例の干渉している、もしくは近接している対象物体であってもよい。

また、上記の「第１のセンサ」は、パレット内の対象物体の位置姿勢を計測するものであればどのようなものであってもよい。第１のセンサの例としては、第１，２の実施形態の固定的に配置されたセンサであってもよいし、その他の実施形態のロボットに装着したセンサであってもよい。

また、上記の「第２のセンサ」は、ロボットによる対象物体の取出し状態を計測するものであればどのようなものであってもよい。第２のセンサの例としては、第１の実施形態のロボットに装着したセンサであってもよいし、その他の実施形態の固定的に配置されたセンサであってもよい。また、上述したように、ロボットの制御パラメータを利用して取出し状態を計測してもよい。

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

Claims (13)

1. 第１のセンサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める計算手段と、
   把持部を有するロボットを制御して、前記１以上の対象物体のうち１つの対象物体を把持対象物体として前記把持部に把持させる制御手段と、
   前記把持部が把持している対象物体を計測するための第２のセンサが前記制御後に行った計測の結果から、前記把持部が把持対象物体の把持に成功しているか否かを判断する判断手段と、
   前記把持部が前記把持対象物体の把持に失敗している場合、前記１以上の対象物体のうち前記把持対象物体に対して相互作用のある１つを選択する選択手段とを備え、
   前記制御手段は、前記選択手段が選択した対象物体を前記把持部に把持させるように前記ロボットを制御する
   ことを特徴とする情報処理装置。
2. 前記計算手段は、
   前記第１のセンサが撮影した前記集合の撮影画像を取得し、該撮影画像を用いて、前記１以上の対象物体の位置姿勢を求めることを特徴とする請求項１に記載の情報処理装置。
3. 前記判断手段は、
   前記第２のセンサが撮影した撮影画像を取得し、該撮影画像を用いて、前記把持対象物体の位置姿勢を求めるための計算処理を行い、
   該計算処理が失敗した場合には、前記把持部が把持対象物体の把持に失敗したと判断する
   ことを特徴とする請求項１又は２に記載の情報処理装置。
4. 前記選択手段は、
   前記１以上の対象物体のうちこれまでに未選択の対象物体であって前記把持対象物体に最も近い位置の対象物体を選択する
   ことを特徴とする請求項１に記載の情報処理装置。
5. 前記選択手段は、
   前記１以上の対象物体のうちこれまでに未選択の対象物体であって前記把持対象物体と重なっている対象物体を選択する
   ことを特徴とする請求項１に記載の情報処理装置。
6. 前記選択手段は、
   前記１以上の対象物体のうちこれまでに未選択の対象物体であって前記第１のセンサからの距離が前記把持対象物体よりも遠い対象物体を選択する
   ことを特徴とする請求項１に記載の情報処理装置。
7. 前記選択手段は、
   前記１以上の対象物体のうちこれまでに未選択の対象物体であって前記把持対象物体と近接している対象物体を選択する
   ことを特徴とする請求項１に記載の情報処理装置。
8. 更に、
   前記ロボットと、
   前記第１のセンサと、
   前記第２のセンサと
   を備えることを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
9. センサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める計算手段と、
   前記１以上の対象物体のうちの１つの対象物体について前記計算手段が求めた結果に基づいて、ロボットが有する把持部が該１つの対象物体の把持に成功するか否かを予測する予測手段と、
   前記予測手段が失敗すると予測した場合には、前記１以上の対象物体のうち前記１つの対象物体に対して相互作用のある１つを選択する選択手段と、
   前記ロボットを制御して、前記選択手段が選択した１つの対象物体を前記把持部に把持させる制御手段と
   を備えることを特徴とする情報処理装置。
10. 更に、
    前記ロボットと、
    前記センサと
    を備えることを特徴とする請求項９に記載の情報処理装置。
11. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の計算手段が、第１のセンサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める計算工程と、
    前記情報処理装置の制御手段が、把持部を有するロボットを制御して、前記１以上の対象物体のうち１つの対象物体を把持対象物体として前記把持部に把持させる制御工程と、
    前記情報処理装置の判断手段が、前記把持部が把持している対象物体を計測するための第２のセンサが前記制御後に行った計測の結果から、前記把持部が把持対象物体の把持に成功しているか否かを判断する判断工程と、
    前記情報処理装置の選択手段が、前記把持部が前記把持対象物体の把持に失敗している場合、前記１以上の対象物体のうち前記把持対象物体に対して相互作用のある１つを選択する選択工程とを備え、
    前記制御工程では、前記選択工程で選択した対象物体を前記把持部に把持させるように前記ロボットを制御する
    ことを特徴とする情報処理方法。
12. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の計算手段が、センサを用いて対象物体の集合を計測した結果から、１以上の対象物体の位置姿勢を求める計算工程と、
    前記情報処理装置の予測手段が、前記１以上の対象物体のうちの１つの対象物体について前記計算工程で求めた結果に基づいて、ロボットが有する把持部が該１つの対象物体の把持に成功するか否かを予測する予測工程と、
    前記情報処理装置の選択手段が、前記予測工程で失敗すると予測した場合には、前記１以上の対象物体のうち前記１つの対象物体に対して相互作用のある１つを選択する選択工程と、
    前記情報処理装置の制御手段が、前記ロボットを制御して、前記選択工程で選択した１つの対象物体を前記把持部に把持させる制御工程と
    を備えることを特徴とする情報処理方法。
13. コンピュータを、請求項１乃至１０の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images