JP5393531B2 - 位置姿勢推定装置、位置姿勢推定方法、プログラム、記憶媒体 - Google Patents

Description

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

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

位置姿勢を推定する方法として、カメラが撮像する二次元画像や距離センサから得られる距離画像を用いて行う方法がある。その中でも、撮像画像から抽出される画像特徴や距離画像から得られる点群データに対して物体の三次元形状モデルをあてはめるモデルフィッティングによる推定が一般的である。例えば、濃淡画像上で検出されるエッジに物体のワイヤフレームモデルの投影像があてはまるように物体の位置姿勢を推定する手法がある。また、距離画像から得られる点群データに対してメッシュモデルなどの三次元形状モデルをあてはめることにより物体の位置姿勢を推定する手法がある。

工作精度やコストの問題から、大量生産される工業部品の形状やサイズには一般的にばらつきがある。このような部品について個体ごとに三次元形状モデルを作ることは現実的ではないため、通常は部品の標準的な形状を表す１つの三次元形状モデルを用いて位置姿勢の推定する。すなわち、三次元形状モデルと実部品の形状は必ずしも一致しない。この部品に対してモデルフィッティングによる位置姿勢を行う場合、モデルと実部品の形状の差が大きい場合、高精度な推定ができないという課題がある。

特許文献１には、モデルを利用して物体の位置認識を行う際に物体の形状のばらつきを吸収する方法が開示されている。この方法では、物体を表す標準モデルと画像（計測データ）に基づいて物体の位置認識を行い、計測データを統計学的に処理し、標準モデルを逐次更新する。

一方、非特許文献１には、顔の位置姿勢を推定する際に、顔の三次元モデルに対する個人差を吸収する方法が開示されている。この方法は、個体ごとの特徴点のずれを、予め求めておいたずれの分布と実際の計測データから求め、このずれを付与した特徴点に対してモデルフィッティングを行う。個体ごとにモデル生成を行うことで、形状のばらつきに左右されることなく位置姿勢を推定できる。

特開平９−８１７４４

Ｘ．Ｃｈａｉ，Ｓｈａｎ，Ｌ．Ｑｉｎｇ，ａｎｄ Ｗ．Ｇａｏ，"Ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｂａｓｅｄ ｏｎ Ｇａｕｓｓｉａｎ ｅｒｒｏｒ ｍｏｄｅｌｓ，"Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｂｉｏｍｅｔｒｉｃｓ（ＩＣＢ ２００６），ＬＮＣＳ ３８３２，ｐｐ．１３６−１４３，２００５． Ｙ．Ｃｈｅｎ ａｎｄ Ｇ．Ｍｅｄｉｏｎｉ，"Ｏｂｊｅｃｔ ｍｏｄｅｌｉｎｇ ｂｙ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｒａｎｇｅ ｉｍａｇｅｓ，"Ｐｒｏｃ．１９９１ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ’９１），ｐｐ．２７２４−２７２９，１９９１． Ｍ．Ｄ．Ｗｈｅｅｌｅｒ，Ｙ．Ｓａｔｏ，ａｎｄ Ｋ．Ｉｋｅｕｃｈｉ，"Ｃｏｎｓｅｎｓｕｓ ｓｕｒｆａｃｅｓ ｆｏｒ ｍｏｄｅｌｉｎｇ ３Ｄ ｏｂｊｅｃｔｓ ｆｒｏｍ ｍｕｌｔｉｐｌｅ ｒａｎｇｅ ｉｍａｇｅｓ，"Ｐｒｏｃ．６ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ（ＩＣＣＶ’９８），ｐｐ．９１７−９２４，１９９８． Ｒ．Ｈａｒｔｌｅｙ ａｎｄ Ａ．Ｚｉｓｓｅｒｍａｎ，"Ｍｕｌｔｉｐｌｅ Ｖｉｅｗ Ｇｅｏｍｅｔｒｙ ｉｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ"，Ｃａｍｂｒｉｄｇｅ Ｕｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ，２０００． Ｙ．Ｌｉｕ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｏ．Ｄ．Ｆａｕｇｅｒａｓ，"Ｄｅｔｅｒｍｉｎａｔｉｏｎ ｏｆ Ｃａｍｅｒａ Ｌｏｃａｔｉｏｎ ｆｒｏｍ ２−Ｄ ｔｏ ３−Ｄ Ｌｉｎｅ ａｎｄ Ｐｏｉｎｔ Ｃｏｒｒｅｓｐｏｎｄｅｎｃｅｓ，"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１２，ｎｏ．１，ｐｐ．２８−３７，１９９０．

特許文献１には、物体の形状のばらつきを吸収し、物体の認識率を向上させる。しかし、この方法は、実物体のばらつきを吸収するように標準モデルを更新するため、物体の認識や概略の位置姿勢推定には適しているが、高精度な位置姿勢推定には適さないという課題があった。

非特許文献１には、この方法はずれを明示的に算出する方法であり、計測データからの特徴の誤検出などによって求めたずれ自体に誤差を含む可能性があるため、高精度な位置姿勢推定には適さないという課題があった。

