JP6180087B2 - 情報処理装置及び情報処理方法 - Google Patents

Description

本発明は情報処理装置及び情報処理方法に関し、特に、３次元形状が既知である物体の位置及び姿勢を計測するために用いて好適な技術に関する。

近年、これまで人間が行なっていた工業製品の組立のような複雑なタスクをロボットが代わりに行うようになりつつある。ロボットによる工業製品の組立では、ハンドなどのエンドエフェクタによって部品を把持するために、部品とロボット（ハンド）との間の相対的な位置及び姿勢を高精度に計測する必要がある。

物体の位置及び姿勢を高精度に計測できる技術として、カメラから得られる２次元画像（濃淡画像・カラー画像）と距離センサから得られる距離画像を同時に利用して位置姿勢を推定する技術が、以下に示す文献で開示されている。非特許文献１では、距離画像中の３次元点群とともに、２次元画像上の特徴を奥行きが不定な３次元の点として扱い、物体の位置及び姿勢を計測する手法が開示されている。

また、非特許文献２では、２次元の濃淡画像上で計量される誤差と距離画像空間内で計量される誤差とを、最尤推定に基づく定式化により同時に最適化することで、高精度に位置及び姿勢を計測する手法が開示されている。

特開２００５−２０１８２４号公報

Ｙ． Ｈｅｌ‐Ｏｒ ａｎｄ Ｍ． Ｗｅｒｍａｎ， "Ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｂｙ ｆｕｓｉｎｇ ｎｏｉｓｙ ｄａｔａ ｏｆ ｄｉｆｆｅｒｅｎｔ ｄｉｍｅｎｓｉｏｎｓ， " ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｖｏｌ．１７，ｎｏ．２， ｐｐ．１９５−２０１， １９９５． 立野，小竹，内山，"実部品のビンピッキングのための距離・濃淡画像を最尤に統合する高精度高安定なモデルフィッティング手法"，第１３回画像の認識・理解シンポジウム（ＭＩＲＵ２０１０），ＯＳ５−１，２０１０．

従来技術である２次元画像と距離画像を併用した位置姿勢推定手法では、２次元画像撮像時と、距離画像撮像時とで、撮像装置と計測対象物体との間の相対的な位置関係が変化しないことが前提である。

一方、パターン投影法などで距離画像を撮影する場合、２次元画像と距離画像とで、異なる照明パターンを当てなければならず、原理的に同時刻に濃淡画像と距離画像を撮影することが出来ない。したがって、例えば、ロボットアーム上に撮像装置を設置して当該装置を移動させながら物体の位置及び姿勢を計測する場合。或いは、ベルトコンベアーによって移動している物体の位置及び姿勢を計測する場合では、２次元画像撮像時と距離画像撮像時で撮像装置と計測対象物体との間の位置関係が異なってしまう。このため、従来技術により物体位置姿勢の推定を行った場合、精度の低下や処理の破綻が発生してしまう。

一方、ロボットアームなどによって撮像装置または計測対象物体が移動する場合は、ロボットのモーション情報を参照することで、撮像時刻間における撮像装置または計測対象物体の移動量を取得することが可能である。このような撮像時刻間に移動する位置姿勢差分量を利用して、撮像時刻と撮像位置姿勢とが異なる複数の画像から計測対象物体の位置姿勢を推定する技術として、例えば特許文献１に記述されるような、モーションステレオと呼ばれる技術が公知としてある。

しかし、これまで提案されているロボットモーション情報を用いたモーションステレオは、２次元画像のみを利用することを前提としたものである。このため、距離画像を用いて、または、２次元画像と距離画像を併用して用いて位置姿勢を推定する場合に適用できない問題点があった。
本発明は前述の問題点に鑑み、撮像装置と計測対象物体との間の位置または／及び姿勢の関係が高速に変化している場合であっても、２次元画像及び距離画像を用いて、当該物体の位置及び姿勢の計測を精度良く行えるようにすることを目的とする。

本発明の情報処理装置は、２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理装置であって、被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存手段と、第１の位置姿勢の前記撮像装置により第１の時刻に撮影された２次元画像を入力する２次元画像入力手段と、第２の位置姿勢の前記撮像装置により前記第１の時刻と異なる第２の時刻に撮像された距離画像を入力する距離画像入力手段と、前記第１の位置姿勢と前記第２の位置姿勢の相対位置姿勢情報である位置姿勢差分情報を取得する相対位置姿勢情報入力手段と、前記位置姿勢差分情報に基づき、前記２次元画像及び前記距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定手段と、を有することを特徴とする。

