JP2012141962A - 位置姿勢計測装置及び位置姿勢計測方法 - 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. 佐藤，井口，"空間コードによる距離画像入力"，電子情報通信学会論文誌，vol.J68-D，no.3，pp.369-375，1985． Z. Zhang, "A flexible new technique for camera calibration", IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.22, No.11, pages 1330-1334, 2000. 大石，中澤，池内，"インデックス画像を用いた複数距離画像の高速同時位置合せ"，vol.J89-D，no.3，pp.513-521，2006． Y. Liu, T. S. Huang, and O. D. Faugeras, "Determination of camera location from 2-D to 3-D line and point correspondences," Proc. CVPR'88, pp.82-88, 1988. A. Johnson and M. Hebert, "Using spin images for efficient object recognition in cluttered 3D scenes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.21, no.5, pp.433-449, 1999. C. Harris and M. Stephens, "A combined corner and edge detector" Proc. the 4th Alvey Vision Conference, pp.147-151, 1988. M. Ulrich, C. Wiedemann, and C. Steger, "CAD-based recognition of 3D objects in monocular images" Proc. 2009 IEEE International Conference on Robotics and Automation (ICRA'09), pp.1191-1198, 2009.

工業製品の生産過程では時間に対する制約が厳しいため、その一工程である部品の位置及び姿勢の計測はできる限り高速に行われる必要がある。また、ロボットの自己位置推定など、位置及び姿勢の計測を高速に処理しなければならない場面は他にも多い。前述した濃淡画像・距離画像に対する３次元形状モデルの当てはめによる位置及び姿勢の推定は、（１）モデルと画像の対応付け、（２）対応付け結果に基づく位置及び姿勢の算出、の２つのステップからなる。高精度な位置及び姿勢を推定するため、これらのステップは複数回繰り返されるのが一般的である。これら２つのステップのうち、モデルと画像の対応付け処理にかかる計算時間が問題になることが多い。

上記非特許文献１に開示される方法では、位置及び姿勢の初期値に基づいて３次元形状モデル中の各線分を画像上に投影し、画像上の投影像の近傍で対応するエッジを探索することにより、モデルと画像の対応付け処理を高速化している。一方、距離画像と３次元形状モデルの対応付けでは、３次元空間において最も近傍にある点を探索する必要がある。

上記非特許文献２では、ｋｄ−ｔｒｅｅを用いて最近傍点の探索を行っている。しかしこの方法では、オーダーがＯ（ＮｌｏｇＭ）の計算が必要になる（Ｎはデータ側の点数、Ｍはモデル側の点数）ため、各画素が計測点となる距離画像のように計測点数が非常に多いデータを利用する場合に時間がかかる。これに対し、従来では、上記非特許文献１の方法と同様に、画像上にモデルを投影することにより対応付けを高速化する方法が提案されている。具体的には、まず位置及び姿勢の初期値に基づいて３次元形状モデル内の幾何特徴の画像への投影計算を行い、幾何特徴の画像上での位置を算出する。次に、この幾何特徴の画像上での位置をもとに、幾何特徴と距離画像の画素とを対応付けすることにより、高速な対応付けを実現している。

しかしながら、前述の３次元形状モデルの画像上への投影に基づいた画像と３次元形状モデルの対応付けでは、３次元形状モデルの投影像と実際の物体の像とが十分にオーバーラップしていることが前提になっている。そのため、投影に用いる位置及び姿勢の初期値と実際の位置及び姿勢とのずれが大きい場合には誤対応が多く生じ、後段の位置及び姿勢の算出に失敗する。濃淡画像のエッジの対応付けでは明示的な特徴（エッジ）を用いているため、初期値のずれに対して比較的ロバストである。

一方、距離画像の対応付けでは、濃淡画像上のエッジのような明示的な特徴を用いていないため、初期値のずれに対してロバスト性が低く、位置及び姿勢の算出に失敗する要因となる。逆に初期値のずれが小さく距離画像とモデルが正しく対応付けできる場合には、距離画像の利用によって高精度な位置及び姿勢の計測が可能になる。このように、初期値のずれに対するロバスト性は、濃淡画像と距離画像で夫々特性が異なる。言い換えると、従来においては、この特性を考慮することまでは行っておらず、良い特性を持つ計測データを段階的・選択的に使うことによる初期値ずれに対するロバスト化に改善の余地があった。

本発明は、このような問題点に鑑みてなされたものであり、初期値のずれ具合の段階に応じてよい特性を持つ計測データを利用することにより、画像と３次元形状モデルの誤対応を減らし、高速でロバストな物体の位置及び姿勢の計測を実現することを目的とする。

本発明の位置姿勢計測装置は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の３次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の距離画像を入力する距離画像入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢を用いて算出される前記３次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出手段とを有する。
本発明の位置姿勢計測装置における他の態様は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の３次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の表面上における点群の３次元座標を入力する３次元データ入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢をもとに、前記３次元データ入力手段から得られる３次元座標の点群である３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出手段とを有する。
また、本発明は、上述した位置姿勢計測装置による位置姿勢計測方法を含む。

本発明によれば、濃淡画像と距離画像を段階的・選択的に用いることにより、位置及び姿勢の初期値と実際の位置及び姿勢とのずれに対してロバストな物体の位置及び姿勢の計測が可能になる。

