JP5938201B2 - 位置姿勢計測装置、その処理方法及びプログラム - Google Patents

Description

本発明は、位置姿勢計測装置、その処理方法及びプログラムに関する。

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

ここで、物体の位置姿勢を計測する方法として、カメラで撮影した濃淡画像やカラー画像上から検出される画像特徴や、距離センサで計測して得られた距離データに対して物体の３次元形状モデルをあてはめるモデルフィッティングによる方法が知られている。

モデルフィッティングによる位置姿勢の計測（推定）では、モデルの幾何特徴と画像上の幾何特徴とを利用して計測対象物体の位置姿勢を表す複数のパラメータを推定する。このとき、あるパラメータの推定に寄与する幾何特徴が他のパラメータの推定に寄与する幾何特徴に比べて著しく少ないと、そのパラメータをロバストに推定できないという問題がある。

この問題に対して、非特許文献１では、寄与する推定パラメータごとに各幾何特徴を分類して用いる方法が開示されている。この方法では、位置姿勢の計測時に、各幾何特徴のヤコビアンを算出し、ヤコビアンが類似しているものは同一パラメータに寄与するものとしてまとめて分類する。そして、このようにして分類した幾何特徴を、それぞれ寄与するパラメータの算出のみに分けて使用することで、特定のパラメータに寄与する幾何特徴が少ない場合においても、ロバスト且つ効率良く推定を行なっている。

Kemp C. , Drummond T. , "Dynamic measurement clustering to aid real time tracking", Computer Vision, 2005. ICCV 2005. Tenth IEEE International Conference on, Page(s): 1500-1507 Vol. 2

ここで、他のパラメータに寄与する幾何特徴に比べて、特定のパラメータに寄与する幾何特徴が少ない物体の一例について挙げてみる。例えば、工業製品に使われる部品の中で考えると、回転対称に近い形状で、且つその回転成分を一意に決定するための幾何特徴が微小な切り欠きや突起しかない部品が挙げられる。

このような部品では、位置姿勢を規定する６パラメータを同時に推定しようとすると、他の成分に比べて、特定の成分（この場合、物体の回転対称性を定義する軸まわりの姿勢）の推定に必要な情報が不足してしまう。

そのため、この特定の成分を推定するのが困難となり、その結果、位置姿勢のロバスト性や精度が低くなるという問題がある。なお、上述した非特許文献１の方法では、カメラを基準とした座標系で規定される位置姿勢成分に基づいて幾何特徴を分類するため、物体（３次元形状モデル）を基準とした座標系で規定される特定の位置姿勢の成分に寄与する幾何特徴を分離することができない。

本発明は、上記課題に鑑みてなされたものであり、情報の不足により物体基準で定義される特定の位置姿勢の成分（パラメータ）を推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえるようにした技術を提供することを目的とする。

上記課題を解決するため、本発明の一態様は、計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、
前記物体の３次元形状モデルを保持するモデル保持手段と、
前記３次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する特定パラメータ設定手段と、
前記３次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータの算出時に与える影響を示す感度を算出する感度算出手段と、
前記３次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段と
を具備し、
前記位置姿勢算出手段は、
前記感度算出手段により算出された感度に基づいて前記特定のパラメータの算出時の前記３次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
ことを特徴とする。

本発明によれば、情報の不足により物体基準で定義される特定の位置姿勢の成分を推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえる。

本発明の一実施の形態に係わる位置姿勢計測装置１０の構成の一例を示す図。 ３次元形状モデルの定義方法の一例を示す図。 位置姿勢計測装置１０の処理の流れの一例を示すフローチャート。 計測対象物体の形状とその座標系の一例を示す図。 図３のＳ１０６の処理の流れの一例を示すフローチャート。 エッジ検出方法の概要の一例を示す図。 ３次元形状モデルの線分の投影像とエッジとの関係の一例を示す図。 図３のＳ１０７の処理の流れの一例を示すフローチャート。 エッジ検出方法の概要の一例を示す図。 変形実施形態に係わる計測対象物体の形状とその座標系の一例を示す図。 距離画像と３次元形状モデルとの対応付け処理の概要を示す図。

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

（実施形態１）
まず、実施形態１について説明する。実施形態１においては、特定の軸に対してほぼ回転対称であり、且つその軸回りの回転を決めるための特徴が少ない形状を有する物体の位置姿勢の計測（推定）を行なう場合について説明する。

ここで、実施形態１に係わる計測対象物体の位置姿勢の計測（推定）方法の概要について簡単に説明する。

実施形態１においては、２次元画像上のエッジに対してモデルのエッジをあてはめることにより６自由度の位置姿勢を算出する。但し、上述した形状の物体では、６自由度の位置姿勢を同時に算出すると、物体の軸回りの回転を決定する特徴が少ないため、回転軸まわりの位置姿勢パラメータが正しく求まらない。

そこで、実施形態１においては、まず、６自由度の位置姿勢を算出し（前段の処理）、その後、軸回りの回転成分の推定に影響するエッジのみを利用して、軸回りの回転成分のみ別途算出を行なう（後段の処理）。前段処理後には、軸回りの回転成分を除き、正しい位置姿勢が算出されていることが期待される。そのため、後段の処理では、軸回りの回転成分の推定に影響力のある３次元形状モデルの幾何特徴のみを利用することで、ロバスト且つ効率良く位置姿勢の推定を行なう。

図１は、本発明の一実施の形態に係わる位置姿勢計測装置１０の構成の一例を示す図である。

位置姿勢計測装置１０は、３次元情報（本実施形態においては、物体の位置姿勢）の計測対象となる物体（以下、計測対象物体、又は単に物体と呼ぶ場合もある）の位置姿勢を計測する。位置姿勢計測装置１０には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ（Central Processing Unit）等の主制御手段、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていても良い。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。

ここで、位置姿勢計測装置１０は、モデル保持部１１と、特定パラメータ指定部１２と、感度算出部１３と、撮像部１４と、概略値入力部１５と、位置姿勢算出部１８とを具備して構成される。

モデル保持部１１は、計測対象物体の３次元幾何モデルデータ（以下、３次元形状モデルと呼ぶ）を保持する。ここで、図２（ａ）及び図２（ｂ）を用いて、本実施形態に係わる３次元形状モデルの定義方法の一例について説明する。３次元形状モデルは、点の集合や各点を結んで構成される線分の集合によって定義される。

本実施形態に係わる３次元形状モデルは、図２（ａ）に示すように、点Ｐ１〜点Ｐ１５の１５点から構成される。点Ｐ１〜点Ｐ１５は、３次元座標値により表される。また、本実施形態に係わる３次元形状モデルは、図２（ｂ）に示すように、線分Ｌ１〜Ｌ２１により構成されている。線分Ｌ１〜Ｌ２１は、線分を構成する点のＩＤ（識別子）により表される。また、ここでは図示を省略するが、３次元形状モデルは、面の情報も保持する。各面は、当該各面を構成する点のＩＤにより表される。この図２（ａ）及び図２（ｂ）に示す３次元形状モデルでは、物体を構成する８つの面の情報が保持されている。

なお、図２（ａ）及び図２（ｂ）に示す３次元形状モデルにおいて、ｐ１９〜Ｌ２１が軸回りの回転成分を一意に決める特徴となる。３次元形状モデルは、特定パラメータ指定部１２による第２の位置姿勢パラメータの指定や、感度算出部１３によるモデル幾何特徴の感度算出に用いられる。その他、第１の位置姿勢算出部１６による第１の位置姿勢パラメータの算出や、第２の位置姿勢算出部１７による第２の位置姿勢パラメータの算出にも用いられる。

