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