本発明の第１の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。 本発明の第１の実施形態を示し、３次元形状モデルを説明する模式図である。 本発明の第１の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。 図３のステップＳ１０３０における位置姿勢計測（位置姿勢算出）の処理手順の一例を示すフローチャートである。 本発明の第１の実施形態を示し、エッジ検出を説明する模式図である。 本発明の第１の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。 図３のステップＳ１０４０における位置姿勢算出の処理手順の一例を示すフローチャートである。 本発明の第１の実施形態を示し、図７のステップＳ１２２０における距離画像と３次元形状モデルとの対応付け処理を説明する模式図である。 本発明の第２の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。 本発明の第３の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。 図１０のステップＳ３０４０における位置姿勢算出の処理手順の一例を示すフローチャートである。

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

（第１の実施形態）
第１の実施形態では、まず濃淡画像の情報を用いて物体の位置及び姿勢を推定した後、濃淡画像上の特徴と距離画像に対して同時にモデルが当てはまるように位置及び姿勢の推定を行う。本実施形態では、位置及び姿勢の初期値は与えられるものとする。実際の位置及び姿勢と初期値とのずれが大きい場合、モデルを投影することによる距離画像（３次元点群）と３次元形状モデルの対応付けは、誤対応が頻発する。第１の実施形態では、距離画像の対応付けを行う前に、画像特徴を利用した位置及び姿勢の推定を行ってモデルの投影像と実際の物体像との画像上のオーバーラップ領域をできるだけ広げ、距離画像の対応付けにおける誤対応を削減する。

図１は、本発明の第１の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。
図１に示すように、位置姿勢計測システムは、位置姿勢計測装置１００、濃淡画像撮像部２００、及び、距離計測部３００を有して構成されている。

また、図１に示すように、位置姿勢計測装置１００は、３次元形状モデル保存部１１０、概略位置姿勢入力部１２０、濃淡画像入力部１３０、画像特徴検出部１４０、距離画像入力部１５０、及び、位置姿勢算出部１６０を有して構成されている。また、位置姿勢計測装置１００には、濃淡画像撮像部２００及び距離計測部３００が接続されている。

濃淡画像撮像部２００は、通常の濃淡画像を撮影するカメラである。撮影される画像は、濃淡画像であってもよいしカラー画像であってもよい。濃淡画像撮像部２００が撮影する画像は、濃淡画像入力部１３０を介して位置姿勢計測装置１００に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、上記非特許文献４に係るZhangの方法によって事前にキャリブレーションしておく。

距離計測部３００は、計測対象である物体表面上の点までの距離を計測する。本実施形態では、距離計測部３００として距離画像を出力する距離センサを用いる。距離画像は、各画素が距離計測部３００からの距離の情報を持つ画像である。本実施形態では、各画素には距離の情報ではなく、距離をもとに算出される計測装置基準の座標系における３次元座標が保存されているものとする。即ち、距離画像からは、３次元点群データが得られる。距離値から３次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。距離センサとして、例えば、液晶プロジェクタから物体に照射した２次元のパターン光をカメラで撮影し、三角測量により距離を計測するアクティブ方式のものを利用する。なお、距離センサはこれに限るものではなく、光切断法やTime-of-flight方式など他のアクティブ式のものや、ステレオカメラによるパッシブ式など、距離画像を出力可能であれば如何なる距離センサであってもよい。本実施形態では、距離センサ用のカメラと濃淡画像撮像部２００のカメラは同一のものを用いる。即ち、同一視点における濃淡画像と距離画像が取得できると仮定する。距離センサ用のカメラと濃淡画像撮像部２００のカメラが異なる場合には、両者の相対的な位置及び姿勢を事前にキャリブレーションしておく。例えば３次元形状が既知であるキャリブレーション物体を撮影し、濃淡画像に基づいた位置及び姿勢と、距離画像に基づいた物体の位置及び姿勢から相対的な位置及び姿勢を求める。距離計測部３００が計測した距離画像は、距離画像入力部１５０を介して位置姿勢計測装置１００に入力される。

次に、位置姿勢計測装置１００を構成する各部について説明する。

３次元形状モデル保存部１１０には、位置及び姿勢を計測する対象である物体の３次元形状モデルを保存する。本実施形態では、物体を線分と面から構成される３次元形状モデルとして記述する。

図２は、本発明の第１の実施形態を示し、３次元形状モデルを説明する模式図である。３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、計測対象物体１０−１の３次元形状モデルは、点Ｐ１〜点Ｐ１４の１４点から構成される。また、図２（ｂ）に示すように、計測対象物体１０−２の３次元形状モデルは、線分Ｌ１〜Ｌ１６により構成されている。また、図２（ｃ）に示すように、点Ｐ１〜点Ｐ１４は３次元座標値によって表される。また、図２（ｄ）に示すように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。さらに、３次元形状モデルは、面の情報を保持する。各面は、各面を構成する点のＩＤによって表される。図２に示す３次元形状モデルでは、直方体を構成する６つの面の情報が記憶されている。３次元形状モデルは、位置姿勢算出部１６０において物体の位置及び姿勢を算出する際に用いられる。

概略位置姿勢入力部１２０は、位置姿勢計測装置１００に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置１００には、位置及び姿勢計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。位置姿勢計測装置１００に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、濃淡画像撮像部２００であるカメラのカメラ中心を原点とし、画像の水平方向をｘ軸、垂直方向をｙ軸、カメラの光軸をｚ軸とした座標系を基準座標系とする。本実施形態では、位置姿勢計測装置１００は、時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものでもよい。また、物体が置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いる。この概略位置姿勢入力部１２０は、濃淡画像または距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される物体の位置及び姿勢を概略の位置及び姿勢として入力する。

