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

Abstract

【課題】 対象物体の位置姿勢をより高精度に求めるための技術を提供すること。
【解決手段】 第１の位置姿勢を有する第１のセンサによる計測結果から、対象物体の位置姿勢を求める（第１の計算）。第１の位置姿勢とは異なる第２の位置姿勢に第２のセンサを移動させ、該第２のセンサによる計測結果から、対象物体の位置姿勢を求める（第２の計算）。第１の計算による位置姿勢と第２の計算による位置姿勢とが規定の条件を満たしている場合に、第２の計算による位置姿勢、若しくは第１の計算による位置姿勢及び第２の計算による位置姿勢から計算される位置姿勢、を出力する。
【選択図】図１

Description

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

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

従来技術である特許文献１では、まずパレットの上方に設置されたカメラにより複数の対象物体全体を撮影し、対象物体を撮影した画像上での２次元位置を求める。次に、ロボットの手先部に搭載したセンサを１つの対象物体の近傍の上方に移動させる。そしてセンサによって対象物体の位置姿勢計測を行い、ロボットハンドで対象物体の把持を行う。全体を撮影するカメラと、計測範囲は狭いが精度よく位置姿勢を検出することができるセンサとを組み合わせて段階的に対象物体を計測する。ここで、位置姿勢計測に失敗したり、ロボットハンドで対象物体を把持することに失敗すると、対象としていた物体を次回の検出対象から外して再度位置姿勢計測を行う。このように次回も失敗する可能性が高い対象物体を検出対象から外すことで、処理の効率化を図ることができる。

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

特許文献１による方法では、位置姿勢計測に失敗したり、ロボットハンドで対象物体を把持することに失敗すると、対象としていた物体を次回の検出対象から外して再度位置姿勢計測を行う。ただし位置姿勢計測の失敗には２種類ある。１つ目は対象物体を検出することができずに位置姿勢の計測結果を出力できない場合である。２つ目は対象物体の位置姿勢とは異なる位置姿勢を誤って求めて計測結果として出力する場合である。いずれの場合も対象物体の位置姿勢を正しく計測できていないという意味で失敗と言える。しかし、特許文献１では前者の失敗についてのみ述べており、後者の失敗については触れていない。そのため、特許文献１において後者の失敗が起こった場合は、位置姿勢計測に失敗したと判定されず、誤った位置姿勢に対象物体があるものとしてロボットハンドによる把持を試みる。実際には対象物体が計測結果とは異なる位置姿勢で置かれているためロボットハンドによる把持は失敗する。このとき、把持〜把持失敗検知という無駄な処理が発生するため効率が悪い。

本発明は、以上の課題に鑑みてなされたものであり、対象物体の位置姿勢をより高精度に求めるための技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は、第１の位置姿勢を有する第１のセンサによる計測結果から、対象物体の位置姿勢を求める第１の計算手段と、前記第１の位置姿勢とは異なる第２の位置姿勢に第２のセンサを移動させ、該第２のセンサによる計測結果から、前記対象物体の位置姿勢を求める第２の計算手段と、前記第１の計算手段が計算した位置姿勢と前記第２の計算手段が計算した位置姿勢とが規定の条件を満たしている場合に、前記第２の計算手段が計算した位置姿勢、若しくは前記第１の計算手段が計算した位置姿勢及び前記第２の計算手段が計算した位置姿勢から計算される位置姿勢、を出力する出力手段とを備えることを特徴とする。

本発明の構成によれば、対象物体の位置姿勢をより高精度に求めることができる。

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

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

［第１の実施形態］
本実施形態では、対象物体の２次元情報若しくは３次元情報を取得（計測）する第１のセンサと、ロボットに装着され該対象物体の２次元情報若しくは３次元情報を取得（計測）する第２のセンサと、を用いて、該対象物体の３次元位置姿勢計測を行う。その際、それぞれのセンサの計測結果の整合性を判断し、整合していると判断した場合は、この計測結果は正しいものと判断し、この計測結果を用いて対象物体に対してロボットによる作業を行う。整合していないと判断した場合は、ロボットハンドの位置を動かして再度計測を行う。このようにすることで、第２のセンサを用いて計測した結果が実際の対象物体の位置姿勢と異なっている場合に、誤った計測結果に基づいてロボットによる作業が行われることを防ぐことができる。また、これにより、把持〜把持失敗検知という無駄な処理が発生しないため、ロボットによる作業の効率化を行うことができる。

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

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

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

センサ部１０１は、２次元情報としての２次元画像を撮影するカメラである。センサ部１０１は、パレット１０４の上方に第１の位置姿勢で固定的に配置されており、山積みされた対象物体１０３の画像を撮影し、撮影した画像をセンサ情報取得部１１１に対して出力する。本実施形態では、センサ部１０１で撮影した画像は画像処理部１１０で処理するが、センサ部１０１内に画像処理機構を持たせて画像処理した結果を出力してもよい。なお、センサ部１０１によって対象物体１０３を撮影する際には、対象物体１０３に対して図示しない照明による照明光を当てる。この照明はセンサ部１０１の周りに配置されており、センサ部１０１が対象物体１０３の２次元画像を撮影するときに均一の照明環境下で撮影可能な状態にする。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

