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

Description

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

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

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

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

特許文献１による方法では、ロボットによる把持に失敗した部品を把持対象から除外するため、把持対象の候補数が減る。山積みされた部品には限りがあるため候補数が減ると効率的に作業を行うことができない。

本発明は、このような問題に鑑みてなされたものであり、ロボットによる把持あるいは把持した部品の組み付けに失敗した場合、対象部品を元あった状態あるいは把持状態に基づき良い状態で返還する為の技術を提供することを目的とする。そしてこれにより、把持対象の候補を減らさずに効率的な作業を行うことを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は、複数の対象物体を含む画像に基づいて、ロボットが有する把持部による把持候補となる複数の把持候補対象物体を決定する決定手段と、
前記ロボットを制御して、前記複数の把持候補対象物体のうちの１つを、把持対象物体として前記把持部に把持させる制御手段と、
前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の把持候補対象物体が存在する領域以外の領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする。

本発明の構成によれば、ロボットによる把持あるいは把持した部品の組み付けに失敗した場合、対象部品を元あった状態あるいは把持状態に基づき良い状態で返還するので、把持対象の候補を減らさずに効率的な作業を行うことができる。

システムの構成例を示す図。 ポリゴン要素で構成される３次元幾何モデルについて説明する図。 ＣＡＤモデル、ＣＡＤモデルを取り囲むＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅを示す図。 システムの動作を示すフローチャート。 エッジ検出を説明する図。 線分の投影像と検出されたエッジの関係を説明する図。 撮影画像を示す図。 撮影画像を示す図。 撮影画像を示す図。 返還領域を説明する図。 システムの構成例を示す図。 システムの動作を示すフローチャート。

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

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

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

そして、ロボットでの把持に失敗したと判断した場合は、第１のセンサを用いて計測したパレット内の状態や第２のセンサを用いて計測した把持状態に基づいて、対象物体をパレット内に返還する領域を算出する。そして、把持している対象物体を返還領域に返還する指示をロボットに対して行う。ロボットは指示に従って対象物体を返還する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

把持状態計測処理部１１５は、センサ情報取得部１１４から出力された撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて対象物体１０３の位置姿勢を、把持状態（把持状態データ）として求める。

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

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

そして、把持状態計測処理部１１５は、上記のようにして求めた「ハンド部が把持している対象物体の位置姿勢」を、判断部１１６及び返還領域算出部１１７に対して送出する。

判断部１１６は、ロボット１００（ハンド部）が対象物体を把持することに成功しているか否かを判断する。なお、把持が成功しているというのは、その後の既定の作業（例えば組み付け作業）のためにロボット１００が対象物体を予め定めた位置姿勢で把持できている状態のことを指す。また、把持が失敗しているというのは（ロボット１００が対象物体を把持しているものの）その後の既定の作業（例えば組み付け作業）のためにロボット１００が対象物体を予め定めた位置姿勢で把持できていない状態のことを指す。

ロボット１００（ハンド部）が対象物体を把持することに成功しているか否かの判断は、次のようにして行う。即ち、把持状態計測処理部１１５で求めた位置姿勢と、ハンド部が対象物体を把持するためにとるべき位置姿勢として予め計算された規定位置姿勢と、の差異が閾値以内であれば、把持が成功したものと判断する。逆に差異が閾値より大きい場合は、把持が失敗したものと判断する。また、把持状態計測処理部１１５での計測そのものが失敗した場合も把持が失敗したものとみなす。

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

返還領域算出部１１７は、判断部１１６からロボット１００による把持が失敗した旨を示す情報を受けると、領域状態計測処理部１１３や把持状態計測処理部１１５から受けた位置姿勢を用いて、ロボット１００が対象物体を返還する領域（返還領域）を求める。

返還指示部１１８は、ロボット１００が返還領域内で対象物体を返還すべき位置姿勢を求め、ロボット作業指示部１２１に対してロボット１００が対象物体をこの位置姿勢でパレット１０４内に返還するよう指示を行う。

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

ロボット作業指示部１２１は、領域状態計測処理部１１３、判断部１１６、返還指示部１１８、等が求めた情報に基づいて、ハンド部を移動させる為の指示信号を生成する。例えば、ロボット作業指示部１２１は、領域状態計測処理部１１３から対象物体の位置姿勢を受けると、該位置姿勢にある対象物体を把持できる位置姿勢にハンド部を移動させて把持を行うことを指示するための指示信号を生成する。また、判断部１１６からの指示に応じて、既定の作業（例えば組み付け作業）を行うよう、ハンド部を移動させるための指示信号を生成する。また、返還指示部１１８からの指示に応じて、ロボット１００が把持している対象物体をパレット１０４内に返還するよう指示するための指示信号を生成する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ステップＳ４０５では、ハンド部に装着されているセンサ部１０２は、ハンド部が把持している把持対象物体を撮影し、撮影した画像をセンサ情報取得部１１４に対して送出する。センサ部１０２にはパターン光を照射するプロジェクタが備えられており、センサ情報取得部１１４は、パターン光が照射された把持対象物体の画像を取得する。

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

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

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

そして把持状態計測処理部１１５は、把持対象物体の位置姿勢を求めると、該位置姿勢を判断部１１６及び返還領域算出部１１７に対して送出する。なお、把持状態計測処理部１１５は、把持のずれが大きいなどの理由により、モデルによる位置姿勢検出が出来なかった場合はその旨を判断部１１６に出力する。

ステップＳ４０７では、判断部１１６は、把持状態計測処理部１１５が求めた位置姿勢と、予めティーチング作業などにより教示した把持対象物体を把持すべき位置姿勢と、を比較して、把持が成功したか否かを判断する。例えば、それぞれの位置姿勢を比較して、相対位置が２００μｍ以内かつ相対角度（ｘ、ｙ、ｚ成分それぞれ）が0.5度以内であれば組み付けが可能な許容範囲として把持が成功したと判断する。これらの条件を満たさない、もしくは把持状態計測処理部１１５が位置姿勢検出できなかった場合は把持が失敗したと判断する。

把持が成功したと判断した場合、処理はステップＳ４０８に進む。一方、把持が失敗したと判断した場合、処理はステップＳ４０９に進む。

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

ステップＳ４０９では、把持対象物体をパレット１０４の領域内に戻すため、判断部１１６は、返還領域算出部１１７に対して把持が失敗したことを示す情報を出力する。この情報を受けた返還領域算出部１１７は、把持対象物体をパレット１０４内のどの領域に返還するのかを求める。本実施形態では領域状態として把持候補物体の位置姿勢を用いる。

