特開 2024-52204 コンピュータプログラム、および、処理装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024052204

(43)【公開日】2024-04-11

(54)【発明の名称】コンピュータプログラム、および、処理装置

(51)【国際特許分類】

   G06T 7/00 20170101AFI20240404BHJP

   G01B 11/24 20060101ALI20240404BHJP

【ＦＩ】

G06T7/00 C

G01B11/24 A

G06T7/00 300F

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022158759

(22)【出願日】2022-09-30

(71)【出願人】

【識別番号】000005267

【氏名又は名称】ブラザー工業株式会社

(74)【代理人】

【識別番号】110001058

【氏名又は名称】鳳国際弁理士法人

(72)【発明者】

【氏名】平林 裕貴

【テーマコード（参考）】

2F065

5L096

【Ｆターム（参考）】

2F065AA04

2F065AA37

2F065AA53

2F065BB05

2F065FF04

2F065FF05

2F065FF09

2F065FF11

2F065GG04

2F065HH04

2F065HH07

2F065JJ01

2F065JJ03

2F065JJ05

2F065LL62

2F065PP22

2F065PP25

2F065QQ03

2F065QQ21

2F065QQ24

2F065QQ28

2F065QQ31

2F065RR09

2F065UU05

5L096AA09

5L096BA05

5L096CA05

5L096FA26

5L096FA60

5L096FA66

5L096FA67

5L096JA11

5L096MA07

(57)【要約】

【課題】点群データを処理する新たな技術を開示する。
【解決手段】
処理装置は、対象物体を示す第１点群データと、１以上の物体を示す第２点群データと、を取得し、第１点群の複数個の第１対象点のそれぞれと、第２点群の複数個の第２対象点のそれぞれと、の局所的特徴量を取得する。処理装置は、局所的特徴量に基づいて第１対象点と第２対象点とを類似グループに分類し、第１点セットと、第１点セットに対応する複数個の第２点セットと、を決定する。第１点セットは第１対象点から選択されるｎ個の点から構成され、第２点セットは第２対象点から選択されるｎ個の点から構成され、１つの第２点セットを構成するｎ個の点は、第１点セットを構成するｎ個の点のうちの対応する点と同一の類似グループに属する。処理装置は、複数個の第２点セットの中から第１点セットが有する大局的特徴量と同一または類似する大局的特徴を有する特定の点セットを探索する。
【選択図】 図２

【特許請求の範囲】

【請求項1】

コンピュータプログラムであって、
第１点群データと、第２点群データと、を取得する点群取得機能であって、前記第１点群データは、対象物体を示す複数個の点を含む第１点群の三次元座標を示し、前記第２点群データは、前記対象物体を含む１以上の物体を示す複数個の点を含む第２点群の三次元座標を示す、前記点群取得機能と、
前記第１点群の少なくとも一部である複数個の第１対象点のそれぞれと、前記第２点群の少なくとも一部である複数個の第２対象点のそれぞれと、の局所的特徴量を取得する局所的特徴量取得機能であって、前記局所的特徴量は、１つの対象点の特徴を示す量であり、１つの対象点を含む点群のうちの複数個の点に基づいて算出される量である、前記局所的特徴量取得機能と、
前記複数個の第１対象点と前記複数個の第２対象点とを複数個の類似グループに分類する分類機能であって、前記分類は、各対象点の局所的特徴量に基づいて行われ、１つの前記類似グループに属する１以上の点は、互いに類似する前記局所的特徴量を有する、前記分類機能と、
第１点セットと、前記第１点セットに対応する複数個の第２点セットと、を決定するセット決定機能であって、前記第１点セットは前記複数個の第１対象点の中から選択されるｎ個（ｎは２以上の整数）の点から構成され、前記複数個の第２点セットのそれぞれは前記複数個の第２対象点の中から選択されるｎ個の点から構成され、１つの前記第２点セットを構成するｎ個の点のそれぞれは、前記第１点セットを構成するｎ個の点のうちの対応する点と同一の前記類似グループに属する、前記セット決定機能と、
前記第１点セットと、前記複数個の第２点セットのそれぞれと、の大局的特徴量を取得する大局的特徴量取得機能であって、前記大局的特徴量は、１つの点セットの特徴を示す量であり、１つの点セットを構成するｎ個の点を含む点群のうちの複数個の点に基づいて算出される量である、前記大局的特徴量取得機能と、
前記複数個の第２点セットの中から、前記第１点セットが有する前記大局的特徴量と同一または類似する前記大局的特徴を有する特定の点セットを探索する探索機能と、
前記探索機能による探索の結果を用いて、特定処理を実行する特定処理機能と、
をコンピュータに実現させる、コンピュータプログラム。

【請求項2】

請求項１に記載のコンピュータプログラムであって、
前記分類機能は、前記複数個の第１対象点と前記複数個の第２対象点とをｍ個（ｍはｎ以上の整数）の前記類似グループに分類し、
前記セット決定機能は、前記第１点セットを構成するｎ個の点をｍ個の前記類似グループのうちのｎ個のグループから１つずつ選択することによって、前記第２点セットを決定する、コンピュータプログラム。

【請求項3】

請求項１に記載のコンピュータプログラムであって、
前記セット決定機能は、
前記複数個の類似グループから選択されるｎ個のグループから成る第１グループセットから１つずつ選択されたｎ個の前記第１対象点から成る第１種の前記第１点セットと、前記複数個の類似グループから選択されるｎ個のグループから成る第２グループセットであって前記第１グループセットとは異なる前記第２グループセットから１つずつ選択されたｎ個の前記第１対象点から成る第２種の前記第１点セットと、を決定し、
前記セット決定機能は、
前記第１種の前記第１点セットに対応する前記第２点セットとして、前記第１グループセットから１つずつ選択されたｎ個の前記第２対象点であって、前記第１種の前記第１点セットを構成するｎ個の前記第１対象点の間の距離に基づく第１距離条件を満たすｎ個の前記第２対象点から成るセットを決定し、
前記第２種の前記第１点セットに対応する前記第２点セットとして、前記第２グループセットから１つずつ選択されたｎ個の前記第２対象点であって、前記第２種の前記第２点セットを構成するｎ個の前記第１対象点の距離に基づく第２距離条件を満たすｎ個の前記第２対象点から成るセットを決定し、
前記第１距離条件と前記第２距離条件とは、互いに異なる条件である、コンピュータプログラム。

【請求項4】

請求項１に記載のコンピュータプログラムであって、
前記大局的特徴量は、点セットを構成するｎ個の点の間の距離と、点セットを構成するｎ個の点の法線ベクトルに基づく量と、を含み、
前記セット決定機能は、
前記第１点セットを構成するｎ個の前記第１対象点の間の距離に基づいて、距離条件を決定し、
前記複数個の第２対象点から選択されるｎ個の点のセットのうち、前記距離条件を満たすセットを前記第２点セットとして決定し、
前記大局的特徴量取得機能は、前記セット決定機能によって決定された前記第２点セットについて、前記法線ベクトルに基づく量を算出する、コンピュータプログラム。

【請求項5】

請求項１に記載のコンピュータプログラムであって、
前記複数個の類似グループの代表点の間の前記局所的特徴量の差異は、基準値以上である、コンピュータプログラム。

【請求項6】

請求項１に記載のコンピュータプログラムであって、
前記特定処理機能は、探索される前記特定の点セットに基づいて、前記第２点群によって示される前記対象物体の位置と姿勢とを特定する物体特定処理を実行し、
Ｊ個（Ｊは３以上の整数）の前記第１点セットと前記特定の点セットとの組み合わせが探索される場合に、前記特定処理機能は、
Ｊ個の前記組み合わせをＪ個未満のグループに分類し、
前記グループごとに前記組み合わせの信頼性を評価し、
前記信頼性の評価結果に基づいてＪ個未満の前記グループの中から選択される一部のグループに属する前記組み合わせを用いて、前記第２点群によって示される前記対象物体の位置と姿勢とを特定する、コンピュータプログラム。

【請求項7】

請求項６に記載のコンピュータプログラムであって、
前記特定処理機能は、Ｊ個の前記組み合わせのそれぞれに基づいて、前記第１点群の座標系の複数個の点を前記第２点群の座標系に投影し、
前記第２点群の座標系に投影された複数個の投影点を用いて、Ｊ個の前記特定の点セットをＪ個未満のグループに分類し、
前記複数個の投影点を用いて、前記グループごとに前記特定の点セットの信頼性を評価する、コンピュータプログラム。

【請求項8】

請求項７に記載のコンピュータプログラムであって、
前記複数個の投影点は、前記対象物体の重心を含む３個の点を含み、
前記特定処理機能は、
前記重心に基づいて前記第２点群の座標系における前記対象物体の位置を特定し、
前記重心を含む３個の点に基づいて前記対象物体の姿勢を特定する、コンピュータプログラム。

【請求項9】

処理装置であって、
第１点群データと、第２点群データと、を取得する点群取得部であって、前記第１点群データは、対象物体を示す複数個の点を含む第１点群の三次元座標を示し、前記第２点群データは、前記対象物体を含む１以上の物体を示す複数個の点を含む第２点群の三次元座標を示す、前記点群取得部と、
前記第１点群の少なくとも一部である複数個の第１対象点のそれぞれと、前記第２点群の少なくとも一部である複数個の第２対象点のそれぞれと、の局所的特徴量を取得する局所的特徴量取得部であって、前記局所的特徴量は、１つの対象点の特徴を示す量であり、１つの対象点を含む点群のうちの複数個の点に基づいて算出される量である、前記局所的特徴量取得部と、
前記複数個の第１対象点と前記複数個の第２対象点とを複数個の類似グループに分類する分類部であって、前記分類は、各対象点の局所的特徴量に基づいて行われ、１つの前記類似グループに属する１以上の点は、互いに類似する前記局所的特徴量を有する、前記分類部と、
第１点セットと、前記第１点セットに対応する複数個の第２点セットと、を決定するセット決定部であって、前記第１点セットは前記複数個の第１対象点の中から選択されるｎ個（ｎは２以上の整数）の点から構成され、前記複数個の第２点セットのそれぞれは前記複数個の第２対象点の中から選択されるｎ個の点から構成され、１つの前記第２点セットを構成するｎ個の点のそれぞれは、前記第１点セットを構成するｎ個の点のうちの対応する点と同一の前記類似グループに属する、前記セット決定部と、
前記第１点セットと、前記複数個の第２点セットのそれぞれと、の大局的特徴量を取得する大局的特徴量取得部であって、前記大局的特徴量は、１つの点セットの特徴を示す量であり、１つの点セットを構成するｎ個の点を含む点群のうちの複数個の点に基づいて算出される量である、前記大局的特徴量取得部と、
前記複数個の第２点セットの中から、前記第１点セットが有する前記大局的特徴量と同一または類似する前記大局的特徴を有する特定の点セットを探索する探索部と、
前記探索部による探索の結果を用いて、特定処理を実行する特定処理部と、
を備える、処理装置。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書は、物体の表面を表す三次元点群を処理する技術に関する。

【背景技術】

【0002】

点群を示す三次元データを使用して物体の位置と姿勢を推定する技術が知られている。例えば、特許文献１のロボットの制御装置は、モデル点群から選択されたポイントペアのポイントペア特徴量（ＰｏｉｎｔＰａｉｒＦｅａｔｕｒｅ）を、各ポイントペアについて計算する。制御装置は、モデル点群から、複数のキーポイントを選択する。制御装置は、シーン点群におけるポイントペアのポイントペア特徴量を計算し、複数のキーポイントに関するポイントペア特徴量に類似するポイントペア特徴量をもつポイントペアを探索する。制御装置は、探索したポイントペアの各々について座標変換パラメータを決定し、座標変換パラメータが示す姿勢ごとにカウント数を決定する。制御装置は、カウント数が多い順に複数の候補姿勢を選択し、モデル点群とシーン点群との整合度が最も高くなる姿勢を、最終的な姿勢として決定する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１８－１８９５１０号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