特定パラメータ指定部１２は、第２の位置姿勢算出部１７による算出対象となる位置姿勢パラメータの成分（第２の位置姿勢パラメータ）を指定する。以下の説明においては、第１の位置姿勢算出部１６及び第２の位置姿勢算出部１７により推定される位置姿勢パラメータの成分をそれぞれ第１の位置姿勢パラメータ及び第２の位置姿勢パラメータと呼ぶ。具体的には、３次元形状モデルの座標系（モデル座標系）を基準とした物体の位置姿勢（６パラメータ）を第１の位置姿勢パラメータとする。更に、６パラメータのうち、そのパラメータの推定に寄与する幾何特徴が物体形状において少ないために算出が困難なパラメータを第２の位置姿勢パラメータとする。本実施形態においては、計測対象となる物体として、ほぼ回転対象で、且つその回転軸周りの姿勢成分を決める幾何特徴が少ない形状を有する物体の位置姿勢の計測（推定）を行なう場合について説明する。そのため、物体の回転軸周りの姿勢成分を第２の位置姿勢パラメータとして求める。

感度算出部１３は、特定パラメータ指定部１２により指定された第２の位置姿勢パラメータに対する感度を算出する。この感度は、３次元形状モデルを構成する各幾何特徴（各エッジ線分）に対応して算出される。本実施形態で用いるエッジを利用した位置姿勢の計測では、３次元形状モデルのエッジと、それに対応付けられた２次元画像上の点との間の距離を最小化するようにして物体の位置姿勢の推定を行なう。従って、第２の位置姿勢パラメータを微小変化させたときの線分の移動方向と、線分自身の持つ方向とが平行となる場合、その線分に属するエッジは、第２の位置姿勢パラメータの推定に寄与しない。

そこで、感度算出部１３においては、第２の位置姿勢パラメータを微小変化させたときの３次元形状モデルの線分の移動方向と、線分自身の持つ方向との差を感度として算出する。感度算出部１３により算出された感度は、第２の位置姿勢算出部１７による第２の位置姿勢パラメータの算出に際して、各幾何特徴の寄与度を設定する際に用いられる。

撮像部１４は、例えば、カメラであり、計測対象物体の２次元画像を撮像する。カメラの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、例えば、「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」に開示された方法により事前に校正しておけば良い。

概略値入力部１５は、位置姿勢計測装置１０に対する計測対象物体の位置姿勢の概略情報（以下、概略値と呼ぶ）を入力する。位置姿勢計測装置１０には、位置姿勢の計測の基準となる３次元の座標系（基準座標系）が予め規定されているものとする。本実施形態においては、撮像部１４の中心を原点とし、画像の水平方向をｘ軸とし、垂直方向をｙ軸とし、撮像部１４の光軸をｚ軸とした座標系を基準座標系とする。これと同様に、物体の３次元形状モデルにも、位置姿勢の計測の基準となる３次元の座標系（モデル座標系、すなわち、物体を基準とした物体座標系）が予め規定されているものとする。なお、位置姿勢計測装置１０に対する物体の位置姿勢とは、基準座標系における物体の位置姿勢を示す。

本実施形態においては、位置姿勢計測装置１０は、時間軸方向に連続して計測を行なうものとして、位置姿勢計測装置１０が当該物体から過去（例えば、直前）に計測した計測値を概略値として用いる。なお、物体の位置姿勢の概略値は、必ずしもこのような値である必要はない。例えば、過去に計測した物体の位置姿勢の計測値に基づいて、物体の位置姿勢の変化量を推定し、過去の位置姿勢と推定された変化量とから現在の位置姿勢を予測することによりこのような概略値を求めるようにしても良い。また、例えば、物体が置かれているおおよその位置や姿勢が予めわかっている場合にはその値を概略値として用いても良い。この概略値は、第１の位置姿勢算出部１６による第１の位置姿勢パラメータの算出に用いられる。

位置姿勢算出部１８は、計測対象物体の位置姿勢を計測（推定）する機能を果たし、第１の位置姿勢算出部１６と、第２の位置姿勢算出部１７とを具備して構成される。

第１の位置姿勢算出部１６は、概略値入力部１５により入力された概略値に基づいて、モデル保持部１１に保持されたモデル幾何特徴と、撮像部１４により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の位置姿勢を表す第１の位置姿勢パラメータ（本実施形態においては、６パラメータ）を算出する。以下、第１の位置姿勢パラメータの算出により表される物体の位置姿勢を第１の位置姿勢と呼ぶ。

第２の位置姿勢算出部１７は、第１の位置姿勢に基づいて、モデル保持部１１に保持されたモデル幾何特徴と、撮像部１４により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の第２の位置姿勢パラメータを算出する。このとき、第２の位置姿勢算出部１７は、感度算出部１３により算出された感度に基づき、第２の位置姿勢パラメータの推定に影響し易い幾何特徴の寄与度を上げて第２の位置姿勢パラメータの算出を行なう。これにより、ロバスト且つ効率良く算出を行なうことができる。以下、第２の位置姿勢パラメータの算出により推定された物体の位置姿勢を第２の位置姿勢と呼ぶ。

ここで、図３を用いて、図１に示す位置姿勢計測装置１０における物体の位置姿勢の計測処理の流れの一例について説明する。なお、位置姿勢計測装置１０は、図３に示すＳ１０４〜Ｓ１０７の一連の処理を繰り返すことで、時間軸方向に連続して物体の位置姿勢の計測を行なう。

［Ｓ１０１］
位置姿勢計測装置１０は、まず、モデル保持部１１に保持された３次元形状モデルの各幾何特徴を取得する。より具体的には、特定パラメータ指定部１２において、３次元形状モデルの各幾何特徴を入力する。

［Ｓ１０２］
位置姿勢計測装置１０は、特定パラメータ指定部１２において、３次元形状モデルの各幾何特徴に基づいて、第２の位置姿勢パラメータを指定する。ここで、図４（ａ）には、本実施形態において対象とする物体の具体例とその座標系とが示される。本実施形態においては、図４（ａ）に示すように、物体が、特定軸に対してほぼ回転対称な形状を有することを前提とする。特定パラメータ指定部１２においては、まず、この物体の回転対称性を定義する軸（以下、モデル中心軸と呼ぶ）を指定する。

モデル中心軸の指定は、例えば、ユーザからの操作部（例えば、マウスやキーボード）を介した入力に基づいて行なう。具体的には、図４（ａ）に示すような３次元形状モデルの場合、当該モデルの回転対称な面上から対面する２点ａ、ｂ及びｃ、ｄの２組をユーザからの入力として取得する。このとき、線分ａｂと線分ｃｄとの交点を通り、且つ四角形ａｃｂｄに垂直な方向を持つベクトルをモデル中心軸として算出する（図４（ｂ）参照）。

但し、モデル中心軸の指定方法は、このような方法に限られない。例えば、三角柱のような回転対称形状の場合には、回転対称な三角形の３頂点をユーザからの入力として取得し、当該三角形の重心を通り、且つ三角形に垂直なベクトルをモデル中心軸として指定しても良い。或いは、モデル面上で回転対称性のある面（図４（ａ）の場合、八角形）をユーザからの入力として取得し、その重心を通り、且つ入力された面に垂直なベクトルをモデル中心軸としても良い。その他、モデル形状の回転対称性を定義する回転軸を算出できるのであれば、いかなる方法を用いても構わない。

モデル中心軸の指定が済むと、特定パラメータ指定部１２は、モデル中心軸に対して互いに直交する２つの軸を設定し、モデル中心軸と、当該設定した２つの軸とが座標軸となるようにモデル座標系を変換して新しいモデル座標系とする（図４（ｃ））。ここでは、新しいモデル座標系において、ｙ軸（実線）がモデル中心軸と一致するようにする。

