JP2012026895A - 位置姿勢計測装置、位置姿勢計測方法、およびプログラム - Google Patents

Abstract

【課題】計測データに質の低いデータが含まれている場合に、高速・高精度に物体の位置姿勢を計測する。
【解決手段】対象物体の３次元モデルを保持する保持部と、対象物体の位置姿勢の概略値を入力する入力部と、対象物体の距離画像を取得する取得部と、距離画像に保持される情報を３次元座標系上の３次元点の位置情報に変換する変換部と、距離画像に含まれる計測誤差に起因する３次元点の位置情報の信頼度を算出する信頼度算出部と、信頼度が高い３次元点から順に、所定の数だけ位置姿勢計測に用いる３次元点を選択する選択部と、概略値に基づいて３次元モデルを平行回転移動することにより、３次元モデルを構成する面と選択部により選択された所定の数の３次元点のそれぞれとを対応付ける対応付け部と、対応付けられた所定の数の３次元点と３次元モデルを構成する面との３次元距離の二乗和が最小となる位置姿勢を、対象物体の位置姿勢として決定する決定部と、を備える。
【選択図】 図１

Description

本発明は、３次元形状情報が既知である位置姿勢計測装置、位置姿勢計測方法、およびプログラムに関する。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。このような位置及び姿勢の計測は、ロボットによる部品把持のためだけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間（現実物体）と仮想物体の位置合わせなど様々な目的に応用される。

位置及び姿勢を計測する方法として、カメラ等の撮像装置から得られる２次元画像や距離センサから得られる距離画像を用いて行う方法がある。非特許文献１では、２次元画像上の特徴であるエッジに対して、線分の集合で表される物体の３次元モデルの投影像を当てはめることにより、物体の位置及び姿勢を計測する方法が開示されている。

この方法では、既知の情報として与えられる概略の位置及び姿勢に基づいて、３次元モデル中の線分を２次元画像上に投影し、投影された線分上に離散的に配置される制御点の夫々に対応するエッジを２次元画像上で検出する。このようにして得られたモデル（制御点）とエッジとの対応に基づいて、制御点が属する線分の投影像と対応するエッジとの画像上での距離の二乗和が最小となるように概略の位置及び姿勢を補正することで最終的な位置及び姿勢が得られる。また、非特許文献２では、距離画像を変換して得られる物体表面上の３次元点群に物体の３次元モデル（ポリゴンモデル）を当てはめることにより物体の位置及び姿勢を計測する方法が開示されている。非特許文献１で開示される方法と同様に、この方法でも概略の位置及び姿勢が既知の情報として与えられることが前提となっている。概略の位置及び姿勢に基づいてポリゴンモデルを平行移動・回転し、点群中の各点ごとに最も近接するポリゴンと対応付ける。そして、対応付けられたポリゴンと計測点との３次元空間における３次元距離の二乗和が最小となるように概略の位置及び姿勢を補正することで最終的な位置及び姿勢が得られる。このような計測データにモデルを当てはめる方法では、計測データの数が多いほど個々の計測データに含まれる計測誤差の影響が小さくなるため、多数の計測データを利用することで計測精度が向上することが期待される。

T. Drummond and R. Cipolla, "Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002. D. A. Simon, M. Hebert, and T. Kanade, "Real-time 3-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation (ICRA’94), pp.2235-2241, 1994. N. Gelfand, L. Ikemoto, S. Rusinkiewicz, and M. Levoy, "Geometrically stable sampling for the ICP algorithm," Proc. 4th International Conference on 3-D Digital Imaging and Modeling (3DIM 2003), pp.260-267, 2003. W. Hoff and T. Vincent, "Analysis of head pose accuracy in augmented reality," IEEE Transactions on Visualization and Computer Graphics, vol.6, no.4, pp.319-334, 2000. G. Welch and G. Bishop, "SCAAT: incremental tracking with incomplete information," Proc. 24th annual conference on Computer graphics and interactive techniques (SIGGRAPH’97), pp.333-344, 1997.

一方、工業製品の組立といった作業をロボットが行うことにより、作業を高速化することが期待される。ロボットによる作業を高速化するためには、ロボットの動作を高速化するだけでなく、動作を決定するのに必要なロボットと作業対象との相対的な位置及び姿勢の計測も高速に行う必要がある。また、ロボットを利用することでより精密な作業を行うことが期待されるため、位置及び姿勢の計測の高速化だけでなく高精度化も要求される。上述の３次元モデルを当てはめる方法では、多数の計測データを利用することにより高精度に部品の位置及び姿勢を計測することが期待される。しかし、多数の計測データを利用すると計算時間が増加してしまうため、なるべく少ない計測データから高精度に位置及び姿勢を計測することが求められる。