このように、点群を示す点群データは、位置と姿勢の決定など、種々の処理に使用され得る。

【0005】

本明細書は、点群を示す三次元データを処理する新たな技術を開示する。

【課題を解決するための手段】

【0006】

本明細書に開示された技術は、以下の適用例として実現することが可能である。

【0007】

［適用例１］コンピュータプログラムであって、第１点群データと、第２点群データと、を取得する点群取得機能であって、前記第１点群データは、対象物体を示す複数個の点を含む第１点群の三次元座標を示し、前記第２点群データは、前記対象物体を含む１以上の物体を示す複数個の点を含む第２点群の三次元座標を示す、前記点群取得機能と、前記第１点群の少なくとも一部である複数個の第１対象点のそれぞれと、前記第２点群の少なくとも一部である複数個の第２対象点のそれぞれと、の局所的特徴量を取得する局所的特徴量取得機能であって、前記局所的特徴量は、１つの対象点の特徴を示す量であり、１つの対象点を含む点群のうちの複数個の点に基づいて算出される量である、前記局所的特徴量取得機能と、前記複数個の第１対象点と前記複数個の第２対象点とを複数個の類似グループに分類する分類機能であって、前記分類は、各対象点の局所的特徴量に基づいて行われ、１つの前記類似グループに属する１以上の点は、互いに類似する前記局所的特徴量を有する、前記分類機能と、第１点セットと、前記第１点セットに対応する複数個の第２点セットと、を決定するセット決定機能であって、前記第１点セットは前記複数個の第１対象点の中から選択されるｎ個（ｎは２以上の整数）の点から構成され、前記複数個の第２点セットのそれぞれは前記複数個の第２対象点の中から選択されるｎ個の点から構成され、１つの前記第２点セットを構成するｎ個の点のそれぞれは、前記第１点セットを構成するｎ個の点のうちの対応する点と同一の前記類似グループに属する、前記セット決定機能と、前記第１点セットと、前記複数個の第２点セットのそれぞれと、の大局的特徴量を取得する大局的特徴量取得機能であって、前記大局的特徴量は、１つの点セットの特徴を示す量であり、１つの点セットを構成するｎ個の点を含む点群のうちの複数個の点に基づいて算出される量である、前記大局的特徴量取得機能と、前記複数個の第２点セットの中から、前記第１点セットが有する前記大局的特徴量と同一または類似する前記大局的特徴を有する特定の点セットを探索する探索機能と、前記探索機能による探索の結果を用いて、特定処理を実行する特定処理機能と、をコンピュータに実現させる、コンピュータプログラム。

【0008】

上記構成によれば、局所的特徴量に基づいて第１点群の少なくとも一部である複数個の第１対象点と第２点群の少なくとも一部である複数個の第２対象点とが複数個の類似グループに分類される。そして、第１対象点から選択されるｎ個の点から構成される第１点セットと、第２対象点から選択されるｎ個の点から構成される複数個の第２点セットと、が決定される。１つの第２点セットを構成するｎ個の点のそれぞれは、第１点セットを構成するｎ個の点のうちの対応する点と同一の類似グループに属する。この結果、局所的特徴量に基づいて、大局的特徴量を用いた探索に適した複数個の第２点セットを決定できる。このために、複数個の第２点セットの中から、第１点セットが有する大局的特徴量と同一または類似する大局的特徴を有する点セットを探索する際に、探索の精度を向上できる。したがって、探索の結果を用いて特定処理を実行する際に、適切な特定処理を実行することができる。また、大局的特徴量を用いた探索に適さない余分な複数個の第２点セットが決定されることを抑制できるので、探索の計算時間を抑制できる。

【0009】

なお、本明細書に開示された技術は、種々の形態で実現可能であり、例えば、処理方法および処理装置、それらの方法または装置の機能を実現するためのコンピュータプログラム、そのコンピュータプログラムを記録した記録媒体（例えば、一時的ではない記録媒体）、等の形態で実現することができる。

【図面の簡単な説明】

【0010】

【図1】一実施例としてのデータ処理装置を示す概略図。

【図2】データ処理装置２００によって実行される処理の例を示すフローチャート。

【図3】モデル点群ＰＣｍとシーン点群ＰＣｓの例を示す図。

【図4】局所的特徴量を用いた分類処理のフローチャート。

【図5】局所座標系の決定の説明図。

【図6】ＳＨＯＴ特徴量の算出の説明図。

【図7】モデル対象点ＰＴｍとシーン対象点ＰＴｓとを説明する図。

【図8】大局的特徴量を用いたマッチング処理のフローチャート。

【図9】点ペアＰＰｉのＰＰＦ特徴量の算出の説明図。

【図10】物体特定処理のフローチャート。

【図11】点ペアＰＰｉの座標系の説明図。

【図12】変換行列ＭＰの算出式を示す図。

【図13】モデル参照点の投影の説明図。

【発明を実施するための形態】

【0011】

Ａ．実施例
Ａ１．装置構成
図１は、一実施例としてのデータ処理装置を示す概略図である。データ処理装置２００は、例えば、パーソナルコンピュータである。データ処理装置２００には、三次元センサ１１０と、ロボットアーム１２０とが、接続されている。ロボットアーム１２０の前には、トレー１３０が配置されている。トレー１３０上には、処理対象の複数の物体（以下、対象物体ＯＢとも呼ぶ）が乱雑に積まれている。後述するように、データ処理装置２００は、三次元センサ１１０からの情報を使用してロボットアーム１２０を制御し、ロボットアーム１２０に対象物体ＯＢを把持させる。図示を省略するが、データ処理装置２００は、ロボットアーム１２０に、対象物体ＯＢをトレー１３０から別の場所（例えば、製造ラインのベルトコンベア）に移動させる。

【0012】

三次元センサ１１０は、物体の表面上の複数の点のそれぞれの三次元座標を測定するセンサである。三次元センサ１１０の種類は、種々の種類であってよく、例えば、立体視、構造化光パターン、飛行時間のいずれかであってよい。立体視は、パッシブステレオ方式とも呼ばれ、複数のカメラによる撮影画像を使用して物体の表面上の三次元座標を測定する方式である。構造化光パターンは、アクティブステレオ方式とも呼ばれ、物体に投影された光のパターンを使用して物体の表面上の三次元座標を測定する方式である。飛行時間は、レーザや赤外線などの光が送信されてから、物体からの反射光が戻るまでの時間を使用して物体の表面上の三次元座標を測定する方式である。三次元センサは、三次元センサから物体の表面までの距離（深度とも呼ばれる）を測定する深度センサとも呼ばれる。本実施例では、三次元センサ１１０の測定方向１１０ｘは、トレー１３０の方向を向いている（測定方向１１０ｘは、測定可能な方向を示している）。三次元センサ１１０は、トレー１３０上の複数の対象物体ＯＢを含む物体群（例えば、トレー１３０と複数の対象物体ＯＢ）を測定する。

【0013】

ロボットアーム１２０の構成は、物体を移動させることが可能な種々の構成であってよい。本実施例では、ロボットアーム１２０は、オープンチェーン構造を有するリンク機構１２５を形成する複数のリンク１２１－１２４と、物体を挟み持つことが可能な複数の指１２６と、を有している。複数の指１２６は、リンク機構１２５の先端に取り付けられている。リンク機構１２５と複数の指１２６とは、図示しない複数の電気モータによって駆動される。データ処理装置２００は、複数の電気モータを制御することによって、ロボットアーム１２０を動かすことができる。

【0014】

データ処理装置２００は、プロセッサ２１０と、記憶装置２１５と、表示部２４０と、操作部２５０と、通信インタフェース２７０と、を有している。これらの要素は、バスを介して互いに接続されている。記憶装置２１５は、揮発性記憶装置２２０と、不揮発性記憶装置２３０と、を含んでいる。

【0015】

プロセッサ２１０は、データ処理を行うように構成された装置であり、例えば、ＣＰＵである。揮発性記憶装置２２０は、例えば、ＤＲＡＭであり、不揮発性記憶装置２３０は、例えば、フラッシュメモリである。不揮発性記憶装置２３０は、コンピュータプログラムＰＧと、モデル点群データＰＤｍと、シーン点群データＰＤｓと、を格納している。コンピュータプログラムＰＧは、後述する図２の処理をプロセッサ２１０に実現させる。コンピュータプログラムＰＧは、例えば、ロボットアーム１２０の製造業者や、ロボットアーム１２０の制御プログラムの製造事業者によって提供される。モデル点群データＰＤｍとシーン点群データＰＤｓとについては後述する。

【0016】

表示部２４０は、液晶ディスプレイ、有機ＥＬディスプレイなどの、画像を表示するように構成された装置である。操作部２５０は、ボタン、レバー、表示部２４０上に重ねて配置されたタッチパネルなどの、ユーザによる操作を受け取るように構成された装置である。ユーザは、操作部２５０を操作することによって、種々の指示をデータ処理装置２００に入力可能である。通信インタフェース２７０は、他の装置と通信するためのインタフェースである（例えば、ＵＳＢインタフェース、有線ＬＡＮインタフェース、IEEE802.11の無線インタフェース）。通信インタフェース２７０には、三次元センサ１１０とロボットアーム１２０とが接続される。

【0017】

Ａ２．データ処理装置２００の処理
図２は、データ処理装置２００によって実行される処理の例を示すフローチャートである。図２の処理は、トレー１３０上に配置される対象物体ＯＢの位置と姿勢を特定し、該特定結果を用いてロボットアーム１２０を制御して、ロボットアーム１２０に対象物体ＯＢの把持および移動を行わせる処理である。データ処理装置２００のプロセッサ２１０は、コンピュータプログラムＰＧを実行することによって、図２の処理を実行する。

【0018】

Ｓ１０では、プロセッサ２１０は、不揮発性記憶装置２３０から、モデル点群データＰＤｍ（図１）を取得する。図３（Ａ）は、モデル点群データＰＤｍによって示されるモデル点群ＰＣｍの例を示す斜視図である。モデル点群ＰＣｍは、マッチング（後述）の基準となる対象物体ＯＢを示している。モデル点群ＰＣｍによって示される対象物体ＯＢを、後述するシーン点群ＰＣｓによって示される対象物体ＯＢと区別するために基準物体ＯＢｍとも呼ぶ。モデル点群ＰＣｍは、基準物体ＯＢｍの表面を示す複数の点ＰＴを含む。これらのモデル点群ＰＣｍの複数の点ＰＴは、例えば、一の点ＰＴと該一の点と最も近い点ＰＴとの距離が数ｍｍ程度になるように、基準物体ＯＢｍの表面の全体に分散して配置されている。モデル点群データＰＤｍは、これらの複数の点ＰＴのそれぞれの３次元座標（ｘｍ、ｙｍ、ｚｍ）を示す三次元データである。モデル点群ＰＣｍの各点ＰＴの座標は、互いに直交する３個の座標軸Ｘｍ、Ｙｍ、Ｚｍ（図３（Ａ））によって定められる座標系で表される（以下、この座標系を、モデル座標系と呼ぶ）。図３（Ａ）の例では、基準物体ＯＢｍは、第１部分ＯＢｐ１と第２部分ＯＢｐ２とを備えている。第１部分ＯＢｐ１は、底面Ｂｆを有し、図３（Ａ）に破線で示す直線ＡＸを軸線とする等径の円筒状の部分である。第２部分ＯＢｐ２は、直線ＡＸを軸線とし、第１部分ＯＢｐ１と接続する端の径が第１部分ＯＢｐ１と同じであり、第１部分ＯＢｐ１から離れるに連れて縮径する円筒状の部分である。図中では、基準物体ＯＢｍの外面のうちの後ろに隠れた部分の複数の点ＰＴの図示が、省略されている。なお、基準物体ＯＢｍの形状（すなわち、対象物体ＯＢの形状）は、他の任意の形状であってよい。例えば、対象物体ＯＢは、電気部品（スイッチ、端子など）、または、機械部品（ネジ、ナット、パイプなど）などの、種々の形状の部品であってよい。

