JP2013072857A - ３次元位置・姿勢認識装置およびその方法、プログラム - Google Patents

Abstract

【課題】本発明は、対象体本来の形状のうち部分的な形状の情報を欠いた計測点群を用いる場合であっても、対象体の３次元位置・姿勢を適切に認識可能な３次元位置・姿勢認識装置およびその方法、プログラムの提供を目的とする。
【解決手段】本発明にかかる３次元位置・姿勢認識装置は、計測点群と、対応するモデル点群とをマッチングさせ、モデル点群のうち、マッチング結果に基づいて特定される点を削除処理するモデル処理手段１０と、計測点群と、モデル処理手段１０において削除処理したモデル点群とをマッチングさせ、対象体３の３次元位置・姿勢を認識する認識手段１１とを備えることを特徴とする。
【選択図】図１

Description

本発明は３次元位置・姿勢認識装置およびその方法、プログラムに関し、特に、ロボット動作等に必要な、対象体の３次元位置・姿勢認識に関する。

部品等の対象体の空間位置および空間姿勢の少なくとも一方を含む３次元座標値の組によって３次元位置・姿勢を定義する場合、対象体の３次元位置・姿勢を認識するために、その対象体の３次元形状を示す、各点が３次元位置情報を有する点群データ同士のマッチングを行う方法が従来から知られている。

この方法では、対象体を計測し、ステレオ法等を用いて生成した３次元位置情報を各点が有する計測点群と、当該対象体に対応してあらかじめ準備された、各点が３次元位置情報を持つ点群であるモデル点群とをＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）マッチング等させて、当該対象体の３次元位置・姿勢を認識することができる。

ここで、対象体を計測した計測点群にノイズによる点群が含まれると、当該ノイズの影響により適切にマッチングできない場合があった。

このような場合の対策として、例えば特許文献１および２では、あらかじめ計測点群からノイズと判断される点群を除去し、ノイズを除去した計測点群とモデル点群とをマッチングさせることで、適切にマッチングしている。

また特許文献３では、計測点群におけるノイズの除去方法として、同一平面上にあると推定される線分を抽出することで平面を求め、求めた平面に基づいてノイズを判断する方法が示されている。

特開２００７−３２２５１号公報 特開２００６−６４４５３号公報 国際公開第２００５／８８２４４号公報

一方で、対象体の３次元計測は複数のカメラを用いる等により行われるが、対象体の所望範囲の形状が撮像範囲に入らない場合がある。また撮像範囲に入っている場合であっても、対象体を照らす照明が不十分である等ことによって、画像取得において十分な解像度を得られない場合もある。

このような場合に、取得できなかった部分の計測点を欠いた、すなわち、対象体本来の形状のうち部分的な形状の情報を欠いた計測点群（欠落計測点群）が得られることになる。この欠落計測点群とモデル点群とをマッチングさせると、適切に対応すべき点群が欠落していることから、適切なマッチングが抑制される。そして、計測点群とモデル点群とを適切にマッチングできないことにより、対象体の３次元位置・姿勢を適切に認識できないという問題があった。

本発明は、上記のような問題を解決するためになされたものであり、対象体本来の形状のうち部分的な形状の情報を欠いた計測点群を用いる場合であっても、対象体の３次元位置・姿勢を適切に認識可能な３次元位置・姿勢認識装置およびその方法、プログラムの提供を目的とする。

請求項１の発明である３次元位置・姿勢認識装置は、対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する取得手段と、前記計測点群と、対応する前記モデル点群とをマッチングさせるマッチング手段と、前記モデル点群のうち、前記マッチング手段におけるマッチング結果に基づいて特定される点を削除処理するモデル処理手段と、前記計測点群と、前記モデル処理手段において削除処理した前記モデル点群とをマッチングさせ、前記対象体の３次元位置・姿勢を認識する認識手段とを備えることを特徴とする。

請求項２の発明は、請求項１に記載の３次元位置・姿勢認識装置であって、前記モデル処理手段が、前記マッチング手段におけるマッチングの結果、対応する計測点群の各点との間隔が所定距離以上である前記モデル点群の各点を削除処理することを特徴とする。

請求項３の発明は、請求項１または２に記載の３次元位置・姿勢認識装置であって、前記マッチング手段および前記認識手段における前記マッチングは、ＩＣＰマッチングであることを特徴とする。

