JP2015132523A - 位置姿勢計測装置、位置姿勢計測方法及びプログラム - 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 ３-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation （ICRA'94）, pp.2235-2241, 1994. Y. Hel-Or and M. Werman, "Pose estimation by fusing noisy data of different dimensions, " IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.17, no.2, pp.195-201, 1995. 日浦，山口，佐藤，井口，"距離画像と濃淡画像の融合による任意形状物体の実時間追跡"，電子情報通信学会論文誌D-II, vol.J80-DII, no.11, pp.2904-2911, 1997. Z. Zhang, "A flexible new technique for camera calibration," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.22, no.11, pp.1330-1334, 2000. 井口征士、佐藤宏介「三次元画像計測」（昭晃堂，１９９０） C. Harris and M. Stephens, "A combined corner and edge detector" Proc. the 4th Alvey Vision Conference, pp.147-151, 1988. C. Wiedemann, et al., "Recognition and Tracking of 3D Objects; in: Pattern Recognition," G. Rigoll （Editor）, Lecture Notes in Computer Science, Springer Verlag , Vol. 5096, pp.132-141, 2008

工業製品の組立をロボットが行う場合には、組立の繰り返し精度が向上するだけでなく、組立が高速化されることも期待されている。組立を高速に行うためには、ロボットの動作自体を高速にするだけでなく、動作を決定するのに必要な位置姿勢の計測も高速に行う必要がある。

前述したように２次元画像と距離画像とを相補的に利用して位置姿勢を計測する場合は、基本的に計測データの取得処理と位置姿勢の算出処理とが分離されている。距離画像を取得するために異なるパターンを計測対象の物体に投影し、複数の画像を取得する必要がある場合には、それらの画像がすべて撮影されるまで位置姿勢算出に係る処理を開始できず、多くの時間がかかってしまうという問題点がある。

本発明は前述の問題点に鑑み、２次元画像と距離画像とを利用して高速に位置姿勢を計測できるようにすることを目的としている。

本発明に係る位置姿勢計測装置は、距離を計測するためのパターンを物体に照射する照明手段と、前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、前記物体の３次元形状を表すモデルを保持する保持手段と、前記撮像手段による前記パターンが照射された物体の撮影と並行して、前記撮像手段によって撮影された２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理手段と、前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理手段と、前記第１の処理手段及び前記第２の処理手段による対応付けに基づいて前記物体の位置及び姿勢を導出する導出手段とを有することを特徴とする。

本発明によれば、２次元画像と距離画像とを利用した位置姿勢計測を高速化することができる。これにより、ロボットによる組立やロボットの自律移動をより高速化することができる。

本発明の第１の実施形態における位置姿勢計測装置の機能構成例を示すブロック図である。 本発明の実施形態における３次元形状モデルを説明するための図である。 本発明の第１の実施形態において、位置及び姿勢を計測する処理手順の一例を示すフローチャートである。 エッジを検出する方法を説明するための図である。 線分の投影像と検出されたエッジとの関係を説明するための図である。 物体に照射されるコードパターンの例を示す図である。 本発明の第２の実施形態における位置姿勢計測装置の機能構成例を示すブロック図である。 本発明の第２の実施形態において、位置及び姿勢を計測する処理手順の一例を示すフローチャートである。

（第１の実施形態）
本実施形態では、濃淡画像を撮影した後に距離画像を生成するための画像を撮影し、距離画像を生成するための画像の撮影と並行して、濃淡画像上の特徴に３次元モデルを当てはめるのに必要な処理を行う。本実施形態では、物体の概略の位置及び姿勢は既に認識されているものとする。

図１は、本実施形態における位置姿勢計測装置１００の機能構成例を示すブロック図である。
図１に示すように位置姿勢計測装置１００は、撮像部１１０、照明部１２０、モデル保存部１３０、概略位置姿勢入力部１４０、濃淡画像処理部１５０、距離画像処理部１６０、及び位置姿勢算出部１７０を備えている。以下、位置姿勢計測装置１００を構成する各部について説明する。