多数の計測データから必要な数の計測データを抽出する最もシンプルな方法は、等間隔のサンプリングである。しかし、等間隔にサンプリングする場合、サンプリングされる計測データの組合せによっては位置及び姿勢を一意に決定できないことが起こりうる。これに対して、非特許文献３では、既にサンプリングされた計測データから、決定するための計測データが不足している自由度を判定し、その自由度の決定に必要な計測データを優先的にサンプリングする方法が開示されている。しかし、この方法では、計測データとモデルとの対応情報のみからサンプリングされる計測データが決定されており、計測誤差などの計測データ自体の質は考慮されていない。少ない計測データから位置及び姿勢の算出を行う場合には個々の計測データの影響が大きくなるため、質の低い計測データが含まれる場合には計測精度が低下したり、場合によっては計算が発散したりといった課題が生じる。

上記の課題に鑑み、本発明は計測データ自体の質に基づいて計測データをサンプリングすることで、高速・高精度に物体の位置及び姿勢を計測することを目的とする。

上記の目的を達成する本発明に係る位置姿勢計測装置は、
３次元形状情報が既知である対象物体の位置および姿勢を計測する位置姿勢計測装置であって、
前記３次元形状情報を表す３次元モデルを保持する保持手段と、
前記対象物体の位置および姿勢の概略値を入力する入力手段と、
撮像手段により撮像された前記対象物体の距離情報を取得する取得手段と、
前記距離情報を３次元座標系における３次元点の位置情報に変換する変換手段と、
前記距離情報に含まれる計測誤差に起因する前記位置情報に含まれる誤差の小ささを示す指標である、前記位置情報のそれぞれに対する信頼度を算出する信頼度算出手段と、
前記信頼度が高い前記３次元点から順に、所定の数だけ位置および姿勢の計測に用いる前記３次元点を選択する選択手段と、
前記入力手段に入力された前記概略値に基づいて前記３次元モデルを平行移動または回転移動することにより、前記３次元モデルを構成する面と、前記選択手段により選択された前記所定の数の３次元点のそれぞれと、を対応付ける対応付け手段と、
前記対応付け手段により対応付けられた前記所定の数の３次元点と、前記３次元モデルを構成する面との３次元距離の二乗和が最小となる位置および姿勢を、前記対象物体の位置および姿勢として決定する決定手段と、
を備えることを特徴とする。

本発明によれば、計測データに質の低い計測データが含まれている場合であっても、少ない計測データから高速・高精度に物体の位置及び姿勢を計測することが可能になる。

（ａ）第１実施形態に係る位置姿勢計測装置１００のハードウェア構成を示す図、（ｂ）第１実施形態に係る位置姿勢計測装置１００の各処理部の構成を示す図。 第１実施形態に係る３次元形状モデルを説明する図。 第１実施形態に係る位置及び姿勢の計測の処理手順を示すフローチャート。 第１実施形態に係る計測データ選択の処理手順を示すフローチャート。 第２実施形態に係る位置姿勢計測装置の構成を示す図。 第２実施形態に係る位置及び姿勢の計測の処理手順を示すフローチャート。 第２実施形態に係る位置及び姿勢の更新の処理手順を示すフローチャート。

（第１実施形態）
本実施形態は、３次元形状情報が３次元モデルとして既知である対象物体に対して、当該対象物体の位置および姿勢を計測する位置姿勢計測装置を対象とする。第１実施形態では、距離センサが計測する距離画像を変換して得られる３次元座標系における３次元点群に、物体の３次元モデルを当てはめて位置及び姿勢の計測を行う際に、本実施形態に係る計測データのサンプリング方法を適用する場合について説明する。なお、必要なデータをすべて選択してから位置姿勢の計測を行う。

まず、図１（ａ）を参照して、本実施形態に係る位置姿勢計測装置のハードウェア構成を説明する。ＣＰＵ１１は装置全体の動作、具体的には後述の各処理部の動作を制御する。メモリ１２は、ＣＰＵ１１の動作に用いるプログラムやデータを格納する。バス１３は各構成モジュール間のデータ転送を司る。インタフェース１４はバス１３と各種装置とのインタフェースである。また、外部記憶装置１５は、ＣＰＵ１１に読み込むためのプログラムやデータを格納する外部記憶装置である。キーボード１６およびマウス１７はプログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成する。表示部１８はプロセスの動作結果を表示する。データ入出力部１９は装置外部とのデータ入出力を行う。不図示の距離画像計測装置はデータ入出力部１９を介して接続されている。

