(57)【要約】【課題】本発明は、最近傍点探索処理を高速化を実現することができる、最近傍点探索装置およびその方法、プログラム、３次元位置・姿勢認識装置およびその方法を提供することを目的とする。【解決手段】本発明にかかる最近傍点探索装置は、第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定する軸設定手段４１と、取得手段１３において取得した第２点群における各点を注目点として、各注目点の３次元位置に対応し、かつ、基準軸に沿う特定範囲において、第１点群の各点と注目点との距離を算出し、第１点群における最近傍点を探索する最近傍点探索手段２２とを備える。【選択図】図１７
2013137591.tif 000002
【特許請求の範囲】
【請求項１】
同一または互いに対応する３次元対象体をそれぞれが表現した、第１点群および第２点群の各点の３次元位置情報を取得する取得手段と、
前記取得手段において取得した前記第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定する軸設定手段と、
前記取得手段において取得した前記第２点群における各点を注目点として、各前記注目点の３次元位置に対応し、かつ、前記基準軸に沿う特定範囲において、前記第１点群の各点と前記注目点との距離を算出し、当該距離の最も近い前記第１点群の点である最近傍点を探索する最近傍点探索手段とを備えることを特徴とする、
最近傍点探索装置。
【請求項２】
前記軸設定手段が、前記第１点群の分布の分散の値が最も大きい方向に沿うように、前記基準軸を設定することを特徴とする、
請求項１に記載の最近傍点探索装置。
【請求項３】
前記軸設定手段が、前記第１点群の分布の情報エントロピーの値が最も大きい方向に沿うように、前記基準軸を設定することを特徴とする、
請求項１に記載の最近傍点探索装置。
【請求項４】
前記軸設定手段が、前記基準軸を設定する際、前記第１点群の分布中心から所定距離以上離れた点である外れ点を除外して、前記第１点群の分布の方向的な集中度を評価することを特徴とする、
請求項１〜３のいずれかに記載の最近傍点探索装置。
【請求項５】
前記基準軸の方向に前記第１点群の各点をソートするソート手段をさらに備え、
前記最近傍点探索手段が、前記特定範囲において、前記ソート手段によってソートした順に前記第１点群の各点と前記注目点との距離を算出し、前記最近傍点を探索することを特徴とする、
請求項１〜４のいずれかに記載の最近傍点探索装置。
【請求項６】
前記最近傍点探索手段が、前記第１点群の重心と前記第２点群の重心とが一致するように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合に、前記注目点の３次元位置を基準とした、前記第１点群または前記第２点群の外形サイズを、前記基準軸に沿う前記特定範囲として用いることを特徴とする、
請求項１〜５のいずれかに記載の最近傍点探索装置。
【請求項７】
前記最近傍点探索手段が、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合に、前記注目点の３次元位置を基準とした、各前記注目点と各前記最近傍点との間の、前記位置変換後の距離を、前記基準軸に沿う前記特定範囲として用いることを特徴とする、
請求項１〜５のいずれかに記載の最近傍点探索装置。
【請求項８】
前記最近傍点探索手段が、前記最近傍点の探索処理ごとに前記特定範囲を変更可能であることを特徴とする、
請求項１〜７のいずれかに記載の最近傍点探索装置。
【請求項９】
前記第１点群および前記第２点群の各点の３次元位置情報が、ＣＡＤデータで与えられることを特徴とする、
請求項１〜８のいずれかに記載の最近傍点探索装置。
【請求項１０】
請求項１〜９のいずれかに記載の最近傍点探索装置と、
前記最近傍点探索手段における前記最近傍点の探索処理の結果に基づいて、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させる、位置変換手段と、
前記位置変換手段における位置変換結果に基づいて、前記第１点群と前記第２点群との相対的な空間位置関係を把握し、前記対象体の３次元位置および姿勢を認識する認識手段とを備えることを特徴とする、
３次元位置・姿勢認識装置。
【請求項１１】
コンピュータにインストールされて実行されることにより、前記コンピュータを請求項１〜９のいずれかに記載の最近傍点探索装置として機能させることを特徴とする、
最近傍点探索プログラム。
【請求項１２】
（ａ）同一または互いに対応する３次元対象体をそれぞれが表現した、第１点群および第２点群の各点の３次元位置情報を取得する工程と、
（ｂ）取得した前記第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定する工程と、
（ｃ）取得した前記第２点群における各点を注目点として、各前記注目点の３次元位置に対応し、かつ、前記基準軸に沿う特定範囲において、前記第１点群の各点と前記注目点との距離を算出し、当該距離の最も近い前記第１点群の点である最近傍点を探索する工程とを備えることを特徴とする、
最近傍点探索方法。
【請求項１３】
前記工程（ｂ）が、前記第１点群の分布の分散の値が最も大きい方向に沿うように、前記基準軸を設定する工程であることを特徴とする、
請求項１２に記載の最近傍点探索方法。
【請求項１４】
前記工程（ｂ）が、前記第１点群の分布の情報エントロピーの値が最も大きい方向に沿うように、前記基準軸を設定する工程であることを特徴とする、
請求項１２に記載の最近傍点探索方法。
【請求項１５】
前記工程（ｂ）が、前記基準軸を設定する際、前記第１点群の分布中心から所定距離以上離れた点である外れ点を除外して、前記第１点群の分布の方向的な集中度を評価する工程であることを特徴とする、
請求項１２〜１４のいずれかに記載の最近傍点探索方法。
【請求項１６】
（ｄ）前記工程（ｂ）の後、前記工程（ｃ）の前に、前記基準軸の方向に前記第１点群の各点をソートする工程をさらに備え、
前記工程（ｃ）が、前記特定範囲において、前記工程（ｄ）でソートした順に前記第１点群の各点と前記注目点との距離を算出し、前記最近傍点を探索する工程であることを特徴とする、
請求項１２〜１５のいずれかに記載の最近傍点探索方法。
【請求項１７】
前記工程（ｃ）が、前記特定範囲として、前記第１点群の重心と前記第２点群の重心とが一致するように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合の、前記注目点の３次元位置を基準とした、前記基準軸に沿う特定距離範囲を用いる工程であることを特徴とする、
請求項１２〜１６のいずれかに記載の最近傍点探索方法。
【請求項１８】
前記工程（ｃ）が、前記特定範囲として、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合の、前記注目点の３次元位置を基準とした、前記基準軸に沿う特定距離範囲を用いる工程であることを特徴とする、
請求項１２〜１６のいずれかに記載の最近傍点探索方法。
【請求項１９】
前記工程（ｃ）において、前記最近傍点の探索処理ごとに前記特定範囲を変更可能であることを特徴とする、
請求項１２〜１８のいずれかに記載の最近傍点探索方法。
【請求項２０】
前記工程（ａ）において取得される前記第１点群および前記第２点群の各点の３次元位置情報が、ＣＡＤデータで与えられることを特徴とする、
請求項１２〜１９のいずれかに記載の最近傍点探索方法。
【請求項２１】
請求項１２〜２０のいずれかに記載の最近傍点探索方法の各工程と、
（ｅ）前記工程（ｃ）における前記最近傍点の探索処理の結果に基づいて、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させる工程と、
（ｆ）前記工程（ｅ）における位置変換結果に基づいて、前記第１点群と前記第２点群との相対的な空間位置関係を把握し、前記対象体の３次元位置および姿勢を認識する工程とを備えることを特徴とする、
３次元位置・姿勢認識方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は最近傍点探索装置およびその方法、プログラム、３次元位置・姿勢認識装置およびその方法に関し、特に、最近傍点探索処理の高速化に関する。
【背景技術】
【０００２】
物品などの対象体の空間位置および空間姿勢の少なくとも一方を含む３次元座標値の組によって３次元位置および姿勢を定義する場合、対象体の３次元位置および姿勢を認識するために、その対象体の３次元形状を示す、各点が３次元位置情報を有する点群データ同士のマッチングを行う方法が従来から知られている。
【０００３】
例えば、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）アルゴリズムを用いて、３次元位置および姿勢情報を有する点群データ同士のマッチングを行う方法（ＩＣＰマッチング）がある。
【０００４】
この方法は、対象体を計測し、ステレオ法等を用いて生成した３次元位置情報を各点が有する計測点群と、当該対象体に対応してあらかじめ対象体の３次元モデル上に規定され、各点が３次元位置情報を持ち、全体として対象体の全周形状を示す点群であるモデル点群とをＩＣＰマッチングさせて、当該対象体の３次元位置および姿勢を認識する方法である。計測点群およびモデル点群は、同一または互いに対応する３次元対象体をそれぞれが表現した点群である。
【０００５】
このＩＣＰマッチングは、計測点群の各点を注目点とし、あらかじめ適宜に配置したモデル点群の中から最も近い点（最近傍点）をそれぞれ対応点として求め、それらの位置が合致するように計測点群全体およびモデル点群全体のうち少なくとも一方の位置変換（回転移動と平行移動）を行う方法である。点群全体の位置変換（回転移動、平行移動）の変換値によって、計測点群とモデル点群との相対的な空間位置関係を把握し、対象体の位置および姿勢を認識する。
【０００６】
ＩＣＰマッチングにおいては、注目点に対する最近傍点としての対応点を求めるため、モデル点群における全ての点について注目点との距離を求め、当該距離が最も近い点を探す処理、すなわち、最近傍点探索処理（Ｎｅａｒｅｓｔ  ｎｅｉｇｈｂｏｒ  ｓｅａｒｃｈ：ＮＮＳ）が必要となる。この場合、計測点群における全ての点について最近傍点を求めるには、多くの処理時間を必要とする。
【０００７】
その対処法として、例えば特許文献１では、まず、所定範囲ごとに単純ソート（順序づけ）した点群から平滑化曲線を形成し、当該平滑化曲線から外れる点群を、ノイズとして削除する。このようにノイズを削除することによって、ノイズを最近傍点探索処理の対象から除外することができるので、処理時間の短縮化、すなわち、処理の高速化を図ることができる。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２００１−５２１６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、特許文献１によっても、なお最近傍点探索処理の対象となる点群の数は多く、処理時間の短縮化、すなわち、処理の高速化は十分ではなかった。
【００１０】
本発明は、上記のような問題を解決するためになされたものであり、最近傍点探索処理を高速化を実現することができる、最近傍点探索装置およびその方法、プログラム、３次元位置・姿勢認識装置およびその方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
請求項１の発明である最近傍点探索装置は、同一または互いに対応する３次元対象体をそれぞれが表現した、第１点群および第２点群の各点の３次元位置情報を取得する取得手段と、前記取得手段において取得した前記第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定する軸設定手段と、前記取得手段において取得した前記第２点群における各点を注目点として、各前記注目点の３次元位置に対応し、かつ、前記基準軸に沿う特定範囲において、前記第１点群の各点と前記注目点との距離を算出し、当該距離の最も近い前記第１点群の点である最近傍点を探索する最近傍点探索手段とを備えることを特徴とする。
【００１２】
請求項２の発明は、請求項１に記載の最近傍点探索装置であって、前記軸設定手段が、前記第１点群の分布の分散の値が最も大きい方向に沿うように、前記基準軸を設定することを特徴とする。
【００１３】
請求項３の発明は、請求項１に記載の最近傍点探索装置であって、前記軸設定手段が、前記第１点群の分布の情報エントロピーの値が最も大きい方向に沿うように、前記基準軸を設定することを特徴とする。
【００１４】
請求項４の発明は、請求項１〜３のいずれかに記載の最近傍点探索装置であって、前記軸設定手段が、前記基準軸を設定する際、前記第１点群の分布中心から所定距離以上離れた点である外れ点を除外して、前記第１点群の分布の方向的な集中度を評価することを特徴とする。
【００１５】
請求項５の発明は、請求項１〜４のいずれかに記載の最近傍点探索装置であって、前記基準軸の方向に前記第１点群の各点をソートするソート手段をさらに備え、前記最近傍点探索手段が、前記特定範囲において、前記ソート手段によってソートした順に前記第１点群の各点と前記注目点との距離を算出し、前記最近傍点を探索することを特徴とする。
【００１６】
請求項６の発明は、請求項１〜５のいずれかに記載の最近傍点探索装置であって、前記最近傍点探索手段が、前記第１点群の重心と前記第２点群の重心とが一致するように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合に、前記注目点の３次元位置を基準とした、前記第１点群または前記第２点群の外形サイズを、前記基準軸に沿う前記特定範囲として用いることを特徴とする。
【００１７】
請求項７の発明は、請求項１〜５のいずれかに記載の最近傍点探索装置であって、前記最近傍点探索手段が、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合に、前記注目点の３次元位置を基準とした、各前記注目点と各前記最近傍点との間の、前記位置変換後の距離を、前記基準軸に沿う前記特定範囲として用いることを特徴とする。
【００１８】
請求項８の発明は、請求項１〜７のいずれかに記載の最近傍点探索装置であって、前記最近傍点探索手段が、前記最近傍点の探索処理ごとに前記特定範囲を変更可能であることを特徴とする。
【００１９】
請求項９の発明は、請求項１〜８のいずれかに記載の最近傍点探索装置であって、前記第１点群および前記第２点群の各点の３次元位置情報が、ＣＡＤデータで与えられることを特徴とする。
【００２０】
また、請求項１０の発明は、３次元位置・姿勢認識装置であって、請求項１〜９のいずれかに記載の最近傍点探索装置と、前記最近傍点探索手段における前記最近傍点の探索処理の結果に基づいて、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させる、位置変換手段と、前記位置変換手段における位置変換結果に基づいて、前記第１点群と前記第２点群との相対的な空間位置関係を把握し、前記対象体の３次元位置および姿勢を認識する認識手段とを備えることを特徴とする。
【００２１】
また、請求項１１の発明は、最近傍点探索プログラムであって、コンピュータにインストールされて実行されることにより、前記コンピュータを請求項１〜９のいずれかに記載の最近傍点探索装置として機能させることを備えることを特徴とする。
【００２２】
また、請求項１２の発明は、最近傍点探索方法であって、（ａ）同一または互いに対応する３次元対象体をそれぞれが表現した、第１点群および第２点群の各点の３次元位置情報を取得する工程と、（ｂ）取得した前記第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定する工程と、（ｃ）取得した前記第２点群における各点を注目点として、各前記注目点の３次元位置に対応し、かつ、前記基準軸に沿う特定範囲において、前記第１点群の各点と前記注目点との距離を算出し、当該距離の最も近い前記第１点群の点である最近傍点を探索する工程とを備えることを特徴とする。
【００２３】
請求項１３の発明は、請求項１２に記載の最近傍点探索方法であって、前記工程（ｂ）が、前記第１点群の分布の分散の値が最も大きい方向に沿うように、前記基準軸を設定する工程であることを特徴とする。
【００２４】
請求項１４の発明は、請求項１２に記載の最近傍点探索方法であって、前記工程（ｂ）が、前記第１点群の分布の情報エントロピーの値が最も大きい方向に沿うように、前記基準軸を設定する工程であることを特徴とする。
【００２５】
請求項１５の発明は、請求項１２〜１４のいずれかに記載の最近傍点探索方法であって、前記工程（ｂ）が、前記基準軸を設定する際、前記第１点群の分布中心から所定距離以上離れた点である外れ点を除外して、前記第１点群の分布の方向的な集中度を評価する工程であることを特徴とする。
【００２６】
請求項１６の発明は、請求項１２〜１５のいずれかに記載の最近傍点探索方法であって、（ｄ）前記工程（ｂ）の後、前記工程（ｃ）の前に、前記基準軸の方向に前記第１点群の各点をソートする工程をさらに備え、前記工程（ｃ）が、前記特定範囲において、前記工程（ｄ）でソートした順に前記第１点群の各点と前記注目点との距離を算出し、前記最近傍点を探索する工程であることを特徴とする。
【００２７】
請求項１７の発明は、請求項１２〜１６のいずれかに記載の最近傍点探索方法であって、前記工程（ｃ）が、前記特定範囲として、前記第１点群の重心と前記第２点群の重心とが一致するように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合の、前記注目点の３次元位置を基準とした、前記基準軸に沿う特定距離範囲を用いる工程であることを特徴とする。
【００２８】
請求項１８の発明は、請求項１２〜１６のいずれかに記載の最近傍点探索方法であって、前記工程（ｃ）が、前記特定範囲として、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させた場合の、前記注目点の３次元位置を基準とした、前記基準軸に沿う特定距離範囲を用いる工程であることを特徴とする。
【００２９】
請求項１９の発明は、請求項１２〜１８のいずれかに記載の最近傍点探索方法であって、前記工程（ｃ）において、前記最近傍点の探索処理ごとに前記特定範囲を変更可能であることを特徴とする。
【００３０】
請求項２０の発明は、請求項１２〜１９のいずれかに記載の最近傍点探索方法であって、前記工程（ａ）において取得される前記第１点群および前記第２点群の各点の３次元位置情報が、ＣＡＤデータで与えられることを特徴とする。
【００３１】
また、請求項２１の発明は、３次元位置・姿勢認識方法であって、請求項１２〜２０のいずれかに記載の最近傍点探索方法の各工程と、（ｅ）前記工程（ｃ）における前記最近傍点の探索処理の結果に基づいて、各前記注目点と各前記最近傍点とが近づくように、前記第１点群および前記第２点群のうち少なくとも一方を位置変換させる工程と、（ｆ）前記工程（ｅ）における位置変換結果に基づいて、前記第１点群と前記第２点群との相対的な空間位置関係を把握し、前記対象体の３次元位置および姿勢を認識する工程とを備えることを特徴とする。
【発明の効果】
【００３２】
請求項１〜２１の発明によれば、第１点群の分布の方向的な集中度を評価し、当該集中度が最も低い方向に沿うように、基準軸を設定すること、また、各注目点の３次元位置に対応し、かつ、基準軸に沿う特定範囲において、最近傍点を探索することにより、点群の分布の方向的な集中度を考慮した最近傍点の探索領域の限定をすることができるので、探索が必要な領域に存在する点の数を効果的に減らすことができる。よって、最近傍点探索処理の際に、探索の対象となる点が減り、最近傍点探索処理の高速化を実現することができる。
【００３３】
特に請求項４の発明によれば、軸設定手段が、基準軸を設定する際、外れ点を除外して第１点群の分布の方向的な集中度を評価することにより、外れ点が存在するために、分布の方向的な集中度を適切に評価できない場合であっても、分布の方向的な集中度を適切に評価できる。
【図面の簡単な説明】
【００３４】
【図１】３次元位置・姿勢認識装置のハードウェア構成を示す図である。
【図２】３次元位置・姿勢認識装置の機能を概念的に示す図である。
【図３】マッチング手段の機能を概念的に示す図である。
【図４】３次元位置・姿勢認識装置の動作を示すフローチャートである。
【図５】３次元位置・姿勢認識装置の動作を示すフローチャートである。
【図６】最近傍点探索処理を説明するための図である。
【図７】最近傍点探索処理を説明するための図である。
【図８】最近傍点探索処理を説明するための図である。
【図９】最近傍点探索処理を説明するための図である。
【図１０】最近傍点探索処理を説明するための図である。
【図１１】最近傍点探索処理を説明するための図である。
【図１２】最近傍点探索処理を説明するための図である。
【図１３】３次元位置・姿勢認識装置の機能を概念的に示す図である。
【図１４】マッチング手段の機能を概念的に示す図である。
【図１５】３次元位置・姿勢認識装置の動作を示すフローチャートである。
【図１６】最近傍点探索処理を説明するための図である。
【図１７】最近傍点探索処理を説明するための図である。
【図１８】最近傍点探索処理を説明するための図である。
【発明を実施するための形態】
【００３５】
まず、本発明の前提技術として、対象体の３次元位置および姿勢を認識するためのＩＣＰマッチングを行う場合に、特に、最近傍点探索処理をモデル点群の特定領域においてのみ行う方法について、以下説明する。
【００３６】
ＩＣＰマッチングのために用いる計測点群の生成等の動作を行う３次元位置・姿勢認識装置として、例えば図１に示すようなロボットに備えられたコンピュータを想定する。
【００３７】
図１に示すロボットは、対象体３（物品など）を異なる方向から撮影する複数の撮影手段であるカメラ４およびカメラ５と、カメラ４およびカメラ５を例えば先端（ハンド部７）付近に取り付けたアーム６と、アーム６を支える胴体部８とを備える。
【００３８】
コンピュータ１０は、ロボットと有線（図１のＤＬ２）または無線で通信可能であって、カメラ４およびカメラ５の撮影結果に基づいて、対象体３の３次元位置および姿勢を認識する機能を有する。
【００３９】
コンピュータ１０には、演算処理手段や制御手段としてのＭＰＵのほか、記憶デバイスやディスプレイやマウス等のマンマシン・インターフェイスを備えられる。そして、このコンピュータにおいて３次元位置・姿勢認識装置としての機能を実現させるプログラムが、あらかじめインストールされている。
【００４０】
図２においては、３次元位置・姿勢認識装置の機能を概念的に示す。
【００４１】
図２に示すように３次元位置・姿勢認識装置は、対象体３を撮像するカメラ５およびカメラ５と、カメラ４およびカメラ５の撮影結果に基づいて、対象体３の計測点群の各点の３次元位置情報を生成する生成手段１１と、対象体３の３次元モデル上に規定されたモデル点群の各点の３次元位置情報を記憶する記憶手段１２と、計測点群およびモデル点群の各点の３次元位置情報を取得する取得手段１３と、取得手段１３からの出力をもとにＩＣＰアルゴリズムを実行するマッチング手段１４と、マッチング手段１４における結果に基づいて、対象体３の３次元位置および姿勢を認識する認識手段１５とを備える。
【００４２】
生成手段１１において、対象体３の計測点群は、カメラ４およびカメラ５で撮影された対象体３のステレオ画像から抽出された、対象体３の形状を規定する特徴的な点の集合であって、各点がその３次元位置座標によって表現されている。
【００４３】
記憶手段１２は、対象体３の３次元モデル上に規定されたモデル点群の各点の３次元位置情報を記憶する。３次元モデルとしては、対象体３が工業製品の場合にはそのＣＡＤデータなどを利用可能である。
【００４４】
取得手段１３は、生成手段１１から対象体３の計測点群の各点の３次元位置情報を取り込んで取得するとともに、モデル点群の各点の３次元位置情報を記憶手段１２から読出して取得する。
【００４５】
さらに図３において、マッチング手段１４の機能を詳細に示す。
【００４６】
図３に示すように、マッチング手段１４は、ソート軸（基準軸）を設定する軸設定手段２０と、点群における各点に対してソートするソート手段２１と、最近傍点を探索する最近傍点探索手段２２と、点群の位置変換を行う位置変換手段２３とを備える。
【００４７】
軸設定手段２０は、後述するように、最近傍点を探索される点群が定義された座標系において、ソート軸（基準軸）を設定する。ここでソート軸とは、点群の各点に順序づけるための基準となる軸であり、ソート軸の単純増加または単純減少の方向に沿って、点群の各点は順序づけられる（ソートされる）。
【００４８】
ソート手段２１は、軸設定手段２０において設定されたソート軸に沿って、実際に点群の各点を順序づける機能を有する。
【００４９】
最近傍点探索手段２２は、一方の点群における特定の点（上記の注目点に相当）から、他方の点群における各点（上記の対応点に相当）までの距離を順次計算し、当該距離が最も近い点である最近傍点を探索する機能を有する。
【００５０】
位置変換手段２３は、一方の点群における各点（注目点）と、その各点について探索された各最近傍点とを近づけるように、少なくとも一方の点群を位置変換させる機能を有する。当該機能によって、双方の点群がマッチングする。
【００５１】
３次元位置・姿勢認識装置は、ＩＣＰマッチングを行うための最近傍点探索装置としての機能も包含している。すなわち、計測点群およびモデル点群の各点の３次元位置情報を取得する取得手段１３と、ソート軸（基準軸）を設定する軸設定手段２０と、点群における各点に対してソートするソート手段２１と、最近傍点を探索する最近傍点探索手段２２とを備える、最近傍点探索装置が包含されているということができる。
【００５２】
次に、図４に示すフローチャートを用いて、３次元位置および姿勢認識処理の流れを説明する。
【００５３】
まず、３次元位置および姿勢を認識したい対象体３（たとえば物品）を異なる方向からカメラ等（カメラ４およびカメラ５）で撮影し、その画像を取得する（ステップＳ１）。対象体３が複数ある場合には、その画像中から目標とする対象体３の部分を切り出して用いることも可能である。
【００５４】
次に、生成手段１１において、それらの画像に対しステレオ処理、すなわち２つのカメラ４およびカメラ５からの視差に基づく三角測量の原理によって、各カメラから対象体３上の各点の３次元的な相対位置の特定を行うことにより、計測点群に相当する各点の３次元位置情報を生成する（ステップＳ２）。
【００５５】
次に、取得手段１３において、計測した計測点群と、記憶手段１２にあらかじめ記憶されている、対象体３に対応するモデル点群とを、それぞれ取得する（ステップＳ３）。
【００５６】
次に、マッチング手段１４において、計測点群とモデル点群とをＩＣＰマッチングさせる（ステップＳ４）。
【００５７】
詳細な内容については、図５を参照して後述する。
【００５８】
次に認識手段１５において、ＩＣＰマッチング処理における点群の位置変換の変換値から、計測点群とモデル点群との相対的な空間位置関係を把握し、対象体３の３次元位置および姿勢を認識する（ステップＳ５）。
【００５９】
次に、マッチング手段１４における動作（図４のステップＳ４）について、図５を参照しつつ説明する。以下では、計測点群における特定の点を注目点とし、モデル点群における対応点（最近傍点）を探索する場合の、ＩＣＰマッチング処理について説明する。
【００６０】
まず、軸設定手段２０において、最近傍点を探索されるモデル点群が定義された座標系において、ソート軸を設定する。ソート軸の設定は、モデル点群が定義された座標系のＸ軸方向、Ｙ軸方向、Ｚ軸方向のうち、モデル点群の各点が存在する範囲、すなわち、点群の分布幅が最も広い座標軸（座標値の最大値から最小値を減じた値が最大となる座標軸）に沿うようになされる（ステップＳ４０）。例えば、モデル点群が、Ｘ軸方向に長く広がる分布を示している場合には、Ｘ軸がソート軸となる。
【００６１】
次に、ソート手段２１において、モデル点群の各点に対し、ソート軸に沿った順序づけ（ソート）を行う（ステップＳ４１）。
【００６２】
次に最近傍点探索手段２２において、計測点群における注目点に対応する、モデル点群における最近傍点を探索する（ステップＳ４２）。
【００６３】
最近傍点探索処理は、モデル点群における各点について、ソート手段２１においてソートした順に行われる。
【００６４】
このとき、モデル点群の探索領域は、以下のように限定することができる。
【００６５】
すなわち、ソート軸上において、注目点の３次元位置を基準とする特定距離範囲を指定し、当該範囲に区切られる領域のみを探索領域とすることができる。特定距離範囲は、探索処理ごとに変更することも可能である。
【００６６】
特定距離範囲は、例えば、最近傍点探索処理の前に、計測点群の重心とモデル点群の重心とを一致させる処理（粗位置合わせ処理）を行う場合には、計測点群またはモデル点群の外形サイズ（全周サイズ）に基づいて、例えばその一定割合分の長さに設定することができる。
【００６７】
また、既に最近傍点探索処理がなされ、点群の位置変換処理が行われている場合（図６参照）には、前回、ある注目点（Ａ０）の最近傍点として探索されたモデル点群の点（Ｂ１）を記憶しておき、位置変換処理を行うことによって３次元位置が移動した注目点（Ａ１）からモデル点群の前回の最近傍点（Ｂ１）までの距離（Ｌ１）を、特定距離範囲として用いることができる。
【００６８】
図６は、Ｘ軸がソート軸として設定されている場合であり、注目点（Ａ１）を中心として、Ｘ軸の正方向および負方向に距離（Ｌ１）の、特定距離範囲が設けられている。図６においては、モデル点群の点（Ｂ１）、点（Ｂ２）、点（Ｂ３）、点（Ｂ４）については探索されるが、点（Ｂ５）および点（Ｂ６）については、特定距離範囲による探索領域から外れているため探索対象とされない。
【００６９】
このように設定すれば、前回探索された最近傍点は確実に探索領域に含まれ、位置変換処理によって移動した３次元位置における最近傍点が異なっていれば（より近傍の点が存在すれば）、最近傍点を効率的に更新することができる。図６においては、注目点（Ａ１）から点（Ｂ３）までの距離（Ｌ３）が、注目点（Ａ１）から点（Ｂ１）までの距離（Ｌ１）より短いため、最近傍点は点（Ｂ３）に更新される。
【００７０】
次に位置変換手段２３において、計測点における各注目点と、それらに対応するモデル点群における各最近傍点とが近づくように、計測点群およびモデル点群のうち少なくとも一方を位置変換する（ステップＳ４３）。
【００７１】
以上のように、ソート軸上の特定範囲に区切られる領域に最近傍点探索処理の探索領域を限定することにより、各注目点の最近傍点探索の処理時間を短くし、ＩＣＰマッチング処理、さらには対象体の３次元位置および姿勢認識処理の高速化を実現することができる。
【００７２】
しかし上記のような場合でも、ソート軸（基準軸）をどのように設定するかによって、十分に処理の高速化が実現できない場合がある。
【００７３】
例えば図７に示すような、他の点群の分布とは一定距離離れた３次元位置に外れ点（Ｘ０）があるような分布の点群がある場合、上記の例のように、点群の分布幅が最も広い軸に沿うようにソート軸を設定すると、ソート軸はＸ軸に沿うものとなる。なお、外れ点（外れ値、アウトライヤー）は、計測によるノイズである場合だけでなく、適切な測定点である場合もある。
【００７４】
ソート軸をＸ軸に沿うように設定し、注目点を中心としてソート軸上の特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、例えば図８に示すような領域を探索することとなり、探索領域内に多くの点が含まれてしまう（図８においては、７点）。
【００７５】
一方で、例えばソート軸をＹ軸に沿うように設定し、注目点を中心としてソート軸上の特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、例えば図９に示すような領域を探索することとなり、探索領域内に含まれる点の数は、図８に示す場合より少なくなる（図９においては、４点）。
【００７６】
探索領域に含まれる点の数が少ないほど最近傍点探索処理にかかる時間は短くなるから、ソート軸上の同じ特定範囲（Ｌ１）に区切られる探索領域内の最近傍点探索を行う場合でも、ソート軸の設定によって、高速化の度合いが異なることが分かる。
【００７７】
また、例えば図１０に示すような、Ｘ軸およびＹ軸に沿わない方向（図１０においては、図面上の右斜め方向）に広がりを持つ分布の点群がある場合、上記の例のように、点群の分布幅が最も広い軸に沿うようにソート軸を設定すると、例えば、ソート軸はＸ軸に沿うものとなる。
【００７８】
ソート軸をＸ軸に沿うように設定し、注目点を中心としてソート軸上の特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、例えば図１１に示すような領域を探索することとなり、探索領域内に多くの点が含まれてしまう（図１１においては、６点）。
【００７９】
一方で、ソート軸を、例えば図１２に示すＸ’軸に沿うように設定し、注目点を中心としてソート軸上の特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、図１２に示すような領域を探索することとなり、探索領域内に含まれる点の数は、図１１に示す場合より少なくなる（図１２においては、３点）。
【００８０】
よってこの場合にも、ソート軸上の同じ特定範囲（Ｌ１）に区切られる探索領域内の最近傍点探索を行う場合でも、ソート軸の設定によって、高速化の度合いが異なることが分かる。
【００８１】
以下に示す実施形態では、上記のような問題を踏まえ、ソート軸の選定を適切に行うことによって、高速化を実現する最近傍点探索装置を示す。
【００８２】
＜実施形態＞
＜構成＞
ＩＣＰマッチングのために用いる計測点群の生成等の動作を行う３次元位置・姿勢認識装置として、例えば図１に示すようなロボットに備えられたコンピュータを想定する。各構成についての説明については、上述した内容と同様であるので、詳細な説明を省略する。
【００８３】
図１３においては、３次元位置・姿勢認識装置の機能を概念的に示す。
【００８４】
図１３に示すように３次元位置・姿勢認識装置は、対象体３を撮像するカメラ５およびカメラ５と、カメラ４およびカメラ５の撮影結果に基づいて、対象体３の計測点群（第２点群）の各点の３次元位置情報を生成する生成手段１１と、対象体３の３次元モデル上に規定されたモデル点群（第１点群）の各点の３次元位置情報を記憶する記憶手段１２と、計測点群（第２点群）およびモデル点群（第１点群）の各点の３次元位置情報を取得する取得手段１３と、取得手段１３からの出力をもとにＩＣＰアルゴリズムを実行するマッチング手段４０と、マッチング手段４０における結果に基づいて、対象体３の３次元位置および姿勢を認識する認識手段１５とを備える。マッチング手段４０以外の機能部の説明については、上述した内容と同様であるので、詳細な説明を省略する。
【００８５】
さらに図１４において、マッチング手段４０の機能を詳細に示す。
【００８６】
図１４に示すように、マッチング手段４０は、ソート軸（基準軸）を設定する軸設定手段４１と、点群における各点に対してソートするソート手段２１と、最近傍点を探索する最近傍点探索手段２２と、少なくとも一方の点群の位置変換を行う位置変換手段２３とを備える。軸設定手段４１以外の機能部の説明については、上述した内容と同様であるので、詳細な説明を省略する。
【００８７】
軸設定手段４１は、点群の分布を解析し、分布の方向的な集中度（一様性）の低い方向に沿うように、ソート軸（基準軸）を設定する。詳細は、後述する。
【００８８】
＜動作＞
次に、マッチング手段４０における動作（図４のステップＳ４に対応）について、図１５を参照しつつ説明する。以下では、計測点群における特定の点を注目点とし、モデル点群における対応点（最近傍点）を探索する場合の、ＩＣＰマッチング処理について説明する。
【００８９】
まず、軸設定手段４１において、最近傍点を探索されるモデル点群が定義された座標系において、ソート軸を設定する。
【００９０】
ソート軸の設定に際して、まず、外れ点（図７等に示すＸ０）の探索を行う（ステップＳ５０）。ここで外れ点とは、ノイズおよび測定点のいずれであるかは問わないが、点群の分布中心（重心）からの距離が所定距離以上である点である。所定距離としては、例えば、３次元位置および姿勢を認識する対象体の外形サイズ等を用いることができる。
【００９１】
外れ点（Ｘ０）がある場合には、当該点を軸設定の際の考慮する点群から除外する。なお、外れ点（Ｘ０）がないことがあらかじめ分かっているような場合、例えば、モデル点群を参照することによりそれが分かる場合には、当該動作は省略されてもよい。
【００９２】
次に、外れ点（Ｘ０）を除外した点群において、モデル点群の分布する３次元座標系全方向の、分布の方向的な集中度を評価する（ステップＳ５１）。ここで点群の分布の方向的な集中度とは、その方向に点群の分布位置がばらつく程度を示す指標であり、「分布の方向的な集中度が高い」とは、点群の分布位置がばらつかないことであり、「分布の方向的な集中度が低い」とは、点群の分布位置がばらついていることである。
【００９３】
分布の方向的な集中度の評価には、例えば、その方向成分の分散や情報エントロピーを用いることができる。
【００９４】
そして、分布の方向的な集中度の最も低い方向、すなわち、分散の最も大きい方向、情報エントロピーの最も大きい方向に沿うように、ソート軸（基準軸）を設定する（ステップＳ５２）。
【００９５】
以下、分散を用いた場合を例として説明する。
【００９６】
モデル点群の分布の分散（ｕ2）は、例えば以下に示す式（１）で表される。
【００９７】
【数１】
2013137591.tif 000003
【００９８】
ここで、ｘｉはｎ個それぞれの点における軸方向成分を示し、ｘの相加平均とｘｉとの差の２乗の総和が分散値として計算される。
【００９９】
分散を求める際には、分散を求める軸上に各点の成分を射影し、それぞれの点の軸上の成分によるヒストグラムを生成する（図１６参照）。ヒストグラムを生成する際のビン幅については、固定幅を設定してもよいし、点群存在幅を基準として所定数分割した幅を設定してもよい。
【０１００】
式（１）に示す分散の値が最大となる方向成分を求め、その方向に沿うようにソート軸を設定する。
【０１０１】
ここで、分散の値が最大となる方向成分を求めることは主成分分析（Ｐｒｉｎｃｉｐａｌ  Ｃｏｍｐｏｎｅｎｔ  Ａｎａｌｙｓｉｓ：ＰＣＡ）に対応し、ソート軸として設定する方向は、主成分分析における第１主成分の方向に対応する。
【０１０２】
主成分分析の第１主成分の方向は、分散が最大となるため、第１主成分と直交する第２主成分の方向には、分散が最小となる。
【０１０３】
よって、第１主成分方向の軸上で特定範囲に区切った領域（領域Ａ）に含まれる点群と、他の方向、例えば第２主成分方向の軸上で特定範囲に区切った領域（領域Ｂ）に含まれる点群とを比較した場合、領域Ａでは、特定範囲および第２主成分方向の分布広がりにより形成される、点群の分布広がりとなるのに対し、領域Ｂでは、特定範囲および第１主成分方向の分布広がりにより形成される、点群の分布広がりとなる。
【０１０４】
よって、領域Ａにおける点群の分布広がりの方が狭くなり、領域Ａに含まれる点の数も少なくなる。すなわち、当該領域において最近傍点探索処理を行うことによって、探索が必要となる点数は減り、処理の高速化が可能となる。
【０１０５】
なお、第１主成分の方向は、点群の分布に対して３次元的に一意に決定することが可能であるが、本発明におけるソート軸としては、第１主成分を平行に移動した軸（第１主成分と直交する方向に平行移動した軸）であってもよい。
【０１０６】
次に、ソート手段２１において、モデル点群の各点に対し、ソート軸に沿った順序づけ（ソート）を行う（ステップＳ４１）。なお、外れ点として除外されている点を、ソートの対象に含めてもよい。
【０１０７】
次に最近傍点探索手段２２において、計測点群における注目点に対応する、モデル点群における最近傍点を探索する（ステップＳ４２）。
【０１０８】
最近傍点探索処理は、モデル点群における各点について、ソート手段２１においてソートした順に行われる。ただし、ソートした順でない順序で、各点についての最近傍点探索処理が行われてもよく、その場合、ソート手段２１が備えられなくてもよい。
【０１０９】
このとき、モデル点群の探索領域は、ソート軸上において、注目点の３次元位置を基準とする特定距離範囲を指定し、当該範囲に区切られる領域のみを探索領域とすることができる。特定距離範囲の設定方法については、上述したとおりである。
【０１１０】
次に位置変換手段２３において、計測点における各注目点と、それらに対応するモデル点群における各最近傍点とが近づくように、計測点群およびモデル点群のうち少なくとも一方を位置変換する（ステップＳ４３）。
【０１１１】
以上のように、適切に設定したソート軸上の特定範囲に区切られる領域に、最近傍点探索処理の探索領域を限定することにより、各注目点の最近傍点探索の処理時間を短くし、ＩＣＰマッチング処理、さらには対象体の３次元位置および姿勢認識処理の高速化を実現することができる。
【０１１２】
特に、ソート軸を点群の分布の方向的な集中度が低い方向に沿って設定したため、より効率的に、最近傍点探索処理の探索領域を限定することができる。
【０１１３】
例えば図１７に示すような、他の点群の分布とは一定距離離れた３次元位置に外れ点（Ｘ０）があるような分布の点群がある場合、まず、外れ点を探索することによって、軸設定の際に考慮する点群から外れ点（Ｘ０）を除外することができる。
【０１１４】
次に、外れ点（Ｘ０）を除く点群の分布を解析し、分布の方向的な集中度の低い方向に沿うようにソート軸を設定すると、ソート軸ＳはＹ軸に沿うものとなる。
【０１１５】
ソート軸Ｓ上において、注目点を中心とする特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、例えば図１７に示すような領域を探索することとなり、探索領域内に含まれる点は少なくなる（図１７においては、４点）。
【０１１６】
また、例えば図１８に示すような、Ｘ軸およびＹ軸に沿わない方向（図１８においては、図面上の右斜め方向）に広がりを持つ分布の点群がある場合、まず、外れ点（Ｘ０）を探索する。図１８の場合には外れ点（Ｘ０）に該当する点は存在しないので、分布する全ての点を、軸設定の際に考慮する点群とする。
【０１１７】
次に、点群の分布を解析し、分布の方向的な集中度の低い方向に沿うようにソート軸を設定すると、ソート軸Ｓは図１８に示すように、図面上の右斜め方向に沿うものとなる。
【０１１８】
ソート軸Ｓ上において、注目点を中心とする特定範囲（Ｌ１）を区切り、その探索領域での最近傍点探索処理を行った場合、例えば図１８に示すような領域を探索することとなり、探索領域内に含まれる点は少なくなる（図１８においては、３点）。
【０１１９】
よってどちらの場合であっても、最近傍点探索処理を行う探索領域を適切に限定し、より効率よく最近傍点探索処理を行うことができる。
【０１２０】
＜変形例＞
上記の実施形態においては、計測点群における各注目点の、モデル点群における最近傍点を探索する最近傍点探索処理について記載したが、計測点群同士、あるいはモデル点群同士での最近傍点探索処理に適用してもよいし、モデル点群における各注目点の、計測点群における最近傍点を探索する最近傍点探索処理に適用してもよい。
【０１２１】
また、実施形態に示す最近傍点探索処理は、ＩＣＰマッチングを行うための前処理として行われているが、他の用途のために行われる場合であってもよい。
【０１２２】
また、実施形態の図７〜１２、図１６〜１８においては、説明を簡略化させるために２次元座標系を用いているが、もちろん、３次元座標系に拡張して適用することができる。
【０１２３】
＜効果＞
本発明にかかる実施形態によれば、モデル点群（第１点群）の分布の方向的な集中度（分散や情報エントロピー）を評価し、当該集中度が最も低い方向に沿うように、ソート軸（基準軸）を設定する軸設定手段４１と、計測点群（第２点群）における各点を注目点として、各注目点の３次元位置に対応し、かつ、基準軸に沿う特定範囲（Ｌ１）において、モデル点群（第１点群）の各点と注目点との距離を算出し、モデル点群（第１点群）の最近傍点を探索する最近傍点探索手段２２とを備えることで、点群の分布の方向的な集中度を考慮した最近傍点の探索領域の限定をすることができるので、探索が必要な領域に存在する点の数を効果的に減らすことができる。よって、最近傍点探索処理の際に、探索の対象となる点が減り、最近傍点探索処理の高速化を実現することができる。
【０１２４】
また、点群の分布が外れ点（Ｘ０）を含んでいる場合（図１７参照）や、点群の分布が規定された軸（Ｘ軸、Ｙ軸、Ｚ軸）に沿わない広がりを持っている場合（図１８参照）等、点群の分布幅に基づくソート軸の設定では探索処理の高速化が実現できない場合であっても、適切にソート軸（基準軸）を設定し、効率のよい最近傍点探索処理を行うことができる。
【０１２５】
また、本発明にかかる実施形態によれば、軸設定手段４１が、ソート軸（基準軸）を設定する際、モデル点群（第１点群）の分布中心から離れた外れ点（Ｘ０）を除外して、モデル点群（第１点群）の分布の方向的な集中度を評価することで、外れ点（Ｘ０）が存在するために、分布の方向的な集中度を適切に評価できない場合であっても、外れ点（Ｘ０）を探索し、その外れ点（Ｘ０）を分布の方向的な集中度の判断には含めないことにより、分布の方向的な集中度を適切に評価できる。
【０１２６】
また、本発明にかかる実施形態によれば、モデル点群（第１点群）の重心と計測点群（第２点群）の重心とが一致させた後、最近傍点探索手段２２が、注目点の３次元位置を基準として、モデル点群（第１点群）または計測点群（第２点群）の外形サイズの例えば一定割合分の長さを、ソート軸（基準軸）に沿う特定距離範囲として設定することで、点群同士の重心を一致させた状態での、注目点の周辺領域を効果的に探索することができ、最近傍点探索の効率を高めることができる。
【０１２７】
また、本発明にかかる実施形態によれば、各注目点と各最近傍点とが近づけた後、最近傍点探索手段２２が、注目点の３次元位置を基準とした、各注目点と各最近傍点との間の、位置変換後の距離を、ソート軸（基準軸）に沿う特定距離範囲として設定することで、位置変換させる前に最近傍点とされた対応点と、注目点との間の、位置変換後の距離を基準に特定距離範囲を設定でき、前回探索された最近傍点を確実に探索領域に含みつつ、位置変換後の最近傍点を効率的に探索することができる。
【０１２８】
また、本発明にかかる実施形態によれば、最近傍点探索手段２２が、最近傍点の探索処理ごとに特定範囲を変更可能であることで、それぞれの最近傍点探索処理において、適切な探索領域を設定することができ、処理の高速化が可能となる。
【０１２９】
また、本発明にかかる実施形態によれば、最近傍点探索処理の結果に基づいて、各注目点と各最近傍点とが近づくように、点群を位置変換させる位置変換手段２３と、位置変換結果に基づいて、モデル点群（第１点群）と計測点群（第２点群）との相対的な空間位置関係を把握し、対象体３の３次元位置および姿勢を認識する認識手段１５とを備えることで、最近傍点探索処理を高速化し、さらに、３次元位置および姿勢認識の高速化も実現できる。
【０１３０】
なお本発明は、その発明の範囲内において、本実施形態における任意の構成要素の変形もしくは省略が可能である。
【符号の説明】
【０１３１】
３  対象体
４，５  カメラ
６  アーム
７  ハンド部
８  胴体部
１０  コンピュータ
１１  生成手段
１２  記憶手段
１３  取得手段
１４，４０  マッチング手段
１５  認識手段
２０，４１  軸設定手段
２１  ソート手段
２２  最近傍点探索手段
２３  位置変換手段
【図１】
2013137591.tif 000004
【図２】
2013137591.tif 000005
【図３】
2013137591.tif 000006
【図４】
2013137591.tif 000007
【図５】
2013137591.tif 000008
【図６】
2013137591.tif 000009
【図７】
2013137591.tif 000010
【図８】
2013137591.tif 000011
【図９】
2013137591.tif 000012
【図１０】
2013137591.tif 000013
【図１１】
2013137591.tif 000014
【図１２】
2013137591.tif 000015
【図１３】
2013137591.tif 000016
【図１４】
2013137591.tif 000017
【図１５】
2013137591.tif 000018
【図１６】
2013137591.tif 000019
【図１７】
2013137591.tif 000020
【図１８】
2013137591.tif 000021