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