JP2012123781A

JP2012123781A - 情報処理装置、情報処理システム及び情報処理方法

Info

Publication number: JP2012123781A
Application number: JP2011228202A
Authority: JP
Inventors: Keisuke Tateno; 圭祐立野; Daisuke Kotake; 大輔小竹; Shinji Uchiyama; 晋二内山
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2010-11-19
Filing date: 2011-10-17
Publication date: 2012-06-28
Anticipated expiration: 2031-10-17
Also published as: JP5839929B2; WO2012066769A1; US20130230235A1

Abstract

【課題】距離画像のノイズに影響を受けにくく、かつ高精度なモデルフィッティングを行うことを目的とする。
【解決手段】物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存部２１０と、物体を撮像した２次元画像を入力する２次元画像入力部２２０と、物体を撮像した距離画像を入力する距離画像入力部２３０と、２次元画像入力部２２０から入力された２次元画像から画像特徴を検出する画像特徴検出部２５０と、距離画像入力部２３０から入力された距離画像から画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出部２６０と、画像特徴の３次元座標と３次元モデルとを照合するモデル照合部２７０とを備える。
【選択図】図７

Description

本発明は、３次元モデルが既知である物体の位置及び姿勢を計測する技術に関する。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。ここで、位置及び姿勢を計測する方法としては、カメラが撮影する濃淡画像上から検出される特徴や、距離センサから得られる距離画像に対して物体の３次元形状モデルをあてはめるモデルフィッティングによる計測が一般的である。

下記の非特許文献１では、濃淡画像上から検出される特徴としてエッジを利用する方法が開示されている。この方法では、物体の形状を３次元ラインの集合によって表し、物体の概略の位置及び姿勢は既知であるとしている。そして、物体が撮像された濃淡画像上で検出されるエッジに３次元ラインの投影像が当てはまるように概略の位置及び姿勢を補正することにより、物体の位置及び姿勢を計測する。

Ｔ．ＤｒｕｍｍｏｎｄａｎｄＲ．Ｃｉｐｏｌｌａ，"Ｒｅａｌ−ｔｉｍｅｖｉｓｕａｌｔｒａｃｋｉｎｇｏｆｃｏｍｐｌｅｘｓｔｒｕｃｔｕｒｅｓ，"ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＰａｔｔｅｒｎＡｎａｌｙｓｉｓａｎｄＭａｃｈｉｎｅＩｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２４，ｎｏ．７，ｐｐ．９３２−９４６，２００２．Ｐ．Ｊ．ＢｅｓｌａｎｄＮ．Ｄ．ＭｃＫａｙ，"Ａｍｅｔｈｏｄｆｏｒｒｅｇｉｓｔｒａｔｉｏｎｏｆ３−Ｄｓｈａｐｅｓ，"ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＰａｔｔｅｒｎＡｎａｌｙｓｉｓａｎｄＭａｃｈｉｎｅＩｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１４，ｎｏ．２，ｐｐ．２３９−２５６，１９９２．Ｒ．Ｙ．Ｔｓａｉ，"Ａｖｅｒｓａｔｉｌｅｃａｍｅｒａｃａｌｉｂｒａｔｉｏｎｔｅｃｈｎｉｑｕｅｆｏｒｈｉｇｈ−ａｃｃｕｒａｃｙ３Ｄｍａｃｈｉｎｅｖｉｓｉｏｎｍｅｔｒｏｌｏｇｙｕｓｉｎｇｏｆｆ−ｔｈｅ−ｓｈｅｌｆＴＶｃａｍｅｒａｓａｎｄｌｅｎｓｅｓ，"ＩＥＥＥＪｏｕｒｎａｌｏｆＲｏｂｏｔｉｃｓａｎｄＡｕｔｏｍａｔｉｏｎ，ｖｏｌ．ＲＡ−３，ｎｏ．４，１９８７．Ｊ．Ｍａｔａｓ，Ｏ．Ｃｈｕｍ，Ｍ．Ｕｒｂａ，ａｎｄＴ．Ｐａｊｄｌａ．"Ｒｏｂｕｓｔｗｉｄｅｂａｓｅｌｉｎｅｓｔｅｒｅｏｆｒｏｍｍａｘｉｍａｌｌｙｓｔａｂｌｅｅｘｔｒｅｍａｌｒｅｇｉｏｎｓ．"Ｐｒｏｃ．ｏｆＢｒｉｔｉｓｈＭａｃｈｉｎｅＶｉｓｉｏｎＣｏｎｆｅｒｅｎｃｅ，ｐａｇｅｓ３８４−３９６，２００２．

上述した従来技術では、濃淡画像上から検出した画像特徴に対して、画像上の距離が最小になるようにモデルをフィッティングするため、見た目の変化が乏しい奥行き方向の変化を高精度に推定することは一般的には困難である。また、２次元的に近傍になる特徴に対してフィッティングを行うため、２次元的に近傍であるが奥行き方向に大きく離れているような特徴がある場合、その特徴に誤対応し位置姿勢推定が不安定になるという問題がある。

ここで、上記非特許文献２で開示されている技術のように、距離画像に対して位置姿勢推定を行う方法が存在する。このような距離画像を利用するという方法があるということから容易に想到できることとして、前記従来技術を、距離画像を用いる方法として単純に拡張し、濃淡画像の代わりに距離画像に対して処理を行うという方法が考えられる。距離画像を濃淡画像とみなして画像特徴を検出することにより、３次元座標が既知である画像特徴が得られるため、画像特徴とモデルとの誤差を３次元空間中で直接的に最小化することが可能になる。これにより、従来技術と比較して、奥行き方向も高精度に推定することが可能である。また、モデルと３次元的に近傍となる画像特徴に対してフィッティングを行うため、従来手法で問題となるような、奥行き方向に離れているが２次元的には近傍となる特徴に対しても、正しく対応することが可能となる。

しかしながら、この手法では、距離画像のノイズ上からも画像特徴を検出してしまうため、距離画像にノイズが含まれる場合には、ノイズ上の画像特徴へ誤対応することにより位置姿勢推定に失敗するという問題がある。