本発明によれば、２次元画像または距離画像の撮像間において計測対象物体または計測装置が移動する場合においても、２次元画像と距離画像を併用した高精度な対象物体の位置姿勢推定処理を行うことが可能となる。また、計測対象物体または計測装置が移動しながら距離画像のみを撮像する場合においても、複数の視点から撮像された距離画像を併用して位置姿勢推定処理を行うことで、高精度な推定が可能となる。

本発明の実施形態における情報処理装置の構成例を示す図である。 ロボットアームと撮像装置、計測対象部品との関係を説明する図である。 時刻間に撮像装置が移動している場合撮像装置の状態を説明する図である。 位置姿勢推定方法の処理手順を示すフローチャートである。 位置姿勢算出の詳細な処理手順を説明するフローチャートである。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
［ハンドアイロボット系による実施形態］
本実施形態では、情報処理装置を、図２（ａ）に示すような撮像装置が先端に装着されたロボットアームが移動しながら、固定された計測対象物体５０を撮像するシーンにおける、物体位置姿勢推定手法に適用した場合について説明する。

本実施形態では、１回の画像の取得は、濃淡画像を撮影した後、距離画像を撮影するという順番で行う。濃淡画像の撮影と距離画像の計測は、基本的に異なる時刻において実行されるため、時刻間に撮像装置が移動している場合は、図３中３１０及び３２０に示すように、２種の画像の撮像時における物体と撮像装置との相対的な位置姿勢は異なる。この撮像時刻間に移動する撮像装置の位置姿勢差分量を、ロボットアームのモーション情報から取得し、２次元画像及び距離画像と合わせて利用することで、撮像装置と計測対象物体５０の相対的な位置及び姿勢を高精度に推定する。

図１は、本発明の一実施の形態に係わる情報処理装置１の構成の一例を示す図である。情報処理装置１は、２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する。情報処理装置１は、位置姿勢推定用照合データ保存部１１０と、２次元画像入力部１２０と、距離画像入力部１３０と、概略位置姿勢入力部１４０と、相対位置姿勢情報入力部１５０と、位置姿勢算出部１６０とを具備して構成される。

また、位置姿勢推定用照合データ保存部１１０は、位置姿勢推定用照合データ１０を保存し、位置姿勢算出部１６０に接続されている。２次元画像撮像装置２０は、２次元画像入力部１２０に接続されている。距離画像撮像装置３０は、距離画像入力部１３０に接続されている。ロボット４０は、相対位置姿勢情報入力部１５０に接続されている。なお、本実施形態では、位置姿勢推定用照合データ保存部１１０に保存された位置姿勢推定用照合データ１０が、現実に撮像される観察対象物体の形状に即していることが情報処理装置１を適用できる条件となる。

次に、情報処理装置１を構成する各部について説明する。
位置姿勢推定用照合データ１０は、計測対象となる物体（以下、計測対象物体、又は単に物体と呼ぶ場合もある）の形状を表す３次元幾何情報である。以下、位置姿勢推定用照合データ１０を３次元形状モデルと呼称する。３次元形状モデルは、点の集合や各点を結んで構成される面の情報、面を構成する線分の情報によって定義される。３次元形状モデルは、位置姿勢推定用照合データ保存部１１０に保存され、位置姿勢算出部１６０に入力される。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では２次元画像撮像装置２０は濃淡画像を出力する。２次元画像撮像装置２０が第１の位置姿勢において撮影した２次元画像は、２次元画像入力部１２０を介して、情報処理装置１に入力される。

カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、または、[Ｒ． Ｙ． Ｔｓａｉ、 "Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ、 " ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ、 ｖｏｌ．ＲＡ−３、 ｎｏ．４、 １９８７．]、で開示される方法により事前にキャリブレーションしておく。

距離画像撮像装置３０は、計測対象である被測定物体の表面上の点の３次元情報を計測するために、第２の位置姿勢において距離画像を撮像する。撮像した距離画像は、距離画像入力部１３０を介して、情報処理装置１に入力される。距離画像撮像装置３０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとしては、波長の異なる色ＩＤを付与したマルチスリットラインを対象に照射し、その反射光をカメラで撮像して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。

その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。距離画像撮像装置３０が計測した距離画像は、距離画像入力部１３０を介して情報処理装置１に入力される。また、距離画像撮像装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する濃淡画像の各画素と、距離画像撮像装置が出力する距離画像の各画素の対応は既知であるとする。