次に、図１（ｂ）を参照して、本実施形態における位置姿勢計測装置１００の構成について説明する。図１（ｂ）に示されるように、位置姿勢計測装置１００は、３次元モデル保存部１０１と、概略位置姿勢入力部１０２と、計測データ入力部１０３と、計測データ選択部１０４と、位置姿勢算出部１０５とを備える。また、位置姿勢計測装置１００には、３次元データ計測部１０６が接続されている。以下、位置姿勢計測装置１００を構成する各部の機能について説明する。

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

３次元モデル保存部１０１は、位置及び姿勢を計測する対象である計測対象物体の３次元モデルを保存する。本実施形態では、物体を線分と面から構成される３次元モデルとして記述する。図２は本実施形態に係る３次元モデルを説明する図である。３次元モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示されるように、計測対象物体２０の３次元モデルは点Ｐ１〜点Ｐ１４の合計１４点により構成されている。また、図２（ｂ）に示されるように、計測対象物体２０の３次元モデルは線分Ｌ１〜Ｌ１６により構成されている。ここで、図２（ｃ）に示されるように、点Ｐ１〜点Ｐ１４は夫々３次元座標を持つ。また、図２（ｄ）に示されるように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。例えば、線分Ｌ１は、点のＩＤである点Ｐ１と点Ｐ６によって表される。さらに、３次元モデルは、面の情報を保持する。各面は、各面を構成する点のＩＤによって表される。図２に示される３次元モデルでは、直方体を構成する６つの面の情報が記憶されている。３次元モデルは、計測データ選択部１０４において計測データをサンプリングする際、及び位置姿勢算出部１０５において物体の位置及び姿勢を算出する際に用いられる。

概略位置姿勢入力部１０２は、位置姿勢計測装置１００に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置１００には、位置及び姿勢計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。位置姿勢計測装置１００に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、距離センサを構成するカメラの光軸をz軸とした座標系を基準座標系とする。また本実施形態では、位置姿勢計測装置１００は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果に基づいて位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢の概略値を予測してもよい。また、物体が置かれているおおよその位置や姿勢が予めわかっている場合にはその値を概略値として用いる。

計測データ入力部１０３は、距離画像の各画素に格納されている奥行き値を基準座標系（３次元座標系）における３次元座標に変換し、３次元点群の各位置情報として位置姿勢計測装置１００に入力する。

計測データ選択部１０４は、計測データ入力部１０３によって入力された３次元点群から、計測データの質または位置及び姿勢算出への寄与度に基づいて、必要な計測データをサンプリングする。

位置姿勢算出部１０５は、計測データ選択部１０４によって選択された計測データ（３次元点群）に対して、３次元モデル保存部１０１に保存される３次元モデルを当てはめることにより物体の位置及び姿勢を計測する。

以上説明した各処理部の動作について説明する。図３は、第１実施形態に係る位置及び姿勢の計測の処理手順を示すフローチャートである。ステップＳ３０１において、概略位置姿勢入力部１０２を介して、位置姿勢計測装置１００に対する物体の位置及び姿勢の概略値が位置姿勢計測装置１００に入力される。前述したように、本実施形態では、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。

ステップＳ３０２において、計測データ入力部１０３を介して、物体の位置及び姿勢を算出するための計測データが取得される。ここで計測データとは、計測対象物体の３次元データである。前述したように、３次元データ計測部１０６は距離画像を出力する。従って、計測データ入力部１０３は、距離画像の各画素に保持される奥行き情報を、基準座標系における３次元座標を持つ３次元点群データに変換して位置姿勢計測装置１００に入力する。距離画像から３次元点群への変換は、例えば、画素位置に対応する視線ベクトルに奥行き値を乗算することにより行う。

ステップＳ３０３において、計測データ入力部１０３を介して入力された３次元点群データから、位置及び姿勢を算出するのに必要な計測データを選択する。この必要な計測データの選択処理の詳細について、図４を参照して説明する。図４は、計測データ選択の処理手順を示すフローチャートである。

