(57)【要約】【課題】小型化、軽量化を図りつつ、位置姿勢が変更できる可動部を持つ可動手段を精度良く制御できるようにする。【解決手段】第１の位置姿勢で、対象物体１０３の２次元情報又は３次元情報を取得する第１のセンサ部１０１と、ロボット１００に装着され、対象物体１０３の２次元情報を取得する第２のセンサ部１０２と、第１のセンサ部１０１で取得した情報に基づいて、対象物体１０３の３次元位置姿勢を計測する３次元位置姿勢計測部１１３と、３次元位置姿勢計測部１１３による計測結果及び対象物体のモデル情報に基づいて、第２のセンサ部１０２が情報を取得する第２の位置姿勢を算出する第２センサ位置姿勢決定部１１４と、第２の位置姿勢で第２のセンサ部１０２が取得した情報と対象物体のモデル情報とに基づいて、対象物体１０３の３次元位置姿勢を計測する３次元位置姿勢計測部１１７とを備える。【選択図】図１
2014046433.tif 000002
【特許請求の範囲】
【請求項１】
位置姿勢が変更できる可動部を持つ可動手段を制御するための情報処理システムであって、
前記可動手段とは別に配置され、第１の位置姿勢で、対象物体の２次元情報又は３次元情報を取得する第１のセンサ手段と、
前記可動手段に装着され、前記対象物体の２次元情報を取得する第２のセンサ手段と、
前記第１のセンサ手段で取得した情報に基づいて、前記対象物体の３次元位置姿勢を計測する第１の計測手段と、
前記第１の計測手段による計測結果及び前記対象物体のモデル情報に基づいて、前記第２のセンサ手段が情報を取得する第２の位置姿勢を算出する第２センサ位置姿勢決定手段と、
前記第２の位置姿勢で前記第２のセンサ手段が取得した情報と前記対象物体のモデル情報とに基づいて、前記対象物体の３次元位置姿勢を計測する第２の計測手段とを備えたことを特徴とする情報処理システム。
【請求項２】
前記第２センサ位置姿勢決定手段は、事前に登録されたモデル情報の観察位置として前記対象物体と第２センサとの相対的な位置姿勢を維持するように、前記第１の計測手段による前記対象物体の位置姿勢からの位置関係を算出することを特徴とする請求項１に記載の情報処理システム。
【請求項３】
前記第２の計測手段で前記対象物体の３次元位置姿勢を計測する初期値として、前記第１の計測手段による計測結果と、前記第２センサ位置姿勢決定手段で算出した第２の位置姿勢とを利用することを特徴とする請求項１又は２に記載の情報処理システム。
【請求項４】
前記第１の計測手段で計測した前記対象物体の３次元位置姿勢と、前記第２の計測手段で計測した前記対象物体の３次元位置姿勢とに基づいて、前記対象物体の３次元位置姿勢を算出する調整手段を備えることを特徴とする請求項１乃至３のいずれか１項に記載の情報処理システム。
【請求項５】
前記第２センサ位置姿勢決定手段で第２の位置姿勢を算出する際に前記可動手段の可動範囲であるか否かの判定を行うことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理システム。
【請求項６】
位置姿勢が変更できる可動部を持つ可動手段を制御するための情報処理方法であって、
前記可動手段とは別に、第１の位置姿勢で配置された第１のセンサ手段により、対象物体の２次元情報又は３次元情報を取得するステップと、
前記第１のセンサ手段で取得した情報に基づいて、前記対象物体の３次元位置姿勢を計測する第１の計測ステップと、
前記第１の計測ステップによる計測結果及び前記対象物体のモデル情報に基づいて、前記可動手段に装着された第２のセンサ手段が前記対象物体の２次元情報を取得する第２の位置姿勢を算出するステップと、
前記第２の位置姿勢で前記第２のセンサ手段が取得した情報と前記対象物体のモデル情報とに基づいて、前記対象物体の３次元位置姿勢を計測する第２の計測ステップとを有することを特徴とする情報処理方法。
【請求項７】
位置姿勢が変更できる可動部を持つ可動手段を制御するための処理をコンピュータに実行させるプログラムであって、
前記可動手段とは別に、第１の位置姿勢で配置された第１のセンサ手段により、対象物体の２次元情報又は３次元情報を取得する処理と、
前記第１のセンサ手段で取得した情報に基づいて、前記対象物体の３次元位置姿勢を計測する第１の計測処理と、
前記第１の計測処理による計測結果及び前記対象物体のモデル情報に基づいて、前記可動手段に装着された第２のセンサ手段が前記対象物体の２次元情報を取得する第２の位置姿勢を算出する処理と、
前記第２の位置姿勢で前記第２のセンサ手段が取得した情報と前記対象物体のモデル情報とに基づいて、前記対象物体の３次元位置姿勢を計測する第２の計測処理とをコンピュータに実行させるためのプログラム。
【請求項８】
請求項７に記載のプログラムを記録したことを特徴とするコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢が変更できる可動部を持つ可動手段を制御するための情報処理システム、方法、プログラム及びコンピュータ読み取り可能な記録媒体に関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンド等のエンドエフェクタによって部品をピックして組立を行う。
従来より、ピックするための部品の供給は、パーツフィーダと呼ばれる部品１つ１つを並べて供給するための装置を利用したり、パレット（箱）に部品を様々な姿勢で山積みしたりすることによって行われている。パーツフィーダを利用する場合は、部品１つ１つの位置姿勢が予め決まった状態で供給されるため、ロボットによるピックは比較的容易に行われる。しかしながら、パーツフィーダ装置を用意するためのコストが余計にかかる。また、部品の形状に合わせて異なるパーツフィーダを用意しなければならないこともある。一方、部品を山積みにして供給する場合は、パレットに部品を置くだけでよいためコスト増を回避することができる。さらに、近年の少量多品種生産の傾向を受けて、様々な部品へ素早く対応することができる山積み供給に注目が集まっている。
ロボットと画像センサによる山積み部品のピッキングに関する技術として、例えば特許文献１では、複数のセンサとロボットの動作を組み合わせてピッキングを行う方法が提示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３５５６５８９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
山積み部品を正確にピッキングする際には、把持位置の手前で部品との相対位置関係を補正する必要がある。それは、多軸ロボットの絶対位置指定での動作精度が悪いことや、一つのセンサから観察ができないオクルージョンの発生等により、把持位置での精度が確保されていないときが発生するためである。そのため、ロボットの把持位置での部品の観察が必要となる。
特許文献１の構成では、３次元を取得するセンサは、ベースラインを確保するための筐体にパターンを照射する装置、撮像する装置を搭載するため、大型化し重量が重くなる。そのため、ロボットのアーム部に可搬重量に加算され、運動時の慣性も大きくなり、より剛性の高いロボットが必要になり、コストが増加する。
一方で、２次元画像カメラによる姿勢が不定な対象物体を撮像した画像には、パースによる見え方が変わるため、画像中の幾何特徴によるテンプレートマッチングだけでは十分な位置検出精度を得ることが難しい課題もある。
【０００５】
本発明は上記のような点に鑑みてなされたものであり、小型化、軽量化を図りつつ、位置姿勢が変更できる可動部を持つ可動手段を精度良く制御できるようにすることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の情報処理システムは、位置姿勢が変更できる可動部を持つ可動手段を制御するための情報処理システムであって、前記可動手段とは別に配置され、第１の位置姿勢で、対象物体の２次元情報又は３次元情報を取得する第１のセンサ手段と、前記可動手段に装着され、前記対象物体の２次元情報を取得する第２のセンサ手段と、前記第１のセンサ手段で取得した情報に基づいて、前記対象物体の３次元位置姿勢を計測する第１の計測手段と、前記第１の計測手段による計測結果及び前記対象物体のモデル情報に基づいて、前記第２のセンサ手段が情報を取得する第２の位置姿勢を算出する第２センサ位置姿勢決定手段と、前記第２の位置姿勢で前記第２のセンサ手段が取得した情報と前記対象物体のモデル情報とに基づいて、前記対象物体の３次元位置姿勢を計測する第２の計測手段とを備えたことを特徴とする。
【発明の効果】
【０００７】
本発明によれば、小型化、軽量化を図りつつ、位置姿勢が変更できる可動部を持つ可動手段を精度良く制御することができる。
【図面の簡単な説明】
【０００８】
【図１】第１の実施形態に係るロボットの制御システムの構成を示す図である。
【図２】情報処理部の構成を示す図である。
【図３】３次元幾何モデルを説明するための図である。
【図４】情報処理部の処理手順を示すフローチャートである。
【図５】エッジ検出を説明する図である。
【図６】線分の投影像と検出されたエッジの関係を説明する図である。
【図７】ロボットと第２のセンサ部と対象物体との関係を説明するための図である。
【発明を実施するための形態】
【０００９】
以下、添付図面を参照して、本発明の好適な実施形態について説明する。
（第１の実施形態）
第１の実施形態では、対象物体に関する３次元情報を取得する第１のセンサ手段（プロジェクタ及びカメラ）と、ロボットに装着され対象物体の２次元情報を取得するための第２のセンサ手段（カメラ）とを用いて、対象物体の３次元位置姿勢計測を行う。
【００１０】
図１に、本実施形態に係るロボットの制御システムの構成を示す。同図に示すように、ロボット１００と、第１のセンサ部１０１、第２のセンサ部１０２、情報処理部１１０、及びロボットコントローラ部１２０により構成される情報処理システムとを備える。これらの構成により、パレット１０４内に山積みした対象物体１０３の計測を行い、ロボット１００によるピック作業を行う。
【００１１】
位置姿勢が変更できる可動部を持つ可動手段であるロボット１００は、多関節ロボットであり、ロボットコントローラ部１２０からの制御命令を受けて稼働する。ロボット１００の先端にはエンドエフェクタであるハンドが装着されており、対象物体１０３に対する作業を行うことが可能である。本実施形態では、エンドエフェクタとして、対象物体を把持することが可能なチャック機構を持つハンドを用いる。エンドエフェクタにはモータ駆動可能なハンドを用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよい。センサ部１０１の位置姿勢、ロボット１００やハンドの位置や軌道、ロボット１００のアームとセンサ部１０２の相対位置姿勢のキャリブレーション作業は、本実施形態の実施に先立って公知の技術によって予め行われているものとする。これによって、情報処理部１１０で出力される位置姿勢の座標系を、ワークスペース座標系で指定した位置姿勢にハンドが設定されるようにロボット１００を制御することが可能になる。
【００１２】
第１のセンサ部１０１は、プロジェクタと、２次元画像を撮影するカメラとから構成される。第１のセンサ部１０１は、パレット１０４の上方の第１の位置姿勢に固定的に配置され、山積みされた対象物体１０３の画像を撮影し、情報処理部１１０に距離画像と２次元画像を出力する。第１のセンサ部１０１のプロジェクタと、カメラとの相対的な位置関係は予めキャリブレーションにより求められているものとする。本実施形態では、第１のセンサ部１０１で撮影した画像は画像処理されて距離画像として出力されるが、情報処理部１１０に画像処理機構を持たせて距離画像に変換してもよい。
第１のセンサ部１０１のプロジェクタは、対象物体１０３に対してパターン光を照射し、そのパターンを第１のセンサ部１０１のカメラにより撮影し、照明したパターンと撮像したパターンとの対応から三角法により距離を算出する。パターンとしては、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターン等を利用する。２次元的なパターンやランダムドットのようなパターンでもよい。
また、第１のセンサ部１０１は、回折格子と照明とカメラとから構成されてもよい。この場合は、回折格子と照明により対象物体にパターン光を投射し、カメラによりパターンを撮影する。パターン光の照射は、プロジェクタに限定されるものでなく、レーザースリット光を走査した画像を取得しても構わない。光の反射時間を計測して、距離画像を生成する機構を用いても構わない。パターン光を照射することで、工業部品等の表面の模様が少ない対象物体とする場合には有利である。模様が多い場合には、カメラを用いた２台のステレオカメラでも両方の撮像画像に対応する領域から距離画像を生成することができるので、それを用いても構わない。
【００１３】
第２のセンサ部１０２は、２次元画像を撮影する小型カメラから構成される。第２のセンサ部１０２は、ロボット１００のハンド部分近くに固定的に装着されて、ロボット１００の各関節の角度によって制御可能な第２の位置姿勢でエンドエフェクタ付近にあるものを計測する。第２のセンサ部１０２のカメラと、ロボット座標系であるエンドエフェクタとの相対的な位置関係は予めキャリブレーションにより求められているものとする。本実施形態では、第２のセンサ部１０２で撮影した画像は情報処理部１１０で処理するが、第２のセンサ部１０２内に画像処理機構を持たせて画像処理した結果を出力してもよい。
【００１４】
対象物体１０３は工業製品を構成する部品であり、ロボット１００によりピックされて製品に組み付けられる。素材は、プラスチック、金属、ビニール等、様々なものを用いることができる。複数の対象物体１０３が様々な姿勢になるようにパレット１０４に山積みされている。
【００１５】
パレット１０４は、対象物体１０３を入れるための箱である。パレット１０４の素材に制約はないが、プラスチックや紙素材を用いることが多い。また、形状にも制約はないが、作りやすさの点から立方体或いは直方体にすることが多い。また、大きさにも制約はないが、通常はセンサ部１０１で計測可能な範囲内に収まるようにする。
【００１６】
次に、情報処理部１１０の構成について説明する。図２に、情報処理部１１０の構成を示す。
第１センサ情報取得部１１１は、第１のセンサ部１０１が撮影した距離画像及び２次元画像を取得して、その取得した画像を３次元位置姿勢計測部１１３に出力する。
【００１７】
モデル情報保持部１１５は、３次元位置姿勢計測部１１３の位置姿勢を計測するために利用される計測モデル情報を保持する。計測モデル情報の１つとして、３次元ＣＡＤに基づく３次元幾何モデルが挙げられる。もう１つの計測モデル情報として、実際の対象物体もしくは対象物体を模した３次元幾何モデルを予め定められた複数の視点から見た参照画像モデルが挙げられる。
３次元幾何モデルは、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをコンピュータグラフィック分野で使われる複数のポリゴン要素に変換したものである。本実施形態では、ポリゴン要素を利用した場合について説明を行う。図３に示すように、モデルは点、線、面といった構成要素からなる。図３（ａ）〜（ｃ）はいずれも同じモデルである。図３（ａ）及び（ｄ）は、モデルの各頂点を示す図である。図３（ｂ）及び（ｅ）は、モデルの各辺となる線を示す図である。図３（ｃ）及び（ｆ）は、モデルの各面を示す図である。モデルには、図３（ｆ）に示すようにモデルを構成する面の法線データが含まれている。
参照画像モデルは、複数の２次元画像からなるデータである。実写画像に基づく参照画像モデルは、対象物体を中心にして様々な方向からカメラで撮影を行って得た画像から作成する。撮影を行うためにやぐらを組んでカメラを複数配置してもよいし、人が手でカメラを持って撮影してもよいし、ロボットに装着したカメラでロボットを動かしながら撮影を行ってもよい。どの方法で撮影を行ってもよいが、撮影したときのカメラと対象物体との相対位置姿勢を求め、撮影画像と合わせて記憶しておく。相対位置姿勢は、やぐらに複数のカメラを配置した場合は、やぐらの形状から求めることができる。人が手でカメラを持つ場合は、カメラに位置姿勢センサを装着して求めることができる。ロボットに装着したカメラで撮影する場合は、ロボットの制御情報を利用して求めることができる。
対象物体を模した３次元幾何モデルに基づく参照画像モデルは、ＣＡＤモデルの中心から、頂点が等しい距離になるＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅを設定して、Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点からＣＡＤモデルの中心を向いてみたときの画像を用いる。Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点は複数あり、隣り合ったそれぞれの頂点は同じ距離となる。ある頂点を基準位置として、他の頂点との相対的な関係を元にどの方向から見たかの観察方向を画像と共に、設定情報として記憶しておく。
対象物体が１種類と予め分かっているときは、その種類の計測モデル情報だけ保存する。複数種類の対象物体を扱うときは、複数の計測モデル情報を保存しておき、利用時に切り替える。
【００１８】
第１の計測手段である３次元位置姿勢計測部１１３は、第１センサ情報取得部１１１から出力された距離画像及び２次元画像と、モデル情報保持部１１５が保持している計測モデル情報とを利用して対象物体１０３の位置姿勢を求める。そして、その求めた位置姿勢情報を第２センサ位置姿勢決定部１１４に出力する。
計測モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルの辺である線と、第１センサ情報取得部１１１から出力された２次元画像から抽出したエッジ成分との対応付けを行うことで、対象物体の位置姿勢を求める。本実施形態では、２次元画像に３次元幾何モデルが当てはまるように、計測対象物体の位置姿勢の概略値を反復演算によって繰り返し補正する。
本実施形態では、計測モデル情報を利用して画像中に含まれる対応する幾何特徴との関係から、モデルの位置姿勢を推定する方法を利用する。３次元幾何モデルの線分と、画像中のエッジとの対応から２次元画像を利用して位置合わせをする方法と、距離画像の含まれる対応する距離点群とモデル表面との距離を最小化するように位置姿勢を推定する方法を用いる方法を説明する。他に２次元画像を利用する場合には、テンプレートマッチングによる画素比較による方法や、幾何特徴としてテクスチャの勾配特徴を利用して対応を行う幾何特徴点対応による方法等を利用しても構わない。これらの方法は、対象物体を様々な方向したときのモデルを視点別に保持しておき、対応する観察視点のモデルを参照することで位置姿勢を行うことができる。
計測モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより最も一致する参照画像を求め、その参照画像に対応付けられた対象物体との位置姿勢情報に基づいて、対象物体の位置姿勢を求める。
また、計測モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルから抽出した表面の点群データと、第１センサ情報取得部１１１から出力された距離画像から抽出される距離点群との対応付けを行うことで対象物体の位置姿勢を求める。距離点群と計測モデル情報とを利用して対象物体の位置姿勢を求めるために、本実施形態ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。対象物体の位置姿勢を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための手法はＩＣＰ法に限るものではない。
３次元位置姿勢計測部１１３で求めた位置姿勢は、第２センサ位置姿勢決定部１１４と位置姿勢調整部１１８に出力される。
【００１９】
第２センサ位置姿勢決定部１１４は、３次元位置姿勢計測部１１３で求めた対象物体１０３の位置姿勢に基づいて、第２のセンサ部１０２が対象物体１０３を計測する第２の位置姿勢を求める。その際、３次元位置姿勢計測部１１７の撮像座標系で利用する計測モデル情報に合わせた位置姿勢になるように第２のセンサ部１０２を移動する。該位置姿勢情報を位置姿勢出力部１１９に出力する。また、該位置姿勢は、３次元位置姿勢計測部１１７の初期値として利用される。第２のセンサ部１０２が対象物体１０３を計測する位置姿勢を求める方法については後述する。
【００２０】
第２センサ情報取得部１１２は、第２のセンサ部１０２が撮影した２次元画像を取得して、その取得した２次元画像を３次元位置姿勢計測部１１７に出力する。
【００２１】
第２の計測手段である３次元位置姿勢計測部１１７は、第２センサ情報取得部１１２から出力された２次元画像と、モデル情報保持部１１６が保持している計測モデル情報とを利用して対象物体１０３の位置姿勢を求める。そして、その求めた位置姿勢情報を位置姿勢調整部１１８に出力する。
【００２２】
位置姿勢調整部１１８は、３次元位置姿勢計測部１１３及び１１７の推定結果として、パラメータの推定精度を考慮して統合する。
計測モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより最も一致する参照画像を求め、その参照画像に対応付けられた対象物体との位置姿勢情報に基づいて対象物体１０３の位置姿勢を求める。
計測モデル情報として３次元幾何モデルを利用する場合は、３次元位置姿勢計測部１１３と同様に３次元幾何モデルの辺である線と、第２センサ情報取得部１１２から出力された２次元画像から抽出したエッジ成分との対応付けを行うことで対象物体１０３の位置姿勢を求める。
【００２３】
位置姿勢出力部１１９は、第２のセンサ部１０２が対象物体１０３を計測する第２の位置姿勢の座標値をロボットコントローラ部１２０に出力する。
【００２４】
ロボットコントローラ部１２０は、第２のセンサ部１０２が対象物体１０３を計測する第２の位置姿勢の座標値を入力として、ロボット１００を所定の位置まで移動する。第２の位置姿勢とは異なる位置姿勢への移動、或いは対象物体１０３を把持や吸着できる位置姿勢にハンドを移動させて把持や吸着を行うことを指示する方法は、ロボットコントローラ部１２０のプログラムでも構わないし、他のシーケンサが制御しても構わない。ロボットの作業は、移動、把持、吸着に限られるものではなく、対象物体１０３の外観検査等、他の作業も含むことは言うまでもない。さらに、ロボットも、多関節ロボットではなく、ＮＣ制御可能な可動式の機械でもよいことは言うまでもない。
【００２５】
図４は、第１の実施形態における情報処理部１１０の処理手順を示すフローチャートである。
ステップＳ４０１では、パレット１０４の上方の第１の位置姿勢に固定された第１のセンサ部１０１により対象物体１０３の画像を撮影する。撮影した画像データは、第１センサ情報取得部１１１に出力される。第１のセンサ部１０１の位置姿勢は予めキャリブレーションで求められているものとする。
【００２６】
ステップＳ４０２では、３次元位置姿勢計測部１１３が、第１センサ情報取得部１１１で取得した対象物体１０３の画像から、複数ある対象物体１０３のうち少なくとも１つの対象物体の位置姿勢を求める。選択した１つの対象物体を対象物体１０３'とする。対象物体１０３'の位置姿勢を求めるために、モデル情報保持部１１５は保持している計測モデル情報を出力して、３次元位置姿勢計測部１１３が対象物体１０３'の画像と計測モデル情報とのマッチングを行う。
計測モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルの辺である線分と、第１センサ情報取得部１１１から出力された２次元画像から抽出したエッジ成分との対応付けを行うことで対象物体の位置姿勢を求める。本実施形態では、２次元画像に３次元幾何モデルが当てはまるように、非線形最適化手法の一つであるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて、対象物体の位置姿勢（６次元ベクトルｓで表す）の概略値を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための最適化手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。
【００２７】
図５は、エッジ検出を説明する図である。何らかの方法（例えばテンプレートマッチング）により得られた対象物体の概略位置姿勢とキャリブレーション済みのセンサ部１０１の内部パラメータを用いて、３次元幾何モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。次に、画像上で等間隔になるように投影された線分５０１上に制御点５０２を設定し、制御点５０２ごとに、投影された線分５０１の法線方向５０３に１次元のエッジ５０４の検出を行う（図５（ａ））。エッジは画素値の濃度勾配の極値として検出されるので、図５（ｂ）に示すように、複数のエッジ５０５が検出されることがある。本実施形態では、検出されるエッジをすべて仮説として保持する。
３次元幾何モデルの辺である線分と第１センサ情報取得部１１１から出力された２次元画像のエッジ成分との対応付けを行い、対象物体の位置姿勢を求めるには、位置姿勢を算出するための係数行列と誤差ベクトルの算出を行う。ここで、係数行列の各要素は、画像上での点と直線の距離を位置姿勢の関数としたときの、位置姿勢の各要素に関する１次の偏微分係数である。誤差ベクトルは、エッジについては投影された線分と検出されたエッジの画像上での符号付き距離である。
【００２８】
係数行列の導出について説明する。図６は、線分の投影像と検出されたエッジの関係を説明する図である。図６では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点６０３（投影された各線分を画像上で等間隔に分割した点）の画像上での座標を（ｕ0，ｖ0）、該制御点６０３が所属する線分の画像上での傾きをｕ軸に対する傾きθと表す。傾きθは、線分６０６の両端の三次元座標をｓに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該線分６０６の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点６０３の対応点６０７の画像上での座標を（ｕ’，ｖ’）とする。ここで、対応点６０７の座標（ｕ’，ｖ’）を通り、傾きがθである直線（図６の破線）上の点（ｕ，ｖ）は、式（１）と表される（θは定数とする）。ここで、ｄは、式（２）で表される定数である。
【００２９】
【数１】
2014046433.tif 000003
【００３０】
制御点６０３の画像上での位置は、対象物体の位置姿勢により変化する。また、対象物体の位置姿勢の自由度は６自由度である。すなわち、ｓは６次元ベクトルであり、対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す三次元ベクトル等によって表現される。位置姿勢により変化する点の画像上での座標（ｕ，ｖ）は、座標（ｕ0，ｖ0）の近傍で１次のテイラー展開によって、式（３）、式（４）のように近似することができる。ただし、△ｓi（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。
【００３１】
【数２】
2014046433.tif 000004
【００３２】
位置姿勢の概略値と実際の位置姿勢の差がそれほどないと仮定すると、正しいｓによって得られる制御点の画像上での位置は、式（１）、式（２）が表す直線上にあると仮定できる。式（３）、式（４）によって近似されるｕ，ｖを式（１）、式（２）に代入することにより、式（５）が得られる。ここで、ｒは、式（６）で表される定数である。
【００３３】
【数３】
2014046433.tif 000005
【００３４】
式（５）、式（６）は、対応付けが行われたすべてのエッジについて立式することができる。なお、すべてのエッジについて立式せずに一部のエッジについてだけ立式してもよい。
式（５）、式（６）は、ｓの各成分の微小変化△ｓi（ｉ＝１，２，・・・，６）についての方程式であるため、式（７）のような△ｓiに関する線形連立方程式を立てることができる。
【００３５】
【数４】
2014046433.tif 000006
【００３６】
ここで、式（７）を式（８）のように表す。
ＪΔｓ＝Ｅ・・・（８）
【００３７】
線形連立方程式の係数行列Ｊを算出するため偏微分係数の算出を行う。式（８）をもとに、行列Ｊの一般化逆行列（ＪT・Ｊ）-1・ＪTを用いて位置姿勢の補正値△ｓを最小二乗基準で求める。しかしながら、エッジには誤検出等による外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、式（７）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える。重みは、例えば式（９）に示すようなＴｕｋｅｙの関数により与える。
【００３８】
【数５】
2014046433.tif 000007
【００３９】
ｃ1，ｃ2は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数等、誤差が大きい情報には小さな重みを与え、誤差が小さい情報には大きな重みを与える関数であれば限定されるものではない。各計測情報（エッジ又は点群データ）に対応する重みをｗiとする。ここで、式（１０）のように重み行列Ｗを定義する。
【００４０】
【数６】
2014046433.tif 000008
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗiが入る。この重み行列Ｗを用いて、式（８）を式（１１）のように変形する。
ＷＪ△ｓ＝ＷＥ・・・（１１）
【００４１】
式（１１）を式（１２）のように解くことにより補正値△ｓを求める。
【００４２】
【数７】
2014046433.tif 000009
【００４３】
算出された位置姿勢の補正値△ｓにより、位置姿勢の概略値を補正する。
ｓ←ｓ＋△ｓ
ｓの収束判定を行い、収束していれば終了し、そうでなければ繰り返し算出を行う。収束判定では、補正値△ｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。このように、収束するまで繰り返すことで、位置姿勢を算出できる。
【００４４】
なお、本実施形態では、最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いる方法について説明したが、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法等、他の非線形最適化計算手法を用いてもよい。
計測モデル情報として参照画像モデルを利用する場合は、参照画像モデルをテンプレートとしたテンプレートマッチングにより最も一致する参照画像を求め、その参照画像に対応付けられた対象物体との位置姿勢情報に基づいて対象物体の位置姿勢を求める。参照画像の輝度をＴ（ｉ，ｊ）、２次元画像の輝度をＩ（ｉ，ｊ）として、参照画像をｍ×ｎ画素の画像としたとき、一致度は式（１３）で求めることができる。
【００４５】
【数８】
2014046433.tif 000010
【００４６】
計測モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルから抽出した表面の点群と、第１センサ情報取得部１１１から出力された距離画像点群との対応付けを行うことで対象物体の位置姿勢を求める。距離点群を求めるには、空間符号化法、光切断法等の公知技術を用いればよいため本実施形態では詳しく説明しない。３次元幾何モデルの点群と距離画像点群との対応付けを行うために、本実施形態ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。
３次元幾何モデルの表面点群を、式（１４）に示すＰとする。また、距離画像点群を、式（１５）に示すＡとする。
【００４７】
【数９】
2014046433.tif 000011
【００４８】
３次元幾何モデルの表面点群Ｐを変換して距離点群Ａに位置合わせする。点群Ｐの各点ｐiに距離が最も近い点群Ａの点をｂｉ∈Ａとしたとき、式（１６）の誤差関数を定義できる。Ｒとｔはそれぞれ姿勢パラメータと移動ベクトルである。
【００４９】
【数１０】
2014046433.tif 000012
【００５０】
誤差関数Ｅを小さくするＲとｔを求め、式（１７）による補正を行う。
【００５１】
【数１１】
2014046433.tif 000013
【００５２】
誤差関数Ｅを小さくするＲとｔを求める方法については、文献「Ｋ．Ｓ．Ａｒｕｎ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｉｎ，“Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ ３−Ｄ Ｐｏｉｎｔ Ｓｅｔｓ，”ＰＡＭＩ，ｖｏｌ．９，ｎｏ．５，１９８７」に載っている。
Ｐの収束判定を行い、収束していれば終了し、そうでなければ繰り返し補正計算を行う。収束判定では、Ｐがほとんど変化しないことをもって収束したと判定する。このように収束するまで繰り返すことで、位置姿勢を算出することができる。
【００５３】
ステップＳ４０３では、第１の計測結果から第２のセンサ部１０２の位置姿勢を計算する。第２のセンサ部１０２の撮影するための位置姿勢を第２センサ位置姿勢決定部１１４にて計算する。
一般に多軸ロボットの場合、絶対位置精度を出すことは難しい。また、対象物体が傾いている場合には、把持する際のエンドエフェクタの向きと、第１のセンサ部１０１から観察される向きとが異なっている場合には、推定精度によりズレが生じている場合もある。
そのため、ロボット１００は、対象物体１０３'を把持動作する直前に、それらの誤差を補正するために第２のセンサ部１０２で対象物体の位置姿勢を算出して、ロボットに補正位置を指示することで、確実に対象物体を把持することが可能となる。そこで、ロボット１００が対象物体１０３'を把持位置より上空で第２のセンサ部１０２により位置姿勢を計測することとする。
図７（ａ）は、ロボット１００が対象物体１０３'を把持する直前に対象物体の上空で第２のセンサ部１０２の撮影をしているときの配置を示す。ロボット１００は、対象物体１０３'を所定の向きから把持するように設定されている。第２のセンサ部１０２は、２次元画像を取得するため、画像中のエッジから物体の位置姿勢を求めることができる。
図７（ｂ）は、第２のセンサ部１０２が撮影した２次元画像の例である。ロボットのエンドエフェクタの把持方向とカメラの向きは異なっているため、撮影した画像中の対象物体１０３'にはパースが発生している場合がある。２次元画像のテンプレートマッチングでは、このようなパースが発生している場合には、平面のホモグラフィーを求めて補正することで画像を正射影に変換することでマッチングを行うことができる。
ここで、第２のセンサ部１０２の撮像する位置姿勢を計算する際の座標系表現について述べる。
図７（ｃ）に、各座標系の原点を模式的に示す。基準となる世界座標系をＭWとする。第１のセンサ部１０１の取り付けられた座標系をＭGとする。ロボット座標系をＭRとし、その相対座標系としてエンドエフェクタの座標系をＭEとする。第２のセンサ部１０２の座標系をＭCは、変換行列ＭLによるＭEの相対座標系である。対象物体１０３'の上部の幾何特徴を有する平面の座標系をＭPとする。第１のセンサ部１０１のＭGのカメラ座標系基準で３次元位置姿勢計測部１１３により求められた位置姿勢をＭP'とすると、式（１８）となる。
ＭP＝ＭGＭP'・・・（１８）
【００５４】
対象物体１０３'の上空での観察ポイントは、把持姿勢を考慮して事前に設定されているものとして、対象物体に対する相対的な座標変換をＭTとする。求められた対象物体１０３'のＭPから、エンドエフェクタの待機位置として設定される座標系をＭE'とすると、式（１９）のようになる。
ＭE'＝ＭPＭT・・・（１９）
【００５５】
第２のセンサ部１０２が撮影する位置姿勢は、エンドエフェクタの座標系ＭEが、ＭE'と同じになるように指定すればよいこととなる。座標系の逆変換をＭ-1と表現すると、ＭEはロボット座標系のＭRより、式（２０）のように表現できる。
ＭE＝ＭR-1ＭE'・・・（２０）
ここで求めた、ＭEが第２の位置姿勢としてロボット座標系での位置姿勢の値となる。
【００５６】
なお、第２のセンサ部１０２が対象物体１０３'を撮影する位置姿勢での座標系ＭC'は、対象物体ＭPから相対的座標系で表現すると、式（２１）のように表現できる。
ＭC'＝ＭTＭL・・・（２１）
この対象物体からの相対座標系ＭC'を用いることで、撮影時の対象物体の幾何特徴を有する平面との配置関係が設定でき、平面のパースの見え方を補正するためのホモグラフィー行列を求めることができる。
ＭC'の成分のうち、姿勢成分をＲCとして３ｘ３行列とし、並進成分をＴCとして３ｘ１行列で表す。対象物体１０３'のＭPの平面の法線をＮCとし、平面と第２のセンサ部１０２までの距離をＤCとすると、ホモグラフィー行列Ｈは式（２２）となる。
【００５７】
【数１２】
2014046433.tif 000014
【００５８】
このホモグラフィー行列Ｈを用いることで、対象物体１０３'の撮像画像にパースが生じていても、正射影に変換して、２次元画像中の特徴から位置姿勢を推定することができる（ただし、対象物体１０３'の幾何特徴は、平面に配置されているものとする）。これにより、第２のセンサ部１０２の取り付け位置によらず、対象物体１０３'の平面上の幾何特徴を撮影画像に応じて補正できるので、モデル情報保持部１１６での幾何特徴情報の登録を簡便化することができる。具体的には、モデルを正射影したときの幾何特徴の画像位置を利用すればよい。
【００５９】
ステップＳ４１１では、ステップＳ４０３で求めた第２の位置姿勢がロボット１００の可動範囲であるかどうかの判定を行う。ロボット１００の可動範囲は、可動アームの構成により制限がある。そのため、ロボットのエンドエフェクタが到達不可能な向きで位置姿勢が設定されても、実際にはロボット１００を動かすことができない。ここでは、エンドエフェクタの可動範囲内では無い場合には、ステップＳ４０２から選択されていない部品についての位置姿勢検出を再度行うようにする。これにより、ロボット１００がピックできないことによるシステムの停止を抑制することができる。
３次元位置姿勢計測部１１３は、対象物体１０３’の位置姿勢を求めたら第２センサ位置姿勢決定部１１４と位置姿勢調整部１１８とに対象物体１０３’の位置姿勢情報を出力する。
【００６０】
ステップＳ４０４では、ステップＳ４１１でロボット１００の可動手段範囲内で動作可能と判断した第２の位置姿勢で第２のセンサ部１０２が対象物体１０３'を撮影するようにロボット１００を姿勢制御する。ステップＳ４０４では、まずステップＳ４０３で決定した位置姿勢情報をロボット座標系に変換する。変換方法は、式（１９）を用いればよい。ロボット１００に対して、位置姿勢を指定して移動するコマンドをロボットコントローラ部１２０に対して送出する。
【００６１】
ステップＳ４０５では、ロボット１００に装着した第２のセンサ部１０２により第２の位置姿勢で対象物体１０３'の画像を撮影する。撮影した画像は第２センサ情報取得部１１２に出力される。
【００６２】
ステップＳ４０６では、３次元位置姿勢計測部１１７において第２センサ情報取得部１１２から得られる２次元画像から、モデル情報保持部１１６と、第２センサ位置姿勢決定部１１４の位置姿勢を用いて対象物体１０３'の位置姿勢を求める。
ここでは、ステップＳ４０３で求めたホモグラフィー行列Ｈに基づいて、モデル情報保持部１１６の幾何特徴の位置を投影変換し、その近傍の対応する画像幾何特徴の検出を行い、その座標値をホモグラフィーの逆変換を行って、パースの影響を除去する。検出特徴の位置とモデルの幾何特徴の位置との差を小さくするように、平面内での移動・回転成分を補正値として求める。その補正値を第２センサ位置姿勢決定部１１４から得られた位置姿勢の値を補正することができる。
平面上の２次元特徴に対する補正値を（ｕ，ｖ，θ）とする。ｕはＸ座標、ｖはＹ座標、θは平面内での回転とする。２次元特徴の座標をＸ，Ｙとすると、補正後の位置Ｘ',Ｙ'は式（２３）で示される。
【００６３】
【数１３】
2014046433.tif 000015
【００６４】
補正量が十分小さい場合には、ｃｏｓ(θ)≒１、ｓｉｎ(θ)≒θと近似することができるので、式（２３）は、式（２４）のように近似できる。
【００６５】
【数１４】
2014046433.tif 000016
【００６６】
この式を展開して、補正量のベクトルによる行列表現にすると、式（２５）のようになる。
【００６７】
【数１５】
2014046433.tif 000017
【００６８】
この式を、検出された幾何特徴の各点のホモグラフィー行列補正後の座標値とモデルの位置に対して式（２６）のように示し、疑似逆行列を用いて補正量を求める。ただし、式（２４）の線形近似を行っているため反復計算を行い、残差の少ない補正量を求める。
【００６９】
【数１６】
2014046433.tif 000018
【００７０】
ここでは、幾何特徴の検出座標を用いた補正量の計算を示したが、平面内での対象物体の位置・回転が検出できれば方法は問わない。ハフ変換画像から対象の位置を検出する方法や、ＦＦＴ等の空間画像変換を利用した検出方法等を使っても構わない。
【００７１】
ステップＳ４０７では、位置姿勢調整部１１８の処理として、ステップＳ４０３で得られた第１のセンサ部１０１の位置姿勢の推定値に対して、ステップＳ４０６で得られた位置姿勢の補正値を使って対象物体１０３'の位置姿勢を調整する。
第２のセンサ部１０２が取得する画像は２次元画像であるため、奥行き方向の情報が縮退している。そのため、奥行き方向の推定精度は、画像平面内の移動方向に比べて悪くなっている場合がある。そこで、ステップＳ４０６の補正値のうち、精度が良さそうな補正量に対して重みをつけて補正することとする。
第２のセンサ部１０２の座標系ＭCのうち、視軸方向の回転、視軸に対して垂直な平面でのＸ，Ｙ方向の成分を抽出して、その値に他のパラメータより大きい重みをつけて、ステップＳ４０３で得られた位置姿勢の値を補正する。具体的には、第２のセンサ部１０２のカメラ座標系基準で求めた更新パラメータで、Ｘ，Ｙ，Ｒｏｌｌ成分の値を更新する。これにより、２次元画像のみで位置姿勢を推定する結果に比べて、第１のセンサ部１０１による位置姿勢のパラメータで精度の良い要素と、第２のセンサ部１０２による位置姿勢のパラメータで精度の良い部分に基づいて、位置姿勢を調整することができる。それにより、全体の推定精度を向上することができる。なお、ここではＸ，Ｙ，Ｒｏｌｌ成分について値を更新する例を示したが、対象物体の姿勢や、第１のセンサ部１０１の構成に応じてパラメータを調整すればよい。位置姿勢の推定中に計算される共分散行列や誤差値を参照して、各パラメータの推定精度として、それを重みとして第１のセンサ部１０１と第２のセンサ部１０２の推定値を調整することができる。
【００７２】
ステップＳ４０８では、位置姿勢調整部１１８の対象物体１０３’の位置姿勢情報に基づいてロボット１００に対する作業指示を行う。ロボット１００が対象物体１０３’を把持するハンドを備えていれば対象物体１０３’の把持を指示する。ロボット１００が対象物体１０３’を吸着するパッドを備えていれば対象物体１０３’の吸着を指示する。
【００７３】
ステップＳ４０９では、ステップＳ４０８の指示内容を実行するようロボット１００に対してピッキング、及びその後の把持部品の移動や、取り付け等の所定の動作シーケンスを実行させる。
【００７４】
ステップＳ４１０では、終了指示があったかどうかを判定する。終了指示がなければステップＳ４０１に戻る。終了指示があればフローを終了して全ての動作を停止する。なお、ステップＳ４１０での終了判定を待たずに、ユーザが図示しない緊急停止ボタンを押すことによりフローを終了して全ての動作を停止してもよい。
【００７５】
本実施形態において、ステップＳ４０５からステップＳ４０６での処理は、情報処理部１１０の構成の一部として説明したが、実際には情報処理部１１０と別の装置でも代替させてもよい。２次元画像を撮影して、事前に設定した画像のテンプレートマッチングを行い、平面内の移動量（ｕ，ｖ，θ）を出力する処理部を第２のセンサ部１０２と同じ筐体に配置しても構わない。このようなセンサは、画像処理カメラとして市販されているものを利用することができる。本実施形態では、２次元の画像テンプレートに合わせた向きにセンサの向きをロボットに指示するため、第２のセンサ部１０２で観測される対象部品の姿勢を同じようにすることができる。そのため、姿勢が不定な山積み部品に対しても、従来の２次元画像処理装置を併用することができるため、装置コストも低減することができる。
【００７６】
（第２の実施形態）
第２の実施形態では、第１のセンサ手段による対象物体の位置姿勢の推定値を、第２のセンサ手段による観測方向に変換することができる。そのため、第２のセンサ手段が様々な姿勢に適応する十分な計測モデル情報を保持している場合、第１のセンサ手段の位置姿勢の推定値を第２のセンサ手段の設定情報と関連させて、位置姿勢推定のための初期値として利用することができる。
第２のセンサ部１０２による３次元位置姿勢計測部１１７の処理にて、第１のセンサ部１０１の３次元幾何モデルによる位置姿勢推定を行うことができる。この場合、モデル情報保持部１１５と１１６を共有することができる。共有したものをモデル情報保持部１１５'と呼ぶこととする。
第２センサ位置姿勢決定部１１４は、モデル情報保持部１１５'の３次元幾何モデルを保持しているＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点から観測されるモデルの姿勢に合わせて、第２のセンサ部１０２の位置姿勢を決定すればよい。その際、その時点でのロボットの姿勢や、後工程での姿勢を考慮して、ロボットの移動量が少ない観測ポイントを選択するとタクトタイムが短くなってよい。
第２のセンサ部１０２を移動して撮影した撮像画像から、３次元位置姿勢計測部１１７では、第２センサ位置姿勢決定部１１４の位置姿勢を求める。その際に、第２センサ位置姿勢決定部１１４で算出されたロボットハンドの位置姿勢を第２のセンサ部１０２の座標系に変換したものを対象物体１０３'の位置姿勢の初期値としてする。初期位置姿勢から、Ｇｅｏｄｅｓｉｃ Ｓｐｈｅｒｅの頂点から観測される３次元幾何モデルと、第２のセンサ部１０２のカメラパラメータによるスケール変換が行われ、モデルと対応する幾何特徴の検出が容易になる。
【００７７】
（第３の実施形態）
第１の実施形態において、位置姿勢調整部１１８の処理として、第２のセンサ部１０２のカメラの視軸方向として推定精度が高いＸ，Ｙ，Ｒｏｌｌ方向のパラメータ調整手段を述べた。実際には、対象物体をエンドエフェクタが把持する方向や対象部品の姿勢、第１のセンサ部１０１での位置姿勢のパラメータの推定精度を考慮することで、より高い精度を得ることができる。位置姿勢の推定では、位置の３自由度、姿勢の３自由度のパラメータごとにモデルの観測方程式を偏微分したものをヤコビ行列として、対応する幾何特徴とモデルとの距離を少なくするように反復計算をする。推定パラメータによる各幾何特徴とモデルとの残差から、パラメータのフィッティング具合の値として利用することができる。観測による残差を示す情報を利用して、第１と第２のそれぞれで推定された位置姿勢推定結果のパラメータ調整手段により精度を向上することができる。ノイズを多く含んで残差が大きい場合には、求めたパラメータの確からしさも低いので、重みを小さくし、残差が小さい場合には重みを大きくするような調整を行えばよい。
また、対象物体は様々な形状が考えられるので、長細い形状や平坦で薄い形状等観察方向によっては見えが大きく変化するものがある。第１の実施形態では、把持位置の上空にハンドを一次停止した場合の第２の位置姿勢を決定する例を示した。その際、対象物体の平面上の幾何特徴を利用してホモグラフィー行列による変換により対応を求めたが、変換が不要な位置姿勢に第２のセンサの位置姿勢を決めることもできる。また、位置決めの方法としてテンプレートマッチングを利用する場合には、特徴的な幾何特徴が観測できる方向を第２の位置姿勢としてもよい。例として、部品に添付されている文字やシール、ＱＲコード（登録商標）等の機械読み取りマーカが観測できるように決定することができる。
【００７８】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。すなわち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
【００７９】
＜定義＞
本発明の位置姿勢が変更できる可動部を持つ可動手段としては、ロボットを実施形態として述べたが、基本的に可動機構に第２のセンサが装着していれば、どのような構成であっても構わない。回転台座とＸ，Ｙステージに、パン・チルト機構を有する第２のセンサであっても構わない。また、対象部品の形状や、供給される形態によっては、対象部品の位置姿勢の範囲が決まっている場合がある。その場合には、第２のセンサの位置姿勢も範囲があるため、それに応じた可動装置を用いればよい。
【００８０】
第１のセンサ手段は、可動手段とは別に配置されていればよく、その構成や第１の計測手段で位置姿勢を推定するために必要な観察情報が得られれば何であっても構わない。実施形態では、対象物体の位置と姿勢の情報を推定するために、３次元の情報を取得する構成を述べた。そのように３次元情報を得る手段としては、ステレオカメラで構成されたプロジェクタとカメラ、複数台のカメラ、レーザースリット光の走査／レーザ光パターンとカメラ、ランダムドット照射とカメラ、Time of Flight等、奥行き情報を得るための装置を用いればよい。また、第２のセンサ手段を既定した位置に配置し、第１のセンサ手段をカメラとすれば、第１と第２のステレオカメラとして構成することも可能である。その場合、第１と第２のカメラのベースラインは事前にキャリブレーションしておけばよい。
【００８１】
第２のセンサ手段は、可動手段に装着できるようになっていれば、どのような構成であっても構わない。第２のセンサ手段で得られる観察情報は、第１の計測結果に基づいて可動部が動くことによって調整される。一般的には、２次元のカメラの撮像画像を用いた画像処理による位置姿勢検出を利用することができる。対象部品が第２のセンサ手段から観察したときの平面内の位置のみを検出できればよいので、所定箇所を近接センサ等の対象部品との反射を検出しても構わない。また、２次元の画像ではなく、ラインセンサを用いても、位置を検出することができる。
【００８２】
第１の計測手段及び第２の計測手段による対象部品の位置姿勢の算出方法は、実施形態以外にも、位置姿勢を求めることができる手法であれば何であっても構わない。カラー画像を利用する場合には、対象部品の色の情報を用いて、色の領域の重心と方向を検出した結果を利用することも可能である。また、対象部品の表面の局所領域で模様がある場合には、その模様の幾何特徴を用いて対応付けを行うことも可能である。位置姿勢を求めるには、対応が既知の３点以上があればよいため、計測モデルと対応付けられた画像の幾何特徴の位置を用いれば位置姿勢を推定することができる。
【産業上の利用可能性】
【００８３】
本発明は、工業製品の組立作業で山積みされた部品をピックするのに利用することができる。
【符号の説明】
【００８４】
１００：ロボット、１０１：第１のセンサ部、１０２：第２のセンサ部、１０３：対象物体、１０４：パレット、１１０：情報処理部、１２０：ロボットコントローラ部、１１１：第１センサ情報取得部、１１２：第２センサ情報取得部、１１３：３次元位置姿勢計測部、１１４：第２センサ位置姿勢決定部、１１５：モデル情報保持部、１１６：モデル情報保持部、１１７：３次元位置姿勢計測部、１１８：位置姿勢調整部、１１９：位置姿勢出力部
【図１】
2014046433.tif 000019
【図２】
2014046433.tif 000020
【図３】
2014046433.tif 000021
【図４】
2014046433.tif 000022
【図５】
2014046433.tif 000023
【図６】
2014046433.tif 000024
【図７】
2014046433.tif 000025