領域状態計測処理部１１３で計測したパレット１０４内にある把持候補物体の位置姿勢を利用して返還領域を算出する方法について説明する。本実施形態においては、パレット１０４内の把持候補物体を避けた領域を返還領域として算出する。把持候補物体はロボットが良好な状態で把持可能な物体である。そのため、把持候補物体上に積み重ねるように把持している物体を返還すると、次のサイクルで把持候補から外れてしまうため望ましくない。図８（ａ）は図７と同じくセンサ部１０１が撮影した画像である。把持対象物体１０３’はステップＳ４０４にてロボットにより把持され、ステップＳ４０９の時点においては把持が失敗していると判断されている。図８（ａ）は、ステップＳ４０１で撮影された画像なので、ステップＳ４０９の時点では把持対象物体１０３’はパレット１０４内に存在せずロボットが把持している。図８（ａ）において点線で示した対象物体はステップＳ４０２で位置姿勢を計測できた対象物体（即ち把持候補物体）である。返還領域は、パレット内の領域でこれらの把持候補物体から例えば３０画素以上離れた領域とする。図８（ｂ）に返還領域を斜線で示す。なお、図８（ｂ）では図８（ａ）に示した把持対象物体１０３’の部分を返還領域としていない。なぜなら把持対象物体１０３’の下から次のサイクルで把持候補となる物体が出てくる可能性があるためである。もちろん把持対象物体１０３’の領域を返還領域としてもよいことは言うまでもない。このように、把持候補物体を避けた領域に返還することで把持候補を隠すことなく効率的な作業を行うことができる。

なお、撮影画像中における把持候補物体の領域は、把持候補物体の位置姿勢を有する該把持候補物体の３次元幾何モデルを該撮影画像上に投影した領域であるので、図８（ｂ）で斜線で示した返還領域は、この投影した領域外として求めることができる。これは上記のキャリブレーションを予め行っておくことで可能となる。もちろん、この斜線で示した領域を求める方法はこれに限るものではない。

また、ロボット１００による把持対象物体の把持状態データを利用して返還領域を算出してもよい。例えば、ロボット１００により把持している把持対象物体の姿勢と似ている姿勢の対象物体の近くには返還しなくてもよい。これは、返還した対象物体が周囲の対象物体と間違われないようにするためである。

ようするに、返還領域算出部１１７は、センサ部１０１による撮影画像中の把持候補物体の領域を除く領域に相当する「パレット１０４が載置されている面上の領域」を返還領域として特定する。

ステップＳ４１０では、返還指示部１１８は、ロボット作業指示部１２１に対して把持対象物体の返還を指示する。把持対象物体を返還する位置は、ステップＳ４０９で特定した領域内に把持対象物体が収まるように決定する。例えば、ステップＳ４０９で算出した領域内で、返還指示があった時点でのロボット１００のエンドエフェクタに一番近い位置に戻す。把持対象物体を返還する姿勢については把持状態を計測できているかどうかで決め方が異なる。把持状態を計測できている場合は、予め把持あるいは組み付けが成功しやすい姿勢を定めておき、その姿勢になるようにロボット１００を制御して返還する。把持状態を計測できていない場合は、把持対象物体をそのままパレット１０４の山積みに返還してもよいし、同じような失敗を避けるためにロボット１００を制御することで姿勢を大きく変更して返還してもよい。

ステップＳ４１１では、ロボット作業指示部１２１は、ステップＳ４０８あるいはステップＳ４１０で受けた指示内容をロボット１００に行わせるための指示信号を生成してロボット制御部１２２に対して送出する。ロボット制御部１２２は、この指示信号に従ってロボット１００を制御する。

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

このように、本実施形態では、把持候補物体の位置姿勢を固定的に配置したセンサを利用して計測し、把持状態をロボットに装着したセンサを利用して計測する。ロボットによる把持に失敗したと判断した場合は、対象物体を元あった状態あるいは把持状態に基づき良い状態で返還する。このようにすることで把持対象の候補を減らさずに効率的な作業を行うことができる。

なお、上記の構成は、次のような構成の一例に過ぎず、第２の実施形態以降で述べるいくつかの構成も、次のような構成の一例に過ぎない。即ち、第１のセンサを用いてロボットが有する把持部が把持可能な１以上の対象物体の集合が存在する領域の状態を求める（第１の計算）。そして、ロボットを制御し、上記１以上の対象物体のうちの１つを把持部に把持させる。ここで、把持部が把持している対象物体を把持対象物体とし、第２のセンサを用いて把持対象物体を計測した結果から、該把持対象物体の把持状態を求める（第２の計算）。そして、第２の計算で計算した把持状態に基づいて把持が失敗したと判断した場合には、第１のセンサによる計測結果もしくは第２のセンサによる計測結果を用いて、把持対象物体を配置する領域を求める（第３の計算）。そして、第３の計算での計算に基づいて、ロボットを制御し、把持部に把持対象物体を上記領域内に配置させる。

［第２の実施形態］
本実施形態では、パレット１０４内の領域状態として特徴点やエッジなどの２次元画像特徴情報を用いる。本実施形態に係るシステムの構成は第１の実施形態と同様であるが、領域状態計測処理部１１３、返還領域算出部１１７、返還指示部１１８が行う処理内容が第１の実施形態と異なる。

また、本実施形態に係るシステムの動作についても、基本的には図４のフローチャートに従うが、ステップＳ４０２、ステップＳ４０９、ステップＳ４１０における処理内容が第１の実施形態と異なる。

然るに以下では、第１の実施形態との差分のみについて説明し、以下で特に説明しない限りは、第１の実施形態と同様であるとする。

領域状態計測処理部１１３は、センサ情報取得部１１１から出力された撮影画像とモデル情報保持部１１２が保持しているモデル情報とを元にパレット１０４内の領域状態を計測する。本実施形態では、パレット１０４内の領域状態として、ロボット１００が把持可能な対象物体（把持候補物体）の位置姿勢に加えて、特徴点やエッジなどの２次元画像特徴を計測する。

領域状態計測処理部１１３は、第１の実施形態と同様に、計測した対象物体の位置姿勢をロボット作業指示部１２１に対して出力して、ロボット１００が対象物体を把持するよう指示を行う。また、領域状態計測処理部１１３は、計測した領域状態に含まれる特徴点やエッジなどの２次元画像特徴を返還領域算出部１１７に出力する。