濃淡画像入力部１３０は、濃淡画像撮像部２００で撮影された濃淡画像を位置姿勢計測装置１００に入力する処理を行う。

画像特徴検出部１４０は、濃淡画像入力部１３０から入力された濃淡画像から画像特徴を検出する。本実施形態では、画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部１２０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。

距離画像入力部１５０は、距離計測部３００で得られた距離画像を位置姿勢計測装置１００に入力する処理を行う。

位置姿勢算出部１６０は、画像特徴検出部１４０で検出された画像特徴、及び距離画像入力部１５０によって入力された３次元点群と、３次元形状モデル保存部１１０に保存される３次元形状モデルをもとに、物体の位置及び姿勢を計測する。

図３は、本発明の第１の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。

＜ステップＳ１０１０＞
概略位置姿勢入力部１２０は、位置姿勢計測装置１００（カメラ）に対する物体の位置及び姿勢の概略値を位置姿勢計測装置１００に入力する。前述したように、本実施形態では、前回計測された位置及び姿勢を概略値として用いる。

＜ステップＳ１０２０＞
濃淡画像入力部１３０及び距離画像入力部１５０は、物体の位置及び姿勢を算出するための計測データを取得する。具体的には、濃淡画像入力部１３０及び距離画像入力部１５０は、計測対象物体の濃淡画像と距離画像を取得する。前述したように、距離画像の各画素には、基準座標系における点の３次元座標が記録されている。濃淡画像撮像部２００によって撮影された濃淡画像は、濃淡画像入力部１３０を介して位置姿勢計測装置１００に入力される。また、距離計測部３００から出力された距離画像は、距離画像入力部１５０を介して位置姿勢計測装置１００に入力される。

＜ステップＳ１０３０＞
画像特徴検出部１４０及び位置姿勢算出部１６０は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測（算出）を行う。ステップＳ１０３０では、濃淡画像上の特徴であるエッジに３次元形状モデル中の線分の投影像が当てはまるように、計測対象物体の位置及び姿勢を算出する。

図４は、図３のステップＳ１０３０における位置姿勢計測（位置姿勢算出）の処理手順の一例を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を６次元ベクトルｓで表す。ｓは、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す３次元ベクトルなどによって表現される。以下、図３のステップＳ１０３０における位置姿勢算出の処理手順について順に説明する。

≪ステップＳ１１１０≫
まず、位置姿勢計測装置１００では、初期化処理を行う。ここでは、ステップＳ１０１０で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。

≪ステップＳ１１２０≫
続いて、画像特徴検出部１４０は、ステップＳ１０２０において入力された濃淡画像上において画像特徴の検出・画像特徴と３次元形状モデルとの対応付けを行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態では、上記非特許文献１で開示される方法によりエッジ検出を行う。この非特許文献１で開示されるエッジ検出方法は、トップダウン型の方法であり、モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に行われる。

図５は、本発明の第１の実施形態を示し、エッジ検出を説明する模式図である。
まず、ここでは、ステップＳ１１１０において入力された計測対象物体の位置及び姿勢の概略値と校正済みのカメラの内部パラメータを用いて、３次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は、画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で１次元のエッジ検出を行う（図５（ａ））。エッジは、画素値の濃度勾配の極値として検出されるため、図５（ｂ）に示すように、近傍にエッジが存在する場合には、複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。

≪ステップＳ１１３０≫
続いて、例えば位置姿勢算出部１６０は、３次元形状モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。

図６は、本発明の第１の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。
図６では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。位置及び姿勢の概略値ｓ₀に基づいて投影された線分上のある制御点の画像上での位置を（ｕ₀，ｖ₀）、当該制御点が所属する線分の画像上での傾きをｕ軸に対する傾きθと表す。傾きθは、線分の両端の３次元座標をｓの概略値（ｓ₀）に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。当該線分の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、当該制御点の対応点（エッジ）の画像座標を（ｕ'，ｖ'）とする。ここで、点（ｕ'，ｖ'）を通り、傾きがθである直線（図６の破線）上の点（ｕ，ｖ）は、以下の数式（１）と表せる（θは定数とする）。

制御点の画像上での位置は、計測対象物体の位置及び姿勢により変化する。制御点の画像座標（ｕ，ｖ）は（ｕ₀，ｖ₀）の近傍で１次のテイラー展開によって以下の数式（２）のように近似できる。但しΔｓ_i（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。

正しいｓによって得られる制御点の画像上での位置が数式（１）によって表される直線上にあると仮定すると、数式（２）を数式（１）に代入することにより、観測方程式である以下の数式（３）が得られる。

数式（３）の観測方程式は、対応付けが行われたすべての制御点について立式することができる。数式（３）はｓの各成分の微小変化Δｓ_i（ｉ＝１，２，・・・，６）についての方程式になっており、以下の数式（４）に示すΔｓ_iに関する線形連立方程式を立てることができる。

この線形連立方程式を以下の数式（５）のように表す。

数式（５）において、左辺のＪが求める係数行列であり、右辺のＥが誤差ベクトルである。

≪ステップＳ１１４０≫
続いて、例えば位置姿勢算出部１６０は、数式（５）をもとに、位置及び姿勢の補正値Δｓを最小二乗基準で求める。最小二乗基準では、以下の数式（６）を最小化するように位置及び姿勢の補正値Δｓを求める。