請求項４の発明は、請求項１〜３のいずれかに記載の３次元位置・姿勢認識装置であって、前記対象体の３次元計測によって得られた前記計測点群からノイズを除去するノイズ除去手段をさらに備え、前記取得手段が、前記ノイズ除去手段においてノイズ除去された前記計測点群の各点の３次元位置情報と、前記モデル点群の各点の３次元位置情報とを取得することを特徴とする。

請求項５の発明は、請求項４に記載の３次元位置・姿勢認識装置であって、前記対象体の画像を撮像する撮像手段をさらに備え、前記ノイズ除去手段が、前記計測点群の各点の、前記対象体と前記撮像手段とを通る軸成分の分布に基づいて、前記計測点群からノイズを除去することを特徴とする。

また、請求項６の発明は、３次元位置・姿勢認識方法であって、（ａ）対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する工程と、（ｂ）前記計測点群と、対応する前記モデル点群とをマッチングさせる工程と、（ｃ）前記モデル点群のうち、マッチング結果に基づいて特定される点を削除処理する工程と、（ｄ）前記計測点群と、削除処理した前記モデル点群とをマッチングさせ、前記対象体の３次元位置・姿勢を認識する工程とを備えることを特徴とする。

また、請求項７の発明は、３次元位置・姿勢認識プログラムであって、コンピュータにインストールされて実行されることにより、前記コンピュータを、請求項１〜５のいずれかに記載の３次元位置・姿勢認識装置として機能させることを特徴とする。

請求項１〜７の発明によれば、対象体の計測点群と、対応するモデル点群とをマッチングさせ、そのマッチング結果に基づいて、前記モデル点群のうち、マッチング結果に基づいて特定される点を削除処理し、当該削除処理を行った前記モデル点群を用いて前記計測点群とマッチングさせ、前記対象体の３次元位置・姿勢を認識することにより、対象体の欠落計測点群を用いて３次元位置・姿勢認識を行う場合であっても、欠落計測点群の欠落部分に対応するモデル点群の各モデル点をマッチング結果に基づいて削除することにより、適切にマッチングさせ、対象体の３次元位置・姿勢認識を適切に行うことができる。

特に請求項２の発明によれば、モデル処理手段が、マッチング手段におけるマッチングの結果、対応する計測点群の各点との間隔が所定距離以上であるモデル点群の各点を削除処理することにより、対応点を持たないモデル点群の各モデル点を適切に特定し、欠落計測点群とモデル点群とのマッチングの精度を高めることができる。よって、対象体の３次元位置・姿勢認識を適切に行うことができる。

特に請求項４の発明によれば、対象体の３次元計測によって得られた前記計測点群からノイズを除去するノイズ除去手段をさらに備え、ノイズ除去された前記計測点群の各点の３次元位置情報と、前記モデル点群の各点の３次元位置情報とを取得することにより、欠落計測点群におけるノイズの影響を抑制し、欠落計測点群とモデル点群とのマッチングの精度をさらに高めることができる。よって、対象体の３次元位置・姿勢認識を適切に行うことができる。

特に請求項５の発明によれば、ノイズ除去手段が、欠落計測点群の各点の、対象体と撮像手段とを通る軸成分の分布に基づいて、欠落計測点群からノイズを除去することにより、対象体と撮像手段とを通る軸成分の違いによって、対象体とノイズとを区別することができ、ノイズによる計測点を効果的に除去することができるため、欠落計測点群とモデル点群とのマッチングの精度をさらに高めることができる。よって、対象体の３次元位置・姿勢認識を適切に行うことができる。

３次元位置・姿勢認識装置の構成を示す図である。 ３次元位置・姿勢認識装置の動作を示すフローチャートである。 対象体を撮像する場合の具体例を示す図である。 対象体の計測点群の具体例を示す図である。 マッチング結果の具体例を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識システムをロボットに搭載した場合の具体例を示す図である。 ３次元位置・姿勢認識装置の構成を示す図である。 ３次元位置・姿勢認識装置の動作を示すフローチャートである。 対象体を撮像する場合の具体例を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。 ３次元位置・姿勢認識装置の処理動作を示す図である。

＜定義＞
この明細書における「対象体の３次元位置・姿勢」とは、対象体の空間位置および空間姿勢の少なくとも一方を含む座標値の組として定義される。空間姿勢は、対象体が１軸回転完全対称性を有する場合（例：円柱）には２自由度であり、１軸回転完全対称性を持たない場合には３自由度となる。