上記事情に鑑み、本発明では、標準的な形状の三次元形状モデルに比して、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することを目的とする。

本発明の位置姿勢推定装置は、例えば、予め保持された対象物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出手段と、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出手段と、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出手段と、前記幾何特徴と前記画像特徴とを対応付けする対応付け手段と、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出手段と、前記信頼度及び前記ずれ量に基づいて、前記対象物体の位置姿勢を推定する位置姿勢推定手段とを有することを特徴とする。

本発明によれば、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することができる。

第１の実施形態における構成例である。 第１の実施形態における処理のフローチャートである。 公差データから形状のずれを算出する処理の例を説明するフローチャート（１）である。 標準モデル及び公差データの例である。 ずれ候補量の模式図である。 抽出された線分及び信頼度データベースの例である。 物体の位置姿勢算出処理の例を説明するフローチャートである。 対応付けの例を説明する模式図（１）である。 計測データからずれを算出する処理の例を説明するフローチャートである。 対象物体と計測位置の関係の例を説明する図である。 公差データから形状のずれを算出する処理の例を説明するフローチャート（２）である。 対応付けを説明する模式図（２）である。 本願の実施形態を実現するためのコンピュータの構成例である。

〔第１の実施形態〕
本実施形態では、物体の標準的な形状を表す三次元形状モデル（以下、標準モデルと呼ぶ）と実物体とのずれが一様ではないことを利用して高精度な物体の位置姿勢を推定する。すなわち、ずれが小さいと期待できる部分の情報を積極的に用いることで、高精度な位置姿勢の推定を行う。本実施形態では、物体は工業用部品であると想定し、物体の公差データをもとに、物体の標準モデルを構成する各特徴のずれの統計量を推定し、このずれの統計量をもとに、算出される特徴ごとの信頼度を用いて、物体の位置姿勢を推定する。

図１を用いて、本実施形態における位置姿勢推定装置１００の構成について説明する。図中矢印は、データの流れを示している。位置姿勢推定装置１００は、計測データ取得部１０１、特徴抽出部１０２、蓄積部１０３、位置姿勢算出部１０４で構成される。位置姿勢推定装置１００は、計測装置１０５に接続されている。

計測装置１０５は、現実空間を計測する。例えば、カメラで二次元画像を撮像する。例えば、物体の表面の三次元位置を計測し、距離画像を撮像する。撮像された撮像画像は、計測データ取得部１０１へ送られる。

計測データ取得部１０１は、計測装置１０５から計測データを取得する。例えば、計測データは、カメラによって撮像された二次元画像を取得する。取得した計測データは、特徴抽出部１０２へ送られる。

特徴抽出部１０２は、計測データから特徴を抽出する。例えば、Ｃａｎｎｙオペレータを用いて、画像からのエッジを特徴として抽出する。抽出した特徴は、位置姿勢算出部１０４へ送られる。

蓄積部１０３には、物体の標準モデルを構成する各特徴のずれの統計量が保持されている。例えば、物体の標準モデルは、線分の集合によって記述される。線分は、物体表面を表す面と面の交線や、色や濃度が大きく変わる境界線である。しかしながら、物体の標準モデルはこれに限るものではなく、物体上の特徴的な点の集合や、物体を表す面の集合、またはそれらの組み合わせによって記述してもよい。保持されている物体の標準モデルは、位置姿勢算出部１０４へ送られる。

位置姿勢算出部１０４は、特徴抽出部１０２で計測データから抽出された特徴と、蓄積部１０３に蓄積されている物体の標準モデル、物体の各特徴の標準モデルに対するずれの統計量を用いて物体の位置姿勢を推定する。

図２を用いて、本実施形態における位置姿勢推定装置の全体処理フローの概要について説明する。

ステップＳ２０１は、統計量算出ステップである。本ステップでは、物体の標準モデルを構成する幾何特徴のずれの統計量を算出する。前述したように、本実施形態では物体の標準モデルは線分の集合によって記述されている。ここでは、幾何特徴を線分として、各線分を三次元的に一定間隔で分割したときの分割点の公差にもとづくずれの統計量を算出する。分割点を幾何特徴として用いるのは、後述する位置姿勢の推定工程において、分割点単位処理を行うためである。

ステップＳ２０２は、信頼度算出ステップである。本ステップでは、ステップＳ２０１で求めた各分割点のずれの統計量から、幾何特徴である分割点の信頼度を算出する。

ステップＳ２０３は、位置姿勢算出ステップである。本ステップでは、ステップＳ２０２で算出した信頼度を用いて、物体の位置姿勢を算出する。

なお、ステップＳ２０１及びステップＳ２０２は必ずしも位置姿勢の推定とともに行う必要はなく、ステップＳ２０３で実施される位置姿勢推定の前処理として、別途オフラインで実施してもよい。

以下では、ステップＳ２０１で実施する分割点に対するずれの統計量の算出処理について詳しく説明する。ずれの統計量の算出結果は蓄積部１０３に保持される。ここでは、ずれの統計量を物体の公差データから算出する方法について、図３のフローチャートを用いて説明する。

