(57)【要約】【課題】対象体の３次元位置・姿勢の認識精度を向上させた、３次元位置・姿勢認識装置を提供することを目的とする。【解決手段】認識対象となる対象体とその３次元モデルとの３次元的位置・姿勢のマッチングを、ＩＣＰアルゴリズムによって行う。それには、３次元モデル上の点群（モデル点群）の各点を注目点として採用し、対象体上の計測点群の中から対応点を探索する。計測点群の各点を注目点とする順マッチング処理との比較上、これを逆マッチング処理と呼ぶ。逆マッチングにおいて注目点として採用される点（モデル点）にはノイズが含まれないため、ＩＣＰアルゴリズムにおける対応点探索において、ノイズ点を出発点として対応点を捜すということがない。このため、ノイズ等による誤認識を低減できる。逆マッチングを順マッチング処理と組合せる場合には、先に順マッチングを行ってラフなマッチングを済ませた後に、逆マッチングを行うことが好ましい。【選択図】図５
2011209116.tif 000002
【特許請求の範囲】
【請求項１】
対象体の空間位置と空間姿勢との組み合わせを３次元位置・姿勢として認識する装置であって、
対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する取得手段と、
前記モデル点群の各点を注目点として、当該各注目点を前記計測点群中の対応点に近づけてゆくＩＣＰアルゴリズムを実行する逆マッチング処理を行うマッチング手段と、
前記逆マッチング処理の結果に基づいて、前記対象体の３次元位置・姿勢を認識する認識手段と、
を備えることを特徴とする３次元位置・姿勢認識装置。
【請求項２】
請求項１に記載の３次元位置・姿勢認識装置であって、
前記モデル点群は、前記３次元モデルの全周形状のうち一部分についての３次元形状を表現した点群であることを特徴とする３次元位置・姿勢認識装置。
【請求項３】
請求項２に記載の３次元位置・姿勢認識装置であって、
前記取得手段は、
前記３次元モデルの観察方向に応じて予め準備された複数のモデル点群から１つの点群の各点の３次元位置情報を抽出し、当該１つの点群を前記モデル点群として採用させる抽出手段、
を含むことを特徴とする３次元位置・姿勢認識装置。
【請求項４】
請求項１〜３のいずれかに記載の３次元位置・姿勢認識装置であって、
前記マッチング手段は、
前記計測点群の各点を注目点とし、当該各注目点を前記モデル点群中の対応点に近づけていくＩＣＰアルゴリズムを実行する順マッチング処理を行う順マッチング手段と、
前記順マッチング手段によって前記順マッチング処理された後の前記計測点群と前記モデル点群とに基づいて、前記逆マッチング処理を行う逆マッチング手段と、
を含むことを特徴とする３次元位置・姿勢認識装置。
【請求項５】
請求項１〜４のいずれかに記載の３次元位置・姿勢認識装置であって、
前記計測点群全体の空間位置を代表する計測代表点と、前記モデル点群全体の空間位置を代表するモデル代表点とのそれぞれの空間位置を一致させるように、前記計測点群と前記モデル点群との相対的な空間位置関係を調整する代表点一致化手段、
をさらに備え、
前記マッチング手段は、前記計測点群と前記モデル点群との各点の３次元位置情報として、前記代表点一致化手段によって相対的な空間位置関係が調整された後の、各点の３次元位置情報を用いることを特徴とする３次元位置・姿勢認識装置。
【請求項６】
請求項１〜５のいずれかに記載の３次元位置・姿勢認識装置であって、
前記マッチング手段は、前記逆マッチング処理に際して、前記計測点群中の前記対応点を中心として所定範囲内に存在する前記計測点群中の各点の空間位置を平均した点を、前記モデル点群中の前記注目点に対する対応点として採用することを特徴とする３次元位置・姿勢認識装置。
【請求項７】
対象体を異なる方向から撮影する複数の撮影手段と、
前記複数の撮影手段による撮影結果に基づいて、前記対象体の計測点群の各点の３次元位置情報を生成する生成手段と、
前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報を記憶する記憶手段と、
前記計測点群と前記モデル点群との各点の３次元位置情報に基づいて、前記対象体の３次元位置・姿勢を認識する請求項１〜６のいずれかに記載の３次元位置・姿勢認識装置と、
を備えることを特徴とする３次元位置・姿勢認識システム。
【請求項８】
対象体の空間位置と空間姿勢との組み合わせを３次元位置・姿勢として認識する方法であって、
（ａ）対象体の３次元モデルを表現するモデル点群の各点の３次元位置情報を記憶手段に記憶させるステップと、
（ｂ）前記対象体の３次元計測によって、計測点群の各点の３次元位置情報を生成するステップと、
（ｃ）前記モデル点群の各点を注目点として、当該各注目点を前記計測点群中の対応点に近づけてゆくＩＣＰアルゴリズムを実行する逆マッチング処理を行うステップと、
（ｄ）前記逆マッチング処理の結果に基づいて、前記対象体の３次元位置・姿勢を認識するステップと、
を備えることを特徴とする３次元位置・姿勢認識方法。
【請求項９】
コンピュータにインストールされて実行されることにより、前記コンピュータを、請求項１〜６のいずれかに記載の３次元位置・姿勢認識装置として機能させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象体の３次元位置・姿勢の認識技術に関するもので、特に、ＩＣＰマッチング処理を用いる技術の改良に関する。
【背景技術】
【０００２】
物品などの対象体の空間位置および空間姿勢の少なくとも一方を含む３次元座標値の組によって３次元位置・姿勢を定義する場合、対象体の３次元位置・姿勢を認識するために、その対象体の３次元形状を示す、各点が３次元位置情報を有する点群データ同士のマッチングを行う方法が従来から知られている。例えば非特許文献１、２に示されるようなＩＣＰ(Iterative Closest Point)アルゴリズムを用いて、３次元位置・姿勢情報を有する点群データ同士のマッチングを行う方法（ＩＣＰマッチング）がある。
【０００３】
この方法は、対象体を計測し、ステレオ法等を用いて生成した３次元位置情報を各点が有する計測点群と、当該対象体に対応して予め準備された、各点が３次元位置情報を持つ点群であるモデル点群とをＩＣＰマッチングさせて、当該対象体の３次元位置・姿勢を認識する方法である。
【０００４】
このＩＣＰマッチングは、計測点群の各点を注目点とし、あらかじめ適宜に配置したモデル点群の中から最も近い点（最近傍点）をそれぞれ対応点として求め、それらの位置が合致するように計測点群全体の位置変換（回転移動と平行移動）を行う方法である。計測点群全体の位置変換（回転移動、平行移動）の変換値によって、計測点群とモデル点群との相対的な空間位置関係を把握し、対象体の位置および姿勢を認識する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−２７７１８４号公報
【特許文献２】特開２００８−１４６９１号公報
【特許文献３】特開平７−２７５５６号公報
【非特許文献】
【０００６】
【非特許文献１】IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MUCHINE INTELLIGENCE, "A Method for Registration of 3-D Shapes", FEBRUARY 1992, VOL.14 NO. 2
【非特許文献２】IEEE Int. Conf. on 3-D Imaging and Modeling, "Efficient Variants of the ICP algorithm.", 3DIM' 01
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところが従来のＩＣＰマッチングでは、計測点群とモデル点群との初期位置（３次元座標）が大きく異なっていると、その初期位置に依存してＩＣＰマッチングが失敗し、すなわち本来の対象体の３次元位置・姿勢とは異なる３次元位置・姿勢に計測点群が収束してしまう場合がある（局所解、ローカルミニマム）。このような場合を回避するために、例えば特許文献１、２、３に示すような方法がある。
【０００８】
特許文献１に示すのは、モデル点群の初期値を適切に選択するために、対象体の把持可能領域を推定する方法である。
【０００９】
特許文献２に示すのは、対象体の可視部を予測し、その部分の形状情報のみを用いてモデル点群を決定することにより、モデル点群の選定効率を向上させている。
【００１０】
また特許文献３では、稜線情報を用いた２次元画像からの特徴把握によって、対象体の計測時の欠落等に起因する、誤差の影響を低減している。
【００１１】
しかし、上記のような方法であっても、計測点群に、対象体の本来の３次元形状を示さない点（ノイズ）が含まれる場合、その影響によりＩＣＰマッチングを失敗し対象体の適切な３次元位置・姿勢が認識できない場合があり、そのような場合にもＩＣＰマッチングの精度を向上させる必要があった。
【００１２】
本発明は上記の要請からなされたものであり、計測点群にノイズが含まれるような場合にも精度よく対象体の３次元位置・姿勢の認識可能な、３次元位置・姿勢認識装置、３次元位置・姿勢認識システム、３次元位置・姿勢認識方法、およびそのプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１３】
請求項１の発明は、対象体の空間位置と空間姿勢との組み合わせを３次元位置・姿勢として認識する装置であって、対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する取得手段と、前記モデル点群の各点を注目点として、当該各注目点を前記計測点群中の対応点に近づけてゆくＩＣＰアルゴリズムを実行する逆マッチング処理を行うマッチング手段と、前記逆マッチング処理の結果に基づいて、前記対象体の３次元位置・姿勢を認識する認識手段とを備えることを特徴とする。
【００１４】
請求項２の発明は、請求項１に記載の３次元位置・姿勢認識装置であって、前記モデル点群は、前記３次元モデルの全周形状のうち一部分についての３次元形状を表現した点群であることを特徴とする。
【００１５】
請求項３の発明は、請求項２に記載の３次元位置・姿勢認識装置であって、前記取得手段は、前記３次元モデルの観察方向に応じて予め準備された複数のモデル点群から１つの点群の各点の３次元位置情報を抽出し、当該１つの点群を前記モデル点群として採用させる抽出手段を含むことを特徴とする。
【００１６】
請求項４の発明は、請求項１〜３のいずれかに記載の３次元位置・姿勢認識装置であって、前記マッチング手段は、前記計測点群の各点を注目点とし、当該各注目点を前記モデル点群中の対応点に近づけていくＩＣＰアルゴリズムを実行する順マッチング処理を行う順マッチング手段と、前記順マッチング手段によって前記順マッチング処理された後の前記計測点群と前記モデル点群とに基づいて、前記逆マッチング処理を行う逆マッチング手段とを含むことを特徴とする。
【００１７】
請求項５の発明は、請求項１〜４のいずれかに記載の３次元位置・姿勢認識装置であって、前記計測点群全体の空間位置を代表する計測代表点と、前記モデル点群全体の空間位置を代表するモデル代表点とのそれぞれの空間位置を一致させるように、前記計測点群と前記モデル点群との相対的な空間位置関係を調整する代表点一致化手段をさらに備え、前記マッチング手段は、前記計測点群と前記モデル点群との各点の３次元位置情報として、前記代表点一致化手段によって相対的な空間位置関係が調整された後の、各点の３次元位置情報を用いることを特徴とする。
【００１８】
請求項６の発明は、請求項１〜５のいずれかに記載の３次元位置・姿勢認識装置であって、前記マッチング手段は、前記逆マッチング処理に際して、前記計測点群中の前記対応点を中心として所定範囲内に存在する前記計測点群中の各点の空間位置を平均した点を、前記モデル点群中の前記注目点に対する対応点として採用することを特徴とする。
【００１９】
また、請求項７の発明の３次元位置・姿勢認識システムは、対象体を異なる方向から撮影する複数の撮影手段と、前記複数の撮影手段による撮影結果に基づいて、前記対象体の計測点群の各点の３次元位置情報を生成する生成手段と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報を記憶する記憶手段と、前記計測点群と前記モデル点群との各点の３次元位置情報に基づいて、前記対象体の３次元位置・姿勢を認識する請求項１〜６のいずれかに記載の３次元位置・姿勢認識装置とを備えることを特徴とする。
【００２０】
また、請求項８および請求項９の発明はそれぞれ、請求項１の発明に対応する方法発明およびプログラム発明である。
【発明の効果】
【００２１】
請求項１〜請求項９の発明によれば、対象体の空間位置と空間姿勢との組み合わせを３次元位置・姿勢として認識するにあたって、モデル点群の各点を注目点として、当該各注目点を計測点群中の対応点に近づけてゆくＩＣＰアルゴリズムを実行する逆マッチング処理を行うため、ノイズなどの影響を受けにくく、対象体の３次元位置・姿勢の認識精度を向上させることが可能となる。
【００２２】
特に請求項４の発明では、遠距離での有効性が高い順マッチング処理と、近距離での有効性が高い逆マッチングとのそれぞれの利点を組み合わせた複合的ＩＣＰアルゴリズムを用いることによって、３次元位置・姿勢の認識精度を向上させるにあたっての相乗的な効果がある。
【００２３】
特に請求項５の発明では、計測代表点とモデル代表点とのそれぞれの空間位置を一致させておくことによって、逆マッチング処理の準備段階での精度が向上する。
【００２４】
また特に請求項６の発明では、逆マッチング処理に際して、計測点群中の仮の対応点を中心として所定範囲内に存在する計測点群中の各点の空間位置を平均した点を正式な対応点として採用することにより、計測点群のエッジ点が空間的に分散していても、正確な３次元位置・姿勢の認識が可能となる。
【図面の簡単な説明】
【００２５】
【図１】第１実施形態にかかる３次元位置・姿勢認識システムの構成図である。
【図２】順マッチング処理を示すフローチャートである。
【図３】順マッチング処理を示す概念図である。
【図４】第１実施形態にかかる計測点群とモデル点群とを示す図である。
【図５】順マッチング処理と逆マッチング処理との特性を比較する模式図である。
【図６】第１実施形態にかかる逆マッチング処理を示すフローチャートである。
【図７】第１実施形態にかかる逆マッチング処理を示す概念図である。
【図８】第２実施形態にかかる３次元位置・姿勢認識システムの構成図である。
【図９】第２実施形態における計測点群のエッジとモデル点群のエッジとを示す模式図である。
【図１０】第２実施形態における対応点の決定方法を示す模式図である。
【図１１】第２実施形態にかかるＩＣＰマッチングを示す概念図である。
【図１２】第３実施形態にかかる３次元位置・姿勢認識システムの構成図である。
【図１３】第３実施形態における複数の部分モデルを全体モデルと比較して示す図である。
【図１４】第３実施形態における部分モデル点群を用いた、逆マッチング処理を示した図である。
【図１５】第３実施形態にかかる計測点群とモデル点群とを示す図である。
【図１６】逆マッチング処理のみを行った場合の計測点群とモデル点群とを示した図である。
【図１７】第３実施形態にかかる逆マッチング処理の前に順マッチング処理を行った場合の計測点群とモデル点群とを示した図である。
【図１８】この発明の実施形態の３次元位置・姿勢認識システムがロボットに搭載された状態を示す図である。
【発明を実施するための形態】
【００２６】
＜Ａ．定義＞
特に明示しない限り、この明細書における「対象体の３次元位置・姿勢」とは、対象体の空間位置および空間姿勢の少なくとも一方を含む座標値の組として定義される。空間姿勢は、対象体が１軸回転完全対称性を有する場合（例：円柱）には２自由度であり、１軸回転完全対称性を持たない場合には３自由度となる。
【００２７】
したがって、この発明における「３次元位置・姿勢のマッチング」とは、位置に関する３自由度と姿勢に関する３自由度との合計６自由度のすべてをマッチングさせる処理だけでなく、少なくとも、(1)  位置についての３自由度のマッチング、および、(2)  姿勢についての２以上の自由度のマッチング、のいずれかを含んでいれば、この発明の適用範囲に含まれる。
【００２８】
＜Ｂ．第１実施形態＞
＜Ｂ−１．システムの概要＞
図１は、本発明の第１実施形態にかかる３次元位置・姿勢認識システムＳＹＳ１の構成を示す概念図である。図１に示すように３次元位置・姿勢認識システムＳＹＳ１は、当該システムＳＹＳ１外の対象体３（物品など）を異なる方向から撮影する複数の撮影手段であるカメラ４、５と、カメラ４、５の撮影結果に基づいて、対象体３の３次元位置・姿勢を認識する３次元位置・姿勢認識装置１００Ａとを備える。
【００２９】
３次元位置・姿勢認識装置１００Ａは、ハードウエアとしては、汎用のコンピュータを用いて構成することが可能である。すなわち演算処理手段や制御手段としてのＭＰＵのほか、記憶デバイスやディスプレイやマウスなどのマンマシン・インターフェイスを備えたコンピュータが３次元位置・姿勢認識装置１００Ａとして利用可能である。そして、このコンピュータにおいて３次元位置・姿勢認識装置１００Ａとしての機能を実現させるプログラムが、あらかじめこのコンピュータにインストールされている。
【００３０】
３次元位置・姿勢認識装置１００Ａは、このコンピュータプログラムによって実現される以下の各機能手段(1)〜(5)を備える。
【００３１】
(1)  生成手段６は、カメラ４、５の撮影結果に基づいて、対象体３の計測点群の各点の３次元位置情報を生成する。対象体３の計測点群は、カメラ４、５で撮影された対象体３のステレオ画像から抽出された、対象体３の形状を規定する特徴的な点の集合であって、各点がその３次元位置座標によって表現されている。
【００３２】
(2)  記憶手段７は、対象体３の３次元モデル上に規定されたモデル点群の各点の３次元位置情報を記憶する。３次元モデルとしては、対象体３が工業製品の場合にはそのＣＡＤデータなどを利用可能である。
【００３３】
(3)  取得手段８は、生成手段６から対象体３の計測点群の各点の３次元位置情報を取り込んで取得するとともに、モデル点群の各点の３次元位置情報を記憶手段７から読出して取得する。
【００３４】
(4)  マッチング手段９は、取得手段８からの出力をもとにＩＣＰアルゴリズムを実行する。
【００３５】
(5)  認識手段１０は、マッチング手段９における結果に基づいて、対象体３の３次元位置・姿勢を認識する。
【００３６】
＜Ｂ−２．第１実施形態におけるマッチング処理の概要＞
このシステムＳＹＳ１では、対象体に設定された計測点群の各点の３次元位置を実測し、ＩＣＰアルゴリズムを用いて当該対象体の３次元モデルの３次元位置・姿勢とマッチングさせることによって対象体の３次元位置・姿勢を認識する。
【００３７】
そこではまず、３次元位置・姿勢を認識したい対象体（たとえば物品）を異なる方向からカメラ等で撮影し、その画像を取得する。対象体が複数ある場合には、その画像中から目標とする対象体の部分を切り出して用いることも可能である。
【００３８】
次に、それらの画像に対しステレオ処理、すなわち２つのカメラ４，５からの視差に基づく三角測量の原理によって各カメラから対象体上の各点の３次元的な相対位置の特定を行うことにより、計測点群に相当する各点の３次元位置情報を生成する。
【００３９】
これ以後の段階でＩＣＰアルゴリズムを用いたＩＣＰマッチング処理が適用されるが、この発明ではＩＣＰマッチング処理を２種類に類別する。１つは「順マッチング処理」であり、これは従来のＩＣＰマッチング法で使用されている手法に対応する。そこでは、計測点群に含まれる各点を注目点とし、各注目点に対応する３次元モデル上の点（モデル点群の点）を捜してゆくという原理である。ここでモデル点群とは、対象体の３次元モデル上に規定され、各点が３次元位置情報を有し、全体として対象体の全周形状を示す点群である。
【００４０】
これに対して、逆マッチング処理は本発明の特徴に応じて新たに導入される原理であり、３次元モデル上に設定されたモデル点群に含まれる各点を注目点とし、各注目点に対応する計測点群中の点を捜してゆく。
【００４１】
前者は、計測点群中の注目点からモデル点群中の対応点を算出することから「順マッチング処理」と呼び、後者は、順マッチング処理とは逆方向の注目点−対応点の探索方向を有していることから、「逆マッチング処理」と呼ぶ。
【００４２】
後に詳述するように、計測点群にはノイズが含まれており、計測点群に属する各点（以下「計測点」）からモデル点群中の対応点を捜す順マッチングでは、ＩＣＰアルゴリズムの結果に誤差を含みやすい。それに対してモデル点群中の各点（以下「モデル点」）を注目点として、計測点群から対応点を捜す逆マッチングでは、注目点がモデルとしての理想位置を表現しているから、ＩＣＰアルゴリズムの結果の誤差を減少させることができる。
【００４３】
そこで、この発明では逆マッチング処理を主要なマッチング原理として採用する。その一方で、順マッチング処理にも固有の利点があるため、後述する第３実施形態では、順マッチング処理と逆マッチング処理との組合せを採用する。
【００４４】
以下ではまず、順マッチング処理について説明する。それは、第１実施形態では順マッチング処理は使用しないが、順マッチング処理との比較によって逆マッチング処理の特徴がより明確になることと、後述する第３実施形態においてはこの順マッチング処理も利用するからである。逆マッチング処理についてはその後に詳述する。
【００４５】
＜Ｂ−３．ＩＣＰアルゴリズムによる順マッチング処理＞
図２および図３は、順マッチング処理を説明するフローチャートとその概念図である。ＩＣＰマッチングに用いるＩＣＰアルゴリズムでは、まず、計測点群２とモデル点群１との各点のうち、ＩＣＰマッチングに用いる範囲を決める。このとき全ての点群についてＩＣＰマッチングに用いてもよい（図２ステップＳ１−１）。
【００４６】
次に、当該範囲内で、計測点群２中のある１点（注目点）から最近傍点であるモデル点群１中の１点（対応点）を仮に算出する（図２ステップＳ１−２、図３（ａ））。各注目点とそれに対する対応点が決定したら（図３（ｂ））、注目点と対応点との距離の総和（平均二乗誤差）を最小化するような、計測点群２全体位置変換（回転移動、平行移動）を求める（図２ステップＳ１−３）。通常は回転と平行移動だけで足りるが、対象体として鏡映対称な２種類のものが想定されるときには、それらを互いに区別するために、空間反転の有無判断をこのステップＳ１−３に含めていてもよい。
【００４７】
上記の位置変換を行った後（図２ステップＳ１−４、図３（ｃ））、再度注目点に対する対応点を決定し、注目点と対応点との距離の総和（平均二乗誤差）を計算する（図２ステップＳ１−５）。その値が所定閾値以下あるいはその値と先の総和の値との差が所定閾値以下である場合には、ＩＣＰマッチングを終了する（図２ステップＳ１−６）。そうでない場合には、ステップＳ１−１〜ステップＳ１−５までの動作を繰り返す。
【００４８】
なお、図３においては、計測点群２にのみ位置変換を行っているが、計測点群２とモデル点群１との位置関係は相対的なものであるので、モデル点群１を位置変換してもよいし、双方を位置変換してもよい。
【００４９】
ＩＣＰマッチング終了後の、計測点群２とモデル点群１との位置変換が、計測点群２とモデル点群１との相対的な空間位置関係を示すものであり、この関係により対象体の３次元位置・姿勢を認識することができる。
【００５０】
＜Ｂ−４．ＩＣＰアルゴリズムによる逆マッチング処理＞
ここで、順マッチング処理が大きな誤差を含むかあるいはマッチング処理そのものが失敗する、すなわち図３の計測点群２が対象体３の本来の３次元位置・姿勢を示す座標に収束しない原因のひとつとして、既述したように計測点群２に含まれるノイズの影響がある。ここでノイズとは、複数の画像をステレオ処理した際に計測点群２中に生じる、実際の対象体の３次元形状に対応する３次元位置・姿勢情報を示すものではない点をいう。例えば図４に示すように、計測点群２（黒丸）には、モデル点群１（白丸）中には対応する点がないノイズが含まれる。
【００５１】
図２のステップＳ１−２において、これらノイズについても注目点とし、それぞれの対応点を算出するため、本来含まれるべきでない点をも含めてＩＣＰマッチングが行われてしまう。その結果、ＩＣＰマッチングの失敗を生じやすくしてしまう。
【００５２】
そこで本発明においては、モデル点群１中の全部あるいは一部の点を注目点とし、その注目点から最近傍点である計測点群２中の１点（対応点）を算出するような対応点探索、すなわち逆マッチング処理を採用する。
【００５３】
理解を容易にするために、計測点群を｛Ｄ1，Ｄ2，Ｄ3，…｝とし、モデル点群を｛Ｍ1，Ｍ2，Ｍ3，…｝とした簡単な例を考える。すると、計測点群にノイズがない理想的な状況では、
※  順マッチング処理：Ｄ1→Ｍ1，Ｄ2→Ｍ2，Ｄ3→Ｍ3，…；
※  逆マッチング処理：Ｍ1→Ｄ1，Ｍ2→Ｄ2，Ｍ3→Ｄ3，…；
のように、双方は実質的に同じ結果を与える。
【００５４】
しかしながら、計測点群がノイズ点ＮＰを含んで｛Ｄ1，Ｄ2，ＮＰ，Ｄ3，…｝となっていると、たとえば、
※  順マッチング：Ｄ1→Ｍ1，Ｄ2→Ｍ2，ＤＰ→Ｍ3，Ｄ3→Ｍ3、…；
※  逆マッチング：Ｍ1→Ｄ1，Ｍ2→Ｄ2，Ｍ3→Ｄ3，…；
のように、双方の結果は異なるものとなる。
【００５５】
すなわち、順マッチングではノイズ点ＤＰも強制的に注目点とするために、本来は不要な対応点探索を行ってしまうが、逆マッチングで注目点（すなわち対応点探索の出発点）となるのはモデル点のみであるから、ノイズの影響を受けにくいことになる。
【００５６】
順マッチングでのこのような誤検出の影響は、対象体と３次元モデルとの位置姿勢がある程度近くなった以後、すなわちＩＣＰアルゴリズムの繰返しがある程度進んだ後に生じやすい。それは、双方がまだ遠いとき（図５（ａ））には、モデル点群から見たときに、本来の計測点（以下「真正計測点」）までの距離とノイズ点までの距離と方向とが、あまり変わらないのに対して、双方が近くなってきた段階（図５（ｂ））では、ノイズ点とモデル点群との間の距離が（真正計測点と比較して）相対的にかなり大きくなることがあり、それがＩＣＰアルゴリズムへ及ぼす影響が増大するためである。
【００５７】
これに対して、逆マッチングでは、真正計測点群から離れたノイズ点は対応点として採用されにくくなり、ノイズ点による誤差を抑圧する機能に優れる（図５（ｃ）（ｄ））。このような理由によって、本発明では逆マッチング処理を採用するわけである。
【００５８】
逆マッチング処理は、図１に示すマッチング手段９において行われる。このとき用いられる計測点群２は、対象体３を撮影したカメラ４、５による撮影結果に基づいて生成手段６において生成された、３次元位置情報を有する各点からなるものである。またモデル点群１は、記憶手段７に記憶されている、対象体３の３次元モデル上に規定された点群である。各点が３次元位置情報を有している。
【００５９】
このようにして計測点群２およびモデル点群１それぞれの各点の３次元位置情報が、取得手段８において取得される。
【００６０】
図６および図７は、逆マッチング処理を段階的に説明するためのフローチャートとその概念図である。逆マッチング処理では、まず、計測点群２とモデル点群１との各点のうち、逆マッチング処理に用いる範囲（以下「対象範囲」）を決める（図６ステップＳ２−１）。この対象範囲はたとえば、空間範囲を限定したり、特定の方向から観察できる点だけに限定する（すなわち方向範囲を限定する）ことなどによって行われる。
【００６１】
次に、当該対象範囲内で、モデル点群１中のある１点（注目点）から最近傍点である計測点群２中の１点（対応点）を仮に算出する（図６ステップＳ２−２、図７（ａ））。各注目点とそれに対する対応点が決定したら（図７（ｂ））、注目点と対応点との距離の総和（平均二乗誤差）を最小化するような、計測点群２全体の位置変換（回転移動および平行移動に相当する座標変換マトリクス）を求める（図６ステップＳ２−３）。
【００６２】
上記の位置変換を行った後（図６ステップＳ２−４、図７（ｃ））、再度注目点に対する対応点を決定し、注目点と対応点との距離の総和（平均二乗誤差）を計算する（図６ステップＳ２−５）。その値が所定閾値以下あるいはその値と先の総和の値との差が所定閾値以下である場合には、逆マッチング処理を終了する（図６ステップＳ２−６）。そうでない場合には、ステップＳ２−１〜ステップＳ２−５までの動作を繰り返す。なお、図７においては、モデル点群１にのみ位置変換を行っているが、計測点群２とモデル点群１との位置関係は相対的なものであるので、計測点群２を位置変換してもよいし、双方を位置変換してもよい。
【００６３】
逆マッチング終了後、計測点群２、モデル点群１の位置変換から、計測点群２とモデル点群１との相対的な空間位置関係を認識手段１０において把握し、実際の対象体３の３次元位置・姿勢を認識することができる。
【００６４】
＜Ｂ−５．効果＞
この逆マッチング処理によれば、注目点となるモデル点群１の各点は、その全てが対象体３の３次元形状を示す有効な点であるので、前述の順マッチング処理のようなノイズの影響を抑制することができる。よって、その結果に基づいた３次元位置・姿勢認識の精度を向上させることができる。
【００６５】
＜Ｃ．第２実施形態＞
＜Ｃ−１．代表点の一致化＞
図８に示すのは、第２実施形態にかかる３次元位置・姿勢認識システムＳＹＳ２である。図１に示すシステムＳＹＳ１と比較して、システムＳＹＳ２の３次元位置・姿勢認識装置１００Ｂでは、取得手段８とマッチング手段９との間に、代表点一致化手段１１が介在しているところが異なる。その他の構成については図１に示す場合と同様であるので、詳細な説明は省略する。
【００６６】
ところで、上記の逆マッチング処理を行う場合でも、モデル点群１と計測点群２との初期位置が大きく異なっていると、その初期位置に依存して逆マッチング処理が失敗してしまう場合がある。
【００６７】
そのような原因による逆マッチング処理の失敗を抑制するため、逆マッチング処理を行う前に、計測点群２全体の空間位置を代表する計測代表点としての計測重心点（または計測中心点）の空間位置と、モデル点群１全体の空間位置を代表するモデル代表点としてのモデル重心点（またはモデル中心点）の空間位置とを、予め一致させておくことが有効である。図８における代表点一致化手段１１は、マッチング手段９において逆マッチング処理がなされる前に、計測重心点（または計測中心点）とモデル重心点（またはモデル中心点）とを一致させる（位置あわせ）機能をもつ。
【００６８】
ここで計測重心点とは、計測点群２に属する各点Ｄiのそれぞれの３次元座標を（Ｘi，Ｙi，Ｚi）としたときの各座標値の平均値：
Ｘav＝＜Ｘi＞，  Ｙav＝＜Ｙi＞，  Ｚav＝＜Ｚi＞，
で規定される３次元座標値（Ｘav，Ｙav，Ｚav）を持つ点を指す。ただし、記号＜…＞は平均値を示す。
【００６９】
また、計測中心点とは、各点Ｄiの座標値の集合｛Ｘi｝，｛Ｙi｝，｛Ｚi｝のそれぞれにおける最大値Ｘmax，Ｙmax，Ｚmaxと、最小値Ｘmin，Ｙmin，Ｚminとの区間を２等分割する座標値：
Ｘmid＝（Ｘmax−Ｘmin）／２，
Ｙmid＝（Ｙmax−Ｙmin）／２，
Ｚmid＝（Ｚmax−Ｚmin）／２，
を３次元座標値（Ｘmid，Ｙmid，Ｚmid）として持つ点を指す。
【００７０】
このように、代表点は計測点群の全体の空間的位置をおおまかに示す点であって、他の例としては、計測中央点、すなわち各点Ｄiの総数Ｎdに対して、座標値順位が中央順位（Ｎ／２番目）付近となっている点なども採用可能である。モデル点群についての代表点も同様である。
【００７１】
このようにして計測点群２とモデル点群１との位置あわせを行ってから逆マッチング処理を行えば、モデル点群１中の注目点に対する計測点群２中の対応点が適切に算出され、ノイズの影響がより少なくなる。
【００７２】
＜Ｃ−２．計測点周辺の平均値の利用＞
生成手段６において、ステレオ法を用いて３次元位置情報を生成する場合、いわゆるブロックマッチング（例えば５×５画素や７×７画素といった小領域でのマッチング）を利用しているため、左右画像に存在する対象体のエッジをマッチングして３次元形状を生成すると、そのエッジを示す点群には所定の太さが生じる。このとき、この所定の太さを持った計測点群のエッジと、理想的に太さのほとんどないモデル点群のエッジとで逆マッチング処理すると、計測点群のエッジの太さの分だけ逆マッチング処理に誤差が生じ、逆マッチング処理の失敗を招くことがある。
【００７３】
図９に模擬的に示すように、モデル点群のエッジ３００が本来位置変換されるべき座標（正しい位置）は計測点群のエッジ３０１の中央付近に位置しているが（図９（ｄ））、逆マッチング処理の繰り返し回数がｎ、ｎ＋１、ｎ＋２と重ねられていっても、その座標に到達する前に逆マッチング処理は終了してしまう（図９（ａ）〜（ｃ））ため、逆マッチング処理に誤差が生じる。
【００７４】
よって、図１０に示すような方法で、計測点群のエッジを擬似的に細くする。すなわち、計測点群のエッジが所定の太さを持つ場合、モデル点群における注目点３０２（三角印で示す）からの最近傍点として算出された計測点群（各点を丸印で示す）中の１つの点３０３を中心とする所定範囲内（探索領域３０５）の計測点を平均し、その平均位置の座標値を持つ点を擬似最近傍点３０４（四角印で示す）として算出する。
【００７５】
ここにおける所定範囲としては、典型的には距離を指標として規定される空間範囲であり、たとえば、Ｘ，Ｙ，Ｚの各方向について所定の距離の幅を持つ立方体（図１０で破線で囲む範囲）や、所定距離を半径とする仮想的な球内の範囲を採用することができる。計測点群が対象体のエッジに沿って得られるようなときには、エッジに直交する方向についてのみの座標平均値などのように、方向をも追加指標として所定範囲を設定することもできる。最近傍点として算出した点３０３に代えて、その擬似最近傍点３０４を対応点として逆マッチング処理に用いることで、比較的大きな太さを持つエッジの場合にも、適切に逆マッチング処理ができ、その精度を向上させることができる。
【００７６】
＜Ｃ−３．効果＞
第２実施形態によれば、代表点一致化手段１１によって相対的な空間位置関係が調整された後の、各点の３次元位置情報を用いることで、逆マッチング処理を行う前に、計測点群２とモデル点群１との相対的な空間位置関係が調整されるので、双方の初期位置が大きく異なることによるＩＣＰマッチングの失敗を抑制し、対象体３の３次元位置・姿勢認識の精度を向上させることができる。
【００７７】
さらに、逆マッチング処理に際して、計測点群２中の対応点を中心として所定範囲内に存在する計測点群２中の各点の空間位置を平均した点（擬似最近傍点）を、モデル点群１中の注目点に対する対応点として採用することで、所定の太さを持った計測点群のエッジ３０１を模擬的に細くし、適切に逆マッチング処理を行うことができる。よって、対象体３の３次元位置・姿勢認識の精度を向上させることができる。
【００７８】
＜Ｄ．第３実施形態＞
＜Ｄ−１．構成＞
カメラ等による撮影結果に基づいて生成される計測点群は、カメラ等の観測方向により計測可能な面が限定され、対象体の全周形状の一部分の形状を表すものとなる。図１１に示すように、この部分形状に対応する計測点群２（黒丸）と、対象体の全周形状に対応するモデル点群１（白丸）とを逆マッチング処理させると、計測点群２（黒丸）とは対応しないモデル点群１（白丸）中の部分と逆マッチング処理されてしまう可能性が高まる。よって３次元位置・姿勢認識の精度が低下することになる。
【００７９】
図１２に示すのは、本発明の第３実施形態にかかる３次元位置・姿勢認識システムＳＹＳ３の構成を示す概念図である。図１に示すシステムＳＹＳ１と比較して、システムＳＹＳ３の３次元位置・姿勢認識装置１００Ｃでは、取得手段８内に抽出手段１２が備えられていることが異なる。その他の構成については図１に示すシステムＳＹＳ１と同様であるので、詳細な説明は省略する。なお、図１２においては、図８に示した代表点一致化手段１１は記載されていないが、図８に示す場合と同様に、第３実施形態のシステムＳＹＳ３が代表点一致化手段を備えることも可能である。
【００８０】
＜Ｄ−２．動作＞
上記のような、計測点群２とは対応しないモデル点群１中の部分と逆マッチング処理されてしまうことを防ぐため、記憶手段７において記憶するモデル点群として、第１実施形態において示したような対象体３の全周形状に対応するモデル点群１ではなく、部分モデル点群１０１を採用する（図１３参照）。部分モデル点群１０１は、対象体３のさまざまな部分的な形状に対応する複数の部分モデルから構成されており、対象体３の３次元モデルの観察方向に応じて、例えば各面の形状に対応する点群で構成される「点群の集合」である。
【００８１】
部分モデル点群１０１は、観察方向に応じた形状のみに対応する点群であるので、計測点群２との逆マッチング処理を行う際に、よりその精度を高めることができる。計測点群２を生成したときの観察方向に対応する部分モデル点群１０１を用いて逆マッチング処理すれば、計測点群２に含まれない部分の形状と逆マッチング処理してしまうことがなくなるからである。
【００８２】
抽出手段１２は、記憶手段７において予め記憶された複数の部分モデル点群１０１のうちの１つの点群を特定してその点群（以下「特定点群」）に属する各点の３次元位置情報を抽出するとともに、マッチング手段９において計測点群２と逆マッチング処理させる点群として採用させる。これは、計測点群２のおおまかな分布から「特定点群」を自動選択してもよいし、その段階で対象体３がどのような方向に向いているかについての概略情報があらかじめわかっていれば、その情報に基づいて自動選択してもよい。オペレータがコンピュータの操作入力によって選択することも可能である。
【００８３】
また図１４に示すように、カメラ４、５で２次元の画像が撮影された段階で、その２次元画像から対象体３の３次元姿勢の判別をし（例えば表、横、裏）、記憶手段７における部分モデル点群１０１の選択に反映させることもできる。これによって、３次元位置・姿勢認識の精度を向上させ、また処理時間の短縮にも効果がある。
【００８４】
＜Ｄ−３．効果＞
このように、第３実施形態のシステムＳＹＳ３の３次元位置・姿勢認識装置１００Ｃは、３次元モデルの観察方向に応じて予め準備された複数の部分モデル点群１０１から１つの点群の各点の３次元位置情報を抽出し、当該１つの部分モデル点群１０１をモデル点群として採用させる抽出手段１２を備える。
【００８５】
このため、計測点群２を生成したときと同じ観察方向に対応する部分モデル点群１０１を抽出して逆マッチング処理すれば、計測点群２に含まれない部分の形状と逆マッチング処理してしまうことがなくなり、３次元位置・姿勢認識の精度を高めることができる。
【００８６】
＜Ｅ．第４実施形態＞
＜Ｅ−１．構成＞
図１５に示すのは、第４実施形態にかかる３次元位置・姿勢認識システムＳＹＳ４の構成を示す概念図である。第１実施形態の図１に示すシステムＳＹＳ１と比較して、システムＳＹＳ４の３次元位置・姿勢認識装置１００Ｄは、マッチング手段９内に、取得手段８と接続される順マッチング手段１３と、順マッチング手段１３と接続される逆マッチング手段１４とが備えられていることが異なる。その他の構成については第１実施形態の図１に示すシステムＳＹＳ１と同様であるので、詳細な説明は省略する。なお、図１５においては、図８に示した代表点一致化手段１１は記載されていないが、図８に示す場合と同様に備えることも可能である。また、図１２に示した抽出手段１２も記載されていないが、図１２に示す場合と同様にシステムＳＹＳ４が抽出手段１２を備えることも可能である。
【００８７】
＜Ｅ−２．動作＞
第４実施形態では、逆マッチング手段１４において逆マッチング処理を行う前に、順マッチング手段１３において順マッチング処理を行い、順マッチング手段１３によって順マッチング処理された後の計測点群２とモデル点群１とに基づいて、逆マッチング処理を行う。
【００８８】
順マッチング処理は、前述のように計測点群２における各ノイズからの最近傍点をも算出してしまうが、例えば図１６、図１７のようにノイズが分布する計測点群２（黒丸）がある場合、逆マッチング処理のみを行った場合（図１６）と、順マッチング処理を行った後、逆マッチング処理を行った場合（図１７）とでは、３次元位置・姿勢認識の精度に差異が生じる。すなわちこの場合、図１７に示す方が精度よくＩＣＰマッチングしている。
【００８９】
これは、ノイズも含めた計測点群２（黒丸）全体の分布とモデル点群１（白丸）とで行われる順マッチング処理により、計測点群２（黒丸）とモデル点群１（白丸）とが、計測点群２（黒丸）全体の分布に沿って位置あわせされ、その後の逆マッチング処理を開始する段階での計測点群２（黒丸）、モデル点群１（白丸）双方の初期位置が大きく異ならないことに起因する。これは図５を用いて既に概略説明した事情と対応している。原理的に計測点群２、モデル点群１双方の初期位置を、ノイズも含めた計測点群２全体の分布に沿って予め近づけておくことにより、逆マッチング処理の精度を高め、３次元位置・姿勢認識の精度を高めることができる。
【００９０】
換言すれば、順マッチングは位置・姿勢合わせの初期段階（すなわちラフな位置・姿勢合わせ）には有効であるが、近距離からの位置・姿勢合わせ（すなわち精密な位置・姿勢合わせ）の段階になると精度が低下するという性質があり、近距離からの位置・姿勢合わせには逆マッチングの方が効果が高い。そこで、これらを組み合わせるにあたっては、「先に順マッチングを行い、その後に逆マッチングを行う」という順序に格別の意義がある。この順序を反転させて逆マッチングの方を先に行う場合には、このような相乗的な効果は得られない。
【００９１】
この第４実施形態の後半の動作で実行される具体的な順マッチング処理の詳細は、図２および図３を参照して既に「第１実施形態」の欄で説明したので、この欄では省略する。
【００９２】
第４実施形態での逆マッチング処理の初期値は、順マッチング処理後の計測点およびモデル点のそれぞれの位置情報であるが、それらは生成手段６および記憶手段７から取り込んだ位置情報を起源としており、それらを座標変換したものに相当する。このため、第１〜第３実施形態のように逆マッチングだけを行う場合のほか、この第４実施形態中の逆マッチングもまた、取得手段８によって取得された位置情報に基づいたＩＣＰマッチング処理であると言える。
【００９３】
なお、順マッチング処理は、逆マッチング処理前の位置あわせを行うものであるので、注目点と対応点との距離の総和（平均二乗誤差）が最小化（収束）する前に、同処理を終了してもよい。その具体的な基準としては、たとえば、
・ＩＣＰアルゴリズムにおける繰返し回数の上限閾値を定めておき、繰返し回数（収束パラメータ）が当該閾値に至ると順マッチングを終了する、
・収束パラメータとして上記の平均二乗誤差を採用してその閾値を定めておき、収束パラメータが当該閾値以下になると順マッチングを終了する、
・収束パラメータとして上記の平均二乗誤差を採用し、その時点での収束パラメータが収束パラメータ自身の初期値の所定の％（閾値割合）以下になると順マッチングを終了する、
などを採用することができる。
【００９４】
＜Ｅ−３．効果＞
このように、第４実施形態によれば、順マッチング処理の後に逆マッチング処理を行うことにより、それらの固有の利点を相乗的に組み合わせて、３次元位置・姿勢認識の精度を高めることができる。
【００９５】
＜Ｆ．ロボットに搭載した場合の動作＞
図１８に示すのは、本発明の実施形態にかかる３次元位置・姿勢認識システムを搭載した産業用ロボットである。具体的には産業用ロボットに備えられたコンピュータにインストールされて実行されることにより、３次元位置・姿勢認識装置としての機能を産業用ロボットに果たさせている。図に示すようにロボット２００は、回転自在のロボット本体２０１と、ロボット本体に装着されたアーム２０２とを備える。
【００９６】
アーム２０２は、伸縮自在の多関節アームであり、その先端に装着されたハンド２０３により対象体３を挟む挟持動作を行うことが可能である。ハンド２０３には、カメラ４、５が備えられている。また、対象体３を照らす照明装置５０、５１も備えられている。
【００９７】
ロボット２００は、対象体３をハンド２０３で挟む挟持動作の開始にあたって、カメラ４、５による撮影で２次元の画像データを生成する。その２次元の画像データに基づいて、計測点群の各点の３次元位置情報が図示しない生成手段６において生成される。それらの各点からなる計測点群と、図示しない記憶手段７において記憶された対象体３のモデル点群とを用いて逆マッチング処理することで、対象体３の３次元位置・姿勢を認識し、それに基づいてハンド２０３の機構的制御を行うことによって、対象体３をハンド２０３で正確に挟むことが可能となる。
【００９８】
この産業用ロボットにおける対象体３の３次元位置・姿勢の認識には既述した第１〜第４実施形態のいずれも利用可能であるほか、またそれらの変形例のいずれもが利用可能である。
【００９９】
工業製品などをロボット等の自動機械で取り扱おうとする場合、工業製品の３次元位置・姿勢の認識を自動的に行うことが重要となる。本発明にかかる３次元位置・姿勢認識システムを搭載することで、対象体の３次元位置・姿勢認識精度が向上し、導入の幅が広がる。
【０１００】
＜Ｇ．変形例＞
※  対象物の３次元モデルを表現するモデル点群の位置情報は、データベースとして３次元位置・姿勢認識装置１００Ａ〜１００Ｄの外部にあってもよい。この場合にはネットワーク経由で必要なモデル点群の位置情報を取得できる。また、比較的長期間にわたって同一種類の対象物の認識を行うシステムの場合には、モデル点群の位置情報は、オフラインでシステムにロードされてもよい。さらに、カメラ４，５からの画像情報に基づいて計測点群の位置情報を得るまでの段階は、別体のコンピュータで行ってもよい。すなわち、３次元位置・姿勢認識装置１００Ａ〜１００Ｄのうち、この発明の特徴に相当する本質部分としては、取得手段８から認識手段１０までの機能手段である。
【０１０１】
※  この発明によって３次元位置・姿勢を認識可能な対象体は、物品だけでなく、生物（人間や動植物）も含まれる。また、３次元計測手段としてステレオカメラを用いたが、ステレオカメラに限らず、レーザによる光切断やＴＯＦ（Time of Flight）法を利用した距離画像センサ等により３次元計測点群を取得してもよい。なおＴＯＦ法とは、距離測定の１つの方法であり、光源から出た光を対象物で反射させ、センサに届くまでの光の飛行時間（遅れ時間）と光の速度（３×１０8ｍ／ｓ）とから、被写体までの距離を得る方法である。
【符号の説明】
【０１０２】
１  モデル点群
２  計測点群
３  対象体
１００Ａ〜１００Ｄ  ３次元位置・姿勢認識装置
１０１  部分モデル点群
２００  ロボット
３００，３０１  エッジ
３０２  注目点
３０４  擬似最近傍点
３０５  探索領域
ＳＹＳ１〜ＳＹＳ４  ３次元位置・姿勢認識システム
【図１】
2011209116.tif 000003
【図２】
2011209116.tif 000004
【図３】
2011209116.tif 000005
【図４】
2011209116.tif 000006
【図５】
2011209116.tif 000007
【図６】
2011209116.tif 000008
【図７】
2011209116.tif 000009
【図８】
2011209116.tif 000010
【図９】
2011209116.tif 000011
【図１０】
2011209116.tif 000012
【図１１】
2011209116.tif 000013
【図１２】
2011209116.tif 000014
【図１３】
2011209116.tif 000015
【図１４】
2011209116.tif 000016
【図１５】
2011209116.tif 000017
【図１６】
2011209116.tif 000018
【図１７】
2011209116.tif 000019
【図１８】
2011209116.tif 000020