返還領域算出部１１７は、判断部１１６からロボット１００による把持が失敗したことを示す情報を受け取り、さらに領域状態計測処理部１１３から出力された領域状態データ、把持状態計測処理部１１５から出力された把持状態データを受け取る。これらの情報を受け取った返還領域算出部１１７は、ロボット１００が対象物体を返還すべき返還領域を算出する。返還領域算出部１１７は、領域状態データ、把持状態データ、算出した返還領域を示すデータを返還指示部１１８に対して出力する。

返還指示部１１８は、返還領域算出部１１７から領域状態データ、把持状態データ、返還領域を示すデータを受け取る。これらの情報を受け取った返還指示部１１８は、ロボット１００が対象物体を返還すべき位置姿勢を求め、第１の実施形態と同様、ロボット作業指示部１２１に対してロボット１００が対象物体をパレット１０４内に返還するよう指示を行う。

ステップＳ４０２では、領域状態計測処理部１１３にてセンサ情報取得部１１１が取得した画像からパレット１０４内の領域状態として複数ある対象物体のうち少なくとも１つの把持候補物体の位置姿勢を計測する。さらに、センサ情報取得部１１１から出力された均一輝度の光を対象物体に照射した撮影画像から２次元画像特徴を抽出する。例えばＳｏｂｅｌフィルタやラプラシアンフィルタなどの微分フィルタを利用したり、ＳＩＦＴ特徴を演算で求めたりすることで２次元画像特徴を抽出することができる。

ステップＳ４０２において領域状態計測処理部１１３で把持候補物体の位置姿勢を求めると、領域状態計測処理部１１３は、この求めた位置姿勢をロボット作業指示部１２１に対して出力する。また、領域状態計測処理部１１３は、返還領域算出部１１７に特徴点やエッジなどの２次元画像特徴を出力する。

ステップＳ４０９では、把持対象物体をパレット１０４の領域内に戻すため、判断部１１６は、返還領域算出部１１７に対して把持が失敗したことを示す情報を出力する。この情報を受けた返還領域算出部１１７は、把持対象物体をパレット１０４のどの領域に返還するのかを求める。本実施形態では領域状態として特徴点やエッジなどの２次元画像特徴を用いる。

領域状態計測処理部１１３で計測したパレット１０４内の２次元画像特徴（エッジや特徴点）を利用して返還領域を算出する方法について説明する。本実施形態においては、パレット１０４内の２次元画像特徴が極端に多い領域あるいは極端に少ない領域を返還領域として算出する。２次元画像特徴が極端に多い領域は、例えば対象物体同士が絡まっていたり、対象物体同士で光の乱反射が起きている領域である。また、２次元画像特徴が極端に少ない領域は、プロジェクタの光が直接反射してスペキュラが発生したり他の物体の影になっている領域である。これらの領域は、２次元画像特徴の量が適切でないため、山積みの状態が大きく変わらない限り対象物体の位置姿勢を計測しにくい領域である。図９（ａ）は図７と同じくセンサ部１０１が撮影した撮影画像である。図９（ａ）では撮影画像（パレット１０４内）を例えば１００画素×１００画素のグリッドで区切り、グリッド内で検出された２次元画像特徴の量を計数する。そして、２次元特徴の量が所定の割合（例えば５％〜９５％）のグリッド以外のグリッドを対象物体の位置姿勢を求めにくい返還領域とする。図９（ｂ）に返還領域を斜線で示す。

ようするに、返還領域算出部１１７は、センサ部１０１による撮影画像において、規定範囲外の量の２次元画像特徴を有する領域に相当する「パレット１０４が載置されている面上の領域」を返還領域として特定する。

ステップＳ４１０では、返還指示部１１８は、ロボット作業指示部１２１に対して把持対象物体１０３’の返還を指示する。把持対象物体１０３’を返還する位置は、ステップＳ４０９で特定した領域内に把持対象物体１０３’が収まるように決定する。例えば、ステップＳ４０９で算出した領域内で、返還指示があった時点でのロボット１００のエンドエフェクタに一番近い位置に戻す。把持対象物体１０３’を返還する姿勢については把持状態を計測できているかどうかで決め方が異なる。把持状態を計測できている場合は、予め把持あるいは組み付けが成功しやすい姿勢を定めておき、その姿勢になるようにロボット１００を制御して返還する。把持状態を計測できていない場合は、把持対象物体１０３’をそのままパレット１０４の山積みに返還してもよいし、同じような失敗を避けるためにロボット１００を制御することで姿勢を大きく変更して返還してもよい。

このように、本実施形態では、ロボットによる把持に失敗した場合、山積みの状態が大きく変わらない限り対象物体の位置姿勢を求めにくい領域に対象物体を返還する。このようにすると、対象物体の位置姿勢を求めやすい領域に影響がないため効率的な作業を行うことができる。

［第３の実施形態］
本実施形態では、パレット内の領域状態として距離計測状態などの３次元情報を用いる。本実施形態に係るシステムの構成は第１の実施形態と同様であるが、領域状態計測処理部１１３、返還領域算出部１１７、返還指示部１１８が行う処理内容が第１の実施形態と異なる。

また、本実施形態に係るシステムの動作についても、基本的には図４のフローチャートに従うが、ステップＳ４０２、ステップＳ４０９、ステップＳ４１０における処理内容が第１の実施形態と異なる。

然るに以下では、第１の実施形態との差分のみについて説明し、以下で特に説明しない限りは、第１の実施形態と同様であるとする。

領域状態計測処理部１１３は、センサ情報取得部１１１から出力された撮影画像とモデル情報保持部１１２が保持しているモデル情報とを元にパレット１０４内の領域状態を計測する。本実施形態では、パレット１０４内の領域状態として、ロボット１００が把持可能な対象物体の位置姿勢に加えて、距離計測状態などの３次元情報を計測する。

領域状態計測処理部１１３は、第１の実施形態と同様に、計測した対象物体の位置姿勢をロボット作業指示部１２１に対して出力して、ロボット１００が対象物体を把持するよう指示を行う。また、領域状態計測処理部１１３は、計測した領域状態に含まれている距離計測状態などの３次元情報を返還領域算出部１１７に出力する。