ステップＳ３０１は、モデル特徴抽出ステップである。本ステップでは、標準モデルから線分データを抽出する。

ステップＳ３０２は、公差抽出ステップである。本ステップでは、注目する線分（以下、注目線分）のずれの要因となる公差（以下、ずれ要因公差）を抽出する。ずれ要因公差は、注目線分が含まれる稜線や面に対する公差とする。

図４は、標準モデル及び公差データの一例である。標準モデル４００から抽出される線分４０１を注目線分とした場合について説明する。

線分４０１が含まれる稜線や面に対する公差である寸法公差４０２〜４０４及び平面公差４０５〜４０７がずれ要因公差の候補となる。これらの候補のうち、位置姿勢推定に寄与しない寸法公差４０２、平面公差４０５を除いた寸法公差４０３、４０４及び平面公差４０６、４０７がずれ要因公差となる。

ステップＳ３０３は、公差値算出ステップである。本ステップでは、注目線分を三次元的に所定間隔に分割し、各分割点について、ステップＳ３０２で抽出した各ずれ要因公差によって生じるずれ量の最大値（以下、ずれ候補量）をそれぞれ求める。ずれ候補量は、標準モデル上に設定されたモデル座標系５０３の成分ごとの大きさで表される。

図５は、注目線分を線分４０１としたときに、線分４０１のある分割点５０１に対し寸法公差４０４が与えるずれ候補量５０２を模式的に表した図である。この例では、分割点５０１のずれ候補量５０２はｘ成分のみとなる。この処理を、分割点すべてについて実施する。

ステップＳ３０４は、統計量決定ステップである。本ステップでは、ステップＳ３０３で算出された各分割点のずれ候補量群の成分ごとの最大値を持つベクトルを生成し、このベクトルの大きさｄを分割点におけるずれの統計量とする。これは、すべての許容公差を満たし、かつその中で最大となるずれ量を求めたこととなる。

例えば、分割点５０１のずれ候補量が３つ存在した場合について説明する。各ずれ候補量を、（ｘ成分，ｙ成分，ｚ成分）＝（ｄ_ｘ１，ｄ_ｙ１，ｄ_ｚ１）、（ｄ_ｘ２，ｄ_ｙ２，ｄ_ｚ２）、（ｄ_ｘ３，ｄ_ｙ３，ｄ_ｚ３）とし、それぞれｄ_ｘ１＞ｄ_ｘ２＞ｄ_ｘ３、ｄ_ｙ３＞ｄ_ｙ２＞ｄ_ｙ１、ｄ_ｚ２＞ｄ_ｚ３＞ｄ_ｚ１といった大小関係があるとする。この場合、成分ごとの最大値は、（ｄ_ｘ１，ｄ_ｙ３，ｄ_ｚ２）となり、この大きさｄは、

となり、このｄを分割点５０１におけるずれの統計量とする。

ステップＳ３０５では、ステップＳ３０１で抽出した線分全てに対してステップＳ３０２〜３０４の処理が実施済みの場合には本処理を終了し、実施済みでない場合には、ステップＳ３０６に進み注目線分を変更し、ステップＳ３０２へ戻る。

以下では、ステップＳ２０２で実施する分割点ごとの信頼度の算出方法について詳細に説明する。

信頼度は、ずれの統計量が小さいものほど高く設定する。すなわち、工作精度が高い部分の信頼度は大きくし、低い部分の信頼度は小さくする。例えば、式（１）のようにずれの統計量が小さいものほど信頼度が高くなる関数を定義する。

ｄは前述した分割点のずれの統計量であり、ｃ_１は定数である。信頼度を与える関数は上記関数に限らず、ずれ量が小さいものほど信頼度が高くなる他の関数であればいかなる関数であってもよい。算出された信頼度は、線分ごとに標準モデルから抽出された分割点の三次元位置と共に蓄積部１０３にデータベース（図６）として保持される。

以下では、ステップＳ２０３で行う物体の位置姿勢算出処理について、図７のフローチャートを用いて説明する。

ステップＳ７０１は、画像特徴抽出ステップである。本ステップでは、特徴抽出部１０２において撮像画像から画像特徴であるエッジを抽出する。

ステップＳ７０２は、概略値入力ステップである。本ステップでは、標準モデルの位置姿勢の概略値を入力する。

ステップＳ７０３は、対応付けステップである。本ステップでは、蓄積部１０３に保持されている分割点とステップＳ７０１で抽出したエッジとの対応付けを行う。図８は、対応付けの様子を模式的に表した図である。ここでは、入力された位置姿勢の概略値を用いて標準モデル中の線分８０１とその分割点８０２を画像上に投影し、分割点ごとに投影された線分の法線方向に一定範囲内でエッジ探索を行い、最も近接するエッジを該分割点との対応点８０３とする。