したがって、この発明における「３次元位置・姿勢のマッチング」とは、位置に関する３自由度と姿勢に関する３自由度との合計６自由度の全てをマッチングさせる処理だけでなく、少なくとも、（１）位置についての３自由度のマッチング、および、（２）姿勢についての２以上の自由度のマッチング、のいずれかを含んでいれば、この発明の適用範囲に含まれる。

＜第１実施形態＞
＜構成＞
図１は、本発明の第１実施形態にかかる３次元位置・姿勢認識システム１０００の構成を示す概念図である。図１に示すように３次元位置・姿勢認識システム１０００は、当該３次元位置・姿勢認識システム１０００外の対象体３（部品等）を異なる方向から撮影する複数の撮影手段であるカメラ４およびカメラ５と、カメラ４およびカメラ５の撮影結果に基づいて、対象体３の３次元位置・姿勢を認識する３次元位置・姿勢認識装置１００とを備える。

３次元位置・姿勢認識装置１００は、ハードウェアとしては、汎用のコンピュータを用いて構成することが可能である。すなわち演算処理手段や制御手段としてのＭＰＵの他、記憶デバイスやディスプレイやマウスなどのマンマシン・インターフェイスを備えたコンピュータが３次元位置・姿勢認識装置１００として利用可能である。そして、このコンピュータにおいて３次元位置・姿勢認識装置１００としての機能を実現させるプログラムが、あらかじめこのコンピュータにインストールされている。

３次元位置・姿勢認識装置１００は、このコンピュータプログラムによって実現される各機能手段、すなわち、カメラ４およびカメラ５の撮影結果に基づいて、対象体３の計測点群の各計測点の３次元位置情報を生成する生成手段６と、対象体３の３次元モデル上に規定されたモデル点群の各モデル点の３次元位置情報を記憶する記憶手段７と、生成手段６から対象体３の計測点群の各計測点の３次元位置情報を取得するとともに、記憶手段７からモデル点群の各モデル点の３次元位置情報を取得する取得手段８と、取得手段８において取得した計測点群の各計測点およびモデル点群の各モデル点の３次元位置情報に基づいて、計測点群およびモデル点群のマッチング処理を行うマッチング手段９と、マッチング手段９におけるマッチング結果に基づいて、モデル点群を処理するモデル処理手段１０と、モデル処理手段１０において処理したモデル点群と計測点群とを用いて、３次元位置・姿勢認識を行う認識手段１１とを備える。

対象体３の計測点群とは、カメラ４およびカメラ５で撮影された対象体３のステレオ画像から抽出された、対象体３の形状を規定する特徴的な点の集合であって、各計測点がその３次元位置座標によって表現されている。

一方モデル点群とは、あらかじめ形状が分かっている対象体３の３次元モデル上に規定された点の集合であって、各モデル点がその３次元位置座標によって表現され、全体として対象体３の全周形状を示している。対象体３が工業製品の場合にはそのＣＡＤデータなどを利用可能である。

また、モデル点群を記憶する記憶手段７は、図示するように３次元位置・姿勢認識装置１００に備えられていてもよいし、外部の記憶装置等であって、３次元位置・姿勢認識装置１００の取得手段８が、その記憶装置等から通信等によって取得するものであってもよい。

＜動作＞
次に、３次元位置・姿勢認識装置１００を用いた、対象体３の３次元位置・姿勢認識動作について、図２〜図１０を用いて説明する。

まず、生成手段６において、カメラ４およびカメラ５を用いた、対象体３の計測点群の生成を行う（ステップＳ１）。具体的には、異なる方向から対象体３を撮像するカメラ４およびカメラ５を用い、対象体３の画像を取得する。対象体が複数ある場合には、その画像中から目標とする対象体の部分を切り出して用いることも可能である。さらに、取得した画像に対しステレオ処理、すなわち２つのカメラ４およびカメラ５からの視差に基づく三角測量の原理によって、各カメラから対象体３上の各点の３次元的な相対位置の特定を行うことにより、計測点群に相当する各計測点の３次元位置情報を生成する。

次に、取得手段８において、生成手段６において生成した計測点群の各点の３次元位置情報と、記憶手段７にあらかじめ記憶されている対象体３のモデル点群の各点の３次元位置情報とを取得する（ステップＳ２）。

次に、マッチング手段９において、計測点群とモデル点群とのマッチング処理を行う（ステップＳ３）。本実施形態においては、特にＩＣＰマッチング処理を行う場合を説明する。