ただし、Ｊ_ijは係数行列Ｊのｉ行ｊ列目の要素を表し、Ｅ_iは誤差ベクトルＥのｉ番目の要素を表す。数式（６）を最小にするΔｓは、行列Ｊの一般化逆行列（Ｊ^t・Ｊ）^-1・Ｊ^tを用いて、以下の数式（７）のように求める。

しかしながら、エッジの対応付けでは、誤対応（外れ値）が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、数式（４）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは、例えば以下の数式（８）に示すようなＴｕｋｅｙの関数により与える。

ここで、数式（８）において、ｃ₁は定数である。なお、重みを与える関数は、Ｔｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であれば何でもよい。そして、各計測データに対応する重みをｗ_iとする。この重みｗ_iを用いて、数式（６）を以下の数式（９）のように変形する。

そして、例えば位置姿勢算出部１６０は、数式（９）を最小にする補正値Δｓを以下の数式（１０）のように解くことにより求める。

ただし、数式（１０）の行列Ｗは、以下の数式（１１）で定義される重み行列である。

数式（１１）の重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗ_iが入る。

≪ステップＳ１１５０≫
続いて、例えば位置姿勢算出部１６０は、ステップＳ１１４０において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する。
ｓ←ｓ＋Δｓ

≪ステップＳ１１６０≫
続いて、例えば位置姿勢算出部１６０は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップＳ１１３０に戻る。この収束判断では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。

以上の処理によって、ステップＳ１０１０で入力された位置及び姿勢の概略値が濃淡画像の情報を用いて更新される。この結果、位置及び姿勢の概略値に基づく画像上における３次元形状モデルの投影像と実際の物体の像とのオーバーラップ領域が広がるため、３次元形状モデルの投影に基づく距離画像と３次元形状モデルの対応付けにおける誤対応が減少する。

＜ステップＳ１０４０＞
次に、濃淡画像及び距離画像を用いた位置及び姿勢の算出処理について説明する。本ステップでは、画像特徴検出部１４０及び位置姿勢算出部１６０は、ステップＳ１０３０で更新された位置及び姿勢の概略値を用いて、濃淡画像、距離画像の双方に対して３次元形状モデルが当てはまるような位置及び姿勢の推定（算出）を行う。ここでは、画像上のエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和と３次元点群と３次元形状モデルとの距離の二乗和の和が最小になるように位置及び姿勢の最適化を行う。

図７は、図３のステップＳ１０４０における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図４のフローチャートで示されるステップＳ１０３０の処理手順と同一であるため、説明が重複する部分は省略する。

≪ステップＳ１２１０≫
まず、位置姿勢計測装置１００では、初期化処理を行う。ここでは、ステップＳ１０３０で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。

≪ステップＳ１２２０≫
続いて、画像特徴検出部１４０は、ステップＳ１０２０において入力された濃淡画像上において画像特徴の検出・画像特徴と３次元形状モデルとの対応付けを行う。また、同様にステップＳ１０２０において入力された距離画像と３次元形状モデルとの対応付けを行う。濃淡画像に対する処理は、ステップＳ１１２０の処理と同じであるため、説明を省略する。以下、距離画像と３次元形状モデルとの対応付け処理について説明する。

図８は、本発明の第１の実施形態を示し、図７のステップＳ１２２０における距離画像と３次元形状モデルとの対応付け処理を説明する模式図である。
本ステップでは、上記非特許文献５で開示されている方法と同様に、物体の３次元形状モデル（ポリゴンモデル）８１０を距離画像面８２０上に投影することによって対応付けを行う。ここで、３次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデル８１０のどのポリゴンに対応するかを決定するためである。ここでは、距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデル８１０を描画する。以下、ポリゴンモデル８１０を描画した画像をポリゴン描画画像と呼ぶ。

描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はＲＧＢの各色で表す。背景部分と区別するために、黒（Ｒ＝０、Ｇ＝０、Ｂ＝０）以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をＧＰＵ（Graphics Processing Unit）上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデル８１０を描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。

≪ステップＳ１２３０≫
続いて、例えば位置姿勢算出部１６０は、３次元形状モデルの線分上の制御点と画像上のエッジとの対応、ポリゴンと３次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。モデルの線分上の制御点と画像上のエッジとの対応に基づく係数行列・誤差ベクトルの算出については、ステップＳ１１３０と同じであるため説明を省略する。以下では、距離画像の画素（３次元の点）と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。

３次元空間における平面の方程式は一般に、ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ²＋ｂ²＋ｃ²＝１）の形で表される。ここでは、３次元形状モデル中のある面の計測物体の座標系における平面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｅであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式ａ'ｘ＋ｂ'ｙ＋ｃ'ｚ＝ｅ'に変換できる。位置及び姿勢の概略値に基づく計測物体座標系から基準座標系への座標変換を、以下の数式（１２）のように表す。

ただし、数式（１２）において、Ｒは姿勢を表す３×３回転行列、ｔは位置を表す３次元ベクトルである。このとき、基準座標系における平面の方程式の係数は、以下の数式（１３）のようになる。

平面の座標変換については、例えば金谷「形状ＣＡＤと図形の数学」（共立出版）で詳しく説明がなされている。基準座標系における３次元座標の計測値が（ｘ'，ｙ'，ｚ'）である点Ｐが３次元形状モデル中のある面Ｆと対応付けられているとする。面Ｆ上にある点をＱとする。点Ｑの基準座標系における３次元座標（ｘ，ｙ，ｚ）は、計測対象物体の位置及び姿勢ｓによって変換するものであり、ある位置及び姿勢ｓのときの３次元座標（ｘ₀，ｙ₀，ｚ₀）の近傍で１次のテイラー展開によって、以下の数１４のように近似できる。