まず、ステップＳ４０１において、計測データ選択部１０４は、各計測データについて、計測データの質（信頼性）に相当する信頼度を算出する（信頼度算出処理）。信頼度とは、距離画像に含まれる計測誤差に起因する、位置情報に含まれる誤差の小ささを示す指標である。本実施形態では、計測点が局所的に平面である場合、平面の法線ベクトルが距離センサに正対する場合に安定に計測ができると仮定して、位置姿勢計測装置に対する平面の法線ベクトルの角度に基づいて信頼度を決定する。ここでは、カメラ（撮像装置）の光軸である基準座標系のz軸（撮像方向ベクトル）と平面の法線ベクトルとに基づいて信頼度が算出される。計測点周辺の平面の法線ベクトルは、距離画像における近傍の点（周辺点）に平面当てはめを行うことにより推定する。平面当てはめによって得られた法線ベクトルと基準座標系のz軸との双方を正規化し、それらの内積の絶対値を信頼度とする。しかしながら、信頼度の決定方法はこれに限るものではない。例えば、ステレオカメラを用いたレンジセンサの場合には、画像間でのパッチのマッチング度合いを表す数値（例えば、輝度差の２乗和を示すＳＳＤ）に基づいて信頼度が決定されてもよい。その他、いかなるものであっても、計測データの質を妥当に表現できる指標であれば信頼度として使用されてもよい。

ステップＳ４０２において、計測データ選択部１０４は、ステップＳ４０１で算出された信頼度の降順に計測データをソートする。各計測データは既に選択されたかどうかのフラグＦ_ｉ（ｉ＝１，…，Ｎ_Ｄ）を保持している。このフラグＦはＦＡＬＳＥ（未選択であることを示す）により初期化しておく。ここでＮ_Ｄは計測データの総数である。

ステップＳ４０３において、計測データ選択部１０４は、ステップＳ４０２でソートされた計測データから、信頼度が高い所定の数である上位Ｍ個の計測データを選択する。そして、選択された計測データのフラグＦをＴＲＵＥ（選択されたことを示す）に変更する。ここでＭの値は、計測対象物体の位置及び姿勢を決定するのに必要な最小の計測データの数であるとする。例えば、点と面の対応に基づいて位置及び姿勢を算出する場合には、Ｍは６となる。選択されたＭ個の計測データの夫々を、３次元モデル中の面と対応づける。この計測データと３次元モデル中の面との対応付けは、ステップＳ３０１で入力された概略の位置及び姿勢に基づいて３次元モデルを平行移動・回転移動し、計測データに最も近接する３次元モデル中の面を選択することにより行う。

ステップＳ４０４において、計測データ選択部１０４は、ステップＳ４０３で選択されたＭ個の計測データにより、一意に位置及び姿勢を決定できるかどうかの判定を行う。すなわち、決定されるべき位置及び姿勢のうち、選択されたＭ個の計測データからでは決定できない位置及び姿勢の成分（もしくはその線形和）を判定する。ここでは、物体の位置及び姿勢の補正値を算出する際に用いる方程式の係数ベクトルを用いて判定を行う。方程式の係数ベクトルは、点と面の符号付距離の位置及び姿勢に関する偏微分係数を成分として並べたものである。以下、判定方法について具体的に説明する。説明を簡単にするため、位置及び姿勢を、基準座標系に対する物体の位置及び姿勢ではなく、物体座標系に対するカメラの位置及び姿勢であるとする。基準座標系における点群の３次元座標は、前述の位置及び姿勢s（位置姿勢を表す６次元ベクトル）を用いて物体座標系における３次元座標（x，y，z）に変換される。ある位置及び姿勢ｓ_０により、基準座標系における３次元座標が物体座標系における３次元座標（x_０，y_０，z_０）に変換される。（x，y，z）は位置及び姿勢の関数であり、（x_０，y_０，z_０）の近傍で１次のテイラー展開によって以下の式（１）のように近似できる。

ここで、Δｓ_ｉ（ｉ＝１，２，・・・，６）は位置及び姿勢の各成分の微小変化を表す。ある点に対応付けされた面の物体座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ^２＋ｂ^２＋ｃ^２＝１；ａ，ｂ，ｃ，ｅは定数）とする。概略の位置及び姿勢を微小変化させることにより、物体座標系を基準とした計測データ（点）の３次元座標（ｘ，ｙ，ｚ）が平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅを満たすと仮定すると、式（１）と平面の方程式とから以下の式（２）が得られる。

ただし、ｑ＝ａｘ_０＋ｂｙ_０＋ｃｚ_０（定数）とした。式（２）は位置及び姿勢の各成分の微小変化についての方程式であり、選択されたＭ個の点に基づいて以下の式（３）のようなΔｓ_ｉに関する線形連立方程式を立てることができる。

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