なお、モデル座標系の変換方法は、このような方法に限られない。例えば、モデル座標系のｘ軸がモデル中心軸と一致するように変換しても良い。その他、モデル中心軸がモデル原点を通るようにモデル座標系が変換されるのであれば、いかなる変換であっても構わない。以降の処理は、ｙ軸がモデル中心軸と一致するように変換した、新しいモデル座標系を用いて行なうものとする。基準座標系に対する物体の位置姿勢を６次元ベクトルｓ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］^Ｔで表す。

ここで、ｔｘ、ｔｙ、ｔｚは、それぞれｘ、ｙ、ｚ軸方向の位置（並進）成分を表し、θ、φ、ψは、それぞれｘ、ｙ、ｚ軸に対する姿勢（回転）成分を表す。この６パラメータが、第１の位置姿勢パラメータとなる。この各回転成分の変換順序を考慮することで、座標変換において、物体軸回りの回転を分離する。より具体的には、モデル座標系上の特定軸回りの回転を表す行列のみを分離して、モデル座標系に直接積算することで、モデル座標系における回転を表現する。続いて、特定パラメータ指定部１２は、モデル中心軸に対する回転成分を第２の位置姿勢パラメータとして指定する。本実施形態においては、ｙ軸がモデル中心軸と一致するため、ｙ軸回りの回転成分であるφが第２の位置姿勢パラメータとして指定される。

［Ｓ１０３］
位置姿勢計測装置１０は、感度算出部１３において、第２の位置姿勢パラメータを算出する際の各モデル線分（３次元形状モデルにおける各幾何特徴）の感度を算出する。具体的には、３次元形状モデル上の２つの３次元点ｐｓ、ｐｅとを結ぶ線分Ｌｊ（ｊ＝１，２，・・・，Ｎ）（Ｎ：モデル上の線分の数）の感度ｗｊを次のように算出する。

まず、点ｐｓをモデル中心軸に対してφだけ微小（例えばφ＝０．０１ ｒａｄｉａｎ）回転させた点をｐｓ’で表すものとする。モデル中心軸回りにφ微小回転させる回転行列をＲφと表すと、ｐｓ’は、以下の「数１」で示される。
ｐｓ’＝Ｒ_φｐｓ （数１）

ここで、点ｐｓから点ｐｓ’へ向かう長さ１の３次元ベクトルをｖｅｃ１とする。更に、点ｐｓから点ｐｅへ向かう長さ１の３次元ベクトルをｖｅｃ２とする。このとき、線分Ｌｊの感度ｗｊを、ｖｅｃ１とｖｅｃ２との内積ｖｅｃ１・ｖｅｃ２を用いて、以下の「数２」により定義する。
ｗｊ＝１−｜ｖｅｃ１・ｖｅｃ２｜ （数２）

「数２」により算出された各幾何特徴の感度ｗｊは、０〜１の間の値を持つ。位置姿勢計測装置１０においては、この感度ｗｊを各モデル線分Ｌｊと組にして保持する。なお、Ｓ１０１〜Ｓ１０３の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。

［Ｓ１０４］
位置姿勢計測装置１０は、撮像部１４において、計測対象物体の２次元画像を撮像し、第１の位置姿勢算出部１６及び第２の位置姿勢算出部１７に入力する。

［Ｓ１０５］
位置姿勢計測装置１０は、概略値入力部１５において、位置姿勢計測装置１０に対する物体の位置姿勢の概略値を入力する。本実施形態においては、計測対象物体から過去（例えば、直前）に計測された計測値を概略値として入力する。

［Ｓ１０６］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、２次元画像の情報に基づいて、第１の位置姿勢を算出する。本実施形態においては、２次元画像上の画像特徴（エッジ）に３次元形状モデル中の線分の投影像を当てはまめるようにすることで第１の位置姿勢パラメータを算出し、それにより、第１の位置姿勢を算出する。

ここで、本実施形態においては、２次元画像上で検出されたエッジと、概略値に基づいて当該２次元画像上に投影される３次元形状モデルの線分との距離の二乗和が最小になるべく、計測対象物体の位置姿勢のパラメータｓの最適化を行なう。

このとき、位置姿勢に基づく、モデル座標系（物体座標系）の３次元点Ｘ_ｍ＝［ｘ_ｍ，ｙ_ｍ，ｚ_ｍ］^Ｔから基準座標系の３次元点Ｘ_ｃ＝［ｘ_ｃ，ｙ_ｃ，ｚ_ｃ］^Ｔへの座標変換は、「数３」のように表される。

Ｘ_ｃ＝ＲＸ_ｍ＋Ｔ （数３）

但し、Ｒは物体の姿勢を表す３つの成分θ、φ、ψで表現される３×３行列であり、Ｔはモデルの位置を表す３つの成分から成る３次元ベクトルＴ＝［ｔｘ，ｔｙ，ｔｚ］^Ｔである。更に、概略位置姿勢ｓ_０に対する物体の位置姿勢の微小変化量を６次元ベクトルΔｓ＝［Δｔｘ，Δｔｙ，Δｔｚ，Δθ，Δφ，Δψ］^Ｔとし、微小変化量Δｓを求めて、各成分を補正する処理を繰り返す。これにより、計測対象物体の位置姿勢の算出を行なう。

ここで、図５を用いて、Ｓ１０６の処理における第１の位置姿勢パラメータの算出処理の詳細について説明する。

［Ｓ２０１］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、初期化処理を行なう。この初期化処理では、例えば、図３のＳ１０５の処理で得られた計測対象物体の概略的な位置姿勢を示す概略値を入力する。

（Ｓ２０２）
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、Ｓ１０４の処理で入力された２次元画像上から画像特徴を検出し、当該画像特徴と、３次元形状モデルのモデル幾何特徴との対応付けを行なう。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態においては、「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.」に開示された技術を用いてエッジの検出を行なう。このエッジ検出方法は、３次元形状モデル側から対応するエッジを探索する、トップダウン型の方法であるため、エッジの検出と対応付けとが一度に行なわれる。

ここで、図６を用いて、本実施形態に係わるエッジ検出方法の概要の一例について説明する。まず、計測対象物体が撮像された２次元画像に対して３次元形状モデルを投影する。この投影像（線分）は、Ｓ２０１の処理で入力された計測対象物体の位置姿勢の概略値と、校正済みのカメラの内部パラメータとに基づいて求められる。

次に、投影された各線分上において、等間隔に制御点を設定する。そして、投影線分の法線方向に平行で且つ制御点を通過する線分（以下、探索ライン）上からエッジを１次元探索する（図６（ａ））。エッジは、画素値の濃度勾配の極値として検出されるため、複数のエッジが検出される場合がある（図６（ｂ））。本実施形態においては、検出されるエッジのうち、最も濃度勾配の大きいものを対応するエッジとして検出する。

［Ｓ２０３］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、３次元形状モデルの線分上の制御点と２次元画像上のエッジとの対応関係に基づいて、計測対象物体の位置姿勢を算出するための係数行列・誤差ベクトルを算出する。

ここで、図７は、線分の投影像と検出されたエッジとの関係の一例を示す図である。この場合、画像の水平方向をｕ軸とし、垂直方向をｖ軸としている。ある制御点（投影された各線分を画像上で等間隔に分割した点）の画像上での位置を（ｕ_０，ｖ_０）と表し、当該制御点が属する線分の画像上での傾きを（ｕ軸に対する傾き）θと表す。傾きθは、線分の両端の３次元座標をｓの概略値（Ｓ_０）に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。線分の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、制御点と対応する対応点（エッジ）の座標を（ｕ’，ｖ’）とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線（図７の破線）上の点（ｕ，ｖ）は、
ｕｓｉｎθ−ｖｃｏｓθ＝ｄ （数５）
と表せる。
なお、θは定数とする。また、ｄ＝ｕ’ｓｉｎθ−ｖ’ｃｏｓθ（定数）である。