【0019】

本実施例では、モデル点群ＰＣｍを示すモデル点群データＰＤｍは、基準物体ＯＢｍの立体的な形状を示すＣＡＤデータを使用して、予め生成される（ＣＡＤは、computer-aided designの略）。ＣＡＤデータは、例えば、対象物体ＯＢの設計データである。ＣＡＤデータによって示される基準物体ＯＢｍの外面上に複数の点ＰＴが配置される。そして、各点ＰＴの座標が、ＣＡＤデータに従って、算出される。なお、モデル点群データＰＤｍは、対象物体ＯＢの実物を三次元センサ１１０によって測定することによって、生成されてもよい。

【0020】

Ｓ２０では、プロセッサ２１０は、シーン点群データＰＤｓを取得する。本実施例では、プロセッサ２１０は、三次元センサ１１０（図１）に、複数の対象物体ＯＢを含む空間を測定させることによって、三次元センサ１１０からシーン点群データＰＤｓを取得する。図３（Ｂ）は、シーン点群データＰＤｓによって示されるシーン点群ＰＣｓの例を示す斜視図である。シーン点群ＰＣｓは、三次元センサ１１０の測定範囲内の物体の表面のうち、三次元センサ１１０から視認可能な表面上の複数の点ＰＴである。シーン点群データＰＤｓは、これらの複数の点のそれぞれの３次元座標（ｘｓ、ｙｓ、ｚｓ）を示す三次元データである。図３（Ｂ）は、シーン点群ＰＣｓの一部を示している。図中には、トレー１３０の一部と、トレー１３０上の２個の対象物体ＯＢと、が示されている。図中では、個々の点ＰＴの図示は省略され、複数の点ＰＴによって表される表面がハッチングで示されている。図示を省略するが、トレー１３０上には、複数の対象物体ＯＢが積み重なり得る。この場合、シーン点群ＰＣｓは、積み重なる複数の対象物体ＯＢのうち、三次元センサ１１０から視認可能な表面上の複数の点ＰＴを示している。

【0021】

シーン点群ＰＣｓの点ＰＴの座標は、互いに直交する３個の座標軸Ｘｓ、Ｙｓ、Ｚｓ（図３（Ｂ））によって定められる座標系で表される（以下、この座標系を、シーン座標系と呼ぶ）。シーン座標系は、三次元センサ１１０に対する相対的な位置を表す座標系である。例えば、シーン座標系の原点は、三次元センサ１１０の位置に配置される。また、１個の座標軸（例えば、第３座標軸Ｚｓ）は、三次元センサ１１０の測定方向１１０ｘに平行であってよい。

【0022】

Ｓ３０では、プロセッサ２１０は、シーン点群データＰＤｓに対して、前処理を実行する。前処理では、後述する分類処理やマッチング処理（Ｓ４０、Ｓ５０）にて利用しやすいように、シーン点群ＰＣｓを適宜に間引く処理である。例えば、シーン点群ＰＣｓの点ＰＴの密度がモデル点群ＰＣｍと同程度の密度になるように、シーン点群ＰＣｓは間引かれる。

【0023】

Ｓ３５では、プロセッサ２１０は、モデル点群ＰＣｍおよびシーン点群ＰＣｓに含まれる各点ＰＴの法線ベクトルを算出する。法線ベクトルは、例えば、主成分分析を実行することによって算出される。主成分分析については、局所座標系の決定方法の説明にて後述する。

【0024】

Ｓ４０では、プロセッサ２１０は、局所的特徴量を用いた分類処理（以下、単に分類処理とも呼ぶ）を実行する。図４は、局所的特徴量を用いた分類処理のフローチャートである。分類処理は、モデル点群ＰＣｍおよびシーン点群ＰＣｓからそれぞれ選択される複数個のモデル対象点ＰＴｍおよびシーン対象点ＰＴｓのそれぞれを、各対象点の局所的特徴量を用いて分類する処理である。

【0025】

Ｓ１１０では、モデル点群ＰＣｍからｍ個（ｍは２以上の整数）の代表点ＲＰを選択する。代表点ＲＰの個数ｍは、例えば、３～１５個程度であり、本実施例では、図３（Ａ）に示すように、４個の代表点ＲＰ１～ＲＰ４が選択される。例えば、プロセッサ２１０は、モデル点群ＰＣｍをモニタに表示する。オペレータは、所定のポインティングデバイスを操作して、モニタに表示されたモデル点群ＰＣｍから代表点ＲＰとすべき点ＰＴを指定する。プロセッサ２１０は、指定された点ＰＴを代表点ＲＰとして選択する。

【0026】

ｍ個の代表点ＲＰには、対象物体ＯＢの表面上の互いに異なる特徴を有する複数の部位のそれぞれに位置する点ＰＴが選択される。例えば、図３（Ａ）の例では、代表点ＲＰ１は、縮径された円筒状の第２部分ＯＢｐ２の最も径が短い端部に位置する点ＰＴである。代表点ＲＰ２は、縮径された円筒状の第２部分ＯＢｐ２と、等径の円筒状の第１部分ＯＢｐ１と、の境界部に位置する点ＰＴである。代表点ＲＰ３は、第１部分ＯＢｐ１の軸ＡＸに沿った方向の中央部に位置する点ＰＴである。代表点ＲＰ３は、第１部分ＯＢｐ１と底面Ｂｆとの境界部に位置する点ＰＴである。

【0027】

Ｓ１２０では、プロセッサ２１０は、各代表点ＲＰの局所的特徴量を算出する。局所的特徴量は、１つの注目点（例えば、代表点ＲＰ、あるいは、後述するモデル対象点ＰＴｍやシーン対象点ＰＴｓ）の特徴を示す量であり、注目点を含む点群（モデル点群ＰＣｍまたはシーン点群ＰＣｓ）のうちの注目点の近傍の複数個の点に基づいて算出される量である。本実施例では、局所的特徴量として、ＳＨＯＴ（Signature of Histograms of OrienTations）特徴量が採用される。

【0028】

１つの注目点ＰＴｉのＳＨＯＴ特徴量の算出について説明する。プロセッサ２１０は、注目点ＰＴｉにおける局所座標系を決定する。図５は、局所座標系の決定の説明図である。図５（Ａ）、（Ｂ）には、複数の点ＰＴから選択される注目点ＰＴｉと、注目点ＰＴｉを中心とする半径Ｒ１を有するサポート球ＳＰと、が示されている。サポート球ＳＰ内の領域は、注目点ＰＴｉからの距離がゼロ以上半径Ｒ１以下の領域（以下、サポート領域ＳＲとも呼ぶ）である。

【0029】

図５（Ｂ）に示すように、注目点ＰＴｉに対して決定される局所座標系は、注目点ＰＴｉを原点とし、互いに直交する単位ベクトルによって定義される３個の座標軸Ｘｌ、Ｙｌ、Ｚｌによって定められる座標系である。

【0030】

３個の座標軸Ｘｌ、Ｙｌ、Ｚｌの決定方法、すなわち、注目点ＰＴｉにおける局所座標系の決定方法は、種々の方法であってよい。本実施例では、プロセッサ２１０は、サポート領域ＳＲ（図５（Ｂ））内の複数の点ＰＴに対していわゆる主成分分析を行うことによって、分散共分散行列の３個の固有ベクトル、すなわち、第１固有ベクトルｖｉ、第２固有ベクトルｖｉｉ、第３固有ベクトルｖｉｉｉを算出する。プロセッサ２１０は、３個の固有ベクトルｖｉ、ｖｉｉ、ｖｉｉｉを座標軸Ｘｌ、Ｙｌ、Ｚｌに決定する。これによって、局所座標系が決定される。この方法によって、座標軸Ｘｌは、サポート領域ＳＲ内の複数個の点ＰＴの位置のバラツキ（分散）が最大となる方向に決定される。座標軸Ｙｌは、座標軸Ｘｌと直交する方向のうち、上記バラツキが最大となる方向に決定される。座標軸Ｚｌは、座標軸Ｚｌと座標軸Ｙｌとに直交する方向に決定される。座標軸Ｘｌおよび座標軸Ｙｌとの両方に平行な平面ＰＬ（図５（Ｂ））は、サポート領域ＳＲに含まれる複数の点ＰＴを近似する近似平面であると言うことができるので、座標軸Ｚｌを示す第３固有ベクトルｖｉｉｉは、注目点ＰＴｉにおける法線ベクトルｖｎ（平面ＰＬに垂直なベクトル）でもある。

【0031】

プロセッサ２１０は、注目点ＰＴｉにおける局所座標系を決定すると、該局所座標系を用いて、注目点ＰＴｉの局所的特徴量としてＳＨＯＴ特徴量を算出する。図６は、ＳＨＯＴ特徴量の算出の説明図である。プロセッサ２１０は、注目点ＰＴｉに対して、Ｋ個の参照領域を設定する。本実施例では、参照領域の個数Ｋは、３２である。

【0032】

具体的には、図６（Ａ）に示すように、注目点ＰＴｉのサポート球ＳＰの内側に、サポート球ＳＰよりも小さな内球ＳＰｉが設定される。内球ＳＰｉは、注目点ＰＴｉを中心とし、サポート球ＳＰの半径の半分の半径Ｒ２を有する球である。さらに、注目点ＰＴｉを通り、座標軸Ｚｌと垂直な平面Ｆｚが設定される。内球ＳＰｉと平面Ｆｚとによって、サポート球ＳＰ内のサポート領域ＳＲは、４個の領域、すなわち、上内側領域Ｓｉｕ、上外側領域Ｓｏｕ、下内側領域Ｓｉｂ、下外側領域Ｓｏｂに分割される。上内側領域Ｓｉｕは、内球ＳＰｉ内の領域のうち、平面Ｆｚよりも上側（座標軸Ｚｌの正方向側）の半球の領域である。上外側領域Ｓｏｕは、サポート球ＳＰ内の領域のうち、平面Ｆｚよりも上側の半球の領域から上内側領域Ｓｉｕを除いた領域である。下内側領域Ｓｉｂは、内球ＳＰｉ内の領域のうち、平面Ｆｚよりも下側（座標軸Ｚｌの負方向側）の半球の領域である。下外側領域Ｓｏｂは、サポート球ＳＰ内の領域のうち、平面Ｆｚよりも下側の半球の領域から下内側領域Ｓｉｂを除いた領域である。

【0033】