距離画像には、距離が不連続に変化する領域や面と面との境界などに、多重反射に起因するノイズが発生することが多いため、この問題は実用上大きな問題となる。また、距離画像上で画像特徴の検出を行う場合には、対象物体上のテクスチャなどによって生じる画像特徴の部分を有効に利用して位置姿勢推定することが出来ない。モデルフィッティングの精度は、情報量が多いほど上がるため、対象物体にテクスチャ情報がある場合には、テクスチャ情報も利用して位置姿勢推定できることが望ましい。

本発明は、このような問題点に鑑みてなされたものであり、距離画像のノイズに影響を受けにくく、かつ高精度なモデルフィッティングを行うことを目的とする。

本発明の情報処理装置は、物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存手段と、前記物体を撮像した２次元画像を入力する２次元画像入力手段と、前記物体を撮像した距離画像を入力する距離画像入力手段と、前記２次元画像入力手段から入力された２次元画像から画像特徴を検出する画像特徴検出手段と、前記距離画像入力手段から入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出手段と、前記画像特徴の３次元座標に対し前記３次元モデルを当てはめるモデル当てはめ手段とを有する。
本発明の情報処理システムは、前記情報処理装置と、回転軸および／または並進移動軸からなる可動軸を有するロボットと、前記物体の位置・姿勢に基づいて、前記ロボットの手先を、前記物体を把持できる位置・姿勢に移動させる制御をするロボットコントローラとを有する。
本発明の情報処理方法は、物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存ステップと、前記物体を撮像した２次元画像を入力する２次元画像入力ステップと、前記物体を撮像した距離画像を入力する距離画像入力ステップと、前記２次元画像入力ステップから入力された２次元画像から画像特徴を検出する画像特徴検出ステップと、前記距離画像入力ステップから入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出ステップと、前記画像特徴の３次元座標と前記３次元モデルとを照合するモデル照合ステップとを有する。
また、本発明の情報処理方法における他の態様は、物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存ステップと、前記物体を撮像した２次元画像を入力する２次元画像入力ステップと、前記物体を撮像した距離画像を入力する距離画像入力ステップと、前記２次元画像入力ステップから入力された２次元画像から画像特徴を検出する画像特徴検出ステップと、前記距離画像入力ステップから入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出ステップと、前記３次元モデルが３次元空間中で当てはまるように前記物体の位置・姿勢を算出する位置姿勢算出ステップとを有する。

本発明によれば、距離画像及び距離画像と画素位置の対応が既知の２次元画像を用いて、画像特徴の検出は２次元画像から、画像特徴の座標算出は距離画像から行うことで、距離画像のノイズに影響を受けにくく、かつ高精度なモデルフィッティングを行える。即ち、高精度に対象物体の位置・姿勢の推定を行うことが可能になる。

本発明の第１の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。本発明の第１の実施形態を示し、３次元モデルの定義方法を説明する模式図である。本発明の第１の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。本発明の第１の実施形態の画像特徴検出部において行われる濃淡画像のエッジ特徴の検出方法について詳細な処理手順の一例を示すフローチャートである。本発明の第１の実施形態におけるエッジ検出を説明する模式図である。本発明の第１の実施形態を示し、エッジの３次元座標と３次元モデル中の線分との関係を説明する模式図である。本発明の第２の実施形態に係る情報処理装置（モデル照合装置）を含む情報処理システム（モデル照合システム）の概略構成の一例を示す模式図である。本発明の第２の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。本発明の第３の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。本発明の第３の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。本発明の第４の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。

以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。

（第１の実施形態）
本実施形態では、物体の概略の位置・姿勢は既知であると仮定し、本発明に係る情報処理装置を、３次元モデルと濃淡画像・距離画像を用いて、物体の位置・姿勢を推定する手法に適用した場合について説明する。

図１は、本発明の第１の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。
図１に示すように、情報処理システムは、３次元モデル（３次元形状モデルともいう）１０、２次元画像撮像装置２０、３次元データ計測装置３０、及び、情報処理装置１００を有して構成されている。

本実施形態に係る情報処理装置１００は、観察対象物体の形状を表す３次元モデル１０のデータを利用して位置・姿勢の推定を行うものである。
この情報処理装置１００は、３次元モデル保存部１１０、２次元画像入力部１２０、距離画像入力部１３０、概略位置姿勢入力部１４０、画像特徴検出部１５０、画像特徴３次元情報算出部１６０、及び、位置姿勢算出部１７０を有して構成されている。

２次元画像撮像装置２０は、２次元画像入力部１２０に接続されている。
２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は、濃淡画像であってもよいしカラー画像であってもよい。本実施形態では、２次元画像撮像装置２０は濃淡画像を出力する。２次元画像撮像装置２０が撮影する画像は、２次元画像入力部１２０を介して情報処理装置１００に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、上記非特許文献３に示す方法によって事前にキャリブレーションしておく。

３次元データ計測装置３０は、距離画像入力部１３０に接続されている。
３次元データ計測装置３０は、計測対象である物体表面上の点の３次元情報を計測する。３次元データ計測装置３０としては、距離画像を出力する距離センサを用いる。この距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとしては、対象に照射したレーザ光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであれば如何なるものであっても本発明の本質を損なうものではない。３次元データ計測装置３０が計測した３次元データは、距離画像入力部１３０を介して情報処理装置１００に入力される。また、３次元データ計測装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する２次元画像の各画素と、３次元データ計測装置３０が出力する距離画像の各画素の対応は既知であるとする。

３次元モデル保存部１１０は、観察対象物体の幾何的特徴を表す３次元モデル１０のデータを保存し、画像特徴検出部１５０に接続されている。
情報処理装置１００では、３次元モデル保存部１１０に保存されている観察対象物体の形状を表す３次元モデル１０のデータに基づいて、２次元画像及び距離画像中に撮像される観察対象物体の位置及び姿勢を計測する。なお、本実施形態では、３次元モデル保存部１１０に保存された３次元モデル１０のデータが、現実に撮像される観察対象物体の形状に即していることが情報処理装置１００を適用できる条件となる。

３次元モデル保存部１１０は、位置及び姿勢を計測する対象である物体の３次元モデル（３次元形状モデル）１０のデータを保存する。３次元モデル（３次元形状モデル）１０は、位置姿勢算出部１７０において物体の位置及び姿勢を算出する際に用いられる。本実施形態では、物体を線分と面から構成される３次元モデル（３次元形状モデル）１０として記述する。３次元モデル（３次元形状モデル）１０は、点の集合及び各点を結んで構成される線分の集合によって定義される。