非特許文献３に開示されているように、Ｍ個の計測データから位置及び姿勢を一意に決定できるかどうかは、行列Ｊ^ＴＪのランクによって決定される。行列Ｊ^ＴＪは、３次元点と３次元モデルを構成する面との位置関係を規定する係数行列の転置と当該係数行列との積で表される行列である。行列Ｊ^ＴＪがランク落ちしている場合には、行列Ｊ^ＴＪの固有値０の固有ベクトルに相当する位置及び姿勢の成分を一意に決定することができない。そこで、ステップＳ４０４においては、行列Ｊ^ＴＪを固有値分解して６個の固有ベクトルＥＶｉ（ｉ＝１，…，６）と、それに対応する固有値を算出する。そして、各固有ベクトルに相当する位置及び姿勢成分を決定するための情報量Ｅｉ（ｉ＝１，…，６）を保持する。本実施形態では、情報量Ｅｉの値がある閾値Ｔｈより大きい場合には一意に位置及び姿勢が決定できるものとする。情報量Ｅｉとして、非特許文献３と同様に、固有ベクトルＥＶｉとＭ個の各計測データの係数ベクトル（式（３）の左辺の行列の行に相当する）との内積を二乗した値の総和を保持する。固有値がほぼ０の固有ベクトルの場合には、ＥＶｉはほぼ０になる。

すなわち、この行列Ｊ^ＴＪを固有値分解して取得される固有ベクトルおよび固有値に基づいて、固有ベクトルに対応する対象物体の位置および姿勢を決定することができるか否かを示す特徴量（情報量Ｅｉ（ｉ＝１，…，６））を算出する特徴量算出処理を行う。

次に、ステップＳ４０５において、計測データ選択部１０４は、位置及び姿勢を一意に決定するためにさらに計測データを追加選択する必要があるかどうかを判定する。この判定は、前述の情報量Ｅｉが閾値Ｔｈより大きいかどうかを判定することにより行う。閾値Ｔｈ以下のＥｉがある場合には（ステップＳ４０５；ＹＥＳ）、情報不足の成分があるため、ステップＳ４０６に進む。ステップＳ４０６において、計測データ選択部１０４は新たな計測データを追加選択した後ステップＳ４０５に戻る。ステップＳ４０６の具体的な処理については後述する。ステップＳ４０５において、すべてのＥｉが閾値Ｔｈより大きいと判定された場合には（ステップＳ４０５；ＮＯ）、情報不足の成分はないため、ステップＳ３０３のデータ選択処理を終了し、ステップＳ３０４へ進む。なお、計算時間に余裕がある場合には、ステップＳ４０５における閾値Ｔｈを大きくし、ステップＳ４０６のデータ追加処理を実行する回数を多くする。これにより多くの計測データを利用して高精度化を図ることができる。一方、計算時間に余裕がない場合には閾値Ｔｈを小さくして、ステップＳ４０６のデータ追加処理を実行する回数をなるべく少なくすることで高速化する。

ステップＳ４０６において、計測データ選択部１０４は、最もＥｉが小さい固有ベクトルに近い係数ベクトルを持つ計測データを選択する。すなわち、特徴量（情報量Ｅｉ（ｉ＝１，…，６））が閾値以下であるか否かを判定し、その特徴量が閾値以下である場合に、既に選択された所定の数の３次元点では対象物体の位置および姿勢を決定できないとして、新たに３次元点を追加選択する処理を行う。具体的には、ソートされた計測データからフラグＦがＦＡＬＳＥである計測データを選択し、最も近接する面との対応付けを行い、固有ベクトルと係数ベクトルとの内積の絶対値が閾値以上（≧Ｔｈ＿ｄｏｔ）であるか否か判断し、閾値以上（≧Ｔｈ＿ｄｏｔ）であれば選択する。一方、閾値未満（＜Ｔｈ＿ｄｏｔ）の場合には計測データを選択せずに、再びステップＳ４０５に戻る。計測データを選択した場合には、各固有ベクトルＥＶｉに対して情報量Ｅｉを算出し直す。すなわち、選択された計測データの係数ベクトルと各固有ベクトルとの内積の二乗値をＥＶｉに加算する。

以上でステップＳ３０３の処理が終了する。

ステップＳ３０４において、位置姿勢算出部１０５は、ステップＳ３０３で選択された計測データを用いて、物体の位置及び姿勢を算出する。ここでは、対応付けられたモデル中の面と計測点（所定の数の３次元点およびさらに追加された３次元点）との３次元空間における３次元距離の二乗和が最小となるようにステップＳ３０１で入力された概略の位置及び姿勢を繰り返し補正する。具体的には、選択された各計測データについて立式される式（２）を、位置及び姿勢の補正値Δｓ_ｉ（ｉ＝１，２，…，６）に関する連立方程式としてΔｓ_ｉを算出する。算出されたΔｓ_ｉをもとに位置及び姿勢ｓを補正する。なお、位置及び姿勢による３次元位置の変換は非線形の変換であるため、ｓを補正した後に式（２）の右辺の係数や左辺の値を算出し直して、Δｓ_ｉの算出と補正を繰り返し行う。

