JP5854815B2 - 情報処理装置、情報処理装置の制御方法、およびプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理装置の制御方法、およびプログラムに関する。

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

特許文献１では、まずパレットの上方に設置されたカメラにより複数の対象物体全体を撮影し、撮影した画像上での２次元位置を求める。次に、カメラから１つの対象物体までの直線を求め、ロボットの手先部に搭載したセンサがその直線上に来るようにロボットを動かす。そしてセンサによって対象物体の位置姿勢計測を行う。このように全体を撮影するカメラと、計測範囲は狭いが精度よく位置姿勢を検出することができるセンサとを組み合わせて段階的に処理することで、対象物体を高精度かつ高速に計測している。

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

Ｋ．Ｓ．Ａｒｕｎ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｉｎ，"Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ ３−Ｄ Ｐｏｉｎｔ Ｓｅｔｓ，" ＰＡＭＩ，ｖｏｌ．９，ｎｏ．５，１９８７

しかしながら、特許文献１による方法では、必ずパレットの上方に設置されたカメラと対象物体とを結ぶ直線上でロボットの手先部に搭載したセンサによる計測を行うことが決まっている。そのため対象物体が向いている方向によっては必ずしも計測しやすい状態にはならない。例えば対象物体には計測するための照明光を当てるが、対象物体が向いている方向によっては照明光が対象物体に反射してカメラに映り込んでしまう。想定外の照明反射光がカメラに映り込むと画像処理を行う際の妨げになるため位置姿勢の計測が困難になる。あるいは対象物体を計測する方向から対象物体の位置姿勢を特定する特徴部分が見えにくいように対象物体が置かれている場合は、精度よく位置姿勢を求めることが困難になる。

上記の課題に鑑み、本発明は、対象物体がどのような方向を向いていても高精度かつ高速かつロバストに対象物体を計測することを目的とする。

上記の目的を達成する本発明に係る情報処理装置は、
対象物体に関する２次元情報もしくは３次元情報を第１の情報として取得する第１のセンサ手段と、
前記第１の情報を解析して前記対象物体の位置姿勢を計測する第１の計測手段と、
前記対象物体に対して作業を実行するロボットに装着されており、前記第１の計測手段による計測結果に応じて決定される位置姿勢で、前記対象物体に関する２次元情報もしくは３次元情報を第２の情報として取得する第２のセンサ手段と、
前記第２の情報を解析して前記対象物体の位置姿勢を計測する第２の計測手段と、
を備え、
前記第１の計測手段による計測結果に応じて、前記第２のセンサ手段が前記対象物体を撮影する際に前記対象物体へ投影される投影光の正反射光が、前記第２のセンサ手段の撮影範囲に入らない位置姿勢を、前記第２のセンサ手段の位置姿勢として決定する決定手段をさらに備えることを特徴とする。

本発明によれば、対象物体がどのような方向を向いていても高精度かつ高速かつロバストに対象物体を計測することができる。

第１実施形態における情報処理装置の構成を示す図。 第１実施形態における３次元形状モデルを説明する図。 第１実施形態における参照画像モデル生成を説明する図。 第１実施形態における情報処理装置の処理手順を示すフローチャート。 画像からのエッジ検出を説明する図。 線分の投影像と検出されたエッジの関係を説明する図。 センサ部により撮影した画像を説明する図。 センサ部と対象物体との関係を説明する図。 センサ部により撮影した画像を説明する図。 第２実施形態における３次元形状モデルを説明する図。

（第１実施形態）
第１実施形態では、対象物体に関する２次元情報（２次元画像）を取得する第１のセンサ部（カメラ）と、ロボットに装着されており対象物体の３次元情報を取得するための第２のセンサ部（プロジェクタとカメラ）とを用いて対象物体の３次元位置姿勢計測を行う。具体的には、第１のセンサ部を用いて計測した結果に基づいて、計測を行うための第２のセンサ部のプロジェクタ照明反射光が第２のセンサ部のカメラに映り込まないように、ロボットに装着された第２のセンサ部の計測位置姿勢を設定する。カメラと対象物体との相対的な姿勢関係によっては想定外のプロジェクタ照明反射光がカメラに映り込み、画像処理の精度が低下する。画像処理の精度が低下すると３次元位置姿勢計測の精度も低下してしまう。そこで本実施形態では、３次元位置姿勢計測を精度よく行うために、プロジェクタ照明反射光がカメラに映り込まない位置姿勢で第２のセンサ部による計測を行う。

図１を参照して、第１実施形態における情報処理装置の構成を説明する。同情報処理装置は、ロボット１００と、第１のセンサ部１０１と、第２のセンサ部１０２と、画像処理部１１０と、ロボットコントローラ部１２０とを備える。画像処理部１１０は、第１のセンサ情報取得部１１１と、モデル情報保持部１１２と、第１の３次元位置姿勢計測部１１３と、センサ位置姿勢決定部１１４と、第２のセンサ情報取得部１１５と、第２の３次元位置姿勢計測部１１６とを備える。ロボットコントローラ部１２０は、ロボット作業指示部１２１と、ロボット制御部１２２とを備える。これらの構成によりパレット１０４内に山積みした対象物体１０３の三次元位置姿勢の計測を行い、その計測結果に基づいてロボット１００によるピッキング作業を行う。

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

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

センサ部１０１は、２次元画像（第１の画像）を撮影するカメラである。センサ部１０１は、パレット１０４の上方において第１の位置姿勢で固定的に配置され、山積みされた対象物体１０３の画像を撮影し、第１のセンサ情報取得部１１１に対して出力を行う。本実施形態では、センサ部１０１により撮影した画像は画像処理部１１０が処理するが、センサ部１０１内に画像処理機構を持たせて画像処理した結果を出力してもよい。