画像上における制御点の位置は、計測対象物体の位置姿勢ｓにより変化する。従って、制御点の座標（ｕ，ｖ）は、（ｕ_０，ｖ_０）の近傍での１次のテイラー展開によって「数６」のように近似できる。但し、Δｓ_ｉ（ｉ＝１，２，．．．，６）はｓの各成分の微小変化を表す。つまり、Δｓ_１＝Δｔｘ、Δｓ_２＝Δｔｙ、Δｓ_３＝Δｔｚ、Δｓ_４＝Δθ、Δｓ_５＝Δφ、Δｓ_６＝Δψである。

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

但し、ｒ＝ｕ_０ｓｉｎθ−ｖ_０ｃｏｓθ（定数）である。

「数７」の観測方程式は、対応付けが行なわれた全ての制御点について立式することができる。ここで、「数７」は、ｓの各成分の微小変化Δｓ_ｉ（ｉ＝１，２，・・・，６）についての方程式となっているため、「数８」に示すΔｓ_ｉに関する線形連立方程式を立式できる。

正規化画像座標系における画像座標ｕ、ｖは、それぞれｕ＝ｘ_ｃ／ｚ_ｃ、ｖ＝ｙ_ｃ／ｚ_ｃで表される。従って、「数３」に基づいて算出されたｕ、ｖから、左辺の行列の各要素を算出する。この線形連立方程式を「数９」のように表す。
Ｊ_１Δｓ＝Ｅ_１ （数９）
左辺のＪ_１が算出対象となる係数行列であり、右辺のＥ_１が誤差ベクトルである。

［Ｓ２０４］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、計測対象物体の位置姿勢の補正値Δｓを求める。すなわち、行列Ｊ_１の一般化逆行列（Ｊ_１ ^Ｔ・Ｊ_１）^−１・Ｊ_１ ^Ｔを用いて、「数９」を「数１０」のように解く。
Δｓ＝（Ｊ_１ ^Ｔ・Ｊ_１）^−１・Ｊ_１ ^ＴＥ_１ （数１０）

［Ｓ２０５］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、Ｓ２０４の処理で算出された補正値Δｓを用いて、概略値を補正する。
ｓ←ｓ＋Δｓ

［Ｓ２０６］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、Ｓ２０３の処理に戻る。なお、収束しているか否かは、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。

以上の処理によって、図３のＳ１０５の処理で入力された概略値が、２次元画像の情報を用いて更新される。本実施形態においては、図５に示す処理で最終的に算出された物体の位置姿勢を第１の位置姿勢とする。なお、Ｓ１０６（Ｓ２０１〜Ｓ２０６）において、第１の位置姿勢パラメータとして、位置姿勢の６自由度を算出したが、これに限られない。例えば、第１の位置姿勢パラメータとして、第２の位置姿勢パラメータを除く５自由度についてのみ算出するようにしても良い。なお、本実施形態では、概略位置姿勢（概略値）を更新することで第１の位置姿勢を算出する場合について説明したが、第１の位置姿勢の算出方法はこれに限らない。例えば、物体をさまざまな姿勢で観察した画像をテンプレートとして多数用意しておき、このテンプレートと観察画像とのマッチングにより第１の位置姿勢を算出しても良い。この他、第２の位置姿勢パラメータを除く５自由度が正しく推定できる方法であれば、いかなる方法であっても構わない。

［Ｓ１０７］
図３の説明に戻る。Ｓ１０６の処理で算出された第１の位置姿勢に関して、第２の位置姿勢パラメータφを除き、正しい位置姿勢が算出されていることが期待される。そこで、位置姿勢計測装置１０は、第１の位置姿勢を概略位置姿勢（計測対象物体の位置姿勢を示す概略値）として、第２の位置姿勢パラメータφを算出する。このとき、第２の位置姿勢パラメータφの推定に影響し易い幾何特徴を利用することで、ロバスト且つ効率良く推定を行なう。

第１の位置姿勢を計測対象物体の概略値とした場合、Ｓ１０６の処理と同様に、「数５」の関係が成り立つ。ここで、モデル座標系を基準としたｙ軸方向に対するΔφの微小回転を表す３×３行列をΔＲ_ｙとする。このとき、第１の位置姿勢からモデル座標系のｙ軸方向にΔφだけ微小回転させたときの、モデル座標系（物体座標系）の３次元点Ｘｍ＝［ｘ_ｍ，ｙ_ｍ，ｚ_ｍ］^Ｔから基準座標系の３次元点Ｘ_ｃ＝［ｘ_ｃ，ｙ_ｃ，ｚ_ｃ］^Ｔへの座標変換は、「数１１」のように表される。
Ｘ_ｃ＝ＲΔＲ_ｙＸ_ｍ＋Ｔ （数１１）

そこで、Ｓ１０７の処理では、微小変化量Δφを求めて、モデルのｙ軸回転方向の補正を行なう。これにより、計測対象物体の位置姿勢の算出を行なう。

図８を用いて、Ｓ１０７の処理における第２の位置姿勢パラメータの算出処理の詳細について説明する。この処理では、Ｓ１０３の処理で算出された３次元形状モデルの幾何特徴（各モデル線分）毎の感度に基づき、当該各モデル線分毎に寄与度を変えて最適化を行なう。

［Ｓ３０１］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、初期化処理を行なう。この初期化処理では、例えば、図３のＳ１０６の処理で得られた第１の位置姿勢を計測対象物体の概略的な位置姿勢を示す概略値として入力する。

［Ｓ３０２］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、Ｓ１０４の処理で入力された２次元画像上から画像特徴を検出し、当該画像特徴と、３次元形状モデルのモデル幾何特徴との対応付けを行なう。すなわち、図５のＳ２０２と同様の処理を行なう。なお、図５のＳ２０２の処理では、モデル線分を画像上に投影して得られる各制御点から、当該制御点を通り且つ投影された線分に直交する線分上で１次元のエッジ検出を行なっていた。これに対して、Ｓ３０２の処理では、第１の位置姿勢に関して、第２の位置姿勢パラメータφを除き、正しい位置姿勢が算出されていることが期待される。従って、第１の位置姿勢に基づいて、各制御点に対応する幾何特徴の存在しうる軌跡を算出したものをエッジ検出の探索範囲としても良い。具体的には、第１の位置姿勢を基準として第２の位置姿勢パラメータφのみ変化させた場合、各制御点の軌跡は、楕円軌跡上に乗る。そこで、その楕円軌跡上で画素値の濃度勾配の極値を探索することでエッジを検出する。

図９は、第１の位置姿勢を基準として、第２の位置姿勢パラメータφを変化させたときの線分上の制御点の軌跡を示している。この楕円軌跡上で、エッジの検出を行なうことで対応する画像上のエッジが探索される。

［Ｓ３０３］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、微小変化するパラメータをΔφのみに限定して、図５のＳ２０３と同様の処理を行なう。これにより、３次元形状モデル線分上の制御点と２次元画像上のエッジとの対応関係から、Δφに関する「数１２」のような線形連立方程式を立てることができる。

この線形連立方程式を「数１３」のように表す。
Ｊ_２Δφ＝Ｅ_２ （数１３）
左辺のＪ_２が算出対象となる係数行列であり、右辺のＥ_２が誤差ベクトルである。