ロボット４０は、回転または並進移動軸からなる可動軸を複数有し、２次元画像撮像装置２０及び距離画像撮像装置３０からなる撮像装置の位置姿勢を変更するための可動機器である。以下、２次元画像撮像装置２０と距離画像撮像装置３０を合わせた機器を撮像装置２３０と称する。本実施形態では、アーム先端に撮像装置２３０を装着した、６軸の回転可動軸により構成される６自由度ロボットアームを用いる。ロボットの６軸可動軸の情報は、相対位置姿勢情報入力部１５０を介して、情報処理装置１に入力される。

アーム先端に装着された撮像装置２３０の、アーム先端から撮像装置２３０までの位置姿勢は、撮像装置２３０のオフセット位置姿勢としてあらかじめ算出しておき、以後変化しないものとして保持する。撮像装置２３０の位置姿勢は、アーム先端の位置姿勢からオフセット位置姿勢をかけることで算出できる。

なお、ロボット４０として用いる機器はこれに限るものではく、例えば、7軸回転軸を有する垂直多関節型ロボットであってもよいし、スカラ型ロボットやパラレルリンクロボットであってもよい。その他、回転または並進移動軸からなる可動軸を複数有し、モーション情報が取得できる限り、いかなる形式のロボットであってもよい。

概略位置姿勢入力部１４０は、２次元画像撮像装置２０と距離画像撮像装置３０からなる撮像装置２３０に対する物体の位置及び姿勢の概略値を入力する。本実施形態では、情報処理装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。

しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものでもよい。

また、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、対象物体の大まかな位置と姿勢を推定してもよい。あるいは、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。

また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめ分かっている場合にはその値を概略値として用いる。

相対位置姿勢情報入力部１５０は、２次元画像の撮像時刻から距離画像の撮像時刻の間に、ロボット４０が移動した位置姿勢差分量を入力する。本実施形態では、ロボット４０内の制御モジュールからアーム先端の位置及び姿勢が直接取得できる場合を前提として、撮像時刻間のアーム先端位置姿勢の差分から位置姿勢差分量を算出する。

しかしながら、撮像時刻間の撮像装置２３０の移動量を算出する方法は、これに限るものでなく、例えば、ロボット４０の可動軸に合わせて装着されたロータリーエンコーダから各軸回転量を取得する。そして、各軸回転量とロボットのリンク長さから、順運動学計算によりアーム先端位置の位置及び姿勢を算出することにより、アーム先端に設置された撮像装置２３０の位置姿勢差分量を計算してもよい。撮像時刻間に移動するアーム先端の位置姿勢差分量が算出できる限り、いずれの手法でもよい。

位置姿勢算出部１６０では、２次元画像撮像装置２０及び距離画像撮像装置３０により撮像された濃淡画像及び距離画像に対して、位置姿勢推定用照合データ保存部１１０に保存された３次元形状モデルを当てはめて、計測対象物体５０の位置及び姿勢を計測する。位置姿勢算出処理の詳細については後述する。

以上が、情報処理装置１の構成の一例についての説明である。なお、情報処理装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。

次に、本実施形態における位置姿勢推定方法の処理手順について説明する。
図４は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。
まず、Ｓ４０１では濃淡画像の撮影と概略位置姿勢の取得を行う。本実施形態では、２次元画像撮像装置２０から濃淡画像を撮像する。そして、濃淡画像撮像時刻における撮像装置２３０と物体との相対的な位置姿勢の概略値（図３：３１０）を、概略位置姿勢入力部１４０により、位置姿勢算出部１６０に入力する。本実施形態では、前述したように、前の時刻において計測された位置及び姿勢を、物体の概略位置姿勢として用いる。

次に、Ｓ４０２では、距離画像撮像装置３０からの距離画像の取得を行う。本実施形態では、距離画像は撮像装置２３０から計測対象物体５０の表面までの距離が格納されているものとする。前述のように、２次元画像撮像装置２０と距離画像撮像装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。

次に、Ｓ４０３では、相対位置姿勢情報入力部１５０から、撮像時刻間に移動した撮像装置２３０の位置姿勢差分量の算出を行う。まず、相対位置姿勢情報入力部１５０を介し、ロボット４０内の制御モジュールからロボットアーム先端の位置姿勢を取得する。そして、ロボットアーム先端から撮像装置２３０までの位置姿勢オフセットをかけることで、濃淡画像と距離画像各々の撮像時刻における撮像装置２３０の位置姿勢を計算する。