このＩＣＰマッチング処理は、計測点群の各計測点と、あらかじめ適宜配置したモデル点群の中の最も近い点（最近傍点）とをそれぞれ対応させ、それらの位置が合致するように計測点群全体の位置変換（回転移動と平行移動等）を行う方法である。そして、計測点群全体の位置変換（回転移動、平行移動等）の変換値によって、計測点群とモデル点群との相対的な空間位置関係を把握し、対象体３の３次元位置・姿勢を認識する。なお、計測点群とモデル点群との位置関係は相対的なものであるので、モデル点群を位置変換してもよいし、双方を位置変換してもよい。

各計測点と対応するモデル点との距離の総和（平均二乗誤差）を最小化するように当該処理を繰り返し、総和が所定値以下となった時点、あるいは先のＩＣＰマッチング処理における距離の総和との差が所定値以下になった時点で、処理動作を終了する。

ここで、対象体３の３次元位置情報を計測する際に、カメラ４およびカメラ５の撮像範囲２０に対象体３の形状全てが含まれない場合（図３（ａ）参照）や、撮像時の照明等の影響により、対象体３の部分的な形状が適切に撮像できない場合（図３（ｂ）参照）には、取得できなかった部分の計測点群を欠いた、対象体３本来の形状のうち部分的な形状の情報を欠いた計測点群（欠落計測点群）が得られることになる。本実施形態では特に、欠落計測点群が得られた場合について説明する。

図４は、このように部分的に３次元位置情報を欠いた欠落計測点群の分布を示す図である。図において、横軸はＸ軸成分を、縦軸はＹ軸成分をそれぞれ示している。図４中のＰ領域およびＱ領域では、計測点を特定できず、３次元位置情報を欠いた状態となっている。

このような欠落計測点群を用いてモデル点群とのマッチング処理を行うと、３次元位置情報を欠いた部分（Ｐ領域およびＱ領域）の影響を受け、適切なマッチングが抑制される。

例えば図５に示すように、計測点（黒丸）が得られていない部分が適切にマッチングされないことにより、欠落計測点群（黒丸）全体とモデル点群（白丸）全体との相対位置関係が正しく収束せず、ずれた位置でマッチングされてしまう。

よって以下の動作に示すように、一度欠落計測点群とマッチングを行ったモデル点群に対し所定の処理を行い、再度、欠落計測点群と当該処理を行ったモデル点群（後述の削除モデル点群）とのマッチングを行うことで、欠落計測点群において３次元位置情報を欠いた部分の影響を抑制し、適切なマッチングを可能とする。

まず、モデル処理手段１０において、マッチング手段９におけるマッチング処理の結果、適切にマッチングされなかったモデル点群の各モデル点を、削除する処理を行う（ステップＳ４）。

具体的には、まず欠落計測点群とモデル点群とのＩＣＰマッチング動作を開始する（図６参照）。図においては、黒丸は計測点群を示し、白丸はモデル点群を示す（図６以下同様）。欠落計測点群には、計測点が欠けている部分があり、また、対象体３の本来の形状を示すものではないノイズも含まれている。

各計測点と対応する各モデル点との距離の総和が所定値以下となるか、あるいは先のＩＣＰマッチング処理における距離の総和との差が所定値以下になることで、ＩＣＰマッチング動作が終了する。そうすると、欠落計測点群とモデル点群とは、所定の間隔で接近した状態となる（図７参照）。

ここで、モデル点群の各モデル点について、最も近い計測点群の点（最近傍点）までの距離を算出し、当該距離が閾値以上である点を、計測点群にマッチング対応点を持たないモデル点（非対応モデル点）とする。当該非対応モデル点とする閾値は、例えば計測点群を生成する際の間隔、解像度、３次元位置・姿勢認識に要求される精度等を考慮して、決定することができる。

この非対応モデル点には、計測点群において欠落しているわけではないが、閾値以上に離れた状態で収束したモデル点も含まれる。しかしこのようなモデル点も、適切にマッチングされていない点であるとみなし非対応モデル点に含める。

すなわち、本実施形態では欠落計測点群が得られた場合について説明するが、部分的な形状の情報を欠いていない計測点群についても、本発明は適用可能である。

非対応モデル点をモデル点群から削除し、当該処理をモデル点全点について行い、得られたモデル点群を削除モデル点群とする（図８参照）。計測点が欠けていた部分に対応するモデル点は、所定距離以内に最近傍点がないため、図８に示すように削除される。

このように、欠落計測点群において３次元位置情報の欠けた部分に合わせて、モデル点群における非対応モデル点を削除することにより、欠落計測点群において３次元位置情報を欠いた部分の影響を抑制し、適切にマッチングすることができる。