さらに、座標軸Ｚｌを含む４個の平面Ｆａ～Ｆｄが設定される。図６（Ｂ）には、下外側領域Ｓｏｂと下内側領域Ｓｉｂとを座標軸Ｚｌの正方向側から、負方向側に向かって見た図が示されている。平面Ｆａは、座標軸Ｚｌと座標軸Ｘｌとを含む平面である。平面Ｆｃは、座標軸Ｚｌと座標軸Ｙｌとを含む平面である。平面Ｆｂと平面Ｆｄとは、互いに直交し、かつ、平面Ｆａ、Ｆｃとの間の角度が４５度である平面である。図６（Ｂ）に示すように、平面Ｆａ～Ｆｄにより、下外側領域Ｓｏｂは、８個の参照領域Ｓｏｂ１～Ｓｏｂ８に分割される。図６（Ｂ）に示すように、平面Ｆａ～Ｆｄにより、下内側領域Ｓｉｂは、８個の参照領域Ｓｉｂ１～Ｓｉｂ８に分割される。図示は省略するが、上外側領域Ｓｏｕと上内側領域Ｓｉｕも、同様に、平面Ｆａ～Ｆｄにより、それぞれ、８個の参照領域に分割される。これによって、合計で３２個の参照領域が注目点に対して設定される。

【0034】

プロセッサ２１０は、３２個の参照領域のそれぞれの領域特徴量ＨＧ１～ＨＧ３２を算出する。図６（Ｃ）には、領域特徴量ＨＧ１～ＨＧ３２の概念図である。１つの領域特徴量は、対応する参照領域内に含まれる複数個の点ＰＴを、各点ＰＴの法線ベクトルと注目点ＰＴｉの法線ベクトルｖｎ（座標軸Ｚｌ）との角度θに応じて、Ｌ個のビンに分類して得られるヒストグラムデータである。本実施例では、１つの領域特徴量のビン数Ｌは、１１である。各点ＰＴの法線ベクトルは、上述した注目点ＰＴｉの法線ベクトルｖｎ（座標軸Ｚｌ）を決定する処理と同様の主成分分析を、各点ＰＴについて実行することによって図２のＳ３５にて算出済みである。角度θは、各点ＰＴの法線ベクトルと、注目点ＰＴｉの法線ベクトルｖｎと、の内積を算出することによって求められる。角度θは、０～１８０度の値を取る。１つの領域特徴量は、１１個の各ビンに分類された点数を示す１１次元のベクトルで表される。このために、３２個の参照領域と一対一で対応する３２個の領域特徴量の全体は、（３２×１１）次元、すなわち、３５２次元のベクトルである。プロセッサ２１０は、この３５２次元のベクトルを正規化したベクトルＦＶを、１つの注目点ＰＴｉの局所的特徴量として算出する。

【0035】

図４のＳ１３０では、プロセッサ２１０は、ｍ個の代表点ＲＰの局所的特徴量の差異は、基準以上であるか否かを判断する。例えば、プロセッサ２１０は、ｍ個の代表点ＲＰから選択される２個の代表点ＲＰの全てのペアについて、２個の代表点ＲＰの局所的特徴量の差異を示す評価値を算出する。プロセッサ２１０は、全てのペアの評価値が基準以上の差異があることを示す場合に、ｍ個の代表点ＲＰの局所的特徴量は、互いに異なると判断する。例えば、プロセッサ２１０は、全てのペアについて、２個の代表点ＲＰの局所的特徴量（３５２次元ベクトル）のユークリッド距離を評価値として算出する。プロセッサ２１０は、全てのペアのユークリッド距離が所定の閾値ＴＨ以上である場合には、ｍ個の代表点ＲＰの局所的特徴量の差異は基準以上であると判断し、１以上のペアのユークリッド距離が閾値ＴＨ未満である場合には、ｍ個の代表点ＲＰの局所的特徴量の差異は基準未満であると判断する。

【0036】

ｍ個の代表点ＲＰの局所的特徴量の差異が基準以上である場合には（Ｓ１３０：ＹＥＳ）、プロセッサ２１０は、Ｓ１４０に処理を進める。ｍ個の代表点ＲＰの局所的特徴量の差異が基準未満である場合には（Ｓ１３０：ＮＯ）、プロセッサ２１０は、Ｓ１１０に戻って、ｍ個の代表点ＲＰの選択をやり直す。

【0037】

Ｓ１４０では、プロセッサ２１０は、モデル点群ＰＣｍの中から、複数個のモデル対象点ＰＴｍを選択する。図７は、モデル対象点ＰＴｍとシーン対象点ＰＴｓとを説明する図である。モデル対象点ＰＴｍ（図７（Ａ））は、モデル点群ＰＣｍの複数個の点ＰＴのうち、局所的特徴量の算出対象とされる点である。例えば、モデル点群ＰＣｍの複数個の点ＰＴのうち、１０～５０％程度の点がモデル対象点ＰＴｍとして選択される。モデル点群ＰＣｍの複数個の点ＰＴのうちの一部だけを局所的特徴量の算出対象とすることで、計算時間を短縮することができる。複数個のモデル対象点ＰＴｍは、例えば、基準物体ＯＢｍの表面の全体に分散するように、一部分に過度に偏らないように選択される。

【0038】

Ｓ１５０では、プロセッサ２１０は、複数個のモデル対象点ＰＴｍの局所的特徴量をそれぞれ算出する。局所的特徴量（本実施例ではＳＨＯＴ特徴量）の算出方法は、上述した代表点ＲＰの局所的特徴量の算出方法と同じである。

【0039】

Ｓ１６０では、プロセッサ２１０は、複数個のモデル対象点ＰＴｍを、局所的特徴量に基づいて（ｍ＋１）個の類似グループに分類する。類似グループは、互いに類似する局所的特徴量を有する対象点が属するグループである。本実施例では、代表点ＲＰの個数ｍは４であるので、プロセッサ２１０は、複数個のモデル対象点ＰＴｍを５つの類似グループに分類する。５つの類似グループは、代表点ＲＰ１と類似する対象点のグループＧ１と、代表点ＲＰ２と類似する対象点のグループＧ２と、代表点ＲＰ３と類似する対象点のグループＧ３と、代表点ＲＰ４と類似する対象点のグループＧ４と、代表点ＲＰ１～ＲＰ４のいずれとも類似しない対象点のグループＧ５である。プロセッサ２１０は、一のモデル対象点ＰＴｍの局所的特徴量と、一の代表点ＲＰの局所的特徴量と、の間のユークリッド距離を算出する。プロセッサ２１０は、該ユークリッド距離が所定の閾値ＴＨｖ未満である場合には、一のモデル対象点ＰＴｍと一の代表点ＲＰとは類似すると判断し、該ユークリッド距離が所定の閾値ＴＨｖ以上である場合には、一のモデル対象点ＰＴｍと一の代表点ＲＰとは類似しないと判断する。

【0040】

図７（Ａ）において、一重丸は代表点ＲＰ１と類似するモデル対象点ＰＴｍを示し、二重丸は代表点ＲＰ２と類似するモデル対象点ＰＴｍを示す。同様に、四角、三角、Ｘ印は、それぞれ、代表点ＲＰ３と類似するモデル対象点ＰＴｍ、代表点ＲＰ４と類似するモデル対象点ＰＴｍ、代表点ＲＰ１～ＲＰ４のいずれとも類似しないモデル対象点ＰＴｍを示す。

【0041】

Ｓ１７０では、プロセッサ２１０は、シーン点群ＰＣｓの中から、複数個のシーン対象点ＰＴｓを選択する。シーン対象点ＰＴｓ（図７（Ｂ））は、シーン点群ＰＣｓの複数個の点ＰＴのうち、局所的特徴量の算出対象とされる点である。例えば、シーン点群ＰＣｓの複数個の点ＰＴのうち、１０～５０％程度の点がシーン対象点ＰＴｓとして選択される。複数個のシーン対象点ＰＴｓは、例えば、モデル点群ＰＣｍによって示される物体の表面の全体に分散するように、一部分に過度に偏らないように選択される。

【0042】

Ｓ１８０では、プロセッサ２１０は、複数個のシーン対象点ＰＴｓの局所的特徴量をそれぞれ算出する。局所的特徴量（本実施例ではＳＨＯＴ特徴量）の算出方法は、上述した代表点ＲＰの局所的特徴量の算出方法と同じである。

【0043】

Ｓ１９０では、プロセッサ２１０は、複数個のシーン対象点ＰＴｓを局所的特徴量に基づいて、（ｍ＋１）個のグループに分類する。モデル対象点ＰＴｍの分類と同様に、プロセッサ２１０は、複数個のシーン対象点ＰＴｓを上述の５つのグループＧ１～Ｇ５に分類する。モデル対象点ＰＴｍの分類と同様に、プロセッサ２１０は、一のシーン対象点ＰＴｓの局所的特徴量と、一の代表点ＲＰの局所的特徴量と、の間のユークリッド距離を算出する。プロセッサ２１０は、該ユークリッド距離が所定の閾値ＴＨｖ未満である場合には、一のシーン対象点ＰＴｓと一の代表点ＲＰとは類似すると判断し、該ユークリッド距離が所定の閾値ＴＨｖ以上である場合には、一のシーン対象点ＰＴｓと一の代表点ＲＰとは類似しないと判断する。

【0044】

図７（Ｂ）において、一重丸は代表点ＲＰ１と類似するシーン対象点ＰＴｓを示し、二重丸は代表点ＲＰ２と類似するシーン対象点ＰＴｓを示す。同様に、四角、三角、Ｘ印は、それぞれ、代表点ＲＰ３と類似するシーン対象点ＰＴｓ、代表点ＲＰ４と類似するシーン対象点ＰＴｓ、代表点ＲＰ１～ＲＰ４のいずれとも類似しないシーン対象点ＰＴｓを示す。

【0045】

局所的特徴量を用いた分類処理が終了すると、図２のＳ５０では、プロセッサ２１０は、大局的特徴量を用いたマッチング処理（以下、単にマッチング処理とも呼ぶ）を実行する。図８は、大局的特徴量を用いたマッチング処理のフローチャートである。マッチング処理は、２個のモデル対象点ＰＴｍから構成されるモデル点ペアと、２個のシーン対象点ＰＴｓから構成されるシーン点ペアと、を決定し、後述する大局的特徴量を用いて、モデル点ペアとシーン点ペアとのマッチングを行う処理である。

【0046】

Ｓ２１０では、プロセッサ２１０は、上述した（ｍ＋１）個のグループのうち、いずれの代表点ＲＰとも類似しないグループを除いたｍ個のグループから、１つの注目グループペアを決定する。ｍ個のグループから２個のグループを選択する組み合わせは、_ｍＣ_２種類存在する。Ｓ２１０では、これらの_ｍＣ_２種類のグループペアが順次に注目グループペアとして決定される。本施例では、５つのグループＧ１～Ｇ４の中から注目グループペアが決定される。したがって、グループＧ１、Ｇ２のペアと、グループＧ１、Ｇ３のペアと、グループＧ１、Ｇ４のペアと、グループＧ２、Ｇ３のペアと、グループＧ２、Ｇ４のペアと、グループＧ３、Ｇ４のペアと、の６種類のペアが順次に注目グループペアとして決定される。

【0047】