図２は、本発明の第１の実施形態を示し、３次元モデル１０の定義方法を説明する模式図である。３次元モデル１０は、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、３次元モデル１０−１は、点Ｐ１〜点Ｐ１４の１４点から構成される。また、図２（ｂ）に示すように、３次元モデル１０−２は、線分Ｌ１〜Ｌ１６により構成されている。そして、図２（ｃ）に示すように、点Ｐ１〜点Ｐ１４は、３次元座標値によって表される。また、図２（ｄ）に示すように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。

２次元画像入力部１２０は、２次元画像撮像装置２０が撮像した２次元画像を情報処理装置１００に入力する。

距離画像入力部１３０は、３次元データ計測装置３０が計測する距離画像を位置姿勢計測装置である情報処理装置１００に入力する。なお、カメラによる画像の撮影と、距離センサによる距離の計測は同時に行われると仮定する。ただし、対象物体が静止している場合など、情報処理装置１００と観察対象物体との位置及び姿勢が変化しない場合には、必ずしも同時に行う必要はない。

ここで、２次元画像入力部１２０から入力された２次元画像と距離画像入力部１３０から入力された距離画像とは略同一視点から撮像されており、画像間の対応が既知である。

概略位置姿勢入力部１４０は、情報処理装置１００に対する物体の位置及び姿勢の概略値を入力する。ここで、情報処理装置１００に対する物体の位置及び姿勢とは、濃淡画像を撮像する２次元画像撮像装置２０のカメラ座標系における物体の位置及び姿勢を表す。しかしながら、カメラ座標系に対する相対的な位置及び姿勢が既知でかつ変化しなければ、位置姿勢計測装置である情報処理装置１００のいずれの部分を基準としてもよい。本実施形態では、情報処理装置１００は、時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものでもよい。また、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、対象物体の大まかな位置と姿勢を推定してもよい。あるいは、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、当該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えば、トランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであれば如何なるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いる。

画像特徴検出部１５０は、２次元画像入力部１２０から入力された２次元画像から画像特徴を検出する。本実施形態では、画像特徴としてエッジを検出する。

画像特徴３次元情報算出部１６０は、画像特徴検出部１５０で検出されたエッジのカメラ座標系における３次元座標を、距離画像入力部１３０により入力された距離画像を参照することで算出する。画像特徴の３次元情報算出方法については後述する。

位置姿勢算出部１７０は、画像特徴３次元情報算出部１６０で算出された画像特徴の３次元座標（３次元情報）に基づいて、観察対象物体の位置及び姿勢を算出する。この際、位置姿勢算出部１７０は、画像特徴の３次元座標と３次元モデルとの差に基づいて観察対象物体の位置・姿勢を算出するようにしてもよい。処理の詳細については、後述する。

次に、本実施形態における位置・姿勢推定方法の処理手順について説明する。
図３は、本発明の第１の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。

＜ステップＳ１０１０＞
まず、ステップＳ１０１０において、例えば情報処理装置１００は、初期化を行う。概略位置姿勢入力部１４０は、情報処理装置１００（カメラ）に対する物体の位置及び姿勢の概略値を情報処理装置１００に入力する。本実施形態における位置・姿勢計測方法は、物体の概略の位置・姿勢を、計測データに基づいて逐次更新していく方法である。そのため、位置姿勢計測を開始する前に予め撮像装置の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。前述したように、本実施形態では、前の時刻において計測された位置及び姿勢を用いる。

＜ステップＳ１０２０＞
続いて、ステップＳ１０２０において、２次元画像入力部１２０及び距離画像入力部１３０は、モデルフィッティングにより物体の位置及び姿勢を算出するための計測データを取得する。具体的には、２次元画像入力部１２０は、２次元画像撮像装置２０から観察対象物体の２次元画像（濃淡画像）を取得して情報処理装置１００に入力し、距離画像入力部１３０は、３次元データ計測装置３０から距離画像を取得して情報処理装置１００に入力する。本実施形態では、距離画像は、カメラから観察対象物体表面点までの距離が格納されているものとする。前述のように、２次元画像撮像装置２０と３次元データ計測装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。

＜ステップＳ１０３０＞
続いて、ステップＳ１０３０において、画像特徴検出部１５０は、ステップＳ１０２０において入力された濃淡画像上において、３次元モデル（３次元形状モデル）１０と対応付けるための、画像特徴の検出を行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配の極値となる点である。本実施形態では、上記非特許文献１で開示される方法によりエッジ検出を行う。

図４は、本発明の第１の実施形態の画像特徴検出部において行われる濃淡画像のエッジ特徴の検出方法について詳細な処理手順の一例を示すフローチャートである。
まず、ステップＳ１１１０において、画像特徴検出部１５０は、ステップＳ１０１０で入力された観察対象物体の概略位置及び姿勢と２次元画像撮像装置２０の内部パラメータを用いて、３次元モデル（３次元形状モデル）１０を画像平面へ投影する。これにより、３次元モデル（３次元形状モデル）１０を構成する各線分の、２次元画像上での座標と方向が計算される。線分の投影像は画像上でも線分となる。

続いて、ステップＳ１１２０において、画像特徴検出部１５０は、ステップＳ１１１０で算出した投影線分上に制御点を設定する。ここで、制御点とは、投影線分を等間隔に分割するように設定した、３次元線分上の点である。以後、この制御点を、Ｅｄｇｅｌｅｔと呼称する。Ｅｄｇｅｌｅｔは、３次元座標と線分の３次元方向、及び、投影結果として得られる２次元座標と２次元方向の情報を保持する。Ｅｄｇｅｌｅｔの数が多いほど、処理時間が長くなるため、Ｅｄｇｅｌｅｔの総数が一定になるように、Ｅｄｇｅｌｅｔ間の間隔を逐次変更してもよい。具体的に、ステップＳ１１２０では、投影線分の分割によるＥｄｇｅｌｅｔの算出が行われる。

続いて、ステップＳ１１３０において、画像特徴検出部１５０は、ステップＳ１１２０で求めたＥｄｇｅｌｅｔに対応する２次元画像中のエッジを検出する。