返還領域算出部１１７は、判断部１１６からロボット１００による把持が失敗したことを示す情報を受け取り、さらに領域状態計測処理部１１３から出力された領域状態データ、把持状態計測処理部１１５から出力された把持状態データを受け取る。これらの情報を受け取った返還領域算出部１１７は、ロボット１００が対象物体を返還すべき返還領域を算出する。返還領域算出部１１７は、領域状態データ、把持状態データ、算出した返還領域を示すデータを返還指示部１１８に対して出力する。

返還指示部１１８は、返還領域算出部１１７から領域状態データ、把持状態データ、返還領域を示すデータを受け取る。これらの情報を受け取った返還指示部１１８は、ロボット１００が対象物体を返還すべき位置姿勢を求め、第１の実施形態と同様、ロボット作業指示部１２１に対してロボット１００が対象物体をパレット１０４内に返還するよう指示を行う。

ステップＳ４０２では、領域状態計測処理部１１３にてセンサ情報取得部１１１が取得した画像からパレット１０４内の領域状態として複数ある対象物体のうち少なくとも１つの把持候補物体の位置姿勢を計測する。さらに、センサ情報取得部１１１から出力された撮影画像（パターン画像）を元に距離計測状態などの３次元情報を抽出する。距離計測状態とは、センサ情報取得部１１１から出力された２次元のパターン画像から抽出した距離画像がセンサ部１０１のカメラの各画素に対応して適正に計測できているかを示す状態のことを意味する。求めた距離画像の中で対応を求めることができずに距離値が不定（Ｕｎｋｎｏｗｎ）状態の画素や、想定される距離範囲外（例えばパレット底面に対してカメラと反対側にある、あるいはパレットの底面からカメラ側に２０ｃｍ以上高さがあると計測される場合）の画素は距離計測状態が悪い画素とする。それ以外の画素は距離計測状態が良好の画素とする。パターン画像から距離画像を求めるには例えば空間符号化法、光切断法などの公知技術を用いて求める。

ステップＳ４０２において領域状態計測処理部１１３で把持候補物体の位置姿勢を求めると、領域状態計測処理部１１３は、この求めた位置姿勢をロボット作業指示部１２１に対して出力する。また、領域状態計測処理部１１３は、返還領域算出部１１７に距離計測状態などの３次元情報を出力する。

ステップＳ４０９では、把持対象物体をパレット１０４の領域内に戻すため、判断部１１６は、返還領域算出部１１７に対して把持が失敗したことを示す情報を出力する。この情報を受け付けた返還領域算出部１１７は、把持対象物体をパレット１０４のどの領域に返還するのかを求める。本実施形態においては領域状態として距離計測状態などの３次元情報を用いる。

領域状態計測処理部１１３で計測したパレット１０４内の距離計測状態を利用して返還領域を算出する方法について説明する。本実施形態においては、パレット１０４内の距離計測状態から距離値が不定（Ｕｎｋｎｏｗｎ）状態の画素や、想定される距離範囲外（例えばパレット底面に対してカメラと反対側にある、あるいはパレットの底面からカメラ側に２０ｃｍ以上高さがあると計測される場合）の画素が多い領域を返還領域として算出する。これらの領域はプロジェクタのパターン光とカメラで撮影した画像との対応付けが出来ていない、言い換えると距離計測状態が悪く対象物体の位置姿勢を求めにくい領域である。図９（ａ）と同様に距離画像のパレット１０４内を例えば１００画素×１００画素のグリッドで区切り、グリッド内で検出された距離計測状態が悪い画素を数え、所定の割合（例えば５０％）以上あるグリッドを対象物体の位置姿勢を求めにくい返還領域とする。

ようするに、返還領域算出部１１７は、センサ部１０１による距離画像において、距離計測状態が不良であることを示す条件を満たす領域に相当する「パレット１０４が載置されている面上の領域」を返還領域として特定する。

ステップＳ４１０では、返還指示部１１８は、ロボット作業指示部１２１に対して把持対象物体１０３’の返還を指示する。把持対象物体１０３’を返還する位置は、ステップＳ４０９で算出した領域内に把持対象物体１０３’が収まるように決定する。例えば、ステップＳ４０９で特定した領域内で、返還指示があった時点でのロボット１００のエンドエフェクタに一番近い位置に戻す。把持対象物体１０３’を返還する姿勢については、把持状態を計測できているかどうかで決め方が異なる。把持状態を計測できている場合は、予め把持あるいは組み付けが成功しやすい姿勢を定めておき、その姿勢になるようにロボット１００を制御して返還する。把持状態を計測できていない場合は、把持対象物体１０３’をそのままパレット１０４の山積みに返還してもよいし、同じような失敗を避けるためにロボット１００を制御することで姿勢を大きく変更して返還してもよい。

このように、本実施形態では、ロボットによる把持に失敗した場合、距離計測状態が悪く対象物体の位置姿勢を求めにくい領域に対象物体を返還する。このようにすると対象物体の位置姿勢を求めやすい領域に影響がないため効率的な作業を行うことができる。

［第４の実施形態］
本実施形態では、パレット内の領域状態としてパレット内の距離分布を用いる。本実施形態に係るシステムの構成は第１の実施形態と同様であるが、領域状態計測処理部１１３、返還領域算出部１１７、返還指示部１１８が行う処理内容が第１の実施形態と異なる。

また、本実施形態に係るシステムの動作についても、基本的には図４のフローチャートに従うが、ステップＳ４０２、ステップＳ４０９、ステップＳ４１０における処理内容が第１の実施形態と異なる。

然るに以下では、第１の実施形態との差分のみについて説明し、以下で特に説明しない限りは、第１の実施形態と同様であるとする。

領域状態計測処理部１１３は、センサ情報取得部１１１から出力された撮影画像とモデル情報保持部１１２が保持しているモデル情報とを元にパレット１０４内の領域状態を計測する。本実施形態では、パレット１０４内の領域状態として、ロボット１００が把持可能な対象物体の位置姿勢に加えて、パレット内の距離分布を計測する。

領域状態計測処理部１１３は、計測した対象物体の位置姿勢情報をロボット作業指示部１２１に対して出力して、ロボット１００が対象物体を把持するよう指示を行う。また、領域状態計測処理部１１３は、計測した領域状態に含まれているパレット内の距離分布を返還領域算出部１１７に出力する。