撮像部１１０は、２次元画像を撮影するカメラである。本実施形態では、撮像部１１０は通常照明下の濃淡画像（２次元画像）を撮影するとともに、距離計測用のパターンが照射された状態の画像（以下、距離計測用画像）の撮影も行う。距離計測用画像は複数枚撮影され、これらの距離計測用画像群をもとに１枚の距離画像が生成される。後述するように、本実施形態では位置姿勢計測装置１００における全体の処理を高速化するため、距離計測用画像の撮影と並行して濃淡画像処理部１５０の処理を行う。そのため、濃淡画像の撮影は距離計測用画像の撮影が完了する前に行う。すなわち、濃淡画像の撮影は、すべての距離計測用画像の撮影の前に行ってもよいし、一部の距離計測用画像の撮影の前に行ってもよい。距離計測用画像の撮影が完了する前に濃淡画像の撮影を終えることにより、距離計測用画像の撮影を行っている間に濃淡画像処理部１５０の処理を行う時間を生成する。なお、本実施形態では濃淡画像の撮影は、すべての距離計測用画像の撮影の前に行う。
撮像部１１０が撮影する濃淡画像は、濃淡画像処理部１５０に入力され、距離計測用画像は、距離画像処理部１６０に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば非特許文献５に記載の方法によって事前にキャリブレーションしておくものとする。

照明部１２０は、距離計測用のパターンを計測対象となる物体に対して照射する。本実施形態では、照明部１２０として液晶プロジェクタを用いる。液晶プロジェクタでは、任意の２次元パターンを照射することが可能である。なお、距離計測に必要な、液晶プロジェクタの内部パラメータ及びカメラに対する外部パラメータは、例えば非特許文献６井口征士、佐藤宏介らの『三次元画像計測』（昭晃堂，１９９０）に開示される方法により事前に校正しておくものとする。なお、照明装置は液晶プロジェクタに限るものではなく、距離計測用パターンを照射する照明装置であればいかなる照明装置であってもよい。例えば、ＤＭＤ（デジタルミラーデバイス）やＬＣＯＳを用いたプロジェクタであってもよい。

本実施形態では、空間コード化法によって距離計測を行う。空間コード化法では、図６に示すような複数のコードパターンを計測対象とする物体に投影し、撮影された画像の各画素と照射されたパターンとの対応付けを行い、画素単位で距離計測を行う。ただし、距離計測の方法はこれに限るものではなく、方向を変えたスリット光を時系列に投影することによって距離計測を行う光切断法であってもよい。

モデル保存部１３０は、位置及び姿勢を計測する対象となる物体の３次元形状モデルを保存する。本実施形態では、物体を線分と面とから構成される３次元形状モデルを用いる。位置姿勢算出部１７０は、３次元形状モデルの線分が濃淡画像上のエッジに当てはまり、距離画像から得られる３次元点群に３次元形状モデルの面が当てはまるように位置及び姿勢を推定する。

図２は、本実施形態における３次元形状モデルを説明するための図である。３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、観察対象となる物体２００の３次元形状モデルは、点Ｐ₁〜点Ｐ₁₄の１４点から構成され、さらに図２（ｂ）に示すように、線分Ｌ₁〜Ｌ₁₆により構成されている。また、図２（ｃ）に示すように、点Ｐ₁〜点Ｐ₁₄は３次元座標値によって表され、図２（ｄ）に示すように、線分Ｌ₁〜Ｌ₁₆は、線分を構成する点のＩＤによって表される。さらに３次元形状モデルは、面の情報を保持しており、各面は、各面を構成する点のＩＤによって表される。図２に示す３次元形状モデルでは、直方体を構成する６つの面の情報が記憶されている。

概略位置姿勢入力部１４０は、位置姿勢計測装置１００に対する物体の位置及び姿勢の概略値を入力する。本実施形態では、位置姿勢計測装置１００には、位置及び姿勢計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。位置姿勢計測装置１００に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、カメラの中心を原点とし、カメラの光軸をｚ軸とした座標系を基準座標系とする。

本実施形態では、位置姿勢計測装置１００は時間軸方向に連続して計測を行うものとし、前回（前時刻）の計測値を位置及び姿勢の概略値として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測してもよい。また、物体が置かれているおおよその位置や姿勢を予め認識している場合にはその値を概略値として用いる。