３次元位置姿勢計測部１１３は、センサ情報取得部１１１から出力された対象物体１０３の撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて、対象物体１０３の位置姿勢を求める。そして３次元位置姿勢計測部１１３は、求めた位置姿勢を、センサ位置姿勢決定部１１４および位置姿勢整合判断部１１７に対して出力する。

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

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

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

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

３次元位置姿勢計測部１１６は、センサ情報取得部１１５から出力された撮影画像と、モデル情報保持部１１２が保持しているモデル情報と、を用いて対象物体１０３の詳細な位置姿勢を求める。そして３次元位置姿勢計測部１１６は、求めた位置姿勢を位置姿勢整合判断部１１７に出力する。

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

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

位置姿勢整合判断部１１７は、３次元位置姿勢計測部１１３から出力された位置姿勢と、３次元位置姿勢計測部１１６から出力された位置姿勢と、が整合しているか否かを判断する。そして位置姿勢整合判断部１１７は、この判断の結果をロボット作業指示部１２１に対して送出する。即ち、整合していると判断した場合には、整合していることを示す情報と、３次元位置姿勢計測部１１６が求めた位置姿勢と、を判断の結果としてロボット作業指示部１２１に対して送出する。あるいは、整合していることを示す情報と、３次元位置姿勢計測部１１３が求めた位置姿勢と、３次元位置姿勢計測部１１６が求めた位置姿勢と、を判断の結果としてロボット作業指示部１２１に対して送出してもよい。一方、整合していないと判断した場合には、整合していないことを示す情報を、判断の結果としてロボット作業指示部１２１に対して送出する。位置姿勢整合判断部１１７による判断処理については後述する。

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

ロボット作業指示部１２１は、ハンド部を移動させる為の指示信号を生成する。例えば、ロボット作業指示部１２１は、センサ位置姿勢決定部１１４から第２の位置姿勢を受けると、ハンド部を第２の位置姿勢（移動先）に移動させるための指示信号を生成する。また、ロボット作業指示部１２１は、位置姿勢整合判断部１１７から出力された位置姿勢を受けると、該位置姿勢にある対象物体１０３を把持や吸着できる位置姿勢にハンド部を移動させて把持や吸着を行うことを指示するための指示信号を生成する。また、ロボット作業指示部１２１は、位置姿勢整合判断部１１７から整合していないことを示す情報を受けると、ハンド部を現在の位置姿勢とは異なる位置姿勢に移動させるための指示信号を生成する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

３次元位置姿勢計測部１１３は、以上のようにして求めた対象物体１０３’の位置姿勢を、センサ位置姿勢決定部１１４と位置姿勢整合判断部１１７とに対して出力する。なお、ステップＳ４０２で求めた対象物体１０３’の位置姿勢は、ステップＳ４０７で整合があると判断されるまで画像処理部１１０内の不図示のメモリに保持しておく。あるいはステップＳ４１２でロボットのハンド部の位置姿勢がｎ回変更されたと判断されるなどしてステップＳ４０１に戻るまで画像処理部１１０内の不図示のメモリに保持しておく。

ステップＳ４０３では、センサ位置姿勢決定部１１４は、ステップＳ４０２で求めた対象物体１０３’の位置姿勢から、センサ部１０２が対象物体１０３’を撮影する為の位置姿勢である第２の位置姿勢を求める。そしてセンサ位置姿勢決定部１１４は、求めた第２の位置姿勢をロボット作業指示部１２１に対して出力する。

なお、ステップＳ４０２により少なくとも１つの対象物体１０３の位置姿勢が求まるので、ここでは説明上、１つの対象物体１０３’の位置姿勢が求まっているものとしている。もちろん複数の対象物体１０３の位置姿勢が求まっていて、そのうちの１つを対象物体１０３’として選択してもよいことは言うまでもない。

第２の位置姿勢は、センサ部１０２が対象物体１０３’を撮影することができるのであれば、如何なる位置姿勢であってもよい。ただ、センサ部１０１のカメラと対象物体１０３’とを結ぶ直線と、センサ部１０２のカメラ光軸と、の角度差が所定の角度以上となることが望ましい。センサ部１０１とセンサ部１０２はそれぞれ同じ対象物体１０３’を撮影するため、センサ部１０１からの対象物体１０３’の見え方と、センサ部１０２からの対象物体１０３’の見え方は異なるほうがよい。一般的に、カメラを利用した３次元位置姿勢計測では、奥行き方向の計測精度は撮影画像面方向での計測精度よりも低い。例えばセンサ部１０１と対象物体１０３’とを結ぶ直線上でセンサ部１０２による対象物体１０３’の撮影を行うと、それぞれ対象物体１０３’を同じ方向から見ていることになるため、奥行き方向の精度の悪さを補い合うことができない。一方、角度をずらして別々の方向から対象物体１０３’を計測することで、奥行き方向の精度の悪さを補い合うことができる。また、それぞれ（一部ではあるが）異なる特徴を利用して位置姿勢計測することになるため、整合判断の信頼度が増す。