センサ部１０１によって対象物体１０３の撮影を行う際には、対象物体１０３に対して不図示の照明による照明光（投影光）を当てる。この照明はセンサ部１０１の周りに配置されており、センサ部１０１が対象物体１０３の２次元画像を撮影するときに均一の照明環境下で撮影可能な状態にする。

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

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

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

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

次に画像処理部１１０内の各構成について説明する。

第１のセンサ情報取得部１１１は、センサ部１０１が撮影した２次元画像を取得する。取得した２次元画像を３次元位置姿勢計測部１１３に出力する。

モデル情報保持部１１２は、３次元位置姿勢計測部１１３および３次元位置姿勢計測部１１６で対象物体１０３の位置姿勢を計測するために利用されるモデル情報を保持する。モデル情報の１つとして、３次元ＣＡＤに基づく３次元幾何モデル情報があげられる。

３次元幾何モデルは、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換したものである。本実施形態ではポリゴン要素を利用した場合について説明を行う。３次元幾何モデルは、図２に示すような点、線、面といった構成要素からなる。図２（ａ）乃至図２（ｃ）はいずれも同じ３次元幾何モデルである。図２（ａ）および図２（ｄ）は３次元幾何モデルの各頂点を示す。図２（ｂ）および図２（ｅ）は３次元幾何モデルの各辺となる線を示す。図２（ｃ）および図２（ｆ）は３次元幾何モデルの各面を示す。３次元幾何モデルには、図２（ｆ）に示すように３次元幾何モデルを構成する面の法線データが含まれている。

もう１つのモデル情報として、実際の対象物体１０３もしくは対象物体１０３を模した３次元幾何モデルを予め定められた複数の視点から観察した参照画像情報があげられる。

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

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

３次元位置姿勢計測部１１３は、第１のセンサ情報取得部１１１から出力された２次元画像を解析したデータと、モデル情報保持部１１２が保持しているモデル情報とを利用して対象物体１０３の位置姿勢を求める（第１の計測処理）。求めた位置姿勢情報をセンサ位置姿勢決定部１１４に出力する。

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

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

センサ位置姿勢決定部１１４は、３次元位置姿勢計測部１１３により求めた対象物体１０３の位置姿勢に基づいて、第２のセンサ部１０２が対象物体１０３を計測する際の第２のセンサ部１０２の位置姿勢である第２の位置姿勢を求める。求めた位置姿勢情報をロボット作業指示部１２１に出力する。センサ部１０２が対象物体１０３を計測する際のセンサ部１０２の位置姿勢を求める方法については後述する。

第２のセンサ情報取得部１１５は、センサ部１０２が撮影した２次元画像を取得する。センサ部１０２にはパターン光を照射する小型プロジェクタが備えられており、第２のセンサ情報取得部１１５はパターン光が照射された対象物体１０３の画像を取得する。取得した２次元画像を３次元位置姿勢計測部１１６に出力する。

３次元位置姿勢計測部１１６は、第２のセンサ情報取得部１１５から出力された２次元画像を解析したデータと、モデル情報保持部１１２が保持しているモデル情報とを利用して対象物体１０３の位置姿勢を求める（第２の計測処理）。求めた位置姿勢情報をロボット作業指示部１２１に出力する。

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

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

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

ロボット作業指示部１２１は、センサ位置姿勢決定部１１４および３次元位置姿勢計測部１１６の計測結果に基づいて、ロボットに対する作業指示を行う。ロボット作業指示部１２１は、センサ位置姿勢決定部１１４からの出力に応じて、センサ部１０２が対象物体１０３を計測する第２の位置姿勢まで移動することを指示する。また３次元位置姿勢計測部１１６からの出力に応じて、対象物体１０３を把持や吸着できる位置にハンド１０５を移動させて把持や吸着を行う。ロボットの作業は移動、把持、吸着に限られるものではなく、対象物体１０３の外観検査など、他の作業も含むことは言うまでもない。さらに、ロボットも多関節ロボットではなくＮＣ制御可能な可動式の機械でもよいことは言うまでもない。

ロボット制御部１２２は、ロボット作業指示部１２１からの指示情報を受けて、ロボット１００の制御を行う。

図４のフローチャートを参照して、第１実施形態における情報処理装置の処理手順を説明する。

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

ステップＳ４０２において、３次元位置姿勢計測部１１３は、第１のセンサ情報取得部１１１が取得した対象物体１０３の画像から複数ある対象物体１０３のうち少なくとも１つの対象物体１０３の位置姿勢を計測する。対象物体１０３の位置姿勢を求めるために、モデル情報保持部１１２は保持しているモデル情報を出力して、３次元位置姿勢計測部１１３が対象物体１０３の画像とモデル情報とのマッチングを行う。

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

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

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

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

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

位置姿勢の概略値と実際の位置姿勢との差がそれほどないと仮定すると、正しい６次元ベクトルｓによって得られる制御点の画像上での位置は式（１）が表す直線上にあると仮定できる。式（２）によって近似されるｕ、ｖを式（１）に代入することにより、式（３）が得られる。 ただし、ｒ＝ｕ０ｓｉｎθ−ｖ０ｃｏｓθ
（定数）である。

式（３）は対応付けが行われたすべてのエッジについて立式することができる。なお、すべてのエッジについて立式せずに一部のエッジについてだけ立式してもよい。

式（３）は６次元ベクトルｓの各成分の微小変化△ｓｉ（ｉ＝１，２，・・・，６）についての方程式であるため、式（４）のような△ｓｉに関する線形連立方程式を立てることができる。

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

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

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

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

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

算出された位置姿勢の補正値△ｓにより、位置姿勢の概略値を補正する（ｓ←ｓ＋△ｓ）。

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

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

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