以上述べたように、本実施形態では、計測データの質に基づいて計測データを選択し、位置及び姿勢算出を行うことで、高精度かつ高速な位置及び姿勢の計測を行うことができる。

（第２実施形態）
第１実施形態では、まず計測データを選択した後に位置及び姿勢の算出を行った。これに対して第２実施形態では、計測データを選択するごとに位置及び姿勢を逐次的に更新する場合について説明する。

まず図５を参照して、本実施形態に係る位置姿勢計測装置５００の構成を説明する。図５に示されるように、位置姿勢計測装置５００は、３次元モデル保存部５０１と、概略位置姿勢入力部５０２と、計測データ入力部５０３と、計測データ選択部５０４と、位置姿勢更新部５０５とを備える。また、位置姿勢計測装置５００には、３次元データ計測部５０６が接続されている。以下、位置姿勢計測装置５００を構成する各部について説明する。３次元モデル保存部５０１、概略位置姿勢入力部５０２、計測データ入力部５０３、３次元データ計測部５０６は、第１実施形態における３次元モデル保存部１０１、概略位置姿勢入力部１０２、計測データ入力部１０３、3次元データ計測部１０６と同一であるため説明を省略する。

位置姿勢更新部５０５は、計測データ入力部５０３を介して入力された計測データ（３次元点群）から、計測データを抽出して該計測データに基づいて位置及び姿勢を更新することを繰り返し行う。

図６は、第２実施形態に係る位置及び姿勢の計測の処理手順を示すフローチャートである。ステップＳ６０１の処理は、第１実施形態におけるステップＳ３０１の処理と同じである。ステップＳ６０２の処理は、第１実施形態におけるステップＳ３０２の処理と同じである。次にステップＳ６０３において、位置姿勢更新部５０５は、計測データの質と位置及び姿勢計測への寄与度に基づいて、計測データ入力部５０３を介して入力された計測データから計測データを選択して、位置及び姿勢を更新する処理を繰り返すことにより位置及び姿勢を算出する。

図７を参照して、このステップＳ６０３における位置及び姿勢の更新の具体的な処理手順について説明する。

ステップＳ７０１の処理は、第１実施形態におけるステップＳ４０１の処理と同じである。加えて、位置姿勢更新部５０５は、概略の位置及び姿勢のあいまい性（概略値信頼度）を表す指標となる６×６共分散行列Ｐを入力する（概略値信頼度入力処理）。共分散行列とは、対角要素に位置及び姿勢の各成分の分散を持ち、非対角成分に成分間の共分散を持つ行列である。本実施形態では、概略の位置及び姿勢の共分散行列は固定値として不図示の入力部を介して与えられるものであり、６行６列の行列として与えられる。

ステップＳ７０２の処理も、第１実施形態におけるステップＳ４０２の処理と同じである。ステップＳ７０３において、計測データ選択部５０４は、現在の位置及び姿勢の共分散行列Ｐに基づいて、最もあいまい性の高い位置及び姿勢の成分を推定し、その成分を更新するのに適した計測データを選択する。

まず、最もあいまい性の高い位置及び姿勢の成分を推定するために、共分散行列Ｐの逆行列を固有値分解し、６個の固有値とそれに対応する固有ベクトルＥＶｉ（ｉ＝１，…，６）を算出する（固有ベクトル算出処理）。線形近似の範囲内では、共分散行列Ｐの逆行列は第１実施形態における行列Ｊ^ＴＪに相当する（非特許文献４参照）。

次に計測データ選択部５０４は、最もあいまい性の高い位置及び姿勢の成分として、最も固有値が小さい固有ベクトルＥＶｉを選択し、その固有ベクトルに最も近い係数ベクトルを持つ計測データを選択する。ここで、係数ベクトルとは、点と面の符号付距離の位置及び姿勢についての偏微分係数を並べたものであり、式（３）の左辺の行列の行に相当する。具体的には、ステップＳ７０２でソートされた計測データからフラグＦがＦＡＬＳＥである計測データを選択し、最も近接する面との対応付けを行う。選択された計測データのフラグＦをＴＲＵＥに変更する。得られた対応情報に基づいて、点と面の符号付距離の位置及び姿勢についての偏微分係数を算出して係数ベクトルを得る（係数ベクトル算出処理）。そして固有ベクトルと係数ベクトルとの内積の絶対値が閾値Ｔｈ＿ｄｏｔ以上であればステップＳ７０４において位置及び姿勢の更新を行う。