返還領域算出部１１７は、判断部１１６からロボット１００による把持が失敗したことを示す情報を受け取り、さらに領域状態計測処理部１１３から出力された領域状態データ、把持状態計測処理部１１５から出力された把持状態データを受け取る。これらの情報を受け取った返還領域算出部１１７は、ロボット１００が把持対象物体を返還すべき返還領域を算出する。返還領域算出部１１７は、領域状態データ、把持状態データ、算出した返還領域を示すデータを返還指示部１１８に対して出力する。

返還指示部１１８は、返還領域算出部１１７から領域状態データ、把持状態データ、返還領域を示すデータを受け取る。これらの情報を受け取った返還指示部１１８は、ロボット１００が把持対象物体を返還すべき位置姿勢を求め、ロボット作業指示部１２１に対してロボット１００が把持対象物体をパレット１０４内に返還するよう指示を行う。

ステップＳ４０２では、領域状態計測処理部１１３にてセンサ情報取得部１１１が取得した画像からパレット１０４内の領域状態として複数ある対象物体のうち少なくとも１つの把持候補物体の位置姿勢を計測する。さらに、センサ情報取得部１１１から出力されたパターン画像を元にパレット内の距離分布を抽出する。距離分布とは、パレット１０４内の対象物体をセンサ部１０１から見たときの表面の距離分布のことを意味する。センサ情報取得部１１１から出力された２次元のパターン画像から求めた距離画像を距離分布とする。パターン画像から距離画像を求めるには、空間符号化法、光切断法などの公知技術を用いればよい。さらに、求めた距離画像の中で距離値が不定（Ｕｎｋｎｏｗｎ）状態の画素や、想定される距離範囲外（例えばパレット底面に対してカメラと反対側にある、あるいはパレットの底面からカメラ側に２０ｃｍ以上高さがあると計測される場合）の画素は、「距離計測状態が悪い画素」とする。そしてこのような「距離計測状態が悪い画素」については、周囲の距離計測状態が良い画素の距離値を使って補間してもよい。こうすることで連続的な距離分布を求めることができる。

ステップＳ４０２において領域状態計測処理部１１３で把持候補物体の位置姿勢を求めると、ロボット作業指示部１２１に把持候補物体の位置姿勢を出力する。また、返還領域算出部１１７に距離分布を出力する。

ステップＳ４０９では、把持対象物体をパレット１０４の領域内に戻すため、判断部１１６は、返還領域算出部１１７に対して把持が失敗したことを示す情報を出力する。この情報を受けた返還領域算出部１１７は、把持対象物体をパレット１０４のどの領域に返還するのかを求める。本実施形態においては領域状態としてパレット内の距離分布を用いる。

領域状態計測処理部１１３で計測したパレット１０４内の距離分布を利用して返還領域を算出する方法について説明する。本実施形態においては、パレット１０４内の距離分布のヒストグラムを求め、分散が小さく平坦に近い領域を返還領域として算出する。これらの領域は山が入り組んでいないため、返還した把持対象物体が次のサイクルで他の対象物体に引っかかって取出せない状況を避けることができる領域である。また、把持対象物体を返還したときに思わぬ場所に転がったりしない領域である。図９（ａ）と同様に距離画像のパレット１０４内を例えば１００画素×１００画素のグリッドで区切る。そして、グリッド内で検出された距離分布のヒストグラムから分散を計算し、分散が所定値より小さい（例えばσ範囲が±１ｃｍ以内）グリッドを把持対象物体を戻すべき返還領域とする。

ようするに、返還領域算出部１１７は、センサ部１０１による距離画像における各領域のうち、領域内の距離の分散が規定値よりも小さい領域に相当する「パレット１０４が載置されている面上の領域」を返還領域として特定する。

ステップＳ４１０では、返還指示部１１８は、ロボット作業指示部１２１に対して把持対象物体１０３’の返還を指示する。把持対象物体１０３’を返還する位置は、ステップＳ４０９で算出した領域内に把持対象物体１０３’が収まるように決定する。例えば、ステップＳ４０９で特定した領域内で、返還指示があった時点でのロボット１００のエンドエフェクタに一番近い位置に戻す。把持対象物体１０３’を返還する姿勢については把持状態を計測できているかどうかで決め方が異なる。把持状態を計測できている場合は、予め把持あるいは組み付けが成功しやすい姿勢を定めておき、その姿勢になるようにロボット１００を制御して返還する。把持状態を計測できていない場合は、把持対象物体１０３’をそのままパレット１０４の山積みに返還してもよいし、同じような失敗を避けるためにロボット１００を制御することで姿勢を大きく変更して返還してもよい。

このように、本実施形態では、ロボットによる把持に失敗した場合、距離分布のヒストグラムの分散が小さい領域に把持対象物体を返還する。このようにすると次のサイクルで他の対象物体にひっかかったり、返還した時に思わぬ場所に転がったりしないため効率的な作業を行うことができる。

［第５の実施形態］
本実施形態では、パレット内の領域状態としてパレットの枠の位置情報を用いる。本実施形態に係るシステムの構成は第１の実施形態と同様であるが、領域状態計測処理部１１３、返還領域算出部１１７、返還指示部１１８が行う処理内容が第１の実施形態と異なる。

また、本実施形態に係るシステムの動作についても、基本的には図４のフローチャートに従うが、ステップＳ４０２、ステップＳ４０９、ステップＳ４１０における処理内容が第１の実施形態と異なる。

然るに以下では、第１の実施形態との差分のみについて説明し、以下で特に説明しない限りは、第１の実施形態と同様であるとする。

領域状態計測処理部１１３は、センサ情報取得部１１１から出力された撮影画像とモデル情報保持部１１２が保持しているモデル情報とを元にパレット１０４内の領域状態を計測する。本実施形態では、パレット１０４内の領域状態としてロボット１００が把持可能な対象物体の位置姿勢に加えて、パレットの枠の位置情報を計測する。

領域状態計測処理部１１３は、第１の実施形態と同様に、計測した対象物体の位置姿勢情報をロボット作業指示部１２１に対して出力して、ロボット１００が対象物体を把持するよう指示を行う。また、領域状態計測処理部１１３は、計測した領域状態に含まれているパレットの枠の位置情報を返還領域算出部１１７に出力する。

返還領域算出部１１７は、判断部１１６からロボット１００による把持が失敗したことを示す情報を受け取り、さらに領域状態計測処理部１１３から出力された領域状態データ、把持状態計測処理部１１５から出力された把持状態データを受け取る。これらの情報を受け取った返還領域算出部１１７は、ロボット１００が把持対象物体を返還すべき返還領域を算出する。返還領域算出部１１７は、領域状態データ、把持状態データ、算出した返還領域を示すデータを返還指示部１１８に対して出力する。