３次元位置姿勢計測部１１３で対象物体１０３の位置姿勢を求めたら、センサ位置姿勢決定部１１４に対象物体１０３の位置姿勢情報を出力する。

ステップＳ４０３において、センサ位置姿勢決定部１１４は、ステップＳ４０２で求めた対象物体１０３の３次元位置姿勢に基づいて、センサ部１０２で対象物体１０３を計測する第２の位置姿勢を決定する。本実施形態では、対象物体１０３の計測を行うためのプロジェクタ照明の反射光がカメラに直接的に映り込まないように、ロボット１００に装着したセンサ部１０２の位置姿勢を求める。センサ位置姿勢決定部１１４は、決定した位置姿勢をロボット作業指示部１２１に出力する。

図７（ａ）および図７（ｂ）は、センサ部により対象物体１０３を撮影した画像である。図７（ａ）はセンサ部１０１で撮影した画像である。図７（ｂ）はセンサ部１０２で撮影した画像である。図７（ａ）にはパレット１０４に山積みされた多くの対象物体１０３が写っている。ステップＳ４０２で少なくとも１つの対象物体１０３の３次元位置姿勢が求まる。ここで説明上１つの対象物体１０３’の３次元位置姿勢が求まっているものとする。もちろん複数の対象物体１０３の３次元位置姿勢が求まっていて、そのうちの１つを対象物体１０３’として選択してもよいことは言うまでもない。図７（ａ）および図７（ｂ）において対象物体１０３’を点線で囲っている。これらの点線はあくまで対象物体１０３’がどれであるかを分かりやすく示すための点線であり、センサ部の撮影画像には映っていない。

図７（ｂ）は対象物体１０３’の詳細な３次元位置姿勢を計測するためにセンサ部１０２が撮影した画像である。図７（ｂ）の画像はセンサ部１０１と対象物体１０３’とを直線で結んだ直線上にセンサ部１０２を配置して撮影した画像とする。すなわち従来技術である特許文献１に記載の方法によって撮影位置を決めているものとする。センサ部１０２の計測範囲はセンサ部１０１の計測範囲と比較して狭い。そのためセンサ部１０１による画像よりもセンサ部１０２による画像のほうが画面に占める対象物体１０３’の面積が大きい。詳細な３次元位置姿勢を計測するために本実施形態では空間符号化法を適用する。センサ部１０２に備わっている小型プロジェクタで幅の異なる複数の縞模様のパターン光を投射して小型カメラにより画像を取得する。図７（ｂ）では対象物体１０３’に縞模様のパターン光が投射されていることが分かる。縞模様パターンの縞模様は、対象物体の形状に合うようにその模様が変化し、変化したパターンが撮影される。画像は縞模様パターンの縞の幅を変えて複数枚撮影する。

対象物体１０３’には、近い位置からセンサ部１０２のプロジェクタによる照明光が当たる。図７（ｂ）のように対象物体１０３’のある面が、センサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向に対して垂直に近い状態にあると、照明光が対象物体１０３’の当該面で正反射し、カメラに照明反射光が直接写り込む。より正確には、カメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’の当該面の法線方向とがほぼ同じ方向のときに、プロジェクタ照明光が正反射して反射光がカメラに直接映り込む。そのため対象物体１０３’の当該面の画像部分は輝度が飽和状態となり画像に白とびが生じる。空間符号化法により３次元計測を行うには、縞模様パターンの黒い部分および白い部分に対応する領域の境界位置を精度よく求める必要がある。しかし対象物体１０３’が存在する画像領域で白とびが発生すると、白い領域が膨張して撮影されるため境界位置がずれて検出される。境界位置を精度よく求めることができなくなるので位置姿勢を精度よく求めることが難しくなる。

また、従来技術である特許文献１ではロボットの手先部に搭載したセンサとしてラインセンサを用いている。ラインセンサを用いた場合でも、ラインパターン光が対象物体の面で正反射し、センサに反射光が直接写り込むと、正反射したラインの画像部分は膨張する。そのためライン位置を精度よく求めることが難しい。ライン位置を精度よく求めることができなくなるので位置姿勢を精度よく求めることが難しくなる。

センサ位置姿勢決定部１１４は、ステップＳ４０３において、センサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’を構成する面の法線との関係に基づいて、センサ部１０２で撮影する第２の位置姿勢を決定する。図８（ａ）および図８（ｂ）は、センサ部１０２のプロジェクタと、カメラと、対象物体１０３’を構成する面との関係を説明する図である。図８（ａ）は、カメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’の当該面の法線方向とがほぼ同じ方向になっている状態である。プロジェクタの光軸とカメラの光軸とが対象物体１０３’の面で正反射する形になっている。このような状態になると、プロジェクタのパターン光が対象物体１０３’の面で正反射してカメラに写り込む。結果として白とびが生じて画像処理により対象物体１０３’の３次元位置姿勢を検出することが難しくなる。なお、プロジェクタの光軸とカメラの光軸がセンサ部１０２の姿勢に対してわずかながら斜め方向なのは、プロジェクタの照射範囲と、カメラの撮影範囲とをなるべく重複させるためである。カメラの光軸とプロジェクタの光軸はそれぞれ図８に示す方向でなくてもよいことは言うまでもない。カメラの光軸が真下方向で、プロジェクタの光軸が反時計回り方向にさらに傾いた方向であってもよい。

一方、図８（ｂ）では、対象物体１０３’の面がセンサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向に対して角度をつけて斜めに傾いている。そのためプロジェクタの光が正反射してカメラに映り込むことはない。より正確には、カメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’の当該面の法線方向とがずれているため、プロジェクタの光が正反射してカメラに写り込むことはない。結果として画像に白とびが生じることを抑え、画像処理により対象物体１０３’の３次元位置姿勢を検出することが容易になる。

