JP2019168473A - 位置姿勢計測装置及び方法 - Google Patents

Abstract

【課題】 ロバストな位置姿勢計測を行うことを目的とする。【解決手段】 位置姿勢計測装置に、対象物体が含まれる画像から、前記対象物体の少なくとも１つの概略位置姿勢を取得する取得手段と、前記取得した概略位置姿勢の分解能に基づいて、前記対象物体の位置姿勢の導出のための初期値として、少なくとも１つの位置姿勢候補を新たに生成する生成手段と、前記対象物体のモデル情報と、前記初期値として生成された位置姿勢候補とに基づいて、前記対象物体の位置姿勢を導出する導出手段とを備える。【選択図】 図２

Description

本発明は、形状が既知である物体の位置姿勢を計測する技術に関する。

近年コンピュータビジョンの発展とともに、ロボットによる、より複雑なタスクを実行するための技術の研究が盛んに行われるようになってきた。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンドなどのエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、まず、カメラで撮像された実環境を撮影する。そして、撮影された画像に、対象物体のモデル情報をフィッティングすることで実環境における対象物体の位置姿勢を計測し、計測結果に基づいて移動計画を策定して実際にアクチュエータを制御する必要がある。

このようなロボットによる部品把持のための実環境におけるモデル情報のフィッティングでは、実環境のノイズや部品の形状及びテクスチャの複雑さに対してロバスト性が求められ、これまでも様々な研究がなされてきた。

特許文献１に記載の方法では、カメラの位置姿勢をロバストに推定する方法として、まず、カメラの位置姿勢が取り得る範囲に所定のサンプリング間隔で複数の初期位置姿勢を生成する。そして、それぞれの初期位置姿勢においてフィッティングの繰り返し計算を行い、最も成績の良いフィッティング結果をフィッティングの最終結果とする方法が提案されている。

特開２０１２−２６９７４号公報

しかしながら、特許文献１に開示されている方法は、カメラの位置姿勢が取り得る範囲として妥当な範囲を設定できることを前提としており、この前提が満たされない場合はロバスト化の効果が十分に得られないという問題がある。

本発明は、以上の課題に鑑みてなされたものであり、ロバストな位置姿勢計測を行うことを目的とする。

上記目的を達成するために、本発明によれば、位置姿勢計測装置に、対象物体が含まれる画像から、前記対象物体の少なくとも１つの概略位置姿勢を取得する取得手段と、前記取得した概略位置姿勢の分解能に基づいて、前記対象物体の位置姿勢の導出のための初期値として、少なくとも１つの位置姿勢候補を新たに生成する生成手段と、前記対象物体のモデル情報と、前記初期値として生成された位置姿勢候補とに基づいて、前記対象物体の位置姿勢を導出する導出手段とを備える。

本発明によれば、ロバストな位置姿勢計測を行うことができる。

第１〜第７の実施形態におけるシステムの構成を示した図である。 プログラムのモジュール構成を示す図である。 ３次元モデル情報の構成を示す図である。 フィッティング処理手順の全体を示すフローチャートである。 概略位置姿勢の分布から位置姿勢候補を生成する方法を説明する図である。 位置姿勢の算出手順を示すフローチャートである。 対応点の探索の原理を示す図である。 エッジの投影像と検出されたエッジの関係を説明する図である。 位置姿勢の取り得る分布を修正して初期位置姿勢を生成する方法を示す図である。 概略位置姿勢における３次元モデルを示す図である。 概略位置姿勢の３次元モデルからエッジ検出点の分布の求める原理を示す図である。 計測対象物体の背景にノイズが含まれる輝度画像を示す図である。 輝度画像からエッジ検出点の分布を求める原理を示す図である。 第６の実施形態の概念を示す図である。 第８の実施形態におけるロボットシステムの構成例を示す図である。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。

［第１の実施形態］
本実施形態では、撮像装置と投影装置によって対象物体の輝度画像と距離画像を同時に取得し、３次元モデル情報を輝度画像に対して２次元の尺度で位置合わせし、距離画像に対して３次元の尺度で位置合わせを行い、２次元と３次元の尺度で同時に評価する際に、本発明を適用する方法を説明する。なお、ここでは２次元と３次元の尺度で同時に位置合わせをする例で説明するが、本発明は２次元の尺度のみであったり、３次元の尺度のみであっても全く同様に適用可能である。

３次元モデル情報の初期位置の設定においては、撮像部で撮影された輝度画像を全探索することで、位置姿勢の取り得る分布（概略位置姿勢）を取得する。取得された概略位置姿勢の分布に基づいて、複数の初期位置姿勢を生成し、それぞれの位置姿勢候補で位置合わせを行い、最も合致した位置姿勢を最終的な位置合わせ結果として出力する。

こうすることで、局所解となり得る位置姿勢を網羅的に検証することとなり、位置合わせのロバスト性を向上できる。以降処理手順の詳細について説明する。なお、本発明における全ての実施形態において、特に明記しない限り、位置合わせとは、計測対象物体の２次元または３次元形状モデルを、画像中の計測対象物体の位置、または、位置および姿勢に合わせる（フィッティングを行う）ことを示す。

図１は、第１の実施形態における位置姿勢計測装置のシステム構成を示す図である。

計測対象物体１０１は、位置及び姿勢（位置姿勢）を計測する対象としての計測対象物体である。本実施形態では、説明を簡単にするために、計測対象物体１０１は図１に示すように、一つ置かれているものとする。しかし、以下に説明する位置姿勢計測処理は、計測対象物体の形状、個数、載置形態に大きく依存するものではない。例えば複数の異形状の計測対象物体が混合されて山積みされているような状態において、ある計測対象物体の位置姿勢を計測する場合にも、本発明は適用可能である。

投影装置１０２は、計測対象物体１０１にパターン光を投影する。投影装置１０２につついては詳しくは後述する。

撮像装置１０３は、静止画もしくは動画像を撮像する装置であり、本実施形態では、計測対象物体１０１および、投影装置１０２によってパターン光が投影された計測対象物体１０１を撮像するために使用する。そして、撮像装置１０３は、撮像した画像（撮像画像）を位置姿勢計測装置１０４に対して送出する。撮像装置１０３については詳しく後述する。

位置姿勢計測装置１０４は、投影装置１０２と撮像装置１０３に接続されており、本発明の具体的な処理が記述されたプログラムを内部に保持し、内蔵するＣＰＵがそのプログラムを実行することで、投影装置１０２及び撮像装置１０３を制御し、所望の位置姿勢計測結果を算出する装置である。

次に、位置姿勢計測装置１０４の機能構成例について、図２のブロック図を用いて説明する。

３次元モデル情報２０２は、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換した、計測対象物体の形状情報である。本実施形態では、計測対象物体１０１の形状を模した３次元幾何モデルであって、ポリゴン要素で構成される３次元幾何モデルを用いる。３次元モデル情報の構成については図３を用いて説明する。

ポリゴン要素で構成される３次元幾何モデルは、図３に示すような点、線、面といった構成要素からなる。図３（ａ）〜（ｃ）はいずれも同じ３次元幾何モデルを示している。

ポリゴン要素で構成される３次元幾何モデルのモデル情報では、図３（ａ）に例示している３次元幾何モデルの各頂点について、図３（ｄ）に示すごとく、該頂点のインデックスと、該頂点の３次元座標値とを管理している。