正しいｓによって変換される（ｘ，ｙ，ｚ）は、法線ベクトルが面Ｆと同じで点Ｐを通る平面（ａ'ｘ＋ｂ'ｙ＋ｃ'ｚ＝ｅ'，ｅ'＝ａ'ｘ'＋ｂ'ｙ'＋ｃ'ｚ'） 上にあると仮定すると、数式（１４）を平面の方程式に代入することで以下の数式（１５）が得られる。

数式（１５）の観測方程式は、対応付けが行われたすべての点群データについて立式することができる。
数式（３）及び数式（１５）の観測方程式は、ｓの各成分の微小変化Δｓ_i（ｉ＝１，２，・・・，６）についての方程式になっており、数式（１５）に示すΔｓ_iに関する線形連立方程式を、以下の数式（１６）のように立てることができる。

この数式（１６）に示す線形連立方程式を、以下の数式（１７）のように表す。

数式（１７）において、左辺のＪが求める係数行列であり、右辺のＥが誤差ベクトルである。

≪ステップＳ１２４０≫
続いて、例えば位置姿勢算出部１６０は、数式（１７）をもとに、位置及び姿勢の補正値Δｓを算出する。しかしながら、エッジの対応付けデータや距離画像の対応付けデータに誤対応などの外れ値が混入することは避けられない。そのため、ステップＳ１１４０で述べたようなロバスト推定手法により重み行列Ｗを用いて、以下の数式（１８）により補正値Δｓを算出する。

≪ステップＳ１２５０≫
続いて、例えば位置姿勢算出部１６０は、ステップＳ１２４０において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する。
ｓ←ｓ＋Δｓ

≪ステップＳ１２６０≫
続いて、例えば位置姿勢算出部１６０は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップＳ１２３０に戻る。この収束判断では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。

上述したように、第１の実施形態に係る位置姿勢計測装置１００では、３次元形状モデルが濃淡画像の画像特徴及び距離画像に当てはまるように、位置姿勢計測装置１００に対する物体の概略の位置及び姿勢を更新する位置姿勢算出部１６０を有している。
そして、この位置姿勢算出部１６０では、少なくとも濃淡画像の情報を利用して画像面上での物体像と３次元形状モデルの投影像が最もオーバーラップするように第１の位置及び姿勢を算出する（図３のＳ１０３０）。なお、第１の位置及び姿勢は、位置及び姿勢の一部の成分であり、具体的には、当該位置姿勢計測装置１００のカメラの光軸に垂直な位置及び光軸まわりの姿勢である。また、位置姿勢算出部１６０では、第１の位置及び姿勢を用いて画像面上に投影される３次元形状モデルの投影像をもとに３次元形状モデルと濃淡画像の画像特徴及び距離画像との対応付けを行って、第２の位置及び姿勢を算出する（図３のＳ１０４０）。

このように、第１の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定（算出）して位置及び姿勢の概略値を更新した後、３次元形状モデルを投影することによる距離画像と３次元形状モデルの対応付けを行うことで誤対応を削減する。その結果、濃淡画像と距離画像を用いることで高精度に位置及び姿勢の推定（算出）が行えるとともに、位置及び姿勢の概略値のずれに対してロバストになる。

［変形例１−１］（２Ｄの自由度を減らす）
上述した第１の実施形態では、前段の処理である濃淡画像のみを利用する位置及び姿勢の推定において、位置及び姿勢の６自由度すべてを推定した。しかしながら、この処理においてはすべての自由度を推定する必要はなく、濃淡画像を用いて精度高く推定できる成分のみを推定してもよい。背景や物体の構造が複雑な場合には、濃淡画像上のエッジとモデルが誤対応することは避けられない。推定する成分を限定することで計測データに対するオーバーフィッティングが緩和されるため、誤対応に対するロバスト性の向上が期待できる。

本変形例では、濃淡画像上のエッジを利用して、カメラの光軸に垂直な方向の位置成分と光軸まわりの姿勢成分の３つを未知パラメータとして推定する。これら３つのパラメータを３次元ベクトルｓ'で表す。制御点の画像座標（ｕ，ｖ）は（ｕ₀，ｖ₀）の近傍で１次のテイラー展開によって、以下の数式（１９）のように近似できる。但しΔｓ'_i（ｉ＝１，２，３）はｓ'の各成分の微小変化を表す。

数式（１９）を数式（１）に代入することにより、以下の観測方程式である数式（２０）が得られる。

対応付けが行われたすべての制御点について立式される数式（２０）を連立方程式として、第１の実施形態と同様の処理によって補正値Δｓ'_i（ｉ＝１，２，３）を算出する。本変形例における処理は、概略の位置及び姿勢に基づく投影像の形状と実際の物体の画像上での形状が類似している場合に有効である。

［変形例１−２］（繰り返し計算の中で対応付けをやり直してもよい）
上述した第１の実施形態では、ステップＳ１０３０、Ｓ１０４０の位置及び姿勢の算出処理において、計測データと３次元形状モデルとの対応付け情報は、固定した情報として取り扱った。しかし、実際には、推定する位置及び姿勢の変化によって対応付け情報は変化する。そのため、位置及び姿勢を更新した後で、再度、対応付け処理をやり直してもよい。ただし、対応付け処理は時間がかかる処理であるため、位置及び姿勢が更新されるたびに対応付けをやり直すのではなく、数回更新された後に対応付け処理を行ってもよい。