返還指示部１１８は、返還領域算出部１１７から領域状態データ、把持状態データ、返還領域を示すデータを受け取る。これらの情報を受け取った返還指示部１１８は、ロボット１００が把持対象物体を返還すべき位置姿勢を求め、ロボット作業指示部１２１に対してロボット１００が把持対象物体をパレット１０４内に返還するよう指示を行う。

ステップＳ４０２では、領域状態計測処理部１１３にてセンサ情報取得部１１１が取得した画像から領域状態として複数ある対象物体のうち少なくとも１つの把持候補物体の位置姿勢を計測する。さらに、センサ情報取得部１１１から出力された均一輝度の光を対象物体に照射した撮影画像からパレットの枠の位置情報を計測する。パレットの枠の位置は、センサ情報取得部１１１から出力された均一輝度の光を対象物体に照射した２次元画像から、パレット形状をテンプレートとしてテンプレートマッチングで求めたり、パレットの上辺である４本の直線を検出して求めることができる。

ステップＳ４０２において領域状態計測処理部１１３で把持候補物体の位置姿勢を求めると、領域状態計測処理部１１３は、この求めた位置姿勢をロボット作業指示部１２１に対して出力する。また、領域状態計測処理部１１３は、返還領域算出部１１７にパレットの枠の位置情報を出力する。

ステップＳ４０９では、把持対象物体をパレット１０４の領域内に戻すため、判断部１１６は、返還領域算出部１１７に対して把持が失敗したことを示す情報を出力する。この情報を受けた返還領域算出部１１７は、把持対象物体をパレット１０４のどの領域に返還するのかを求める。本実施形態においては領域状態としてパレットの枠の位置情報を用いる。

領域状態計測処理部１１３で計測したパレット１０４の枠の位置情報を利用して返還領域を算出する方法について説明する。本実施形態では、パレット１０４の端である枠付近に対象物体を置いても、以降のサイクルでロボット１００が把持対象物体を把持する際にパレットの枠に干渉する可能性が高まるため、パレット１０４の枠から離れたパレット１０４内の領域を返還領域とする。図１０のように枠の位置から例えば１００画素離れた領域を返還領域とする。

ようするに、返還領域算出部１１７は、センサ部１０１による撮影画像中の対象物体の集合を収容している容器の枠から規定距離だけ内側の領域に相当する「パレット１０４が載置されている面上の領域」を返還領域として特定する。

ステップＳ４１０では、返還指示部１１８は、ロボット作業指示部１２１に対して把持対象物体１０３’の返還を指示する。把持対象物体１０３’を返還する位置は、ステップＳ４０９で算出した領域内に把持対象物体１０３’が収まるように決定する。例えば、ステップＳ４０９で特定した領域内で、返還指示があった時点でのロボット１００のエンドエフェクタに一番近い位置に戻す。把持対象物体１０３’を返還する姿勢については把持状態を計測できているかどうかで決め方が異なる。把持状態を計測できている場合は、予め把持あるいは組み付けが成功しやすい姿勢を定めておき、その姿勢になるようにロボット１００を制御して返還する。把持状態を計測できていない場合は、把持対象物体１０３’をそのままパレット１０４の山積みに返還してもよいし、同じような失敗を避けるためにロボット１００を制御することで姿勢を大きく変更して返還してもよい。

このように、本実施形態ではロボットによる把持に失敗した場合、ロボットが対象物体を把持するときに枠に干渉しにくい領域に対象物体を返還する。このようにするとロボットにより対象物体を把持するときにパレットの枠にロボットが干渉しないため効率的な作業を行うことができる。

［第６の実施形態］
本実施形態では、ロボットに装着され対象物体に関する３次元情報（距離画像や３次元点群データを求めるための２次元画像）を取得するセンサ（プロジェクタとカメラ）を用いて対象物体が山積みされたパレット内の領域状態を計測する。組み付け対象物体の近くに配置されロボットが把持した対象物体と組み付け対象物体の３次元情報（距離画像や３次元点群データを求める為の２次元画像）を取得するセンサ（プロジェクタとカメラ）を用いてロボットによる対象物体の組み付け状態計測を行う。ロボットでの組み付けに失敗したと判断した場合は、計測したパレット内の領域状態に基づいて対象物体をパレット内に返還する領域を算出する。そして、ロボットに対して把持している対象物体を返還領域に返還する指示を行う。ロボットは指示に従って対象物体を返還する。以下では、第１の実施形態との差分のみについて説明し、以下で特に説明しない限りは、第１の実施形態と同様であるとする。

先ず、本実施形態に係るシステムの構成例について、図１１を用いて説明する。図１１において、図１に示した機能部と同じ機能部には同じ参照番号を付しており、これに係る説明は省略する。図１１に示す如く、本実施形態では、センサ部１０５を上記のセンサ部１０２のようにロボット１００のハンド部近傍に装着し、センサ部１０６を組み付け対象物体１０７の近くに配置する。組み付け対象物体１０７は筒状の物体であり、対象物体１０３を中に嵌め込み組み付けることができる。

画像処理部１１０内の組み付け状態計測処理部１１９は、ロボット１００が対象物体を組み付け対象物体１０７に組み付ける際の状態を計測する。判断部１２９は、組み付け状態計測処理部１１９で計測した組み付け状態が成功しているか否かの判断を行い、成功していればロボット作業指示部１２１にロボット１００による組み付け作業を指示する。失敗していれば返還領域算出部１１７に組み付け失敗していることを示す情報を出力する。

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

ステップＳ１２０１では、ハンド部近傍に装着されているセンサ部１０５は、対象物体１０３を撮影し、撮影した画像をセンサ情報取得部１１１に対して送出するので、センサ情報取得部１１１は、この撮影画像を領域状態計測処理部１１３に対して送出する。

ステップＳ１２０２では、領域状態計測処理部１１３は、センサ情報取得部１１１から受けた撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて、把持候補物体の位置姿勢を含む領域状態を計測する。把持候補物体の位置姿勢を計測する処理には、第１に実施形態においてステップＳ４０２で説明した第２の方法を利用することができる。また、領域状態は第１乃至５の実施形態で説明したものと同等である。