また、このモデル情報では、図３（ｂ）に例示している３次元幾何モデルの各辺について、図３（ｅ）に示すごとく、該辺のインデックスと、該辺の両端における頂点のインデックスとを管理している。

また、このモデル情報では、図３（ｃ）に例示している３次元幾何モデルの各面（ポリゴン）について、図３（ｆ）に示すごとく、該ポリゴンのインデックスと、該ポリゴンのインデックスと、該ポリゴンの法線ベクトルとを管理している。

このような３次元モデル情報２０２は、位置姿勢計測装置１０４内の適当なメモリ、もしくは、位置姿勢計測装置１０４がアクセス可能な外部メモリに格納されている。

概略位置姿勢取得部２０３は、計測対象物体１０１のとりうる位置姿勢（概略位置姿勢）を取得する。具体的な方法については、後述する。

位置姿勢候補生成部２０４は、位置姿勢分布決定部２０３によって得られた概略位置姿勢の分布に基づいて複数の位置姿勢候補の生成を行う。具体的な方法については、後述する。

位置姿勢候補選択部２０５は、複数の位置姿勢候補の中から一つ以上、位置姿勢を算出するべき位置姿勢候補を選択する。複数の位置姿勢候補をふるい分けすることで計算時間を短縮する。具体的な方法については、後述する。

位置姿勢算出部２０６は、位置姿勢選択部２０５で選択された位置姿勢候補の示す位置姿勢で、３次元モデルを撮像装置１０３によって得られた輝度画像及び距離画像に位置合わせをし、最終的な位置姿勢を算出する。

撮像装置１０３は、カメラであり、計測対象物体１０１が配置されている作業スペースを撮影する。本発明の説明において単に「座標系」や「位置姿勢」といった場合は、それぞれこのカメラの「座標系」、このカメラ座標系における「位置姿勢」をあらわす。一方で「モデルの座標系」は部品の３次元モデルの点や面の位置を表すのに使用するモデル毎に定義される座標系を表す。カメラの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は例えば、以下の文献に記載されている方法により、事前に校正しておく。
Ｚ． Ｚｈａｎｇ， “Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｖｏｌ．２２， ｎｏ．１１， ｐｐ．１３３０−１３３４， ２０００

投影装置１０２は、液晶プロジェクタでパターン光を投影する。プロジェクタの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）についてもカメラの校正と同様の原理で事前に校正しておく。投影装置１０２は、もちろん、液晶プロジェクタに限るものではなく、パターン光の投影ができれば他の方式のものでもよい。例えば、ＤＭＤ（デジタルミラーデバイス）やＬＣＯＳを用いたプロジェクタであってもよい。

位置姿勢計測装置１０４は、投影装置１０２、撮像装置１０３を制御し、計測対象物体の輝度画像及び距離画像を取得する。距離画像は、画像内に写りこんだパターン光の形状に基づいて三角測量の原理で生成される。この方法は一般的にアクティブステレオと呼ばれるが、本発明は、アクティブステレオに限定されるものではなく、パッシブステレオなど他の方法で距離画像を生成しても同様に本発明の実施は可能である。

図４は、以上の要素で構成される情報処理装置のフィッティング処理の手順を示したフローチャートである。本フローチャートで処理手順の概要を説明し、後に本発明の実施において特に重要な点を詳細に説明する。

（ステップＳ４０１）
ステップＳ４０１では、撮像部１０３が作業スペースの計測対象物体１０１を撮影し、計測対象物体１０１の輝度画像及び距離画像を取得する。

（ステップＳ４０２）
ステップＳ４０２では、輝度画像を全探索して、計測対象物体１０１の位置姿勢の取り得る分布（複数の概略位置姿勢）を決定する。画像を全探索して、画像中の計測対象物体１０１の概略位置姿勢を算出することは広く行われており、例えば、以下の非特許文献では、パターンマッチングを用いて部品の３次元モデルの画像内における位置姿勢を算出する方法が開示されている。
吉井裕人、 “アンサンブル分類木を用いたバラ積み部品の概略位置姿勢検出”、 画像の認識・理解シンポジウム（ＭＩＲＵ２０１０）、 ２０１０

このような方法では、画像内で複数回パターンマッチングを行い、１回のマッチングで得られた結果を１回の投票として記録し、最後に投票結果を集計して全探索の結果として提示する。この１回の投票は、情報として計測対象物体の位置姿勢の６パラメータを持つ（ただし、実際には計算を効率化するため、一部の次元はクラス化されて代表値で表されることもある）。本発明では、この位置姿勢の６パラメータを伴う複数の投票を対象部品の取り得る位置姿勢分布として利用する。

図５（ａ）は、このようにして得られた概略位置姿勢の分布の一例である。本実施形態では、簡単のため、位置姿勢をｘ、ｙの２パラメータで表すが、実際のフィッティングにおける６パラメータの位置姿勢においても同様に実行することが可能である。

なお、本実施形態における画像内での特定の部品の位置姿勢を探索する手法は、輝度画像のみに限定されるものではなく、距離画像などの３次元情報を探索する手法であってもよい。

さらに本実施形態では、画像を全探索する場合を説明したが、本発明の実施においては必ずしも得られた画像を全探索する必要はない。例えば、部品が常に作業スペース内のほぼ同じ場所にあることが分かっている場合には、その領域に限定して部分的に探索するようにしても問題はない。

（ステップＳ４０３）
ステップＳ４０３では、ステップＳ４０２で得られた概略位置姿勢の分布に基づいて所定の個数の位置姿勢候補を生成する。

まず、概略位置姿勢の分布より平均ベクトルμと共分散行列Σを算出してガウス分布を適用する。図５（ｂ）はこうして得られた２次元ガウス分布の確率密度が一定となる等高線を図５（ａ）に重ねたものである。

次に求められたガウス分布に従って、所定の個数の位置姿勢候補を生成する。所定の個数とはシステムに要求される許容時間から逆算して適切な個数を設定すればよい。図５（ｃ）はこのようにして生成した位置姿勢候補である。

この例では、ステップＳ４０２で得られた概略位置姿勢の分布（図５（ｃ）で黒丸でプロットされている点）と、新たに生成した位置姿勢候補（図５（ｃ）で黒三角でプロットされている点）の合計が２０個になるように生成し、これら２０個の点を初期位置姿勢とする（初期位置姿勢生成）。

なお、本実施形態では、概略位置姿勢として挙げられている位置姿勢には一定の合理性があると考え、位置姿勢候補として採用した。しかし本発明の実施においては、概略位置姿勢として挙げられたものは採用せずに、新たに生成した位置姿勢だけを採用してもよい。

また、確率密度モデルとして、ガウス分布を適用したが、実際には得られた概略位置姿勢の分布を要約できる分布であれば他の分布（所定の分布関数）であってもよいし、もっと簡単に方形状の分布を適用してもよい。

（ステップＳ４０４）
ステップＳ４０４では、ステップＳ４０３で得られた複数の位置姿勢候補の中からスコア（３次元モデルと輝度画像及び距離画像の合致度を表す）の高い位置姿勢候補を一以上の所定の個数、選択する（初期位置姿勢選択）。具体的には後述する位置姿勢算出における位置姿勢更新処理、ステップＳ６０１〜Ｓ６０６を１回行い、その結果得られるスコアを比較して高いものから順に所定の個数、抽出する。