そして、両時刻における撮像装置２３０の位置姿勢の差分を求めることで、撮像時刻間に撮像装置２３０が移動した位置姿勢差分量（図３：３３０）を算出する。この位置姿勢差分量を、濃淡画像撮像時刻における概略位置姿勢（図３：３１０）にかけることで、距離画像撮像時刻における概略位置姿勢（図３：３２０）が算出できる。

Ｓ４０４では、Ｓ４０１及びＳ４０２において入力された濃淡画像及び距離画像と３次元形状モデルとの対応を算出し、対応付けた結果に基づいて計測対象物体５０の位置及び姿勢を算出する。ここで、本実施形態では、Ｓ４０３で算出した距離画像撮像時における撮像装置２３０と計測対象物体との相対位置姿勢の概略値（図３：３２０）を基準にして、計測対象物体５０の位置及び姿勢の算出処理を行う。

以下、図５を用いて、Ｓ４０４における位置姿勢算出処理について詳細な処理を詳述する。
まず、距離画像撮像時の対象物体の概略位置姿勢に基づいて、Ｓ４０２において計測された距離画像中の３次元点群と、３次元形状モデルとの対応付けを行う。距離画像撮像時の概略位置姿勢と校正済みの距離画像撮像装置３０の内部パラメータを用いて、３次元形状モデルを構成する各面を、距離画像上に投影する。そして、投影した各面に対応する距離画像上の距離点群を、各面に対応する３次元点として保持する。

次に、濃淡画像上のエッジと３次元形状モデルとの対応付けを行う。まず、距離画像撮像時の概略位置姿勢（図３：３２０）を基準に、Ｓ４０３で算出した撮像時刻間の撮像装置２３０の位置姿勢差分量（図３：３３０）を引くことで、濃淡画像撮像時の概略位置姿勢（図３：３１０）を算出する。そして、算出した概略位置姿勢と校正済みの２次元画像撮像装置２０の内部パラメータを用いて、３次元形状モデルの各線分を画像へ投影し、画像上で検出されたエッジと、３次元形状モデルとを対応付けする。エッジが、各制御点に対応して複数検出された場合には、複数検出されたエッジのうち、投影された線分に対して画像上で最も近いエッジを対応付ける（Ｓ５０１）。

次に、Ｓ５０２において、Ｓ５０１で検出した、３次元形状モデル中の各線分に対応する濃淡画像上のエッジと、３次元形状モデル中の各面に対応する距離画像中の３次元点との対応データに基づいて、計測対象物体５０の位置及び姿勢を算出する。本ステップでは、算出した対応データに基づいて、計測データと３次元形状モデルとの間の誤差が最小になるように、線形連立方程式を解くことで、位置姿勢の更新を行う。

ここで、画像上の距離と３次元空間中での距離は尺度が異なるため、単純に連立方程式を解くだけでは計測データのどちらか一方に寄与率が偏ってしまう。そのため、本実施形態では、非特許文献２に示すような、最尤推定に基づく最適化を行うことで、尺度を合わせた位置姿勢推定処理を行う。最尤推定に基づく位置姿勢推定処理を行う方法に関しては、本発明の本質に関わる話ではないため、詳細な処理の記述は省略する。詳細は非特許文献２を参照されたい。

なお、計測対象物体５０の位置及び姿勢の算出方法は、非特許文献２の手法に限るものでなく、例えば、Levenberg-Marquardt法による繰り返し演算を行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化計算手法を用いてもよい。

次に、Ｓ５０３において、Ｓ５０２で更新した位置姿勢が、収束しているか否か（すなわち、さらに反復計算を必要とするか否か）の判定を行う。収束していなければ、再度、更新した位置姿勢を用いて、Ｓ５０１の対応付け処理を行う。なお、収束しているか否かは、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。収束していると判断した場合は、この処理は終了し、撮像装置２３０と計測対象物体５０間の相対的な位置姿勢の最終的な推定値が決定される。

図４のフローチャートの説明に戻る。
Ｓ４０５では、Ｓ４０４で推定された位置姿勢に基づいて、ロボット４０に制御指令値を出力する。本実施形態では、Ｓ４０４において推定する前の計測対象物体５０の位置姿勢と、推定後の計測対象物体５０の位置姿勢との差分を、ロボット座標系に変換することによって、ロボット４０に出力する制御指令値を計算する。そして、制御指令値をロボット４０の制御モジュールに出力することで、ロボット４０のアーム先端を計測対象物体５０へと移動させるべく、位置姿勢操作を行う。ロボット４０への制御指令値を出力したら、Ｓ４０５の処理を終了し、Ｓ４０６に進む。