ステップＳ７０４において、位置姿勢更新部５０５は、ステップＳ７０３で選択された一つの計測データを用いて、拡張カルマンフィルタの原理を応用したＳＣＡＡＴ（Ｓｉｎｇｌｅ Ｃｏｎｓｔｒａｉｎｔ Ａｔ Ａ Ｔｉｍｅ）アルゴリズム（非特許文献５参照）により、概略の位置及び姿勢の更新と、位置及び姿勢の共分散行列の更新とを行う。通常の拡張カルマンフィルタは、位置及び姿勢のすべての成分が決定できる数の計測データが必要となるが、ＳＣＡＡＴアルゴリズムでは単一の計測データから位置及び姿勢を部分的に更新することができる。

上記更新の後、ステップＳ７０５において、位置及び姿勢の更新を継続するかどうかの判定を行う。ステップＳ７０４で更新された共分散行列の各要素の大きさが一定値以下になった場合や、予め定められた計算時間に到達した場合に、更新処理を終了する（ステップＳ７０５；ＮＯ）。一方、そうでない場合（ステップＳ７０５；ＹＥＳ）、ステップＳ７０３に戻る。

以上述べたように、本実施形態では、計測データの質と位置及び姿勢計測の寄与度との双方に基づいて計測データを選択し、位置及び姿勢を更新する処理を繰り返すことにより、高精度かつ高速な位置及び姿勢の計測を行うことができる。

（第３実施形態）
前述の実施形態では、計測点の周辺が局所的に平面であると想定し、該平面とカメラが正対するときに安定して計測が行えるとして、平面の法線ベクトルと視線ベクトルの角度をもとに計測データの質を算出していた。しかしながら、計測データの質の算出方法はこれに限るものではない。例えば、計測点近傍の３次元座標が類似していれば計測の品質が高いとして、計測点の距離画像上における周辺点の３次元位置のばらつきに基づいて、計測データの質を算出してもよい。具体的には、距離画像における計測点の近傍の複数の点の３次元座標を算出し、３次元座標の共分散行列を求め、共分散行列を固有値分解したときの最大固有値を計測データの質としてもよい。

（第４実施形態）
前述の実施形態では、距離センサが計測する距離画像を変換して得られる３次元点群に物体の３次元モデルを当てはめて位置及び姿勢の計測を行う場合について説明した。しかしながら、本発明を適用可能な計測データは距離画像（３次元点群）に限るものではなく、例えば２次元画像上で検出される画像特徴であってもよい。画像特徴としては、特徴点やエッジを用いることができる。エッジを用いる場合には、３次元空間中における面と点の符号付距離ではなく、３次元画像上における線と点の符号付距離を位置及び姿勢の関数としたときの偏微分係数を係数行列とする。また、特徴点を用いる場合には、２次元画像上における座標の違いを位置及び姿勢の関数としたときの偏微分係数を係数行列とする。

エッジを用いる場合には、計測データの質として、２次元画像上で周辺に存在するエッジの数を利用することができる。すなわち、あるモデル中の線分に対応付けられるエッジの近辺に多数のエッジがある場合には誤対応である可能性が高いため、計測データの質を低く設定する。また、エッジ検出カーネルのサイズを計測データの質として利用してもよい。すなわち、２次元画像上において異なるサイズのエッジ検出カーネルによってエッジ検出を行い、対応付けされたエッジを検出したときのカーネルのサイズが大きいほどエッジ位置の精度が低いため、計測データの質を低く設定する。

一方、特徴点を用いる場合には、計測データの質として、特徴点をマッチングする際のマッチング度を利用してもよい。３次元モデルが点群によって表されており、各点が周辺の輝度情報をパッチとして保持する場合には、画像上の特徴点周辺のパッチとモデル上のパッチの輝度値の差の２乗和を示すＳＳＤに基づいて計測データの質を決定する。すなわち、輝度値のマッチング度が低い場合には計測データの質を低く設定する。

しかしながら、計測データの質の算出方法はこれに限るものではなく、計測データの質を妥当に表現できる指標であればいかなる指標であってもよい。

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

Claims (6)