図５は、本発明の第１の実施形態におけるエッジ検出を説明する模式図である。
エッジの検出は、Ｅｄｇｅｌｅｔの探索ライン５１０（制御点５２０の２次元方向の法線方向）上において、撮影画像上の濃度勾配から極値を算出することにより行う（図５（ａ））。エッジは、探索ライン５１０上において濃度勾配が極値をとる位置に存在する（図５（ｂ））。ここでは、Ｅｄｇｅｌｅｔ５２０の探索ライン５１０上で検出された全てのエッジの２次元座標を、Ｅｄｇｅｌｅｔ５２０の対応候補点として保存する。以上の処理を全てのＥｄｇｅｌｅｔに対して繰り返し、続いて、ステップＳ１１４０において、画像特徴検出部１５０は、対応候補のエッジの方向を算出する。このステップＳ１１４０の処理が終了すれば、ステップＳ１０３０の処理を終了し、ステップＳ１０４０に進む。

＜ステップＳ１０４０＞
その後、図３のステップＳ１０４０において、画像特徴３次元情報算出部１６０は、ステップＳ１０３０で求めたＥｄｇｅｌｅｔと対応点５３０との３次元的な誤差を算出するために、距離画像を参照することで対応点５３０の３次元座標を算出する。即ち、ここでは、画像特徴の３次元座標の算出を行う。

まず、画像特徴３次元情報算出部１６０は、Ｅｄｇｅｌｅｔの対応候補点の中から、処理する対応候補点を１点選ぶ。次に、選んだ対応候補点の３次元座標を算出する。本実施形態では、濃淡画像と距離画像は同軸に撮像されているため、ステップＳ１０３０で算出した対応候補点の２次元座標がそのまま距離画像上の２次元座標となる。次に、対応候補点の２次元座標に対応する距離値を距離画像から参照し、対応候補点の２次元座標と距離値から、対応候補点の３次元座標を算出する。具体的に、画像特徴３次元情報算出部１６０は、距離画像から画像特徴の検出位置を中心にした所定の範囲内の距離値を参照することで、画像特徴の３次元座標を少なくとも１つ以上算出する。また、画像特徴３次元情報算出部１６０は、距離画像から画像特徴の検出位置を中心にした所定の範囲内の距離値を参照し、画像特徴の３次元座標と３次元モデル１０との距離が所定以下になるような３次元座標を算出するようにしてもよい。ここで、距離画像から参照した距離値をDepthとして、３次元座標をＸ、Ｙ、Ｚと表すと、求める３次元座標は、以下の数式（１）と表せる。

ここで、数式（１）において、ｆは焦点距離、（ux、uy）は距離画像上の２次元座標、（cx、cy）は画像中心を表すカメラ内部パラメータである。以上の数式（１）により、対応候補点の３次元座標が算出される。以上の処理を全てのＥｄｇｅｌｅｔの全ての対応候補点に対して繰り返し、対応候補点の３次元座標の算出処理が終了したら、ステップＳ１０４０の処理を終了し、ステップＳ１０５０に進む。

＜ステップＳ１０５０＞
ステップＳ１０５０において、位置姿勢算出部１７０は、非線形最適化計算を用いて、計測データに３次元形状モデルが３次元空間中で当てはまるように観察対象物体の概略の位置・姿勢を反復演算により補正することで観察対象物体の位置・姿勢を算出する。本ステップでは、非線形最適化手法としてGauss-Newton法を用いる。なお、非線形最適化手法はGauss-Newton法に限るものではない。例えば、より計算がロバストであるLevenberg-Marquardt法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。ここでは、ステップＳ１０４０で算出したエッジの３次元座標と、推定される位置及び姿勢に基づいてカメラ座標系に変換した３次元モデル中の線分との距離に基づいて位置・姿勢の最適化を行う。

図６は、本発明の第１の実施形態を示し、エッジの３次元座標と３次元モデル中の線分との関係を説明する模式図である。対応候補点の３次元座標とＥｄｇｅｌｅｔの３次元座標との誤差ベクトルをｅｒｒとし、Ｅｄｇｅｌｅｔが通る直線と対応候補点の最近傍となる垂線ベクトル（単位ベクトル）をＮ、Ｅｄｇｅｌｅｔの方向ベクトル（単位ベクトル）をＤとおくと、求める符号付距離ｄは以下の数式（２）及び（３）で表せる。

位置姿勢算出部１７０は、この符号付距離を位置及び姿勢の微小変化の関数として線形近似し、符号付距離が０となるように各計測データについて線形方程式を立式する。そして、位置姿勢算出部１７０は、線形方程式を連立方程式として解くことにより、物体の位置及び姿勢の微小変化を求め、位置及び姿勢を補正する。以上の処理を繰り返すことにより、位置姿勢算出部１７０は、最終的な位置及び姿勢を算出する。誤差の最小化処理に関しては、本発明の本質に関わる話ではないため、記述は省略する。

＜ステップＳ１０６０＞
続いて、ステップＳ１０６０において、例えば情報処理装置１００は、位置・姿勢の算出を終了する入力がなされたか否かを判断する。この判断の結果、位置・姿勢の算出を終了する入力がされた場合には、本フローチャートの処理を終了し、一方、位置・姿勢の算出を終了する入力がされなかった場合には、ステップＳ１０１０に戻り、新たな画像を取得して再度、位置・姿勢の算出を行う。

本実施形態によれば、濃淡画像からエッジの検出を行い、検出されたエッジの３次元座標を距離画像から算出することにより、奥行き方向の精度が高く、かつ距離画像のノイズに影響を受け難い安定的な位置・姿勢の推定が可能となる。また、距離画像上では検出できないエッジを濃淡画像から検出することが可能であるため、より多くの情報量を用いた高精度な位置・姿勢の推定が可能となる。

次に、本発明の第１の実施形態の変形例について説明する。

［変形例１］≪近傍の距離値を参照して対応点の３次元座標を算出する≫
前述の第１の実施形態では、画像特徴の３次元座標を算出するのに、画像特徴の２次元位置に対応する距離値を用いていた。しかしながら、画像特徴の３次元座標を算出する方法は、これに限るものではない。例えば、画像特徴の２次元位置の近傍（画像特徴の検出位置近傍）を探索し、複数の距離値の中央値を算出して、エッジの３次元座標を算出してもよい。具体的には、画像特徴３次元情報算出部１６０は、画像特徴の２次元位置を中心とした近傍９画素の距離値を全て参照し、その中で中央値となる距離値を用いて、画像特徴の３次元座標を算出する。また、近傍の距離値からそれぞれ独立に画像特徴の３次元座標を求め、Ｅｄｇｅｌｅｔとの距離が最短となる３次元座標を画像特徴の３次元座標として求めてもよい。これらの方法は、距離画像中のジャンプエッジにノイズが多量に含まれる場合に有効である。なお、３次元座標を算出する方法は、以上に限るものでなく、画像特徴の３次元座標が算出できる限りいずれの手法を用いてよい。