このように、センサ部１０１とセンサ部１０２とで別々の方向から対象物体１０３’を計測する。即ち、センサ部１０２に、センサ部１０１のカメラの位置と対象物体１０３’の位置とを通る直線と規定の角度をなす直線上の位置から、対象物体１０３’を撮影させる。これは、例えば、次の方法で実現できる。

まず、対象物体１０３’を中心にしてセンサ部１０１のカメラと対象物体１０３’とを結ぶ直線と予め定めた角度をなす直線上であって、センサ部１０２のカメラの焦点が合う対象物体１０３’からの距離範囲の位置姿勢を第２の位置姿勢の候補位置姿勢とする。そして、現在のセンサ部１０２の位置姿勢に最も位置の近い候補位置姿勢を第２の位置姿勢とする。最も近い位置を選択することでセンサ部１０２の移動時間を短くすることができるので、部品ピックのタクトタイムを短くすることができる。なお、カメラの焦点が合う距離範囲は、カメラの最短撮影距離と被写界深度とによって決まる。

ステップＳ４０４では、ロボット作業指示部１２１は、センサ位置姿勢決定部１１４から受けた第２の位置姿勢にロボット１００のハンド部を移動させるための指示信号を生成し、生成した指示信号をロボット制御部１２２に送出する。ロボット制御部１２２は、この指示信号に従ってロボット制御部１２２を制御し、ハンド部を第２の位置姿勢に移動させる。これにより、センサ部１０２は、第２の位置姿勢で対象物体１０３’を撮影することが出来る。

ステップＳ４０５では、ロボット１００に装着されたセンサ部１０２は、第２の位置姿勢で対象物体１０３’を撮影し、撮影した画像をセンサ情報取得部１１５に対して出力する。そしてセンサ情報取得部１１５は、センサ部１０２から取得した撮影画像を３次元位置姿勢計測部１１６に対して送出する。

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

モデル情報として３次元幾何モデルを利用する場合は、３次元幾何モデルから抽出した表面の点群と、センサ情報取得部１１５から出力された撮影画像から求めた距離画像点群との対応付けを行うことで対象物体の位置姿勢を求める。３次元幾何モデルの表面点群をＰとし、以下のように定義する。

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

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

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

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

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

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

３次元位置姿勢計測部１１６は、対象物体１０３’の位置姿勢を求めると、求めた位置姿勢を位置姿勢整合判断部１１７に対して出力する。

ステップＳ４０７では、位置姿勢整合判断部１１７は、３次元位置姿勢計測部１１３から出力された対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６から出力された対象物体１０３’の位置姿勢と、の整合性を判断する。この整合性の判断では、３次元的な位置姿勢の差異がないかどうかを判断することで行う。位置姿勢の差異が所定未満であれば整合しているとする。位置姿勢の差異が所定以上であれば整合していないとみなす。

まず、センサ部１０１による計測とセンサ部１０２による計測が１回ずつ行われている場合について説明する。３次元位置姿勢計測部１１３から出力された対象物体１０３’の位置姿勢をｓ、３次元位置姿勢計測部１１６から出力された対象物体１０３’の位置姿勢をｐとする。ｓをｐに変換する同次変換行列をＴとする。

（１６）〜（１８）よりＴの平行移動成分x，y，zと姿勢変換成分θx、θy、θzをそれぞれ求める。これらの値がいずれも所定の閾値未満であれば位置姿勢ｓと位置姿勢ｐとは位置姿勢の差異がほとんどないということで整合していると判断する。しかし、いずれかの値が所定の閾値以上であれば整合していないと判断する。

そして、位置姿勢整合判断部１１７は、整合していると判断した場合、整合していることを示す情報と、３次元位置姿勢計測部１１６から受けた位置姿勢と、をロボット作業指示部１２１に対して送出する。なお、３次元位置姿勢計測部１１６から受けた位置姿勢の代わりに、３次元位置姿勢計測部１１３から受けた位置姿勢と、３次元位置姿勢計測部１１６から受けた位置姿勢と、の平均位置姿勢を出力しても良い。この平均位置姿勢は、それぞれの位置姿勢に重み値を乗じた平均であっても良い。そして処理はステップＳ４０８に進む。

一方、位置姿勢整合判断部１１７は、整合していないと判断した場合には、整合していないことを示す情報を、ロボット作業指示部１２１に対して送出する。そして処理はステップＳ４１１に進む。

次に、センサ部１０１による計測が１回とセンサ部１０２による計測が２回以上行われてる場合について説明する。センサ部１０２による計測が２回以上行われるのは、ステップＳ４０７で整合していないと判断した場合はステップＳ４１１に進んでロボットハンドの位置姿勢を変更して再度ステップＳ４０５及びステップＳ４０６で対象物体１０３’の計測を行うためである。３次元位置姿勢計測部１１３から出力された対象物体１０３’の位置姿勢をｓ、３次元位置姿勢計測部１１６から出力された対象物体１０３’の位置姿勢をｐ（１）、ｐ（２）・・・ｐ（ｉ）（それぞれ１回目、２回目・・・ｉ回目）とする。なお、ここでは、センサ部１０２による計測がｉ回行われたとしている。