［変形例１−３］（概略値はclosed-formで求めてもよい）
上述した第１の実施形態では、位置及び姿勢の概略値を、過去の計測結果から入力した。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではなく、濃淡画像や距離画像の情報を元に繰り返し計算なしに直接的に算出してもよい。例えば、画像全体から検出された線分と３次元形状モデル中の線分と対応付けて位置及び姿勢を直接的に算出してもよい。具体的には、まず画像全体に対して例えばＣａｎｎｙのエッジ検出器によってエッジ検出を行い、隣接するエッジ群をラベリングする。次に、同一ラベルを持つエッジ群に対して、折れ線近似によって線分当てはめを行う。このようにして得られた画像上の線分と３次元形状モデル中の線分の対応をもとに上記非特許文献６に係るLiuの手法により位置及び姿勢を算出してもよい。また、例えば、上記非特許文献７に係るJohnsonの方法により、距離画像に基づいて概略の位置及び姿勢を算出してもよい。また、例えば、上記非特許文献９に係るUlrichらの方法のように、物体の姿勢に応じた固有のパターンを画像上で網羅的に探索する方法によって位置及び姿勢の概略値を算出してもよい。固有のパターンは、濃淡値からなるテンプレート画像であってもよいし、二値のエッジテンプレート画像であってもよい。また、網羅的に探索する方法は、姿勢クラスを識別するパターン認識による方法であってもよい。

［変形例１−４］（第１の位置姿勢を濃淡＋距離から算出）
上述した第１の実施形態では、前段の処理において濃淡画像のみを利用して位置及び姿勢を推定した。しかしながら、濃淡画像の情報だけでなく、誤対応が悪い影響を与えない程度に距離画像の情報を利用してもよい。具体的には、第１の実施形態のステップＳ１０３０の処理のかわりにＳ１０４０の処理を実施する。このとき、距離画像の誤対応が位置及び姿勢の算出に大きく影響しないように、数式（８）における閾値ｃ₁の値を大きくして全体的に重みが小さくなるようにする。

（第２の実施形態）（２Ｄの前に奥行きを求める）
上述した第１の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定して位置及び姿勢の概略値を更新した後、３次元形状モデルを投影することによる距離画像と３次元形状モデルの対応付けを行った。しかしながら、位置及び姿勢の概略値の更新方法は、これに限るものではない。例えば、距離画像のデータを用いて、位置及び姿勢のうち、カメラからの奥行きに相当する位置成分を推定（算出）してもよい。位置及び姿勢の概略値の真の値に対する誤差が大きい場合は、３次元形状モデルを投影することにより距離画像と３次元形状モデルを正確に対応付けることは難しい。しかしながら、カメラから物体表面までの距離がほぼ同一とみなせる場合には、多少対応付けが間違っていたとしても、奥行きを真の値に近づけることは可能である。

そこで本実施形態では、まず距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第１の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値が真の値と大きく異なる場合であっても、ロバストに位置及び姿勢を計測（算出）することが可能になる。

図９は、本発明の第２の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図９の各ステップのうち、ステップＳ２０１０、Ｓ２０２０、Ｓ２０４０については、それぞれ、第１の実施形態における図３のステップＳ１０１０、Ｓ１０２０、Ｓ１０４０と同一であるため、その説明を省略する。以下では、図９の処理のうち、ステップＳ２０２５、Ｓ２０３０について説明する。

＜ステップＳ２０２５＞
ステップＳ２０２５では、例えば位置姿勢算出部１６０は、距離画像の情報に基づいて物体の位置及び姿勢のうち、奥行きに相当する位置成分の概略値を更新する。

具体的な処理としては、まずステップＳ２０１０において入力される位置及び姿勢の概略値をもとに、第１の実施形態のステップＳ１２２０において説明した方法により、距離画像と３次元形状モデルとの対応付けを行う。次に、例えば位置姿勢算出部１６０は、対応付け結果に基づいて、物体の位置を更新する。物体の位置は、次のように更新する。ここで、基準座標系における物体の位置の概略値を表す３次元ベクトルをｔ₀＝［ｔ_x ｔ_y ｔ_z］^tとする。次に、例えば位置姿勢算出部１６０は、モデル面との対応付けがなされている距離画像上の画素について、基準座標系の原点（カメラの投影中心）から出て当該画素を通る光線と３次元形状モデル面との交点の３次元座標を算出する。光線上の点の３次元座標ｘは、光線の方向ベクトルをｄ＝［ｄ_x ｄ_y ｄ_z］^tとすると、ｘ＝αｄによって表される（αは媒介変数）。この点が基準座標系におけるモデル面ａ'ｘ＋ｂ'ｙ＋ｃ'ｚ＝ｅ'上にあるとして、αを求めてｘを算出することにより、交点の３次元座標を算出する。距離画像上の画素に保持されている３次元座標をｘ_data、算出された交点の３次元座標をｘ_calcとする。ｘ_calcのｚ成分の平均値／z_calcがx_dataのｚ成分の平均値／z_dataに一致するように物体の位置ｔ₀を更新する。更新した位置ｔ'は、以下の数式（２１）により算出する。単純に位置ｔ₀のｚ成分のみを更新しないのは、物体の観察方向に応じてｚ成分の位置への影響が変わるためである。

＜ステップＳ２０３０＞
画像特徴検出部１４０及び位置姿勢算出部１６０は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測（算出）を行う。基本的な処理は、第１の実施形態におけるステップＳ１０３０と同一であるが、入力する位置及び姿勢の概略値として、ステップＳ２０２５によって更新された位置及び姿勢の概略値を利用する。以降の処理の説明は省略する。

