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

Description

本発明は、三次元形状が既知である物体の位置姿勢を計測する技術に関する。

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

位置姿勢を計測する方法として、二次元画像上から検出される特徴や距離画像に対して物体の三次元形状モデルをあてはめるモデルフィッティングによる計測がある。二次元画像に対するモデルフィッティングでは、物体の位置姿勢に基づいて三次元形状モデルを画像上に投影したときの投影像が、検出された特徴に当てはまるように位置姿勢を推定する。距離画像に対するモデルフィッティングでは、距離画像の各点を三次元座標を持つ三次元点群に変換し、三次元空間において三次元形状モデルが三次元点群に当てはまるように位置姿勢を推定する。

しかし、二次元画像上での特徴の検出位置や点群の三次元座標には、画素の量子化誤差
やブラー、特徴検出アルゴリズムの精度、カメラ間の対応等に起因する誤差が存在する。

そこで、多数の計測情報（画像上の特徴や点群）に含まれる計測誤差の影響を平均化する等、位置姿勢の計測精度を向上させる取り組みがなされている。

日浦、山口、佐藤、井口、"距離画像と濃淡画像の融合による任意形状物体の実時間追跡"、電子情報通信学会論文誌Ｄ−ＩＩ，ｖｏｌ．Ｊ８０−ＤＩＩ，ｎｏ．１１，ｐｐ．２９０４−２９１１，１９９７．

高精度に位置姿勢を計測する方法として、明示的な特徴検出は行わず、濃淡画像と距離画像の勾配を利用した位置姿勢を推定する方法がある（非特許文献１参照）。つまり、運動による明度変化、距離変化を滑らかであると仮定し、濃淡画像の明度変化と距離画像の距離変化から勾配法に基づき物体の姿勢パラメータを算出する。しかし、濃淡画像の二次元と距離画像の三次元では、尺度が異なり、有効に組み合わせることが難しく、人手によってチューニングすることでしか、対応することができないという課題があった。

本発明は、以上の課題を鑑みてなされたものであり、２次元画像から得られる計測情報と距離データから得られる計測情報を併用した位置姿勢推定を行うことにより、対象物体の位置姿勢を高精度に計測することを目的とする。

上記の課題を解決するために、本発明の情報処理装置は、対象物体の形状を表すモデルを保持する保持手段と、前記対象物体の２次元画像と前記対象物体の表面上の３次元点群の情報とを取得する取得手段と、前記対象物体の概略の位置姿勢を取得する概略位置姿勢取得手段と、前記取得した２次元画像から画像特徴を検出する検出手段と、前記概略位置姿勢に基づいて、前記検出された画像特徴と前記モデルを構成する幾何特徴とを対応付け、前記３次元点群の距離点と前記モデルを構成する幾何特徴とを対応づける対応付け手段と、前記対応付けられた２次元画像上における前記画像特徴に対応する３次元位置と、３次元空間で前記概略位置姿勢に配置した前記モデルの幾何特徴の３次元位置との間の３次元空間における第１の距離を取得する第１の取得手段と、前記対応づけられた距離点の３次元位置と３次元空間で前記概略位置姿勢に配置した前記モデルの幾何特徴との３次元空間における第２の距離を取得する第２の取得手段と、３次元空間における前記第１の残差及び第２の残差に基づいて、前記対象物体の位置姿勢を導出する位置姿勢導出手段とを備える。

本発明により、対象物体の位置姿勢を高精度に計測することができる。

本発明の第１の実施形態における情報処理装置の構成を示す図である。 本発明の第１の実施形態における３次元形状モデルの構成要素を説明する図である。 本発明の第１の実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。 本発明の第１の実施形態における、３次元ずれ算出処理の詳細な手順を示すフローチャートである。 本発明の第１の実施形態における、３次元形状モデルと画像特徴間の距離算出を説明する図である。 本発明の第１の変形例における、３次元形状モデルと画像特徴間の距離算出を説明する図である。 本発明の第２の変形例における、３次元形状モデルと画像特徴間の距離算出を説明する図である。 本発明の第２の実施形態における情報処理装置の構成を示す図である。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第１の実施形態］
本実施形態における位置姿勢推定の処理について詳細を述べる。なお、本実施形態で述べる「位置姿勢」とは、対象物体と当該部品を撮影するカメラとの相対的な位置姿勢の関係のことを意味する。

図１は、本実施形態における情報処理装置１の構成を示している。図１に示すように、情報処理装置１は、モデル保持部１１０、画像入力部１２０、距離データ入力部１３０、概略位置姿勢入力部１４０、特徴検出部１５０、３次元ずれ算出部１６０、位置姿勢推定部１７０から構成されている。また、図１に示す構成は、本発明の情報処理装置の適用例となる構成である。なお、情報処理装置１は、コンピュータ（ＣＰＵ）がメモリに蓄積されたプログラムを実行して所定の機能を実現する、通常の情報処理装置によって実現され得ることは明らかである。