ステップＳ１２０３では、ロボット作業指示部１２１は、領域状態計測処理部１１３から受けた位置姿勢にロボット１００のハンド部を移動させてから把持対象物体を把持させて組み付け対象物体１０７に組み付けさせるための指示信号を生成する。そしてロボット作業指示部１２１は、この生成した指示信号をロボット制御部１２２に送出する。

ステップＳ１２０４では、ロボット制御部１２２は、ロボット作業指示部１２１からの指示信号に従ってロボット１００を制御し、把持対象物体を把持して組み付け対象物体１０７に対して組み付けさせる。

ステップＳ１２０５では、センサ部１０６は、ハンド部が把持している把持対象物体と、組み付け対象物体１０７と、を撮影し、撮影した画像をセンサ情報取得部１１４に対して送出する。

ステップＳ１２０６では、組み付け状態計測処理部１１９は、センサ情報取得部１１４から取得した撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて、ロボット１００による組み付け状態を計測する。組み付け状態は第１の実施形態のステップＳ４０６で説明した方法を利用して、ロボット１００による把持対象物体の位置姿勢と組み付け対象物体１０７の位置姿勢とを求めることで計測する。

ステップＳ１２０７では、判断部１２９が、組み付けが成功したか否かを判断する。この判断は、ステップＳ１２０６で計測した把持対象物体の位置姿勢と組み付け対象物体１０７の位置姿勢との相対位置姿勢と、予めティーチング作業などにより教示した対象物体と組み付け対象物体１０７との相対位置姿勢と、を比較することで行う。

例えば、それぞれの相対位置姿勢を比較して、相対位置が２００μｍ以内かつ相対角度（ｘ、ｙ、ｚ成分それぞれ）が0.5度以内であれば組み付けが成功したと判断する。これらの条件を満たさない、もしくは組み付け状態計測処理部１１９が対象物体もしくは組み付け対象物体１０７の位置姿勢を検出できなかった場合は組み付けが失敗したと判断する。成功したと判断した場合は処理をステップＳ１２１１に進める。失敗したと判断した場合は処理をステップＳ１２０８に進める。

ステップＳ１２０８、Ｓ１２０９、Ｓ１２１０、Ｓ１２１１はそれぞれ第１の実施形態のステップＳ４０９、Ｓ４１０、Ｓ４１１、Ｓ４１２と同様であるため説明を省略する。ただしステップＳ１２１０はステップＳ１２０９での指示にのみ従う（第１の実施形態のように把持が成功した場合の作業は本実施形態では行わない）。

このように、本実施形態では、把持候補物体の位置姿勢をロボットに装着したセンサを利用して計測し、組み付け状態を組み付け対象物体の近くに固定的に配置したセンサを利用して計測する。ロボットによる組み付けに失敗したと判断した場合は、対象物体を元あった状態に基づき良い状態で返還する。このようにすることで把持対象の候補を減らさずに効率的な作業を行うことができる。

［第７の実施形態］
第１乃至６の実施形態では、返還領域を境界線に囲まれた領域とした。これは返還領域を撮影画像の画素ごとに返還領域＝１、非返還領域＝０と２値で表現していることと同じである。変形例として、撮影画像の画素ごとに０から１までの連続的な値を持たせて表現してもよい。例えば２次元画像特徴や３次元情報を信頼度付きで計測し、信頼度を用いて返還領域を表現してもよい。返還指示を行う際は最も信頼度の低い位置にロボットにより把持している対象物体を返還してもよい。

また、第６の実施形態ではセンサ部１０６を利用して計測した組み付け状態に基づいて対象物体を返還する技術について説明した。変形例として、ロボット１００の制御パラメータを利用して組み付け状態を計測してもよい。ロボット１００の制御パラメータに基づき、ロボット１００が予め定めた組み付け動作とは異なる動作をしていると判断した場合は、組み付けが失敗したと判断して対象物体を返還してもよい。

また、対象物体を返還する方法にはバリエーションがあり、第１乃至６の実施形態で説明したそれぞれのうちどれを選択するかは装置を使用するユーザが対象物体の大きさ、形状の複雑さ、素材などから決定すればよい。把持対象物体を返還する方法は、キーボードやマウスなどのユーザインターフェースを操作してユーザが選択しても良いし、状況に応じて画像処理部１１０が切り替えても良い。

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

また、センサ部１０５とセンサ部１０６とを同じセンサ部で構成してもよい。具体的にはロボットに装着したセンサ部１０５にセンサ部１０６の役割も持たせることで実現する。まずロボット１００によりセンサ部１０５をパレット１０４の上方に配置してパレット内の領域状態を計測する。ロボット１００による対象物体の組み付けを行ったらセンサ部１０５で把持している対象物体の位置姿勢と組み付け対象物体の位置姿勢とを計測して組み付け状態を求める。組み付けに失敗していることを計測したら、センサ部１０５を用いて計測した領域状態に基づいて対象物体を返還する領域を算出し、返還を指示する。

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

また、センサ部を１次元ラインセンサで構成して、ベルトコンベアなどに載って動いている対象物体を走査することで対象物体に関する情報を取得してもよい。

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

また、上記の説明ではセンサ部１０２の位置姿勢を変更するためにロボットを用いたが、ロボットを用いることに限らない。例えば、直動ステージと回転ステージを組み合わせた機構部にセンサ部１０２を搭載して、ステージの制御によって位置姿勢を変更しても良い。対象物体を操作するロボットとは別に、位置姿勢変更構成を設けても良い。

また、ロボット作業指示部１２１は、領域状態計測処理部１１３から複数の把持候補物体の位置姿勢を受け取った場合にそれらのうち１つの把持候補物体の位置姿勢を選択するが、領域状態計測処理部１１３で選択してもよい。

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

＜各実施形態の効果＞
第１の実施形態では、把持候補物体の位置姿勢を固定的に配置したセンサを利用して計測し、把持状態をロボットに装着したセンサを利用して計測する。ロボットによる把持に失敗したと判断した場合は、対象物体を元あった状態あるいは把持状態に基づき良い状態で返還する。このようにすることで把持対象の候補を減らさずに効率的な作業を行うことができる。

第２の実施形態では、ロボットによる把持に失敗した場合、山積みの状態が大きく変わらない限り対象物体の位置姿勢を求めにくい領域に対象物体を返還する。このようにすると、対象物体の位置姿勢を求めやすい領域に影響がないため効率的な作業を行うことができる。