［変形例２］≪エッジ以外の特徴でもよい≫
前述の第１の実施形態では、濃淡画像から検出したエッジと、３次元モデル中の３次元ラインとの対応付けを行っていた。しかしながら、対応付けに用いる特徴は画像上のエッジに限るものではない。例えば、画像上の特徴として輝度が特徴的に変化する点点特徴を検出、距離画像から３次元座標を算出し、予め３次元モデルとして保持していた３次元点と対応付けてもよい。ここでは、濃淡画像から検出でき、３次元モデルと対応が計算できる特徴である限り、特徴の表現に制限はない。

［変形例３］≪面に基づく特徴でもよい≫
前述の第１の実施形態では、濃淡画像から検出したエッジと、３次元モデル中の３次元ラインとの対応付けを行っていた。しかしながら、対応付けに用いる特徴は画像上のエッジに限るものではない。例えば、画像上の特徴として、安定的に検出できる面領域を検出してもよい。具体的には、画像輝度に基づく領域検出器を用いて、視点変化・輝度変化に安定な面領域を検出し、その面領域の３次元座標及び面の３次元法線を距離画像から算出して、３次元モデル上の３次元面と対応付けてもよい。領域検出の手法としては、例えば、上記非特文献４に記載の画像輝度に基づく領域検出器を用いればよい。また、面領域の３次元面法線及び３次元座標の算出は、例えば、濃淡画像中の面領域内の距離値を距離画像から３点取得し、３点の外積計算から３次元面の法線を、距離値の中央値から３次元面の３次元座標を計算すればよい。なお、濃淡画像から面領域する方法は上述の方法に限るものでなく、濃淡画像から、安定的に面領域を検出できる手法である限り特に制限はない。また、面領域の３次元面法線及び３次元座標を算出する方法は、上述の方法に限るものではなく、面領域内に対応する距離値から、３次元座標及び３次元法線を算出できる方法である限り、いかなる方法を用いてもよい。

［変形例４］≪濃淡画像と距離画像は略同一視点でなくてもよい≫
前述の第１の実施形態では、濃淡画像と距離画像は同一の視点から撮像され、撮像時点で画像間の対応が既知であるとしていた。しかしながら、濃淡画像と距離画像が同一の視点である場合に限るものではない。例えば、濃淡画像を撮像する撮像装置と距離画像を撮像する撮像装置が別の位置姿勢にあり、濃淡画像と距離画像をそれぞれ別の視点から撮像してもよい。この場合は、撮像装置間の相対的な位置姿勢は既知であるとして、距離画像中の３次元点群を濃淡画像に投影することにより、濃淡画像と距離画像の対応を取る。同一の物体を撮像する撮像装置間の相対的な位置姿勢が既知であり、その画像間の対応が計算できる限り、撮像装置の位置関係に特に制限はない。

（第２の実施形態）≪モデル照合手段としてバリエーションを増やす≫
第１の実施形態では、本発明において物体の位置・姿勢の推定に適用する方法について説明した。第２の実施形態では、本発明を物体の照合に適用する方法について説明する。

図７は、本発明の第２の実施形態に係る情報処理装置（モデル照合装置）を含む情報処理システム（モデル照合システム）の概略構成の一例を示す模式図である。
図７に示すように、情報処理システム（モデル照合システム）は、３次元モデル（３次元形状モデル）１０、２次元画像撮像装置２０、３次元データ計測装置３０、及び、情報処理装置（モデル照合装置）２００を有して構成されている。

本実施形態に係る情報処理装置２００は、３次元モデル保存部２１０、２次元画像入力部２２０、距離画像入力部２３０、概略位置姿勢入力部２４０、画像特徴検出部２５０、画像特徴３次元情報算出部２６０、及び、モデル照合部２７０を有して構成されている。

２次元画像撮像装置２０は、２次元画像入力部２２０に接続されている。３次元データ計測装置３０は、距離画像入力部２３０に接続されている。

３次元モデル保存部２１０は、３次元モデル１０のデータを保存し、画像特徴検出部２５０に接続されている。情報処理装置（モデル照合装置）２００は、３次元モデル保存部２１０に保存されている観察対象物体の形状を表す３次元モデル１０のデータに基づいて、２次元画像及び距離画像中に観察対象物体が撮像されているか否かを判断する。
３次元モデル保存部２１０は、照合する対象である物体の３次元モデル（３次元形状モデル）１０を保存する。３次元形状モデルの保持方法は、第１の実施形態における３次元モデル保存部１１０と同一である。本実施形態では、照合する対象の数だけ、３次元モデル（３次元形状モデル）１０を複数保持する。

画像特徴３次元情報算出部２６０は、画像特徴検出部２５０で検出されたエッジの３次元座標を、距離画像入力部２３０により入力された距離画像を参照することで算出する。なお、画像特徴の３次元情報算出方法については後述する。

モデル照合部２７０は、画像特徴３次元情報算出部２６０で算出された画像特徴の３次元座標（３次元情報）と、３次元モデル（３次元形状モデル）１０とを照合する。この際、モデル照合部２７０は、画像特徴の３次元座標と３次元モデル１０との一致度または不一致度に基づいて３次元モデルを照合するようにしてもよい。処理の詳細については、後述する。

また、２次元画像入力部２２０、距離画像入力部２３０、概略位置姿勢入力部２４０、画像特徴検出部２５０は、それぞれ、第１の実施形態における２次元画像入力部１２０、距離画像入力部１３０、概略位置姿勢入力部１４０、画像特徴検出部１５０と同一であるので説明を省略する。

次に、本実施形態における位置・姿勢推定方法の処理手順について説明する。
図８は、本発明の第２の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。