次に、情報処理装置１を構成する各部について説明する。

モデル保持部１１０は、対象物体の形状を表す３次元形状モデル１０を保持する。本実施例では、３次元形状モデル（例えば、ＣＡＤモデルやポリゴンモデル）は、図２（ｂ）に示すような、３次元位置と３次元法線方向から構成される物体表面上の局所的な３次元平面情報（以後、局所面特徴と称する）と、図２（ａ）に示すような、３次元位置と３次元線分方向から構成される物体輪郭上の局所的な３次元線分情報（以後、局所線特徴と称する、単にモデル特徴と称した場合は、局所面特徴または局所線特徴のどちらかを指す）によって構成されるものとする。

ただし、３次元形状モデルとして保持する形状情報は、対象形状を表す３次元的なモデル情報であれば良く、表現形式に特に制限はない。例えば、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成される面および線の集合で表されるポリゴン形式の形状情報など、他の表現で表しても良い。例えば、３点と３辺および１面により構成されるポリゴンの集合として、３次元形状モデルを表現してもよいし、単純な３次元点の集合として３次元形状を表現してもよい。また陰関数の組み合わせによりパラメトリックに３次元形状を表現する方法を用いてもよい。対象物体の形状に即する限り、３次元形状モデルの表現方法に特に制限はない。３次元形状モデル１０は、モデル保持部１１０に保存され、位置姿勢推定部１５０に入力される。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では２次元画像撮像装置２０は濃淡画像を出力する。２次元画像撮像装置２０が撮影する画像は画像入力部１２０を介して情報処理装置１に入力される。なお、情報処理装置１に入力される画像は、２次元画像撮像装置２０により撮影される画像に限るものではなく、記憶媒体等に保存されたファイルまたは外部ネットワークから読み込んだ画像を入力してもよい。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、また、以下の文献で開示される方法によって事前にキャリブレーションしておく。
Ｒ． Ｙ． Ｔｓａｉ， “Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ，” ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， ｖｏｌ．ＲＡ−３， ｎｏ．４， １９８７

画像入力部１２０は、２次元撮像装置２０によって撮像された画像を情報処理装置１に入力する。画像入力部１２０は、撮像装置の出力がＮＴＳＣなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がＩＥＥＥ１３９４などのデジタル出力であれば、例えばＩＥＥＥ１３９４インタフェースボードによって実現される。また、画像取得部１２０は、予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出すことにより画像を取得するものであってもよい。

距離画像撮像装置３０は、計測対象である物体表面上の点の３次元情報を計測する。本実施形態では、距離画像撮像装置３０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。

本実施形態では、距離センサとしては、波長の異なる色ＩＤを付与したマルチスリットラインを対象に照射、その反射光をカメラで撮像して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

距離データ入力部１３０は、距離画像撮像装置３０によって計測された距離画像を情報処理装置１に入力する。なお、情報処理装置１に入力される距離データは、上述の距離画像に限るものではなく、例えば３次元点群などの画像の形となっていないデータであってもかまわない。また、距離画像撮像装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する濃淡画像の各画素と、距離画像撮像装置が出力する距離画像の各画素の対応は既知であるとする。

ただし、本実施形態の適用は、濃淡画像と距離画像が同一の視点である場合に限るものではない。例えば、濃淡画像を撮像する撮像装置と距離画像を撮像する撮像装置が別の位置姿勢にあり、濃淡画像と距離画像をそれぞれ別の視点から撮像しても良い。この場合は、撮像装置間の相対的な位置姿勢は既知であるとして、距離画像中の３次元点群を濃淡画像に投影することにより、濃淡画像と距離画像の対応を取る。同一の物体を撮像する撮像装置間の相対的な位置姿勢が既知であり、その画像間の対応が計算できる限り、撮像装置の位置関係に特に制限はない。以下、２次元画像撮像装置２０と距離画像撮像装置３０を合わせた機器を撮像装置２０３０と称する。

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

しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものでもよい。また、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチング（パターンマッチング）を行うことによって、対象物体の大まかな位置と姿勢を推定してもよい。あるいは、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。

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

特徴検出部１５０では、画像入力部１２０から入力された二次元画像から画像特徴を検出する。本実施形態では画像特徴としてエッジを検出する。しかしながら、本実施形態の画像特徴は、エッジに限られるものではない。例えば、画像特徴として特徴点を検出してもよい。この場合、対象物体の三次元形状モデルを三次元点群データによって表し、特徴点と三次元の点の対応に基づいて位置姿勢を算出する。また、特定の特徴のみ用いるのではなく、複数の特徴（特徴点とエッジ）を位置姿勢の算出に利用してもよい。以上に示すように、画像特徴としては、３次元形状モデルとの対応付けを行うことが可能である特徴であれば、いかなる特徴であっても特に問題ない。特徴検出処理の詳細については後述する。