Ｓ２２０では、プロセッサ２１０は、注目グループペアに属する複数個のモデル対象点ＰＴｍから１つの注目モデル点ペアを決定する。具体的には、注目グループペアを構成する一のグループに属する複数個のモデル対象点ＰＴｍから選択される１個の点と、注目グループペアを構成する他のグループに属する複数個のモデル対象点ＰＴｍから選択される１個の点と、のペアが注目モデル点ペアとして決定される。一のグループに属するモデル対象点ＰＴｍの個数がＮ１個であり、他のグループに属するモデル対象点ＰＴｍの個数がＮ２個である場合には、（Ｎ１×Ｎ２）種のモデル点ペアが決定可能である。Ｓ２２０では、これらの（Ｎ１×Ｎ２）種類のモデル点ペアが順次に注目モデル点ペアとして決定される。例えば、注目グループペアが、グループＧ１、Ｇ２のペアである場合には、代表点ＲＰ１に類似するモデル対象点ＰＴｍ（図７（Ａ）の一重丸）と、代表点ＲＰ２に類似するモデル対象点ＰＴｍ（図７（Ａ）の二重丸）と、の点ペアが、注目モデル点ペアとして決定される。

【0048】

Ｓ２３０では、プロセッサ２１０は、注目モデル点ペア間の距離Ｄｍに基づいて距離条件を決定する。距離条件は、後述するシーン点ペア間の距離Ｄｓが満たすべき条件である。具体的には、プロセッサ２１０は、注目モデル点ペアを構成する一のモデル対象点ＰＴｍと他のモデル対象点ＰＴｍとの間のユークリッド距離を、距離Ｄｍとして算出する。プロセッサ２１０は、例えば、距離Ｄｓが距離Ｄｍを含む所定の距離範囲内であること、具体的には、距離Ｄｓが距離Ｄｍ±５％の範囲内であることを、距離条件として決定する。

【0049】

Ｓ２４０では、プロセッサ２１０は、注目グループペアに属する複数個のシーン対象点ＰＴｓから距離条件を満たす複数個のシーン点ペアを、マッチングに用いるシーン点ペアとして決定する。具体的には、注目グループペアを構成する一のグループに属する複数個のシーン対象点ＰＴｓから選択される１個の点と、注目グループペアを構成する他のグループに属する複数個のシーン対象点ＰＴｓから選択される１個の点と、のペアがシーン点ペアとして決定される。一のグループに属するシーン対象点ＰＴｓの個数がＮ３個であり、他のグループに属するシーン対象点ＰＴｓの個数がＮ４個である場合には、（Ｎ３×Ｎ４）種のシーン点ペアが決定可能である。Ｓ２４０では、プロセッサ２１０は、これらの（Ｎ３×Ｎ４）種類のシーン点ペアの全てについて、シーン点ペアを構成する一のシーン対象点ＰＴｓと他のシーン対象点ＰＴｓとの間のユークリッド距離を、シーン点ペア間の距離Ｄｓとして算出する。プロセッサ２１０は、（Ｎ３×Ｎ４）種類のシーン点ペアの全てについて、シーン点ペア間の距離ＤｓがＳ２３０にて決定された距離条件を満たすか否かを判断する。プロセッサ２１０は、（Ｎ３×Ｎ４）種類のシーン点ペアのうち、距離条件を満たす全てのシーン点ペアを、マッチングに用いるシーン点ペアとして決定する。例えば、注目グループペアが、グループＧ１、Ｇ２のペアである場合には、代表点ＲＰ１に類似するシーン対象点ＰＴｓ（図７（Ｂ）の一重丸）と、代表点ＲＰ２に類似するシーン対象点ＰＴｓ（図７（Ｂ）の二重丸）と、の点ペアが、距離条件を満たす場合に、マッチングに用いるシーン点ペアとして決定される。

【0050】

Ｓ２５０では、プロセッサ２１０は、全ての注目グループペアに対応する全てのモデル点ペア、すなわち、上述した（Ｎ１×Ｎ２）種のモデル点ペアを注目モデルペアとして処理したか否かを判断する。未処理のモデル点ペアがある場合には（Ｓ２５０：ＮＯ）、プロセッサ２１０は、Ｓ２２０に戻る。全てのモデル点ペアが処理された場合には（Ｓ２５０：ＹＥＳ）、プロセッサ２１０は、Ｓ２６０に処理を進める。

【0051】

Ｓ２６０では、プロセッサ２１０は、全てのグループペア、すなわち、上述した_ｍＣ_２種類のグループペアを注目グループペアとして処理したか否かを判断する。未処理のグループペアがある場合には（Ｓ２６０：ＮＯ）、プロセッサ２１０は、Ｓ２１０に戻る。全てのグループペアが処理された場合には（Ｓ２６０：ＹＥＳ）、プロセッサ２１０は、Ｓ２７０に処理を進める。

【0052】

Ｓ２７０では、プロセッサ２１０は、Ｓ２２０、Ｓ２４０にて決定済みの全てのモデル点ペアおよびシーン点ペアのそれぞれについて、大局的特徴量を算出する。大局的特徴量は、ｎ個（ｎは２以上の整数）の点セットの特徴を示す量である。大局的特徴量は、点セットを構成するｎ個の点を含む点群（例えば、モデル点群ＰＣｍやシーン点群ＰＣｓ）のうちの複数個の点ＰＴ（例えば、ｎ個の点の近傍の複数個の点）に基づいて算出される。本実施例では、点セットとして、２個の点ペア（モデル点ペアやシーン点ペア）が採用されており、点ペアの特徴を示す大局的特徴量として、ＰＰＦ（Point Pair Feature）特徴量が採用されている。

【0053】

図９は、点ペアＰＰｉのＰＰＦ特徴量の算出の説明図である。ＰＰＦ特徴量は、点ペアＰＰｉの特徴を示すパラメータを含んでいる。図９（Ａ）～（Ｄ）は、点ペアＰＰｉの特徴を示す４個のパラメータＤ、Ｆ１～Ｆ３の概略図である。図９（Ａ）～（Ｄ）には、点ペアＰＰｉを構成する第１点Ｐと第２点Ｑとが示されている。ベクトルｖＰｎは、第１点Ｐの法線ベクトルであり、ベクトルｖＱｎは、第２点Ｑの法線ベクトルである。法線ベクトルｖＰｎ、ｖＱｎは、図２のＳ３５にて算出済みである。

【0054】

図６（Ａ）のパラメータＤは、第１点Ｐと第２点Ｑとの間のユークリッド距離である。図６（Ｂ）のパラメータＦ１は、基ベクトルｖＰＱと、第１点Ｐの法線ベクトルｖＰｎと、のなす角度である（Ｆ１は、０度以上１８０度以下）。基ベクトルｖＰＱは、第１点Ｐを始点とし、第２点Ｑを終点とするベクトルである。図中の関数Ａｎｇは、２個の引数ベクトルのなす角度を算出する関数である。図６（Ｃ）のパラメータＦ２は、基ベクトルｖＰＱと、第２点Ｑの法線ベクトルｖＱｎと、のなす角度である（Ｆ１は０度以上１８０度以下）。図６（Ｄ）のパラメータＦ３は、第１点Ｐの法線ベクトルｖＰｎと、第２点Ｑの法線ベクトルｖＱｎと、のなす角度である（Ｆ３は０度以上１８０度以下）。

【0055】

プロセッサ２１０は、Ｓ２２０、Ｓ２４０にて決定済みの全てのモデル点ペアおよびシーン点ペアのそれぞれのＰＰＦ特徴量を算出する。ただし、各モデル点ペア間の距離Ｄｐ、および、シーン点ペア間の距離Ｄｓは、上述したＳ２３０、Ｓ２４０にて算出済みであるので、再度の算出は行われない。

【0056】

Ｓ２８０では、プロセッサ２１０は、大局的特徴量に基づいてモデル点ペアとシーン点ペアとのマッチングを行う。具体的には、プロセッサ２１０は、注目モデル点ペアに対応する複数個のシーン点ペアの中から、注目モデル点ペアの大局的特徴量と同一または類似する大局的特徴量を有するシーン点ペアを探索する。ここで、注目モデル点ペアに対応する複数個のシーン点ペアは、注目モデルペアについて、Ｓ２３０にて決定された複数個のシーン点ペアを意味する。すなわち、注目モデル点ペアに対応する各シーン点ペアは、注目モデル点ペアと同じグループペアに属し、かつ、注目モデル点ペアについて決定された距離条件を満たす。プロセッサ２１０は、Ｓ２２０にて決定された全てのモデル点ペアを注目モデルペアとして、上記の探索を行う。

【0057】

一のモデル点ペアの大局的特徴量（Ｄ、Ｆ１、Ｆ２、Ｆ３）を（Ｄｍ、Ｆ１ｍ、Ｆ２ｍ、Ｆ３ｍ）とし、比較対象のシーン点ペアの大局的特徴量（Ｄ、Ｆ１、Ｆ２、Ｆ３）を（Ｄｓ、Ｆ１ｓ、Ｆ２ｓ、Ｆ３ｓ）とする。プロセッサ２１０は、以下の条件式（１）～（４）が全て満たされる場合に、一のモデル点ペアの大局的特徴量と、比較対象のシーン点ペアの大局的特徴量と、が同一または類似であると判断する。プロセッサ２１０は、条件式（１）～（４）のいずれかが満たされない場合に、一のモデル点ペアの大局的特徴量と、比較対象のシーン点ペアの大局的特徴量と、が非類似であると判断する。条件式（１）～（４）を用いることで、パラメータごとに閾値と比較することによって、類似するか否かの判断が行われるので、例えば、実験的に、パラメータごとに適切な閾値を設定することで、判断の精度を向上し得る。また、条件式（１）～（４）のいずれかが満たされない時点で判断を終了できるので、判断時間を短縮できる。

【0058】

｜Ｄｍ－Ｄｓ｜＜ＴＨｄ …（１）
｜Ｆ１ｍ－Ｆ１ｓ｜＜ＴＨ１ …（２）
｜Ｆ２ｍ－Ｆ２ｓ｜＜ＴＨ２ …（３）
｜Ｆ３ｍ－Ｆ３ｓ｜＜ＴＨ３ …（４）

【0059】

プロセッサ２１０は、探索されたシーン点ペアと、一のモデル点ペアと、をマッチングした組み合わせとして決定する。マッチングしたモデル点ペアとシーン点ペアとの組み合わせを「特定組み合わせ」とも呼ぶ。一般的には、多数の特定組み合わせが探索される。以下では、探索された特定組み合わせの個数をＪ個とする（Ｊは３以上の整数）。

【0060】

大局的特徴量を用いたマッチング処理が終了すると、図２のＳ６０では、プロセッサ２１０は、プロセッサ２１０は、物体特定処理を実行する。図１０は、物体特定処理のフローチャートである。物体特定処理は、マッチング処理の結果に基づいて、トレー１３０内の対象物体ＯＢの位置と姿勢を特定する処理である。

【0061】

Ｓ３０５では、プロセッサ２１０は、Ｓ５０のマッチング処理にて探索されたＪ個の特定組み合わせから１つの組み合わせを選択する。選択された特定組み合わせを構成するモデル点ペアとシーン点ペアを、物体特定処理における注目モデルペアと注目シーン点ペアとする。

【0062】

Ｓ３１０では、注目モデル点ペアの座標系と、注目シーン点ペアの座標系と、を作成する。図１１は、点ペアＰＰｉの座標系の説明図である。図１１には、図９と同様に、点ペアＰＰｉを構成する第１点Ｐおよび第２点Ｑと、第１点Ｐの法線ベクトルｖＰｎと、第２点Ｑの法線ベクトルｖＱｎと、が図示されている。

【0063】