整合性の判定は、（ｓ，ｐ（１））、（ｓ，ｐ（２））・・・（ｓ，ｐ（ｉ））のうち少なくとも１つが整合しているか否かを判定することで行う。（ｓ、ｐ（ｋ））（ｋ＝１…ｉ）の整合性を判断する方法についてはセンサ部１０１による計測とセンサ部１０２による計測が１回ずつ行われている場合と同様である。即ち、同次変換行列Ｔの平行移動成分、姿勢変換成分のそれぞれが所定値未満かどうかで判断する。

そして、位置姿勢整合判断部１１７は、整合していると判断した場合、整合していることを示す情報と、位置姿勢（ｓ，ｐ（１））、…、（ｓ，ｐ（ｉ））のうち整合している位置姿勢の１つもしくは平均と、をロボット作業指示部１２１に対して送出する。そして処理はステップＳ４０８に進む。

あるいは、（ｓ，ｐ（１），ｐ（２）・・・ｐ（ｉ））のうちの少なくとも２つの位置姿勢の差異が所定未満である組があるか求め、組があれば整合しているとして、そのうちの１つあるいは平均を対象物体の位置姿勢としてもよい。この場合も求めた位置姿勢をロボット作業指示部１２１に対して送出する。位置姿勢の組を求めることができなければ整合していないとみなす。

一方、位置姿勢整合判断部１１７は、整合していないと判断した場合には、整合していないことを示す情報を、ロボット作業指示部１２１に対して送出する。そして処理はステップＳ４１１に進む。

ステップＳ４０８ではロボット作業指示部１２１は、位置姿勢整合判断部１１７から整合していることを示す情報を受けると、この情報と共に位置姿勢整合判断部１１７から受けた位置姿勢に基づいてロボット１００に作業を行わせるための指示信号を生成する。ロボット１００が対象物体１０３’を把持するハンドを備えていれば、この位置姿勢を有する対象物体１０３’の把持を指示する。ロボット１００が対象物体１０３’を吸着するパッドを備えていれば、この位置姿勢を有する対象物体１０３’の吸着を指示する。

ステップＳ４０９では、ステップＳ４０８で生成された指示信号が示す指示内容を実行するようロボット制御部１２２がロボット１００を制御する。ロボット１００が対象物体１０３’を把持する場合は、ロボット制御部１２２がロボット１００に対して対象物体１０３’を把持させるための制御を行う。ロボット１００が対象物体１０３’を吸着する場合は、ロボット制御部１２２がロボット１００に対して対象物体１０３’を吸着させるための制御を行う。

ステップＳ４１０では、画像処理部１１０の不図示の制御部は、不図示の指示部などから終了指示が入力されたか否かを判断する。この判断の結果、終了指示が入力された場合には、処理は終了する。一方、終了指示が入力されていない場合は、処理はステップＳ４０１に戻る。なお、ステップＳ４１０での終了判定を待たずに、ユーザが図示しない緊急停止ボタンを押すことによりフローを終了して全ての動作を停止してもよい。また、処理の終了条件は特定の条件に限ったものではない。

ステップＳ４１１では、ロボット作業指示部１２１は、整合していないことを示す情報を受けると、現在の位置姿勢とは異なる新たな位置姿勢を求める。そしてロボット作業指示部１２１は、この求めた位置姿勢にロボット１００のハンド部を移動させるための指示信号を生成し、生成した指示信号をロボット制御部１２２に送出する。然るにロボット制御部１２２は、この指示信号に従ってロボット１００を制御することで、ハンド部をこの新たな位置姿勢に移動させる。これにより、センサ部１０２は、この新たな位置姿勢で対象物体１０３’を撮影することが出来る。

あるいは、ロボット作業指示部１２１ではなくセンサ位置姿勢決定部１１４が新たな位置姿勢を求め、求めた位置姿勢をロボット作業指示部１２１に対して出力し、ロボット１００のハンド部を移動させてもよい。

ステップＳ４１１の処理は、具体的には以下の（１）〜（３）のいずれかの方法によって実現する。

（１） 対象物体１０３’を中心にして、センサ部１０１と対象物体１０３’とを結ぶ直線と、センサ部１０２のカメラの光軸と、で形成する面に対して垂直な方向に予め定めた角度だけセンサ部１０２を回転移動させる。センサ部１０２の光軸を、該面から外すことで、これまでセンサ部１０１及びセンサ部１０２で観察できていなかった対象物体１０３’の特徴（エッジなど）を観察することができる。そのため次回の整合判断時の判断材料が増える。

（２） センサ部１０１と対象物体１０３’とを結ぶ直線と、センサ部１０２のカメラの光軸と、がなす角度を一定に保つようにセンサ部１０２の位置姿勢を変更する。センサ部１０１と対象物体１０３’とを結ぶ直線を中心軸と考えると、センサ部１０２が移動するときのカメラの光軸の軌跡は円錐の側面をなす曲面（の少なくとも一部）となる。このようにすることでセンサ部１０２はパレット１０４から見て高さが大きく変化しない。そのためセンサ部１０２がパレット１０４やパレット１０４が置いてある面に衝突することがない。

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