３次元ずれ算出部１６０では、濃淡画像及び距離データから検出された観測情報と、モデル保持部１１０により保持された３次元形状モデル１０との間の３次元空間中のずれ（以後、残差と称する）を計量する。

本実施形態では、画像特徴の方向と位置に基づいて定義される３次元平面と、モデル特徴の３次元位置との距離を計量する。しかしながら、この方法に限るものではない。例えば、画像特徴の方向を利用せず、視点から画像エッジへ伸ばす方向に、画像エッジの３次元位置を仮に設定し、その位置に基づいてモデル特徴（幾何特徴）との距離を算出してもよい。また、３次元平面を算出せずに、視点から画像特徴の位置へ伸ばした視線ベクトルと、モデル特徴の位置と方向から定義される３次元直線との最短距離に基づいて、画像特徴とモデル特徴との残差を計量しても良い。３次元空間中において、画像エッジと局所線特徴との間の残差を計量出来る限り、他の方法でもよい。本実施形態における処理の詳細については後述する。

位置姿勢算出部１７０では、３次元ずれ算出部１６０において計量された、濃淡画像及び距離データと３次元形状モデル１０とのずれ量に基づいて、撮像装置２０３０と対象物体との間の位置及び姿勢を計測する。本実施形態は、濃淡画像と距離画像を同時に利用して位置姿勢を推定する方式に限るものではなく、濃淡画像のみ、距離画像のみを用いて位置姿勢推定を行う場合においても同様に適用可能である。処理の詳細については後述する。

以上が、情報処理装置１の構成の一例についての説明である。

次に、本実施形態における位置姿勢推定の処理手順について説明する。図３は、本実施形態における位置姿勢推定の処理手順を示すフローチャートである。

（ステップＳ１１００）
まず、ステップＳ１１００において、撮像装置２０３０に対する対象物体の位置及び姿勢の概略値を、概略位置姿勢入力部１４０を介して情報処理装置１に入力する。本実施形態では、前述したように、前の時刻において計測された位置及び姿勢を、物体の概略位置姿勢として用いる。概略位置姿勢の入力が終了したら、ステップＳ１１００の処理を終了し、ステップＳ１２００に進む。

（ステップＳ１２００）
次に、ステップＳ１２００において、対象物体の濃淡画像を取得する。２次元画像撮像装置２０から濃淡画像を取得し、画像入力部１２０を介して情報処理装置１に入力する。濃淡画像の取得が終了したら、ステップＳ１２００の処理を終了し、ステップＳ１３００に進む。

（ステップＳ１３００）
次に、ステップＳ１３００において、対象物体の距離データを取得する。距離画像撮像装置３０からの距離画像の取得を行い、距離データ入力部１３０を介して情報処理装置１に入力する。本実施形態では、距離画像は撮像装置から対象物体の表面までの距離が格納されているものとする。前述のように、２次元画像撮像装置２０と距離画像撮像装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。距離画像の取得した後、距離画像から３次元点群を算出する。距離画像からの三次元点群の算出は、距離画像の各画素について、画素位置に対応する視線ベクトルに奥行き値を乗算することにより行う。距離画像の取得および３次元点群の算出が終了したら、ステップＳ１３００の処理を終了し、ステップＳ１４００に進む。

（ステップＳ１４００）
次に、ステップＳ１４００では、ステップＳ１２００において取得した濃淡画像から画像特徴の検出を行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配の極値となる点である。本実施形態ではエッジ検出方法として、以下の文献で開示されるＣａｎｎｙエッジアルゴリズムを用いる。
Ｃａｎｎｙ， Ｊ．， “Ａ Ｃｏｍｐｕｔａｔｉｏｎａｌ Ａｐｐｒｏａｃｈ Ｔｏ Ｅｄｇｅ Ｄｅｔｅｃｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓ． Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ８（６）：６７９−６９８， １９８６．

エッジ検出処理の結果、濃淡画像中でのエッジ位置とその方向が得られる。また、エッジ検出方法はこれに限るものではなく、例えば、Ｓｏｂｅｌフィルタやｐｒｅｗｉｔｔフィルタなどのエッジ検出フィルタを用いて、その応答値からエッジ位置と方向を算出してもよい。また、上述したような画像全体からエッジを検出する方式に限らず、着目領域近傍で局所的にエッジ検出を行うような方法を用いても良い。濃淡画像上で検出され、局所線特徴と対応づけられるエッジ特徴であれば、いかなる方法で検出しても問題はなく、その検出方法の選択に特に制限はない。濃淡画像から全ての画像エッジを検出する処理を終了すればステップＳ１４００の処理を終了し、ステップＳ１５００に進む。

（ステップＳ１５００）
次に、ステップＳ１５００では、ステップＳ１４００において濃淡画像から抽出された画像エッジ、および、Ｓ１３００において入力された距離点群と３次元形状モデル１０との対応を算出し、対応間の３次元空間におけるずれ量を算出する。