点ペアＰＰｉの座標系は、原点Ｏｐと、互いに直交する３個の座標軸Ｘｐ、Ｙｐ、Ｚｐを定義する３個の単位ベクトルｖＸＬ、ｖＹＬ、ｖＺＬと、を決定することによって、作成される。本実施例では、原点Ｏｐは、第１点Ｐと第２点Ｐとの中点に決定される。３個の単位ベクトルは、原点Ｏｐを始点とするベクトルである。座標軸Ｘｐを定義する単位ベクトルｖＸＬは、第１点Ｐから第２点Ｑに向かう方向を正方向とする。座標軸Ｚｐを定義する単位ベクトルｖＺＬは、法線ベクトルｖＰｎと法線ベクトルｖＱｎと和であるベクトル（ｖＰｎ＋ｖＱｎ）の座標軸Ｘｐと垂直な成分の方向を正方向とする。座標軸Ｙｐを定義する単位ベクトルｖＹＬは、該ベクトルｖＹＬと座標軸Ｘｐを定義する単位ベクトルｖＸＬとの外積が座標軸Ｚｐを定義する単位ベクトルｖＺＬと一致するように決定される。

【0064】

プロセッサ２１０は、注目モデル点ペアの座標系を定義するために、モデル座標系において、原点Ｏｐｍと、３個の単位ベクトルｖＸＬｍ、ｖＹＬｍ、ｖＺＬｍと、を決定する。プロセッサ２１０は、注目シーン点ペアの座標系を定義するために、シーン座標系において、原点Ｏｐｓと、３個の単位ベクトルｖＸＬｓ、ｖＹＬｓ、ｖＺＬｓと、を決定する。

【0065】

Ｓ３１５では、プロセッサ２１０は、注目モデル点ペアの座標系と、注目シーン点ペアの座標系と、の間の変換行列ＭＰを算出する。変換行列ＭＰは、モデル座標系とシーン座標系とが同一の座標系であるという条件下で、注目モデル点ペアの座標系を、変換行列ＭＰを用いて移動させた場合に、注目モデル点ペアの座標系と、注目シーン点ペアの座標系と、が一致するように、算出される。変換行列ＭＰは、同次変換行列である。同次変換行列は、回転と並進移動を含む座標変換を表現する４行４列の行列である。

【0066】

図１２は、変換行列ＭＰの算出式を示す図である。変換行列ＭＰは、図１２（Ａ）の式に示すように、第１変換行列Ｍｓと、第２変換行列Ｍｍの逆行列と、の積である。

【0067】

第１変換行列Ｍｓは、図１２（Ｂ）に示すように、４行４列の同次変換行列である。第１変換行列Ｍｓは、注目シーン点ペアの座標系を定義する同次変換行列である。第１－第３列は、回転を示している。第４列は、平行移動を示している。第１列の４個の成分は、注目シーン点ペアの座標系の座標軸Ｘｐを定義する単位ベクトルｖＸＬｓの３個の成分（ｒｓ００、ｒｓ１０、ｒｓ２０）とゼロとで構成される。第２列の４個の成分は、注目シーン点ペアの座標系の座標軸Ｙｐを定義する単位ベクトルｖＹＬｓの３個の成分（ｒｓ０１、ｒｓ１１、ｒｓ２１）とゼロとで構成される。第３列の４個の成分は、注目シーン点ペアの座標系の座標軸Ｚｐを定義する単位ベクトルｖＺＬｓの３個の成分（ｒｓ０２、ｒｓ１２、ｒｓ２２）とゼロとで構成される。第４列の４個の成分は、注目シーン点ペアの座標系の原点Ｏｐｓの座標（ｔｓｘ、ｔｓｙ、ｔｓｚ）と１とで構成される。

【0068】

第２変換行列Ｍｍは、図１２（Ｃ）に示すように、４行４列の同次変換行列である。第２変換行列Ｍｍは、注目モデル点ペアの座標系を定義する同次変換行列である。第１列の４個の成分は、注目モデル点ペアの座標系の座標軸Ｘｐを定義する単位ベクトルｖＸＬｍの３個の成分（ｒｍ００、ｒｍ１０、ｒｍ２０）とゼロとで構成される。第２列の４個の成分は、注目モデル点ペアの座標系の座標軸Ｙｐを定義する単位ベクトルｖＹＬｍの３個の成分（ｒｍ０１、ｒｍ１１、ｒｍ２１）とゼロとで構成される。第３列の４個の成分は、注目モデル点ペアの座標系の座標軸Ｚｐを定義する単位ベクトルｖＺＬｍの３個の成分（ｒｍ０２、ｒｍ１２、ｒｍ２２）とゼロとで構成される。第４列の４個の成分は、注目モデル点ペアの座標系の原点Ｏｐｍの座標（ｔｍｘ、ｔｍｙ、ｔｍｚ）と１とで構成される。

【0069】

Ｓ３２０では、プロセッサ２１０は、全ての特定組み合わせを処理したか否かを判断する。未処理の特定組み合わせがある場合には（Ｓ３２０：ＮＯ）、プロセッサ２１０は、Ｓ３０５に戻る。全ての特定組み合わせが処理された場合には（Ｓ３２０：ＹＥＳ）、プロセッサ２１０は、Ｓ３２５に処理を進める。以上の説明から解るように、変換行列ＭＰは、Ｊ個の特定組み合わせのそれぞれについて算出されるので、Ｓ３２５に進んだ時点でＪ個の変換行列ＭＰが算出されている。

【0070】

Ｓ３２５では、算出された各変換行列ＭＰを用いて、モデル重心を含むモデル参照点をシーン座標系に投影する。図１３は、モデル参照点の投影の説明図である。図１３（Ａ）には、３個のモデル参照点、具体的には、モデル重心Ｐｇと、２個のモデル端点Ｐｕ、Ｐｂと、が図示されている。これらのモデル参照点は、モデル点群ＰＣｍの座標系であるモデル座標系において定義されている。モデル重心Ｐｇは、基準物体ＯＢｍの重心を示す。基準物体ＯＢｍは、直線ＡＸを軸とした回転対称性を有するので、モデル重心Ｐｇは、直線ＡＸ上に位置する。２個のモデル端点Ｐｕ、Ｐｂは、直線ＡＸ上の点であって、基準物体ＯＢｍの直線ＡＸの方向の両端の位置を示す点である。モデル参照点Ｐｇ、Ｐｕ、Ｐｂのモデル座標系における座標は、例えば、モデル点群ＰＣｍの生成に用いられた基準物体ＯＢｍのＣＡＤデータを用いて、予め算出され、モデル点群データとともに不揮発性記憶装置２３０に記録されている。モデル参照点Ｐｕ、Ｐｂを結ぶ線分を参照線分Ｌｍとも呼ぶ。モデル重心Ｐｇは、参照線分Ｌｍ上に位置している。

【0071】

モデル座標系の点Ｐｍ（例えば、モデル参照点Ｐｇ、Ｐｕ、Ｐｂ）をシーン座標系に投影して得られる投影点Ｐｍｓの座標は、図１２（Ｄ）の式によって算出される。図１２（Ｄ）において、（ｘｍ、ｙｍ、ｚｍ）はモデル座標系における点Ｐｍの座標である。（ｘｍｓ、ｙｍｓ、ｚｍｓ）はシーン座標系における投影点Ｐｍｓの座標である。図１２（Ｄ）に示すように、投影点Ｐｍｓの座標は、モデル座標系の点Ｐｍを変換行列ＭＰによって変換することによって算出される。

【0072】

図１３（Ｂ）には、モデル参照点Ｐｇ、Ｐｕ、Ｐｂをシーン座標系に投影した結果が示されている。Ｊ個の変換行列ＭＰが算出された場合には、投影によって、Ｊ本の投影線分Ｌｍｓがシーン座標系において決定される。一本の投影線分Ｌｍｓは、モデル参照点Ｐｕ、Ｐｂを１つの変換行列ＭＰによって変換して得られる投影参照点Ｐｕｓ、Ｐｕｂを結ぶ線文である。投影線分Ｌｍｓ上には、モデル重心Ｐｇを変換行列ＭＰによって変換して得られる投影重心Ｐｇｓが位置している。換言すれば、投影線分Ｌｍｓは、モデル座標系の参照線分Ｌｍ（図１３（Ａ））を変換行列ＭＰによってシーン座標系に投影して得られる線分である。図１３（Ａ）に示す複数の投影線分Ｌｍｓは、Ｓ３１５にて算出済みのＪ個の変換行列ＭＰと一対一で対応している。このために、投影される投影線分Ｌｍｓの本数は、Ｊ本である。

【0073】

１つ投影線分Ｌｍｓは、変換行列ＭＰによってシーン座標系に投影されるモデル座標系における対象物体ＯＢの位置と姿勢を示している。変換行列ＭＰの算出に用いられる特定組み合わせのマッチングが正確に行われている場合には、投影線分Ｌｍｓは、シーン点群ＰＣｓによって示される対象物体ＯＢの位置と姿勢を正確に示す。例えば、投影線分Ｌｍｓ上の投影重心Ｐｇｓは、対象物体ＯＢの位置を示す。投影線分Ｌｍｓの方向は、対象物体ＯＢの姿勢を示す。実際には、Ｊ本の投影線分Ｌｍｓに対応するＪ個の特定組み合わせの中には、信頼性の低い不正確なマッチングが含まれ得る。換言すれば、Ｊ本の投影線分ＬｍｓおよびＪ本の投影線分Ｌｍｓに対応するＪ個の特定組み合わせは、それぞれ、シーン点群ＰＣｓによって示される対象物体ＯＢの位置と姿勢の候補を示す。以下では、Ｊ本の投影線分Ｌｍｓに対応するＪ個の特定組み合わせの信頼性を評価して、シーン点群ＰＣｓによって示される物体群に含まれる対象物体ＯＢの位置と姿勢を最終的に特定するための処理を行われる。

【0074】

Ｓ３３０では、シーン座標系に投影されたＪ個の投影重心Ｐｇｓに基づいて、Ｊ個の特定組み合わせを１以上の大グループに分類する。例えば、プロセッサ２１０は、Ｊ個の投影重心Ｐｇｓから、１個の注目重心をランダムで選択する。プロセッサ２１０は、注目重心から所定の閾値ＴＨｒ以内に存在する１以上の投影重心Ｐｇｓを１つのグループにまとめる。プロセッサ２１０は、注目点の選択とグループの作成とを、Ｊ個の投影重心Ｐｇｓの全てがいずれかのグループに分類されるまで繰り返す。プロセッサ２１０は、１つのグループに属する１以上の投影重心Ｐｇｓを有する１以上の投影線分Ｌｍｓを１つの大グループに分類する。Ｊ個の投影線分ＬｍｓとＪ個の特定組み合わせとは一対一で対応しているので、投影線分Ｌｍｓを大グループに分類することは、特定組み合わせを大グループに分類することと等価である。

【0075】

図１３（Ｂ）の例では、破線の円Ｃ１内の５個の投影重心Ｐｇｓを有する投影線分Ｌｍｓが１つの大グループにまとめられ、破線の円Ｃ２内の４個の投影重心Ｐｇｓを有する投影線分Ｌｍｓが１つの大グループにまとめられる。１つの対象物体ＯＢの位置と姿勢を示す複数の投影線分Ｌｍｓが、１つの大グループにまとめられるように、所定の閾値ＴＨｒは実験的に調整されている。例えば、図１３（Ｂ）では、右側の対象物体ＯＢ１の位置と姿勢を示す５個の投影線分Ｌｍｓが１つの大グループにまとめられ、左側の対象物体ＯＢ２の位置と姿勢を示す４個の投影線分Ｌｍｓが１つの大グループにまとめられている。

【0076】