このように、第２の実施形態では、距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第１の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値のずれが補正され、後段の処理において誤対応を削減することが可能である。

［変形例２−１］（エッジは特徴点であってもよい）
以上、説明した本発明の実施形態、変形例では、濃淡画像上の特徴はエッジであり、３次元形状モデル中の線分をエッジに当てはめることにより位置及び姿勢の計測を行った。しかしながら、濃淡画像上の特徴はエッジに限るものではなく、例えば２次元的な特徴である特徴点であってもよい。特徴点は、コーナー点など画像上で特徴的な点であり、例えば、上記非特許文献８に係るHarrisの検出器によって検出することができる。３次元形状モデルを３次元点群によって表すことで、各特徴点をモデル中の３次元点群の点と対応付けることができる。特徴点を用いる場合には、まず画像全体からHarrisの検出器によって特徴点を検出する。次に、モデル中の３次元点群の各点を位置及び姿勢の概略値に基づいて画像上に投影し、画像上で最も近い特徴点と対応付けを行う。

（第３の実施形態）（後段の位置姿勢推定は２．５Ｄのみ利用）
上述した実施形態では、後段の位置及び姿勢の推定においては濃淡画像と距離画像の双方の情報を用いていた。しかしながら、後段の位置及び姿勢の推定方法はこれに限るものではない。計測対象物体が多面体である場合、少なくとも３面以上の十分な面積を持つ面が撮像装置から見えていれば、距離画像のみから精度よく位置及び姿勢を推定することができる。また、計測対象物体が曲面から構成される場合には、撮像装置から観察される物体表面上の点の法線ベクトルが十分に３次元的に分布していれば距離画像のみから精度よく位置及び姿勢を推定することができる。

そこで本実施形態では、まず、第１の実施形態で述べたように、濃淡画像を用いて物体の位置及び姿勢の概略値を更新した後に、更新された概略値を初期値として距離画像のみを用いて位置及び姿勢を推定する。

図１０は、本発明の第３の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図１０の各ステップのうち、ステップＳ３０１０、Ｓ３０２０、Ｓ３０３０については、それぞれ、第１の実施形態における図３のステップＳ１０１０、Ｓ１０２０、Ｓ１０３０と同一であるため、その説明を省略する。以下では、図１０の処理のうち、ステップＳ３０４０について説明する。

＜ステップＳ３０４０＞
ステップＳ３０４０では、位置姿勢算出部１６０は、ステップＳ３０３０で更新された位置及び姿勢の概略値を用いて、距離画像に対して３次元形状モデルが当てはまるような位置及び姿勢の推定（算出）を行う。ここでは、距離画像から得られる３次元点群と３次元形状モデルとの距離の二乗和が最小になるように位置及び姿勢の最適化を行う。

図１１は、図１０のステップＳ３０４０における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図７のフローチャートで示されるステップＳ１０４０の処理手順から濃淡画像に対する処理を除いた処理手順であるため、説明が重複する部分は省略する。

≪ステップＳ３１１０≫
まず、位置姿勢計測装置１００では、初期化処理を行う。ここでは、ステップＳ３０３０で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。

≪ステップＳ３１２０≫
続いて、画像特徴検出部１４０は、ステップＳ３０２０で入力された距離画像と３次元形状モデルとの対応付けを行う。距離画像と３次元形状モデルとの対応付けについては、ステップＳ１２２０の処理と同じであるため、説明を省略する。

≪ステップＳ３１３０≫
続いて、例えば位置姿勢算出部１６０は、ポリゴンと３次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。３次元形状モデルの面と３次元点との対応に基づく係数行列・誤差ベクトルの算出方法については、ステップＳ１２３０と同じであるため、説明を省略する。本ステップでは、距離画像の情報のみを用いるため、ｓの各成分の微小変化Δｓ_i（ｉ＝１，２，・・・，６）に関する線形連立方程式は以下の数式（２２）になる。

この数式（２２）に示す線形連立方程式を、以下の数式（２３）のように表す。

数式（２３）において、左辺のＪが求める係数行列であり、右辺のＥが誤差ベクトルである。

≪ステップＳ３１４０≫
続いて、例えば位置姿勢算出部１６０は、数式（２３）をもとに、位置及び姿勢の補正値Δｓを算出する。距離画像と３次元形状モデルの対応付けデータに誤対応などの外れ値が混入することは避けられないため、ステップＳ１１４０で述べたようなロバスト推定手法により重み行列Ｗを用いて、以下の数式（２４）により補正値Δｓを算出する。

≪ステップＳ３１５０≫
続いて、例えば位置姿勢算出部１６０は、ステップＳ３１４０において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する。
ｓ←ｓ＋Δｓ

≪ステップＳ３１６０≫
続いて、例えば位置姿勢算出部１６０は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップＳ３１３０に戻る。この収束判断では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。

以上述べたように、第３の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢の概略値を推定した後に、推定した概略値を初期値として距離画像を用いて物体の位置及び姿勢を推定する。これにより、距離画像の情報のみから高精度に位置及び姿勢を推定する場合には、後段の処理において濃淡画像に関する処理を行う必要がないため、高速に位置及び姿勢を推定することが可能になる。