Ｓ４０６では、本位置姿勢推定処理の動作を終了する入力がなされたかどうかを判定する。入力された場合には終了し、入力されなかった場合にはＳ４０１に戻り、新たな画像を取得して再度位置姿勢算出を行う。

前述したように、本実施形態では、２次元画像及び距離画像の撮像間において撮像装置２３０が移動する場合においても、撮像装置２３０の位置姿勢移動差分情報をロボットの可動軸情報から取得するようにした。これにより、２次元画像と距離画像を併用した高精度な位置姿勢推定処理を行うことが可能となる。

次に、前述した実施形態の変形例を説明する。
［第１の変形例（距離画像と濃淡画像を撮像する順番は問わない例）］
前述した実施形態では、画像の取得順として、濃淡画像を撮影した後、距離画像を撮影する事例について説明していた。しかしながら、画像撮影順は、特に制限があるものではなく、濃淡画像と距離画像が正常に撮影できる限り、どちらが先でもよい。

例えば、距離画像を先に撮像し、濃淡画像を後に撮像してもよい。この場合、Ｓ４０１では距離画像を撮像し、Ｓ４０２で濃淡画像を撮像するという、前述の実施形態とは逆の手順となる。同様に、Ｓ４０４においても、相対位置姿勢情報入力部１５０から取得した撮像装置２３０の位置姿勢移動差分を、距離画像撮像時の概略位置姿勢にかけることで、濃淡画像撮像時の概略位置姿勢を求めるというように、前述の実施形態とは逆の手順になる。以上に示すように、濃淡画像と距離画像を撮像する順番は、どちらかに制限されることは特になく、いずれの順番でもよい。

［第２の変形例（距離画像のみでもよい例）］
前述の実施形態では、濃淡画像と距離画像を撮像する場合について説明していた。しかし、前述の情報処理装置１は、濃淡画像と距離画像を両方撮影する場合に限るものではない。例えば、時刻を変えて距離画像のみが撮像される場合でも、適用できる。この場合、ロボットのモーション情報を利用し、撮像時の位置姿勢が異なる複数の距離画像（第１の距離画像及び第２の距離画像）の情報を併用して、計測対象物体５０の位置姿勢を推定することになる。本変形例における情報処理装置は、図１の構成から２次元画像撮像装置２０と２次元画像入力部１２０を抜いた構成となる。

また、位置姿勢推定処理に関しては、同様に、Ｓ４０１における濃淡画像の撮像が、距離画像の撮像に置き換わる。また、Ｓ４０４における濃淡画像からのエッジ検出処理も、距離画像中の点と３次元形状モデルとの対応付け処理に置き換わる。これ以外は、前述の実施形態と変更はない。距離画像のみを撮像する場合においても、撮像装置２３０が移動しながら撮像した複数の距離画像と、ロボットの可動軸情報から取得した撮像装置２３０の位置姿勢移動差分情報を用いる。この場合も、複数の視点から撮像した距離画像を併用した高精度な位置姿勢推定処理を行うことが可能となる。

［第３の変形例（撮像装置は固定、対象物体がロボットアームにより把持されて移動する場合の例）］
前述の実施形態では、図２（ａ）に示すように、撮像装置２３０がロボットアーム先端に装着されて、ロボットアームの移動により撮像装置２３０の位置姿勢が変化する場面において、本発明の情報処理装置を適用する場合について説明した。しかし、本発明の情報処理装置は、前述のケースに限るものでなく、例えば、図２（ｂ）に示すように、撮像装置２３０は固定されており、ロボットアームによって把持された対象物体がアームの稼働に伴って移動する場合においても、同様に適用可能である。

この場合、Ｓ４０３において、相対位置姿勢情報入力部１５０から取得する位置姿勢移動差分情報は、前述の実施形態においては撮像時刻間の撮像装置２３０の位置姿勢移動差分情報であった。これに対し、本ケースでは、撮像時刻間の計測対象物体５０の位置姿勢差分情報となる。ただし、Ｓ４０４において、算出している位置姿勢は、撮像装置２３０と計測対象物体５０との相対的な位置姿勢であるため、処理自体は、前述の実施形態と変わりは無い。