本実施形態では、撮像装置２０３０の視点位置（カメラ中心）と画像エッジの位置と方向から定義される３次元平面と、局所線特徴の３次元位置との距離を、画像エッジと局所線特徴との残差として算出する。これにより、３次元空間中での残差に統一して、位置姿勢を推定することが可能になる。

以下、図４を用いて、ステップＳ１５００における３次元ずれ算出処理について詳細な処理を詳述する。

（ステップＳ１５１０）
まず、ステップＳ１５１０では、ステップＳ１１００において入力された、対象物体の概略位置姿勢に基づいて、ステップＳ１４００において抽出された濃淡画像中の画像エッジと、３次元形状モデル１０との対応付けを行う。それぞれの物体の概略位置姿勢と校正済みの２次元画像撮像装置３０の内部パラメータを用いて３次元形状モデルを構成する局所線特徴を画像へ投影し、投影像近傍の領域を探索して検出されたエッジと局所線特徴とを対応付けする。局所線特徴の投影像近傍で複数のエッジが検出された場合には、複数検出されたエッジのうち、投影された局所線特徴に対して画像上で最も近いエッジを対応付ける。

（ステップＳ１５２０）
次に、ステップＳ１５２０では、ステップＳ１１００において入力された、対象物体の概略位置姿勢に基づいて、ステップＳ１３００において取得した距離画像および３次元点群と３次元形状モデル１０との対応付けを行う。概略位置姿勢と校正済みの距離画像撮像装置３０の内部パラメータを用いて、３次元形状モデルを構成する各局所面特徴を距離画像上に投影する。そして、各局所面特徴の投影位置で参照される距離値に基づく距離点を各局所面特徴に対応する３次元点として保持する。

（ステップＳ１５３０）
次に、ステップＳ１５３０では、ステップＳ１５１０で算出した局所線特徴と画像エッジの対応間の残差とそのヤコビアンを算出する。前述した通り、本実施形態では、撮像装置２０３０の視点位置（カメラ中心）から画像エッジを通る３次元平面を算出し、該３次元平面と、局所線特徴の位置と間の符号付き距離を、画像エッジと局所線特徴との３次元空間中における残差として算出する。図５（ａ）は、画像エッジを通る３次元平面と局所線特徴との残差計量を説明する図である。

まず、画像エッジの画像座標（ｕ，ｖ）から、焦点距離が１となる正規化画像座標系における座標（ｕ’，ｖ’）を算出することで、視点位置から見た画像エッジの３次元位置ｑ_ｉｍｇを算出する。視点位置から見た画像エッジの３次元位置ｑ_ｉｍｇは、撮像装置２０３０の焦点距離（ｆｘ，ｆｙ）、および、主点位置（ｐｘ，ｐｙ）に基づいて、以下の式より計算できる

次に、画像エッジの３次元空間中への逆投影像となる３次元平面を算出する。具体的には、視点位置と画像エッジの３次元位置ｑ_ｉｍｇを通り、画像エッジの方向ｄ_ｉｍｇに平行な３次元平面の法線ｎ_ｉｍｇを算出する。該３次元平面の法線ｎ_ｅは、画像エッジの方向ｄ_ｉｍｇと画像エッジの３次元位置ｑ_ｉｍｇより、以下の式より算出できる。

以上により得られた３次元平面と局所線特徴の３次元位置との間の、面と点間の符号付き距離を算出することで、画像エッジと局所線特徴との３次元空間内における残差ｅｒｒ_ｅｄｇｅを算出する。対象物体の概略位置姿勢ｓに基づいて算出した、視点位置から見た局所線特徴の位置をｐ_ｅとすると、画像エッジの逆投影像となる該３次元平面の法線ｎ_ｉｍｇと画像エッジの３次元位置ｑ_ｉｍｇより、３次元空間内における残差ｅｒｒ_ｅｄｇｅは以下の式より算出できる。
ｅｒｒ_ｅｄｇｅ＝−ｎ_ｉｍｇ・（ｑ_ｉｍｇ−ｐ_ｅ） （式３）
次に、局所線特徴と画像エッジの３次元空間中における残差に関するヤコビアンを算出する。ここで、残差のヤコビアンとは、位置姿勢６自由度のパラメータが微小に変化したときに、局所線特徴と画像エッジとの間の残差が変化する割合を表す値である。また、対象物体の位置及び姿勢ｓは６次元ベクトルであり、対象物体の位置を表す３つの要素（ｓ_１，ｓ_２，ｓ_３）と、姿勢を表す３つの要素（ｓ_４，ｓ_５，ｓ_６）からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す３次元ベクトルなどによって表現される。この位置及び姿勢ｓの各パラメータで、対応間残差ｅｒｒ_ｅｄｇｅを偏微分することによって、以下のヤコビアン行列を算出する。

以上の手順を、ステップＳ１５１０で対応づけられた画像エッジと局所線特徴との全ての対応組みに対して処理を行い、全ての対応組みに関する局所線特徴と画像エッジとの３次元空間中における残差とヤコビアンを計算する。