（第４の実施形態）
上述した実施形態では、濃淡画像とともに対象物体の３次元の計測データとして距離画像を位置姿勢推定に利用していた。しかしながら、位置姿勢推定に利用する３次元の計測データは画素ごとに３次元座標を持つ距離画像に限るものではなく、物体の表面上における点群の３次元座標を表す計測データであれば他の形態であってもよい。この場合、位置姿勢計測装置１００は、物体の表面上における点群の３次元座標を入力する３次元データ入力部（不図示）を具備する構成となる。そして、位置姿勢算出部１６０は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第１の位置及び姿勢を算出する。さらに、位置姿勢算出部１６０は、第１の位置及び姿勢をもとに、３次元データ入力部（不図示）から得られる３次元座標の点群である３次元点群または当該３次元点群及び濃淡画像の画像特徴群と、３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する。ここで、例えば、物体の表面上における点群の３次元座標を表す計測データとしては、３次元座標の集合であってもよい。なお、本実施形態と他の実施形態との違いは、３次元形状モデルと３次元の計測データの対応付けだけであるため、以下では対応付け方法のみを説明し、重複する説明は割愛する。

ステップＳ１２２０の説明において述べたように、３次元の計測データが距離画像の場合には、物体の位置及び姿勢の概略値に基づいて３次元形状モデルを距離画像上に投影することで距離画像上の各画素と３次元形状モデルのポリゴンとの対応づけを行った。３次元計測データが３次元座標の集合である場合には画像上への投影を行うことができないため、各計測点に最も近いポリゴンを３次元的に探索する。

具体的な方法を以下に示す。
まず、予め３次元形状モデルを構成する各ポリゴン上に計測点の密度より細かく複数の点（以降、面モデル点と呼ぶ）を設定し、各面モデル点の物体座標系における３次元座標と属するポリゴンのＩＤを保存しておく。対応付けを行う際には、非特許文献２に開示されるように、ｋｄ−ｔｒｅｅを用いて各計測点に対応する面モデル点を探索する。面モデル点にはポリゴンのＩＤが紐づけられているため、この探索を行うことで計測点とポリゴンが対応付けされる。

以上述べたように、第４の実施形態では、対象物体の３次元の計測データが画素ごとに３次元座標を持つ距離画像でなくとも、本発明による位置姿勢推定を適用することが可能である。

上述した本発明の各実施形態では、位置姿勢算出部１６０は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第１の位置及び姿勢を算出する（図３のＳ１０３０、図９のＳ２０３０、図１０のＳ３０３０）。また、位置姿勢算出部１６０は、第１の位置及び姿勢を用いて算出される３次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する（図３のＳ１０４０、図９のＳ２０４０、図１０のＳ３０４０）。

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

本発明は、例えば、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。

１００ 位置姿勢計測装置、１１０ ３次元形状モデル保存部、１２０ 概略位置姿勢入力部、１３０ 濃淡画像入力部、１４０ 画像特徴検出部、１５０ 距離画像入力部、１６０ 位置姿勢算出部、２００ 濃淡画像撮像部、３００ 距離計測部

Claims (10)

1. 計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
   前記物体の３次元形状モデルを保存する保存手段と、
   前記物体の濃淡画像を入力する濃淡画像入力手段と、
   前記物体の距離画像を入力する距離画像入力手段と、
   当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
   前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢を用いて算出される前記３次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする位置姿勢計測装置。
2. 前記第１の位置及び姿勢は、位置及び姿勢の一部の成分であることを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記第１の位置及び姿勢は、当該位置姿勢計測装置のカメラの光軸に垂直な位置及び光軸まわりの姿勢であることを特徴とする請求項２に記載の位置姿勢計測装置。
4. 前記概略位置姿勢入力手段は、前記濃淡画像または前記距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される前記物体の位置及び姿勢を概略の位置及び姿勢として入力することを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
5. 前記位置姿勢算出手段は、前記第１の位置及び姿勢を算出する前に、前記距離画像の情報を用いて位置及び姿勢のうち、当該位置姿勢計測装置からの奥行きに相当する位置を算出し、前記概略の位置及び姿勢を更新することを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測装置。
6. 前記画像特徴群における画像特徴は、エッジであることを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測装置。
7. 前記画像特徴群における画像特徴は、特徴点であることを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測装置。
8. 計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
   前記物体の３次元形状モデルを保存する保存手段と、
   前記物体の濃淡画像を入力する濃淡画像入力手段と、
   前記物体の表面上における点群の３次元座標を入力する３次元データ入力手段と、
   当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
   前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢をもとに、前記３次元データ入力手段から得られる３次元座標の点群である３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする位置姿勢計測装置。
9. 計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
   前記物体の３次元形状モデルを保存する保存ステップと、
   前記物体の濃淡画像を入力する濃淡画像入力ステップと、
   前記物体の距離画像を入力する距離画像入力ステップと、
   前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
   前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢を用いて算出される前記３次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出ステップと
   を有することを特徴とする位置姿勢計測方法。
10. 計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
    前記物体の３次元形状モデルを保存する保存ステップと、
    前記物体の濃淡画像を入力する濃淡画像入力ステップと、
    前記物体の表面上における点群の３次元座標を入力する３次元データ入力ステップと、
    前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
    前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第１の位置及び姿勢を算出するとともに、前記第１の位置及び姿勢をもとに、前記３次元データ入力ステップから得られる３次元座標の点群である３次元点群または当該３次元点群及び前記濃淡画像の画像特徴群と、前記３次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第２の位置及び姿勢を算出する位置姿勢算出ステップと
    を有することを特徴とする位置姿勢計測方法。

Images