＜ステップＳ２０１０＞
まず、ステップＳ２０１０において、例えば情報処理装置１００は、初期化を行った後、３次元モデル（３次元形状モデル）１０と照合するための計測データを取得する。具体的には、２次元画像入力部１２０は、２次元画像撮像装置２０から観察対象物体の２次元画像（濃淡画像）を取得して情報処理装置１００に入力し、距離画像入力部１３０は、３次元データ計測装置３０から距離画像を情報処理装置１００に入力する。また、概略位置姿勢入力部２４０は、物体の概略の位置・姿勢を入力する。本実施形態では、物体の置かれているおおよその位置や姿勢は予めわかっているものとし、その値を物体の概略位置姿勢として用いる。２次元画像と距離画像の入力は、第１の実施形態におけるステップＳ１０２０の処理と同一であるのでその詳細な説明を省略する。

＜ステップＳ２０２０＞
続いて、ステップＳ２０２０において、画像特徴検出部２５０は、ステップＳ２０１０において入力された濃淡画像上において、画像特徴の検出を行う。ここで、画像特徴の検出は、３次元モデル（３次元形状モデル）１０ごとに行う。画像特徴の検出処理は、第１の実施形態におけるステップＳ１０３０の処理と同一であるのでその詳細な説明を省略する。３次元モデル（３次元形状モデル）１０ごとに画像特徴の検出処理を繰り返し、全ての３次元モデル（３次元形状モデル）１０に対して処理が終了したら、ステップＳ２０２０の処理を終了し、ステップＳ２０３０に進む。

＜ステップＳ２０３０＞
続いて、ステップＳ２０３０において、画像特徴３次元情報算出部２６０は、ステップＳ２０２０で求めたＥｄｇｅｌｅｔの対応候補点の３次元座標を算出する。３次元座標の算出は、全ての３次元モデル（３次元形状モデル）１０のＥｄｇｅｌｅｔに対して行う。対応候補点の３次元座標の算出処理は、第１の実施形態におけるステップＳ１０４０の処理と同一であるのでその詳細な説明を省略する。全ての３次元モデル（３次元形状モデル）１０に対して処理が終了したら、ステップＳ２０３０の処理を終了し、ステップＳ２０４０に進む。

＜ステップＳ２０４０＞
続いて、ステップＳ２０４０において、モデル照合部２７０は、３次元モデル（３次元形状モデル）１０ごとにＥｄｇｅｌｅｔと対応点との誤差の統計量を計算することで、計測データにもっとも類似する３次元モデル（３次元形状モデル）１０を判断する。本ステップでは、３次元モデル（３次元形状モデル）１０と計測データとの間の誤差として、ステップＳ２０３０で算出したエッジの３次元座標と、推定される位置及び姿勢に基づいてカメラ座標系に変換した３次元モデル１０中の線分との距離の絶対値を求める。この線分と３次元点との距離の算出は、ステップＳ１０５０で記載した数式と同様であるため、その詳細な説明を省略する。そして、個々の３次元モデル（３次元形状モデル）１０中の誤差から、統計量として中央値を算出し、これを３次元モデル（３次元形状モデル）１０の照合度として保持する。全ての３次元モデル（３次元形状モデル）１０に対して、この誤差の統計量の算出を行い、誤差の統計量が最も小さくなる３次元モデル（３次元形状モデル）１０を判定することで、３次元モデル（３次元形状モデル）１０を照合する。具体的には、モデル照合部２７０は、画像特徴の３次元座標と３次元モデル１０との差が所定以下になるように、照合をする。なお、誤差の統計量としては、誤差の中央値以外でも、例えば平均値や最頻値を用いてもよい。誤差の少なさを判定できる指標であれば、いずれの方法でよく、特に制限はない。

本実施形態によれば、濃淡画像から検出したエッジの３次元座標を距離画像から取得し、エッジの３次元座標と３次元モデル１０との対応に基づいてモデルの照合を行うことで、距離画像にノイズがある場合にも安定的にモデルの照合を行うことが可能になる。

（第３の実施形態）≪画像特徴を画像からまとめて抽出する≫
第１及び第２の実施形態では、物体の概略の位置・姿勢に基づき３次元モデルの投影像近傍の範囲内で抽出した画像特徴に対してモデル当てはめを行う方法について説明した。第３の実施形態では、本発明を、画像全体から一度に抽出した画像特徴に対して距離画像に基づいて３次元情報を付与し、３次元特徴と３次元モデルとに基づいて物体の位置・姿勢を推定する方式に適用した場合について説明する。

図９は、本発明の第３の実施形態に係る情報処理装置（位置姿勢推定装置）を含む情報処理システム（位置姿勢推定システム）の概略構成の一例を示す模式図である。
図９に示すように、情報処理システム（位置姿勢推定システム）は、３次元モデル（３次元形状モデル）１０、２次元画像撮像装置２０、３次元データ計測装置３０、及び、情報処理装置（位置姿勢推定装置）３００を有して構成されている。

本実施形態に係る情報処理装置３００は、３次元モデル保存部３１０、２次元画像入力部３２０、距離画像入力部３３０、概略位置姿勢入力部３４０、画像特徴検出部３５０、画像特徴３次元情報算出部３６０、及び、位置姿勢算出部３７０を有して構成されている。

２次元画像撮像装置２０は、２次元画像入力部３２０に接続されている。３次元データ計測装置３０は、距離画像入力部３３０に接続されている。

３次元モデル保存部３１０は、３次元モデル１０のデータを保存し、位置姿勢算出部３７０に接続されている。情報処理装置（位置姿勢推定装置）３００は、３次元モデル保存部３１０に保存されている観察対象物体の形状を表す３次元モデル１０のデータに基づいて、２次元画像及び距離画像中の観察対象物体に当てはまるように物体の位置及び姿勢を推定する。

画像特徴検出部３５０は、２次元画像入力部３２０から入力された２次元画像の全体または一部領域から画像特徴を検出する。本実施形態では、画像特徴検出部３５０は、２次元画像の全体から、エッジ特徴を画像特徴として検出する。なお、画像からの線分エッジ検出処理の詳細については後述する。

画像特徴３次元情報算出部３６０は、画像特徴検出部３５０で検出された線分エッジの３次元座標を、距離画像入力部３３０により入力された距離画像を参照することで算出する。なお、画像特徴の３次元情報算出方法については後述する。