濃淡画像処理部１５０は、３次元形状モデルの線分が濃淡画像上の特徴（エッジ）に当てはまるように位置姿勢を推定するのに必要な処理を行う。濃淡画像処理部１５０は、距離画像を必要とせずに実行できる処理を濃淡画像に対して行う。具体的には、概略位置姿勢入力部１４０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各線分を濃淡画像上に投影し、投影された線分の近傍で対応するエッジ探索を行い、位置姿勢を算出するために必要な観測方程式を立式する。なお、この処理の詳細については後述する。濃淡画像の撮影を先に行うことにより、濃淡画像処理部１５０の処理を距離計測用画像群の撮影中に並行して行い、全体的な処理を高速化している。

距離画像処理部１６０は、３次元形状モデルの面が距離画像から生成される３次元点群に当てはまるように位置姿勢を推定するのに必要な処理を行う。この処理は、距離計測用画像群から１枚の距離画像を得る処理と、距離画像を３次元点群に変換して各点と３次元モデルの面との対応付けを行って観測方程式を立式する処理との２段階に分けられる。この処理の詳細についても後述する。

位置姿勢算出部１７０は、濃淡画像処理部１５０、及び距離画像処理部１６０において立式された観測方程式からなる連立方程式を解くことにより、計測対象となる物体の位置及び姿勢を算出する。

図３は、本実施形態において、位置及び姿勢を計測する処理手順の一例を示すフローチャートである。
まず、ステップＳ３０１において、概略位置姿勢入力部１４０は、位置姿勢計測装置１００（具体的にはカメラ）に対する物体の位置及び姿勢の概略値を入力する。前述したように本実施形態では、前の時刻において計測された位置及び姿勢を概略値として用いる。

続いてステップＳ３０２において、撮像部１１０は、通常照明下で計測対象となる物体の濃淡画像を撮影する。ここで、通常照明とは、一般的な環境に配置されている蛍光灯などの照明であってもよいし、照明部１２０による同一色（例えば白色）の照明であってもよい。すなわち、距離計測などを目的とした特殊なパターンの照明以外の照明であれば、いかなる照明であってもよい。本ステップで撮影された濃淡画像は、濃淡画像処理部１５０に入力される。

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

図４は、本実施形態において、エッジを検出する方法を説明するための図である。まず、ステップＳ３０１において入力された計測対象となる物体の位置及び姿勢の概略値と、校正済みのカメラの内部パラメータとを用いて、濃淡画像に投影する３次元形状モデルを構成する各線分の投影像を算出する。このとき、線分の投影像は濃淡画像上でも線分となる。

次に、図４（ａ）に示すように、濃淡画像上で等間隔になるように投影された線分４０２上に制御点４０１を設定し、制御点４０１ごとに、投影された線分４０２の法線方向の探索ライン４０３に一定範囲内で１次元のエッジ４０４を検出する。エッジ４０４は画素値の濃度勾配の極値として検出されるため、図４（ｂ）に示すように、近傍にエッジが存在する場合には複数のエッジ４０５が検出されることがある。本実施形態では、検出されるエッジのうち、最も濃度勾配の大きいものを対応するエッジとする。

次に、３次元形状モデルの線分上の制御点と濃淡画像上のエッジとの対応をもとに、位置及び姿勢を算出するための観測方程式を立式する。ここで、計測対象とする物体の位置及び姿勢を６次元ベクトルｓで表す。

図５は、線分の投影像と検出されたエッジとの関係を説明するための図である。図５に示す例では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点５０１の濃淡画像上での位置を（ｕ₀，ｖ₀）とし、制御点５０１が所属する線分５０２の濃淡画像上の傾きをｕ軸に対する傾きθと表す。傾きθについては、線分の両端の３次元座標を６次元ベクトルｓの概略値（ｓ₀）に基づいて濃淡画像上に投影し、濃淡画像上で両端の座標を結んだ直線の傾きとして算出する。該線分の濃淡画像上の法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、制御点５０１の対応点（エッジ）５０３の座標を（ｕ'，ｖ'）とする。ここで、座標（ｕ'，ｖ'）を通り、傾きがθである直線５０４上の座標（ｕ，ｖ）は、以下の式（１）の関係を満たす。

濃淡画像上の制御点の位置は、計測対象となる物体の位置及び姿勢により変化し、計測対象となる物体の位置及び姿勢の自由度は６自由度である。すなわち、６次元ベクトルｓは、計測対象となる物体の位置を表す３つの要素と、姿勢を表す３つの要素とからなる。姿勢を表す３つの要素は、例えばオイラー角や、方向が原点を通る回転軸を表してノルムが回転角を表す３次元ベクトルなどによって表現される。位置及び姿勢により変化する制御点の座標（ｕ，ｖ）は座標（ｕ₀，ｖ₀）の近傍で１次のテイラー展開によって以下の式（２）を用いて近似することができる。但しΔｓ_i（ｉ＝１，２，・・・，６）は６次元ベクトルｓの各成分の微小変化を表す。