次に、認識手段１１において、欠落計測点群と削除モデル点群とを用いたマッチング処理を行い（図９参照）、対象体３の３次元位置・姿勢認識を行う（ステップＳ５）。

削除モデル点群を用いて欠落計測点群とのマッチング処理を行うことで、図７に示す場合（削除処理を行わないモデル点群を用いてマッチング処置する場合）よりも、適切にマッチングを行うことができる（図１０参照）。

＜ロボットに搭載した場合の動作＞
図１１は、本発明の実施形態にかかる３次元位置・姿勢認識システムを搭載した産業用ロボットである。具体的には産業用ロボットに備えられたコンピュータにインストールされて実行されることにより、３次元位置・姿勢認識装置としての機能を産業用ロボットに果たさせている。

図１１に示すようにロボット２００は、回転自在のロボット本体２０１と、ロボット本体に装着されたアーム２０２とを備える。

アーム２０２は、伸縮自在の多関節アームであり、その先端に装着されたハンド２０３により対象体３を挟む挟持動作を行うことが可能である。ハンド２０３には、カメラ４およびカメラ５が備えられている。また、対象体３を照らす照明装置５０および照明装置５１も備えられている。

ロボット２００は、対象体３をハンド２０３で挟む挟持動作の開始にあたって、カメラ４およびカメラ５による撮影で対象体３の画像データを生成する。その画像データに基づいて、計測点群の各計測点の３次元位置情報が図示しない生成手段６において生成する。

得られた計測点群が欠落計測点群である場合、まず、欠落計測点群と、図示しない記憶手段７において記憶された対象体３のモデル点群とを用いて図示しないマッチング手段９においてマッチング処理し、さらに、図示しないモデル処理手段１０において当該マッチング結果に基づく各モデル点の削除処理を行うことで、削除モデル点群を生成する。

さらに図示しない認識手段１１において、欠落計測点群と削除モデル点群とのマッチング処理によって、対象体３の３次元位置・姿勢を認識し、それに基づいてハンド２０３の機構的制御を行うことによって、対象体３をハンド２０３で正確に挟むことが可能となる。

工業製品などをロボット等の自動機械で取り扱おうとする場合、工業製品の３次元位置・姿勢の認識を自動的に行うことが重要となる。本発明にかかる３次元位置・姿勢認識システムを搭載することで、対象体３の３次元位置・姿勢認識精度が向上し、導入の幅が広がる。

＜効果＞
本発明にかかる実施形態によれば、３次元位置・姿勢認識装置において、欠落計測点群と、対応するモデル点群とをマッチングさせ、モデル点群のうち、マッチング結果に基づいて特定される点を削除処理するモデル処理手段１０と、欠落計測点群と、モデル処理手段１０において削除処理した削除モデル点群とをマッチングさせ、対象体３の３次元位置・姿勢を認識する認識手段１１とを備えることで、対象体３の欠落計測点群を用いて３次元位置・姿勢認識を行う場合であっても、欠落計測点群の欠落部分に対応するモデル点（非対応モデル点）をマッチング結果に基づいて削除することにより、適切にマッチングさせ、対象体３の３次元位置・姿勢認識を適切に行うことができる。

また、本発明にかかる実施形態によれば、３次元位置・姿勢認識装置において、モデル処理手段１０が、マッチング手段９におけるマッチングの結果、対応する欠落計測点群の各計測点との間隔が所定距離以上であるモデル点群の各モデル点を削除処理することで、対応点を持たないモデル点群の各モデル点（非対応モデル点）を適切に特定し、欠落計測点群と削除モデル点群とのマッチングの精度を高めることができる。よって、対象体の３次元位置・姿勢認識を適切に行うことができる。

＜第２実施形態＞
＜構成＞
図１２は、本発明の第２実施形態にかかる３次元位置・姿勢認識システム１００１の構成を示す概念図である。図１に示す３次元位置・姿勢認識システム１０００との違いは、３次元位置・姿勢認識装置１０１において、生成手段６から対象体３の計測点群の各計測点の３次元位置情報を取得し、計測点群におけるノイズを除去するノイズ除去手段１２がさらに備えられることである。

取得手段８は、ノイズ除去手段１２においてノイズ除去された計測点群の各計測点の３次元位置情報を取得するとともに、記憶手段７からモデル点群の各モデル点の３次元位置情報を取得する。