ステップＳ７０４は、位置姿勢補正値算出ステップである。本ステップでは、各分割点８０２における信頼度を用いて、位置姿勢算出部１０４において位置姿勢を算出するための係数行列及び誤差ベクトルを算出する。ある分割点の画像での投影位置を（ｕ_ｉ，０，ｖ_ｉ，０）とし、この分割点８０２の対応点の画像上での検出位置を（ｕ_ｉ’，ｖ_ｉ’）とする。分割点（ｕ_ｉ，０，ｖ_ｉ，０）を含む標準モデル上の線分８０１（以下、モデル線分）の投影像の画像上での傾き８０４をθとする。該対応点８０３を通り、傾き８０４がθである直線８０５の方程式は式（２）となる。

が表せる（θは定数とする）。ここで、

（定数）である。

分割点８０２の画像上での投影位置と物体の位置姿勢の関係を

のように表す。物体の位置姿勢の微小変化をΔｓとすると、式（３）は式（４）のように一次近似できる。

物体の位置姿勢をΔｓだけ変化させたときに、式（４）で表される分割点の投影位置が式（２）で表される直線上にのると仮定すると、式（５）で表されるΔｓに関する方程式が得られる。

ただし、

（定数）である。標準モデル中の線分８０１の各分割点８０２について成り立つ式（５）を連立方程式として解くことにより、物体の位置姿勢の補正値Δｓが得られる。位置姿勢に基づく剛体変換と画像上への透視投影変換は非線形の変換であるため、補正値Δｓの算出とｓの補正を繰り返すことにより、最終的な物体の位置姿勢が得られる。

本実施形態では、さらに式（５）の両辺に分割点の信頼度ｈ_ｉを重みとして乗じることにより、工作精度の高い分割点の寄与度を高く、工作精度の低い分割点の寄与度を低くして位置姿勢の補正量Δｓを算出する。すなわち、式（６）で表される連立方程式を解くことにより、位置姿勢の補正量Δｓを算出する。

ここで式（６）を式（７）のように表す。

式（７）を解いて得られるΔｓを用いて概略の位置姿勢を補正することで、物体の位置姿勢が算出する。

ステップＳ７０５において、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１・Ｊ^Ｔを式（７）の両辺に乗じることで位置姿勢補正量Δｓを求める。

ステップＳ７０６は、位置姿勢補正ステップである。本ステップでは、算出した位置姿勢の補正量Δｓにより、位置姿勢の概略値を補正する。

ステップＳ７０７において、収束判定を行う。収束していれば、処理を終了し、そうでなければステップＳ７０１に戻る。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルＲの２乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。

以上述べたように、第１の実施形態では、標準モデルを構成する幾何特徴のずれの統計量を公差データから求め、ずれの統計量が小さい特徴ほど高い重みを付与して物体の位置姿勢を求める方法について説明した。信頼度の高い特徴を積極的に用いることにより、高精度な位置姿勢推定が可能となる。

〔第２の実施形態〕
第１の実施形態では、物体を構成する幾何特徴の標準モデルに対するずれの統計量を、公差から算出される最大のずれに基づいて特定の値として算出した。しかしながら、物体のずれは統計的に発生するのが一般的であり、最大のずれに基づいて信頼度を決定することによって予想以上に信頼度が低くなる特徴が発生する。一方、実際の公差は、一般的に標準値を中心とした正規分布となる。このため、公差は標準に対する差の値ではなく、正規分布のように確率分布によって表される場合もある。本実施形態では、標準モデルを構成する特徴のずれが確率分布によって表される場合の位置姿勢の推定方法について説明する。

全体の処理フローは、第１の実施形態にて図２を用いて説明したものと同様である。以下では、ステップＳ２０１、Ｓ２０２及びＳ２０３で実施される各処理について説明する。

まず、ステップＳ２０１で実施される標準モデルに対するずれの算出処理について、図３を用いて説明する。なお、ステップＳ３０４以外の処理は、第１の実施形態で説明した内容と同様であるため、ここではステップＳ３０４の処理についてのみ説明する。

ステップＳ３０４において、ステップＳ３０３で算出された各分割点のずれ候補量群から成分ごとのずれの分布を求める。各分割点のずれの分布は成分間で独立であり、成分ごとに平均０、標準偏差がずれ候補量群の最大値の３分の１である１次元の正規分布に従うとする。例として、分割点５０１のずれ候補量が３つ存在した場合について説明する。各ずれ候補量を、（ｘ成分，ｙ成分，ｚ成分）＝（ｄ_ｘ１，ｄ_ｙ１，ｄ_ｚ１）、（ｄ_ｘ２，ｄ_ｙ２，ｄ_ｚ２）、（ｄ_ｘ３，ｄ_ｙ３，ｄ_ｚ３）とし、それぞれｄ_ｘ１＞ｄ_ｘ２＞ｄ_ｘ３、ｄ_ｙ３＞ｄ_ｙ２＞ｄ_ｙ１、ｄ_ｚ２＞ｄ_ｚ３＞ｄ_ｚ１といった大小関係があるとする。この場合、各成分の最大値は（ｄ_ｘ１，ｄ_ｙ３，ｄ_ｚ２）となり、ずれの標準偏差は（σ_ｘ，σ_ｙ，σ_ｚ）＝（ｄ_ｘ１／３，ｄ_ｙ３／３ｄ_ｚ２／３）となる。これより、成分ごとのずれの確率分布は、