あるいは、対象物体１０３’を中心にして、センサ部１０１と対象物体１０３’とを結ぶ直線と、センサ部１０２のカメラの光軸と、で形成する面に沿ってセンサ部１０２を回転移動させてもよい。あるいはセンサ部１０２のカメラ光軸に沿って近づけたり遠ざけたりしてもよい。あるいはセンサ部１０２の位置姿勢をランダムに変更してもよい。

ステップＳ４１２では、画像処理部１１０の不図示の制御部は、ステップＳ４１１で新たな位置姿勢を計算した回数（ハンド部の位置姿勢を変更した回数）ｃ（制御部によりカウントしている）が、規定回数であるｎ回に達したか否かを判断する。この判断の結果、ｎ回に達した場合には、対象物体１０３’以外の位置姿勢を計測対象とすべく処理はステップＳ４０１に戻り、まだ達していない場合は、処理はステップＳ４０５に戻る。

ステップＳ４０１で再度対象物体を撮影する前に図示しない揺動部によりパレット１０４を揺動させて対象物体１０３の山積み状態を変化させてもよい。もしくはステップＳ４０２で位置姿勢を求めた対象物体１０３’以外の対象物体を対象としてステップＳ４０５に戻りセンサ部１０２で計測を行うようにしてもよい。ステップＳ４０１に戻る際に回数ｃは０にリセットする。

以上の説明により、本実施形態によれば、センサ部１０２を用いて計測した結果が実際の対象物体の位置姿勢と異なっている場合に、誤った計測結果に基づいてロボットによる作業が行われることを防ぐことができる。また、これにより、把持〜把持失敗検知という無駄な処理が発生しないため、ロボットによる作業の効率化を行うことができる。

＜変形例＞
第１の実施形態の変形例として、センサ部１０１は対象物体に関する３次元情報を取得するセンサであっても良く、例えば、対象物体に対する距離を示す距離データを測定する距離センサであっても良い。

また、センサ部１０２も対象物体に関する３次元情報を取得するセンサであっても良く、例えば、対象物体に対する距離を示す距離データを測定する距離センサであっても良い。

この場合、センサ情報取得部１１１およびセンサ情報取得部１１５はそれぞれセンサ部１０１、センサ部１０２から距離データを取得し、３次元位置姿勢計測部１１３、１１６に出力する。３次元位置姿勢計測部１１３は、センサ情報取得部１１１から出力された距離データとモデル情報保持部１１２から出力される３次元幾何モデルの表面の点群データとの対応付けを行うことで対象物体の位置姿勢を求める。３次元位置姿勢計測部１１６は、センサ情報取得部１１５から出力された距離データとモデル情報保持部１１２から出力される３次元幾何モデルの表面の点群データとの対応付けを行うことで対象物体の位置姿勢を求める。距離データとモデル情報とを利用して対象物体の位置姿勢を求めるために、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。対象物体の位置姿勢を反復演算によって繰り返し補正する。なお、対象物体の位置姿勢を求めるための最適化手法はＩＣＰ法に限るものではない。

また、第１の実施形態の変形例として、センサ部１０１は対象物体に関する２次元情報（２次元画像）と３次元情報（距離データ）とを取得するセンサ（カメラと距離センサとの組み合わせ）であってもよい。また、センサ部１０２も対象物体に関する２次元情報（２次元画像）と３次元情報（距離データ）とを取得するセンサ（カメラと距離センサとの組み合わせ）であってもよい。２次元画像とモデル情報との対応付け、および距離データとモデル情報との対応付けを同時に解く方法は周知であり、例えば、特開２０１１−２７６２３号公報に開示されている方法を適用することができる。

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

［第２の実施形態］
第１の実施形態では、３次元位置姿勢計測部１１３が求めた位置姿勢と、３次元位置姿勢計測部１１６が求めた位置姿勢と、の整合性を、３次元的な位置姿勢の差異に基づいて判断した。本実施形態では、整合性を３次元幾何モデルから抽出した幾何特徴群同士の対応付け誤差に基づいて判断する。

本実施形態に係るシステムの構成は第１の実施形態と同様であるが、位置姿勢整合判断部１１７の動作のみが第１の実施形態と異なる。そのため、本実施形態では、図４のフローチャートにおいてステップＳ４０７における処理のみが、第１の実施形態と異なる。以下では、第１の実施形態と異なる点のみについて説明し、それ以外については第１の実施形態と同様であるため、説明を省略する。

ステップＳ４０７では、位置姿勢整合判断部１１７は、以下に列挙する（ａ）〜（ｄ）の何れかを実行することで、位置姿勢の整合性を判断する。なお、（ａ）〜（ｄ）では幾何特徴としてエッジ、点、面を用いているが、幾何特徴はこれらに限られるものではない。

（ａ） エッジ対応付けで誤差を求める場合
３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とに基づいて、例えばセンサ部１０２の位置姿勢から見た対象物体の３次元幾何モデルをそれぞれ２次元画像に投影する。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づくモデル投影像をＭＡ、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づくモデル投影像をＭＢとする。