位置姿勢算出部３７０は、画像特徴３次元情報算出部３６０で算出された画像特徴の３次元座標（３次元情報）、及び、３次元モデル保存部３１０に保存されている観察対象物体の形状を表す３次元モデル１０に基づいて、観察対象物体の３次元的な位置及び姿勢を算出する。処理の詳細については、後述する。

また、３次元モデル保存部３１０、２次元画像入力部３２０、距離画像入力部３３０、概略位置姿勢入力部３４０は、それぞれ、第１の実施形態における３次元モデル保存部１１０、２次元画像入力部１２０、距離画像入力部１３０、概略位置姿勢入力部１４０と同一であるので説明を省略する。

次に、本実施形態における位置・姿勢推定方法の処理手順について説明する。
図１０は、本発明の第３の実施形態に係る情報処理装置における位置・姿勢推定方法（情報処理方法）の処理手順の一例を示すフローチャートである。

＜ステップＳ３０１０＞
まず、ステップＳ３０１０において、例えば情報処理装置３００は、初期化を行う。物体の概略の位置・姿勢の入力は、第１の実施形態におけるステップＳ１０１０の処理と同一であるのでその詳細な説明を省略する。

＜ステップＳ３０２０＞
続いて、ステップＳ３０２０において、２次元画像入力部３２０及び距離画像入力部３３０は、モデルフィッティングにより物体の位置及び姿勢を算出するための計測データを取得する。２次元画像（濃淡画像）と距離画像の入力は、第１の実施形態におけるステップＳ１０２０の処理と同一であるのでその詳細な説明を省略する。

＜ステップＳ３０３０＞
続いて、ステップＳ３０３０では、画像特徴検出部３５０は、ステップＳ３０２０において入力された濃淡画像上において、画像特徴の検出を行う。本実施形態においては、検出する画像特徴として、上述したようにエッジ特徴を検出する。エッジを検出する手法としては、例えば、sobelフィルタなどのエッジ検出フィルタを利用してもよいし、Cannyアルゴリズムを利用してもよい。ここでは、画像の画素値が不連続に変化する領域を検出することができれば、いずれの手法でもよく、手法の選択に特に制限はない。本実施形態では、Cannyアルゴリズムを利用してエッジ検出を行う。また、エッジ検出を行う領域は、画像全体から行ってもよいし、画像一部の領域に限定して処理を行ってもよい。画像から観察対象物体上の特徴が得られる限り、領域の設定に特に制限はなく、いかなる方式を用いてもよい。本実施形態では、画像全体からエッジ検出を行う。濃淡画像に対してCannyアルゴリズムによるエッジ検出を行うことで、エッジ領域と非エッジ領域に分けられた２値画像が得られる。画像中全領域のエッジ領域の検出が終了したら、ステップＳ３０３０の処理を終了し、ステップＳ３０４０に進む。

＜ステップＳ３０４０＞
続いて、ステップＳ３０４０において、画像特徴３次元情報算出部３６０では、ステップＳ３０３０で検出した濃淡画像中のエッジの３次元座標を算出する。３次元座標の算出は、ステップＳ３０３０で検出したエッジ領域の画素全てに対して行ってもよいし、画像上で等間隔になるようにエッジ領域の画素をサンプリングして、処理を行ってもよい。処理コストとして、問題がない範囲であれば、対象とするエッジ領域の画素の決め方に特に制限はない。本実施形態では、ステップＳ３０３０で検出したエッジ領域全画素に対して、３次元座標の算出処理を行う。ここで、エッジの３次元座標の算出処理は、第１の実施形態におけるステップＳ１０４０の処理とほぼ同一であるのでその詳細な説明を省略する。ただし、第１の実施形態と異なる点は、第１の実施形態でＥｄｇｅｌｅｔの対応候補点ごとに行っていた処理を、本実施形態ではステップＳ３０３０で検出したエッジ領域の全画素を対象にして処理する点である。濃淡画像中の全エッジ領域画素の３次元座標算出処理が終了したら、ステップＳ３０４０の処理を終了し、ステップＳ３０５０に進む。

＜ステップＳ３０５０＞
続いて、ステップＳ３０５０において、位置姿勢算出部３７０は、非線形最適化計算を用いて、計測データに３次元形状モデルが３次元空間中で当てはまるように観察対象物体の概略の位置・姿勢を反復演算により補正することで観察対象物体の位置・姿勢を算出する。まず、ステップＳ３０５０では、ステップＳ３０４０で算出したエッジ画素の３次元座標と、３次元モデル中の３次元ラインとの対応付けを行う。次いで、ステップＳ３０１０で入力された観察対象物体の概略位置及び姿勢に基づいてカメラ座標系に変換した３次元モデル中の３次元ライン各々と、ステップＳ３０４０で算出したエッジ画素の３次元座標各々との距離を算出し、距離がもっとも近くなるエッジ画素の３次元座標と、３次元モデル中の３次元ラインとの組を対応付ける。そして、対応付けられたエッジ画素の３次元座標と３次元モデル中の３次元ラインとの距離に基づいて位置・姿勢の最適化を行う。位置・姿勢の最適化処理は、第１の実施形態におけるステップＳ１０５０の処理とほぼ同一であるのでその詳細な説明を省略する。位置及び姿勢の推定処理を繰り返すことにより、最終的な位置及び姿勢が算出されたら、ステップＳ３０５０の処理を終了し、ステップＳ３０６０に進む。

＜ステップＳ３０６０＞
続いて、ステップＳ３０６０において、例えば情報処理装置３００は、位置・姿勢の算出を終了する入力がなされたか否かを判断する。この判断の結果、位置・姿勢の算出を終了する入力がされた場合には、本フローチャートの処理を終了し、一方、位置・姿勢の算出を終了する入力がされなかった場合には、ステップＳ３０１０に戻り、新たな画像を取得して再度、位置・姿勢の算出を行う。

本発明の第１の実施形態〜第３の実施形態において、位置姿勢算出部１７０、モデル照合部２７０及び位置姿勢算出部３７０は、画像特徴の３次元座標に対し３次元モデルを当てはめる「モデル当てはめ手段」を構成する。

次に、本発明の第１の実施形態及び第３の実施形態の変形例を変形例５として説明する。