となる。

次に、ステップＳ２０２及びＳ２０３で実施される信頼度の算出処理及び位置姿勢推定処理について説明する。まず、標準モデル及び計測データの双方に統計的な誤差が含まれる場合の最尤推定法について説明し、次に第１の実施形態と同様に幾何特徴が線分の分割点とした場合について具体的に説明する。

以下では、標準モデル及び計測データに統計的な誤差が含まれる場合の最尤推定法ついて説明する。

まず、計測データの適合具合を示す尤度関数を算出する。計測データの特徴位置を表すベクトルをｐ_ｉ、ｐ_ｉに対応する標準モデルの特徴位置を表すベクトルをｑ_ｉ、物体の位置姿勢を表す６次元ベクトルをｓとする。計測データの特徴位置をｐ_ｉ、標準モデルの特徴位置ｑ_ｉ及び物体の位置姿勢ｓには、式（９）の関係があるものとする。関数ｇは、三次元空間から二次元画像面への変換である透視投影変換や、モデル座標系からカメラ座標系への変換であるモデルビュー変換などを含む。

標準モデルのずれをΔｑ_ｉ、計測誤差をΔｐ_ｉとすると、計測データ空間での標準モデルの特徴と計測データの特徴との誤差ｅ_ｉは、式（１０）のように線形近似される。

Δｑ_ｉ、Δｐ_ｉが統計的な誤差であるとすると、誤差ｅ_ｉには、平均が０で分散が、

である統計的誤差が含まれる。Δｑ_ｉ、Δｐ_ｉが正規分布に従うとするとｅ_ｉに含まれる統計的誤差も正規分布に従うため、誤差ｅ_ｉの尤度関数は式（１２）となる。

最尤推定では、式（１３）で表される尤度の積が最大となるように物体の位置姿勢を推定する。

実際には、尤度の積の対数を取り、その符号を反転した

を最小化する問題に置き換えられる。このうち第一項は位置姿勢に依存しない定数であるため、最終的には、

を最小化する問題となる。式（１５）は、分散Σ_ｉの逆行列を信頼度として重みつけした誤差ベクトルのノルムの２乗和である。

以下では、幾何特徴を線分の分割点とした場合について説明する。

式（９）の関数ｇは、分割点を画像面に投影する関数となり、式（１６）で表される。

ただし、

及び、

とする。ｆはカメラの焦点距離である。ｘ_ｃ，ｙ_ｃ，ｚ_ｃは、カメラ座標系における分割点の三次元位置、ｘ_ｗ，ｙ_ｗ，ｚ_ｗは世界座標系（物体座標系）における分割点の位置、行列Ｒ及びｔは世界座標系からカメラ座標系への座標変換行列の回転成分及び平行移動成分である。

誤差ｅ_ｉは、分割点の投影位置と対応点を通過する直線との距離（スカラ値）で表される。対応点を通過する直線の傾きθ_ｉには誤差はないものと仮定すると、誤差ｅ_ｉは式（１９）となる。

誤差ｅ_ｉがスカラ値であるため，ｅ_ｉの共分散行列Σｉ（式（１１））もスカラとなる。ステップＳ２０２で信頼度をΣｉの逆数として計算し、ステップＳ２０３で第１の実施形態で図７を用いて説明したフローを実施することで物体の位置姿勢を求める。なお、ステップＳ７０４で算出される係数拡大行列及び誤差ベクトルは、式（６）の重みｈを式（１８）で計算されるΣ_ｉの逆数に置き換えたものとなる。

以下では、幾何特徴を線分にしたときの共分散行列Σ_ｉの算出方法について具体的に説明する。

共分散行列Σｉ（式（１１））の右辺第１項Σ_ｐｉは、画像から検出される線分の検出誤差の共分散行列である。線分の検出誤差については、探索ラインの方向にのみあいまい性を持つと仮定し、線分の検出誤差が平均０、標準偏差σの１次元ガウス分布に従うと仮定する。実画像におけるσを推定するのは困難である。そのため、線分の検出誤差は主に画像の量子化誤差に起因するとして、例えばσを１画素に設定する。すなわち、Σ_ｐｉ は１となる。また、共分散行列Σｉ（式（１１））の右辺第２項はステップＳ２０１で求めた標準モデルのずれの共分散行列Σ_ｑｉの両辺にヤコビアン∂ｇ／∂ｑ_ｉを乗じたものである。標準モデルのずれの共分散行列Σ_ｑｉは、成分間に相関がないとして、ステップＳ２０１で求めたずれの標準偏差を（σ_ｘ，σ_ｙ，σ_ｚ）とすると、

となる。また、ヤコビアン∂ｇ／∂ｑ_ｉは、式（１６）、（１７）及び（１８）より、