位置及び姿勢の概略値と実際の位置及び姿勢との差が僅かであると仮定すると、正しい６次元ベクトルｓによって得られる濃淡画像上の制御点の位置は、式（１）が表す直線上にあると仮定できる。また、式（２）によって近似されるｕ、ｖを式（１）に代入することにより、以下の式（３）に示す観測方程式が得られる。

以上のように、濃淡画像処理部１５０により式（３）の観測方程式が、対応付けが行われたすべての制御点について立式される。

また、ステップＳ３０３においては、濃淡画像処理部１５０により濃淡画像への処理が行われている最中に、並行して距離計測用画像の撮影も行う。本実施形態では、空間コード化法により距離計測を行うことを想定しているため、異なるコードパターンを照射した複数の画像を撮影する必要がある。複数の画像を撮影するのは多くの時間がかかるため、濃淡画像への処理と並行して距離計測用の画像を撮影し、装置全体として高速化する。

距離計測用画像の撮影手順としては、まず、照明部１２０は、図６に示すコードパターンを計測対象となる物体に順次照射する。図６に示すコードパターンは、６ビットのグレイコードパターンを示している。照明部１２０はパターン１から順に各コードパターンを照射し、撮像部１１０は各コードパターンが照射されるごとに距離計測用画像を撮影する。撮影された距離計測用画像は距離画像処理部１６０に入力される。そして、すべてのコードパターンに関する距離計測用画像が撮影されるまで、コードパターンを変更して計測対象となる物体にコードパターンを照射して撮影を行う。

次に、ステップＳ３０４において、距離画像処理部１６０は、ステップＳ３０３において撮影された距離計測用画像群から距離画像を生成し、さらに距離画像の各画素と３次元形状モデルの面との対応付け、及び観測方程式の立式を行う。

まず、距離計測用画像群から距離画像を生成するために、各距離計測用画像に対して２値化処理を施して各画素を２値化する。２値化処理の結果より、各画素にはｎビットのコードが割り当てられる。なお、ビット数は利用するコードパターンの数となり、本実施形態では６ビットであるものとする。このコードを２進コードに変換して照明パターンの画像座標と対応づけを行い、その対応関係と前述の構成パラメータとをもとに画素ごとの奥行き値を計算して距離画像を生成する。距離画像の各画素に対し画素位置に対応する視線ベクトルに奥行き値を乗算することにより、基準座標系における３次元座標を持つ３次元点群に変換する。

次に、３次元形状モデルの面と距離画像から得られた３次元点群との対応に基づいて位置及び姿勢を算出するための観測方程式を立式する処理について説明する。まず、３次元点群の各点と３次元形状モデルの面との対応付けを行う。この処理では、位置及び姿勢の概略値に基づいて３次元形状モデルもしくは３次元点群を座標変換し、３次元点群中の各点につき、３次元空間における距離が最小となる３次元形状モデルの面を対応付ける。

基準座標系における３次元点群の点の座標は、計測対象となる物体の位置及び姿勢を表す６次元ベクトルｓによって計測対象となる物体の座標系における３次元座標（ｘ，ｙ，ｚ）に変換される。ここで、位置及び姿勢の概略値によって、３次元点群中のある点が計測対象となる物体の座標（ｘ₀，ｙ₀，ｚ₀）に変換されるものとする。３次元座標（ｘ，ｙ，ｚ）は計測対象となる物体の位置及び姿勢により変化するものであり、概略値から算出される３次元座標（ｘ₀，ｙ₀，ｚ₀）の近傍で１次のテイラー展開によって以下の式（４）により近似できる。

ここで、３次元点群中のある点に対応付けられた３次元形状モデルの面の、計測対象となる物体座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ²＋ｂ²＋ｃ²＝１、ａ，ｂ，ｃ，ｅは定数）とする。また、正しい６次元ベクトルｓによって変換される３次元座標（ｘ，ｙ，ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅを満たすものと仮定する。これにより、前述の式（４）を平面の方程式に代入すると以下の式（５）が得られる。