［Ｓ３０４］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、図３のＳ１０３の処理で算出された第２の位置姿勢パラメータに対する各線分Ｌｊの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、３次元形状モデル（の線分）の２次元画像上への投影像を、当該画像上で等間隔になるように分割した制御点とその方向により表される。従って、各制御点は、３次元形状モデルを構成する線分のいずれかの投影像である。そこで、３次元形状モデルの線分Ｌｊの投影により生成された制御点には、そのモデル線分Ｌｊと組にしておいたｗｊを重みとして利用する。ここで、各制御点の感度ｗｊを用いて、「数１４」のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外は全て０の正方行列である。対角成分（ｗｓ，ｗｔ，・・・）には、行列Ｊ_２の各行に対応するモデル幾何特徴の感度ｗｊが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δφの推定に寄与しないデータの影響を抑制する。なお、各幾何特徴の重みの設定方法は、これに限られない。例えば、ｗｊの代わりに、ｗｊ×ｗｊを用いても良い。この他、各幾何特徴が特定軸回転方向の推定に寄与する度合いを感度の値に基づいて設定できる方法であれば、いかなる方法であっても構わない。この重み行列Ｗを用いて、「数１３」を「数１５」のように変形する。
ＷＪ_２Δφ＝ＷＥ_２ （数１５）

［Ｓ３０５］
位置姿勢計測装置１０は、「数１５」を「数１６」のように解くことにより補正値Δφを求める。
Δφ＝（Ｊ_２ ^ＴＷＪ_２）^−１Ｊ_２ ^ＴＷＥ_２ （数１６）

［Ｓ３０６］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、Ｓ３０５の処理で算出された位置姿勢の補正値ΔＲ_ｙにより、概略値Ｒを補正する。
Ｒ←ＲΔＲ_ｙ

［Ｓ３０７］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、Ｓ３０２の処理に戻る。なお、収束しているか否かは、補正値Δφがほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。

以上説明したように実施形態１によれば、特定の軸に対してほぼ回転対称であり、その軸回りの回転を決めるための特徴が少ない形状を有する物体に対しても、ロバスト且つ効率良く位置姿勢の計測が行なえる。具体的には、前段の処理において、物体の位置姿勢を推定し、後段の処理において、モデル座標系を基準とした特定軸回転方向に限定した位置合わせを行なう。後段の処理では、モデル座標系を基準とした特定軸回転成分の推定への影響度に応じて、各幾何特徴を重み付けを行なう。

なお、本実施形態においては、特定の軸周りの回転を決めるための特徴が少ない形状を有する物体を対象とした。しかし、特定の軸周りの回転を決めるための特徴の数の大小にかかわらず、本手法は応用可能である。すなわち、特定の軸に対してほぼ回転対称であり、その軸回りの回転を決めるための特徴を有する物体であれば、本手法を適用することで、特定の軸周りの回転成分をロバストかつ効率良く推定することができる。

（変形実施形態１−１）
上述した実施形態１においては、特定の軸に対してほぼ回転対称な形状を有する物体の位置姿勢の計測（推定）を行なう場合について説明した。これに対して、変形実施形態１−１では、モデル座標系の特定軸への並進方向を決める特徴が少ない物体の位置姿勢を計測する場合について説明する。

例えば、細長い四角柱のような形状に、その長軸方向の位置成分を決定する特徴が少ない形状を有する物体では、位置姿勢の６自由度成分を公平に扱って位置姿勢を算出すると、特定軸への並進方向のパラメータが正しく求まらない。

そこで、エッジを利用して６自由度の位置姿勢を一度算出した後で、特定軸方向の並進成分の推定に影響するエッジを用いて、３次元形状モデルの特定軸方向の並進パラメータのみ後段の処理で推定する。なお、本実施形態における位置姿勢計測装置１０の構成は、実施形態１と同様であるため、その説明については省略する。

続いて、変形実施形態１−１に係わる位置姿勢計測装置１０における処理の流れの一例について説明する。なお、基本的な処理の流れは、図３、図５及び図８と同様である。実施形態１と異なる処理は、Ｓ１０１〜Ｓ１０３、Ｓ１０７、Ｓ３０３の処理が挙げられる。そのため、ここでは、実施形態１と相違する処理について重点的に説明する。

まず、図３を参照して、Ｓ１０１〜Ｓ１０３及びＳ１０７の処理について説明する。

［Ｓ１０１］
位置姿勢計測装置１０は、特定パラメータ指定部１２及び感度算出部１３において、３次元形状モデルの各幾何特徴を入力する。ここで、図１０（ａ）は、本実施形態において対象とする物体の具体例とその座標系を示している。

［Ｓ１０２］
位置姿勢計測装置１０は、特定パラメータ指定部１２において、第１の位置姿勢パラメータと、第２の位置姿勢パラメータとを指定する。本実施形態では、図１０（ａ）に示すように、３次元形状モデルが、特定軸への並進方向を決める特徴の少ない形状であることを前提とする。このモデルに対して、モデルの並進方向を定義する軸（以後、モデル中心軸と呼ぶ）を決定する。モデル中心軸の設定は、例えば、ユーザからの入力により行なう。具体的には、図１０（ａ）に示すような四角柱形状のモデルの場合、四角柱の相対する２つの面を結ぶモデルのエッジ線分上から２点ａ、ｂをモデル中心軸として保持する（図１０（ｂ））。但し、モデル中心軸の設定方法は、これに限られない。例えば、モデル面上の頂点から主軸位置と方向とを求め、これらにより表されるベクトルをモデル中心軸としても良い。その他、モデル上で連続する類似形状により推定が困難な並進成分を定義する軸を算出できるのであれば、いかなる方法であっても良い。

次に、モデル中心軸と互いに直交する２つの軸を設定する。ここで、モデル中心軸と、設定した２つの軸とが座標軸となるようにモデル座標系を変換して定義し直す（図１０（ｃ））。変換後のモデル座標系において、ｙ軸（実線）がモデル中心軸と一致している。但し、モデル座標系の変換方法は、これに限られない。例えば、モデル座標系のｘ軸がモデル中心軸と一致するように変換を行なっても良い。その他、モデル中心軸の方向が、モデルの類似形状の方向と同一になるようにモデル座標系が変換されるのであれば、いかなる変換方法であっても良い。なお、以後の処理は、変換後のモデル座標系を用いて行なうものとする。変換後のモデル座標系を基準として、実施形態１と同様に、位置姿勢計測装置１０の位置姿勢を６次元ベクトルｓ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］^Ｔで表す。この６パラメータが第１の位置姿勢パラメータとなる。更に、特定パラメータ指定部１２は、モデル中心軸と一致する座標軸に対する並進成分を第２の位置姿勢パラメータとして指定する。本実施形態においては、ｙ軸がモデル中心軸と一致するため、ｙ軸方向の並進成分であるｔｙが第２の位置姿勢パラメータとして指定される。

［Ｓ１０３］
位置姿勢計測装置１０は、感度算出部１３において、第２の位置姿勢パラメータを算出する際の各モデル線分（３次元形状モデルにおける各幾何特徴）の感度を算出する。具体的には、モデル上の二つの３次元点ｐｓ、ｐｅとを結ぶ線分Ｌｊ（ｊ＝１，２，・・・，Ｎ）（Ｎ：モデル上の線分の数）の感度ｗｊを次のように算出する。

まず、点ｐｓをモデル中心軸に対してｔｙだけ微小（例えば、ｔｙはｙ軸方向のモデル長さの１／１００など）移動させた点をｐｓ’により表すものとする。モデル軸方向にｔｙ微小移動させる並進ベクトルをＴｙと表すと、ｐｓ’は、以下の「数１７」で表される。
ｐｓ’＝ｐｓ＋Ｔｙ （数１７）

ここで、点ｐｓから点ｐｓ’へ向かう長さ１の３次元ベクトルをｖｅｃ１とする。更に、点ｐｓから点ｐｅへ向かう長さ１の３次元ベクトルをｖｅｃ２とする。このとき、線分Ｌｊの感度ｗｊを、ｖｅｃ１とｖｅｃ２との内積ｖｅｃ１・ｖｅｃ２を用いて以下の「数１８」により定義する。