［変形例５］≪最小二乗でなく、マッチングに基づく位置姿勢推定でもよい≫
前述の第１の実施形態及び第３の実施形態では、濃淡画像及び距離画像から検出した特徴の３次元座標と、３次元モデル中の３次元ラインとに基づいて、両者の３次元空間中の位置差が小さくなるような位置姿勢補正量を算出することで、位置及び姿勢を推定する処理を行っていた。しかしながら、位置及び姿勢を推定する処理は、上述の方法に限るものではない。例えば、位置姿勢補正量を計算せず、濃淡画像及び距離画像から算出した特徴の３次元座標と、３次元モデル中の３次元ラインとの３次元空間中の位置差が最も小さくなるような位置姿勢を、一定範囲で走査することで求めてもよい。位置姿勢の算出方法としては、濃淡画像及び距離画像から算出した特徴の３次元座標と、３次元モデル中の３次元ラインとが当てはまるような位置姿勢を算出できる方法である限り、位置・姿勢を算出する方式に制限はなく、いかなる方式を選択してもよい。

（第４の実施形態）≪ロボットを含む情報処理システム≫
本発明に係る情報処理装置の好適な適用事例としては、産業用ロボットアームの手先部に設置して、把持する物体を観察対象物体として、その位置・姿勢を計測（算出）する目的に利用することが挙げられる。

以下、図１１を用いて、本発明に係る情報処理装置の適用例を説明する。
図１１は、本発明の第４の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。ここで、図１１に示す情報処理装置としては、第１の実施形態に係る情報処理装置１００を適用しているが、例えば、第２の実施形態に係る情報処理装置２００や第３の実施形態に係る情報処理装置３００を適用してもよい。

図１１には、ロボット４０を用いて観察対象物体６０を把持するロボットシステムの構成例が示されている。ロボット４０は、ロボットコントローラ５０により制御され、指令された位置に手先を移動させて観察対象物体６０を把持するロボットである。このロボット４０は、回転軸および／または並進移動軸からなる可動軸を有するロボットである。観察対象物体６０は、作業台に置かれる位置が変わるため、概略把持位置を現在の観察対象物体６０の位置に補正する必要がある。

２次元画像撮像装置２０及び３次元データ計測装置３０は、情報処理装置１００に接続されている。３次元モデル１０のデータは、観察対象物体６０の形状に即しており、情報処理装置１００に入力される。位置姿勢計測装置である情報処理装置１００は、２次元画像撮像装置２０及び３次元データ計測装置３０の撮像装置から得られる、観察対象物体６０が撮像された２次元画像と距離画像に対して、３次元モデル１０が当てはまるように、撮像装置に対する観察対象物体６０の位置・姿勢を推定する。ロボットコントローラ５０は、情報処理装置１００が出力する観察対象物体６０の位置・姿勢の情報に基づいてロボット４０の制御を行い、ロボット４０の手先が観察対象物体６０を把持できる位置・姿勢に移動させる。

本実施形態における情報処理装置により、ロボットシステムは、観察対象物体の位置が不定でも位置・姿勢の推定を行うことで、観察対象物体を把持することが可能となる。

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

２００情報処理装置（モデル照合装置）、２１０３次元モデル保存部、２２０２次元画像入力部、２３０距離画像入力部、２４０概略位置姿勢入力部、２５０画像特徴検出部、２６０画像特徴３次元情報算出部、２７０モデル照合部

Claims

物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存手段と、
前記物体を撮像した２次元画像を入力する２次元画像入力手段と、
前記物体を撮像した距離画像を入力する距離画像入力手段と、
前記２次元画像入力手段から入力された２次元画像から画像特徴を検出する画像特徴検出手段と、
前記距離画像入力手段から入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出手段と、
前記画像特徴の３次元座標に対し前記３次元モデルを当てはめるモデル当てはめ手段と
を有することを特徴とする情報処理装置。
前記モデル当てはめ手段は、前記画像特徴の３次元座標と前記３次元モデルとの一致度または不一致度に基づいて３次元モデルを照合することを特徴とする請求項１に記載の情報処理装置。
前記モデル当てはめ手段は、前記画像特徴の３次元座標と前記３次元モデルとの差に基づいて前記物体の位置・姿勢を算出することを特徴とする請求項１に記載の情報処理装置。
前記２次元画像と前記距離画像とは略同一視点から撮像されており、画像間の対応が既知であることを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
前記画像特徴３次元情報算出手段において、前記画像特徴の検出位置近傍に対応する距離値を前記距離画像から参照することで、前記画像特徴の３次元座標を少なくとも１つ以上算出することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
前記画像特徴３次元情報算出手段において、前記距離画像から前記画像特徴の検出位置近傍に対応する１つ以上の距離値を参照して計算した距離値の統計量に基づいて、前記画像特徴の３次元座標を算出することを特徴とする請求項５に記載の情報処理装置。
前記画像特徴検出手段は、前記２次元画像から検出する画像特徴としてエッジまたは点または面領域を検出することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
請求項１乃至７のいずれか１項に記載の情報処理装置と、
回転軸および／または並進移動軸からなる可動軸を有するロボットと、
前記物体の位置・姿勢に基づいて、前記ロボットの手先を、前記物体を把持できる位置・姿勢に移動させる制御をするロボットコントローラと
を有することを特徴とする情報処理システム。
物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存ステップと、
前記物体を撮像した２次元画像を入力する２次元画像入力ステップと、
前記物体を撮像した距離画像を入力する距離画像入力ステップと、
前記２次元画像入力ステップから入力された２次元画像から画像特徴を検出する画像特徴検出ステップと、
前記距離画像入力ステップから入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出ステップと、
前記画像特徴の３次元座標と前記３次元モデルとを照合するモデル照合ステップと
を有することを特徴とする情報処理方法。
物体の幾何的特徴を表す３次元モデルのデータを保存する３次元モデル保存ステップと、
前記物体を撮像した２次元画像を入力する２次元画像入力ステップと、
前記物体を撮像した距離画像を入力する距離画像入力ステップと、
前記２次元画像入力ステップから入力された２次元画像から画像特徴を検出する画像特徴検出ステップと、
前記距離画像入力ステップから入力された距離画像から前記画像特徴に対応する３次元座標を算出する画像特徴３次元情報算出ステップと、
前記３次元モデルが３次元空間中で当てはまるように前記物体の位置・姿勢を算出する位置姿勢算出ステップと
を有することを特徴とする情報処理方法。