（ステップＳ１５４０）
次に、ステップＳ１５４０では、ステップＳ１５２０で算出した局所面特徴と距離点との対応間の残差とそのヤコビアンを算出する。図５（ｂ）は、局所面特徴と対応点（距離画像中の３次元点）との残差の計算を説明する図である。対象物体の概略位置姿勢ｓに基づいて算出した、撮像装置２０３０から見た局所面特徴の位置をｐ_ｒ、面の法線方向をｎ_ｒとし、所面特徴点に対応する距離点の３次元座標をｑ_ｒとすると、局所面特徴と距離点との間の残差は、以下の式より算出できる。
ｅｒｒ_{ｒａｎｇｅ}＝−ｎ_ｒ・（ｑ_ｒ−ｐ_ｒ） （式５）
そして、局所線特徴のヤコビアンと同様に、仮想カメラの位置及び姿勢ｓの各パラメータで、対応間距離ｅｒｒ_{ｒａｎｇｅ}を偏微分することによって、局所面特徴と距離点間の残差のヤコビアン行列は以下のように算出できる。

以上の手順を、ステップＳ１５２０で対応づけられた全ての局所面特徴と３次元点群との対応組みに対して処理を行い、全ての対応組みに関する局所面特徴と３次元点群との３次元空間中における残差とヤコビアンを計算する。

以上の処理により、局所線特徴と画像エッジの対応組みに関する残差とヤコビアン、および、局所面特徴と３次元点群の対応組みに関する残差とヤコビアンの算出が終了したら、ステップＳ１５００の処理を終了し、ステップＳ１６００に進む。

（ステップＳ１６００）
次に、ステップＳ１６００では、ステップＳ１５００において算出した残差を最小化するように対象物体の位置及び姿勢を算出（位置姿勢導出）する。上述したように、画像エッジと局所線特徴間の残差と、距離点と局所面特徴の残差は、３次元空間中の残差として共通の尺度で計量されているため、各対応間の残差を最小になるように線形連立方程式を解くことで、尺度が統一化して併用した位置姿勢の推定が可能である。

ステップＳ１５１０で濃淡画像上のエッジと対応づけられた局所線特徴の数をｉとして、ステップＳ１５３０で算出した各局所線特徴と画像エッジとの３次元空間中における残差をｅｒｒ_{ｅｄｇｅｉ}、そのヤコビアンをＪ_{ｅｄｇｅｉ}とする。また、ステップＳ１５２０で距離画像中の距離点と対応づけられた局所面特徴の数をｊとして、ステップＳ１５４０で算出した各局所面特徴と距離点との３次元空間中における残差をｅｒｒ_{ｒａｎｇｅｊ}、そのヤコビアンをＪ_{ｒａｎｇｅｊ}、とすると、各対応間の誤差を最小化する位置姿勢の更新分は、以下のようなΔｓに関する線形連立方程式として立式できる。

ここで、ヤコビアンをＪ、対応データ間の誤差をＥとおくと、式８を以下の式で表すことができる。
ＪΔｓ＝Ｅ （式８）
次式に示すように一般化逆行列を用いて式１２を解くことにより補正値Δｓを求める。
Δｓ＝（Ｊ^ＴＪ）^−１Ｊ^ＴＥ （式９）
これにより得られたΔｓを用いて、位置及び姿勢を更新する。

なお、ここでは、はずれ値除去を行わない単純な非線形最適化手法について説明した。しかし、例えばｍ推定を利用したはずれ値除去を行ってもよい。また、本実施形態では、非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた。しかしながら、非線形最適化手法はこれに限るものではない。

例えばよりシンプルな方法である最急降下法によって行ってもよい。また、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法やＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。また、最適化計算ベースの位置姿勢算出以外にも、概略の位置及び姿勢を中心に一定範囲において６自由度の値が網羅的にカバーできるように多数の位置及び姿勢を生成し、それぞれの位置及び姿勢において観測できるモデル特徴と濃淡画像及び距離画像とのマッチング具合を評価することで、位置姿勢の推定を行っても良い。また、位置姿勢算出においては、位置および姿勢の両方を同時に推定する例を説明したが、本実施形態は、位置のみ、姿勢のみ、または、位置と姿勢の任意の成分の組み合わせのみを推定する場合においても同様に適用可能である。

以上の処理により、位置及び姿勢の更新処理が終了すればステップＳ１６００の処理を終了し、ステップＳ１７００に進む。

（ステップＳ１７００）
次に、ステップＳ１７００では、ステップＳ１６００で更新した位置姿勢が、収束しているか否か、すなわち、さらに反復計算を必要とするか否かの判定を行う。具体的には、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。収束していなければ、ステップＳ１５００に戻り、更新した位置姿勢を用いて、再度３次元ずれ算出処理および位置姿勢算出処理を行う。収束していると判断した場合は、この処理は終了する。

以上の処理により、対象物体の位置及び姿勢の更新を終えたら、撮像装置と対象物体間の相対的な位置姿勢の最終的な推定値が決定される。