ｗｊ＝１−｜ｖｅｃ１・ｖｅｃ２｜ （数１８）
各幾何特徴の感度ｗｊは、０〜１の間の値を持つ。位置姿勢計測装置１０においては、この感度ｗｊを各モデル線分Ｌｊと組にして保持する。なお、Ｓ１０３の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。

［Ｓ１０７］
ここで、Ｓ１０４の処理で算出された第１の位置姿勢は、特定の自由度に関するパラメータｔｙを除き、正しい位置姿勢が算出されていることが期待される。そのため、Ｓ１０７の処理では、位置姿勢計測装置１０は、第１の位置姿勢を概略値として、特定の自由度に関するパラメータｔｙのみに関する最適化を行ない、計測対象物体の位置姿勢を算出する。このとき、第２の位置姿勢パラメータｔｙの推定に影響し易い幾何特徴を利用することで、ロバスト且つ効率良く推定を行なう。

第１の位置姿勢を概略値とした場合、Ｓ１０４の処理と同様に「数６」の関係が成り立つ。ここで、モデル座標系を基準としたｙ軸方向に対するΔｔｙの微小並進を表す３次元ベクトルをΔＴ＝［０，ｔｙ，０］^Ｔとする。このとき、第１の位置姿勢からモデル座標系のｙ軸方向に微小並進させたときの、モデル座標系（物体座標系）の３次元点Ｘ_ｍ＝［ｘ_ｍ，ｙ_ｍ，ｚ_ｍ］^Ｔから基準座標系の３次元点Ｘ_ｃ＝［ｘ_ｃ，ｙ_ｃ，ｚ_ｃ］^Ｔへの座標変換は、「数１９」のように表される。
Ｘ_ｃ＝Ｒ（Ｘ_ｍ＋ΔＴ）＋Ｔ （数１９）

そこで、Ｓ１０７の処理では、微小変化量Δｔｙを求めて、モデル座標系ｙ軸方向の並進パラメータの補正を行なうことで計測対象物体の位置姿勢の算出を行なう。

次に、図８を参照して、Ｓ３０３の処理について説明する。

［Ｓ３０３］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、３次元形状モデルの幾何特徴と画像特徴との対応関係に基づいて、位置姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施形態においても、「数１２」のφをｔｙに置き換えた形で、上記同様の関係が成立する。この方程式を実施形態１と同様の方法で解き、パラメータを更新することで第２の位置姿勢を算出する。

以上説明したように変形実施形態１−１によれば、モデル座標系の特定軸の並進方向を決める特徴が少ない物体に対してもロバスト且つ効率良く位置姿勢の計測が行なえる。具体的には、前段の処理において、物体の位置姿勢を算出し、後段の処理において、モデルの特定軸並進パラメータの推定に影響し易い幾何特徴を用いて並進パラメータのみ推定する。

なお、本実施形態においては、特定軸の並進方向を決めるための特徴が少ない形状を有する物体を対象とした。しかし、特定の軸方向の並進を決めるための特徴の数の大小にかかわらず、本手法は応用可能である。すなわち、特定軸の並進方向に対して類似性の高い形状において、その軸の並進方向を決めるための特徴を有する物体であれば、本手法を適用することで、特定軸の並進成分をロバストかつ効率良く推定することができる。

（変形実施形態１−２）
上述した実施形態１においては、第２の位置姿勢パラメータの算出時に各幾何特徴を重み付けすることで、第２の位置姿勢パラメータの推定に影響し易い幾何特徴の寄与度を上げていた。これに対して、変形実施形態１−２では、各幾何特徴について求めた感度を所定の閾値によって判定し、パラメータの推定に大きく影響する幾何特徴のみを選択して利用する場合について説明する。

例えば、実施形態１を説明した図８のＳ３０２の処理において、幾何特徴の感度の閾値ｗθを予め設定しておき、各幾何特徴の感度ｗｊと比較する。これにより、第２の位置姿勢パラメータの算出に利用する幾何特徴を予め取捨選択する。より具体的には、例えば、ｗθ＝０．９と設定しておき、ｗｊ＞ｗθを満たす幾何特徴のみ対応付けを行なう。これにより、第２の位置姿勢パラメータの推定に影響し難い幾何特徴の対応付け処理が省略される。これにより、第２の位置姿勢パラメータを算出する線形方程式の係数行列のサイズを小さくできるため、位置姿勢算出時の処理を高速に行なうことができる。

以上説明したように変形実施形態１−２によれば、３次元形状モデルの各幾何特徴それぞれに対して算出した感度に応じて、上記対応付け処理に利用する幾何特徴を判定する。これにより、上述した実施形態１の構成よりも、位置姿勢算出時の処理を高速に行なうことができる。

（変形実施形態１−３）
上述した実施形態１においては、実施形態１を説明した図３のＳ１０３における各幾何特徴の感度算出において、モデル座標系でモデル幾何特徴をφだけ回転させたときの微小変位ベクトルと、モデル線分の方向とに基づいて感度の算出を行なっていた。

これに対して、第１の位置姿勢に基づいて、モデル線分の画像面上への投影像を微小回転させたときの、画像上での微小変位ベクトルと、画像面上でのモデル線分の方向とに基づいて感度の算出を行なっても良い。具体的には、３次元形状モデル上の二つの３次元点ｐｓ、ｐｅとを結ぶ線分Ｌｊ（ｊ＝１，２，・・・，Ｎ）（Ｎ：モデル上の線分の数）の感度ｗｊを以下に示すように算出する。

まず、モデル上の二つの３次元点ｐｓ、ｐｅを第１の位置姿勢に基づいて画像上に投影した２次元座標をｕｓ、ｕｅとする。次に、実施形態１と同様に、点ｐｓをモデル中心軸に対してφだけ微小（例えば、φ＝０．０１ ｒａｄｉａｎ）回転させた点をｐｓ’により表すものとする。

更に、ｐｓ’を第１の位置姿勢に基づいて画像面上に投影した２次元座標をｕｓ’とする。ここで、点ｕｓから点ｕｓ’へ向かう長さ１の２次元ベクトルをｖｅｃ１とする。また更に、点ｕｓから点ｕｅへ向かう長さ１の２次元ベクトルをｖｅｃ２とする。このとき、線分Ｌｊの感度ｗｊを、ｖｅｃ１とｖｅｃ２との内積ｖｅｃ１・ｖｅｃ２を用いて「数２」と同様の方法で算出する。なお、変形実施形態１−１の例に対して、微小変化量φをｔｙに置き換えることでｐｓ’を算出して感度を算出しても良い。

以上説明したように変形実施形態１−３によれば、位置姿勢推定に用いる２次元エッジの特性に合わせた感度が算出できるため、ロバスト且つ効率良く位置姿勢の計測が可能となる。

（変形実施形態１−４）
上述した実施形態１においては、実施形態１を説明した図３のＳ１０３における各幾何特徴の感度算出において、モデル座標系でモデル幾何特徴をφだけ回転させたときの微小変位ベクトルと、モデル線分の方向とに基づいて感度の算出を行なった。

これに対して、各幾何特徴の偏微分を算出することで幾何特徴の移動方向を求め、これを利用して感度を算出しても良い。具体的には、モデル上の二つの３次元点ｐｓ、ｐｅとを結ぶ線分Ｌｊ（ｊ＝１，２，・・・，Ｎ）（Ｎ：モデル上の線分の数）の感度ｗｊを次のように算出する。まず、変形実施形態１−３と同様に、モデル上の二つの３次元点ｐｓ、ｐｅを第１の位置姿勢に基づいて画像面上に投影した２次元座標をｕｓ、ｕｅとする。ここでｕｓ＝（ｕ，ｖ）と表す。モデル中心軸に対する微小回転をφにより表すものとして、φに対するｕｓの偏微分を求め、微小変位ベクトルｖｅｃ１＝［∂ｕ／∂φ，∂ｖ／∂φ］^Ｔを算出する。