以上のように、本発明による情報処理装置は、撮像装置２３０が移動し計測対象物体５０が固定される場合でも、撮像装置２３０が固定され計測対象物体５０が移動する場合でも、いずれの場合においても適用することは可能であり、特に制限は無い。

［第４の変形例（距離画像を幾何的に補正してから、位置姿勢推定処理を行ってもよい例）］
前述の実施形態では、Ｓ４０４の位置姿勢算出処理の中で撮像時刻間の位置姿勢差分量に基づく補正を行っていた。具体的には、撮像時刻間に移動する撮像装置２３０の位置姿勢差分量に基づいた位置姿勢の補正を行うことで、濃淡画像撮像時と距離画像撮像時における撮像装置２３０と計測対象物体５０との相対位置姿勢の違いを補正していた。しかし、撮像時刻間の位置姿勢差分量に基づく補正は、この方法に限るものではない。

例えば、距離画像を先に撮像し、濃淡画像を後に撮像する場合において、撮像時刻間の位置姿勢差分量に基づいて、距離画像から求めた計測対象物体５０表面上の3次元点群の座標を、濃淡画像撮像時の撮像装置２３０の位置姿勢を基準とする座標系に変換する。そして、非特許文献２の手法を適用して対応付け・計測対象物体５０の位置姿勢の算出を行ってもよい。

以上のように、濃淡画像撮像時と距離画像撮像時における撮像装置２３０と計測対象物体５０との相対位置姿勢の違いの補正は、位置姿勢情報を補正してもよいし、計測データの座標自体を補正してもよい。位置姿勢差分量に基づいて補正を行う方法である限り、方法の選択に特に制限はない。

［第５の変形例（位置姿勢推定の方法は、誤差最小化の枠組みによる方法でなくてもよい例）］
前述の実施形態では、位置姿勢を推定する方法として、濃淡画像中のエッジ及び距離画像中の３次元点と、３次元形状モデルとの誤差最小化の枠組みとして行っていた。しかし、位置姿勢を推定する方式はこれに限るものではない。例えば、あらかじめ対象物体を様々な方向から撮影し、各観察姿勢に対する濃淡画像のテンプレートと距離画像のテンプレートを抽出しておく。そして、ランタイム時にそれらのテンプレートを用いて、実際に撮像された濃淡画像と距離画像に対してテンプレートマッチングすることで、対象物体の位置及び姿勢を推定する方式をとってもよい。

この場合、テンプレートマッチングの類似度算出において、濃淡画像に対する類似度と距離画像に対する類似度を足すことで、濃淡・距離画像を併用したテンプレートマッチングを行う。このとき、濃淡画像に対してマッチングするときは、濃淡画像撮像時の位置姿勢 (図３：３１０) になるように概略位置姿勢を補正して、テンプレートマッチング処理を行う。また、距離画像に対してマッチングするときは、距離画像撮像時の位置姿勢(図３：３２０)になるように、撮像時刻間に移動した位置姿勢差分量（図３：３３０）を用いて概略位置姿勢を補正して、テンプレートマッチング処理を行う。位置姿勢を推定する手法は、以上に挙げた方法に限るもので無く、濃淡画像と距離画像に基づいて、対象物体または撮像装置の位置姿勢を推定できれば、いずれの手法を用いてもよい。

［第６の変形例（撮像装置移動量の取得はロボットのモーション情報に限らない例）］
前述の実施形態では、２次元画像の撮像時刻から距離画像の撮像時刻の間に、撮像装置２３０が移動した位置姿勢差分量を算出する方法として、撮像時刻間のロボット４０のアーム先端位置姿勢の差分から位置姿勢差分量を算出する方法を述べた。しかし、撮像装置２３０の移動量を算出する手段はロボットのモーション情報を用いる方法に限るものでない。

例えば、撮像装置２３０に、磁気センサや超音波センサなど、６自由度の位置及び姿勢を計測する物理センサを取り付け、それらより撮像時刻間の位置姿勢差分量を算出してもよい。また、撮像間における撮像装置２３０の移動が、回転運動のみに限定できる場面であれば、撮像装置２３０にジャイロセンサを取り付け、その回転差分量のみを計測してもよい。撮像時刻間の撮像装置２３０の位置姿勢差分量が算出できる限り、いずれの方法を用いてもよく、手段・機器の選択に特に制限はない。

［第７の変形例（ロボットによる対象物体操作を行う例）］
前述の実施形態では、ロボット４０のアーム先端に装着された撮像装置により、固定された計測対象物体５０の３次元位置姿勢の算出する方法について説明した。しかし、情報処理装置１は、対象物体の３次元位置姿勢の算出に限るものでなく、算出した位置姿勢を用いて、ロボット４０により対象物体に対して作業を行ってもよい。