センサ部１０２はロボットに装着されているため、センサ部１０２の位置姿勢はロボットの制御情報から求めることができる。また対象物体１０３’の位置姿勢はステップＳ４０２で求まっており、かつモデルとして３次元幾何モデルを用いている場合は対象物体の面の法線も図２で説明した通り分かっている。ロボット制御情報と、ステップＳ４０２で求めた対象物体１０３’の位置姿勢と、対象物体１０３’の面の法線情報とを用いて、センサ部１０２のプロジェクタの光が正反射してカメラに映り込まないようにする。このときの姿勢をセンサ部１０２で計測を行う第２の位置姿勢の姿勢とする。具体的には以下の方法Ａ、方法Ｂ、方法Ｃのいずれかの方法で実現する。

＜方法Ａ＞
センサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向をまず仮に鉛直下向きにして対象物体１０３’が存在する領域を撮影する。画像中の対象物体１０３’部分に白とびが発生していなければ、プロジェクタの照明が正反射してカメラに映り込んでいないと判断できる。白とびが発生しているかどうかの判定は画像の輝度を調べれば分かる。白とびが発生していなければそのときに対象物体１０３’を撮影しているセンサ部１０２の位置姿勢を第２の位置姿勢とする。画像中の対象物体１０３’部分に白とびが発生していればセンサ部１０２の姿勢を、対象物体１０３’の位置を中心にしてカメラおよびプロジェクタの光軸を含む面内で予め決めた角度（例えば５度）回転させる。白とびが発生しなくなったらそのときに対象物体１０３’を撮影しているセンサ部１０２の位置姿勢を第２の位置姿勢とする。回転させても白とびが発生しているときは、白とびが発生しなくなるまでさらに予め決めた角度回転させる。なお、センサ部１０２の姿勢回転は、カメラおよびプロジェクタの光軸を含む面に垂直な面内で回転させてもよいし、任意の向きに回転させてもよい。方法Ａでは実際に撮影した画像に白とびが発生したかどうか判定するため、白とびが発生しない第２の位置姿勢を確実に決めることができる。

＜方法Ｂ＞
センサ部１０２の位置姿勢をシミュレーションする。センサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向を鉛直下向きにして、対象物体１０３’が存在する領域を撮影する位置にセンサ部１０２を仮想的に配置する。対象物体１０３’の位置を中心にカメラおよびプロジェクタの光軸を含む面内で予め決めた角度（例えば５度）ずつセンサ部１０２を回転させる。カメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’の面の法線方向とが所定の角度以上（例えば１０度以上）ずれている位置を求め、その位置姿勢を第２の位置姿勢とする。第２の位置姿勢候補が複数あるときはセンサ部１０２の姿勢が鉛直下向きに近い位置姿勢を第２の位置姿勢とする。センサ部１０２の姿勢回転は、カメラおよびプロジェクタの光軸を含む面に垂直な面内で回転させてもよいし、任意の向きに回転させてもよい。方法Ｂではシミュレーションを行うため、ロボットを実際に動かして計測を繰り返すといった時間をかけずに第２の位置姿勢を決定することができる。

＜方法Ｃ＞
３次元幾何モデルや参照画像モデルに予め計測するのに適切なセンサ部１０２の位置姿勢を登録しておいてもよい。位置姿勢は複数登録してもよいし、範囲を定めて登録してもよい。現在のセンサ部１０２の位置姿勢から最も近い登録済みの位置姿勢を第２の位置姿勢とする。

なお方法Ａ〜方法Ｃのいずれの場合においても、センサ部１０２と対象物体１０３’との間の距離は、センサ部１０２のカメラの焦点が合う距離範囲に基づいて決定する。カメラの焦点が合う距離範囲は、カメラの最短撮影距離と被写界深度とによって決まる。よってセンサ部１０２の位置は対象物体１０３’を中心とした一定距離範囲内となる。

３次元幾何モデルは複数の面を持っているため、全ての面について白とびが発生しないようにセンサ部１０２で撮影する位置を決定するとよい。しかし３次元幾何モデルの形状によっては、全ての面について白とびが発生しないようにすることが難しい場合もある。その場合は、なるべく多くの複数の面で角度差が所定の閾値以上になるようにしてもよい。また、面積が小さい面は最終的な結果に与える影響がそれほど大きくないため、面積が小さい面については無視してもよい。計算量が減るためセンサ部１０２が計測を行う第２の位置姿勢を決定する速度が向上する。また、対象物体１０３’のうち、予めロボットハンドが把持もしくは吸着する部分を指定しておき、その付近以外の面は無視してもよい。ロボットハンドが把持もしくは吸着する周辺の位置姿勢を特に精度よく求めることで、ロボットによるピッキング作業をより精度よく行うことができる。

また、センサ部１０２が対象物体１０３’を撮影する第２の位置姿勢は、３次元空間中に１箇所しかないわけではない。空間的な範囲で決められるため、その範囲の中で１箇所を選んで撮影すればよい。あるいは後に述べる方法を同時に適用して決まる位置姿勢を第２の位置姿勢として選んでもよい。

モデルとして参照画像モデルを用いる場合は、それぞれの参照画像モデルについて対象物体１０３’の面の角度情報を関連付けておいて、センサ部１０２の位置姿勢と参照画像モデルに関連付けた角度情報とに基づいてセンサ部１０２で撮影する位置姿勢を決定する。角度情報の関連付けは、ＣＡＤデータをモデル情報保持部１１２に読み込む事前準備の過程で事前準備のプログラムに行わせればよい。