モデル投影像ＭＡのエッジ成分上に複数の制御点を設け、２次元画面内でエッジに対して垂直な方向にモデル投影像ＭＢのエッジ成分を探索する。モデル投影像ＭＢの対応点が見つかったら、制御点から対応点までの画素数（誤差）を求める。モデル投影像ＭＡの複数の制御点について同様にモデル投影像ＭＢの対応点を探索し、制御点から対応点までの画素数（誤差）の平均値を求める。平均値が予め定めた値未満であれば、誤差が少ないと判断でき、３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とは整合していると判断できる。平均値が予め定めた値以上であれば、誤差が多いため整合していないと判断できる。

（ｂ） 点群対応付けで誤差を求める場合
３次元位置姿勢計測部１１３で求めた位置姿勢と、３次元位置姿勢計測部１１６で求めた位置姿勢とに基づいて、それぞれ３次元幾何モデルから抽出した表面の点群データをパレットが置かれた空間に固定されたワークスペース座標系内に配置する。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づく点群をＭＡ’、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づく点群モデルをＭＢ’とする。

点群ＭＡ’のある１つの点から最も近い点群ＭＢ’の点までの距離（誤差）を求める。点群ＭＡ’の複数の点について同様に点群ＭＢ’の再近傍点を探索し、点間距離（誤差）の平均値を求める。平均値が予め定めた値未満であれば、誤差が少ないと判断できる。よって、３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とは整合していると判断できる。平均値が予め定めた値以上であれば、誤差が多いため整合していないと判断できる。

（ｃ） 点群とモデルとの対応付けで誤差を求める場合
３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づく３次元幾何モデルから抽出した表面の点群データと、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づく３次元幾何モデルとの誤差を求める。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づく点群をＭＡ’、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づく３次元幾何モデルをＭＢ”とする。

点群ＭＡ’のある１つの点から最も近いモデルＭＢ”の面までの距離（誤差）を求める。点群ＭＡ’の複数の点について同様にモデルＭＢ”の面までの距離（誤差）の平均値を求める。平均値が予め定めた値未満であれば、誤差が少ないと判断できる。よって、３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とは整合していると判断できる。平均値が予め定めた値以上であれば、誤差が多いため整合していないと判断できる。

点群データと３次元幾何モデルとは逆の関係にしてもよいことは言うまでもない。即ち、３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づく３次元幾何モデルと、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づく３次元幾何モデルから抽出した表面の点群データとの誤差を求めてもよい。

（ｄ） モデル対応付けで誤差を求める場合
３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とに基づいて、それぞれ３次元幾何モデルをパレットが置かれた空間に固定されたワークスペース座標系内に配置する。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づくモデルをＭＡ”、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づくモデルをＭＢ”とする。

モデルＭＡ”のある面の中心点から対応するモデルＭＢ”の面の中心点までの距離（誤差）を求める。モデルＭＡ”の複数の面の中心点について同様にモデルＭＢ”の対応点を探索し、点間距離（誤差）の平均値を求める。平均値が予め定めた値未満であれば、誤差が少ないと判断できる。よって、３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とは整合していると判断できる。平均値が予め定めた値以上であれば、誤差が多いため整合していないと判断できる。

［第３の実施形態］
第１の実施形態の変形例として、センサ部１０１とセンサ部１０２とを同じセンサ部で構成してもよい。具体的には、ロボットに装着したセンサ部１０２にセンサ部１０１の役割も持たせることで実現する。

まず、センサ部１０２がパレット１０４の上方で第１の位置姿勢で対象物体を撮影することができるようにロボットを制御する。センサ部１０２が第１の位置姿勢で対象物体を撮影して３次元位置姿勢計測部１１３で位置姿勢を計測すると、次に第２の位置姿勢を決定する。第２の位置姿勢は、第１の実施形態で説明したとおりに求める。そして、ロボットを制御することでセンサ部１０２を第２の位置姿勢に移動させると、対象物体の位置姿勢を３次元位置姿勢計測部１１６で計測する。そしてセンサ部１０２を第１の位置姿勢に移動させて求めた対象物体の位置姿勢と、第２の位置姿勢に移動させて求めた対象物体の位置姿勢との整合性を判断する。整合性の判断方法および判断した後の処理については第１，２の実施形態で説明したとおりである。

また、ステップＳ４０７で整合性の判断を行い、整合していないと判断したとき、センサ部１０２を移動させる前に、３次元位置姿勢計測部１１３もしくは３次元位置姿勢計測部１１６での画像処理パラメータを変更して再度対象物体の位置姿勢を求めてもよい。変更する画像処理パラメータとしては、エッジ検出の閾値、図５に示す制御点の密度、３次元幾何モデルから抽出する表面の点群密度などがあげられる。なお、変更するパラメータはこれらに限られないことは言うまでもない。画像処理パラメータを変更して整合性があると判断した場合はセンサ部１０２を移動させずにステップＳ４０８に進む。こうすることでセンサ部１０２を動かす手間を省くことができる。画像処理パラメータを変更しても整合性がないと判断した場合はセンサ部１０２の移動を行うべくステップＳ４１１に進む。