となる。式（２０）及び式（２１）より共分散行列Σｉ（式（１１））の右辺第２項が求まる。

以上述べたように、第２の実施形態では、標準モデルから抽出される特徴のずれが確率分布によって表される場合に、確率分布から算出される信頼度をもとに位置姿勢を求める方法について説明した。

〔第３の実施形態〕
第１及び第２の実施形態では、ステップＳ２０１で実施される標準モデルに対する特徴のずれの算出を公差データから求める方法について説明したが、その他の方法であっても良い。例えば、実際に同種の多数の物体を計測して形状のばらつきを求めてもよい。本実施形態では、距離撮像装置による三次元形状計測を複数の物体に対して行い、各物体に対する三次元計測結果のばらつきをもとに標準モデルのずれを求める。物体全体の形状を計測するために、各サンプルに対して複数の視点から三次元計測を行い、その結果を統合して、線分によって記述される一つの標準モデルを生成する。このようにして生成された複数の標準モデルから、標準モデルに対するずれを算出する。

また、ステップＳ２０１の前に、標準モデルを得るために物体の計測を行う物体計測ステップを実行してもよい。そして、物体の標準モデルはあらかじめ用意されているとするが、三次元計測結果から得られる複数の標準モデルをもとに生成してもよい。

図９のフローチャートを用いて、実際に物体を計測することによる標準モデルのずれを求める方法について説明する。

ステップＳ９０１において、ある計測位置から距離撮像装置によって物体の計測データを取得する。ここで物体の計測データとは、物体表面上の点群データであり、各点は距離撮像装置の座標系における三次元位置によって表される。

ステップＳ９０２において、予め決めておいた計測位置すべてからの計測が終了していない場合には、ステップＳ９０３に進む。

ステップＳ９０３において、計測位置を移動する。図１０は、計測位置を模式的に表した図である。物体１００１を原点とした球面上を適当な刻み幅で計測装置１００２を移動させ、計測位置を移動させる。計測位置を移動した後、ステップＳ９０１へ戻り処理を続ける。予め決めておいた計測位置すべてからの計測が終了した場合には、ステップＳ９０４へ進む。これにより、物体を多方向から複数回計測し、計測精度を向上させる。

ステップＳ９０４において、複数の視点位置において取得された点群データを一つの点群データに統合する（非特許文献２参照）。さらに、統合された点群データをメッシュモデルに変換する（非特許文献３参照）。生成されたメッシュモデルから、三次元線分を抽出する。ここでは、一定値以上の角度をなすメッシュの交線を三次元線分として抽出する。

ステップＳ９０５において、予め定められていた物体すべての計測が終了していなかった場合には、ステップＳ９０６に進む。

ステップＳ９０６において、計測対象物体を変更する。ここでいう物体は、同種の物体であり、個体ごとのばらつき情報を得ることを目的とし、物体の変更を行う。物体の変更後、ステップＳ９０１へ戻り処理を続ける。ステップＳ９０５において、予め決められた物体すべての計測が終了した場合には、ステップＳ９０７へ進む。

ステップＳ９０７において、物体ごとに計測された結果から得られる三次元線分の標準モデルに対するばらつきを算出する。ここでは、個別のメッシュモデルからは同一の三次元線分が抽出されるものとする。標準モデルに対するばらつきは、第１実施形態、第２実施形態と同様に、三次元線分を一定間隔で分割したときの分割点ごとに算出する。

以上述べたように、第３の実施形態では、標準モデルに対する実物体の形状のばらつきを、実物体を計測することで求める方法について説明した。これにより、あらかじめ公差データとしてばらつきの情報が得られない場合であっても、実計測によってばらつきを推定し、その結果を用いて高精度な位置姿勢の推定を行うことができる。

〔変形例１〕
第１、第２及び第３の実施形態では、物体の形状が線分の集合として記述される場合の方法について説明したが、これに限らず別の特徴によって記述されてもよい。例えば、点の集合として記述してもよい。物体を表現する各点について、第１の実施形態や第２の実施形態で述べた方法と同様にして標準モデルに対するばらつきを算出し、物体の位置姿勢を推定する際の重みとして利用する。ここでは第１の実施形態と同様に、各点について、スカラ値の重みｈ_ｉが算出されているものとする。物体の概略の位置姿勢が与えられているとすると、計測データに対し次のような観測方程式が立式される。

ここでｕ’_ｉ、ｖ’_ｉは画像上での点の検出位置、ｕ_ｉ、ｖ_ｉは概略の位置姿勢によって算出される画像上での点の位置である。すべての点について立式される式（２２）の両辺に重みｈ_ｉを乗じた式を位置姿勢の補正値Δｓ_ｉについての連立方程式とすることにより式（２３）が得られる。

式（２３）を解いて位置姿勢の補正値Δｓ_ｉを算出し、概略の位置姿勢を補正する処理を繰り返すことにより、最終的な物体の位置姿勢が算出される。

〔変形例２〕
第１、第２及び第３の実施形態では、計測データをカメラから得られる二次元画像として説明したが、計測データを距離画像撮像装置から得られる三次元点群データとしてもよい。以下では、計測データを三次元点群とした場合の処理について説明する。