以上のように、距離画像処理部１６０により式（５）に表される観測方程式が、対応付けが行われたすべての３次元点群について立式される。なお、ステップＳ３０３において、距離計測用画像の撮影が終了しているが、濃淡画像処理部１５０による処理が完了していない場合には、濃淡画像処理部１５０による処理の完了を待たずに、ステップＳ３０４の処理を開始するものとする。これにより、装置全体として高速化する。

ステップＳ３０３における濃淡画像処理部１５０による処理、及びステップＳ３０４における距離画像処理部１６０による処理が完了すると、次に、ステップＳ３０５において、位置姿勢算出部１７０は、計測対象となる物体の位置姿勢を算出する。この処理においては、ステップＳ３０３及びステップＳ３０４において立式された観測方程式をもとに、位置姿勢の補正値を算出し、位置姿勢の概略値を補正する。前述した式（３）及び式（５）は６次元ベクトルｓの各成分の微小変化であるΔｓ_i（ｉ＝１，２，・・・，６）についての方程式であるため、以下の式（６）のような補正値Δｓ_iに関する線形連立方程式を立てることができる。

この連立方程式を解くことによって、個々の位置姿勢の補正値Δｓ_iが得られる。この補正値Δｓ_iを用いて位置及び姿勢の概略値を補正することによって最終的な物体の位置及び姿勢を算出する。

ここで、式（６）の右辺の値は、エッジ（ｄ_i−ｒ_i）については画像上での符号付距離であり、３次元点群（ｅ_i−ｑ_i）については３次元空間中での符号付距離であるため、尺度が一致していない。そこで、画像上での誤差にエッジの奥行きを乗じることにより３次元空間中の誤差に近似する。具体的には、エッジについての方程式の両辺に、エッジの奥行きを乗じることによって尺度を３次元空間中の距離に統一する。２次元画像からは奥行き情報を得ることはできないため、エッジの奥行きは何らかの近似的な方法により求める必要がある。本実施形態では、位置及び姿勢の概略値によって算出された各制御点の奥行きを、画像上の誤差に乗じることによって３次元空間中の誤差に変換する。

そして、式（６）の両辺に対して行列Ｊの一般化逆行列（Ｊ^T・Ｊ）^-1・Ｊ^Tを乗じることにより位置及び姿勢の補正値Δｓを求め、位置及び姿勢の概略値を補正する。なお、図３に示した手順によれば、位置及び姿勢の補正は一度しか行われないが、補正された位置及び姿勢の概略値をもとに、ステップＳ３０３及びＳ３０４において対応付け処理をやり直し、より精度を高めるために位置及び姿勢の補正を反復してもよい。

以上のように本実施形態によれば、まず濃淡画像を撮影し、その後、距離計測用画像を撮影しながら濃淡画像に対する処理を並行して行うようにしたので、位置及び姿勢の概略値が得られる場合の位置姿勢の計測をより高速に行うことができる。

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

また、本実施形態では、距離計測用の画像を撮影している最中に、特徴検出や対応付け、観測方程式などの濃淡画像に対する処理を並行して行って位置姿勢の計測を高速化した。しかしながら、濃淡画像に対する処理はこれに限るものではなく、前述の濃淡画像上の特徴に対して立式された観測方程式を用いた位置及び姿勢の算出まで行ってもよい。このようにして算出された位置及び姿勢を概略値とすることにより、距離画像と３次元形状モデルとの対応付けの精度を向上させることができる。その結果、誤対応が減るため、濃淡画像の情報から得られた位置及び姿勢の精度を、距離画像を用いて向上させることができる。

（第２の実施形態）
第１の実施形態では、物体の位置及び姿勢の概略値が既知である場合に、濃淡画像上の特徴に３次元形状モデルを当てはめるのに必要な処理を、距離計測用画像の撮影と並行して行って計測の高速化を図った。本実施形態では、物体の位置及び姿勢の概略値が未知である場合に、濃淡画像をもとに位置及び姿勢の概略値を推定する例について説明する。位置及び姿勢が未知な状態から発見的に位置及び姿勢を推定する処理は一般的に時間が多くかかる処理であるため、距離計測用画像の撮影と並行して発見的な物体の位置及び姿勢の推定を行い、計測装置全体として高速化させる。