なお、長さは１になるよう正規化するものとする。更に、ｕｓとｕｅを結ぶ長さ１のベクトルをｖｅｃ２とする。このとき、線分Ｌｊの感度ｗｊを、ｖｅｃ１とｖｅｃ２との内積ｖｅｃ１・ｖｅｃ２を用いて「数２」と同様の方法で算出する。なお、変形実施形態１−１の例に対して、φの代わりにｔｙに対する偏微分を算出してｖｅｃ１を求め、感度を算出しても良い。

以上説明したように変形実施形態１−４によれば、実施形態１と同様に、ロバスト且つ効率良く位置姿勢の計測が行なえる。

（実施形態２）
次に、実施形態２について説明する。上述した実施形態１においては、２次元画像を利用して計測対象物体の位置姿勢を計測（推定）する場合に説明した。これに対して、実施形態２においては、細長い四角柱のような形状（変形実施形態１−１参照）に、その長軸方向の位置成分を決定する特徴が少ない（図１０参照）物体に対して距離画像を利用して位置姿勢の推定を行なう場合について説明する。例えば、細長い四角柱のような形状に、その長軸方向の位置成分を決定する特徴が少ない物体では、位置姿勢の６自由度成分を同時に位置姿勢を算出すると、３次元形状モデルの特定軸への並進方向のパラメータが正しく求まらない。

そこで、実施形態２では、モデルの幾何特徴を利用して位置姿勢を一度算出した後で、特定軸の並進成分の推定に影響するモデル幾何特徴の寄与度を変更して（上げて）モデルの特定軸方向の並進パラメータのみ後段で推定する。なお、実施形態２では、幾何特徴として３次元形状モデルの面を利用する。

なお、実施形態２に係わる位置姿勢計測装置１０の基本的な構成は、実施形態１を説明した図１と同様であるため、その説明については省略する。以下、実施形態２に係わる位置姿勢計測装置１０における各構成部について説明する。なお、モデル保持部１１、特定パラメータ指定部１２、概略値入力部１５については、変形実施形態１−１と同様の機能を果たすため、その説明については省略する。

感度算出部１３は、特定パラメータ指定部１２により指定された第２の位置姿勢パラメータに対する感度を算出する。この感度は、３次元形状モデルを構成する各面に対応して算出される。実施形態２に係わる位置合わせでは、３次元形状モデルの面（モデル面）と、距離画像上で対応付けられた３次元点との距離を最小化するように計測対象物体の位置姿勢を調整する。

そのため、第２の位置姿勢パラメータ方向にモデル面上の点を微小変化させたときの移動方向と、モデル面の法線方向とが垂直となる場合、そのモデル面に属す３次元点は、第２の位置姿勢パラメータの推定に寄与しない。従って、第２の位置姿勢パラメータ方向にモデルを微小変化させたときのモデル面の移動方向と、モデル面自身の持つ方向との相違度を感度として算出する。算出した感度は、第２の位置姿勢算出部１７において、第２の位置姿勢パラメータを算出する際の各幾何特徴の寄与度を設定する際に用いられる。

撮像部１４は、例えば、距離センサであり、距離画像を撮像する。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。勿論、距離センサは、これに限られず、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であっても良いし、また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であっても良い。その他、距離画像を計測するものであれば、いかなる形式の装置であっても良い。

第１の位置姿勢算出部１６は、計測対象物体の位置姿勢を示す概略値に基づいて、モデル保持部１１に保持されたモデル幾何特徴と、撮像部１４により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の位置姿勢を表す第１の位置姿勢パラメータを算出する。実施形態２においては、モデルの３次元面と、撮像画像から得られる点との距離を最小化することで第１の位置姿勢を算出する。

第２の位置姿勢算出部１７は、第１の位置姿勢に基づいて、モデル保持部１１に保持されたモデル幾何特徴と、撮像部１４により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、第２の位置姿勢パラメータを推定して第２の位置姿勢を算出する。このとき、感度算出部１３により算出された感度に基づき、第２の位置姿勢パラメータ算出時の各モデル幾何特徴の寄与度を設定する。これにより、ロバスト且つ効率良く算出を行なう。

続いて、実施形態２に係わる位置姿勢計測装置１０の処理の流れの一例について説明する。実施形態２に係わる位置姿勢計測装置１０の基本的な処理の流れは、実施形態１を説明した図３、図５及び図８と同様である。なお、Ｓ１０１、Ｓ１０２、Ｓ１０５、Ｓ２０１、Ｓ２０４〜Ｓ２０６、Ｓ３０１、Ｓ３０５、Ｓ３０６については、変形実施形態１−１と同様である。そのため、これらの処理についての説明は省略する。

まず、図３を参照して、Ｓ１０３、Ｓ１０４及びＳ１０６の処理について説明する。

［Ｓ１０３］
位置姿勢計測装置１０は、感度算出部１３において、第２の位置姿勢パラメータを算出する際の各モデル幾何特徴の感度を算出する。具体的には、モデルを構成する面Ｓｊ（ｊ＝１，２，・・・，Ｎ）（Ｎ：モデル上の面の数）の感度ｗｊを次のように算出する。なお、ここでは、各モデル面は、同一の法線を持つように区分されているものとする。

まず、モデル面Ｓｊ上の点ｐｓをモデル中心軸に対してｔｙだけ微小（例えばｔｙはｙ軸方向のモデル長さの１／１００など）移動させた点をｐｓ’により表すものとする。モデル軸方向にｔｙ微小移動させる並進ベクトルをＴｙと表すと、ｐｓ’は、以下の「数２０」で表される。
ｐｓ’＝ｐｓ＋Ｔｙ （数２０）

ここで、点ｐｓから点ｐｓ’へ向かう長さ１の３次元ベクトルをｖｅｃ１とする。更に、面Ｓｊの法線を表す長さ１の３次元ベクトルをｖｅｃ２とする。このとき、面Ｓｊの感度ｗｊを、ｖｅｃ１とｖｅｃ２との内積ｖｅｃ１・ｖｅｃ２を用いて以下の「数２１」により定義する。
ｗｊ＝｜ｖｅｃ１・ｖｅｃ２｜ （数２１）

算出した各幾何特徴の感度ｗｊは０〜１の間の値を持つ。位置姿勢計測装置１０においては、この感度ｗｊを各モデル面Ｓｊと組にして保持する。なお、Ｓ１０３の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。

［Ｓ１０４］
位置姿勢計測装置１０は、撮像部１４において、計測対象物体の距離画像を撮像する。

［Ｓ１０６］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、距離画像の情報に基づいて物体の位置姿勢の計測を行なう。実施形態２においては、距離画像上の特徴である３次元点に３次元形状モデルの投影像を当てはめるようにして計測対象物体の位置姿勢を算出する。Ｓ１０６の処理における第１の位置姿勢の算出処理は、図５と同様であるため、その説明については省略する。ここでは、距離画像から変換して得られる３次元点と、推定される位置姿勢に基づいて画像上に投影される３次元形状モデルの面との距離の二乗和が最小になるように位置姿勢の最適化を行なう。

次に、図５を参照して、Ｓ２０２及びＳ２０３の処理について説明する。以下の説明では、計測対象物体の位置姿勢を、実施形態１と同様に６次元ベクトルｓで表す。