以上述べたように、本実施形態では、２次元画像とモデルとの距離として、２次元画像から得られる画像特徴を３次元空間中に逆投影することで得られる３次元平面とモデル特徴の３次元位置との間の距離を算出することで、３次元空間中の距離として統一的に計量して２次元画像と距離データを併用した位置姿勢推定を行う方法について述べた。これにより、計測情報の寄与率の偏りのない高精度な位置姿勢推定を行うことが可能になる。また、画像エッジの残差の尺度変換処理を自体必要なくなるため、従来の濃淡画像と距離データを併用した位置姿勢推定手法よりも高速な位置姿勢推定が可能となる。

［第１の変形例］
第１の実施形態では、画像特徴とモデル特徴との残差計量方法として、２次元画像から得られる画像特徴の方向と位置および視点位置により定義される３次元平面と、モデル特徴との距離を計量する方法について説明した。しかし、画像特徴とモデル特徴との残差を、３次元空間中の距離として統一的に計量する方法は、上述の方式に限るものではない。

例えば、画像特徴の方向を利用せず、視点から画像エッジの位置へ伸ばす直線の３次元点と、モデル特徴との距離を算出してもよい。具体的には、画像エッジの３次元位置の奥行き値がモデル特徴の奥行き値と同じに設定する。そして、視点位置とモデル特徴の３次元位置を通りモデル特徴の３次元方向に平行な３次元平面と、画像エッジの３次元位置との間の符号付き距離を算出する。

以下、逆投影による画像エッジの３次元位置に基づく残差計量方法について詳細を説明する。以下はこれまでに説明した第１の実施形態における、画像エッジと局所線特徴間の残差を算出するステップＳ１５３０に置き換わる処理である。

図６は、逆投影により仮に設定した画像エッジの３次元位置と局所線特徴との間の３次元空間中における残差の計算を説明する図である。

まず、画像エッジを３次元空間中に逆投影し、３次元空間中における画像エッジの仮の３次元位置ｑ_ｅを算出する。対象物体の概略位置姿勢ｓに基づいて算出した、視点位置から見た局所線特徴の位置をｐ_ｅ＝（ｐ_ｅｘ，ｐ_ｅｙ，ｐ_ｅｚ）とする。画像エッジを３次元空間中へ逆投影して計算される３次元位置ｑ_ｅの奥行き値が、局所線特徴の奥行き値ｐ_ｅｚと同じになると仮定すると、画像エッジの仮の３次元位置ｑ_ｅは、式１より得られる画像エッジの位置ｑ_ｉｍｇより、以下の式より算出される。
ｑ_ｅ＝（ｑ_ｉｍｇｐ_ｅｚ） （式１０）
そして、得られた画像エッジの３次元位置ｑ_ｅに基づき、局所線特徴と画像エッジとの対応間の３次元空間内における残差ｅｒｒ_ｅｄｇｅを算出する、対応間残差ｅｒｒ_ｅｄｇｅは、画像エッジの３次元位置ｑ_ｅと、局所線特徴の方向ｄ_ｅに沿って局所線特徴の位置ｐ_ｅと視点位置を通る３次元平面との、符号付き距離として算出する。該３次元平面の法線方向をｎ_ｅ＝ｄ_ｅ×ｐ_ｅとすると、局所線特徴と画像エッジとの間の３次元空間内における残差ｅｒｒ_ｅｄｇｅは以下の式より算出できる。
ｅｒｒ_ｅｄｇｅ＝−ｎ_ｅ・（ｑ_ｅ−ｐ_ｅ） （式１１）
こうして得られた生成した局所線特徴と画像エッジとの間の３次元空間内における残差ｅｒｒ_ｅｄｇｅに基づき、ステップＳ１５３０の以降の処理を第１の実施形態と同様に実行する
また、画像特徴とモデル特徴との残差計量方法は、以上に示す方法に限るものではなく、画像特徴とモデル特徴との間の、３次元空間中における残差を計量できる限り、残差計量方法に特に制限はなく、いかなる方法を用いても特に問題はない。

以上、画像特徴とモデル特徴との残差を、３次元空間中の距離として統一的に計量する方法として、画像特徴の方向を利用せず、視点から画像エッジの位置へ伸ばす直線上の３次元点と、モデル特徴との距離を算出する方法について説明した。

［第２の変形例］
第１の実施形態および第１の変形例では、画像特徴の位置と方向、または、モデル特徴の位置と方向から定義される３次元平面に基づいて、画像特徴とモデル特徴との残差計量を行う方法について説明した。

しかし、画像特徴とモデル特徴との残差計量を行う方法は、上述の３次元平面に基づく方法に限るものではない。例えば、視点位置から画像特徴の位置へ伸ばした３次元直線と、モデル特徴の位置と方向から定義される３次元直線との最短距離に基づいて、画像特徴とモデル特徴との残差を計量しても良い。以下、画像特徴の視線ベクトルとモデル特徴を表す３次元直線との距離に基づく残差計量方法について詳細を説明する。以下は、これまでに説明した第１の実施形態における、画像エッジと局所線特徴間の残差を算出するステップＳ１５３０に置き換わる処理である。