その他、第１実施形態と同様の構成については、詳細な説明を省略する。

＜動作＞
次に、３次元位置・姿勢認識装置１０１を用いた、対象体３の３次元位置・姿勢認識動作について、図１３〜図２１を用いて説明する。

まず、生成手段６において、カメラ４およびカメラ５を用いた、対象体３の計測点群の生成を行う（ステップＳ１）。ここで取得される計測点群は、第１実施形態と同様に欠落計測点群とする。

ここで、図１４に示すように、カメラ４およびカメラ５の撮像範囲２０に含まれる他の物体３０（図においては部分的に含まれている）がある場合、当該物体３０の形状を計測した計測点は、生成手段６において生成される計測点群にノイズとして含まれることになる。

このようなノイズを含む計測点群を用いてモデル点群とのマッチング処理を行うと、対象体３の形状を示すものではないノイズの影響を受け、マッチングの精度が低下する。

よって、計測点群におけるノイズを除去し、取得手段８においてはノイズを除去した計測点群を取得する（ステップＳ２）ことで、マッチングの精度を高めることができる。

計測点群におけるノイズの除去は、ノイズ除去手段１２において以下のように行うことができる（ステップＳ６）。

図１４に示すように、カメラ４およびカメラ５の撮像範囲２０において支配的に表示されている対象体３に対し、物体３０が写りこんでいるような場合に、計測された各計測点の３次元位置情報を、ＸＹＺ軸成分それぞれで抽出する。物体３０は、図に示すように対象体３と離間していてもよいし、互いに重なり合うように配置されていてもよい。

ここで、対象体３とカメラ４およびカメラ５（厳密には、カメラ４およびカメラ５の中点）とを結ぶ軸をＺ軸とし、Ｚ軸と直交する軸をＸ軸、さらにＸ軸およびＺ軸と直交する軸をＹ軸とする。ＸＹＺ軸は、鉛直方向または水平方向等に限られるものではなく、上記の定義において空間中自由に設定できるものである。

各計測点の３次元位置情報の、それぞれの軸成分について抽出し、成分の値の大きい順に並べ替えてグラフに示したものが図１５（ａ）および図１５（ｂ）である。横軸は、Ｘ軸成分、Ｙ軸成分、Ｚ軸成分それぞれを大きい順に並べ替えた場合の配列順を示す配列番号であり、縦軸は、Ｘ軸成分、Ｙ軸成分、Ｚ軸成分それぞれの座標値である。

３つのグラフのうち、最も上方に位置しているグラフ（菱形点）はＸ軸成分に対応し、次に上方に位置するグラフ（四角点）はＹ軸成分に対応し、最も下方に位置しているグラフ（三角点）はＺ軸成分に対応する。

なお当該表示方法については、図１５に示すような、各成分の中でその値自体の順により並べグラフに表示する場合には限られず、例えばＺ軸成分の値を、３次元位置情報の組におけるＸ軸成分またはＹ軸成分の値の順に並べて表示するものであってもよい。

床面に置かれた対象体３を鉛直上方からカメラ４およびカメラ５を用いて撮像する場合（図１１参照）を想定すると、対象体３の高さと物体３０の高さとの違いがＺ軸成分の値の違いとして、または、対象体３と物体３０との上面の傾斜の違いがＺ軸成分グラフの傾きの違いとして現れる。

図１５におけるＺ軸成分を示すグラフに注目すると、支配的な成分である配列番号６２５近傍までの成分は、それ以降の成分とはＺ軸成分の値、およびＺ軸成分グラフの傾きが異なっている。

このことから、支配的な成分が対象体３の上面を示すものと考えると、対象体３の上面の（Ｚ軸方向）高さが物体３０の上面の高さよりも高く、また、対象体３の上面が略水平（Ｚ軸方向に直交）であるのに対して物体３０の上面は傾斜していることが読み取れる。

このような場合、すなわち、対象体３の上面とは異なる高さの上面を有する物体３０が撮像範囲２０に写りこんでいる場合、または、対象体３の上面とは異なる傾斜の上面を有する物体３０が撮像範囲２０に写りこんでいる場合、以下の方法で物体３０による計測点をノイズとして除去することができる。

まず、Ｚ軸成分を示すグラフにおいて、近似直線を求める。当該近似直線は、例えばグラフにおいて任意の２点を抽出し、当該２点を通る直線の誤差範囲内に含まれる点が一番多い直線を、近似直線として求めることができる。図１５（ａ）には、Ｚ軸成分を示すグラフにおける任意の２点を結んだ直線を点線で示す。図１５（ｂ）には、Ｚ軸成分を示すグラフにおける近似直線を点線で示す。近似直線を求める際の誤差範囲は、計測点の測定誤差等を考慮して設定することができる。