図７は、本実施形態における位置姿勢計測装置７００の機能構成例を示すブロック図である。
図７に示すように、位置姿勢計測装置７００は、撮像部７１０、照明部７２０、モデル保存部７３０、濃淡画像処理部７５０、距離画像処理部７６０、及び位置姿勢算出部７７０を備えている。なお、濃淡画像処理部７５０以外の各部の機能は第１の実施形態の位置姿勢計測装置１００と同一であるため、説明を省略する。

第１の実施形態では、濃淡画像処理部１５０は位置及び姿勢の概略値が既知であり、濃淡画像上の特徴（エッジ）に３次元形状モデルの線分を当てはめるのに必要な処理を行っていた。これに対して本実施形態では、濃淡画像処理部７５０は、濃淡画像をもとに位置及び姿勢が未知な状態から発見的な位置及び姿勢の推定を行う。

図８は、本実施形態において、位置及び姿勢を計測する処理手順の一例を示すフローチャートである。まず、ステップＳ８０１の処理は、図３のステップＳ３０２の処理と同じであるため説明を省略する。

次に、ステップＳ８０２においては、距離計測用画像の撮影を行う。距離計測用画像の撮影手順については第１の実施形態と同様である。また、距離計測用画像の撮影を行っている最中に並行して、濃淡画像処理部７５０は、濃淡画像に基づいて、計測対象となる物体の位置及び姿勢を発見的に推定する。推定方法は、濃淡画像に基づく方法であればいかなる方法であってもよい。例えば、非特許文献８に開示されているパターンマッチングによる方法を用いてもよい。

非特許文献８に記載の方法は、計測対象となる物体の３次元形状モデルをもとに様々な視点位置から見た画像のデータベースを予め生成し、入力画像に最も適合する画像を探索することによって発見的に位置及び姿勢を推定する。この手法は、多数の画像と入力画像とをマッチングする処理が必要であり多くの時間がかかる処理である。この処理を、距離計測用画像の撮影と並行して行うことにより全体としての処理が高速化される。

また、多数の画像をもとに物体の位置姿勢の識別器を事前に学習しておき、入力画像を識別器にかけることによって発見的に物体の位置及び姿勢を推定してもよい。その他、濃淡画像から得られる特徴に基づく仮説検証や投票などによって物体の位置及び姿勢を推定してもよい。この処理によって推定された物体の位置及び姿勢の情報は、ステップＳ８０３及びＳ８０４において、位置及び姿勢の概略値として利用される。

ステップＳ８０３においては、濃淡画像処理部７５０及び距離画像処理部７６０は、３次元形状モデルとの対応付けや観測方程式の立式など、それぞれ第１の実施形態で説明した濃淡画像及び距離画像に対する処理を行う。なお、濃淡画像に対する処理は、ステップＳ８０２において位置及び姿勢の概略値の推定が終了した段階で可能であるため、距離計測用画像の撮影が完了する前に開始してもよい。ステップＳ８０４においては、図３のステップＳ３０５と同様であるため、説明は省略する。

以上のように本実施形態によれば、物体の位置及び姿勢の概略値が未知である場合に、まず、濃淡画像を撮影し、距離計測用画像の撮影と並行して濃淡画像に基づいて発見的に物体の位置姿勢を推定するようにした。これにより、位置姿勢の概略値が未知であっても高速に位置姿勢を計測することができる。

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

１１０ 撮像部
１２０ 照明部
１３０ モデル保存部
１５０ 濃淡画像処理部
１６０ 距離画像処理部
１７０ 位置姿勢算出部

Claims (13)

1. 距離を計測するためのパターンを物体に照射する照明手段と、
   前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、
   前記物体の３次元形状を表すモデルを保持する保持手段と、
   前記撮像手段による前記パターンが照射された物体の撮影と並行して、前記撮像手段によって撮影された２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理手段と、
   前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理手段と、
   前記第１の処理手段及び前記第２の処理手段による対応付けに基づいて前記物体の位置及び姿勢を導出する導出手段とを有することを特徴とする位置姿勢計測装置。
2. 前記第１の処理手段は、前記２次元画像から検出した特徴と前記モデルとを対応付けた方程式を生成することを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記物体の位置及び姿勢の概略値の情報を入力する入力手段をさらに有し、
   前記第１の処理手段は、前記入力手段によって入力された概略値に基づいて前記２次元画像から検出した特徴と前記モデルとを対応付けることを特徴とする請求項１又は２に記載の位置姿勢計測装置。