まず、ステップＳ２０１で実施するずれの算出において、公差データを用いて求める方法について、図１０及び１１を用いて説明する。ここでは、物体の標準モデルは面の集合として記述されているものとして、ずれの統計量を面ごとに定める。

ステップＳ１１０１において、注目する面（以下、注目面）のずれ要因公差を抽出する。ずれ要因公差は、注目面が含まれる稜線や面に対する公差とする。標準モデル４００から抽出される面１２０１（図１２）を注目面とした場合、面１２０１が含まれる稜線や面に対する公差となる寸法公差４０２〜４０４及び平面公差４０５〜４０７から、位置姿勢推定に寄与しない面の法線に対して直交する方向に誤差を与える寸法公差４０２、４０３及び平面公差４０５、４０７を除いたもの、つまり寸法公差４０４及び平面公差４０６がずれ要因公差となる。

ステップＳ１１０２において、ステップＳ１１０２で抽出した各ずれ要因公差が注目面に与えるずれ量の最大値（ずれ候補量）をそれぞれ求める。ずれ量は、標準モデルに対する面の重心のずれとする。

ステップＳ１１０３において、現在注目している面の重心の標準モデルに対するずれ候補量のうち、最大となるものを、この面のずれの統計量として抽出する。

ステップＳ１１０４において、すべての面に対する処理が終了していると判断される場合には処理を終了し、そうでなければステップＳ１１０５において、注目面を変更してステップＳ１１０２に戻る。

次に、ステップＳ２０２で実施する信頼度の算出方法について説明する。ここでは、前述した各面のずれの統計量について、式（１）に基づいて信頼度を求める。

最後に、ステップＳ２０３で実施する位置姿勢の算出方法について説明する。算出処理は、第１の実施形態において説明した方法とほぼ同様であるため、以下では異なる部分についてのみ補足的に説明する。

ステップＳ７０１では、距離画像撮像装置が撮像する距離画像から、三次元点群データを抽出する。

ステップＳ７０３で実施される対応付けについて説明する。図１２で示すように、計測点群の各点から法線方向に対応する面を探索する。標準モデルは、概略の位置姿勢に基づいて距離画像撮像装置の座標系に変換されているものとして、探索ラインと交わる面を対応面とする。

ステップＳ７０４で実施される係数行列及び誤差ベクトルの算出方法について説明する。物体座標系における標準モデルから抽出される面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｋとする。物体の位置姿勢補正量をΔｓとし、この補正によって点群データの各点が対応する標準モデルの面上にのると仮定すると、式（２４）が得られる。

ただし、
ｌ＝ａｘ_０＋ｂｙ_０＋ｃｚ_０（定数）
式（２４）の両辺に制御点が含まれる面の信頼度ｈ_ｉを乗じ、すべての分割点に対して一連の処理を行うことで、連立方程式（式（２５））が得られる。

ステップＳ７０５において、式（２５）を解いて位置姿勢の補正値Δｓ_ｉを算出し、概略の位置姿勢を補正する。

本変形例では物体の標準モデルが面の集合として記述されている場合の方法について説明したが、他の特徴によって記述されていてもよい。例えば、標準モデルが点群データとして記述されていてもよい。

〔変形例３〕
これまで説明した実施形態、変形例では、物体の概略の位置姿勢が与えられるとして、概略の位置姿勢を繰り返し補正することにより最終的な位置姿勢の算出を行っていた。しかしながら、位置姿勢の算出方法はこれに限るものではなく、概略の位置姿勢が未知の場合に、直接的に位置姿勢を算出してもよい。例えば、計測データが二次元画像上の特徴点である場合には、ＤＬＴ（Ｄｉｒｅｃｔ Ｌｉｎｅａｒ Ｔｒａｎｓｆｏｒｍ）法によって直接的に位置姿勢を算出することが一般的に行われる（非特許文献４参照）。ＤＬＴ法は、物体の位置を表す３変数と姿勢を表す回転行列の各要素である９変数を線形連立方程式を解くことによって算出する方法であり、未知変数は１２個である。各点について２本の方程式が成り立つため、少なくとも６個の計測データが必要となる。この連立方程式を解く際に、第１の実施形態で説明したように各方程式に標準モデルからのずれに基づく重みを乗じることにより、工作精度の高い部分の情報を優先的に使うことができる。

位置姿勢を直接的に算出する方法はこれに限るものではなく、三次元の点の対応をもとに上述の１２変数を算出してもよい。また、特徴は点に限るものではなく、線分の対応をもとに算出してもよい（非特許文献５参照）。

〔変形例４〕
これまで説明した実施形態、変形例では、標準モデルのずれ量を、線分等間隔に分割した点の距離または面の重心間の距離としたが、これに限らず他のものであってもよい。線分を特徴として扱う場合には、線分間の距離をずれ量としてもよい。線分間の距離は、線分の端点間の距離及び、１方の線分の端点とこの端点から下ろした法線ともう１方の線分との交点の距離のうち最小となる距離とする。また、面を特徴として扱う場合には、平面度をずれ量としてもよい。平面度は、面の歪の最大値（山）と最小値（谷）の高低差とする。ずれ量算出のフローは、図３または図９を用いて説明したものと同様であり、特徴及びずれ量を、線分及び線分間の距離または面及び平面度に置き換えることで実現される。