このようにして選択された位置姿勢候補に対してのみ、反復を伴う位置姿勢算出処理を行うことでフィッティングにかかる総時間を短縮する。ここでも所定の個数はシステムに要求される許容時間から逆算して適切な個数を設定すればよい。またここにおけるスコアは、３次元モデルと、輝度画像及び距離画像との合致している度合いを表す値なら何でもよい。

例えば３次元モデルと輝度画像及び距離画像で誤差ベクトル（後述）を求めそのノルムの逆数を用いる方法が考えられるがその他の指標でもよい。ここで説明した選択処理ではスコアで評価する前に位置姿勢更新処理、ステップＳ６０１〜ステップＳ６０６を１回行っているが（１回対応づけている）、この回数は必ずしも１回である必要はなく、高速且つ妥当な初期位置姿勢の見込みを確かめられる方法ならばなんでもよい。

例えば位置姿勢更新を１回も行わずにスコアを算出してもよいし、または位置姿勢更新を２回以上行ってからスコアを算出してもよい。このように選択処理の具体的な手順については無数の方法が考えられるが本発明の実施においては特にどれでなくてはならないということはない。

（ステップＳ４０５）
ステップＳ４０５では、ステップＳ４０４で選択された一以上の所定の個数の位置姿勢候補のそれぞれで、最終的な位置姿勢を決定する。ここでの位置姿勢算出処理の詳細については図６を用いて説明する。図６はステップＳ４０５で示した１つの初期位置姿勢に関する位置姿勢算出処理の詳細な手順を示したフローチャートである。

本実施形態では、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法により、計測対象物体の位置姿勢候補（以下、ｓで表す）を反復演算により繰り返し補正することにより、位置姿勢を算出する。ここでは、画像上で検出されたエッジと推定される位置姿勢とに基づいて、画像上に投影される３次元モデル上のエッジとの距離、距離画像の点群データを構成する各点と位置姿勢に基づいて、３次元計測装置の座標系に変換される面との距離の総和が最小化されるように位置姿勢を最適化する。

より具体的には、２次元画像上での点と直線の符号付距離、３次元空間中での点と平面との符号付距離を、１次のテイラー展開によってそれぞれ部品の位置姿勢の微小変化の１次関数として表現する。そして、符号付距離が０になるような位置姿勢の微小変化に関する線形の連立方程式を立式して解くことにより、部品の位置姿勢の微小変化を求めて位置姿勢を補正することを繰り返す。

（ステップＳ６０１）
ステップＳ６０１では、初期化を行う。ここでは、ステップＳ４０４で選択された位置姿勢候補を、これ以降「現在の位置姿勢」として記載する位置姿勢の初期値として設定する。

（ステップＳ６０２）
ステップＳ６０２では、対応付けを行う。まず、現在の位置姿勢に基づいて、３次元モデルの各エッジの画像への投影及び各面の３次元計測装置の座標系への座標変換を行う。その後エッジ及び点群データの対応付けを行う。

図７はエッジの対応付け方法を示す図である。

図７（ａ）は輝度画像上に３次元モデルのエッジを投影した図である。まず画像上で等間隔になるように、投影されたエッジ７０１上に制御点７０２を設定し、各制御点ごとに、投影されたエッジ７０１の法線方向に探索ライン７０３をとる。制御点を原点とした探索ラインの所定の範囲内で１次元のエッジの検出を行い、検出されたエッジの中で制御点に最も近い点を対応点１００５として保持する。

図７（ｂ）は原点が制御点で、横軸に探索ライン、縦軸に輝度勾配の絶対値をとるグラフである。先の画像上にエッジは画素値の輝度勾配の絶対値の極値として検出される。ここでは輝度勾配の絶対値の極値が所定の閾値７０５よりも大きく、且つ最も制御点に近い点７０４を対応点とする。

次に点群データの対応付けを行う。点群データ中の各点につき、三次元空間において最も近傍する面を探索し対応付ける。

（ステップＳ６０３）
ステップＳ６０３では、線形連立方程式を解くための係数行列と誤差ベクトルの算出を行う。ここで係数行列の各要素は、位置姿勢の微小変化に対する一次の偏微分係数である。エッジについては、画像座標の偏微分係数を算出し、点群については３次元座標の偏微分係数を算出する。誤差ベクトルは、エッジについては投影されたエッジと検出されたエッジの画像上での距離、点群データについてはモデルの面と点の３次元空間中での距離である。

図８は、エッジの投影像と検出されたエッジとの関係を説明する図である。図８では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点（投影された各エッジを画像上で等間隔に分割する点）の画像上での位置を（ｕ_０、ｖ_０）、該制御点が所属するエッジの画像上での傾きをｕ軸に対する傾きθと表す。傾きθは、エッジの両端の３次元座標をｓに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該エッジの画像上での法線ベクトルは（ｓｉｎθ、−ｃｏｓθ）となる。また、該制御点の対応点の画像座標を（ｕ’、ｖ’）とする。ここで、点（ｕ’、ｖ’）を通り、傾きがθである直線上の点（ｕ、ｖ）は、

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

とした。

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

正しいｓによって得られる制御点の画像上での位置は、数１が表す直線上にあると仮定できる。数２によって近似されるｕ、ｖを数１に代入することにより、数３が得られる。

ただし、

（定数）とした。

３次元計測装置の座標系における３次元座標は、計測対象部品の位置姿勢ｓによってモデル座標系における３次元座標（ｘ、ｙ、ｚ）に変換することができる。概略の位置姿勢により、ある点がモデル座標系の点（ｘ_０、ｙ_０、ｚ_０）に変換されるとする。（ｘ、ｙ、ｚ）は、計測対象物体の位置姿勢により変化するものであり、（ｘ_０、ｙ_０、ｚ_０）の近傍で１次のテイラー展開によって数４のように近似できる。

ステップＳ６０２において、点群データ中のある点に対応付けられた３次元モデルの面の、モデル座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ^２＋ｂ^２＋ｃ^２＝１、ａ、ｂ、ｃ、ｅは定数）とする。正しいｓによって変換される（ｘ、ｙ、ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ^２＋ｂ^２＋ｃ^２＝１）を満たすと仮定する。数４を平面の方程式に代入すると数５が得られる。

ただし、

（定数）とした。

数３は、ステップＳ６０２において対応付けが行われたすべてのエッジについて成り立つ。また、数５はＳ６０２において対応付けが行われたすべての点群データについて成り立つため、数６のようなΔｓ_ｉに関する線形連立方程式が成り立つ。

ここで数６を数７のように表す。

数６の線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば以下の文献に開示されている方法によって行う。
Ｖ． Ｌｅｐｅｔｉｔ ａｎｄ Ｐ． Ｆｕａ， ‘Ｋｅｙｐｏｉｎｔ ｒｅｃｏｇｎｉｔｉｏｎ ｕｓｉｎｇ ｒａｎｄｏｍｉｚｅｄ ｔｒｅｅｓ，’ ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｖｏｌ．２８， ｎｏ．９， ２００６．

（ステップＳ６０４）
ステップＳ６０４では、数７をもとに、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１・Ｊ^Ｔを用いてΔｓを求める。しかしながら、エッジや点群データには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジ（点群データ）では、誤差ｄ−ｒ（ｅ−ｑ）が大きくなる。そのため、数６、数７の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。