図７は、画像特徴の視線ベクトルとモデル特徴の位置と方向から定義される３次元直線との間の３次元空間中における残差の計算を説明する図である。

まず、焦点距離が１となる正規化画像座標系における画像エッジの座標を（ｕ’，ｖ’）としたとき、画像エッジの視線ベクトルｖ_ｉｍｇは、以下の式より算出できる。

そして、画像エッジの視線ベクトルｖ_ｉｍｇと、局所線特徴の３次元位置をｐ_ｅおよび方向をｄ_ｅから定義される３次元直線との間の最短距離を算出することで、画像エッジと局所線特徴の３次元空間中の残差ｅｒｒ_ｅｄｇｅを算出する。画像エッジの視線ベクトルと局所線特徴の位置と方向から定義される直線との間の最短距離となる方向ｎは、以下の式より算出できる。
ｎ＝ｄ_ｅ×ｖ_ｉｍｇ （式１３）
以上で求めた最短距離方向ｎと、局所線特徴の３次元位置をｐ_ｅおよび画像エッジの視線ベクトルｖ_ｉｍｇより、画像エッジと局所線特徴の３次元空間中の残差ｅｒｒ_ｅｄｇｅを以下の式により算出する。
ｅｒｒ_ｅｄｇｅ＝−ｎ・（ｖ_ｉｍｇ−ｐ_ｅ） （式１４）
こうして得られた生成した局所線特徴と画像エッジとの間の３次元空間内における残差ｅｒｒ_ｅｄｇｅに基づき、ステップＳ１５３０の以降の処理を第１の実施形態と同様に実行する
以上、画視点から画像特徴の位置へ伸ばした視線ベクトルと、モデル特徴の位置と方向から定義される３次元直線との最短距離に基づいて、画像特徴とモデル特徴との距離を算出する方法について説明した。

［第３の変形例］
以上説明した実施形態・変形例では、二次元画像上の特徴としてエッジを用いていた。しかしながら、二次元画像上の特徴はエッジに限るものではなく、その他の特徴であってもよい。例えば、対象物体の三次元形状モデルを三次元点群データによって表し、画像特徴として特徴点を検出して、特徴点と三次元の点の対応に基づいて位置姿勢を算出してもよい。また、一つの種類の特徴のみ用いるのではなく、複数種の特徴（特徴点とエッジ）を位置姿勢の算出に利用してもよい。

［第２の実施形態］
本発明による情報処理装置１の好適な適用事例としては、以下のような形態も考えられる。すなわち、２次元画像撮像装置２０および距離画像撮像装置３０により得られる２次元画像と距離画像を基に対象物体４０の位置姿勢を推定し、産業用ロボットアーム６０によりその把持・組み付け・分解などの操作を行う利用例があげられる。以下、図８を用いて本発明の一実施例である情報処理装置１の適用例を説明する。図８では、情報処理装置１とロボット６０を用いて対象物体４０を把持するロボットシステムの構成例を示す。

ロボット６０はロボットコントローラ５０により制御され、指令された位置に手先を移動させ物体の把持・組み付け・分解などを行うロボットである。対象物体４０は、作業台に置かれる位置が変わるため、現在の対象物体４０の位置姿勢を推定し、ロボットの把持制御を行う必要がある。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラ、距離画像撮像装置３０は物体表面の距離を計測する距離センサであり、産業用ロボットアームの手先等の対象物体４０を撮像できる位置に設置する。

情報処理装置１は、２次元画像撮像装置２０および距離画像撮像装置３０から得られる２次元画像と距離画像を基に対象物体４０の位置姿勢を推定する。情報処理装置１で推定された対象物体４０の位置姿勢は、ロボット６０に入力され、対象物体４０の把持などを行うようにロボットアームを制御する。なお、ロボットアーム６０による対象物体４０の操作は、以上に示す把持操作に限るものではなく、例えば，対象物体４０を別の物体に組み付ける組み付け動作を行っても良い。本発明の情報処理装置により、ロボットシステムは対象物体４０の位置が不定でも位置姿勢推定を行うことで、対象物体４０を把持・組み付け・分解などの操作を行うことが可能となる。

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

＜実施例の効果＞
第１の実施形態では、２次元画像とモデルとの距離として、２次元画像から得られる画像特徴を３次元空間中に逆投影することで得られる３次元平面とモデル特徴の３次元位置との間の距離を算出することで、３次元空間中の距離として統一的に計量して２次元画像と距離データを併用した位置姿勢推定を行う方法について述べた。従来手法のような概略位置姿勢の奥行き値に基づく残差尺度の変換処理を行わないため、高精度な位置姿勢推定を行うことが可能になる。また、画像エッジの残差の尺度変換処理自体必要なくなるため、従来の濃淡画像と距離データを併用した位置姿勢推定手法よりも高速な位置姿勢推定が可能となる。