図９は、図７（ｂ）とほぼ同じ領域を、センサ部１０２を数度右側に回転した位置姿勢から撮影した画像である。撮影位置姿勢は、本実施形態で説明したように、カメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３’の面の法線方向とが所定の角度以上ずれている位置を求めることで決定した。対象物体１０３’が白とびせずに写っているため、縞模様パターンの黒い部分および白い部分に対応する領域の境界位置を精度よく求めることができる。境界位置を精度よく求めることができるため、対象物体１０３’の位置姿勢を精度よく求めることが容易になる。

なお対象物体の形状に基づいて第２の位置姿勢は変わるため、第２の位置姿勢は対象物体の種類によって変わる。

ステップＳ４０４において、ロボット制御部１２２は、ステップＳ４０３で決定した第２の位置姿勢でセンサ部１０２が対象物体１０３’を撮影するようにロボット１００を姿勢制御する。ステップＳ４０４では、まずステップＳ４０３で決定した位置姿勢情報をロボット作業指示部１２１がセンサ位置姿勢決定部１１４から受け取る。ロボット作業指示部１２１は、センサ位置姿勢決定部１１４が出力した情報に基づいて、センサ部１０２が対象物体１０３’を撮影するようにロボット１００を動かすことを指示する。ロボット制御部１２２は、ロボット作業指示部１２１による指示に基づいてロボット１００を制御する。

ステップＳ４０５において、ロボット１００に装着したセンサ部１０２は、対象物体１０３’の画像を撮影する。撮影した画像は第２のセンサ情報取得部１１５に出力される。

ステップＳ４０６において、３次元位置姿勢計測部１１６は、第２のセンサ情報取得部１１５が取得した対象物体１０３’の画像を元に対象物体１０３’の位置姿勢を求める。対象物体１０３’の位置姿勢を求めるために、モデル情報保持部１１２は保持しているモデル情報を出力して、３次元位置姿勢計測部１１６が対象物体１０３’とモデル情報とのマッチングを行う。

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

３次元幾何モデルの表面点群をＰとし、距離画像点群をＡとすると、ＰおよびＡはそれぞれ式（１１）および式（１２）のように表すことができる。

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

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

誤差関数Ｅを小さくするＲとｔを求める方法については、非特許文献１に記載の方法を用いればよい。

Ｐの収束判定を行い、収束していれば終了し、そうでなければ繰り返し補正計算を行う。収束判定では、Ｐがほとんど変化しないことをもって収束したと判定する。このように収束するまで繰り返すことで、位置姿勢を算出できる。

モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより最も一致する参照画像を求め、その参照画像に対応付けられた対象物体との位置姿勢情報に基づいて対象物体の位置姿勢を求める。参照画像の距離値をＴ（ｉ，ｊ）、パターン画像から求めた距離画像の距離値をＩ（ｉ，ｊ）として、参照画像をｍ×ｎ画素の画像としたとき、一致度は式（１５）により求めることができる。

３次元位置姿勢計測部１１６で対象物体１０３’の位置姿勢を求めたら、ロボット作業指示部１２１に対象物体１０３’の位置姿勢情報を出力する。なお、ステップＳ４０３で決定した第２の位置姿勢で撮影を行うため、パターン画像は図９のように白とびしていない画像となっている。そのため対象物体１０３’の位置姿勢を精度よく求めることができる。

ステップＳ４０７において、ロボット作業指示部１２１は、３次元位置姿勢計測部１１６から受け取った対象物体１０３’の位置姿勢情報に基づいて、ロボット１００に対する作業指示をロボット制御部１２２へ行う。ロボット１００が対象物体１０３’を把持するハンドを備えていれば対象物体１０３’の把持を指示する。ロボット１００が対象物体１０３’を吸着するパッドを備えていれば対象物体１０３’の吸着を指示する。 ステップＳ４０８において、ロボット制御部１２２は、ステップＳ４０７でロボット作業指示部１２１により指示された指示内容を実行するようにロボット１００に対して制御を行う。ロボット１００が対象物体１０３’を把持する場合は、ロボット制御部１２２がロボット１００に対して対象物体１０３’を把持する制御を行う。ロボット１００が対象物体１０３’を吸着する場合は、ロボット制御部１２２がロボット１００に対して対象物体１０３’を吸着する制御を行う。

ステップＳ４０９において、ロボット制御部１２２は、ユーザから終了指示があったかどうかを判定する。ユーザからの終了指示があったと判定された場合（Ｓ４０９；ＹＥＳ）、処理を終了する。一方、ユーザからの終了指示がないと判定された場合（Ｓ４０９；ＮＯ）、ステップＳ４０１へ戻る。なお、ステップＳ４０９での終了判定を待たずに、ユーザが不図示の緊急停止ボタンを押すことによりフローを終了して全ての動作を停止してもよい。

以上述べたように、第１実施形態では、対象物体に関する２次元情報（２次元画像）を取得する第１のセンサ部（カメラ）と、ロボットに装着され対象物体の３次元情報を取得するための第２のセンサ部（プロジェクタとカメラ）とを用いて対象物体の３次元位置姿勢計測を行う。第１のセンサ部を用いて計測した結果に基づいて、計測を行うための第２のセンサ部のプロジェクタ照明反射光が第２のセンサ部のカメラに映り込まないように、ロボットに装着された第２のセンサ部の計測位置姿勢を求める。これにより、対象物体がどのような方向を向いていても高精度かつ高速かつロバストに計測を行うことができる。