そこで、誤差ｄ−ｒ（ｅ−ｑ）が大きいデータには小さな重みを与え、誤差ｄ−ｒ（ｅ−ｑ）が小さいデータには、大きな重みを与える。重みは、例えば数８に示すようなＴｕｋｅｙの関数により与える。

ｃ_１、ｃ_２は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差 大きいデータには小さな重みを与え、誤差小さいデータには大きな重みを与える関数であればなんでもよい。

各データ（エッジまたは点群データ）に対応する重みをｗ_ｉとする。ここで数９のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗ_ｉが入る。この重み行列Ｗを用いて、数７を数１０のように変形する。

数１１のように数１０を解くことにより補正値Δｓを求める。

（ステップＳ６０５）
ステップＳ６０５では、ステップＳ６０４において算出された位置姿勢の補正値Δｓにより、位置姿勢を更新する。

（ステップＳ６０６）
ステップＳ６０６では、位置姿勢が収束したか否かの判定を行い、収束したと判定された場合はその時の位置姿勢をフィッティング結果として処理を終了する。収束していないと判定された場合は、収束するまでステップＳ６０２〜Ｓ６０５を繰り返し行う。収束判定は、ステップＳ６０５の補正量が所定の値以下でほぼ変化がないと判定された場合に収束したと判定する。なお、収束判定の方法はこの方法に限定されるものではなく、例えば、所定の回数、反復したら収束したものとして次のステップに進むことにしてもよい。

（ステップＳ４０６）
ステップＳ４０６では、ステップＳ４０５で求められた一以上の所定の個数の位置姿勢算出結果のスコアを比較して、高いスコアを示す一以上の位置姿勢を最終的なフィッティング結果として出力する。ここにおけるスコアも、ステップＳ４０４で説明したものと同様の指標である。

このように、本実施形態によれば、計測対象物体を画像内で全探索した結果から、計測対象物体の概略位置姿勢の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢候補を網羅的に検証することで、ノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことができる。

なお、本発明における計測対象物体の位置姿勢の算出方法は、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

なお、本実施形態では、位置姿勢計測装置１０４は、撮像画像を撮像装置１０３から直接取得しているが、これに限るものではなく、例えば、撮像装置１０３によって撮像された撮像画像を蓄積しているメモリから取得するようにしても良い。この場合、撮像装置１０３は必ずしも、位置姿勢計測装置１０４に接続されている必要はない。

本実施形態においては、フィッティングする形状情報の組み合わせとして計測対象物体の３次元モデル情報と撮像装置で撮影された部品を含む画像の組み合わせを例に説明したが、形状情報の組み合わせは必ずしもこの組み合わせでなくてもよく、形状情報間で対応を取って位置合わせが可能な情報であればなんでもよい。物体の形状情報の例としては３次元モデル情報、画像のほかに、レーザーレンジセンサーなどで計測して得られた３次元点群情報なども考えられる。

［第２の実施形態］
第１の実施形態では、計測対象物体の概略位置姿勢の分布から、多次元ガウス分布を適用し、その分布に従って位置姿勢候補を生成した。しかし、ガウス分布のような確率密度モデルを適用しないで位置姿勢候補を生成する方法も考えられる。

その方法の一例として、第２の実施形態では、概略位置姿勢の分布を所定のサンプリング間隔で集計して、その集計結果に基づいて位置姿勢候補を生成する方法を説明する。以下はこれまでに説明した位置姿勢分布を決定する第１の実施形態の説明におけるステップＳ４０３に置き換わる処理である。

まず、位置姿勢分布を集計する範囲を決定する。図９（ａ）は、図５（ａ）に示した位置姿勢分布が与えられたときに撮影画像の範囲をｘ１〜ｘ２、ｙ１〜ｙ２としてサンプリングするｂｉｎを区切った様子を示す図である。ｂｉｎの幅ｄはあらかじめ設定しておく定数で、例えば計測対象物体の長辺サイズを所定の値で割った値などが考えられる。ここでは長辺サイズは５０ｍｍの計測対象物体を所定の値１０で割って５ｍｍをｄとして設定する。または、例えば計測対象物体の長辺サイズではなく撮影範囲を所定の数で割った値をｄとして設定してもよい。

次に、上記ｂｉｎを設定した範囲で集計をする。図９（ｂ）は各グリッド内に存在する位置姿勢の数を得票数としてカウントし、グリッドの中央に記載した図である。こうしてカウントされたグリッドの中から最大となる４票を得た左から４番目、上から３番目のｂｉｎを選定する。

最後に、選定されたｂｉｎに所定の間隔で位置姿勢候補を生成する。所定の間隔は、システムに要求される許容時間から逆算して得られる位置姿勢候補の個数に基づき、適切な値を設定すればよい。また、間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけｂｉｎの内部にランダムに生成してもよい。図９（ｃ）はこのようにして位置姿勢候補を９個生成した時の様子を示す図である。

こうして生成した位置姿勢候補に対して、ステップＳ４０４以降の処理を第１の実施形態同様に実行する。

このように、本実施形態によれば、概略位置姿勢の分布を所定のサンプリング間隔で集計して利用することで、第１の実施形態をより簡便で高速に実施することができる。

［第３の実施形態］
第１および第２の実施形態では、計測対象物体が取り得る複数の概略位置姿勢の分布が得られるものとして、概略位置姿勢の分布に基づいて、位置姿勢候補の生成を行う方法について述べた。しかし、位置姿勢候補の生成は，概略位置姿勢の分布に基づく方式に限るものではない。例えば、テンプレートマッチングなどにより概略位置姿勢を推定する場合には、マッチングするテンプレートの数や探索するステップ幅に応じて、推定される位置姿勢の分解能が決定される。このような場合、実際の位置姿勢に対して、概略位置姿勢の分解能の範囲内で、推定される位置姿勢に曖昧性が発生することになる。第３の実施形態では、この点を鑑み、概略位置姿勢推定における分解能に基づいて位置姿勢候補の生成を行う方法について説明する。以下は、これまでに説明した第１の実施形態の説明における位置姿勢候補の生成工程であるステップＳ４０３に置き換わる処理である。

まず、概略位置姿勢の分解能を決定する。ここで、概略位置姿勢の分解能とは，位置と姿勢を表現する最小ステップ幅のことを表す。例えば、位置の分解能が１［ｍｍ］である場合は、１［ｍｍ］よりも細かい値で概略位置姿勢が推定されることはない。概略位置姿勢の分解能は、概略位置姿勢の標本化幅や精度，計測対象物体のサイズ等により決定される。例えば、概略位置姿勢の推定方法として、第一の実施形態のステップＳ４０２で例に挙げたようなパターンマッチングベースの方法を利用する場合、姿勢の分解能はマッチングに利用するパターンとして登録する姿勢のサンプリング幅から決定される。例えば測地ドーム状に８度刻みで姿勢パラメータをサンプリングしてパターンマッチングする場合、サンプリング幅となる８度が姿勢の分解能となる。また、位置の分解能は、姿勢の分解能によって位置の推定を間違いうる範囲を計算することで求める。具体的には、以下の式に基づいて，姿勢パラメータの分解能ω［ｒａｄ］と計測対象物体の長辺サイズｗ ［ｍｍ］に基づいて、姿勢に基づく位置の最大ズレ幅ｄ ［ｍｍ］を求め，算出したｄを位置の分解能として設定する。