4. 前記第１の処理手段による処理が終了する前に前記撮像手段により前記パターンが照射された画像群の撮影が終了した場合に、前記第１の処理手段による処理と並行して前記第２の処理手段による処理を開始することを特徴とする請求項１〜３の何れか１項に記載の位置姿勢計測装置。
5. 前記特徴はエッジであることを特徴とする請求項１〜４の何れか１項に記載の位置姿勢計測装置。
6. 前記特徴は特徴点であることを特徴とする請求項１〜４の何れか１項に記載の位置姿勢計測装置。
7. 距離を計測するためのパターンを物体に照射する照明手段と、
   前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、
   前記物体の３次元形状を表すモデルを保持する保持手段と、
   前記撮像手段による前記パターンが照射された物体の撮影と並行して前記撮像手段によって撮影された２次元画像から前記物体の位置及び姿勢を推定し、さらに、前記２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理手段と、
   前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理手段と、
   前記第１の処理手段及び前記第２の処理手段による対応付けに基づいて前記物体の位置及び姿勢を導出する導出手段とを有することを特徴とする位置姿勢計測装置。
8. 前記撮像手段は、前記パターンが照射された物体の画像群を撮影する前に前記２次元画像を撮影することを特徴とする請求項１〜７の何れか１項に記載の位置姿勢計測装置。
9. 前記撮像手段は、前記パターンが照射された物体の画像群の撮影を完了する前に前記２次元画像を撮影することを特徴とする請求項１〜７の何れか１項に記載の位置姿勢計測装置。
10. 距離を計測するためのパターンを物体に照射する照明手段と、前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、前記物体の３次元形状を表すモデルを保持する保持手段とを備えた位置姿勢計測装置の位置姿勢計測方法であって、
    前記撮像手段による前記パターンが照射された物体の撮影と並行して、前記撮像手段によって撮影された２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理工程と、
    前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理工程と、
    前記第１の処理工程及び前記第２の処理工程による対応付けに基づいて前記物体の位置及び姿勢を導出する導出工程とを有することを特徴とする位置姿勢計測方法。
11. 距離を計測するためのパターンを物体に照射する照明手段と、前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、前記物体の３次元形状を表すモデルを保持する保持手段とを備えた位置姿勢計測装置の位置姿勢計測方法であって、
    前記撮像手段による前記パターンが照射された物体の撮影と並行して前記撮像手段によって撮影された２次元画像から前記物体の位置及び姿勢を推定し、さらに、前記２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理工程と、
    前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理工程と、
    前記第１の処理工程及び前記第２の処理工程による対応付けに基づいて前記物体の位置及び姿勢を導出する導出工程とを有することを特徴とする位置姿勢計測方法。
12. 距離を計測するためのパターンを物体に照射する照明手段と、前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、前記物体の３次元形状を表すモデルを保持する保持手段とを備えた位置姿勢計測装置を制御するためのプログラムであって、
    前記撮像手段による前記パターンが照射された物体の撮影と並行して、前記撮像手段によって撮影された２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理工程と、
    前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理工程と、
    前記第１の処理工程及び前記第２の処理工程による対応付けに基づいて前記物体の位置及び姿勢を導出する導出工程とをコンピュータに実行させることを特徴とするプログラム。
13. 距離を計測するためのパターンを物体に照射する照明手段と、前記物体の２次元画像を撮影するとともに、前記照明手段によって異なるパターンにより前記物体が照射された画像群を撮影する撮像手段と、前記物体の３次元形状を表すモデルを保持する保持手段とを備えた位置姿勢計測装置を制御するためのプログラムであって、
    前記撮像手段による前記パターンが照射された物体の撮影と並行して前記撮像手段によって撮影された２次元画像から前記物体の位置及び姿勢を推定し、さらに、前記２次元画像から特徴を検出して、前記保持手段に保持されているモデルと対応付ける第１の処理工程と、
    前記パターンが照射された画像群から距離画像を生成し、前記生成した距離画像の画素と前記保持手段に保持されているモデルとを対応付ける第２の処理工程と、
    前記第１の処理工程及び前記第２の処理工程による対応付けに基づいて前記物体の位置及び姿勢を導出する導出工程とをコンピュータに実行させることを特徴とするプログラム。

Images