例えば、ロボット４０のアーム先端に対象物体を把持することが可能なエンドエフェクタを装着し、対象物体に対して把持作業を行ってもよい。この場合、エンドエフェクタとしては、モータ駆動可能なハンドを用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよく、対象物体に対する作業に応じて適切に選択する限り、選択に制限は特にない。ロボットアームとエンドエフェクタとの相対位置姿勢のキャリブレーション作業は本実施形態の実施に先立って公知の技術によって予め行われているものとする。

これによって、位置姿勢算出部１６０で算出する位置姿勢をパレットが置かれた空間に固定されたワークスペース座標系に変換することが可能になる。さらに、ワークスペース座標系で指定した位置姿勢にハンドが設定されるようにロボットを制御することが可能になる。エンドエフェクタによる対象物体に対する作業としては、先にあげた把持・吸着に限らず、計測対象物体５０を他の物体へ組み付ける組付作業や対象物体の外観検査など、他の作業を行ってもよく、ロボット４０による計測対象物体５０への作業内容には、特に制限はない。

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

１ 情報処理装置、１０ 位置姿勢推定用照合データ（３次元形状モデル）、２０ ２次元画像撮像装置、３０ 距離画像撮像装置、４０ ロボット、１１０ 位置姿勢推定用照合データ保存部、１２０ ２次元画像入力部、１３０ 距離画像入力部、１４０ 概略位置姿勢入力部、１５０ 相対位置姿勢情報入力部、１６０ 位置姿勢算出部

Claims (13)

1. ２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理装置であって、
   被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存手段と、
   第１の位置姿勢の前記撮像装置により第１の時刻に撮影された２次元画像を入力する２次元画像入力手段と、
   第２の位置姿勢の前記撮像装置により前記第１の時刻と異なる第２の時刻に撮像された距離画像を入力する距離画像入力手段と、
   前記第１の位置姿勢と前記第２の位置姿勢の相対位置姿勢情報である位置姿勢差分情報を取得する相対位置姿勢情報入力手段と、
   前記位置姿勢差分情報に基づき、前記２次元画像及び前記距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定手段と、
   を有することを特徴とする情報処理装置。
2. ２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理装置であって、
   被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存手段と、
   第１の時刻に撮影された第１の位置姿勢の被測定物体の２次元画像を入力する２次元画像入力手段と、
   前記第１の時刻と異なる第２の時刻に撮影された第２の位置姿勢の被測定物体の距離画像を入力する距離画像入力手段と、
   前記第１の位置姿勢と前記第２の位置姿勢の相対位置姿勢情報である位置姿勢差分情報を取得する相対位置姿勢情報入力手段と、
   前記相対位置姿勢情報に基づき、前記２次元画像及び前記距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定手段と、
   を有することを特徴とする情報処理装置。
3. 距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理装置であって、
   被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存手段と、
   第１の位置姿勢の前記撮像装置により第１の時刻に撮像された距離画像を第１の距離画像として入力し、第２の位置姿勢の前記撮像装置により前記第１の時刻と異なる第２の時刻に撮像された距離画像を第２の距離画像として入力する距離画像入力手段と、
   前記第１の位置姿勢と第２の位置姿勢の相対的な位置姿勢情報を取得する相対位置姿勢情報入力手段と、
   前記相対位置姿勢情報に基づき、前記第１の位置姿勢及び第２の位置姿勢において撮像した距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定手段と、
   を有することを特徴とする情報処理装置。
4. 距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理装置であって、
   被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存手段と、
   第１の時刻に撮影された第１の位置姿勢の被測定物体の距離画像を第１の距離画像として入力し、前記第１の時刻と異なる第２の時刻に撮影された第２の位置姿勢の被測定物体の距離画像を第２の距離画像として入力する距離画像入力手段と、
   前記第１の位置姿勢と第２の位置姿勢の相対的な位置姿勢情報を取得する相対位置姿勢情報入力手段と、
   前記相対位置姿勢情報に基づき、前記第１の位置姿勢及び第２の位置姿勢において撮像した距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定手段と、
   を有することを特徴とする情報処理装置。