本実施形態では、以上に示す方法により、姿勢のサンプリング幅と計測対象物体のサイズに基づいて、概略位置姿勢の分解能を決定する。ただし，概略位置姿勢の決定方法は以上に示す方法には限るものではない。例えば、位置の分解能を，概略位置姿勢の推定に利用する画像の解像度に基づいて設定しても良いし、以下の非特許文献に示す方式によって撮像素子ノイズ量や計測対象物体の形状に基づいて推定した位置姿勢推定の曖昧性に基づいて設定しても良い。
Ｗ． Ｈｏｆｆ ａｎｄ Ｔ． Ｖｉｎｃｅｎｔ， “Ａｎａｌｙｓｉｓ ｏｆ ｈｅａｄ ｐｏｓｅ ａｃｃｕｒａｃｙ ｉｎ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ， ”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｖｉｓｕａｌｉｚａｔｉｏｎ ａｎｄ Ｃｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ， ｖｏｌ．６， ｎｏ．４， ｐｐ．３１９−３３４， ２０００．

また、設定パラメータとしてあらかじめ人が設定してもよい。概略位置姿勢の取りうる最小ステップ幅としての分解能を決定できる限り、いかなる決定方法を用いても良く、その決定方法によって本発明の本質が損なわれることはない。

次に、概略位置姿勢の分解能に基づいて、位置姿勢候補を生成する。具体的には、概略位置姿勢を中心とした概略位置姿勢の分解能幅の範囲内で、所定の間隔で位置姿勢をサンプリングすることで、位置姿勢候補を生成する。サンプリングする間隔は、システムに要求される許容時間から逆算して得られる位置姿勢候補の個数に基づき、適切な値を設定すればよい。また、間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけランダムに生成してもよい。概略位置姿勢としては、第一の実施形態のステップＳ４０２において得られた概略位置姿勢の分布の平均値を用いればよい。また、概略位置姿勢の分布ではなく、単一の概略位置姿勢が取得される場合には、それを用いればよい。なお、位置姿勢候補の生成方法は以上の方法に限るものではなく、例えば，第一の実施形態と同様に，概略位置姿勢の分解能から、位置姿勢が取りうる範囲をガウス分布に近似し、ガウス分布に基づいて位置姿勢を生成しても良い。この場合，位置姿勢６パラメータに関する多次元ガウス分布の平均ベクトルμは、第一の実施形態のステップＳ４０２において取得した概略位置姿勢の分布の平均から求める。そして、ガウス分布の分散共分散行列Σを概略位置姿勢の分解能幅から設定することで、概略位置姿勢の分解能に基づいて多次元ガウス分布を算出する。そして、求めたガウス分布に基づいて、第１の実施形態のステップＳ４０３と同様に所定の個数の位置姿勢をサンプリングすることで位置姿勢候補を生成する。なお，ガウス分布に基づく位置姿勢の生成は，分解能に基づくガウス分布に加え、第１の実施形態で示すような概略位置姿勢の分布を近似した分布を併用して生成しても良い。この場合、具体的には、第１の実施形態のステップＳ４０３の方法で求めたガウス分布に、上述の分解能に基づくガウス分布を足し合わせた分布を算出する。そして、求めた分布に基づいて、第一の実施形態のステップＳ４０３と同様の方法で位置姿勢候補を生成すれば良い。

以上に示すように、位置姿勢候補の生成にあたっては、概略位置姿勢の分解能に基づいて位置姿勢候補を生成できる限り、生成方法に特に制限はなく、いかなる方法を用いても特に問題はない。

こうして生成した位置姿勢候補に対して、ステップＳ４０４以降の処理を第１の実施形態同様に実行する。

このように、本実施形態によれば、概略位置姿勢およびその分解能が既知であるシーンにおいて、分解能に基づいて決定した位置姿勢の誤りうる分布に基づいて候補となる位置姿勢を生成することで、粗い分解能で概略位置姿勢が取得されるような場合においても、ロバストな位置姿勢計測を行うことが可能になる。

［第４の実施形態］
第１の実施形態では、計測対象物体が取り得る概略位置姿勢分布を全探索によって取得した。しかし、実際にロボットが計測対象物体を把持して組み付ける現場では、部品はあらかじめ整列されており、全探索するまでもなく、配置された部品の位置姿勢がほぼ一定であるとみなせる場合がある。

第４の実施形態では、概略位置姿勢における対象部品の３次元モデル情報に含まれるエッジ分布に基づいてモデル情報の存在し得る位置姿勢分布を決定する方法を説明する。以下はこれまでに説明した位置姿勢候補分布を決定する第１の実施形態の説明における、ステップＳ４０２に置き換わる処理である。

図１０は既知である概略位置姿勢における計測対象物体を表す図である。

３次元モデル１００１は、概略位置姿勢で配置された３次元モデルである。

重心１００２は、３次元モデルの重心であり、システムの座標系において３次元モデルの位置を指し示す時はこの重心の座標で示される。

テクスチャ１００３は、計測対象物体が持つ縦縞のテクスチャ（テクスチャ情報）である。このようなテクスチャ１００３をもつ計測対象物体を、エッジを用いて位置合わせする際は、対応点を探索する際に誤って隣の縦縞に対応してしまい、その結果、縦縞に対して垂直な方向にずれたフィッティングをされてしまうリスクがある。

本実施形態では、計測対象物体の取り得る位置姿勢（概略位置姿勢）は、上記の説明のように縦縞に対して垂直な方向に分布していると考え、この方向に網羅的に初期値を設定し、それぞれ位置姿勢を算出した結果を比較する。そして、よく合致しているものを最終的に正しい位置姿勢として出力する。

以降、その詳細を説明する。

図１１（ａ）は、３次元モデル上の制御点を起点としてエッジを探索する様子を示す図である。制御点１１０１、１１０２、１１０３は、制御点の例である。これは第１の実施形態で説明した制御点７０２と同様のものである。それぞれの制御点について、制御点が乗っているエッジと垂直方向に探索ラインをとり、エッジの分布を探索する。エッジの探索については、第１の実施形態のステップＳ６０２で説明した方法と同じ方法で行う。

検出点１１０４は、制御点１１０１を起点に探索した時のエッジの検出点である。このときの検出点１１０４と制御点１１０１との相対位置を、重心１００２を起点に置き換えた位置として記録する。

次に制御点１１０２を起点にエッジを探索する。この場合は図に示されるように探索ライン上にエッジがないため何も検出されない。同様にしてすべての制御点でエッジの探索を行い、検出された点と制御点との相対位置を重心を起点にして置き換えた位置として記録する。

図１１（ｂ）は、このようにして記録されたエッジ検出点の分布（各制御点から検出されたエッジ検出点の位置の分布を重心を起点に置き換えた位置の分布）を図示したものである。図中にＸでプロットされた２つの点は制御点１１０１から検出された２つの点を示す。図１１（ａ）の１１０４と図１１（ｂ）の１１０４とが対応している。図中の黒三角でプロットされた点は他の制御点から検出された点を示す。なお、図１１（ｂ）では説明のためにプロットの記号を使い分けたが、以降の処理ではこれらはすべて同等の分布を示す点として扱う。こうして得られたエッジ検出点の分布は、３次元モデルの制御点から対応点を探索する際に、どちらの方向にどれだけの３次元モデル内在点が誤対応され得るかを表している。

