(57)【特許請求の範囲】
【請求項１】
距離を計測できるセンサによって計測したシーンデータと、目的物体の三次元形状を表すモデルデータとを、物体の局所的な表面形状を表す特徴量について照合することにより物体の三次元位置姿勢を検出する三次元物体認識方法において、
モデルを、候補となる視点からの位置姿勢に配置したとき、モデルによって遮蔽され、本来計測不可能な範囲にシーンデータが存在するか否かを算出し、この幾何学的な矛盾の有無に基づいて、モデルの位置姿勢候補の成否を検証することを特徴とする三次元物体認識方法。
【請求項２】
前記候補となる視点としての、三次元計測におけるセンサの視点及び視線方向が明らかな場合に、前記センサの視線方向に垂直な平面上に前記センサの視点からの距離データを保持したままシーンデータのシーン計測点及びモデルデータのモデル表面を投影するステップと、
前記平面に平行なＸ座標とＹ座標に基づいて、前記平面上に投影されたシーン計測点とモデル表面との重なりを評価するステップと、
前記平面上においてモデル表面と重なるという第１の条件、及び前記センサの視点から見てモデル表面の奥側に存在するという第２の条件を満たすシーン計測点を抽出するステップと、
前記第１及び第２の条件を満たすシーン計測点に基づいて、候補となる位置姿勢に存在するモデルによるシーンの遮蔽状態を判断し、モデルの位置姿勢候補の成否を評価するステップと、を含む請求項１に記載の三次元物体認識方法。
【請求項３】
前記モデルの位置姿勢候補の成否を評価するステップにおいて、モデルの奥側に存在すると判定されるシーン計測点の個数、又はそのようなシーン計測点がシーン計測点全体に占める割合に基づいて、位置姿勢候補の成否を評価する、請求項２に記載の三次元物体認識方法。
【請求項４】
前記モデルデータが物体の表裏に関する情報をさらに有し、
前記候補となる視点としての、三次元計測におけるセンサの視点が明らかでない場合に、仮の視点を設定し、かつ設定した仮の視点からの候補となる位置姿勢にモデルを配置するステップと、
前記仮の視点と第１のシーン計測点とを結ぶ直線を引くステップと、
（１）前記モデルデータから読み取り可能な物体の裏表情報を有するモデル表面であって、（１−１）前記直線上に存在し、（１−２）前記仮の視点と前記第１のシーン計測点との間に存在し、かつ（１−３）前記仮の視点側に表を向けている第１のモデル表面が存在し、かつ
（２）前記モデルデータから読み取り可能な物体の裏表情報を有するモデル表面であって、（２−１）前記直線上に存在し、（２−２）前記第１のシーン計測点よりも前記仮の視点から遠くに存在し、かつ（２−３）前記仮の視点側に裏を向けている第２の物体表面が存在する、シーン計測点の個数を抽出するステップと、
前記第１及び第２の条件を満たすシーン計測点に基づいて、候補となる位置姿勢に存在するモデルによるシーンの遮蔽状態を判断し、モデルの位置姿勢候補の成否を評価するステップと、を含む請求項１に記載の三次元物体認識方法。
【請求項５】
前記モデルの位置姿勢候補の成否を評価するステップにおいて、モデルの内部に存在すると判定されるシーン計測点の個数、又はそのようなシーン計測点がシーン計測点全体に占める割合に基づいて、位置姿勢候補の成否を評価する、請求項４に記載の三次元物体認識方法。
【請求項６】
前記特徴量がスピンイメージであることを特徴とする請求項１に記載の物体認識方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、認識すべき対象物体の三次元表面形状データと、距離センサ等を用いて得られる実際の環境をサンプリングした三次元表面形状データを照合することによって、物体の位置姿勢を認識する方法に関する。
【背景技術】
【０００２】
従来は、複数のビジョンセンサを用いて物体を撮影し、第１〜第ｎの画像（例えば、第１〜第３の画像）から物体の二次元特徴を抽出し、ステレオ計測原理に基づいて各画像間（例えば、第１画像と第２画像との間、第１画像と第３画像との間）で二次元特徴を対応付けし、対応付けによって得られる二次元特徴の三次元情報に基づいて、物体の三次元位置・姿勢を推定する。三次元位置姿勢の検証（信頼性評価）には、直線間の距離や平面間の距離、平面法線の一致度などを用いる（例えば、特許文献１参照）。
【特許文献１】特許第３５２５８９６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
前記従来方法においては、位置姿勢の信頼度評価値は、二次元画像処理のエッジ抽出精度に依存するため、高い精度は得られない、などの問題がある。
【課題を解決するための手段】
【０００４】
本発明は上述した問題点を解決するために創案されたものである。すなわち、位置姿勢の信頼度評価値が、二次元画像処理のエッジ抽出精度に依存しない物体認識の方法を提供する。
【０００５】
本発明の第１の特徴は、距離を計測できるセンサによって計測したシーンデータと、目的物体の三次元形状を表すモデルデータとを、物体の局所的な表面形状を表す特徴量について照合することにより物体の三次元位置姿勢を検出する三次元物体認識方法において、
モデルを、候補となる視点からの位置姿勢に配置したとき、モデルによって遮蔽され、本来計測不可能な範囲にシーンデータが存在するか否かを算出し、この幾何学的な矛盾の有無に基づいて、モデルの位置姿勢候補の成否を検証することにある。
「モデル」とは、認識すべき対象物体の三次元形状データを言う。三次元位置を持った点群と、点を結び物体の表面を表す三角メッシュから構成される。
「シーン」とは、実際の環境をサンプリングした三次元形状データを言う。距離センサによる計測データから生成される。モデルと同様に点群と三角メッシュから構成される。
【０００６】
本発明の第２の特徴は、第１の特徴を有し、さらに（１）前記候補となる視点としての、三次元計測におけるセンサの視点及び視線方向が明らかな場合に、前記センサの視線方向に垂直な平面上に前記センサの視線方向に垂直な平面上に視点からの距離データを保持したままシーンデータのシーン計測点及びモデルデータのモデル表面を投影するステップと、（２）前記平面に平行なＸ座標とＹ座標に基づいて、前記平面上に投影されたシーン計測点とモデル表面との重なりを評価するステップと、（３）前記平面上においてモデル表面と重なるという第１の条件、及び前記センサの視点から見てモデル表面の奥側に存在するという第２の条件を満たすシーン計測点を抽出するステップと、（４）前記第１及び第２の条件を満たすシーン計測点に基づいて、候補となる位置姿勢に存在するモデルによるシーンの遮蔽状態を判断し、モデルの位置姿勢候補の成否を評価するステップと、を含むことにある。
【０００７】
本発明の第３の特徴は、第２の特徴を有し、さらに前記モデルの位置姿勢候補の成否を評価するステップにおいて、モデルの奥側に存在すると判定されるシーン計測点の個数、又はそのようなシーン計測点がシーン計測点全体に占める割合に基づいて、位置姿勢候補の成否を評価する、ことにある。
【０００８】
本発明の第４の特徴は、第１の特徴を有し、さらに（１）前記モデルデータが物体の表裏に関する情報をさらに有し、（２）前記候補となる視点としての、三次元計測におけるセンサの視点が明らかでない場合に、仮の視点を設定し、かつ設定した仮の視点からの候補となる位置姿勢にモデルを配置するステップと、（３）前記仮の視点と第１のシーン計測点とを結ぶ直線を引くステップと、（４−１）前記モデルデータから読み取り可能な物体の裏表情報を有するモデル表面であって、（４−１−１）前記直線上に存在し、（４−１−２）前記仮の視点と前記第１のシーン計測点との間に存在し、かつ（４−１−３）前記仮の視点側に表を向けている第１のモデル表面が存在し、かつ（４−２）前記モデルデータから読み取り可能な物体の裏表情報を有するモデル表面であって、（４−２−１）前記直線上に存在し、（４−２−２）前記第１のシーン計測点よりも前記仮の視点から遠くに存在し、かつ（４−２−３）前記仮の視点側に裏を向けている第２の物体表面が存在する、シーン計測点の個数を抽出するステップと、（５）前記第１及び第２の条件を満たすシーン計測点に基づいて、候補となる位置姿勢に存在するモデルによるシーンの遮蔽状態を判断し、モデルの位置姿勢候補の成否を評価するステップと、を含むことにある。
【０００９】
本発明の第５の特徴は、前記モデルの位置姿勢候補の成否を評価するステップにおいて、モデルの内部に存在すると判定されるシーン計測点の個数、又はそのようなシーン計測点がシーン計測点全体に占める割合に基づいて、位置姿勢候補の成否を評価する、ことにある。
【００１０】
本発明の第６の特徴は、特徴量がスピンイメージであることにある。
「スピンイメージ」とは、三次元形状データの表面の任意の点で生成可能な、その点の周辺の三次元形状特徴を表す特徴量である。
【発明の効果】
【００１１】
本発明の物体認識装置によれば、スピンイメージなどの特徴量ベースの高速な位置決めを行うことで、対象物体の三次元形状データ（モデル）と、距離センサから得られる距離データ（シーン）を照合して、対象物体の三次元的な位置姿勢を迅速に認識することができる。
【００１２】
また、モデルをある位置姿勢に配置した場合のモデルデータと計測データ（シーンデータ）との幾何学的な矛盾に基づいて、モデルの位置姿勢候補の成否を高精度に検証することができる。
【００１３】
特徴量としてスピンイメージを用いた場合、スピンイメージは物体の局所的な三次元形状特徴を捉えた、回転不変な特徴量であるため、物体の一部隠れが発生している状況や、複数の物体が混在している状況においても物体を認識することができる。
【発明を実施するための最良の形態】
【００１４】
以下本発明の好ましい実施形態について、図面を参照して説明する。なお、各図において、共通する部分には同一の符号を付し、重複した説明を省略する。
【００１５】
図１は、本発明の物体認識装置の全体構成図である。図１に示すように、本発明の物体認識装置４０は、物体（ワーク）までの距離を計測する３Ｄセンサ４１、物体認識処理を実行する物体認識処理部４２、物体の三次元形状を表すデータ（例えば、ＣＡＤデータ）を記憶するモデルデータベース部４３を具備する。物体認識処理部４２において認識された物体の位置や姿勢を、ロボット制御部４５へ出力し、ロボット制御部４５からロボット４６へ制御信号を出力し、ハンド４７を動かして物体を把持したりすることができる。
本発明において、自己位置とは、計測位置を意味し、例えば、３Ｄセンサ４１の外界における６自由度の位置と姿勢を意味する。
図示しないが、本発明において、距離センサ（３Ｄセンサ）以外のオドメータ、カメラ、ＧＰＳ、姿勢センサを、必要に応じてオプションとして用いても良い。以下、距離センサを用いた例を説明する。
【００１６】
図２は、物体認識処理部の構成の一例を示す。この図に示すように、物体認識処理部４２は、内部記憶装置３４、中央処理装置３５を備える。物体認識処理部４２は、データ入力装置３２および外部記憶装置３３からデータを受け取り、ロボット制御部４５へデータを送る。
【００１７】
データ入力装置３２は、上述した距離センサを有し、三次元形状上の座標値を物体認識処理部４２に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置姿勢や移動距離も入力するのがよい。なお、データ入力装置３２は、キーボード等の通常の入力手段も有するのがよい。
外部記憶装置３３は、ハードディスク、フロッピー（登録商標）ディスク、磁気テープ、コンパクトディスク等である。外部記憶装置３３は、環境モデルのサイズが大きく後述する内部記憶装置３４に入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の全体を保持できない場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。また、外部記憶装置３３は、後述するＣＡＤモデルデータを記憶する。
内部記憶装置３４は、例えばＲＡＭ，ＲＯＭ等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。
中央処理装置３５（ＣＰＵ）は、モデル入力手段、シーン計測手段、対応点ペア作成手段、グループ化手段、一致度検証手段、高精度化手段として機能し、演算や入出力等を集中的に処理し、内部記憶装置３４と共に、プログラムを実行する。
ロボット制御部４５は、物体認識処理部４２から受け取る物体の位置や姿勢に基づいてロボットのアームやハンドの動きを制御する。
【００１８】
上述した本発明の装置は、上述した距離センサと通常のＰＣ（コンピュータ）を組み合わせたものでもよく、或いは、全体を一体にした装置であってもよい。また、自走可能な装置内に一体的に組み込んでもよい。
【００１９】
［１］スピンイメージ
まず、スピンイメージについて説明する。
【００２０】
［１．１］スピンイメージの定義
スピンイメージとは、三次元形状データの表面の任意の点で生成可能な、その点の周辺の三次元形状特徴を表す、回転不変な特徴量である。
【００２１】
図３に、ＣＡＤデータとスピンイメージの例を示す。三角メッシュで構成されるデータのスピンイメージを生成する際は、三角メッシュの頂点ごとに生成する。
モデルの頂点Ａのスピンイメージと、シーンの頂点Ｂのスピンイメージが似ているということは、その周辺の表面形状が似ていることを意味している。これは、モデルとシーンを照合する際、点Ａと点Ｂが対応する可能性があることを示している。
三角メッシュで構成されるデータの頂点Ｐのスピンイメージを作成する場合、まず、頂点Ｐの法線を中心軸とした円筒座標系を考える。円筒座標系では、法線までの距離をα、法線方向の変移をβとする。
【００２２】
図４に円筒座標系のイメージを示す。頂点Ｐの周辺の頂点をα、βの二次元平面（スピンマップと呼ぶ）にプロットすることでスピンイメージを作成する。その際、α、βをある分解能で量子化し、枠内に存在する頂点数を輝度に変換する。
【００２３】
［１．２］類似度算出
スピンイメージは回転不変なデジタル二次元画像として表されるので、正規化相関などの画像比較手段により、類似性を評価できる。
ただし、シーンのスピンイメージを作成する場合、物体の隠れにより本来値が存在する画素の値がゼロとなることや、乱雑な背景により本来値がゼロとなる画素に値が入力されてしまうことがある。したがって、２つのスピンイメージを比較する際、共通して値がゼロでない画素（以下、「重なりがある画素」と表現する。）を比較するよう制限する。
スピンイメージＰとＱの類似度Ｃ（Ｐ，Ｑ）を算出するために、まず、正規化相関係数Ｒ（Ｐ，Ｑ）を算出する。
【００２４】
数１に正規化相関係数Ｒ（Ｐ，Ｑ）の算出式を示す。ただし、この正規化相関において対象となるのは、重なりがある画素のみである。
【数１】
0004900204.tif 000002
Ｎ：スピンイメージＰ、Ｑで重なりがある画素の数
ｐｉ：スピンイメージＰのｉ番目の画素の値
ｑｉ：スピンイメージＱのｉ番目の画素の値
単純にこの正規化相関係数Ｒ（Ｐ，Ｑ）を類似度としてしまうと、重なりが少ないほど類似度が高くなってしまう。例えば、ＰとＱで重なりがある画素が１画素しかない場合、その画素の値が一致すれば、類似度は最高値となってしまう。そこで、重なりがある画素が多いほど類似度が高くなるよう、重み付けを行うことが妥当である。
【００２５】
図５は、本発明の実施形態に係る三次元物体認識の処理フローである。
（１）モデル入力：ステップＳ１０
認識対象物体のモデルを読み込み、モデルのスピンイメージを作成するなどの事前準備を実施する。本処理は、計測のたびに実施する必要はない。例えば対象物体が決まっている場合は、システム起動時に一回だけモデル入力を行うことになる。
【００２６】
（２）シーン計測：ステップＳ２０
実際の環境をセンサで計測して距離データを取得し、三角メッシュ作成処理や法線算出処理を行い、シーンを作成する。
【００２７】
（３）対応点ペア作成：ステップＳ３０
モデルとシーンのスピンイメージの類似度から、モデル頂点とシーン計測点のペアである対応点ペアを複数作成する。
【００２８】
（４）グループ化：ステップＳ４０
複数ある対応点ペアの中から、同時に成立することが可能な対応点ペアをまとめ、一つのグループとする。全ての可能なグループを作成する。
【００２９】
（５）一致度の検証：ステップＳ５０
グループごとに座標変換を行ったとき、モデルとシーンの表面がどの程度一致するかを評価して、グループの検証を行う。
【００３０】
（６）高精度化：ステップＳ６０
検証の結果得られたモデルの位置姿勢候補の成否を検証する。モデルをある位置姿勢に配置したときの、モデルデータとシーンデータとの幾何学的な矛盾に基づいて、位置姿勢候補の成否を検証する。（１）ある場所に距離センサを配置して、１回計測して得られたデータをシーンデータとする場合、（２）複数視点から計測して得られたデータを統合してシーンデータとする場合に分けて、詳細は後述する。
【００３１】
一連の物体認識処理（オンライン処理）を実施した後、得られた物体の位置姿勢を外部機器に通知する。例えば、ロボットアームを対象物体の把持位置に移動させる、などの処理を行う。
【００３２】
以下、図５の処理フローの各ステップの詳細を説明する。
［モデル入力］
図６に本発明の実施形態に係るモデル入力処理フローを示す。
【００３３】
（１）三次元形状データ読込：ステップＳ１１
対象物体の三次元形状データを入力する。例えば、三次元形状データ表現形式の一つであるＰＬＹ形式のファイルを読み込む。ＰＬＹファイルは、頂点の三次元位置と、頂点間を結び物体の表面を表す三角メッシュ情報から成る。
なお、高精度化（ステップＳ６０）において、複数視点から計測して得られたデータを統合してシーンデータとする場合は、後述するように、対象物体の三次元形状データは、表面形状及び表裏が分かるデータとする。
図７に本発明の実施形態に係るＰＬＹ形式のファイルの例を示す。
図８に本発明の実施形態に係るＰＬＹファイルをＣＡＤソフトで表示した例を示す。
【００３４】
（２）法線ベクトル算出：ステップＳ１２
スピンイメージ作成時の円筒座標設定に各頂点の法線情報が必要となるため、ここで作成する。頂点の法線は、その頂点を含む三角メッシュの法線の平均を単位ベクトル化したものである。
図９に本発明の実施形態に係る法線算出手順を示す。
【００３５】
（３）モデルの全頂点のスピンイメージ作成：ステップＳ１３
頂点ごとに円筒座標を設定してその周辺の頂点をスピンマップにプロットすることで、各頂点のスピンイメージを作成する。スピンイメージ生成手順の詳細は後述する。
また、各スピンイメージごとに、値がゼロで無い画素数を記憶する。その中央値の１／２の値がスピンイメージ同士の類似度算出の際に用いられるパラメータλとなる。
【００３６】
なお、一度算出した法線ベクトル、スピンイメージ、およびパラメータλは、ファイルとして記録し、必要に応じて読み込むことができる。よって、一度スピンイメージを作成したモデルを再度読み込む場合は、（２）法線ベクトル作成、（３）モデルの全頂点のスピンイメージ作成処理を省略できる。
【００３７】
［シーン計測］
図１０に本発明の実施形態に係るシーン計測処理フローを示す。以下、各ステップについて説明する。
【００３８】
（１）距離データ取得：ステップＳ２１
距離センサを用いて、０個〜ｎ個の対象物体やそれ以外の物体が存在する実際の環境を計測し、三次元点群データを得る。使用する距離センサは、可能な限り密に等間隔で距離データが得られるものが望ましい。
【００３９】
（２）メッシュ生成：ステップＳ２２
計測した点群から、物体の表面を表す三角メッシュを生成する。三角メッシュの作成方法として、（ａ）正方格子分割し対角線を結ぶ方法や、（ｂ）ドロネー三角形分割法がある。（ａ）正方格子分割は、点群を平面正方格子に投影し、格子ごとに代表点を決定し、隣り合う代表点を決まった形で結んで三角形を形成する手法である。（ｂ）ドロネー三角形分割は、点群を三角形分割したとき、最小角度が最大になるよう分割する方法である。以下ではより精密な三角メッシュが得られる（ｂ）ドロネー三角形分割法を用いる。
計測データを平面に投影し、平面上の点をドロネー三角形分割し、点群を三次元空間に戻したときに辺の長さがしきい値より大きい三角形を除去する、という手順でメッシュ生成を行う。
【００４０】
（３）法線ベクトル算出：ステップＳ２３
スピンイメージ作成時の円筒座標設定条件として、各頂点の法線情報が必要となるため、ここで作成する。
【００４１】
［対応点ペア作成］
対応点ペア作成処理では、モデルとシーンの対応点ペアを複数作成する。
図１１に本発明の実施形態に係る対応点ペア作成処理フローを示す。以下、各ステップについて説明する。
【００４２】
（１）シーン頂点（又はシーン計測点）を一つ選びスピンイメージを作成：ステップＳ３１
シーンの頂点を一つ選択し、その頂点に関するスピンイメージを作成する。シーンの頂点を一つ選択する際、ランダムピックアップを行っているが、他に、これまで選んだ頂点との距離の最小値が最大になる位置の頂点を選ぶ方法や、法線の精度が良い（周辺の頂点が多く、三角メッシュの辺が集中している）頂点を選ぶ方法などが考えられる。
【００４３】
（２）シーン頂点とモデル各頂点の類似度算出：ステップＳ３２
作成したシーンの頂点と、モデルの各頂点のペアで、スピンイメージとの類似度を数１を用いて算出する。
例えば、モデルの頂点数が１０００個の場合、１つのシーン頂点と１０００個のモデル頂点の各ペアでそれぞれ類似度を算出し、１０００個の類似度が算出される。
【００４４】
（３）突出して類似しているペアを対応点ペアに追加：ステップＳ３３
前述した例で言うところの１０００個のペアの中に、他と比較して特に類似度が高いペアがあれば、選択したシーン頂点とモデル頂点で対応点ペアＣｉ（Ｓｉ，Ｍｉ）を作成する。Ｓｉはシーン頂点を、Ｍｉはモデル頂点を示す。
【００４５】
ステップＳ３１〜３３の処理により、シーンのある１つの頂点を対応点とする対応点ペアが作成される。この処理を、シーン頂点数のある割合（パラメータαとして設定される）だけ繰り返す。例えば、パラメータαとして２０％が設定されているとき、シーン頂点数が１０００点あれば、ステップＳ３１〜３３の処理が２００回繰り返される。
この結果、シーン頂点とモデル頂点を結びつける対応点ペアＣｉ（Ｓｉ，Ｍｉ）が複数生成される。
図１２に本発明の実施形態に係るモデルとシーンの対応点ペアのイメージを示す。同図は、シーンとして動物の玩具を並べた環境を計測し、モデルとしてブタを使用して対応点ペアを検出した結果である。
【００４６】
［グループ化］
グループ化処理では、同時に成立することが可能な対応点ペアを一つのグループとする処理を繰り返し、複数のグループを作成する。
図１３に本発明の実施形態に係るグループ化処理フローを示す。以下、各ステップについて説明する。
【００４７】
（１）対応点ペアＣｉのみを含むグループＧｉを作成：ステップＳ４１
最初に、シーン頂点Ｓｉとモデル頂点Ｍｉを結びつける対応点ペアＣｉ（Ｓｉ，Ｍｉ）を、グループＧｉに追加する。この時点では、グループには基準となる一つの対応点ペアしか含まれていない。つまり、Ｇｉ＝｛Ｃｉ｝。
【００４８】
（２）Ｇｉに最も追加しやすい対応点ペアＣｊを決定：ステップＳ４２
Ｇｉ内に含まれている対応点ペアと同時に成り立つことができる新たな対応点ペアＣｊを決定する。同時に成り立つことができるかどうかの基準として、幾何学的一貫性を用いる。幾何学的一貫性の概念について説明する。対応点ペアＣ１（Ｓ１，Ｍ１）とＣ２（Ｓ２，Ｍ２）が同時に成り立つ場合、Ｓ１からＳ２までの距離と、Ｍ１からＭ２までの距離は同じで、かつＳ１法線とＳ２法線とベクトルＳ１→Ｓ２が成す３つの角の角度と、Ｍ１法線とＭ２法線とベクトルＭ１→Ｍ２が成す３つの角の角度は等しくなるはずである。この一致度合いから、対応点ペアＣ１とＣ２が幾何学的一貫性を持っているかどうかを判定することができる。
図１４に本発明の実施形態に係る幾何学的一貫性の概念図を示す。
例えば、Ｇ１＝｛Ｃ１，Ｃ４，Ｃ８｝に対してＣ９を追加できるか評価する際、Ｃ１−Ｃ９，Ｃ４−Ｃ９，Ｃ８−Ｃ９の全てにおいて幾何学的一貫性が成り立てば、Ｇ１にＣ９を追加できると言える。
【００４９】
（３）ＣｊをＧｉに追加する：ステップＳ４３
ＣｊをＧｉに追加してもＧｉが幾何学的一貫性を維持可能な場合、ＣｊをＧｉに追加する。ＣｊをＧｉに追加するとＧｉが幾何学的一貫性を維持不可能な場合、Ｇｉに対する追加処理は終了する。
【００５０】
（４）含まれている対応点ペアが多い順にグループを並び替え：ステップＳ４４
全ての対応点ペアを基準にグループを作成した後、グループ内の対応点ペアが多い順にグループを並び替える。
図１５（Ａ）及び（Ｂ）に本発明の実施形態に係るグループ化された対応点ペアの例を示す。
【００５１】
［一致度の検証］
一致度の検証処理では、モデルとシーンの表面がどの程度一致するかを評価して、各グループが物体の位置姿勢候補として適切かどうかを検証する。
図１６に本発明の実施形態に係る一致度の検証処理フローを示す。以下、各ステップについて説明する。
【００５２】
（１）シーン頂点のＫＤツリー作成：ステップＳ５１
後の対応点ペア拡張処理の高速化のために、ここでシーン頂点のＫＤツリーを作成しておく。仮に入力データ点数がＮ点、参照データ点数がＭ点のときに、近傍点探索処理量が全探索でＯ（ＭＮ）であるのに対して、ＫＤツリーを作成しておくことでＯ（ＮｌｏｇＭ）に削減できる。
【００５３】
（２）グループＧｉの初期位置姿勢算出：ステップＳ５２
グループＧｉに含まれる対応点ペアに基づき、モデルとシーンの対応点間の距離を最小にするような座標変換式Ｔｉを算出する。Ｔｉは３×３の回転行列Ｒと３×１の並進ベクトルｔを含む、４×４の剛体変換式である。この変換式にしたがってモデルを移動させた結果を、グループＧｉが示す物体の初期位置姿勢とする。
【００５４】
（３）対応点ペアの拡張：ステップＳ５３
グループＧｉに含まれるモデル頂点に隣接するモデル頂点に対して、最近傍のシーン頂点をＫＤツリー探索によって見つけ、そのモデル頂点とシーン頂点の間の６次元距離（法線方向のずれも考慮に入れた距離）がしきい値以下ならば、その対応点ペアをグループＧｉに追加する。この処理を、対応点ペアが追加できなくなるまで繰り返す。
図１７図に本発明の実施形態に係る対応点ペア拡張処理の例を示す。同図（Ａ）は拡張前を示し、同図（Ｂ）は拡張後を示す。
【００５５】
（４）Ｇｉを位置姿勢候補Ｇ’に登録：ステップＳ５４
初期位置姿勢が正しく、モデルとシーンの表面が一致していれば、対応点ペアの拡張処理によりグループ内の対応点ペアの数は大幅に増加するはずである。グループ内の対応点ペアの数がしきい値（モデルの頂点数のβ％）より多ければ、このグループは対象物を捉えている可能性が高いとして、位置姿勢候補に残す。そうでなければ、このグループを除外する。
【００５６】
このパラメータβは、運用条件に応じて設定すべきであるが、一般的な運用であれば、対応点ペア数がモデル頂点の１０％以上であればそのグループが対象物体を捉えている可能性が高い。
グループＧｉの対応点ペアの数がしきい値より多ければ、Ｇｉを位置姿勢候補Ｇ’に追加する。その際、Ｇｉに含まれているシーン頂点は、以後の対応点ペア拡張処理に使えないようにする。これにより、位置姿勢候補が重なり合って出現することを防いでいる。
【００５７】
（５）Ｇ’を対応点ペア数が多い順に並び替え：ステップＳ５５
全てのグループで一致度の検証処理を行い、位置姿勢候補Ｇ’を作成した後、対応点ペアが多い順にＧ’を並び替える。
【００５８】
［高精度化］
モデルをある位置姿勢に配置したときの、モデルデータとシーンデータとの幾何学的な矛盾に基づいて、位置姿勢候補の成否を検証する。
【００５９】
［三次元計測の視点が明らかな場合］
ある場所に距離センサを配置して、１回計測して得られたデータをシーンデータとする場合、視点を一意に特定できるので、以下のような処理によってモデルの位置姿勢候補の成否を検証することができる。
図１８に、三次元計測の視点及び視線方向が明らかな場合（ある場所に距離センサを配置して、１回計測して得られたデータをシーンデータとする場合）の高精度化処理フローを示す。以下、各ステップについて説明する。
【００６０】
（１）視線方向に垂直な平面を仮定：ステップＳ６１
図１９（ａ）〜（ｄ）に、高精度化処理の具体例を示す。まず、同図（ａ）に示すように、視線方向に垂直な平面６０を仮定する。
【００６１】
（２）計測点を平面に投影：ステップＳ６２
図１９（ｂ）に示すように、物体６１の各計測点（シーン計測点）６２〜７４を平面６０に投影する。ただし、投影後も各点は視点からの距離データ（奥行きデータ）を保持している。
【００６２】
（３）検証すべき位置姿勢にある三次元モデルを平面に投影：ステップＳ６３
図１９（ｃ）に、検証すべき位置姿勢にある三次元モデル９１及び視線方向に垂直な平面９０を示す。三次元モデル９１の各頂点を、平面９０に投影する。シーン計測点の場合と同様に、投影後も各点は視点からの距離データ（奥行きデータ）を保持している。
【００６３】
（４）モデル表面とシーン計測点の重なり評価：ステップＳ６４
モデル表面とシーン計測点６２〜７４との重なりを評価する。
具体的には、平面上ではモデル表面と重なるが、奥行き的にはモデルの奥側に存在するシーン計測点を検出する。
図１９（ｄ）の例では、シーン計測点７１，７２，７３はモデル表面９２と重なり、シーン計測点７４はモデル表面９３と重なる。モデル表面９３と重なる計測点７４は、本来は見えないモデル表面に存在する点である。このような計測点７４は、その位置にモデルが本当に存在すれば、見えるはずの無い点である。
なお、モデル表面は、本来はシーンと同程度の粒度の三角メッシュである。しかし、理解が容易になるように、上記の例では直方体の各表面＝モデル表面とした。
【００６４】
（５）モデルの位置姿勢の成否判定：ステップＳ６５
モデル頂点と重なるが、本来は見えるはずが無いと評価された計測点の個数をカウントする。そして、そのような計測点の個数が所定の閾値を上回った場合、又はそのような計測点が計測点全体に占める割合が所定の閾値を上回った場合、モデル位置姿勢が間違っていると判定する。
【００６５】
［三次元計測の視点が特定できない場合］
複数視点から計測して得られたデータを統合してシーンデータとする場合、視点を一意に特定できないので、前述の方法は使用できない。
このような場合、ソリッドモデル（中身が詰まった三次元形状モデル）を用いて計測点とモデルとの衝突（幾何学的な矛盾）を判定することができる。
また、計算量軽減のため、サーフェスモデル（表面形状データ。本実施形態においては三角パッチで表現され、表裏が判断可能なデータとする）を用いて計測点とモデルの衝突を判定することができる。
図２０に、三次元計測の視点が特定できない場合（複数視点から計測して得られたデータを統合してシーンデータとする場合）の高精度化処理フローを示す。以下、各ステップについて説明する。
【００６６】
（１）視点を仮定：ステップＳ７１
図２１に示すように、まず擬似的に視点８１を仮定する。
【００６７】
（２）検証すべき位置姿勢にモデルを配置：ステップＳ７２
図２１に示すように、検証すべき位置姿勢に三次元モデル８２を配置する。
【００６８】
（３）モデルと計測点の重なり評価：ステップＳ７３
ある計測点（例えば、計測点８３）に注目し、視点８１と計測点８３を結ぶ直線８４を引く。この直線８４上に、計測点８３の手前と奥にモデルのサーフェス８５，８６が存在する。そして、（１）計測点８３の手前のサーフェス（計測点８３よりも視点８１に近いサーフェス）８５は、視点８１側に表を向け、かつ（２）計測点８３の奥のサーフェス（計測点８３よりも視点８１から遠いサーフェス）８６は、視点８１側に裏を向けている場合、計測点８３はモデル８２の内部に存在すると評価する。
【００６９】
（４）モデルの位置姿勢の成否判定：ステップＳ７４
モデルの内部に存在すると評価された計測点の個数をカウントする。そして、そのような計測点の個数が所定の閾値を上回ったら、又はそのような計測点が計測点全体に占める割合が所定の閾値を上回ったら、モデル位置姿勢が間違っていると判定する。
【００７０】
全ての位置姿勢候補に対して前述のような成否判定を行い、物体の最終的な位置姿勢を得る。対象物体がシーン上に存在しない場合や、複数存在することがあるため、最終的に１つも位置姿勢が得られないことや、２つ以上の位置姿勢が得られることがある。
【００７１】
前記の如く、スピンイメージを用いた特徴量ベースの高速な位置決めを行った後、高精度化（位置姿勢候補の成否判定）を行うことで、対象物体の三次元形状データ（モデル）と、距離センサから得られる距離データ（シーン）を照合して、対象物体の三次元的な位置姿勢を認識することができる。
【００７２】
スピンイメージは、物体の局所的な三次元形状特徴を捉えた、回転不変な特徴量であるため、物体の一部隠れが発生している状況や、複数の物体が混在している状況における物体認識に適用できる。
【００７３】
ただし、特徴量ベースの位置決めは高速だが、モデルとシーンの対応箇所が比較的少ない状態で位置決めを行うため、精度が粗い可能性がある。そこで、特徴量ベースの位置決め結果を初期位置として、高精度化による精密な位置決めを行うことにより、高速・高精度な位置姿勢検出が可能となる。
【００７４】
３Ｄ−ＣＡＤデータをモデルデータとして導入するメリットは、以下のとおりである。（１）実物による教示の手間を削減できる、（２）工業部品の設計では３Ｄ−ＣＡＤの導入が進んでおり、認識したい物体については既に３Ｄ−ＣＡＤデータが存在することが多いので、既存データを活用することで作業量を削減できる。
【００７５】
前記の物体認識手法の大まかな性能として、モデル頂点数1000点程度、シーン頂点数5000点程度のとき、モデルがある程度（モデルの全表面の15%程度以上）出現していれば、3秒程度（CPU Celeron（登録商標）2.8GHz、メモリ512MB）で物体の位置姿勢を認識できた。
【００７６】
実施形態として、三次元形状の計測について説明したが、二次元形状を三次元形状の特別な場合として見ることにより、二次元形状も同様に計測できる。
【００７７】
なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。
【図面の簡単な説明】
【００７８】
【図１】本発明の実施形態に係る物体認識装置の全体構成図である。
【図２】本発明の実施形態に係る物体認識処理部の構成の一例を示す図である。
【図３】本発明の実施形態に係るＣＡＤデータとスピンイメージの例を示す図である。
【図４】本発明の実施形態に係る円筒座標系のイメージを示す図である。
【図５】本発明の実施形態に係る三次元物体認識の処理フローを示す図である。
【図６】本発明の実施形態に係るモデル入力処理フローを示す図である。
【図７】本発明の実施形態に係るＰＬＹ形式のファイルの例を示す図である。
【図８】本発明の実施形態に係るＰＬＹファイルをＣＡＤソフトで表示した例を示す図である。
【図９】本発明の実施形態に係る法線算出手順を示す図である。
【図１０】本発明の実施形態に係るシーン計測処理フローを示す図である。
【図１１】本発明の実施形態に係る対応点ペア作成処理フローを示す図である。
【図１２】本発明の実施形態に係るモデルとシーンの対応点ペアのイメージを示す図である。
【図１３】本発明の実施形態に係るグループ化処理フローを示す図である。
【図１４】本発明の実施形態に係る幾何学的一貫性の概念図である。
【図１５】（Ａ）及び（Ｂ）は本発明の実施形態に係るグループ化された対応点ペアの例を示す図である。
【図１６】本発明の実施形態に係る一致度の検証処理フローを示す図である。
【図１７】本発明の実施形態に係る対応点ペア拡張処理の例を示す図であって、(Ａ)は拡張前を示し、（Ｂ）は拡張後を示す。
【図１８】本発明の実施形態に係る三次元計測の視点及び視線方向が明らかな場合の三次元計測の視点が明らかな場合の高精度化処理フローを示す図である。
【図１９】図１８に示す高精度化処理の具体的内容を説明するための図である。
【図２０】本発明の実施形態に係る三次元計測の視点が特定できない場合の高精度化処理フローを示す図である。
【図２１】図２０に示す高精度化処理の具体的内容を説明するための図である。
【符号の説明】
【００７９】
３２  データ入力装置
３３  外部記憶装置
３４  内部記憶装置
３５  中央処理装置
４０  物体認識装置
４１  ３Ｄセンサ
４２  物体認識処理部
４３  モデルデータベース部
４５  ロボット制御部
４６  ロボット
【図１】
0004900204.tif 000003
【図２】
0004900204.tif 000004
【図３】
0004900204.tif 000005
【図４】
0004900204.tif 000006
【図５】
0004900204.tif 000007
【図６】
0004900204.tif 000008
【図７】
0004900204.tif 000009
【図８】
0004900204.tif 000010
【図９】
0004900204.tif 000011
【図１０】
0004900204.tif 000012
【図１１】
0004900204.tif 000013
【図１２】
0004900204.tif 000014
【図１３】
0004900204.tif 000015
【図１４】
0004900204.tif 000016
【図１５】
0004900204.tif 000017
【図１６】
0004900204.tif 000018
【図１７】
0004900204.tif 000019
【図１８】
0004900204.tif 000020
【図１９】
0004900204.tif 000021
【図２０】
0004900204.tif 000022
【図２１】
0004900204.tif 000023