5. 前記位置姿勢推定手段は、第１の位置姿勢における被測定物体の概略位置姿勢を入力する概略位置姿勢入力手段をさらに有し、
   前記概略位置姿勢入力手段により入力される前記第１の位置姿勢における概略位置姿勢と、前記相対位置姿勢情報とに基づいて、前記第２の位置姿勢における概略位置姿勢を求めることで、濃淡画像及び／または前記距離画像を用いた位置姿勢推定を行うことを特徴とする請求項１〜４の何れか１項に記載の情報処理装置。
6. 前記位置姿勢推定手段は、前記相対位置姿勢情報に基づいて、第１の位置姿勢または第２の位置姿勢における距離画像を、おのおの第２の位置姿勢または第１の位置姿勢で撮像した場合とほぼ同じ位置から撮像した距離画像になるように変換することで、濃淡画像及び／または前記距離画像を用いた位置姿勢推定処理を行うことを特徴とする請求項１〜４の何れか１項に記載の情報処理装置。
7. 前記相対位置姿勢情報入力手段は、位置及び／または姿勢を取得するセンサを有し、前記センサは、回転及び／または並進移動軸からなる可動軸を有するロボットの可動軸情報より前記位置姿勢差分情報を取得することを特徴とする請求項１〜６の何れか１項に記載の情報処理装置。
8. 前記位置姿勢推定手段により算出した被測定物体の位置姿勢に基づいて、回転及び／または並進移動軸からなる可動軸を複数有するロボットによって、前記被測定物体または前記撮像装置の位置姿勢を変更する位置姿勢操作手段をさらに有することを特徴とする請求項１〜７の何れか１項に記載の情報処理装置。
9. ２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理方法であって、
   被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存工程と、
   第１の位置姿勢の前記撮像装置により第１の時刻に撮影された２次元画像を入力する２次元画像入力工程と、
   第２の位置姿勢の前記撮像装置により前記第１の時刻と異なる第２の時刻に撮像された距離画像を入力する距離画像入力工程と、
   前記第１の位置姿勢と前記第２の位置姿勢の相対位置姿勢情報である位置姿勢差分情報を取得する相対位置姿勢情報入力工程と、
   前記位置姿勢差分情報に基づき、前記２次元画像及び前記距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定工程と、
   を有することを特徴とする情報処理方法。
10. ２次元画像及び距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理方法であって、
    被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存工程と、
    第１の時刻に撮影された第１の位置姿勢の被測定物体の２次元画像を入力する２次元画像入力工程と、
    前記第１の時刻と異なる第２の時刻に撮影された第２の位置姿勢の被測定物体の距離画像を入力する距離画像入力工程と、
    前記第１の位置姿勢と前記第２の位置姿勢の相対位置姿勢情報である位置姿勢差分情報を取得する相対位置姿勢情報入力工程と、
    前記相対位置姿勢情報に基づき、前記２次元画像及び前記距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定工程と、
    を有することを特徴とする情報処理方法。
11. 距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理方法であって、
    被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存工程と、
    第１の位置姿勢の前記撮像装置により第１の時刻に撮像された距離画像を第１の距離画像として入力し、第２の位置姿勢の前記撮像装置により前記第１の時刻と異なる第２の時刻に撮像された距離画像を第２の距離画像として入力する距離画像入力工程と、
    前記第１の位置姿勢と第２の位置姿勢の相対的な位置姿勢情報を取得する相対位置姿勢情報入力工程と、
    前記相対位置姿勢情報に基づき、前記第１の位置姿勢及び第２の位置姿勢において撮像
    した距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の
    位置姿勢を算出する位置姿勢推定工程と、
    を有することを特徴とする情報処理方法。
12. 距離画像が撮像可能な撮像装置により、被測定物体の３次元的な位置姿勢を推定する情報処理方法であって、
    被測定物体の位置姿勢推定用照合データを保存する位置姿勢推定用照合データ保存工程と、
    第１の時刻に撮影された第１の位置姿勢の被測定物体の距離画像を第１の距離画像として入力し、前記第１の時刻と異なる第２の時刻に撮影された第２の位置姿勢の被測定物体の距離画像を第２の距離画像として入力する距離画像入力工程と、
    前記第１の位置姿勢と第２の位置姿勢の相対的な位置姿勢情報を取得する相対位置姿勢情報入力工程と、
    前記相対位置姿勢情報に基づき、前記第１の位置姿勢及び第２の位置姿勢において撮像した距離画像に対して、前記位置姿勢推定用照合データが当てはまるように被測定物体の位置姿勢を算出する位置姿勢推定工程と、
    を有することを特徴とする情報処理方法。
13. コンピュータを、請求項１〜８の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images