Ｓ３３０では、シーン座標系に投影された投影線分Ｌｍｓに基づいて、各大グループに属する特定組み合わせを１以上の小グループに分類する。例えば、プロセッサ２１０は、上述した各大グループに属する１以上の投影線分Ｌｍｓから、１個の注目線分をランダムで選択する。プロセッサ２１０は、注目線分との間の角度θが所定の閾値ＴＨθ以内に存在する１以上の投影線分Ｌｍｓを１つの小グループにまとめる。プロセッサ２１０は、注目線分の選択と小グループの作成とを、大グループに属する投影線分Ｌｍｓの全てがいずれかの小グループに分類されるまで繰り返す。これによって、各大グループに属する特定組み合わせは、１以上の小グループに分類される。

【0077】

図１３（Ｂ）の例では、対象物体ＯＢ１に対応する５個の投影線分Ｌｍｓは、小グループＳｇ１、Ｓｇ２に分類され、対象物体ＯＢ１に対応する４個の投影線分Ｌｍｓは、小グループＳｇ３、Ｓｇ４に分類されている。

【0078】

Ｓ３４０では、プロセッサ２１０は、Ｓ３３０にて作成された大グループの中から、１つの注目大グループを選択する。Ｓ３４５では、プロセッサ２１０は、注目大グループに属する各小グループの代表変換行列を取得する。代表変換行列は、小グループを作成する際に、最初にランダムに選択された投影線分Ｌｍｓに対応する変換行列ＭＰである。投影線分Ｌｍｓに対応する変換行列ＭＰは、モデル座標系における参照線分Ｌｍ（図１３（Ａ））を、シーン座標系における投影線分Ｌｍｓに変換する際に用いられた変換行列ＭＰを意味する。

【0079】

Ｓ３５０では、プロセッサ２１０は、各小グループの評価値ＭＶを算出する。例えば、小グループの評価値は、小グループに代表変換行列を用いてモデル座標系からシーン座標系に投影されるモデル点群ＰＣｍ（すなわち、基準物体ＯＢｍ）と、シーン点群ＰＣｓによって示される対象物体ＯＢと、の間のズレの大きさの評価値である。本実施例では、プロセッサ２１０は、投影済の基準物体ＯＢｍを示す複数の点ＰＴのそれぞれに関して、シーン点群ＰＣｓの複数の点ＰＴのうちの最も近い点ＰＴとの距離Ｄｍｓを算出する。投影済の基準物体ＯＢｍが、シーン点群ＰＣｓの対象物体ＯＢと一致する場合、基準物体ＯＢｍの各点ＰＴについて算出される距離Ｄｍｓは、小さくなる。投影済の基準物体ＯＢｍとシーン点群ＰＣｓの対象物体ＯＢとの間のズレが大きい場合、基準物体ＯＢｍの各点ＰＴについて算出される距離Ｄｍｓは、大きくなる。プロセッサ２１０は、基準物体ＯＢｍの各点ＰＴについて算出される距離Ｄｍｓの平均値を、評価値ＭＶとして算出する。評価値ＭＶが小さいほど、投影済の基準物体ＯＢｍとシーン点群ＰＣｓの対象物体ＯＢとの間のズレが小さい。

【0080】

Ｓ３５５では、プロセッサ２１０は、各小グループの評価値ＭＶに基づいて、１以上の小グループの中から、１つの注目大グループに対応付けるべき１つの特定組み合わせを決定する。プロセッサ２１０は、１以上の小グループのうち、最良の評価値（本実施例では、最小の評価値）を有する小グループを決定する。図１３の例では、注目大グループが対象物体ＯＢ１に対応する大グループ（投影重心Ｐｇｓが円Ｃ１内に位置するグループ）である場合には、小グループＳｇ１が最良の評価値を有する。また、注目大グループが対象物体ＯＢ２に対応する大グループ（投影重心Ｐｇｓが円Ｃ２内に位置するグループ）である場合には、小グループＳｇ４が最良の評価値を有する。プロセッサ２１０は、決定された小グループの代表変換行列に対応する特定組み合わせを、注目大グループに対応付けるべき特定組み合わせとして決定する。例えば、決定された特定組み合わせを用いて得られる変換行列ＭＰを用いて、参照線分Ｌｍをシーン座標系に投影することで、大グループに対応する対象物体ＯＢの位置と姿勢を示す投影線分Ｌｍｓが得られる。そして、大グループは１つの対象物体ＯＢと対応している。このために、大グループに対応付けるべき１つの特定組み合わせを最終的に決定することは、１つの対象物体ＯＢの位置と姿勢を特定することと等価である。

【0081】

Ｓ３６０では、プロセッサ２１０は、全ての大グループを注目大グループとして処理したか否かを判断する。未処理の大グループがある場合には（Ｓ３６０：ＮＯ）、プロセッサ２１０は、Ｓ３４０に戻る。全ての大グループが処理された場合には（Ｓ３６０：ＹＥＳ）、プロセッサ２１０は、物体特定処理を終了する。この時点で、全ての大グループに対応付けるべき特定組み合わせ（変換行列ＭＰ）が決定しているので、シーン点群ＰＣｓによって示される全ての対象物体ＯＢの位置と姿勢が特定されている。

【0082】

物体特定処理が終了すると、図２のＳ７０では、プロセッサ２１０は、ロボット制御処理を実行する。ロボット制御処理は、特定処理機能にて特定済みの対象物体ＯＢの位置と姿勢を使用してロボットアーム１２０を制御することによって、ロボットアーム１２０に対象物体ＯＢを把持させる処理である。本実施例では、プロセッサ２１０は、特定済みの１以上の対象物体ＯＢのうち、最も上方に位置する対象物体ＯＢを把持対象に決定する。プロセッサ２１０は、把持対象の対象物体ＯＢを複数の指１２６を用いて把持するようにロボットアーム１２０を制御する。そして、プロセッサ２１０は、例えば、ロボットアーム１２０を制御して、把持された対象物体ＯＢを、予め決められた位置に移動させる。これによって、図２の処理は終了される。プロセッサ２１０は、例えば、図２の処理を繰り返し実行することによって、トレー１３０上にばら積みされた複数の対象物体ＯＢを、１個ずつ、予め決められた位置に移動させることができる。

【0083】

以上説明した本実施例によれば、プロセッサ２１０は、基準となる対象物体（基準物体ＯＢｍ）を示すモデル点群ＰＣｍ（図３（Ａ））を示すモデル点群データＰＤｍと、対象物体ＯＢを含む物体群を示すシーン点群ＰＣｓ（図３（Ｂ））を示すシーン点群データＰＤｓと、を取得する（図２のＳ１０、Ｓ２０）。プロセッサ２１０は、モデル点群ＰＣｍの一部である複数個のモデル対象点ＰＴｍ（図７（Ａ））のそれぞれと、シーン点群ＰＣｓの一部である複数個のシーン対象点ＰＴｓ（図７（Ｂ））のそれぞれと、の局所的特徴量を取得する（図４のＳ１５０、Ｓ１８０）。プロセッサ２１０は、局所的特徴量に基づいて複数個のモデル対象点ＰＴｍと複数個のシーン対象点ＰＴｓとを複数個の類似グループに分類する（図４のＳ１６０、Ｓ１９０、図７（Ａ）、（Ｂ））。プロセッサ２１０は、モデル点ペアと、モデル点ペアに対応する複数個のシーン点ペアと、を決定する（図８のＳ２２０、Ｓ２４０）。モデル点ペアと、モデル点ペアに対応するシーン点ペアとは、同じグループペアに属する（図８のＳ２２０、Ｓ２４０）。すなわち、シーン点ペアを構成する２個の点のそれぞれは、モデル点ペアを構成する２個の点のうちの対応する点と同一の類似グループに属する。プロセッサ２１０は、モデル点ペアとシーン点ペアとのそれぞれの大局的特徴量を取得する（図８のＳ２７０）。プロセッサ２１０は、プロセッサ２１０は、モデル点ペアとシーン点ペアとのマッチングを行う（図８のＳ２８０）。すなわち、モデル点ペアに対応する複数個のシーン点ペアの中から、モデル点ペアが有する大局的特徴量と同一または類似する大局的特徴を有する特定の点セットが探索される。プロセッサ２１０は、マッチングの結果（探索の結果）を用いて、特定処理（図２のＳ７０）を実行する。本実施例の特定処理は、ロボットアーム１２０を用いて、対象物体ＯＢの把持と移動とを行うロボット制御処理である。

【0084】

この結果、局所的特徴量に基づいて、大局的特徴量を用いたマッチングに適したモデル点ペアやシーン点ペアを決定できる。このために、マッチングの際にマッチングの精度を向上できる。換言すれば、シーン点ペアの中から、モデル点ペアが有する大局的特徴量と同一または類似する大局的特徴を有するシーン点ペアを探索する際に、探索の精度を向上できる。したがって、探索の結果を用いて特定処理（例えば、ロボット制御処理）を実行する際に、適切な特定処理を実行することができる。また、大局的特徴量を用いた探索に適さない余分な複数個のシーン点ペアやモデル点ペアが決定されることを抑制できるので、探索の計算時間を抑制できる。

【0085】

例えば、仮に、局所的特徴量による分類を行わずに、総あたりでモデル点ペアやシーン点ペアを決定するとする。この場合には、例えば、決定されるモデル点ペアは、対象物体の特徴を適切に示さないモデル点ペアを含みやすい。また、１つのモデル点ペアとマッチングすべきシーン点ペアの個数が過剰に多くなりやすい。本実施例によれば、このような不都合を抑制して、過度に計算時間を増加させることなく、マッチング精度を向上できる。

【0086】

さらに、本実施例によれば、プロセッサ２１０は、ｍ個のグループＧ１～Ｇ４のうちの２個のグループから１つずつ選択することによって、モデル点ペアおよびシーン点ペアトを決定する。すなわち、本実施例では、１つのグループ（例えば、グループＧ１）から２個の点を選択して点ペアを構成することはない。この結果、対象物体ＯＢの互いに特徴が異なる部分から選択された点ペアが用いられるので、例えば、対象物体ＯＢの互いに類似した特徴を有する部分から選択された点ペアが用いられる場合よりも、点ペアが対象物体ＯＢの全体の特徴を反映しやすい。この結果、マッチングの精度をさらに向上することができる。

【0087】

さらに、本実施例によれば、プロセッサ２１０は、注目グループペアに属する注目モデル点ペアごとに、それぞれ、距離条件を決定し（Ｓ２３０）、該距離条件を満たすシーン点ペアが決定される（Ｓ２４０）。このように、グループペアが異なれば、用いられる距離条件も異なる。すなわち、第１のグループペア（例えば、グループＧ１、Ｇ２のペア）に属するモデル点ペアに対応するシーン点ペアが満たすべき第１距離条件と、第２のグループペア（例えば、グループＧ２、Ｇ３のペア）に属するモデル点ペアに対応するシーン点ペアが満たすべき第２距離条件と、は互いに異なる条件である。この結果、よりマッチングに適した複数個のシーン点ペアが決定される。例えば、グループペアによって異なる距離条件が用いられることで、マッチングする可能性が低いシーン点ペア（例えば、２個の対象物体ＯＢを跨ぐ点ペア）や、誤ったマッチングにつながりやすいシーン点ペアが、マッチングすべきシーン点ペアとして決定されることを抑制できる。この結果、マッチングの計算時間の短縮や、マッチングの精度向上を実現できる。

【0088】