［Ｓ２０２］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、Ｓ１０４の処理で入力された距離画像と３次元形状モデルとの対応付けを行なう。以下、距離画像とモデルとの対応付け処理について説明する。図１１は、Ｓ２０２の処理における距離画像と３次元形状モデルとの対応付け処理の概要を示している。Ｓ２０２の処理では、「大石，中澤，池内，“インデックス画像を用いた複数距離画像の高速同時位置合せ”，ｖｏｌ．Ｊ８９−Ｄ，ｎｏ．３，ｐｐ．５１３−５２１，２００６．」に開示された技術を用いて、物体の３次元形状モデル（ポリゴンモデル）を距離画像上に投影することによって対応付けを行なう。３次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデルのどのポリゴンに対応するかを決定するためである。距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと計測対象物体の位置姿勢の概略値とを用いて、ポリゴンモデルを描画する。以下、ポリゴンモデルを描画した画像をポリゴン描画画像と呼ぶ。描画する際には、各ポリゴンに固有の色を割り当てて描画を行なう。ポリゴンの色はＲＧＢの各色で表す。背景部分と区別するために、黒（Ｒ＝０、Ｇ＝０、Ｂ＝０）以外の色を用いてポリゴンを描画する。これにより、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をＧＰＵ（Graphics Processing Unit）上で高速に行ない、描画された結果の画像を読み出しても良い。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。

［Ｓ２０３］
位置姿勢計測装置１０は、第１の位置姿勢算出部１６において、３次元形状モデルのポリゴンと距離画像から変換した３次元点との対応をもとに、位置姿勢を算出するための係数行列・誤差ベクトルを算出する。以下、距離画像の画素（３次元の点）と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。

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

但し、Ｒは姿勢を表す３×３回転行列であり、Ｔは位置を表す３次元ベクトルである。このとき、基準座標系における平面の方程式の係数は次式のようになる。

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

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

「数２５」の観測方程式は対応付けが行なわれた全ての点群データについて立式することができる。

「数２５」の観測方程式は、ｓの各成分の微小変化Δｓｉ（ｉ＝１，２，・・・，６）についての方程式になっており、「数２６」に示すΔｓ_ｉに関する線形連立方程式を立てることができる。

この線形連立方程式を「数２７」のように表す。
Ｊ_１Δｓ＝Ｅ_１ （数２７）
左辺のＪ_１が算出対象となる係数行列であり、右辺のＥ_１が誤差ベクトルである。

次に、図８を参照して、Ｓ３０３及びＳ３０４の処理について説明する。

［Ｓ３０３］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、微小変化するパラメータをΔｔｙのみに限定して、Ｓ３０２と同様の処理を行なう。これにより、モデル線分上の制御点と画像上のエッジとの対応から、Δｔｙに関する「数２８」のような線形連立方程式を立てることができる。

この線形連立方程式を「数２９」のように表す。
Ｊ_２Δｔｙ＝Ｅ_２ （数２９）
左辺のＪ_２が算出対象となる係数行列であり、右辺のＥ_２が誤差ベクトルである。

［Ｓ３０４］
位置姿勢計測装置１０は、第２の位置姿勢算出部１７において、Ｓ１０３の処理で算出された第２の位置姿勢パラメータに対する各面Ｓｊの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、画像上の各画素に投影されるモデル面上の点と、その面の法線により表される。つまり、各幾何特徴はモデルを構成する面のいずれかの投影像である。そこで、モデル面Ｓｊの投影により生成されたモデル点には、そのモデル面Ｓｊと組にしてあるｗｊを重みとして組にする。ここで、各制御点の重みを用いて「数３０」のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外は全て０の正方行列である。対角成分（ｗｓ，ｗｔ，・・・）には、行列Ｊ_２の各行に対応するモデル幾何特徴の重みが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δｔｙの推定に寄与しないデータの影響を抑制する。この重み行列Ｗを用いて、「数２９」を「数３１」のように変形する。
ＷＪ_２Δｔｙ＝ＷＥ_２ （数３１）

以上説明したように実施形態２によれば、変形実施形態１−１と同様の物体に対して、距離画像を利用して位置姿勢の計測（推定）を行なえる。これにより、モデル座標系の特定軸の並進方向を決める特徴が少ない物体に対してもロバスト且つ効率良く位置姿勢の算出が可能となる。

具体的には、前段の処理において、３次元形状モデルと画像との３次元点の対応を利用して物体の位置姿勢を推定し、後段の処理において、モデルの特定軸並進方向に限定した位置合わせを行なう。モデルの幾何特徴に関して、特定軸並進方向の位置合わせへの影響度を予め算出しておき、後段のパラメータ推定時に重み付けして利用する。

また、２次元画像及び距離画像を同時に取得できる場合、第１の位置姿勢を算出する前段の位置合わせ、第２の位置姿勢を算出する後段の位置合わせにおいて、２次元画像を利用する手法と距離画像を利用する手法とをそれぞれ切り替えて行なっても良い。また、それら画像を同時に利用しても良い。すなわち、２次元画像及び距離画像の少なくとも一方を用いて計測対象物体の位置姿勢の算出を行なっても良い。

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

Claims (10)

1. 計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、
   前記物体の３次元形状モデルを保持するモデル保持手段と、
   前記３次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する特定パラメータ設定手段と、
   前記３次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータの算出時に与える影響を示す感度を算出する感度算出手段と、
   前記３次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段と
   を具備し、
   前記位置姿勢算出手段は、
   前記感度算出手段により算出された感度に基づいて前記特定のパラメータの算出時の前記３次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
   ことを特徴とする位置姿勢計測装置。
2. 前記位置姿勢算出手段は、
   前記感度算出手段により算出された感度に基づいて前記３次元形状モデルの各幾何特徴をそれぞれ重み付けし、当該重み付けされた幾何特徴と、前記物体の画像から検出された画像特徴と、の対応関係に基づいて、前記特定のパラメータを算出する
   ことを特徴とする請求項１記載の位置姿勢計測装置。
3. 前記位置姿勢算出手段は、
   前記感度算出手段により算出された感度が所定の閾値を越えた幾何特徴と、前記物体の画像から検出された画像特徴と、の対応関係に基づいて、前記特定のパラメータを算出する
   ことを特徴とする請求項１記載の位置姿勢計測装置。
4. 前記３次元形状モデルの幾何特徴は、
   前記３次元形状モデル上の線、面又は方向を持つ点である
   ことを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
5. 前記画像は、２次元画像及び距離画像の少なくとも一方を含む
   ことを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測装置。
6. 前記物体は、
   特定の軸に対して回転対称であり、且つその軸回りの回転を決めるための特徴を有し、
   前記特定のパラメータは、前記軸回りの回転成分を示すパラメータである
   ことを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測装置。
7. 前記物体は、
   特定の軸への並進方向を決める特徴を有し、
   前記特定のパラメータは、前記特定の軸への並進成分を示すパラメータである
   ことを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測装置。
8. 前記位置姿勢算出手段は、前記３次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する第１の算出を行った後、該第１の算出により算出された前記物体の位置姿勢を初期値とし、前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記３次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する第２の算出を行う、ことを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測装置。
9. 計測対象となる物体の位置姿勢を計測する位置姿勢計測装置の処理方法であって、
   特定パラメータ設定手段が、前記物体の３次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する工程と、
   感度算出手段が、前記３次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータの算出時に与える影響を示す感度を算出する工程と、
   位置姿勢算出手段が、前記３次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する工程と
   を含み、
   前記位置姿勢算出手段は、
   前記感度算出手段により算出された感度に基づいて前記特定のパラメータの算出時の前記３次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
   ことを特徴とする処理方法。
10. 請求項９に記載された処理方法の各工程をコンピュータに実行させるためのプログラム。

Images