従って、この分布の位置に局所解が多いこととなり、位置姿勢候補をこの分布に従って生成することでフィッティングのロバスト性を向上させることができる。この分布を第１の実施形態における位置姿勢候補として用いて、ステップＳ４０３以降の処理を行う。

本実施形態では誤対応する可能性があるエッジとしてテクスチャのエッジを例に説明したが、本発明の効果はテクスチャに由来するエッジに限るものではない。計測対象物体の形状に由来するエッジなどテクスチャ以外の特徴に起因するものであっても何ら変わりはない。

形状由来の誤対応しやすいエッジとして簡単な例では、のこぎりのギザギザ歯のような繰り返しパターンを持つ形状が考えられる。このような形状においては、本実施形態の手法によると、ギザギザの歯が並んでいる方向に取り得る位置姿勢が分布していると考えて網羅的に位置姿勢候補を生成することで、最適な位置姿勢を決定することができる。

本実施形態では、本発明で扱うフィッティングの支配的な誤差要因がエッジの誤対応であると考え、位置姿勢候補の分布を撮像画像の２次元平面内において、位置姿勢のうちｘｙ方向の並進成分についてのみ求めた。

しかし、この位置姿勢候補の分布の決定は、２次元に限るものではなく、３次元においても同様に行うことができる。図１１を用いて説明した３次元モデル上の制御点を起点としたエッジの探索と同等の処理を３次元で行えばよい。具体的には３次元モデル上の制御点が乗る面の法線方向に面を探索して分布を生成することが考えられる。

また、２次元及び３次元の回転についても同様に探索を行って位置姿勢候補の分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において３次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。つまり、概略位置姿勢と３次元モデルの情報から、位置姿勢候補を求め、複数の位置姿勢候補を生成することでロバスト性の向上を図ることが本発明の第４の実施形態の主旨である。

このように、本実施形態によれば、概略位置姿勢が既知であるシーンにおいて、既知の概略位置姿勢と３次元モデル情報とから位置姿勢候補の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することで複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。

［第５の実施形態］
第４の実施形態では、計測対象物体の概略位置姿勢と３次元モデルとから仮想環境上で、計測対象物体が取り得る位置姿勢分布を求めた。そして、計測対象物体の概略位置姿勢の３次元モデルと撮像画像の対応点を生成する際に、部品に内在する別のエッジに誤対応してしまう問題に対応した。

この他の問題として、撮像装置によって得られた画像の概略位置姿勢における部品周辺に分布するエッジに誤対応してしまう問題が考えられる。第５の実施形態では配置された計測対象物体の概略位置姿勢が既知である状況下で、位置姿勢候補を、撮像画像に含まれる特徴の、概略位置姿勢における３次元モデル情報の原点を基準とする相対位置分布に基づいて決定することで上記の問題に対応する方法を説明する。

以下はこれまでに説明した位置姿勢分布を決定する第１の実施形態の説明におけるステップＳ４０２に置き換わる処理である。

図１２は、撮像して得られた輝度画像である。枠１２０１は撮像部より得られる輝度画像の外枠である。計測対象物体１２０２は、輝度画像に写りこんだ計測対象物体である。横縞１２０３は、輝度画像に写りこんだ横縞（ノイズ）である。ここでいうノイズとは、計測対象物体１２０２自身ではないということを示している。すなわち、計測対象物体の背景に存在する他の物体であったり、背景そのものに起因するものであってもよい。

本実施形態においては、ノイズが、他の物体に起因するものであることを想定している。このような場合において、計測対象物体１２０２を、エッジを用いてフィッティングする際は、対応点を探索する際に誤って、計測対象物体の上下にある、その他の物体に起因した横縞の背景に対応してしまい、図中の縦方向にずれたフィッティングをされてしまうリスクがある。

本実施形態では、部品の取り得る位置姿勢は上記の説明のように図中の縦方向に分布していると考え、この方向に網羅的に位置姿勢候補を設定し、それぞれ位置姿勢を算出した結果を比較し、よく合致しているものを最終的に正しい位置姿勢として出力する。

以降、その詳細を説明する。（なお、ここでは手法の効果を分かりやすくするために横縞の背景を例に説明するがいかなるテクスチャでも同様の方法で本発明の実施は可能である。）
図１３（ａ）は、輝度画像上で３次元モデル上の制御点を起点としてエッジを探索する方法を示す図である。

座標軸１３０１は、システムの座標系を示す。投影像１３０２は、概略位置姿勢における３次元モデルエッジの輝度画像への投影を示す。投影点１３０３は、概略位置姿勢における３次元モデルの重心を輝度画像に投影した点である。

制御点１３０４、１３０５、１３０６は、制御点の例である。これらは、第１の実施形態で説明した制御点７０２と同様のものである。これらそれぞれの制御点について制御点が乗っているエッジに垂直な方向に探索ラインをとり、エッジを探索する。エッジの探索については第１の実施形態のＳ６０２で説明した方法と同じ方法で行う。検出点１３０７は、制御点１３０１を起点に探索した時のエッジの検出点である。このときの制御点１３０１との相対位置を重心１３０３を起点とした置き換えた位置として記録する。次に制御点１３０２を起点にエッジを探索する。この場合は右方向に一つ検出点が記録される。同様にしてすべての制御点でエッジの探索を行い、検出された点と制御点との相対位置を重心を起点にして置き換えた位置として記録する。

図１３（ｂ）は、このようにして記録されたエッジ検出点の分布（各制御点から検出されたエッジ検出点の位置の分布を重心を起点に置き換えた位置の分布）を図示したものである。図中にＸでプロットされた３つの点は制御点１３０５から検出された３つの点を示す。図１３（ａ）の１３０７と図１３（ｂ）の１３０７は対応している。図中の黒三角でプロットされた点は他の制御点から検出された点を示す。

なお、図１３（ｂ）では説明のためにプロットの記号を使い分けたが、以降の処理ではこれらはすべて同等の分布を示す点として扱う。こうして得られたエッジ検出点の分布は、３次元モデルの制御点から対応点を探索する際に、どちらの方向にどれだけ誤対応点が検出され得るかを表している。従ってこの分布の位置に局所解が多いこととなり、初期位置姿勢をこの分布に従って生成することでフィッティングのロバスト性を向上させることができる。この分布を第１の実施形態における位置姿勢分布として用いてＳ４０３以降の処理を行う。

本実施形態では誤対応する可能性があるエッジとしてテクスチャのエッジを例に説明したが、本発明の効果はテクスチャに由来するエッジに限るものではなく３次元モデルの形状に由来するエッジや背景に由来するエッジなどテクスチャ以外の特徴に起因するものであっても何ら変わりはない。

本実施形態では第４の実施形態同様、位置姿勢分布を撮像画像の２次元平面内において位置姿勢のうちｘｙ方向の並進成分についてのみ求めた。しかし、この位置姿勢分布の決定は２次元に限るものではなく３次元においても同様に行うことができる。