当該近似直線から所定距離以上離れた点（図１５（ｂ）のＲ領域）を求め、これらの点を物体３０の上面を示す点（ノイズ）として除去することができる。なおここでの「距離」とは、グラフ紙面上での２次元距離である。

対象体３の上面と物体３０の上面とが異なる高さである場合には、対象体３の上面を示す部分と物体３０の上面を示す部分とでグラフの連続性が損われる。撮像範囲２０において支配的に撮像された対象体３を示す近似直線は、対象体３の上面を示す部分に対して所定距離以内に引かれることになる（図１５（ｂ）の点線）が、物体３０の上面を示す部分は、その近似直線から所定距離以上離れた位置の点となるため、除去されることになる（図１５（ｂ）のＲ領域）。

対象体３の上面と物体３０の上面との傾きが異なる場合には、対象体３の上面を示す部分と物体３０の上面を示す部分（略水平な上面であれば傾き略０）とでグラフの傾きが異なる。撮像範囲２０において支配的に撮像された対象体３を示す近似直線は、対象体３の上面を示す部分に対して所定距離以内に引かれることになる（図１５（ｂ）の点線）が、物体３０の上面を示す部分は、近似直線から所定距離以上離れた位置の点となるものが大部分であるため、除去されることになる（図１５（ｂ）のＲ領域）。

一方、図１４の場合について横軸にＸ軸成分、縦軸にＹ軸成分をとって示した場合、図１６のような分布となる。

当該分布から、Ｘ軸成分が欠落する領域（図１６においてはＸ＝６０〜７０）が存在していることが分かり、当該領域において、対象体３と物体３０とが空間的に離間していると判断することができる。このような場合には、物体３０を示す点を特定できるため、効果的にノイズを除去できる。Ｙ軸成分が同様に判断できる場合にも適用できる。

Ｘ軸成分、Ｙ軸成分を用いる方法を、Ｚ軸成分を用いる方法と組み合わせて用いることで、より精度よくノイズを除去することができる。

次に、取得手段８において、ノイズ除去手段１２において生成した計測点群と、記憶手段７にあらかじめ記憶されている対象体３のモデル点群とを取得する（ステップＳ２）。

モデル処理手段１０において、マッチング手段９におけるマッチング処理（ステップＳ３）の結果、適切にマッチングされなかったモデル点群の各モデル点を、削除する処理を行う（ステップＳ４）。

具体的には、欠落計測点群とモデル点群とのＩＣＰマッチング動作を開始し（図１７参照）、各計測点と対応する各モデル点との距離の総和が所定値以下となるか、あるいは先のＩＣＰマッチング処理における距離の総和との差が所定値以下になることで、ＩＣＰマッチング動作が終了する。そうすると、欠落計測点群とモデル点群とは、所定の間隔で接近した状態となる（図１８参照）。

ここで、モデル点群とマッチングされる欠落計測点群（図１８）は、ノイズ除去手段１２においてノイズを除去されているため、ノイズ除去していない欠落計測点群（図７）とマッチングする場合よりも、モデル点群と精度よくマッチングすることができる。

そして、モデル点群の各モデル点について、最も近い計測点群の点（最近傍点）までの距離を算出し、当該距離が閾値以上である点を、計測点群にマッチング対応点を持たないモデル点（非対応モデル点）とする。

当該非対応モデル点を、モデル点群から削除し、当該処理をモデル点全点について行い、得られたモデル点群を削除モデル点群とする（図１９参照）。

このように欠落計測点群において３次元位置情報の欠けた部分に合わせて、モデル点群における非対応モデル点を削除することにより、欠落計測点群において３次元位置情報を欠いた部分の影響を抑制し、適切にマッチングすることができる。本実施形態においては、あらかじめノイズを除去した欠落計測点群とモデル点群とをマッチングさせることにより非対応モデル点を特定しているので、第１実施形態における場合よりも、より精度よく非対応モデル点を特定できる。

次に、認識手段１１において、欠落計測点群と削除モデル点群とを用いたマッチング処理を行い（図２０参照）、対象体３の３次元位置・姿勢認識を行う（ステップＳ５）。

削除モデル点群を用いて欠落計測点群とのマッチング処理を行うことで、より適切なマッチングを行うことができる（図２１参照）。