また、ステップＳ４０７で整合性の判断を行い、整合していないと判断したとき、ユーザが状況を把握しやすいようにロボットの表示器やランプなどを用いて警告してもよい。さらに、ステップＳ４１２でｎ回変更したと判断されたとき、ステップＳ４０１に戻らずにユーザにうまくいかない原因を調べさせるため、警告すると同時にロボットシステムを停止してもよい。

また、ステップＳ４０７での整合性の判断では、モデル投影像の一致度によって判断してもよい。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢と３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢とに基づいて、例えばセンサ部１０２から第２の位置姿勢で見た対象物体の３次元幾何モデルをそれぞれ２次元画像に投影する。３次元位置姿勢計測部１１３で求めた対象物体１０３’の位置姿勢に基づくモデル投影像をＭＡ、３次元位置姿勢計測部１１６で求めた対象物体１０３’の位置姿勢に基づくモデル投影像をＭＢとする。一致度は投影像ＭＡおよびＭＢの各画素の輝度差から求めることができる。一致度と予め用意した閾値との比較を行い、整合しているか整合していないかの判断を行う。

また、上記ではモデル情報保持部１１２に保持されたモデルを利用した位置姿勢計測について説明を行った。しかし、位置姿勢計測方法は他の方法も考え得る。例えば、センサ部１０１およびセンサ部１０２をそれぞれステレオカメラで構成し、スレテオカメラによる撮影画像からステレオ計測技術を用いて対象物体の位置姿勢計測を行ってもよい。

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

なお、第１〜３の実施形態（変形例を含む）では様々な構成、様々な変形例について説明したが、何れも下記の構成の一例であり、下記の構成に沿ったものであれば、上記で説明した構成以外の構成を採用しても良い。

第１の位置姿勢を有する第１のセンサによる計測結果から対象物体の位置姿勢を求め（第１の計算）、第１の位置姿勢とは異なる第２の位置姿勢に第２のセンサを移動させ、該第２のセンサによる計測結果から対象物体の位置姿勢を求める（第２の計算）。

そして、第１の計算による位置姿勢と第２の計算による位置姿勢とが規定の条件を満たしている場合に、第２の計算による位置姿勢、若しくは第１の計算による位置姿勢及び第２の計算による位置姿勢から計算される位置姿勢、を出力する。

ここで、第１の計算、第２の計算について、第１の実施形態では、次のような処理を行う。第１の計算では、第１のセンサから、対象物体の撮影画像および／または該対象物体までの距離を表す距離データを、第１の測定データとして取得する。そして、対象物体又は該対象物体を模した仮想物体を複数の視点から見た画像群、若しくは該仮想物体を対象物体情報とし、該対象物体情報と、第１の測定データと、を用いて対象物体の位置姿勢を求める。第２の計算では、第２の位置姿勢に第２のセンサを移動させ、該第２のセンサから、対象物体の撮影画像および／または該対象物体までの距離を表す距離データを第２の測定データとして取得する。そして、対象物体情報と、第２の測定データと、を用いて対象物体の位置姿勢を求める。

第３の実施形態では、第１の計算、第２の計算ではそれぞれ、スレテオカメラとしての第１，２のセンサによる対象物体の撮影画像を用いたステレオ計測技術を用いて対象物体の位置姿勢を求める。

また、「規定の条件」は、第１の実施形態では、「第１の計算による位置姿勢と第２の計算による位置姿勢との差異が規定量よりも小さいこと」、である。第２の実施形態では、「第１の計算による位置姿勢を有する仮想物体を画面上に投影した投影像と、第２の計算による位置姿勢を有する仮想物体を画面上に投影した投影像と、の差異が規定量よりも小さいこと」、である。

［第４の実施形態］
図１に示した各部は適宜統合／分割しても良い。例えば、画像処理部１１０内の各部を１つの装置としても良いし、いくつかの構成要件を１つの装置としても良い。また、画像処理部１１０とロボットコントローラ部１２０とを統合して１つ装置とするようにしても良い。

また、画像処理部１１０を構成する各部はハードウェアで構成しても良いが、ソフトウェア（コンピュータプログラム）で構成しても良い。この場合、画像処理部１１０内のＲＡＭ等のメモリにこのコンピュータプログラムをインストールし、画像処理部１１０内のＣＰＵがこのコンピュータプログラムを実行することで、ＣＰＵは図１に示した画像処理部１１０内の各部の機能を実現することが出来る。

また、第１〜３の実施形態（変形例も含む）で説明した各技術は適宜組み合わせて用いてもかまわないし、ある実施形態で説明した部分的な技術を他の実施形態に組み込んで使用してもかまわない。

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

Claims (13)