図１３を用いて説明した３次元モデル上の制御点を起点としたエッジの探索と同等の処理を３次元で行えばよい。具体的には３次元モデル上の制御点を起点として制御点が乗る面の法線方向に一点の間隔で距離画像上の対応点を探索して対応点の分布をとることが考えられる。

また、２次元及び３次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において３次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。要は扱う次元に依らず、撮像された画像を全探索して位置姿勢分布を求めるのではなく、撮像された画像のうち、概略位置姿勢で配置された３次元モデルの周辺のみを探索することで複数の初期位置姿勢を生成し、ロバスト性の向上を図ることが本発明における第５の実施形態の主旨である。

このように、本実施形態によれば、概略位置姿勢が既知であるシーンにおいて、画像内の概略位置姿勢における３次元モデルの周辺で探索することで位置姿勢分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことができる。

［第６の実施形態］
第４〜第５の実施形態では、位置姿勢分布をフィッティングする先の形状情報として画像を対象に、該画像の上に投影された３次元モデルの位置姿勢を求めた。しかし、各実施形態において補足したように、本発明は形状情報の一方が画像でなくてはならないという制約はなく、二つの形状情報の間に対応が取れる領域があれば、任意の形態の形状情報のフィッティングにおいて本発明は適用可能である。

例えば互いに三次元形状情報同士をフィッティングする際に本発明を適用することも可能である。その例としてここでは、ある対象物体を二つの視点から撮影して生成した二つのメッシュモデルを、互いにオーバーラップ（重複）している領域の形状に基づいてフィッティングする例を説明する。つまり、それぞれのメッシュモデルは、少なくとも１部の領域で重複している。このように３次元形状情報同士をフィッティングする場合においても本発明は適用可能であることを説明する。

図１４は、点線で描かれているメッシュモデル１４０１（第１の形状情報）を、実践で描かれているメッシュモデル１４０２（第２の形状情報）にフィッティングする例を２次元に投影して説明する図である。１４０３は形状情報１４０１上に定められた制御点である。各制御点においてその点が乗っている面の法線方向に一定区間対応点候補を探索する。探索して見つかった対応点候補は図１１（ｂ）または図１３（ｂ）と同様に制御点から対応点候補へのベクトルを形状情報１４０１の重心を中心にしてプロットして三次元空間上での概略位置姿勢候補の分布を生成する。この分布を第１の実施形態における位置姿勢分布として用いてＳ４０３以降の処理を行う。

［第７の実施形態］
第１〜第６の実施形態ではそれぞれ異なる方法で位置姿勢分布を生成した。実際にはこれらを独立に実施するのではなく、これらの一部または全部の和の分布を位置姿勢分布として用いてＳ４０３にて複数の初期位置姿勢を生成（初期位置姿勢生成）することでよりロバストな位置姿勢計測を実現できる。

［第８の実施形態］
本発明に係る情報処理装置の好適な適用事例としては、産業用ロボットアームで部品の把持、移動、解放といった動作を行うシステムにおいて対象部品の位置姿勢計測に利用することが挙げられる。以下、図１５を用いて、本発明に係る情報処理装置の適用例を説明する。

図１５は、本発明の第７の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。１５０１はＰＣ、１５０２はカメラ及びプロジェクタである。ＰＣ１５０１とカメラ及びプロジェクタ１５０２は第１〜第６の実施形態で説明したシステムと同様のものであり、先に説明した処理手順が記述されたプログラムを格納し、対象部品１５０５の位置姿勢の計測が可能である。１５０３はロボットコントローラでロボットアームを制御する。１５０４はロボットアームであり、回転軸及び／または並進移動軸からなる可動軸を有する。ロボットコントローラはＰＣに接続されており、ロボットアームへの命令はＰＣから受け取ったものを具体的な制御命令に変換してロボットアームを動かす。

本実施形態におけるＰＣは上記に加えてさらに次の処理手順を包含する。まず第１の実施形態１〜５で説明したシステム座標系すなわちカメラ座標系とロボットアームで定義されるロボット座標系の相対関係を保持する。ＰＣは対象部品の位置姿勢を計測し、その結果をカメラの座標系からロボット座標系に変換する。次に変換された部品のロボット座標系における位置姿勢を元にロボットコントローラを介してロボットアームが部品を把持できる位置姿勢に移動させる。

このように、本実施形態によれば、カメラとプロジェクトをアーム上に設置したロボットシステムにおいて、本発明による部品の位置姿勢の計測結果に基づいてロボットアームを処理することで未知である部品の把持をすることができる。

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

＜実施形態の効果＞
以上説明したように、対象物体の概略位置姿勢を取得し、取得した概略位置姿勢に基づいて、新たに初期位置姿勢の候補を生成し、それらに基づいて位置姿勢を導出することで、ロバストな位置姿勢計測を可能にしている。

第１の実施形態では、計測対象物体を画像内で全探索した結果から、概略位置姿勢の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の計測対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。

第２の実施形態では、概略位置姿勢の分布を所定のサンプリング間隔で集計して利用することで、第１の実施形態をより簡便で高速に実施する方法を示した。

第３の実施形態では、概略位置姿勢およびその分解能が既知であるシーンにおいて、分解能に基づいて決定した位置姿勢が誤りうる分布に基づいて候補となる位置姿勢を生成することで、粗い分解能で概略位置姿勢が取得されるような場合においても、ロバストな位置姿勢計測を行うことが可能になる
第４の実施形態では、概略位置姿勢が既知であるシーンにおいて、既知の概略位置姿勢と３次元モデル情報から位置姿勢候補を生成し、生成された位置姿勢候補に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することで複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。

第５の実施形態では、概略位置姿勢が既知であるシーンにおいて、画像内の概略位置姿勢における３次元モデルの周辺で探索することで位置姿勢候補を生成し、生成された位置姿勢候補に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。

第６の実施形態では、二つの形状情報の間に対応が取れる領域があれば、任意の形態の形状情報のフィッティングにおいて本発明は適用可能であるという例を示した。これにより、ＣＡＤモデルなどの形状データが未知の物体に対して、複数の方向から撮像したデータをフィッティングしてデータを結合することが可能になる。

第７の実施形態では、第１〜第５の実施形態で示された位置姿勢候補の生成方法のうちの全部または一部の方法で生成された位置姿勢候補の和集合を複数の位置姿勢候補として利用することでより、ロバストな位置姿勢計測を行うことを可能にした。

第８の実施形態ではカメラとプロジェクトをアーム上に設置したロボットシステムにおいて、本発明よる部品の位置姿勢の計測結果に基づいてロボットアームを処理することで未知である部品の把持を可能にした。

＜定義＞
本発明の説明では、フィッティングする形状情報の組み合わせとして第１〜第５の実施形態においては３次元モデル情報とカメラで撮影された計測対象物体を含む画像の組み合わせを、第６の実施形態においてはおよび三次元メッシュモデル同士の組み合わせを例に説明した。しかし、必ずしもこの組み合わせでなくてもよく、形状情報間で対応を取って位置合わせ（フィッティング）可能な情報であればなんでもよい。物体の形状情報の例としては３次元モデル情報、画像のほかに、レーザーレンジセンサーなどで計測して得られた３次元点群情報なども考えられる。

プロジェクタは液晶プロジェクタに限るものではなく、パターン光の投影ができれば他の方式のものでもよい。例えば、ＤＭＤ（デジタルミラーデバイス）やＬＣＯＳを用いたプロジェクタであってもよい。