第３の実施形態では、ロボットによる把持に失敗した場合、距離計測状態が悪く対象物体の位置姿勢を求めにくい領域に対象物体を返還する。このようにすると、対象物体の位置姿勢を求めやすい領域に影響がないため効率的な作業を行うことができる。

第４の実施形態では、ロボットによる把持に失敗した場合、距離分布のヒストグラムの分散が小さい領域に対象物を返還する。このようにすると、次のサイクルで他の対象物体にひっかかったり、返還した時に思わぬ場所に転がったりしないため効率的な作業を行うことができる。

第５の実施形態では、ロボットによる把持に失敗した場合、ロボットが対象物体を把持するときに枠に干渉しにくい領域に対象物体を返還する。このようにすると、ロボットにより対象物体を把持するときにパレットの枠にロボットが干渉しないため効率的な作業を行うことができる。

第６の実施形態では、把持候補物体の位置姿勢をロボットに装着したセンサを利用して計測し、組み付け状態を組み付け対象物体の近くに固定的に配置したセンサを利用して計測する。ロボットによる組み付けに失敗したと判断した場合は、対象物体を元あった状態に基づき良い状態で返還する。このようにすることで把持対象の候補を減らさずに効率的な作業を行うことができる。

＜定義＞
領域状態は、対象物体が存在する領域内の状態を示すものであればどのようなものであってもよい。第１〜５の実施形態ではそれぞれ、領域状態の一例として、把持候補物体の位置姿勢、特徴点やエッジなどの２次元画像特徴、距離計測状態などの３次元情報、パレット内の距離分布、パレットの枠の位置情報、を説明した。

また、上記のセンサ部１０１は、パレット内の領域状態の計測を行うためのものであればどのようなものであってもよい。例としては、第１の実施形態の固定的に配置されたセンサであってもよいし、第６の実施形態のロボットに装着したセンサであってもよい。

また、上記のセンサ部１０２は、ロボットによる把持状態の計測もしくは組み付け状態の計測を行うためのものであればどのようなものであってもよい。例としては、第１の実施形態のロボットに装着したセンサであってもよいし、第６の実施形態の固定的に配置されたセンサであってもよい。また、第７の実施形態に示したように、ロボットの制御パラメータを利用して組み付け状態を計測してもよい。

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

Claims (17)

1. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部による把持候補となる複数の把持候補対象物体を決定する決定手段と、
   前記ロボットを制御して、前記複数の把持候補対象物体のうちの１つを、把持対象物体として前記把持部に把持させる制御手段と、
   前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の把持候補対象物体が存在する領域以外の領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
   前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理装置。
2. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御手段と、
   前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む２次元画像の中で規定範囲外の量の画像特徴を有する領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
   前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理装置。
3. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御手段と、
   前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む距離画像の中で距離計測状態が不良であることを示す条件を満たす領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
   前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理装置。
4. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御手段と、
   前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む距離画像の中で距離の分散が規定値よりも小さい領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
   前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理装置。
5. 前記画像は、位置が固定された第１のセンサにより取得されることを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 更に、
   前記把持部が前記把持対象物体を所定の状態で把持しているかを判断する判断手段を備え、
   前記判断手段は、前記ロボットに備え付けられた第２のセンサにより取得した情報に基づいて前記把持部が前記把持対象物体を所定の状態で把持しているかを判断することを特徴とする請求項５に記載の情報処理装置。
7. 更に、
   前記第１のセンサと前記第２のセンサとを備えることを特徴とする請求項６に記載の情報処理装置。
8. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部による把持候補となる複数の把持候補対象物体を決定する決定手段と、
   前記ロボットを制御して、前記複数の把持候補対象物体のうちの１つを、把持対象物体として前記把持部に把持させ、他の物体に組みつける制御手段と、
   前記把持対象物体と前記他の物体との組みつけの状態が所定の状態でない場合に、前記複数の把持候補対象物体が存在する領域以外の領域の中から、前記把持対象物体を戻す領域を決定する領域決定手段とを備え、
   前記制御手段は、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理装置。
9. 前記画像は、前記ロボットに備え付けられた第３のセンサにより取得されることを特徴とする請求項８に記載の情報処理装置。
10. 更に、
    前記組みつけの状態が所定の状態であるか否かを判断する判断手段を備え、
    前記判断手段は、位置が固定された第４のセンサにより取得した情報に基づいて前記組みつけの状態が所定の状態であるか否かを判断することを特徴とする請求項９に記載の情報処理装置。
11. 更に、
    前記第３のセンサと前記第４のセンサとを備えることを特徴とする請求項１０に記載の情報処理装置。
12. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部による把持候補となる複数の把持候補対象物体を決定する決定工程と、
    前記ロボットを制御して、前記複数の把持候補対象物体のうちの１つを、把持対象物体として前記把持部に把持させる制御工程と、
    前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の把持候補対象物体が存在する領域以外の領域の中から、前記把持対象物体を戻す領域を決定する領域決定工程とを備え、
    前記制御工程では、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理方法。
13. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御工程と、
    前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む２次元画像の中で規定範囲外の量の画像特徴を有する領域の中から、前記把持対象物体を戻す領域を決定する領域決定工程とを備え、
    前記制御工程では、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理方法。
14. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御工程と、
    前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む距離画像の中で距離計測状態が不良であることを示す条件を満たす領域の中から、前記把持対象物体を戻す領域を決定する領域決定工程とを備え、
    前記制御工程では、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理方法。
15. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部に把持対象物体を把持させる制御工程と、
    前記把持部が前記把持対象物体を所定の状態で把持していない場合には、前記複数の対象物体を含む距離画像の中で距離の分散が規定値よりも小さい領域の中から、前記把持対象物体を戻す領域を決定する領域決定工程とを備え、
    前記制御工程では、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理方法。
16. 複数の対象物体を含む画像に基づいて、ロボットが有する把持部による把持候補となる複数の把持候補対象物体を決定する決定工程と、
    前記ロボットを制御して、前記複数の把持候補対象物体のうちの１つを、把持対象物体として前記把持部に把持させ、他の物体に組みつける制御工程と、
    前記把持対象物体と前記他の物体との組みつけの状態が所定の状態でない場合に、前記複数の把持候補対象物体が存在する領域以外の領域の中から、前記把持対象物体を戻す領域を決定する領域決定工程とを備え、
    前記制御工程では、前記ロボットを制御して、前記把持部が把持する把持対象物体を前記決定された領域内に戻させることを特徴とする情報処理方法。
17. コンピュータを、請求項１乃至１１の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images