[変形例]
第１実施形態の変形例として、センサ部１０１は対象物体に関する３次元情報（距離画像や３次元点群データ）を取得するセンサ部（距離画像センサや３次元点群計測センサ）であってもよい。また、センサ部１０２も対象物体に関する３次元情報（距離画像や３次元点群データ）を取得するセンサ部（距離画像センサや３次元点群計測センサ）であってもよい。距離画像を取得するセンサ部としては、第２のセンサ部で利用するものと同様なプロジェクタとカメラからなる距離画像センサや、各画素までの奥行きを光伝播時間によって計測するＴＯＦ型の距離画像センサなどを用いることができる。また、２次元配列に並ぶ画像状の距離データになっていなくとも、まばらな点群として計測されている３次元点群データであってもよい。第１のセンサ情報取得部１１１および第２のセンサ情報取得部１１５は、それぞれセンサ部１０１、センサ部１０２から３次元情報（距離画像や３次元点群データ）を取得し、３次元位置姿勢計測部１１３、１１６に出力する。３次元位置姿勢計測部１１３は、第１のセンサ情報取得部１１１から出力された３次元情報（距離画像や３次元点群データ）とモデル情報保持部１１２から出力される３次元幾何モデルから抽出した表面の点群データとの対応付けを行うことで対象物体の位置姿勢を求める。３次元位置姿勢計測部１１６は、第２のセンサ情報取得部１１５から出力された３次元情報（距離画像や３次元点群データ）とモデル情報保持部１１２から出力される３次元幾何モデルから抽出した表面の点群データとの対応付けを行うことで対象物体の位置姿勢を求める。３次元情報（距離画像や３次元点群データ）とモデル情報とを利用して対象物体の位置姿勢を求めるために、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。対象物体の位置姿勢を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための最適化手法はＩＣＰ法に限るものではない。

また、第１実施形態の変形例として、センサ部１０１は対象物体に関する２次元情報（２次元画像）もしくは３次元情報（距離画像や３次元点群データ）あるいは両者を第１の情報として取得するセンサ部（カメラと距離センサとの組み合わせ）であってもよい。また、センサ部１０２も対象物体に関する２次元情報（２次元画像）もしくは３次元情報（距離画像や３次元点群データ）あるいは両者を第２の情報として取得するセンサ部（カメラと距離センサとの組み合わせ）であってもよい。２次元画像とモデル情報との対応付け、および距離データとモデル情報との対応付けを同時に解く方法として特許文献２に記載の方法を適用することができる。

また、第１実施形態の変形例として、センサ部１０２のプロジェクタはパターン光以外に全面を均一の輝度で照射することもできる。均一の輝度で照射することで通常の照明のように扱うことができる。この場合は、センサ部１０２の小型プロジェクタで均一輝度の光を対象物体に対して照明し、センサ部１０２の小型カメラにより２次元情報（２次元画像）を取得し、第２のセンサ情報取得部１１５に対して出力を行う。またセンサ部１０２は対象物体を均一の明るさで照らす照明と２次元画像を撮影するカメラでもよい。第２のセンサ情報取得部１１５は２次元画像を取得し、３次元位置姿勢計測部１１６に出力する。３次元位置姿勢計測部１１６は、２次元画像とモデル情報保持部１１２から出力されたモデル情報とを利用して対象物体の位置姿勢を計測する。位置姿勢を計測する方法は第１の実施形態における３次元位置姿勢計測部１１３、ステップＳ４０２と同様でよい。

（第２実施形態）
第１実施形態では、第１のセンサ部を用いて計測した結果に基づいて、計測を行うための第２のセンサ部のプロジェクタ照明反射光が第２のセンサ部のカメラに映り込まないように、ロボットに装着された第２のセンサ部の計測位置姿勢を設定した。第２実施形態では、第１のセンサ部を用いて計測した結果に基づいて、対象物体の特徴部分を計測可能にするように、ロボットに装着された第２のセンサ部の位置姿勢を設定する。

第２実施形態における情報処理装置を構成する各処理部は、基本的に図１に示す第１実施形態と同じ構成である。ただし、対象物体１０３、モデル情報保持部１１２が保持するモデル情報、およびセンサ位置姿勢決定部１１４の処理内容が異なる。

第２実施形態における情報処理装置の処理手順を示すフローチャートは、基本的に図４に示す第１の実施形態と同じ構成である。ただし、対象物体１０３、センサ部１０２で対象物体を撮影する位置姿勢を決定するステップＳ４０３の処理内容が異なる。

本実施形態では、第１実施形態との差分構成のみ説明を行う。その他の構成は第１実施形態と同じものとする。

第２実施形態では図１の対象物体１０３の代わりに図１０（ａ）に示す対象物体１０３−２の計測を行う。図１０（ａ）は、対象物体１０３−２がパレット１０４に山積みされている様子を示す。図１０（ｂ）は、１つの対象物体１０３−２の形状を示す。対象物体１０３−２は、直方体形状の上に特徴的な出っ張り部を特徴１００１として有している。特徴１００１は、例えば対象物体１０３−２を製品に組み付けるときのガイドとして利用されるものである。ここで述べる「特徴的」というのは、対象物体１０３−２の３次元位置姿勢を精度よく決定するのに寄与する情報が得られるという意味で用いている。

図１０（ｃ）は、対象物体１０３−２のＣＡＤデータに基づいた３次元幾何モデルである。図２に示した３次元幾何モデルと同様に、点、線、面の情報を持っている。対象物体１０３−２は、特徴１００１に対応するモデル特徴１００２を持っている。

第２実施形態では、３次元位置姿勢計測部１１３により計測したある対象物体１０３−２’の位置姿勢に基づき、センサ位置姿勢決定部１１４は、センサ部１０２が計測を行う際の位置姿勢を決定する。３次元位置姿勢計測部１１３で予め対象物体１０３−２’の位置姿勢を求めているため、対象物体１０３−２’に３次元幾何モデルをある程度の精度で位置合わせできている。センサ位置姿勢決定部１１４は、センサ部１０２によりモデル特徴１００２に対応する特徴１００１を計測可能な位置姿勢を第２の位置姿勢として決定する。特徴１００１を計測可能な第２の位置姿勢でセンサ部１０２による位置姿勢計測を行うことで、モデルの特徴的な部分の位置合わせができるので、精度よく対象物体の位置姿勢を求めることができる。