位置姿勢算出においては、２次元と３次元の尺度で同時に位置合わせをする例を説明したが、本発明は２次元の尺度のみであったり、３次元の尺度のみであっても全く同様に適用可能である。

位置姿勢算出において、計測対象部品の位置姿勢の算出方法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

概略位置姿勢の分布決定において、画像内での特定の部品の位置姿勢を探索する際の画像は輝度画像のみに限定されるものではなく距離画像などの３次元情報を探索する手法であってもよい。

概略位置姿勢の分布決定において、画像を全探索する場合を説明したが、本発明の実施においては必ずしも得られた画像を全探索する必要はない。例えば部品が常に作業スペース内のほぼ同じ場所にあることが分かっている場合はその領域に限定して部分的に探索するなどしても何ら問題はない。

本発明の第４の実施形態で説明した位置姿勢分布決定において、位置姿勢分布の探索範囲は２次元平面内におけるｘｙ方向の並進成分のみに限定されない。位置姿勢分布の決定は２次元に限るものではなく３次元においても同様に行うことができる。図１１を用いて説明した３次元モデル上の制御点を起点としたエッジの探索と同等の処理を３次元で行えばよい。

具体的には３次元モデル上の制御点が乗る面の法線方向に面を探索して分布を生成することが考えられる。また、２次元及び３次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において３次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。

本発明の第５の実施形態で説明した概略位置姿勢分布決定において、位置姿勢分布の探索範囲は２次元平面内におけるｘｙ方向の並進成分のみに限定されない。位置姿勢分布の決定は２次元に限るものではなく３次元においても同様に行うことができる。図１３を用いて説明した３次元モデル上の制御点を起点としたエッジの探索と同等の処理を３次元で行えばよい。具体的には３次元モデル上の制御点を起点として制御点が乗る面の法線方向に一点の間隔で距離画像上の対応点を探索して対応点の分布をとることが考えられる。また、２次元及び３次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。

本発明の第４の実施形態、および第５の実施形態で説明した位置姿勢候補の分布の決定において、形状情報の特徴とはテクスチャに由来するエッジに限るものではなくＳ６０２の対応付けの根拠になり得る特徴であれば何でもよい。例えば３次元モデルの形状に由来するエッジなどテクスチャ以外の特徴でもよい。

位置姿勢候補生成において、概略位置姿勢分布として挙げられている位置姿勢には一定の合理性があると考え、位置姿勢候補として採用した。しかし、本発明の実施においては概略位置姿勢の分布として挙げられたものは採用せずに、新たに生成した位置姿勢候補だけを採用してもよい。

位置姿勢候補生成において、確率密度モデルとしてガウス分布を適用したが、実際には得られた分布を要約できる分布であれば他の分布であってもよいし、もっと簡単に方形状の分布を適用してもよい。

本発明の第２の実施形態で説明した位置姿勢候補の生成において、選定されたｂｉｎに所定の間隔で初期値を生成する際は間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけｂｉｎの内部にランダムに生成してもよい。

位置姿勢候補選択において、スコアは３次元モデルと輝度画像及び距離画像の合致している度合いを表す値なら何でもよい。例えば３次元モデルと輝度画像及び距離画像で誤差ベクトルを求めそのノルムの逆数を用いる方法が考えられるがその他の指標でもよい。

位置姿勢候補選択において、位置姿勢候補をスコアで評価する前に位置姿勢更新処理ステップＳ６０１〜ステップＳ６０６を１回行っているが、この回数は必ずしも１回である必要はなく、位置姿勢候補が最適解に収束する見込みを高速に確かめられる方法ならばなんでもよい。例えば位置姿勢更新を１回も行わずにスコアを算出してもよいし、または位置姿勢更新を２回以上行ってからスコアを算出してもよい。このように選択処理の具体的な手順については無数の方法が考えられるが本発明の実施においては特にどれでなくてはならないということはない。

上記目的を達成するために、本発明によれば、位置姿勢計測装置に、対象物体が含まれる画像から、前記対象物体の少なくとも１つの概略位置姿勢を取得する取得手段と、前記取得した概略位置姿勢の分解能を決定する決定手段と、前記取得した概略位置姿勢と前記決定した当該概略位置姿勢の分解能とに基づいて、前記対象物体の位置姿勢の導出のための初期値として、少なくとも１つの位置姿勢候補を新たに生成する生成手段と、前記対象物体のモデル情報と、前記初期値として生成された位置姿勢候補とに基づいて、前記対象物体の位置姿勢を導出する導出手段とを備える。

Claims (10)

1. 対象物体が含まれる画像から、前記対象物体の少なくとも１つの概略位置姿勢を取得する取得手段と、
   前記取得した概略位置姿勢の分解能に基づいて、前記対象物体の位置姿勢の導出のための初期値として、少なくとも１つの位置姿勢候補を新たに生成する生成手段と、
   前記対象物体のモデル情報と、前記初期値として生成された位置姿勢候補とに基づいて、前記対象物体の位置姿勢を導出する導出手段とを備える位置姿勢計測装置。
2. 前記取得手段は、前記画像にパターンマッチングを行うことにより、前記画像中の前記対象物体の概略位置姿勢を取得することを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記取得手段は、前記対象物体のモデル情報を使って前記画像内を探索することによりパターンマッチングを行うことを特徴とする請求項２に記載の位置姿勢計測装置。
4. 前記取得手段によって取得した概略位置姿勢は、所定の分解能に基づいて生成され、前記生成手段は、前記取得される概略位置姿勢と、該概略位置姿勢が生成される際の分解能とに基づき、少なくとも１つの位置姿勢候補を新たに生成することを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
5. 前記生成手段は、前記取得した概略位置姿勢に対して、少なくとも位置または姿勢のうち一方を前記所定の分解能の範囲内で変化させることにより、少なくとも１つの位置姿勢候補を新たに生成することを特徴とする請求項４に記載の位置姿勢計測装置。
6. 前記分解能は、前記概略位置姿勢の位置と姿勢を表現する最小ステップ幅であることを特徴とする請求項４または５に記載の位置姿勢計測装置。
7. 前記生成手段は、前記取得した概略位置姿勢と、前記対象物体の形状情報とに基づいて、少なくとも１つの位置姿勢候補を新たに生成することを特徴とする請求項１乃至６のいずれか１項に記載の位置姿勢計測装置。
8. 請求項１乃至７のいずれか１項に記載された位置姿勢計測装置と、
   前記対象物体を把持するための把持手段と、
   前記位置姿勢計測装置によって計測される前記対象物体の位置姿勢に基づいて、前記把持手段を制御する制御手段とを備えることを特徴とする情報処理装置。
9. 対象物体が含まれる画像から、前記対象物体の少なくとも１つの概略位置姿勢を取得する取得工程と、
   前記取得した概略位置姿勢の分解能に基づいて、前記対象物体の位置姿勢の導出のための初期値として、少なくとも１つの位置姿勢候補を新たに生成する生成工程と、
   前記対象物体のモデル情報と、前記初期値として生成された位置姿勢候補とに基づいて、前記対象物体の位置姿勢を導出する導出工程とを備える位置姿勢計測方法。
10. 請求項９に記載の位置姿勢計測方法をコンピュータに実行させるためのコンピュータプログラム。