第１の変形例では、画像エッジの３次元点を、視点から画像エッジの位置へ伸ばす直線上におけるモデル特徴の奥行き値と同じ位置に設定し、モデル特徴の３次元位置との距離を算出することで、統一的に残差を計量して２次元画像と距離データを併用した位置姿勢推定を行う方法について述べた。

３次元空間中において画像特徴とモデル特徴との残差を計量し、濃淡画像と距離データを同一の評価尺度で併用することで、高精度な位置姿勢推定を行うことが可能になる。また、画像特徴とモデル特徴との残差を計量において、画像特徴の方向を利用しないため、画像特徴の検出において、画像特徴の方向を検出する必要が無くなる。

第２の変形例では、画像特徴とモデル特徴との３次元空間中における残差計量方法として、視点から画像特徴の位置へ伸ばした視線ベクトルと、モデル特徴の位置と方向から定義される３次元直線との最短距離に基づいて、３次元空間中における画像特徴とモデル特徴との残差を計量する方法について説明した。

３次元空間中において画像特徴とモデル特徴との残差を計量し、濃淡画像と距離データを同一の評価尺度で併用することで、高精度な位置姿勢推定を行うことが可能になる。

第３の変形例では、二次元画像上の画像特徴として特徴点を検出し、対象物体の三次元形状モデルを三次元点群データによって表して、特徴点と三次元の点の対応に基づいて位置姿勢を算出する方法を示した。

特徴点として画像特徴を検出し、特徴点に基づいて位置姿勢を行うことで、２次元画像上で画像特徴としてエッジが得られない場合でも、２次元画像から特徴点が検出できる限り、位置姿勢推定を行うことが可能になる。

第２の実施形態では、本発明の情報処理装置により、対象物体の位置姿勢を推定し、推定結果に基づいてロボットシステムが対象物体の把持および移動を行う方法を示した。

対象物体の位置姿勢が不定でも位置姿勢推定を行うことで、ロボットシステムが対象物体を把持することが可能となる。

Claims (9)

1. 対象物体の形状を表すモデルを保持する保持手段と、
   撮像装置により撮像された前記対象物体の２次元画像と前記対象物体の表面上の３次元点群の情報とを取得する取得手段と、
   前記対象物体の概略の位置姿勢を取得する概略位置姿勢取得手段と、
   前記取得した２次元画像から画像エッジの位置と方向を検出する検出手段と、
   前記概略の位置姿勢に基づいて、前記検出された画像エッジと前記モデルを構成する局所線特徴とを対応付け、前記３次元点群の距離点と前記モデルを構成する局所面特徴とを対応づける対応付け手段と、
   前記撮像装置の視点位置及び該視点位置から見た前記画像エッジの位置を通り、該画像エッジの方向と平行な３次元平面と、３次元空間で前記概略位置姿勢に配置した前記モデルにおいて前記画像エッジと対応付けられた局所線特徴の３次元位置との距離に基づいて、当該画像エッジと対応付けられた局所線特徴との３次元空間における残差を第１の残差として取得する第１の取得手段と、
   前記対応づけられた距離点の３次元位置と３次元空間で前記概略位置姿勢に配置した前記モデルの局所面特徴との３次元空間における残差を第２の残差として取得する第２の取得手段と、
   ３次元空間における前記第１の残差及び第２の残差に基づいて、前記対象物体の位置姿勢を導出する位置姿勢導出手段とを備えることを特徴とする情報処理装置。
2. 前記対応付け手段は、前記概略位置姿勢に基づいて、前記２次元画像上に前記モデルを投影し、該投影されたモデルの局所線特徴に対応する画像エッジを探索することによって、前記投影されたモデルの局所線特徴と該モデルに対応する画像エッジとを対応づけることを特徴とする請求項１に記載の情報処理装置。
3. 前記３次元点群の情報は、前記対象物体を撮影した距離画像から取得することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記対応付け手段は、前記概略位置姿勢に基づいて、前記距離画像上に前記モデルを投影し、該投影されたモデルの局所面特徴に対応する距離点を探索することによって、前記投影されたモデルの局所面特徴と該モデルに対応する距離点とを対応づけることを特徴とする請求項３に記載の情報処理装置。
5. 前記位置姿勢導出手段は、前記取得した第１の残差と第２の残差とが小さくなるように、前記概略位置姿勢を更新することによって、前記対象物体の位置姿勢を導出することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記モデルはＣＡＤモデルであることを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 前記概略位置姿勢は、前記２次元画像または前記３次元点群の情報に対して、パターンマッチングを行うことにより取得されることを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
8. 前記位置姿勢導出手段による前記対象物体の位置姿勢の推定結果に基づいて、前記対象物体を操作する操作手段をさらに備えることを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. コンピュータを、請求項１乃至８のいずれか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images