特徴１００１を計測可能な第２の位置姿勢を求めるには、例えば以下の（１）〜（３）のいずれかの方法で実現する。

（１）モデル特徴１００２の少なくとも１面を観察することが可能となる位置姿勢
センサ部１０２で対象物体１０３−２’の特徴１００１を３次元的に計測可能にするには、モデル特徴１００２の少なくとも１面を観察できる位置姿勢を第２の位置姿勢として決定すればよい。ただし、より高精度に対象物体の位置姿勢を求めようとしたら３面以上観察できる位置であることが望ましい。

このような第２の位置姿勢を求めるには、まずセンサ部１０２のカメラの光軸方向を仮に鉛直下向きにして対象物体１０３−２’が存在する領域を撮影する。モデル特徴１００２の少なくとも１面を観察できていればそのときに対象物体１０３−２’を撮影しているセンサ部１０２の位置姿勢を第２の位置姿勢とする。モデル特徴１００２の面を観察できていなければセンサ部１０２の姿勢を、対象物体１０３−２’の位置を中心にしてカメラおよびプロジェクタの光軸を含む面内で予め決めた角度（例えば５度）回転させる。モデル特徴１００２の少なくとも１面を観察できたらそのときに対象物体１０３−２’を撮影しているセンサ部１０２の位置姿勢を第２の位置姿勢とする。なお、センサ部１０２の姿勢回転は、カメラおよびプロジェクタの光軸を含む面に垂直な面内で回転させてもよいし、任意の向きに回転させてもよい。予め決めた回数だけ回転させてもモデル特徴１００２の面を観察できない時は対象物体１０３−２’の計測は一旦止めて、それ以外の対象物体を対象にしてもよいし、パレットを揺動させて対象物体の山積み状態を変化させてもよい。（１）の方法では対象物体１０３−２’の面が観察できるかどうかを、３次元幾何モデルを介して判定するため、特徴１００１を計測可能な第２の位置姿勢を確実に決めることができる。

なお、モデル特徴を手掛かりに第２の位置姿勢を決定するのは（上でも説明したとおり）対象物体１０３−２’に３次元幾何モデルをある程度の精度で位置合わせできているという前提があるためである。

（２）モデル特徴１００２の所定面に対してセンサ部１０２のカメラ光軸方向が９０度未満となる位置姿勢
センサ部１０２で対象物体１０３−２’の特徴１００１を３次元的に計測可能にするには、例えば図１０（ｃ）におけるモデル特徴１００２の上面法線方向と、センサ部１０２のカメラ光軸方向との相対角度が、９０度未満になるようにすればよい。相対角度を９０度未満にすることで、特徴１００１の少なくとも１面を計測することが可能になるので特徴１００１を３次元的に計測可能になる。（２）の方法でも（１）と同様にまず仮にセンサ部１０２のカメラ光軸方向を鉛直下向きにして計測を行い、条件を満たさなければ所定の角度ずつ回転させていけばよい。

なお、第１実施形態では相対角度を所定の角度以上にするよう第２の位置姿勢を求めていた。第１実施形態での範囲と第２の実施形態での範囲との共通範囲を求めて第２の位置姿勢を決定してもよい。

（３）３次元幾何モデルや参照画像モデルに予め計測するのに適切なセンサ部１０２の位置姿勢を登録しておいてもよい。位置姿勢は複数登録してもよいし、範囲で登録してもよい。現在のセンサ部１０２の位置姿勢から最も近い登録済みの位置姿勢を第２の位置姿勢とする。

なお、３次元幾何モデルのモデル特徴は図１０（ｃ）に示すような立体的な特徴だけでなく、２次元的な幾何特徴（線や円弧の組み合わせ、テクスチャなど）や、１次元的な幾何特徴（線、円弧）や、点特徴でもよい。

また、参照画像モデルを利用する場合は、３次元位置姿勢計測部１１３で計測した対象物体１０３−２’の位置姿勢に基づき、特徴部分をよりよく観察することができる相対位置姿勢の参照画像を選択して該参照画像を作成したときの位置姿勢を第２の位置姿勢とすればよい。

（第３実施形態）
第１の実施形態、第２の実施形態の変形として以下の実施形態を考えることができる。センサ部１０１とセンサ部１０２とを同じセンサ部で構成してもよい。具体的にはロボットに装着したセンサ部１０２にセンサ部１０１の役割も持たせることで実現する。まずセンサ部１０２がパレット１０４の上方の第１の位置姿勢で対象物体１０３を撮影するようロボットを制御する。センサ部１０２が第１の位置姿勢で対象物体１０３を撮影して第１の３次元位置姿勢計測部１１３で位置姿勢を計測したら、次に対象物体１０３を精度よく計測可能な第２の位置姿勢を決定する。第２の位置姿勢を決定する方法は第１実施形態、第２実施形態に記載した方法によって求める。ロボットを制御することでセンサ部１０２を第２の位置姿勢に移動したら、対象物体１０３の撮影を行い第２の３次元位置姿勢計測部１１６で位置姿勢の計測を行う。最後にロボット１００のエンドエフェクタにより対象物体１０３を把持もしくは吸着する。

また、対象物体１０３が鏡面反射成分を持つ物体である場合は、センサ部１０２のカメラの光軸方向とプロジェクタの光軸方向とがなす角の２等分方向と、対象物体１０３の面の法線方向とに基づいて反射が生じにくい位置を第２の位置姿勢として決定してもよい。