なお、本実施例では、注目グループペアに属する注目モデル点ペアごとに異なる距離条件が決定されているが、これに代えて、注目グループペアごとに１つの距離条件が決定あれても良い。例えば、注目グループペアに属する複数個のモデル点ペアの平均距離に基づいて、注目グループペアの１つの距離条件が決定されても良い。この場合であっても、第１のグループペア（例えば、グループＧ１、Ｇ２のペア）に属するモデル点ペアに対応するシーン点ペアが満たすべき第１距離条件と、第２のグループペア（例えば、グループＧ２、Ｇ３のペア）に属するモデル点ペアに対応するシーン点ペアが満たすべき第２距離条件と、は互いに異なる条件になるので、マッチングの計算時間の短縮や、マッチングの精度向上を実現できる。

【0089】

さらに、本実施例の大局的特徴量は、点ペアＰＰｉを構成する２個の点Ｐ、Ｑの間の距離Ｄと、点ペアＰＰｉを構成する２個の点Ｐ、Ｑの法線ベクトルｖＰｎ、ｖＱｎに基づく量Ｆ１～Ｆ３と、を含む（図９）。プロセッサ２１０は、モデル点ペアを構成する２個のモデル対象点ＰＴｍの間の距離Ｄｍに基づいて距離条件を決定し（図８のＳ２３０）、シーン点ペアの候補となる２個のシーン対象点ＰＴｓから成る点ペアのうち、距離条件を満たすペアをシーン点ペアとして決定する（図８のＳ２４０）。プロセッサ２１０は、決定されたシーン点ペアについて、法線ベクトルに基づく量Ｆ１～Ｆ３を算出する（図８のＳ２７０）。この結果、大局的特徴量の複数個のパラメータＤ、Ｆ１～Ｆ３のうち、法線ベクトルに基づく量Ｆ１～Ｆ３は、距離条件を満たすシーン点ペアについてのみ算出されるので、マッチングする可能性が低いシーン点ペアについて量Ｆ１～Ｆ３が無駄に計算されることを抑制して、大局的特徴量を効率良く算出できる。

【0090】

さらに、上記実施例によれば、複数個の類似グループＧ１～Ｇ４の代表点ＲＰ１～ＲＰ４の間の局所的特徴量の差異は、基準値以上である（図４のＳ１１０～Ｓ１３０）。この結果、局所的特徴量の差異が比較的大きな２個の点からなる点ペアが、モデル点ペアおよびシーン点ペアとして決定される。したがって、マッチングにより適したモデル点ペアおよびシーン点ペアを決定できる。

【0091】

さらに、上記実施例によれば、モデル点ペアとシーン点ペアとのマッチングによって、Ｊ個の特定組み合わせが決定される場合に、プロセッサ２１０は、図１０の物体特定処理において、Ｊ個の特定組み合わせをＪ個未満の小グループに分類し（図１０のＳ３３５）、小グループごとに特定組み合わせの信頼性を評価し（図１０のＳ３５０）、評価結果に基づいてＪ個未満の小グループの中から選択される一部のグループに属する特定組み合わせを用いて、対象物体ＯＢの位置と姿勢とを特定する（図１０のＳ３５５）。上記構成によれば、小グループに分類して特定組み合わせの信頼性を評価するので、対象物体ＯＢの位置と姿勢とを効率良く、適切に特定することができる。

【0092】

さらに、上記実施例によれば、プロセッサ２１０は、Ｊ個の特定組み合わせのそれぞれに基づいて、モデル座標系の複数個のモデル参照点Ｐｇ、Ｐｕ、Ｐｂをシーン座標系に投影し（図１０のＳ３２５）、シーン座標系に投影された複数個の投影参照点Ｐｇｓ、Ｐｕｓ、Ｐｂｓを用いて、Ｊ個の特定組み合わせをＪ個未満の小グループに分類する（図１０のＳ３３０、Ｓ３３５）。プロセッサ２１０は、モデル座標系のモデル点群ＰＣｍをシーン座標系に投影して得られる複数個の投影点を用いて、小グループごとにマッチングの信頼性を評価する（図１０のＳ３５５）。この結果、モデル座標系の複数個の点をシーン座標系に投影した複数個の投影点を用いて、Ｊ個の特定組み合わせの分類と信頼性の評価とを適切に実行できる。

【0093】

さらに、本実施例の投影参照点Ｐｇｓ、Ｐｕｓ、Ｐｂｓは、対象物体ＯＢの重心を示す投影重心Ｐｇｓを含む３個の点を含む。投影重心Ｐｇｓに基づいてシーン座標系における対象物体ＯＢの位置が特定され、３個の投影点Ｐｇｓ、Ｐｕｓ、Ｐｂｓ（すなわち、投影線分Ｌｍｓ）に基づいて対象物体ＯＢの姿勢が特定される。この結果、シーン座標系における対象物体ＯＢの位置と姿勢を３個の投影点に基づいて適切に特定できる。

【0094】

以上の説明から解るように、本実施例のモデル点群データＰＤｍは、第１点群データの例であり、シーン点群データＰＤｓは、第２点群データの例である。本実施例のモデル対象点ＰＴｍは、第１対象点の例であり、シーン対象点ＰＴｓは、第２対象点の例である。モデル点ペアは、第１点セットの例であり、シーン点ペアは、第２点セットの例である。

【0095】

Ｂ．変形例：
（１）上記実施例では、局所的特徴量として、ＳＨＯＴ特徴量が採用されている。これに代えて、別の種類の局所的特徴量が採用されても良い。例えば、ＰＦＨ（Point Feature Histogram）、ＨＯＮＶ（Histogram of Oriented Normal Vector）、ＬＳＰ（Local Surface Patches）、ＤｏＮ（Difference of Normals）が採用されても良い

【0096】

（２）上記実施例では、大局的特徴量は、点ペアの特徴を示す量である。変形例では、３個以上の点セットの特徴を示す大局的特徴量が採用されても良い。例えば、３点の点セットの特徴を示す大局的特徴量としては、ＶＰＦ（Vector Pair Feature）、ＲＰＤ（Relative Point Density）等が採用されても良い。

【0097】

（３）上記実施例では、モデル点ペアやシーン点ペアは、１つの類似グループから選択される２個の点からなる点ペアを含まない。これに代えて、モデル点ペアやシーン点ペアは、１つの類似グループから選択される２個の点からなる点ペアを含んでも良い。

【0098】

（４）上記実施例では、グループペアに属するモデル点ペアごとに距離条件が定められ、該距離条件を満たすシーン点ペアがマッチングの対象として決定される。これに代えて、グループペアごとに距離条件が定められても良いし、１つの対象物体ＯＢで１つの距離条件（例えば、対象物体ＯＢの軸線方向の全長に基づく距離条件）が定められても良い。

【0099】

（５）上記実施例では、基準物体ＯＢｍを示すモデル点群ＰＣｍの代表点ＲＰの個数は、４個である。これに限らず、代表点ＲＰの個数は、２以上の任意の個数であって良い。また、複数個の代表点ＲＰは、互いに局所的特徴量が類似する２個の点を含んでも良い。この場合であっても、例えば、複数個のモデル対象点ＰＴｍおよびシーン対象点ＰＴｓは、最も近い局所的特徴量を有する代表点に対応する１つの類似グループに分類されれば良い。

【0100】

（６）上記実施例では、モデル点ペアとシーン点ペアとのマッチングによって得られたＪ個の特定組み合わせは、小グループに分類され、小グループごとに信頼性が評価される。これに代えて、Ｊ個の特定組み合わせは、グループに分類されることなく、１つずつ信頼性が評価されても良い。この場合には、プロセッサ２１０は、最も信頼性が高い特定組み合わせに基づいて、対象物体ＯＢの位置と姿勢を特定すれば良い。

【0101】

（７）上記実施例では、プロセッサ２１０は、マッチング結果を用いて、モデル重心Ｐｇを含む３個のモデル参照点Ｐｇ、Ｐｕ、Ｐｂをシーン座標系に投影し、投影によって得られる３個の投影参照点Ｐｇｓ、Ｐｕｓ、Ｐｂｓによって対象物体ＯＢの位置と姿勢を特定している。これに限らず、４個以上の投影点に基づいて、対象物体ＯＢの位置と姿勢を特定しても良いし、重心を含まない３以上の投影点に基づいて、対象物体ＯＢの位置と姿勢を特定しても良い。

【0102】

（８）複数個の点ＰＴに基づいて法線ベクトルや局所座標系を決定する方法は、主成分分析を使用する方法に限らず、他の種々の方法であってよい。例えば、プロセッサ２１０は、いわゆる最小二乗法に従って、複数の点を近似する近似平面を決定して、該近似平面に垂直な方向に法線ベクトルを決定してもよい。そして、プロセッサ２１０は、決定された平面に基づいて、法線ベクトルを算出してよい。

【0103】

（９）対象物体ＯＢの位置と姿勢との特定結果を用いて行われる処理は、ロボット制御処理に限らず、他の様々な処理であって良い。例えば、製品上に取り付けられた部品の位置と姿勢とを特定し、該製品上において部品が正しい位置に正しい姿勢で取り付けられているか否かを検査する処理が実行されても良い。

【0104】

（１０）点群データを処理する装置の構成は、図１のデータ処理装置２００の構成に代えて、他の種々の構成であってよい。例えば、ネットワークを介して互いに通信可能な複数の装置（例えば、コンピュータ）が、実施例のデータ処理装置２００が実行する処理を一部ずつ分担して実行して、全体として、データ処理装置２００が実行する処理を実行してもよい。

【0105】

上記各実施例において、ハードウェアによって実現されていた構成の一部をソフトウェアに置き換えるようにしてもよく、逆に、ソフトウェアによって実現されていた構成の一部あるいは全部をハードウェアに置き換えるようにしてもよい。例えば、局所的特徴量や大局特徴量の算出の機能を、専用のハードウェア回路によって実現してもよい。

【0106】

また、本発明の機能の一部または全部がコンピュータプログラムで実現される場合には、そのプログラムは、コンピュータ読み取り可能な記録媒体（例えば、一時的ではない記録媒体）に格納された形で提供することができる。プログラムは、提供時と同一または異なる記録媒体（コンピュータ読み取り可能な記録媒体）に格納された状態で、使用され得る。「コンピュータ読み取り可能な記録媒体」は、メモリーカードやＣＤ－ＲＯＭのような携帯型の記録媒体に限らず、各種ＲＯＭ等のコンピュータ内の内部記憶装置や、ハードディスクドライブ等のコンピュータに接続されている外部記憶装置も含み得る。

【0107】

以上、実施例、変形例に基づき本発明について説明してきたが、上記した発明の実施の形態は、本発明の理解を容易にするためのものであり、本発明を限定するものではない。本発明は、その趣旨を逸脱することなく、変更、改良され得ると共に、本発明にはその等価物が含まれる。

【符号の説明】

【0108】

１１０…三次元センサ,１２０…ロボットアーム,１３０…トレー,２００…データ処理装置,２１０…プロセッサ,２１５…記憶装置,２２０…揮発性記憶装置,２３０…不揮発性記憶装置,２４０…表示部,２５０…操作部,２７０…通信インタフェース,ＯＢ…対象物体,ＯＢｍ…基準物体,ＰＣｍ…モデル点群,ＰＣｓ…シーン点群,ＰＤｍ…モデル点群データ,ＰＤｓ…シーン点群データ,ＰＧ…コンピュータプログラム,ＰＰｉ…点ペア,ＰＴ…点,ＰＴｍ…モデル対象点,ＰＴｓ…シーン対象点,Ｐｇ,Ｐｕ,Ｐｂ…モデル参照点,Ｐｇｓ,Ｐｕｓ,Ｐｂｓ…モデル参照点,ＲＰ…代表点

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】