1. 第１の位置姿勢を有する第１のセンサによる計測結果から、対象物体の位置姿勢を求める第１の計算手段と、
   前記第１の位置姿勢とは異なる第２の位置姿勢に第２のセンサを移動させ、該第２のセンサによる計測結果から、前記対象物体の位置姿勢を求める第２の計算手段と、
   前記第１の計算手段が計算した位置姿勢と前記第２の計算手段が計算した位置姿勢とが規定の条件を満たしている場合に、前記第２の計算手段が計算した位置姿勢、若しくは前記第１の計算手段が計算した位置姿勢及び前記第２の計算手段が計算した位置姿勢から計算される位置姿勢、を出力する出力手段と
   を備えることを特徴とする情報処理装置。
2. 前記第１の計算手段は、
   前記第１のセンサから、前記対象物体の撮影画像および／または該対象物体までの距離を表す距離データを、第１の測定データとして取得し、
   前記対象物体又は該対象物体を模した仮想物体を複数の視点から見た画像群、若しくは該仮想物体を対象物体情報とし、該対象物体情報と、前記第１の測定データと、を用いて前記対象物体の位置姿勢を求める
   ことを特徴とする請求項１に記載の情報処理装置。
3. 前記第２の計算手段は、
   前記第２の位置姿勢に前記第２のセンサを移動させ、該第２のセンサから、前記対象物体の撮影画像および／または該対象物体までの距離を表す距離データを第２の測定データとして取得し、
   前記対象物体情報と、前記第２の測定データと、を用いて前記対象物体の位置姿勢を求める
   ことを特徴とする請求項２に記載の情報処理装置。
4. 前記第２の計算手段は、
   前記第１の計算手段が求めた位置と前記第１のセンサの位置とを通る直線と規定の角度以上をなす直線上の位置に前記第２のセンサを移動させ、該第２のセンサから前記第２の測定データを取得する
   ことを特徴とする請求項３に記載の情報処理装置。
5. 前記第２の計算手段は、
   前記第２のセンサの位置姿勢を制御可能なコントローラに対して前記第２の位置姿勢を前記第２のセンサの移動先として指示することで、前記コントローラに、前記第２のセンサを前記第２の位置姿勢に移動させることを特徴とする請求項２乃至４の何れか１項に記載の情報処理装置。
6. 前記出力手段は、
   前記第１の計算手段が計算した位置姿勢と前記第２の計算手段が計算した位置姿勢とが規定の条件を満たさなかったたびに、現在の前記第２のセンサの位置姿勢とは異なる位置姿勢を求め、該求めた位置姿勢に前記第２のセンサを移動させることを特徴とする請求項５に記載の情報処理装置。
7. 前記第２の計算手段は、
   前記第２のセンサが移動するたびに、該第２のセンサから取得した前記第２の測定データと、前記対象物体情報と、を用いて前記対象物体の位置姿勢を求め、
   前記出力手段は、
   前記第２の計算手段がこれまでに計算したそれぞれの位置姿勢のうち、前記第１の計算手段が計算した位置姿勢との差異が規定量よりも小さい位置姿勢もしくはその平均を対象物体の位置姿勢とする
   ことを特徴とする請求項６に記載の情報処理装置。
8. 前記第２の計算手段は、
   前記第２のセンサが移動するたびに、該第２のセンサから取得した前記第２の測定データと、前記対象物体情報と、を用いて前記対象物体の位置姿勢を求め、
   前記出力手段は、
   前記第２の計算手段がこれまでに計算したそれぞれの位置姿勢と前記第１の計算手段が計算した位置姿勢との中で差異が規定量よりも小さい少なくとも２つの位置姿勢の組を求め、その１つもしくは平均を対象物体の位置姿勢とする
   ことを特徴とする請求項６に記載の情報処理装置。
9. 前記規定の条件は、前記第１の計算手段が計算した位置姿勢と前記第２の計算手段が計算した位置姿勢との差異が規定量よりも小さいこと、であることを特徴とする請求項１乃至８の何れか１項に記載の情報処理装置。
10. 前記規定の条件は、前記第１の計算手段が計算した位置姿勢に基づく幾何特徴群と、前記第２の計算手段が計算した位置姿勢に基づく幾何特徴群と、の差異が規定量よりも小さいこと、であることを特徴とする請求項１乃至８の何れか１項に記載の情報処理装置。
11. 前記第２のセンサは、ロボットに装着され、
    前記コントローラは、前記ロボットの位置姿勢を制御することを特徴とする請求項５に記載の情報処理装置。
12. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の第１の計算手段が、第１の位置姿勢を有する第１のセンサによる計測結果から、対象物体の位置姿勢を求める第１の計算工程と、
    前記情報処理装置の第２の計算手段が、前記第１の位置姿勢とは異なる第２の位置姿勢に第２のセンサを移動させ、該第２のセンサによる計測結果から、前記対象物体の位置姿勢を求める第２の計算工程と、
    前記情報処理装置の出力手段が、前記第１の計算工程で計算した位置姿勢と前記第２の計算工程で計算した位置姿勢とが規定の条件を満たしている場合に、前記第２の計算工程で計算した位置姿勢、若しくは前記第１の計算工程で計算した位置姿勢及び前記第２の計算工程で計算した位置姿勢から計算される位置姿勢、を出力する出力工程と
    を備えることを特徴とする情報処理方法。
13. コンピュータを、請求項１乃至１１の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images