また、対象物体１０３が曲面を含む物体である場合は、対象物体１０３を模した３次元幾何モデルを画像面に投影したとき、曲面部（フィレット部）がエッジとして見える視点位置姿勢を極力第２の位置姿勢とするのを避けてもよい。なぜなら曲面部は視点位置姿勢によってエッジの位置が不定になり、位置姿勢計測の精度が下がるためである。曲面部がエッジとして見える視点位置姿勢であるかどうかは、例えばカメラの光軸と対象物体の面の法線が９０度近くになる面があるかどうかで判定する。曲面部がエッジとして見える視点位置姿勢を避けることで位置姿勢計測の精度をよくすることができる。

また、第２の位置姿勢で位置姿勢計測して計測が失敗した場合（例えば、計測結果が得られなかった場合）や、第２の位置姿勢で計測した後に行うロボット作業で失敗した場合（例えば、対象物体を把持、吸着できなかった場合）は、その後似たような状況で失敗を繰り返す可能性がある。そこで失敗したとき計測結果や作業結果について対象物体に対する相対的な位置姿勢を記憶しておき、後の処理ではその位置姿勢を除外するようにしてもよい。失敗した時の状況を再現させないことでロバスト性を向上させることができる。

また、ロボットによる作業として把持や吸着の例を挙げたが、把持を行う場合は対象物体を把持する箇所（ｎ本指のハンドであればｎ箇所のスペース）の位置姿勢を特に精度よく求めるようにしてもよい。吸着を行う場合は対象物体を吸着する箇所（安定的な面）の位置姿勢を特に精度よく求めるようにしてもよい。把持する箇所、吸着する箇所の周囲に存在する特徴的な箇所を利用して第２実施形態で述べた方法を利用する。

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

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

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

Claims (10)

1. 対象物体に関する２次元情報もしくは３次元情報を第１の情報として取得する第１のセンサ手段と、
   前記第１の情報を解析して前記対象物体の位置姿勢を計測する第１の計測手段と、
   前記対象物体に対して作業を実行するロボットに装着されており、前記第１の計測手段による計測結果に応じて決定される位置姿勢で、前記対象物体に関する２次元情報もしくは３次元情報を第２の情報として取得する第２のセンサ手段と、
   前記第２の情報を解析して前記対象物体の位置姿勢を計測する第２の計測手段と、
   を備え、
   前記第１の計測手段による計測結果に応じて、前記第２のセンサ手段が前記対象物体を撮影する際に前記対象物体へ投影される投影光の正反射光が、前記第２のセンサ手段の撮影範囲に入らない位置姿勢を、前記第２のセンサ手段の位置姿勢として決定する決定手段をさらに備えることを特徴とする情報処理装置。
2. 前記対象物体のモデル情報を保持するモデル情報保持手段をさらに備え、
   前記第１の計測手段は、前記第１の情報と前記モデル情報とをマッチングさせることにより、前記対象物体の位置姿勢を計測することを特徴とする請求項１に記載の情報処理装置。
3. 前記対象物体のモデル情報を保持するモデル情報保持手段をさらに備え、
   前記第２の計測手段は、前記第２の情報と前記モデル情報とをマッチングさせることにより、前記対象物体の位置姿勢を計測することを特徴とする請求項１に記載の情報処理装置。
4. 前記モデル情報は、ＣＡＤデータに基づく３次元幾何モデル情報であることを特徴とする請求項２または３に記載の情報処理装置。
5. 前記モデル情報は、前記対象物体もしくはＣＡＤデータに基づく３次元幾何モデルを予め定められた複数の視点から観察した複数の参照画像情報であることを特徴とする請求項２または３に記載の情報処理装置。
6. 前記第２のセンサ手段の位置姿勢は、前記対象物体が複数種類ある場合、前記対象物体のそれぞれの種類に応じて決定されることを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
7. 前記第２の計測手段により計測された前記対象物体の位置姿勢に基づいて、前記ロボットの前記対象物体に対する作業を制御するロボット制御手段をさらに備えることを特徴とする請求項１乃至６の何れか１項に記載の情報処理装置。
8. 前記第２の計測手段による計測結果または当該計測結果に応じて実行された前記ロボットによる作業結果を記憶する記憶手段をさらに備え、
   前記第２のセンサ手段の位置姿勢は、前記第２の計測手段による計測が失敗して計測結果が得られなかった位置姿勢または前記ロボットによる作業が失敗した位置姿勢を除外して決定されることを特徴とする請求項７に記載の情報処理装置。
9. 第１のセンサ手段と、第１の計測手段と、対象物体に対して作業を実行するロボットに装着されている第２のセンサ手段と、第２の計測手段と、決定手段とを備える情報処理装置の制御方法であって、
   前記第１のセンサ手段が、前記対象物体に関する２次元情報もしくは３次元情報を第１の情報として取得する第１のセンサ工程と、
   前記第１の計測手段が、前記第１の情報を解析して前記対象物体の位置姿勢を計測する第１の計測工程と、
   前記第２のセンサ手段が、前記第１の計測工程による計測結果に応じて決定される位置姿勢で、前記対象物体に関する２次元情報もしくは３次元情報を第２の情報として取得する第２のセンサ工程と、
   前記第２の計測手段が、前記第２の情報を解析して前記対象物体の位置姿勢を計測する第２の計測工程と、
   を有し、
   前記決定手段が、前記第１の計測手段による計測結果に応じて、前記第２のセンサ手段が前記対象物体を撮影する際に前記対象物体へ投影される投影光の正反射光が、前記第２のセンサ手段の撮影範囲に入らない位置姿勢を、前記第２のセンサ手段の位置姿勢として決定する決定工程をさらに有することを特徴とする情報処理装置の制御方法。
10. 請求項９に記載の情報処理装置の制御方法の各工程をコンピュータに実行させるためのプログラム。