〔その他の実施形態〕
図１３は、本願の実施形態を実現するためのコンピュータの構成例である。

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

Claims (18)

1. 予め保持された対象物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出手段と、
   前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出手段と、
   撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出手段と、
   前記幾何特徴と前記画像特徴とを対応付けする対応付け手段と、
   前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出手段と、
   前記信頼度及び前記ずれ量に基づいて、前記対象物体の位置姿勢を推定する位置姿勢推定手段と
   を有することを特徴とする位置姿勢推定装置。
2. 前記統計量算出手段は、
   前記三次元形状モデルの公差データに基づいて、前記統計量を算出することを特徴とする請求項１に記載の位置姿勢推定装置。
3. 前記統計量算出手段は、
   前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出手段と、
   前記線分のずれの要因となる公差を抽出する公差抽出手段と、
   前記線分の所定間隔ごとの前記公差の値を算出する公差値算出手段と、
   前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定手段と
   を有することを特徴とする請求項２に記載の位置姿勢推定装置。
4. 前記統計量は、確率分布で表現されることを特徴とする請求項２に記載の位置姿勢推定
   装置。
5. 前記統計量算出手段は、
   前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出手段と、
   前記線分のずれの要因となる公差を抽出する公差抽出手段と、
   前記線分の所定間隔ごとの前記公差の値を算出する公差値算出手段と、
   前記公差の値に基づく正規分布のベクトルを生成し、前記ベクトルを統計量とする統計量決定手段と
   を有することを特徴とする請求項４に記載の位置姿勢推定装置。
6. 前記統計量算出手段は、
   前記三次元形状モデルを構成する面を抽出するモデル特徴抽出手段と、
   前記面の重心における前記公差の値を算出する公差値算出手段と、
   前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定手段と
   を有することを特徴とする請求項２に記載の位置姿勢推定装置。
7. 前記対象物体の形状を構成する幾何特徴を計測する物体計測手段とを更に有し、
   前記統計量算出手段は、前記計測した結果と前記三次元形状モデルとのずれに基づいて、前記統計量を算出することを特徴とする請求項１に記載の位置姿勢推定装置。
8. 前記物体計測手段は、複数の物体ごとに複数回計測することを特徴とする請求項７に記載の位置姿勢推定装置。
9. 前記物体計測手段は、複数の計測位置ごとに複数回計測することを特徴とする請求項７又は８に記載の位置姿勢推定装置。
10. 前記幾何特徴が、点、若しくは、線分、若しくは、面であることを特徴とする請求項１
    乃至９の何れか１項に記載の位置姿勢推定装置。
11. 前記信頼度算出手段は、前記統計量が小さいほど信頼度を高く算出することを特徴とする請求項１乃至１０の何れか１項に記載の位置姿勢推定装置。
12. 前記信頼度算出手段は、尤度関数を用いることにより、信頼度を算出することを特徴とする請求項１乃至１１の何れか１項に記載の位置姿勢推定装置。
13. 前記対象物体の位置姿勢の概略値を入力する概略値入力工程と、を更に有し、
    前記位置姿勢推定手段は、
    前記信頼度及び前記ずれ量及び前記概略値に基づいて、前記対象物体の位置姿勢の補正値を算出する位置姿勢補正値算出手段と、
    前記補正値に基づいて、前記対象物体の位置姿勢の概略値を補正する位置姿勢補正手段と
    を有することを特徴とする請求項１乃至１２の何れか１項に記載の位置姿勢推定装置。
14. 前記対象物体を撮像する撮像手段を更に備えることを特徴とする請求項１乃至１３のいずれか１項に記載の位置姿勢推定装置。
15. 前記撮像手段が、現実空間の距離画像を撮像画像として撮像することを特徴とする請求
    項１４に記載の位置姿勢推定装置。
16. 対象物体の位置姿勢を求める位置姿勢推定方法であって、
    統計量算出手段が、予め保持された前記対象物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出工程と、
    信頼度算出手段が、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出す
    る信頼度算出工程と、
    画像特徴抽出手段が、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像
    特徴抽出工程と、
    対応付け手段が、前記幾何特徴と前記画像特徴とを対応付けする対応付け工程と、
    ずれ量算出手段が、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出工程
    と、
    位置姿勢推定手段が、前記信頼度及び前記ずれ量に基づいて、前記対象物体の位置姿勢を推定する位置姿勢推定工程と
    を有することを特徴とする位置姿勢推定方法。
17. 請求項１６に記載の位置姿勢推定方法をコンピュータに実行させることを特徴とするプログラム。
18. 請求項１７に記載のプログラムを記憶したことを特徴とするコンピュータ読み取り可能な記憶媒体。