＜効果＞
本発明にかかる実施形態によれば、３次元位置・姿勢認識装置において、対象体３の３次元計測によって得られた欠落計測点群からノイズを除去するノイズ除去手段１２をさらに備え、ノイズ除去された欠落計測点群を、削除モデル点群とマッチングさせることで、欠落計測点群におけるノイズの影響を抑制し、欠落計測点群と削除モデル点群とのマッチングの精度をさらに高めることができる。よって、対象体の３次元位置・姿勢認識を適切に行うことができる。

また、本発明にかかる実施形態によれば、３次元位置・姿勢認識装置において、ノイズ除去手段１２が、欠落計測点群の各点の、対象体３とカメラ４およびカメラ５とを通る軸成分の分布に基づいて、欠落計測点群からノイズを除去することで、対象体３とカメラ４およびカメラ５とを通る軸（Ｚ軸）成分の違いによって、対象体３とノイズ（物体３０）とを区別することができ、ノイズによる計測点を効果的に除去することができるため、欠落計測点群と削除モデル点群とのマッチングの精度をさらに高めることができる。よって、対象体３の３次元位置・姿勢認識を適切に行うことができる。

なお本発明は、その発明の範囲内において、各実施形態の自由な組み合わせ、あるいは各実施形態の任意の構成要素の変形、もしくは各実施形態において任意の構成要素の省略が可能である。

３ 対象体
４，５ カメラ
６ 生成手段
７ 記憶手段
８ 取得手段
９ マッチング手段
１０ モデル処理手段
１１ 認識手段
１２ ノイズ除去手段
２０ 撮像範囲
３０ 物体
５０，５１ 照明装置
１００，１０１ ３次元位置・姿勢認識装置
２００ ロボット
２０１ ロボット本体
２０２ アーム
２０３ ハンド
１０００，１００１ ３次元位置・姿勢認識システム

Claims (7)

1. 対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する取得手段と、
   前記計測点群と、対応する前記モデル点群とをマッチングさせるマッチング手段と、
   前記モデル点群のうち、前記マッチング手段におけるマッチング結果に基づいて特定される点を削除処理するモデル処理手段と、
   前記計測点群と、前記モデル処理手段において削除処理した前記モデル点群とをマッチングさせ、前記対象体の３次元位置・姿勢を認識する認識手段とを備えることを特徴とする、
   ３次元位置・姿勢認識装置。
2. 前記モデル処理手段が、前記マッチング手段におけるマッチングの結果、対応する計測点群の各点との間隔が所定距離以上である前記モデル点群の各点を削除処理することを特徴とする、
   請求項１に記載の３次元位置・姿勢認識装置。
3. 前記マッチング手段および前記認識手段における前記マッチングは、ＩＣＰマッチングであることを特徴とする、
   請求項１または２に記載の３次元位置・姿勢認識装置。
4. 前記対象体の３次元計測によって得られた前記計測点群からノイズを除去するノイズ除去手段をさらに備え、
   前記取得手段が、前記ノイズ除去手段においてノイズ除去された前記計測点群の各点の３次元位置情報と、前記モデル点群の各点の３次元位置情報とを取得することを特徴とする、
   請求項１〜３のいずれかに記載の３次元位置・姿勢認識装置。
5. 前記対象体の画像を撮像する撮像手段をさらに備え、
   前記ノイズ除去手段が、前記計測点群の各点の、前記対象体と前記撮像手段とを通る軸成分の分布に基づいて、前記計測点群からノイズを除去することを特徴とする、
   請求項４に記載の３次元位置・姿勢認識装置。
6. （ａ）対象体の３次元計測によって得られた計測点群の各点の３次元位置情報と、前記対象体の３次元モデル上に規定されたモデル点群の各点の３次元位置情報とを取得する工程と、
   （ｂ）前記計測点群と、対応する前記モデル点群とをマッチングさせる工程と、
   （ｃ）前記モデル点群のうち、マッチング結果に基づいて特定される点を削除処理する工程と、
   （ｄ）前記計測点群と、削除処理した前記モデル点群とをマッチングさせ、前記対象体の３次元位置・姿勢を認識する工程とを備えることを特徴とする、
   ３次元位置・姿勢認識方法。
7. コンピュータにインストールされて実行されることにより、前記コンピュータを、請求項１〜５のいずれかに記載の３次元位置・姿勢認識装置として機能させることを特徴とする、
   ３次元位置・姿勢認識プログラム。

Images