1. ３次元形状情報が既知である対象物体の位置および姿勢を計測する位置姿勢計測装置であって、
   前記３次元形状情報を表す３次元モデルを保持する保持手段と、
   前記対象物体の位置および姿勢の概略値を入力する入力手段と、
   撮像手段により撮像された前記対象物体の距離情報を取得する取得手段と、
   前記距離情報を３次元座標系における３次元点の位置情報に変換する変換手段と、
   前記距離情報に含まれる計測誤差に起因する前記位置情報に含まれる誤差の小ささを示す指標である、前記位置情報のそれぞれに対する信頼度を算出する信頼度算出手段と、
   前記信頼度が高い前記３次元点から順に、所定の数だけ位置および姿勢の計測に用いる前記３次元点を選択する選択手段と、
   前記入力手段に入力された前記概略値に基づいて前記３次元モデルを平行移動または回転移動することにより、前記３次元モデルを構成する面と、前記選択手段により選択された前記所定の数の３次元点のそれぞれと、を対応付ける対応付け手段と、
   前記対応付け手段により対応付けられた前記所定の数の３次元点と、前記３次元モデルを構成する面との３次元距離の二乗和が最小となる位置および姿勢を、前記対象物体の位置および姿勢として決定する決定手段と、
   を備えることを特徴とする位置姿勢計測装置。
2. 前記信頼度算出手段は、前記３次元点のそれぞれについて、前記３次元点の周辺点から構成される平面に対する、前記３次元点を基準とした正規化された法線ベクトルと、前記撮像手段を基準とした正規化された撮像方向ベクトルとの内積の絶対値を信頼度として算出することを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記３次元点と前記３次元モデルを構成する面との位置関係を規定する係数行列の転置と、当該係数行列と、の積で表される行列を固有値分解して取得される固有ベクトルおよび固有値に基づいて、前記固有ベクトルに対応する前記対象物体の位置および姿勢を前記決定手段により決定することができるか否かを示す特徴量を算出する特徴量算出手段と、
   前記特徴量が閾値以下であるか否かを判定する判定手段と、
   前記判定手段により前記特徴量が閾値以下であると判定された場合に、新たに３次元点を追加選択する追加選択手段と、
   を更に備え、
   前記対応付け手段は、前記入力手段に入力された前記概略値に基づいて前記３次元モデルを平行移動または回転移動することにより、前記３次元モデルを構成する面と、前記追加選択手段により選択された３次元点と、をさらに対応付け、
   前記決定手段は、前記対応付け手段により対応付けられた前記所定の数の３次元点および前記対応付け手段によりさらに対応付けられた３次元点と、前記３次元モデルを構成する面との３次元距離の二乗和が最小となる位置および姿勢を、前記対象物体の位置および姿勢として決定することを特徴とする請求項１または２に記載の位置姿勢計測装置。
4. 前記対象物体の位置および姿勢の概略値の信頼性を示す概略値信頼度を６行６列の行列として入力する概略値信頼度入力手段と、
   前記行列の逆行列を固有値分解して６個の固有値および当該固有値に対応する６個の固有ベクトルを算出する固有ベクトル算出手段と、
   前記６個の固有値および当該固有値に対応する６個の固有ベクトルのうち、前記固有値が最も小さい固有ベクトルに対応する、３次元点と前記３次元モデルを構成する面との位置関係を規定する係数ベクトルを算出する係数ベクトル算出手段と、
   前記固有ベクトルと前記係数ベクトルとの内積の絶対値が閾値以上であるか否か判断する判断手段と、
   前記判断手段により前記内積の絶対値が閾値以上であると判断された場合に、前記位置および姿勢の概略値を更新する更新手段と、
   をさらに備えることを特徴とする請求項１乃至３の何れか１項に記載の位置姿勢計測装置。
5. ３次元形状情報が既知である対象物体に対して、当該対象物体の位置および姿勢を計測する位置姿勢計測方法であって、
   入力手段が、前記対象物体の位置および姿勢の概略値を入力する入力工程と、
   取得手段が、撮像工程で撮像された前記対象物体の距離情報を取得する取得工程と、
   変換手段が、前記距離情報を３次元座標系における３次元点の位置情報に変換する変換工程と、
   信頼度算出手段が、前記距離情報に含まれる計測誤差に起因する前記位置情報に含まれる誤差の小ささを示す指標である、前記位置情報のそれぞれに対する信頼度を算出する信頼度算出工程と、
   選択手段が、前記信頼度が高い前記３次元点から順に、所定の数だけ位置および姿勢の計測に用いる前記３次元点を選択する選択工程と、
   対応付け手段が、前記入力工程で入力された前記概略値に基づいて、保持手段に保持された前記３次元形状情報を表す３次元モデルを平行移動または回転移動することにより、前記３次元モデルを構成する面と、前記選択工程で選択された前記所定の数の３次元点のそれぞれと、を対応付ける対応付け工程と、
   決定手段が、前記対応付け工程で対応付けられた前記所定の数の３次元点と、前記３次元モデルを構成する面との３次元距離の二乗和が最小となる位置および姿勢を、前記対象物体の位置および姿勢として決定する決定工程と、
   を有することを特徴とする位置姿勢計測方法。
6. 請求項５に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。

Images