(57)【要約】【課題】３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去することで、高精度・高速な照合・認識を実現する。【解決手段】概要姿勢の推定を行い（Ｓ３）、３次元辞書データを概要姿勢に応じて姿勢変換して、「概要変換後の３次元辞書データＰ’」を作成する（Ｓ４）。続いて「概要変換後の３次元辞書データＰ’」の点群を、測定器から対象物への距離方向と垂直な仮想平面へ投影する（Ｓ５）。そして、仮想平面を多数の矩形領域に分割し、各矩形領域毎に、その矩形領域に属する（投影された）複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される３次元辞書データを再構築することにより、３次元辞書データから対象物に関して測定されない領域に該当するデータを除去する（Ｓ６）。【選択図】図２
2013053920.tif 000002
【特許請求の範囲】
【請求項１】
対象物を３次元計測器により所定方向から測定した測定データであって点群より成る３次元測定データと、予め作成され記憶されている対象物の全体像の辞書データであって点群より成る３次元辞書データとに基づいて、対象物の位置・姿勢を認識する３次元物体位置検出装置であって、
前記３次元測定データに基づいて、前記対象物の概略的な位置・姿勢を示す概要姿勢を推定する概要姿勢推定手段と、
前記３次元辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の３次元辞書データを生成する概要姿勢変換手段と、
前記概要変換後の３次元辞書データの点群から、前記３次元測定器からの視野空間を分割した複数の小視野角毎に最も測定器に近い点を抽出して、該抽出した点のみより成る照合用の３次元辞書データを作成する照合用辞書データ作成手段と、
該照合用の３次元辞書データを用いて前記３次元測定データとの照合処理を行うことで、対象物の位置・姿勢を推定する詳細姿勢推定手段と、
を有することを特徴とする３次元物体位置検出装置。
【請求項２】
対象物を３次元計測器により所定方向から測定した測定データであって点群より成る３次元測定データと、予め作成され記憶されている対象物の全体像の辞書データであって点群より成る３次元辞書データとに基づいて、対象物の位置・姿勢を認識する３次元物体位置検出装置であって、
前記３次元測定データに基づいて、前記対象物の概略的な位置・姿勢を示す概要姿勢を推定する概要姿勢推定手段と、
前記３次元辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の３次元辞書データを生成する概要姿勢変換手段と、
前記概要変換後の３次元辞書データの点群から、前記概要変換後の３次元辞書データの各点を仮想平面上に投影し、前記仮想平面を多数の矩形領域に分割し、各矩形領域毎にその矩形領域に属する投影された複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される３次元辞書データを再構築することにより、照合用の３次元辞書データを作成する照合用辞書データ作成手段と、
該照合用の３次元辞書データを用いて前記３次元測定データとの照合処理を行うことで、対象物の位置・姿勢を推定する詳細姿勢推定手段と、
を有することを特徴とする３次元物体位置検出装置。
【請求項３】
前記照合用の３次元辞書データは、前記３次元辞書データから隠面消去して成るものであることを特徴とする請求項１または２記載の３次元物体位置検出装置。
【請求項４】
前記３次元測定データが、前記対象物の一部分を測定したものである場合、
前記対象物の全体像の辞書データである前記３次元辞書データに加えて更に、前記対象物の一部分に係る辞書データである部分辞書データが予め作成され記憶されており、
前記部分辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の部分辞書データを生成する第２の概要姿勢変換手段と、
該概要変換後の部分辞書データの各点毎に、その点の近傍点を前記照合用の３次元辞書データの点群のなかから求めると共に、該近傍点との距離を所定の閾値と比較することで、前記概要変換後の部分辞書データの点群から、前記近傍点との距離が所定の閾値未満である点を抽出して、該抽出した点から成る照合用の部分辞書データを作成する照合用部分辞書データ作成手段とを更に有し、
前記詳細姿勢推定手段は、前記照合用の３次元辞書データの代わりに前記照合用の部分辞書データを用いて、前記３次元測定データとの照合処理を行うことで、前記対象物の位置・姿勢を推定することを特徴とする請求項１〜３の何れかに記載の３次元物体位置検出装置。
【請求項５】
前記対象物の一部分は、前記対象物のエッジ部分であることを特徴とする請求項４記載の３次元物体位置検出装置。
【請求項６】
前記詳細姿勢推定手段は、前記照合処理と姿勢情報の更新とを繰り返す繰り返し処理によって、前記対象物の位置・姿勢を推定することを特徴とする請求項１〜５の何れかに記載の３次元物体位置検出装置。
【請求項７】
前記詳細姿勢推定手段の前記繰り返し処理によって照合完了したら、該照合完了の回数が未だ所定値未満である場合には、そのときの前記姿勢情報に基づいて前記照合用の３次元辞書データまたは前記照合用の部分辞書データを再作成する照合用辞書データ再作成手段を更に有し、
前記詳細姿勢推定手段は、該照合用辞書データ再作成手段によって前記照合用の３次元辞書データまたは前記照合用の部分辞書データが再作成された場合には、該再作成された前記照合用の３次元辞書データまたは前記照合用の部分辞書データを用いて、前記繰り返し処理を実行することを特徴とする請求項６記載の３次元物体位置検出装置。
【請求項８】
対象物を３次元計測器により所定方向から測定した測定データであって点群より成る３次元測定データと、予め作成され記憶されている対象物の全体像の辞書データであって点群より成る３次元辞書データとに基づいて、対象物の位置・姿勢を認識する３次元物体位置検出装置のコンピュータを、
前記３次元測定データに基づいて、前記対象物の概略的な位置・姿勢を示す概要姿勢を推定する概要姿勢推定手段と、
前記３次元辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の３次元辞書データを生成する概要姿勢変換手段と、
前記概要変換後の３次元辞書データの点群から、前記３次元測定器からの視野空間を分割した複数の小視野角毎に最も測定器に近い点を抽出して、該抽出した点のみより成る照合用の３次元辞書データを作成する照合用辞書データ作成手段と、
該照合用の３次元辞書データを用いて前記３次元測定データとの照合処理を行うことで、対象物の位置・姿勢を推定する詳細姿勢推定手段、
として機能させる為のプログラム。
【請求項９】
対象物を３次元計測器により所定方向から測定した測定データであって点群より成る３次元測定データと、予め作成され記憶されている対象物の全体像の辞書データであって点群より成る３次元辞書データとに基づいて、対象物の位置・姿勢を認識する３次元物体位置検出装置のコンピュータを、
前記３次元測定データに基づいて、前記対象物の概略的な位置・姿勢を示す概要姿勢を推定する概要姿勢推定手段と、
前記３次元辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の３次元辞書データを生成する概要姿勢変換手段と、
前記概要変換後の３次元辞書データの点群から、前記概要変換後の３次元辞書データの各点を仮想平面上に投影し、前記仮想平面を多数の矩形領域に分割し、各矩形領域毎にその矩形領域に属する投影された複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される３次元辞書データを再構築することにより、照合用の３次元辞書データを作成する照合用辞書データ作成手段と、
該照合用の３次元辞書データを用いて前記３次元測定データとの照合処理を行うことで、対象物の位置・姿勢を推定する詳細姿勢推定手段、
として機能させる為のプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物の３次元測定データを３次元辞書データと照合することで、対象物の３次元位置・姿勢を認識する装置に関する。
【背景技術】
【０００２】
例えば産業用ロボット等の各種機械で自動化を行う場合（例えばバラ積みされた複数の対象物を、順次、ロボットハンドで把持する、バラ積みピッキングシステム等）、対象物の位置・姿勢の認識が必要となる。対象物の位置・姿勢を認識する手法として、例えば、以下の公知手法が知られている。
【０００３】
すなわち、例えば、対象物をステレオビジョン方式のカメラやレーザースキャナー等の３次元計測器により測定して３次元測定データを得る。そして、この３次元測定データと、事前に用意した対象物の全体像の３次元データ(３次元辞書データ)との間において、繰り返し処理(ＩＣＰ：Iterative Closest Points)により両者の間の誤差を最小とする剛体変換行列を求める照合を行うことで、対象物の位置・姿勢を認識する。例えば、この様な手法は、公知・一般的である。
【０００４】
また、上記ＩＣＰによる照合を行う前に、別の手法によって対象物のおおよその３次元姿勢(概要姿勢)を推定しておき、概要姿勢が既知の対象物と３次元辞書データの間でＩＣＰ照合を行うことも、一般的に行われている公知手法である。概要姿勢を推定する従来技術の例は、特許文献１、２、非特許文献１等に開示されている。
【０００５】
上記３次元辞書データは、運用開始前に事前に、例えば対象物の三次元ＣＡＤデータに基づいて作成されるか、あるいは対象物を様々な方向から上記３次元計測器により測定した結果に基づいて作成されるものであり、上記の通り、対象物の全体像の３次元データとなっている。
【０００６】
一方、運用中に各対象物毎に測定する上記３次元測定データは、対象物の全体像ではなく、測定器側から見える部分（基本的に、１回の測定で計測可能な部分）のみから構成される（上記３次元辞書データ作成の際のような、対象物を様々な方向から複数回計測するような手間・時間が掛かることは、運用時には行なえない）。
【０００７】
そこで、事前に用意される３次元辞書データは全体像であるが、３次元測定データとの照合に用いる３次元辞書は、全体像ではなく、その一部のみとすることが望ましい。すなわち、上記全体像から、対象物に関して測定器側から見ることのできない部分（測定されない部分）に係るデータを除去することで、測定した３次元測定データと同じ部分のみを残したものを、上記照合に用いる３次元辞書とすることが望ましい。これは、測定した３次元測定データに含まれない部分の点を含んだ辞書（つまり、上記全体像）と、測定した３次元測定データとの照合を行うと、測定データには含まれないが辞書には含まれる部分の点が、照合の高精度、高速性を阻害する要因となるからである。
【０００８】
３次元辞書データから、対象物の測定されない領域に該当するデータを除去する手法として、ポリゴンモデルである辞書を用意し、概要姿勢から計算できる測定器から対象物方向へのベクトルとポリゴン面の法線ベクトルの内積から、測定器から見ることが出来る面を抽出し、見えない面に属する点を除去する方法がある（特許文献１）。
【０００９】
また、３次元データとして、ポリゴンモデルを用いることなく、点群データを用いる方式が知られている。この場合、事前に作成される３次元辞書データも、３次元測定データも、多数の点（その３次元座標データ）で構成されることになる。ポリゴンモデルを用いる場合、面情報及び法線情報を必要とするが、点群データを用いる方式では、この様な情報は必要ない。点群データを用いる方式では、基本的に、各点の三次元座標データがあればよく、従って、データ量が少なくて済み、処理負荷も少なくて済む。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特開２００７−３２２３５１号公報
【特許文献２】特開２００９−１２８１９２号公報
【非特許文献】
【００１１】
【非特許文献１】Andrew Edie Johnson, Martial Hebert “Using Spin Images for Efficient Object Recognition in Cluttered 3D Scenes”
【発明の概要】
【発明が解決しようとする課題】
【００１２】
特許文献１の手法では、ポリゴンモデルの３次元辞書データを前提としているので、この手法を（ポリゴンモデルを用いない）上述した点群データ（３次元辞書／測定データ）を用いる方式において利用する場合、ＩＣＰ照合処理には必要ないにもかかわらず、対象物の測定されない領域に該当するデータを除去するためだけに、点群からポリゴンを再計算しなければならず、計算量が多くなり効率が悪い。
【００１３】
本発明の課題は、ポリゴンモデルを用いず点群の３次元辞書データを使用して対象物の位置・姿勢を認識する３次元物体位置検出装置であって、３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去してから、照合を行うことで、高精度・高速な照合・認識を実現する３次元物体位置検出装置等を提供することである。
【課題を解決するための手段】
【００１４】
本発明の３次元物体位置検出装置は、対象物を３次元計測器により所定方向から測定した測定データであって点群より成る３次元測定データと、予め作成され記憶されている対象物の全体像の辞書データであって点群より成る３次元辞書データとに基づいて、対象物の位置・姿勢を認識する３次元物体位置検出装置であって、前記３次元測定データに基づいて、前記対象物の概略的な位置・姿勢を示す概要姿勢を推定する概要姿勢推定手段と、前記３次元辞書データの点群を前記概要姿勢を用いて剛体変換して成る、概要変換後の３次元辞書データを生成する概要姿勢変換手段と、前記概要変換後の３次元辞書データの点群から、前記３次元測定器からの視野空間を分割した複数の小視野角毎に最も測定器に近い点を抽出して、該抽出した点のみより成る照合用の３次元辞書データを作成する照合用辞書データ作成手段と、該照合用の３次元辞書データを用いて前記３次元測定データとの照合処理を行うことで、対象物の位置・姿勢を推定する詳細姿勢推定手段とを有する。
【００１５】
上記３次元物体位置検出装置では、予め作成・記憶されている３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去してから、照合処理を行うことで、高精度・高速に位置・姿勢の認識を行うことができる。
【発明の効果】
【００１６】
本発明の３次元物体位置検出装置等によれば、ポリゴンモデルを用いず点群の３次元辞書データを使用して対象物の位置・姿勢を認識する３次元物体位置検出装置であって、３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去してから、照合を行うことで、高精度・高速な照合・認識を実現することができる。
【図面の簡単な説明】
【００１７】
【図１】本例の３次元物体位置検出装置の構成図である。
【図２】実施例１における３次元物体位置検出装置の処理フローチャート図である
【図３】本例で前提とする計測座標系について説明する為の図である。
【図４】点群の仮想平面上への投影について説明する為の図である。
【図５】（ａ）、（ｂ）は、エッジ部のみである場合には適切に陰面消去できない問題を説明する為の図である。
【図６】実施例２における３次元物体位置検出装置の処理フローチャート図である。
【図７】（ａ）、（ｂ）は、照合処理に係る処理フローチャート図（その１）である。
【図８】照合処理に係る処理フローチャート図（その２）である。
【発明を実施するための形態】
【００１８】
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本例の３次元物体位置検出装置１０の構成図である。
図示の例の３次元物体位置検出装置１０は、３次元データ測定部１１、概要姿勢推定部１２、３次元辞書編集部１３、詳細姿勢推定部１４の各種機能部と、データベースとしての３次元辞書蓄積部１５を有している。
【００１９】
尚、３次元物体位置検出装置１０は、例えばＣＰＵ／ＭＰＵ等の演算プロセッサ、メモリ／ハードディスク等の記憶装置、入出力インタフェース等（何れも不図示）を有している。この記憶装置が上記データベースに相当するが、記憶装置には更に所定のアプリケーションプログラムが記憶されている。上記演算プロセッサが、このアプリケーションプログラムを読出し・実行することにより、上記各種機能部１１〜１４の処理が実現される（特に後述する図２、図６、図７、図８のフローチャートの処理が実現される）。
【００２０】
３次元辞書蓄積部１５には、予め、任意の測定対象物１に係る３次元辞書データが記憶されている。尚、既に背景技術で述べたように、３次元辞書データは、事前に、例えば測定対象物１の三次元ＣＡＤデータに基づいて作成されるか、あるいは、事前に、測定対象物１を様々な方向から３次元データ測定部１１によって複数回測定した結果に基づいて作成されるものであり、上記の通り、測定対象物１の全体像の３次元データとなっている。
【００２１】
３次元データ測定部１１は、任意の測定対象物１の形状・姿勢をスキャンする構成であり、具体例は上述した従来のステレオビジョン方式のカメラやレーザースキャナー等の３次元計測器等を有する構成である。
【００２２】
そして、既に述べたように、３次元データ測定部１１による測定対象物１の計測データ（３次元測定データ）は、測定対象物１の全体像ではなく、３次元データ測定部１１側から見える（計測可能な）部分に係わるデータとなる。換言すれば、測定対象物１に関して測定器側（３次元データ測定部１１側）から見ることのできない部分（測定できない部分）についての計測データは、得られないことになる。
【００２３】
また、上記３次元測定データ、３次元辞書データは、何れも、ポリゴンモデルを用いることなく、点群から構成されるものである。点群は、各点が（ｘ，ｙ，ｚ）の直交座標系で表される点の集合体である。
【００２４】
概要姿勢推定部１２は、３次元データ測定部１１による測定対象物１の計測データ、３次元辞書データ等に基づいて、上述した従来技術によって測定対象物１の概要姿勢の推定を行う処理機能部である。
【００２５】
３次元辞書編集部１３は、上記測定対象物１の計測データ、測定対象物１の概要姿勢の推定結果、３次元辞書蓄積部１５に記憶されている３次元辞書データ等に基づいて、測定器（３次元データ測定部１１）から見えない部分の点を除去した３次元辞書データ（照合用の３次元辞書データ）を生成する。
【００２６】
３次元辞書編集部１３は、例えば図示の概要姿勢変換部１３ａ、仮想平面への投影部１３ｂ、照合用３次元辞書データ作成部１３ｃの各種機能部を有するものであってもよい。これら各種機能部によって上記照合用の３次元辞書データが生成される。これら各種機能部については後述する。
【００２７】
詳細姿勢推定部１４は、上記照合用の３次元辞書データ等を用いて、例えば上述した既存の繰り返し処理(ＩＣＰ)により測定対象物１の位置・姿勢を厳密に認識する（詳細姿勢を推定する）。
【００２８】
以下、上記３次元物体位置検出装置１０に係わる処理について、２つの実施例について説明する。まず、実施例１について説明する。
図２は、実施例１における３次元物体位置検出装置１０の処理フローチャート図である。図示の各種処理のうち、ステップＳ１は事前に行っておくべき処理であり、ステップＳ２以降の各処理は、任意の測定対象物１が計測されるときに、その都度、照合処理（詳細姿勢推定部１４の処理）の前に実施する処理である
（１）３次元辞書データの作成
事前に、３次元辞書データ（点群）を作成する（ステップＳ１）。作成方法は、既に述べた通りであり、作成された３次元辞書データは上記３次元辞書蓄積部１５に記憶される。
【００２９】
本例の３次元辞書データは、点群で構成される下記の辞書データＤであるものとする。
Ｄ＝{dx(i),dy(i),dz(i)}  ；  （iは点番号；ｉ＝１〜Ｍ）
つまり、辞書データＤは、Ｍ個の点から成る点群データであり、各点の３次元座標データを有するものである。尚、Ｄ（ｉ）＝{dx(i),dy(i),dz(i)}等と表現してもよい。
【００３０】
尚、例えばＭ個＝１万個程度が一例として考えられるが、勿論、この例に限るものではない。
（２）３次元点群データの測定
測定器（３次元データ測定部１１）によって測定対象物１の測定を行い、３次元測定データ（測定データＰ）を取得する（ステップＳ２）。測定データＰも、３次元辞書データと同様、点群で構成され、下記の通りＮ個の点から成る点群データである。
【００３１】
Ｐ＝{px(j),py(j),pz(j)}  ；  （ｊは点番号；ｊ＝１〜Ｎ）
尚、Ｍ＞Ｎとなる。例えば、Ｎ個＝３０００個程度が一例として考えられるが、勿論、この例に限るものではない。
【００３２】
（３）概要姿勢の推定
上述した既存の繰り返し処理(ＩＣＰ：Iterative Closest Points)による照合処理、すなわち詳細姿勢推定部１４による詳細姿勢推定処理を行う前に、ステップＳ３〜Ｓ５の処理を行って、照合用の辞書データを作成する。
【００３３】
まず、概要姿勢推定部１２によって、測定対象物１の概略的な３次元姿勢(概要姿勢)を推定する（ステップＳ３）。概要姿勢を推定する処理は、例えば特許文献１、２、非特許文献１等の従来技術を用いればよく、ここでは説明しないが、概要姿勢は、例えば、３×３回転行列Ｒと、３×１併進行列Ｔを用いて、
Ｐ≒Ｒ・Ｄ＋Ｔ
等と表現できる。概要姿勢推定部１２は、ステップＳ３の処理結果として、例えば上記ＲとＴを、３次元辞書編集部１３に対して出力する。３次元辞書編集部１３は、これらＲとＴを用いて、後述する概要変換後の３次元辞書データＰ’を求めることになる。
【００３４】
但し、上記の式「Ｐ≒Ｒ・Ｄ＋Ｔ」は、すべてのiとｊに対して成立するわけではない。たとえば、測定器から見えない部分に係わる点は、３次元辞書データＤには含まれるが３次元測定データ(測定データ)Ｐには含まれないので、ｉ−ｊ対応関係は成立しない。
【００３５】
そこで、以下のステップＳ４〜Ｓ６の処理によって、３次元辞書データＤから、このような測定器から見えない部分の点を取り除くことが必要となる。尚、以下のステップＳ４〜Ｓ６の処理は、３次元辞書編集部１３が実行する。
【００３６】
（４）３次元辞書データの概要姿勢への変換
３次元辞書編集部１３の上記概要姿勢変換部１３ａは、３次元辞書データＤを概要姿勢に基づいて姿勢変換（剛体変換）して成る「概要変換後の３次元辞書データＰ’」を作成する（ステップＳ４）。これは例えば上記（３）で説明したＲとＴを用いて、下記の通り「概要変換後の３次元辞書データＰ’」を算出する。
【００３７】
Ｐ’＝Ｒ・Ｄ＋Ｔ  ；（Ｐ’（ｉ）＝Ｒ・Ｄ（ｉ）＋Ｔ）
ただし、Ｐ’（ｉ）＝{p’x(ｉ),p’y(ｉ),p’z(ｉ)}、ｉは点番号（ｉ＝１〜Ｍ）。つまり、上記辞書データＤの各点（＝{dx(i),dy(i),dz(i)}）を、それぞれ、ＲとＴを用いて座標変換することで、上記Ｐ’（ｉ）が生成される（よって、当然、Ｐ’の点群の点の数は、辞書データＤと同じくＭ個となる）。
【００３８】
（５）変換した３次元辞書データの仮想平面への投影
３次元辞書編集部１３の上記仮想平面への投影部１３ｂは、上記「概要変換後の３次元辞書データＰ’」の各点（＝{p’x(ｉ),p’y(ｉ),p’z(ｉ)}）（ｉ＝１〜Ｍ）を、仮想平面上に投影する。本例では、「概要変換後の３次元辞書データＰ’」の点群を、測定器から対象物への距離方向（またはＺ軸方向）と垂直な仮想平面へ投影する（ステップＳ５）。これは、測定器からの視野空間を分割した複数の小視野角ごとに距離の最も小さい（測定器に近い）点を抽出する為に行うものである。詳しくは後に図３、図４を参照して説明する。
【００３９】
（６）測定器から見えない点を除去した３次元辞書データの作成
３次元辞書編集部１３の上記仮想平面への投影部１３ｂ及び照合用３次元辞書データ作成部１３ｃは、上記のように、測定器からの視野空間を分割した複数の小視野角ごとに距離の最も小さい（測定器に近い）点を抽出する為に、例えば一例としては、ステップＳ５，Ｓ６の処理を行うものである。尚、この処理は、点群データに対して隠面消去（陰面消去）を行うものと見做すこともできる。画素単位で隠面消去（陰面消去；陰点除去）を行う方法としては、例えばＺバッファ法などが知られており、本処理は、Ｚバッファ法と類似する処理であると見做してよい。
【００４０】
上記隠面消去（陰面消去）の為に、本例では、まず上記仮想平面への投影部１３ｂによって上記ステップＳ５の処理を行ったうえで、上記照合用３次元辞書データ作成部１３ｃが、例えば、上記仮想平面を多数の矩形領域に分割し、各矩形領域毎に、その矩形領域に属する（投影された）複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される３次元辞書データを再構築することにより、３次元辞書データから対象物に関して測定されない領域（“測定器から見えない領域”や単に“測定されない領域”等と記す場合もある）に該当するデータを除去する（ステップＳ６）。
【００４１】
このようにして再構築された３次元辞書データの点の集合は、元々の３次元辞書データＤの点群から、測定器から見えない領域に該当する点を除去した点の集合となり、求めるべき３次元辞書データ（照合用の３次元辞書データ）となる。上記詳細姿勢推定部１４は、この「照合用の３次元辞書データ」を用いて、測定対象物１の厳密な／正確な位置・姿勢を求めて、これを出力（表示等）することになる。
【００４２】
尚、１つの点のみが投影された矩形領域があった場合には、例えばこの矩形領域に関してはこの１つの点が選択されることになる。
また、尚、上記矩形領域は、例えば、ＸＹ平面として構成される仮想平面を、Ｘ方向、Ｙ方向それぞれ一定の区分で分割して構成する。
【００４３】
上記ステップＳ５、Ｓ６の処理について、以下、図３、図４を参照して説明する。
まず、図３を参照して、本例で前提とする計測座標系について説明する。
上述した点群の３次元座標は、図３に示す３次元座標系における座標を意味する。
【００４４】
図３に示す３次元座標系は、測定器を原点（＝（０，０，０））とする、測定器から測定可能な範囲の３次元領域に係わる座標系である。更に、測定器から垂直方向（重力方向：測定器からの視線方向）をＺ軸とするものであり、従って、上記仮想平面がＸＹ平面である場合には、仮想平面は上記垂直方向（重力方向）に直交する平面ということになる。尚、ステレオカメラの場合、複数あるカメラのなかの何れか１つを原点（＝（０，０，０））とする。
【００４５】
また、上記Ｚ軸方向を距離方向とする。従って、後述する各点と測定器との距離（測定器に遠い／近い等）は、各点の３次元座標（投影前の座標）におけるｚ座標によって判断できる。例えば図示の“距離が最も小さい点”は、点群のなかでそのｚ座標の値が最も小さい点であると言える。
【００４６】
そして、例えば図４に示すように、上記のような仮想平面上に、「概要変換後の３次元辞書データＰ’」の各点（＝{p’x(ｉ),p’y(ｉ),p’z(ｉ)}）を投影する。
尚、３次元の点群を所定の平面上に投影すること自体は、既存技術であり、ここでは特に説明しないが、例えば投影変換がよく知られており、投影変換の一種である透視投影変換等を用いればよい。尚、よく知られているように、投影変換とは３次元形状を二次元平面上に投影する変換技術であり、透視投影変換は視点をｚ軸上においてｚ軸に垂直な面に投影する変換技術である。尚、図４には、辞書データＰ’の点群のうち、任意の同じ矩形領域内に投影されることになる２つの点（点１、点２）のみを示すものとする。
【００４７】
ここで、上記矩形領域について説明しておく。矩形領域は、仮想平面を矩形状に複数に分割して成る分割領域であり、図４の例では７×４＝２８個の矩形領域がある。各矩形領域の大きさは“wx×wy”とする。尚、wxはＸ方向の長さ、wyはＹ方向の長さである。また、各矩形領域には識別番号（矩形領域番号というものとする）が与えられるものとする。この矩形領域番号は、例えば任意の（例えば左上角の）矩形領域を原点とし、この原点からＸ方向にa番目、Ｙ方向にｂ番目の矩形領域の矩形領域番号は、例えば(ａ，ｂ)等としてよい。この様に、例えば一例としては、矩形領域番号は座標形式で表現できるが、勿論、この様な例に限らない。
【００４８】
そして、「概要変換後の３次元辞書データＰ’」の点群の各点を、それぞれ、仮想領域上へ投影する。そして、投影後の座標を用いて、仮想領域上の各矩形領域毎に、その矩形領域内に投影された全ての点を判別して、例えばこれら各点の点番号ｉを、その矩形領域の矩形領域番号に対応付けて記録する。
【００４９】
その後、これら記録データを参照して、各矩形領域毎に、その矩形領域内に投影された点が複数ある場合には（その矩形領域番号に対応付けて記録された点番号ｉが複数ある場合には）、これら複数の点のなかで最も測定値に近い点（上記の通り、ここでは、最もｚ座標値（＝p’z(ｉ)）が小さい点となる）を選択する（勿論、これは投影前の座標を用いて行う）。そして、選択された点のみを「照合用の３次元辞書データ」として記憶する。
【００５０】
ここで、図４の例では、上述したように（図示のように）、上記点１、点２は、任意の同じ矩形領域内に投影されることになる。そして、図より明らかなように、点１の方が点２よりも測定器に近いので、点１が選択されて「照合用の３次元辞書データ」として記憶されることになる。
【００５１】
尚、任意の矩形領域内に投影された点が１つのみである場合には、この１つの点が選択されて「照合用の３次元辞書データ」として記憶されるようにしてもよい。また、任意の矩形領域内に投影された点が０個である場合には、当然、その矩形領域に関しては何も選択されないことになる。
【００５２】
上述したようにして、その領域内に１以上の点が投影された全ての矩形領域について、それぞれ、測定器に最も近い点のみを選択して「照合用の３次元辞書データ」として記憶することで、これら選択された点群から成る「照合用の３次元辞書データ」が生成されることになる。この「照合用の３次元辞書データ」の点群は、基本的には、測定器から見える部分に係わる点から成るものである（換言すれば、３次元辞書データから、対象物１に関して測定されない領域に該当するデータを除去したものである）。
【００５３】
本手法では、上述した様に、ポリゴンモデルを用いず点群の３次元辞書データを使用して対象物の位置・姿勢を認識する３次元物体位置検出装置において、３次元辞書編集部１３が、３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去して成る「照合用の３次元辞書データ」を作成し、詳細姿勢推定部１４がこの「照合用の３次元辞書データ」を用いて照合を行うことで、高精度・高速な照合・認識（位置・姿勢の詳細推定）を実現することができる。
【００５４】
次に、以下、実施例２について説明する。
尚、実施例２に関して、上記実施例１と略同様の内容に関しては、その説明を簡略化もしくは省略する。
【００５５】
実施例２は、測定データが対象物の一部のみである場合に対応するものである。例えば、３次元測定データが、対象物のエッジ部のみを測定したデータである場合等である。
ここで、対象物の表面の反射等の影響で、面部分の測定結果が得られず、エッジ部分しか測定できない場合が有り得る。これは、表面に模様等がある場合は起こり難いが、そうでない場合には起こり得る。但し、この様な場合でも、辞書データは、例えばＣＡＤデータに基づいて作成する場合や、模様等を付して測定する等した場合には、対象物の全領域のデータとすることも可能である。
【００５６】
尚、上記“対象物の一部のみ（例えばエッジ部のみ）”とは、対象物に関して測定器から見える領域に係わる話であることを前提としている。つまり、実施例２の場合、３次元測定データは、測定器から見える領域であって且つ“対象物の一部のみ（例えばエッジ部のみ）”に係る点群から成るものである。換言すれば、測定器から見える領域であり、したがって本来であれば測定できる部分であるにも係らず、測定できない部分があることを意味する。
【００５７】
上記のように測定結果が対象物の一部のみ（例えばエッジ部のみ）となる場合には、例えば辞書データも対象物の一部のみ（例えばエッジ部のみ）から成る辞書データ（部分辞書データというものとする）を用意することが考えられる。
【００５８】
しかしながら、上記実施例１の手法でこの様な部分辞書データを用いて処理を行った場合、本来であれば隠面消去（陰面消去）されるべき点が、除去されずに残ってしまうという問題が生じる。これは、部分辞書データにおいては、測定されない部分（例えばエッジ部分以外；面部分）に関する点データはないので、当然、この様な点が仮想領域上に投影されることはなく、もし投影されていれば隠面消去（陰面消去）されていたはずの点（測定器からの距離が遠い点）が、除去されずに残ってしまう場合があるからである。
【００５９】
これについて、図５を参照して説明する。
図５（ａ）、（ｂ）の何れも、図上上側には、任意の測定対象物の任意の２つの面のみに係る辞書データを示すものとし、図５（ａ）は上記実施例１の辞書データ、図５（ｂ）は上記部分辞書データであるものとする。これより、図５（ｂ）はエッジ部分のみの点群で構成される辞書データを示し、図５（ａ）はエッジ部分だけでなく面部分も含む点群で構成される辞書データを示すものとする（何れも、図上上側のことである）。
【００６０】
尚、図５（ａ）、（ｂ）の何れも、上記図上上側に示すものは、辞書データを概要姿勢変換した後の状態を示すものとする。
一方、尚、図５（ａ）、（ｂ）の何れも、図上下側には、上記概要姿勢変換後の辞書データの各点を、上記ステップＳ５により仮想領域上に投影し、更にステップＳ６による測定器から見えない部分の点の除去を行った後の状態を示している。
【００６１】
図５（ａ）の場合、図示のように、仮想領域上への投影に関して、上記２つの面が重なる部分に関しては、測定器から遠い側の面（下側の面）に係る点は全て除去され、測定器から近い側の面（上側の面）に係る点のみが残されることになる。つまり、適切に隠面消去（陰面消去）が行われた状態を示している。
【００６２】
一方、図５（ｂ）の場合、図示のように、仮想領域上への投影に関して、上記２つの面が重なる部分において、上記下側の面に関してはエッジ部分の点群がある領域（図上、点線で囲んだ領域等）には、上記上側の面に係る点は１つも存在しない。従って、本来であれば除去されるべき点群である上記下側の面のエッジ部分の点群が、図示の点線で囲む部分に示すように除去されずに残ってしまうことになる。尚、図５（ｂ）ではエッジ部分の点群を直線で示してある。
【００６３】
だからといって、測定データはエッジ部分のみとなるので、実施例１における「照合用の３次元辞書データ」を用いて測定データとの照合を行っても、適切に照合されるわけがないことは明らかである。
【００６４】
以上のことから、実施例２では、例えば図６に示す処理を行うことで、測定データが対象物の一部のみ（例えばエッジ部分のみ）である場合でも、高精度・高速な照合・認識を実現することができる。
【００６５】
以下、図６等を参照して、実施例２の処理について説明する。
尚、実施例２においても、装置の機能ブロック自体は図１と同じであってよく、各機能ブロックの機能内容が実施例１とは異なる部分があるものと見做すことができる。よって、以下の説明では、図１の各機能ブロック（上記符号１１〜１５）を用いて説明する場合もある。
【００６６】
図６の処理では、まず事前処理として、図示のステップＳ１１、Ｓ１２の処理を行う。
その後、任意に対象物の位置・姿勢を認識する際に、図示のステップＳ１３〜Ｓ１９の処理を行うことで、「照合用の３次元辞書データ」を作成する。そして、「照合用の３次元辞書データ」を用いて、照合を行って、詳細な位置・姿勢の推定を行うことになる。
（１１）３次元辞書データの作成
まず、事前処理として、２種類の３次元辞書データＤ１，Ｄ２を作成しておく（ステップＳ１１，Ｓ１２）。３次元辞書データＤ１，Ｄ２は、上記実施例１の辞書データＤと同様、点群で構成される辞書データである。
【００６７】
３次元辞書データＤ１は、対象物の一部（例えばエッジ部分）のみに係わる点群データである。但し、これは対象物の全領域に係るものとなる。換言すれば、測定器からは見えない領域のみというわけではなく、対象物の全体に関して例えばエッジ部分のみに係わる点群データとなる。一方、３次元辞書データＤ２は、対象物全体の点群データであり、上記実施例１の３次元辞書データＤと同じであってよい。
【００６８】
詳しくは以下に説明するが、最終的には、３次元辞書データＤ１から“測定器から見えない部分”に係る点を削除したものを後述する「照合用の部分３次元辞書データ」として作成し、これを用いて詳細姿勢推定部１４が測定データとの照合を行って測定対象物１の位置・姿勢を認識することになる。
【００６９】
尚、上記３次元辞書データＤ１，Ｄ２の作成は、例えばＣＡＤデータを用いる場合には、辞書データＤ２はＣＡＤデータそのもの、辞書データＤ１は例えばＣＡＤデータを表示させると共にユーザによって任意の点を消去させる処理を行うことで作成させるものであってよいが、この例に限らない。あるいは、辞書データＤ１は例えば測定対象物１を様々な方向から測定した実測データに基づいて作成し、辞書データＤ２は測定対象物１の表面に何らかの細工（反射を防ぐように、模様を描く等）したうえで様々な方向から測定した実測データに基づいて作成してもよいが、これらの例に限らない。
【００７０】
尚、上記３次元辞書データＤ１，Ｄ２の点群は、例えば下記のように表現するものとする。
Ｄ１＝{dx(k),dy(k),dz(k)}  ；kは点番号（ｋ＝１〜Ｌ）
Ｄ２＝{dx(i),dy(i),dz(i)}  ；iは点番号（ｉ＝１〜Ｍ）
尚、辞書データＤ２は、実施例１の辞書データＤと同じであってよいので、Ｍ個（例えば１万個）の点群データから成るものとしている。尚、当然、Ｍ＞Ｌである。辞書データＤ１の点群データは、この辞書データＤ２の点群データのうちの一部（例えばエッジ部分の点群データのみ）であると見做してよい。
（１２）３次元点群データの測定
測定器（３次元データ測定部１１）によって測定対象物１の測定を行い、３次元測定データ（測定データＰ）を取得する（ステップＳ１３）。この処理自体は実施例１のステップＳ２の処理と略同様であってよいが、実施例２における測定データＰは、上記実施例１の測定データＰとは異なり、測定対象物１に関して測定器から見える領域であって且つその一部（例えばエッジ部分）のみに係る点群から成るものとなる。この様に内容的には多少異なるが、本説明においても測定データに関しては記号‘Ｐ’を用いるものとする。
【００７１】
上記のように、測定データＰは、測定対象物1の一部分（例えばエッジ部分）のみが測定された点群で構成され、例えば下記のように表される。
Ｐ＝{px(j),py(j),pz(j)}  ；  （ｊは点番号；ｊ＝１〜Ｑ）
尚、Ｍ＞Ｎ＞Ｑとなる。すなわち、ステップＳ１３の測定処理自体は、上記ステップＳ２と略同様であってよい。但し、ステップＳ１３によって得られる測定データＰの点の数（Ｑ個）は、ステップＳ２で得られる測定データＰの点の数（Ｎ個）よりも少ないことになる（例えばエッジ部分に係る点群のみであるので）。
【００７２】
（１３）概要姿勢の推定
上述した既存の繰り返し処理(ＩＣＰ：Iterative Closest Points)による照合を行う前に、ステップＳ１４〜Ｓ１９の処理を行って、照合用の辞書データを作成する。
【００７３】
まず、概要姿勢推定部１２によって、測定対象物１の概略的な３次元姿勢(概要姿勢)を推定する（ステップＳ１４）。概要姿勢を推定する処理は、例えば特許文献１、２、非特許文献１等の従来技術を用いればよく、ここでは説明しないが、概要姿勢は、例えば、３×３回転行列Ｒと、３×１併進行列Ｔを用いて、
Ｐ≒Ｒ・Ｄ２＋Ｔ
等と表現できる。尚、この概要姿勢の推定処理には、例えば測定データＰと３次元辞書データＤ２とを用いるものとするが、この例に限らない。
【００７４】
概要姿勢推定部１２は、ステップＳ１４の処理結果として、例えば上記ＲとＴを、３次元辞書編集部１３に対して出力する。３次元辞書編集部１３は、これらＲとＴを用いて、後述する変換後の部分３次元辞書データＰ１’や変換後の全体３次元辞書データＰ２’を求めることになる。
【００７５】
但し、上記の式「Ｐ≒Ｒ・Ｄ２＋Ｔ」は、すべてのiとｊに対して成立するわけではない。たとえば、測定器から見えない側の点は、３次元辞書データＤ２には含まれるが３次元測定データ(測定データ)Ｐには含まれないので、ｉ−ｊ対応関係は成立しない。
【００７６】
尚、以下のステップＳ１５〜Ｓ１９の処理は、３次元辞書編集部１３が実行する。
尚、以下のステップＳ１５〜Ｓ１７の処理は、上記ステップＳ４〜Ｓ６の処理と略同様であると見做してもよい。
【００７７】
（１４）全体３次元辞書データの概要姿勢への変換
３次元辞書編集部１３は、まず、全体３次元辞書データＤ２を概要姿勢に基づいて姿勢変換（剛体変換）して成る「概要変換後の全体３次元辞書データＰ２’」を作成する（ステップＳ１５）。これは例えば上記（１３）で説明したＲとＴを用いて、下記の通り「概要変換後の全体３次元辞書データＰ２’」を算出する。
【００７８】
Ｐ２’＝Ｒ・Ｄ２＋Ｔ  ；（Ｐ２’（ｉ）＝Ｒ・Ｄ２（ｉ）＋Ｔ）
ただし、Ｐ２’（ｉ）＝{p’x(ｉ),p’y(ｉ),p’z(ｉ)}、ｉは点番号（ｉ＝１〜Ｍ）。つまり、上記辞書データＤ２の点群の各点（Ｍ個の各点）（＝{dx(i),dy(i),dz(i)}）（ｉ＝１〜Ｍ）を、それぞれ、概要姿勢推定部１２で求めたＲとＴを用いて座標変換することで、上記Ｐ２’（ｉ）が生成される。
【００７９】
（１５）変換した全体３次元辞書データの仮想平面への投影
上記「概要変換後の３次元辞書データＰ２’」の各点（＝{p’x(ｉ),p’y(ｉ),p’z(ｉ)}）を、仮想平面上に投影する。本例では、「概要変換後の全体３次元辞書データＰ２’」の点群を、測定器から対象物への距離方向（またはｚ軸方向）と垂直な仮想平面へ投影する（ステップＳ１６）。これは、測定器からの視野空間を分割した複数の小視野角ごとに距離の最も小さい（測定器に近い）点を抽出する為に行うものである。
【００８０】
尚、ステップＳ１６の処理は、上記ステップＳ５の処理と略同様であってよいので、ここではこれ以上は説明しないものとする。
（１６）測定器から見えない点を除去した全体３次元辞書データの作成
上記仮想平面を多数の矩形領域に分割し、各矩形領域毎に、その矩形領域に属する（投影された）複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される全体３次元辞書データＰ２”（Ｐ２”（ｉ））を作成する（ステップＳ１７）。この全体３次元辞書データＰ２”（ｉ）（ｉ＝１〜Ｍ）は、上記「概要変換後の３次元辞書データＰ２’」から、対象物に関して測定されない領域（測定器から見えない領域）に該当するデータを除去して成るものである。
【００８１】
尚、ステップＳ１７の処理は、上記ステップＳ６の処理と略同様であってよいので、ここではこれ以上は説明しないものとする。
（１７）部分３次元辞書データの概要姿勢への変換
３次元辞書編集部１３は、次に、部分３次元辞書データＤ１を概要姿勢に基づいて姿勢変換（剛体変換）して成る「概要変換後の部分３次元辞書データＰ１’」を作成する（ステップＳ１８）。これは例えば上記（１３）で説明したＲとＴを用いて、下記の通り「概要変換後の部分３次元辞書データＰ１’」を算出する。
【００８２】
Ｐ１’＝Ｒ・Ｄ１＋Ｔ  ；（Ｐ１’（ｋ）＝Ｒ・Ｄ１（ｋ）＋Ｔ）
ただし、Ｐ１’（Ｐ１’（ｋ））＝{p’x(ｋ),p’y(ｋ),p’z(ｋ)}、ｋは点番号（ｋ＝１〜Ｑ）。尚、上記の通り、Ｍ＞Ｑである。
【００８３】
上記部分辞書データＤ１の点群の各点（Ｑ個の各点）（＝{dx(ｋ),dy(ｋ),dz(ｋ)}）を、それぞれ、ＲとＴを用いて座標変換することで、上記Ｐ１’（ｋ）が生成される。当然、Ｐ１’（ｋ）は、Ｑ個の点群より成るものである。
【００８４】
（１８）測定器から見えない点を除去した部分３次元辞書データの作成
上記“概要変換後の部分３次元辞書データ”Ｐ１’（ｋ）（ｋ＝１〜Ｑ）の各点毎に、上記辞書データＰ２”（ｉ）（ｉ＝１〜Ｍ）において対応する点を、最近隣点の探索を行うことで求める。すなわち、上記Ｐ１’（ｋ）（ｋ＝１〜Ｑ）の各点毎に、その点と上記辞書データＰ２”（ｉ）の各点との距離を求めて、最も距離が近い点を上記“対応する点”として求める。つまり、これによって、Ｑ個のペアが求まることになる（Ｐ１’とＰ２”とのペア）。
【００８５】
そして、各ペア毎に、そのペアの上記距離を、予め設定されている所定の閾値と比較して、距離が閾値未満であるか否か（距離＜閾値）を判定する。そして、距離が閾値未満であるペアにおけるＰ１’の点を抽出する。そして、抽出した全ての点を格納して成る「照合用の部分３次元辞書データＰ１”」を生成する（ステップＳ１９）。
【００８６】
ここで、上記辞書データＤ１の点群は、上記辞書データＤ２の点群の一部（例えばエッジ部分の点）を抽出して成るものと見做せるものであり、これらを上記のようにＲとＴを用いて同じように概要姿勢変換したものがＰ１’、Ｐ２’である。従って、仮にＰ１’とＰ２’とで上記ステップＳ１９の処理を行ったとするならば、上記Ｐ１’（ｋ）（ｋ＝１〜Ｑ）の全ての点は、最近隣点の探索で得られる上記ペアとなる点との距離が、ほぼ０に近いものになると考えられる。よって、この場合には、上記Ｐ１’（ｋ）（ｋ＝１〜Ｑ）の全ての点は、距離が閾値未満となって抽出されることになる。
【００８７】
しかし、上記Ｐ２”は、Ｐ２’から、測定器から見えない領域に対応する点を除去して成るものである。従って、上記Ｐ１’（ｋ）（ｋ＝１〜Ｑ）の各点のうち、測定器から見えない領域に対応する点は、本来はこの点とペアとなるべきＰ２”（ｉ）の点が存在しないうえに（ステップＳ１７の処理で除去されている）、ペアとなった点との距離は比較的遠くなるはずであり、上記閾値以上となるはずである。
【００８８】
例えば、図４において、点２を仮にエッジ部分の点であるとすると、上記Ｐ２’とＰ１’の何れにおいても点２に相当する点が存在するはずであり、且つ、これらはペアとなるはずであり、両者の距離は非常に近いはずである。しかしながら、上記Ｐ２’における点２は、上記Ｐ２”においては除去されているはずである。それだけでなく、Ｐ２’における点２近辺の点（測定器から見えない領域の点）は全て、上記Ｐ２”においては除去されているはずである。この為、上記Ｐ１’における上記点２に相当する点は、例えば、Ｐ２”において点１がある面における点とペアとなる可能性が高く、図示の通り、点１がある面と点２との距離は、比較的遠いものとなっている（上記閾値以上となるはずである）。
【００８９】
上述したことから、「照合用の部分３次元辞書データＰ１”」は、上記「概要変換後の部分３次元辞書データＰ１’」の点群から、測定器から見えない領域に対応する点が除去されたものであると言える。
【００９０】
従って、詳細姿勢推定部１４がこの「照合用の部分３次元辞書データＰ１”」を用いて照合を行うことで、測定データが対象物の一部分（例えばエッジ部分）のみの測定データである場合であっても、高精度・高速な照合・認識を実現することができる。
【００９１】
ここで、上記詳細姿勢推定部１４の照合処理について、以下、図７、図８を参照して説明する。尚、これは、実施例１、実施例２のどちらにも適用できる。
既に述べたように、詳細姿勢推定部１４は、上記照合用の３次元辞書データ等を用いて、例えば上述した既存の繰り返し処理(ＩＣＰ)により測定対象物１の位置・姿勢を認識する（詳細姿勢を推定する；少なくとも概要姿勢推定部１２よりは正確・厳密な位置・姿勢の推定を行える）。
【００９２】
ここで、ＩＣＰのように繰り返し行う照合処理では、所定の照合処理を１回実行する毎に、姿勢情報が更新され、照合したと判断されるまで照合処理と姿勢情報更新が繰り返される。尚、上記所定の照合処理は、既存の一般的な処理であってよく、ここでは特に説明しない。
【００９３】
図７（ａ）は、この様な既存の処理について概略的に示すフローチャート図である。
図７（ａ）において、３次元点群データの測定（ステップＳ２１）、概要姿勢の推定（ステップＳ２２）は、上記ステップＳ２、Ｓ３またはステップＳ１３，Ｓ１４と同じであってよく、ここでは特に説明しない。同様に、“測定器から見えない点を除去した３次元辞書データ（照合用の３次元辞書データ／照合用の部分３次元辞書データ）の作成”処理（ステップＳ２３）は、上記ステップＳ４〜Ｓ６またはステップＳ１５〜Ｓ１９に相当する処理であり、ここでは特に説明しない。
【００９４】
ステップＳ２３の処理実行後、上記詳細姿勢推定部１４は、図示のステップＳ２４〜Ｓ２６の処理を実行する。すなわち、上記「照合用の３次元辞書データ」または「照合用の部分３次元辞書データＰ１”」を用いて、所定の照合処理を実行する（ステップＳ２４）。この照合処理自体は、詳細姿勢推定の為の既存の処理であり、ここでは特に説明しない。そして、照合完了したか否か（詳細姿勢が求まったか否か；換言すれば、辞書と測定データとの間の誤差を最小とする剛体変換行列が求まったか否か）を判定する（ステップＳ２５）。未だ照合完了していない場合には（ステップＳ２５，ＮＯ）、姿勢情報の更新を行ってから（ステップＳ２６）、再びステップＳ２４の照合処理を実行する。これを、照合完了するまで（ステップＳ２５がＹＥＳとなるまで）繰り返し実行する。
【００９５】
ここで上記ステップＳ２６の処理によって姿勢情報が更新される毎に、測定器から見える点が変化するため、上記実施例１のステップＳ２〜Ｓ６の処理、または上記実施例２のステップＳ１３〜Ｓ１９の処理で作成した「測定器から見えない点を除去した辞書データ」は、その後、姿勢情報が更新された後には、必ずしも測定器から見えない点を除去できていない場合が考えられる。あるいは、逆に、更新後の姿勢情報によれば測定器から見えるはずの点が、除去されている場合も考えられる。これは、処理結果の精度の悪化につながる可能性がある。
【００９６】
この様な問題に対して、例えば図７（ｂ）に示すような処理手順とすることが考えられる。
図７（ｂ）の処理において、ステップＳ３１、Ｓ３２、Ｓ３３、Ｓ３４、Ｓ３５、Ｓ３６の各処理自体は、上記図７（ａ）のステップＳ２１、Ｓ２２、Ｓ２３、Ｓ２４、Ｓ２５、Ｓ２６の処理と同じであってよく、その説明は省略する。更に、ステップＳ３５の判定がＮＯの場合（未だ照合完了していない場合）にステップＳ３６の姿勢情報更新を行うことも、上記図７（ａ）においてステップＳ２５がＮＯの場合にステップＳ２６の姿勢情報更新を行うことを同じであってよい。
【００９７】
図７（ｂ）と図７（ａ）との違いは、図７（ａ）ではステップＳ２６の姿勢情報更新を行う毎にステップＳ２４の照合処理に戻るのに対して、図７（ａ）ではステップＳ３６の姿勢情報更新を行う毎にステップＳ３３の処理に戻る点である。すなわち、姿勢情報更新を行う毎に、更新後の姿勢情報を用いて“測定器から見えない点を除去した３次元辞書データ（照合用の３次元辞書データ）の作成”処理（ステップＳ３３）を行う点で、図７（ａ）とは異なる。尚、この場合、ステップＳ３３の処理が仮に実施例１の上記ステップＳ４〜Ｓ６の処理を行うものである場合、ステップＳ４における“概要姿勢への変換”は、例えば“現在の姿勢情報を用いて３次元辞書データＤの姿勢変換を行う”等に置き換える必要がある。これは後述するステップＳ４８の処理についても同様である。
【００９８】
この様に、姿勢情報の更新を行う毎に、逐一、照合用の３次元辞書データを作成し直すようにすれば、上記問題は解決できる。しかしながら、繰り返し処理の都度に、上記実施例１のステップＳ２〜Ｓ６の処理、または上記実施例２のステップＳ１３〜Ｓ１９の処理に相当する処理を行うことになるので、計算量が多くなり、処理負荷が増大し、ステップＳ３５がＹＥＳになるまでに掛かる時間が大きくなるという問題が生じる。
【００９９】
この為、例えば図８に示す処理を行うことが望ましい。
図８において、ステップＳ４１、Ｓ４２、Ｓ４３、Ｓ４４、Ｓ４５、Ｓ４６の各処理自体は、上記図７（ａ）のステップＳ２１、Ｓ２２、Ｓ２３、Ｓ２４、Ｓ２５、Ｓ２６の各処理と同じであってよく、その説明は省略する。また、ステップＳ４５の判定がＮＯの場合（未だ照合完了していない場合）にステップＳ４６の姿勢情報更新処理を行ってステップＳ４４の照合処理に戻る点も、上記図７（ａ）においてステップＳ２５がＮＯの場合にステップＳ２６の姿勢情報更新処理を行ってステップＳ２４の照合処理に戻る点と同じであってよい。
【０１００】
図８の処理が図７（ａ）の処理と異なる点は、ステップＳ４５の判定（照合完了したか否かの判定）がＹＥＳとなった後である。すなわち、図７（ａ）の場合、ステップＳ２５の判定（照合完了したか否かの判定）がＹＥＳとなったら、処理を終了する。これに対して、図８では、ステップＳ４５の判定がＹＥＳとなったら、続いて、照合回数（照合完了したと判定した回数）が、予め設定された設定値（たとえばＲ回とする）に達したか否かを判定する（ステップＳ４７）。
【０１０１】
そして、未だ照合回数が設定値Ｒに達していない場合、すなわち照合回数が設定値Ｒ未満である場合には（照合回数＜設定値Ｒ）（ステップＳ４７，ＮＯ）、ステップＳ４８の処理を実行して、ステップＳ４４の処理に戻る。ステップＳ４８の処理自体は、上記ステップＳ４３の処理と同じ処理であり、“測定器から見えない点を除去した３次元辞書データ（照合用の３次元辞書データ）の作成”処理である。但し、これは、基本的にはステップＳ４６の姿勢情報更新処理を何回か行った後に、最新の姿勢情報に基づいてステップＳ４８の処理を行うことになる。従って、この点では、ステップＳ４８の処理はステップＳ４３とは異なることになる。
【０１０２】
尚、図示していないが、ステップＳ４８の処理に伴って、上記照合回数を＋１インクリメントする処理を行う。また、ここでは照合回数の初期値は‘０’であるものとする。
そして、再びステップＳ４４〜Ｓ４６の繰り返し処理を行い、再び照合完了した場合には（ステップＳ４５，ＹＥＳ）、再度ステップＳ４７の処理を行い、判定ＮＯならば再度ステップＳ４８の処理を行うことになる。そして、ステップＳ４７の判定がＹＥＳとなったら、すなわち照合回数が設定値Ｒに達した場合には（照合回数＝設定値Ｒとなったら）、本処理を終了する。
【０１０３】
この様に、図８では、照合用の３次元辞書データの再作成をＲ回行うことになる。よって、図７（ａ）のように照合用の３次元辞書データの再作成は一切行わないものとは異なり、上記問題は生じない。また、図７（ｂ）のように姿勢情報を更新する毎に照合用の３次元辞書データの再作成を行うものとも異なり、再作成の回数はＲ回のみで済むので、上記処理負荷が増大する問題も解消される。
【０１０４】
尚、図８に示す処理は、一例であり、この例に限るものではない。基本的には、照合用の３次元辞書データの再作成を、一切行わないものでもなければ、毎回（姿勢情報を更新する毎に）行うものでもないようにするものであれば、何でもよい。例えば、図８において、ステップＳ４７、Ｓ４８の処理を削除すると共に、ステップＳ４６の処理後に分岐処理を追加するようにしてもよい。この分岐処理は、例えば「更新回数＝所定値？」等とし、判定ＹＥＳであればステップＳ４３の処理に戻り、判定ＮＯであればステップＳ４４に戻るようにする。尚、この場合、ステップＳ４３の処理実行に伴って、更新回数を‘０’にリセットする処理が必要となる。
【０１０５】
尚、上記更新回数とはステップＳ４６の姿勢情報更新処理の実行回数であり、上記所定値は予め任意に設定される回数であり、例えばＳ回とする。これによって、ステップＳ４６の姿勢情報更新処理をＳ回行う毎に、照合用の３次元辞書データの再作成が行われることになる。
【０１０６】
以上説明したように、本手法では、ポリゴンによる面情報を保有せず、点群で構成される３次元辞書データを用いて、点群で構成される３次元測定データとの照合を行って対象物の位置・姿勢を認識する装置において、繰り返し処理前に、推定された対象物のおおよその位置・姿勢（概要姿勢）に従って３次元辞書データを剛体変換した点群から、測定器からの視野空間を分割した複数の小視野角ごとに距離の最も小さい（測定器に近い）点を抽出することにより、３次元辞書データから、計測器から見えない（測定できない）領域に対応する点を取り除き（陰面消去（陰点除去）に相当する処理を行い）、計測器から見える（測定できる）領域に対応する点のみから構成される３次元辞書データ（照合用の３次元辞書データ）を作成する。これは、３次元測定データが、対象物の一部のみ（例えばエッジ部のみ）の測定データである場合にも実現できる（照合用の部分３次元辞書データを作成できる）。
【０１０７】
この様にして作成された照合用の３次元辞書データまたは照合用の部分３次元辞書データを用いてＩＣＰ照合処理等の繰り返し処理を行うことにより、対象物の位置・姿勢を高精度・高速に認識することができる。また、ポリゴンを用いないので、３次元辞書データに面情報および法線情報を必要としない。
【符号の説明】
【０１０８】
１０  ３次元物体位置検出装置
１１  ３次元データ測定部
１２  概要姿勢推定部
１３  ３次元辞書編集部
１４  詳細姿勢推定部
１５  ３次元辞書蓄積部
【図１】
2013053920.tif 000003
【図２】
2013053920.tif 000004
【図５】
2013053920.tif 000005
【図６】
2013053920.tif 000006
【図７】
2013053920.tif 000007
【図８】
2013053920.tif 000008
【図３】
2013053920.tif 000009
【図４】
2013053920.tif 000010