ホーム > 特許ランキング > 国立大学法人九州工業大学
知財求人 - 知財ポータルサイト「IP Force」
▶ 国立大学法人九州工業大学の特許一覧 ▶ 株式会社安川電機の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】6192088
(24)【登録日】2017年8月18日
(45)【発行日】2017年9月6日
(54)【発明の名称】物体検出方法および物体検出装置
(51)【国際特許分類】
G06T 7/70 20170101AFI20170828BHJP
G06T 1/00 20060101ALI20170828BHJP
G06T 7/00 20170101ALI20170828BHJP
G01B 11/00 20060101ALI20170828BHJP
G01B 11/26 20060101ALI20170828BHJP
【ＦＩ】
G06T7/70
G06T1/00 300
G06T7/00 C
G01B11/00 A
G01B11/26 Z
【請求項の数】6
【全頁数】22
(21)【出願番号】特願2013-30667(P2013-30667)
(22)【出願日】2013年2月20日
(65)【公開番号】特開2014-160370(P2014-160370A)
(43)【公開日】2014年9月4日
【審査請求日】2015年12月25日
(73)【特許権者】
【識別番号】504174135
【氏名又は名称】国立大学法人九州工業大学
(73)【特許権者】
【識別番号】000006622
【氏名又は名称】株式会社安川電機
(74)【代理人】
【識別番号】100089118
【弁理士】
【氏名又は名称】酒井 宏明
(72)【発明者】
【氏名】江島 俊朗
(72)【発明者】
【氏名】榎田 修一
(72)【発明者】
【氏名】定野 真和
(72)【発明者】
【氏名】出口 央
(72)【発明者】
【氏名】堀内 智之
(72)【発明者】
【氏名】河野 寿之
【審査官】
佐藤 卓馬
(56)【参考文献】
【文献】
定野真和、外６名，サンプル点間の特徴を用いたＨｏｕｇｈ変換に基づくＳｕｒｆａｃｅ Ｍａｔｃｈｉｎｇ法，ＳＳＩＩ２０１１ 第１７回 画像センシングシンポジウム講演論文集，日本，画像センシング技術研究会，２０１１年 ６月 ８日，第IS2-15-1頁−第IS2-15-5頁
【文献】
木村 彰男、外１名，高速一般化ハフ変換−相似変換不変な任意図形検出法−，電子情報通信学会論文誌，日本，社団法人電子情報通信学会，１９９８年 ４月２５日，第４号，第７２６頁−第７３４頁
(58)【調査した分野】（Int.Cl.，ＤＢ名）
Ｇ０６Ｔ ７／７０
Ｇ０１Ｂ １１／００
Ｇ０１Ｂ １１／２６
Ｇ０６Ｔ １／００
Ｇ０６Ｔ ７／００
(57)【特許請求の範囲】
【請求項1】
物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する工程と、
前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する工程と、
実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する工程と、
前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する工程と
を含むことを特徴とする物体検出方法。
前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する工程と、
実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する工程と、
前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する工程と
を含むことを特徴とする物体検出方法。
【請求項2】
前記変換後の位置の組に含まれる前記内部基準点の位置が物体の内部にある場合に、該位置の組に含まれる複数の前記外部基準点の位置と一致する実空間内の投票点へそれぞれ投票する工程と、
一つの前記位置の組に含まれる複数の前記外部基準点の位置に基づいて投票が行われた投票点の組の中で、投票数が最小の投票点の得票数が所定の閾値に達した場合に、該投票点の組をリストアップする工程と、
リストアップした前記投票点の組毎に総得票数を集計し、前記総得票数が多い組の順に該組に含まれる前記投票点の実空間における位置に基づいて、前記物体の位置および姿勢を推定する工程と
をさらに含むことを特徴とする請求項１に記載の物体検出方法。
一つの前記位置の組に含まれる複数の前記外部基準点の位置に基づいて投票が行われた投票点の組の中で、投票数が最小の投票点の得票数が所定の閾値に達した場合に、該投票点の組をリストアップする工程と、
リストアップした前記投票点の組毎に総得票数を集計し、前記総得票数が多い組の順に該組に含まれる前記投票点の実空間における位置に基づいて、前記物体の位置および姿勢を推定する工程と
をさらに含むことを特徴とする請求項１に記載の物体検出方法。
【請求項3】
前記物体および前記モデルのエッジを検出する工程と、
前記モデルの表面に位置する点群および前記物体の表面に位置する前記サンプル点群を前記エッジ上の点と、面上の点とに分類する工程と、
前記物体の形状に応じて、前記始点として選択する点と前記終点として選択する点とを前記エッジ上の点および前記面上の点から選択する工程と
をさらに含むことを特徴とする請求項１または２に記載の物体検出方法。
前記モデルの表面に位置する点群および前記物体の表面に位置する前記サンプル点群を前記エッジ上の点と、面上の点とに分類する工程と、
前記物体の形状に応じて、前記始点として選択する点と前記終点として選択する点とを前記エッジ上の点および前記面上の点から選択する工程と
をさらに含むことを特徴とする請求項１または２に記載の物体検出方法。
【請求項4】
前記サンプル点群から選択する前記始点として、前記面上の点を選択する工程と、
前記サンプル点群から選択する前記終点として、前記エッジ上の点を選択する工程と
をさらに含むことを特徴とする請求項３に記載の物体検出方法。
前記サンプル点群から選択する前記終点として、前記エッジ上の点を選択する工程と
をさらに含むことを特徴とする請求項３に記載の物体検出方法。
【請求項5】
前記サンプル点群から選択する前記始点および終点として、前記始点における法線ベクトルと、前記終点における法線ベクトルまたは勾配ベクトルとのなす角度が所定の閾値より大きい２点を選択する工程
をさらに含むことを特徴とする請求項１〜４のいずれか一つに記載の物体検出方法。
をさらに含むことを特徴とする請求項１〜４のいずれか一つに記載の物体検出方法。
【請求項6】
物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する設定部と、
前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する記憶部と、
実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する演算部と、
前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する推定部と
を備えることを特徴とする物体検出装置。
前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する記憶部と、
実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する演算部と、
前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する推定部と
を備えることを特徴とする物体検出装置。
【発明の詳細な説明】
【技術分野】
【0001】
開示の実施形態は、物体検出方法および物体検出装置に関する。
【背景技術】
【0002】
従来、距離センサによって取得される物体までの距離に基づいて、物体の位置および姿勢を検出する物体検出方法がある。例えば、特許文献１には、距離センサから物体のエッジ上に位置する点までの距離をハフ変換してハフ空間へ投票し、ハフ空間における得票数の多い点に基づいて物体の位置および姿勢を検出する物体検出方法が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特許第５０８８２７８号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、ハフ変換を用いた物体の検出方法では、ハフ空間への投票数が膨大であるため、物体の位置および姿勢の検出に要する処理量が嵩むという問題がある。
【0005】
実施形態の一態様は、上記に鑑みてなされたものであって、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる物体検出方法および物体検出装置を提供することを目的とする。
【課題を解決するための手段】
【0006】
実施形態の一態様に係る物体検出方法では、物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する。前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する。実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する。前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する。
【発明の効果】
【0007】
実施形態の一態様によれば、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる。
【図面の簡単な説明】
【0008】
【発明を実施するための形態】
【0009】
以下、添付図面を参照して、本願の開示する物体検出方法および物体検出装置の実施形態を詳細に説明する。なお、以下に示す実施形態によりこの発明が限定されるものではない。また、以下では、位置および姿勢の検出対象となる物体の形状が円筒形状の場合を例に挙げて説明するが、検出対象の物体の形状は円筒形状に限定されるものではない。
【0010】
図１Ａおよび図１Ｂは、実施形態に係る物体検出方法を示す説明図である。図１Ａに示すように、実施形態に係る物体検出方法では、検出対象の物体と同一形状で、実空間における位置および姿勢が既知のモデルＭの表面形状を示す情報を取得する。
【0011】
具体的には、モデルＭの表面に位置する点群の各点に関する情報を取得する。各点に関する情報には、各点の実空間における３次元の位置情報（位置ベクトル）、各点におけるモデルＭの表面に対する法線ベクトルの情報が含まれる。
【0012】
続いて、モデルＭの外部空間に、物体の位置および姿勢の推定材料となる複数の外部基準点Ｏ１、Ｏ２、Ｏ３を設定し、モデルＭの内部空間に、物体の位置および姿勢の推定材料の可否判定材料となる内部基準点Ｏ４を設定する。なお、設定する基準点の個数はこれに限定されるものではない。
【0013】
そして、モデルＭの表面に位置する点群から順次選択する始点Ｐｉおよび終点Ｐｊの対を含むモデルＭの局所表面の特徴量を算出する。ここでの特徴量は、例えば、始点Ｐｉおよび終点Ｐｊ間の距離、始点Ｐｉおよび終点Ｐｊの法線の内積、始点Ｐｉの法線と始点Ｐｉおよび終点Ｐｊを結ぶベクトルとの内積などである。
【0014】
続いて、算出した特徴量と、始点Ｐｉに対する外部基準点Ｏ１、Ｏ２、Ｏ３および内部基準点Ｏ４の位置ベクトル（始点Ｐｉから外部基準点Ｏ１、Ｏ２、Ｏ３、内部基準点Ｏ４へ向かう矢印参照）の組とを対応付けたテーブルを作成して記憶する。なお、これまでの工程は、実空間に存在する物体の位置および姿勢を検出する以前に、予めオフラインにて行っておく。
【0015】
その後、図１Ｂに示すように、実空間に存在する物体Ｗの表面の表面形状を示す情報を取得する。ここでは、図示を省略したが、物体Ｗの表面の表面形状を示す情報には、物体Ｗの表面に位置するサンプル点群の実空間における３次元の位置情報（位置ベクトル）が含まれる。
【0016】
続いて、サンプル点群から始点Ｐ（ｉ）および終点Ｐ（ｊ）の対を順次選択し、始点Ｐ（ｉ）および終点Ｐ（ｊ）の対を含む局所表面における物体Ｗの特徴量を算出し、先に記憶したテーブルから物体Ｗの特徴量と一致する特徴量が対応付けられた複数の位置ベクトルの組を取得する。
【0017】
ここで取得される位置ベクトルの始点ＰｉのモデルＭにおける位置が、サンプル群から選択した始点Ｐ（ｉ）の物体Ｗにおける位置と一致していれば、位置ベクトルの組を実空間の位置ベクトルへ変換することで、物体Ｗの正しい基準点位置を取得することができる。したがって、かかる変換した３点の位置ベクトルに基づいて、物体Ｗの正しい位置および姿勢を推定して検出することができる。
【0018】
ただし、テーブルから取得される位置ベクトルの始点ＰｉのモデルＭにおける位置は、サンプル群から選択した始点Ｐ（ｉ）の物体Ｗにおける位置と一致しない場合がある。例えば、物体Ｗの位置や姿勢によっては、物体Ｗにおける始点Ｐ（ｉ）の位置と、モデルＭにおける始点Ｐｉの位置とが異なる場合にも、物体Ｗにおける局所表面の特徴量とモデルＭにおける局所表面の特徴量とが一致することがある。
【0019】
かかる場合、テーブルから取得した位置ベクトルの組を実空間の位置ベクトルへ変換しても、物体Ｗの正しい基準点位置を取得することができず、物体Ｗの正しい位置および姿勢を検出することができない。
【0020】
このため、実施形態に係る物体検出方法では、ハフ変換を利用して、テーブルから取得した複数の位置ベクトルの組を実空間における位置ベクトルの組へ変換し、変換後の位置ベクトルが示す３次元位置をハフ空間となる実空間の投票点へ投票する。そして、３点ずつ投票される投票点の組のなかで、得票数の多い投票点の組に含まれる位置ベクトルを物体Ｗの正しい基準点の位置と判定し、かかる位置に基づいて物体Ｗの位置および姿勢を判定する。
【0021】
しかしながら、テーブルから取得した全ての位置ベクトルの組を投票の対象とした場合、物体Ｗの位置および姿勢を検出する処理量が嵩むという問題が生じる。また、前述したように、テーブルから取得した複数の位置ベクトルの組の中には、物体Ｗの位置および姿勢の推定材料として不適切な位置ベクトルも含まれている。
【0022】
さらに、物体Ｗの位置や姿勢等によっては、位置および姿勢の推定材料として不適切な位置ベクトルの投票点が、適切な位置ベクトルの投票点よりも多くの票を獲得することもあり、物体Ｗの位置および姿勢の検出精度が低下する恐れがある。
【0023】
そこで、実施形態に係る物体検出方法では、投票を行う前に、位置および姿勢の推定材料として不適切な位置ベクトルの組を投票の対象から除外することで、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させる。
【0024】
具体的には、テーブルから取得した位置ベクトルの組を実空間の位置ベクトルへ変換した場合に、図１Ｂに示すように、モデルＭに設定された外部基準点Ｏ１、Ｏ２、Ｏ３が実空間で３点Ａ、Ｂ、Ｃに、内部基準点Ｏ４が物体Ｗ外部の点Ｄに位置していたとする。
【0025】
このように、本来であれば物体Ｗの内部に存在するはずの内部基準点Ｏ４が物体Ｗ外部の点Ｄに存在した場合、かかる点Ｄと同じ組に含まれる実空間における３点Ａ、Ｂ、Ｃを投票対象、つまり、物体Ｗの位置および姿勢の推定材料から排除する。
【0026】
したがって、実施形態に係る物体検出方法によれば、投票を行う前に、位置および姿勢の推定材料として不適切な位置ベクトルの組を投票の対象から除外することができるので、物体Ｗの位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させる。
【0027】
なお、モデルＭに設定された内部基準点Ｏ４に対応する実空間の点Ｄが適切に物体Ｗの内部に存在していた場合、外部基準点Ｏ１、Ｏ２、Ｏ３に対応する実空間の３点Ａ、Ｂ、Ｃの位置を投票し、投票結果に基づいて物体Ｗの位置および姿勢を検出する。なお、かかる物体検出方法の詳細については、実施形態に係る物体検出装置の動作と合わせて後述する。
【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】
したがって、終点ＰｊとしてラベルＦが付加された点を限定して選択する構成によれば、モデルの面状における局所平面のそれぞれについて特徴量を取得することで、曲面部分の占める領域が広い物体の位置および姿勢の検出精度を向上させることができる。
【0053】
演算部７６は、サーフレット選択部７５から入力されるサーフレットペアを使用して、始点Ｐｉおよび終点Ｐｊの対を含むモデルＭの局所表面の特徴量を算出する。例えば、演算部７６は、始点Ｐｉおよび終点Ｐｊ間の距離、始点Ｐｉおよび終点Ｐｊの法線ベクトルｎｉの内積、始点Ｐｉの法線ベクトルｎｉと始点Ｐｉおよび終点Ｐｊを結ぶベクトルｆｉ，ｊとの内積を特徴量として算出する。
【0054】
さらに、演算部７６は、始点Ｐｉを原点とする局所座標系を算出し、局所座標系における基準点Ｏの位置ベクトル（図５Ｂに示す一点鎖線矢印参照）を算出する。基準点Ｏの位置ベクトルの算出には、始点Ｐｉの位置ベクトルｐｉ、終点Ｐｊの位置ベクトルｐｊ、位置ベクトルｐｉと位置ベクトルｐｊとの差分ベクトルｆｉ，ｊが用いられる。
【0055】
具体的には、実空間における各基準点Ｏの位置ベクトルは、下記の式（１）によって表すことができる。
【0056】
【数1】
【0057】
この実空間における各基準点Ｏの位置ベクトルを〈ｐｉ、ｎｉ、ｆｉ，ｊ〉により定まる局所座標系を用いて表す。局所座標系は、以下のように定める。
【0058】
【数2】
【0059】
上記式（２）、式（３）、式（４）によって算出される直交基底を用いて、各基準点Ｏの局所座標系における位置ベクトルは、下記の式（５）によって算出する。
【0060】
【数3】
【0062】
続いて、演算部７６は、図７に示すように、先に算出した始点Ｐｉおよび終点Ｐｊの対を含む局所表面の特徴量（Ｈ１、Ｈ２など）と、始点Ｐｉを原点とした局所座標系における基準点Ｏの位置ベクトル（ここでは、外部基準点Ｏ１、Ｏ２、Ｏ３、内部基準点Ｏ４の４つ位置ベクトル）の組とを対応付けたテーブル８１を作成する。
【0063】
ここで、モデルＭには、特徴量が一致する複数の局所表面が存在する。このため、テーブル８１では、一つの特徴量（Ｈ１、Ｈ２など）に対して、それぞれ局所座標系における複数の基準点Ｏの組が対応付けられる。つまり、テーブル８１は、各特徴量（Ｈ１、Ｈ２など）をキーとするハッシュテーブルのデータ構造となる。演算部７６は、こうして形成したテーブル８１を記憶部８へ記憶させる。
【0064】
また、物体検出装置４では、シーンデータ取得部７２と、エッジ検出部７４と、サーフレット選択部７５と、演算部７６と、位置・姿勢推定部７７と、把持対象情報生成部７８とによって、物体Ｗの位置および姿勢を推定して検出する。
【0065】
ここで、図８Ａ〜図１０を合わせて参照しながら、物体検出装置４が物体Ｗの位置および姿勢を検出する手順の一例について説明する。図８Ａ〜図１０は、実施形態に係る物体Ｗの位置および姿勢を検出する手順を示す説明図である。
【0066】
シーンデータ取得部７２は、センサ３から、図８Ａに示すバラ積みされた物体Ｗの３次元形状を示すシーンデータを取得する。具体的には、シーンデータ取得部７２は、図８Ｂに示すように、物体Ｗ表面に位置するサンプル点群の各点の実空間における３次元の位置情報（位置ベクトル）、各点における物体Ｗの表面に対する法線ベクトルｎ（ｉ）の情報等をシーンデータとして取得する。そして、シーンデータ取得部７２は、取得したシーンデータをエッジ検出部７４へ出力する。
【0067】
エッジ検出部７４は、シーンデータ取得部７２から入力されるシーンデータに基づいて、物体Ｗのエッジを検出する。さらに、エッジ検出部７４は、物体Ｗのエッジを形成するサンプル点群に対してエッジ上の点であることを示すラベルＥを付加し、ラベルＥを付加した各サンプル点の勾配ベクトルｇ（ｊ）を算出する。
【0068】
また、エッジ検出部７４は、物体Ｗのエッジ以外の部分を形成するサンプル点群に対して面上の点であることを示すラベルＦを付加する。そして、エッジ検出部７４は、ラベルＥとラベルＦとを付加したモデルデータをサーフレット選択部７５へ出力する。なお、エッジ検出部７４は、エッジ上のサンプル点の勾配ベクトルｇ（ｊ）についてもサーフレット選択部７５へ出力する。
【0069】
サーフレット選択部７５は、エッジ検出部７４から入力されるシーンデータから、物体Ｗの表面に位置する２点の対を始点Ｐ（ｉ）および終点Ｐ（ｊ）の対として順次選択し、選択したサンプル点に関するサーフレットを取得する。
【0070】
例えば、サーフレット選択部７５は、円筒形状の物体Ｗの周面に位置する始点Ｐ（ｉ）を選択する場合、始点Ｐ（ｉ）の実空間における位置ベクトルｐ（ｉ）、ラベルＦ、法線ベクトルｎ（ｉ）の組を始点Ｐ（ｉ）のサーフレットΔ（ｐｉ）として取得する。
【0071】
また、サーフレット選択部７５は、物体Ｗのエッジに位置する終点Ｐ（ｊ）を選択する場合、終点Ｐ（ｊ）の実空間における位置ベクトルｐ（ｊ）、ラベルＥ、終点Ｐ（ｊ）の勾配ベクトルｇ（ｊ）を終点Ｐ（ｊ）のサーフレットΔ（ｐｊ）として取得する。
【0072】
ここでの勾配ベクトルｇ（ｊ）は、終点Ｐ（ｊ）における物体Ｗの周面と端面とのなす角度である。なお、面上の点を終点として選択する場合、サーフレット選択部７５は、終点のサーフレットとして、終点の位置ベクトル、ラベルＦ、終点の法線ベクトルの組を取得する。
【0073】
サーフレット選択部７５は、選択する始点Ｐ（ｉ）および終点Ｐ（ｊ）を変更しながら、複数のサーフレットΔ（ｐｉ）とサーフレットΔ（ｐｊ）との対であるサーフレットペアを取得し、演算部７６へ出力する。
【0074】
かかるサーフレットペアは、演算部７６が始点Ｐ（ｉ）および終点Ｐ（ｊ）の対を含む物体Ｗの局所表面の特徴量を演算する処理などで使用される情報である。したがって、モデルＭのサーフレットペアを取得する場合と同様に、サーフレット選択部７５によって、全ての点を始点Ｐ（ｉ）および終点Ｐ（ｊ）として選択してサーフレットペアを取得することで、物体Ｗの検出精度を向上させることができる。
【0075】
また、ラベルＥが付加された終点Ｐｊをサーフレット選択部７５によって限定して選択させることにより、平面部分の占める領域が広い物体について、処理量を低減しつつ、物体の位置および姿勢の検出精度を向上させることができる。一方、ラベルＦが付加された終点Ｐｊをサーフレット選択部７５によって限定して選択させることにより、曲面部分の占める領域が広い物体について、位置および姿勢の検出精度を向上させることができる。
【0076】
演算部７６は、サーフレット選択部７５から入力されるサーフレットペアを使用して、始点Ｐ（ｉ）および終点Ｐ（ｊ）の対を含む物体Ｗの局所表面の特徴量を算出する。例えば、演算部７６は、始点Ｐ（ｉ）および終点Ｐ（ｊ）間の距離、始点Ｐ（ｉ）および終点Ｐ（ｊ）の法線ベクトルｎ（ｉ）の内積、始点Ｐ（ｉ）の法線ベクトルｎ（ｉ）と始点Ｐ（ｉ）および終点Ｐ（ｊ）を結ぶベクトルｆ（ｉ，ｊ）との内積を特徴量として算出する。そして、演算部７６は、特徴量の算出に使用したサーフレットペアと、算出した物体Ｗの局所表面の特徴量とを位置・姿勢推定部７７へ出力する。
【0077】
位置・姿勢推定部７７は、演算部７６から入力される物体Ｗの局所表面の特徴量と一致する特徴量が対応付けられた基準点Ｏの位置ベクトルの組をテーブル８１から複数取得する。さらに、位置・姿勢推定部７７は、始点Ｐ（ｉ）を原点とする局所座標系を算出する。そして、位置・姿勢推定部７７は、テーブル８１から取得した位置ベクトルの組を、下記式（６）によって、始点Ｐ（ｉ）を原点とする局所座標系における位置ベクトルの組へ変換する。
【0078】
【数4】
【0079】
続いて、位置・姿勢推定部７７は、始点Ｐ（ｉ）を原点とする局所座標系における変換後の位置ベクトルの組を、下記式（７）によって、実空間における位置ベクトルの組へと変換する。
【0080】
【数5】
【0081】
これにより、図８Ｂに一点鎖線で示すように、外部基準点Ｏ５、Ｏ６、Ｏ７、および、内部基準点Ｏ８の実空間における位置ベクトルが算出される。ここで、位置・姿勢推定部７７は、図９に示すように、黒丸で示す物体Ｗ表面のサンプル点と、白丸で示す内部基準点Ｏ８との位置関係に基づいて、外部基準点Ｏ５、Ｏ６、Ｏ７の可否を判定する。なお、図９に示すＸ軸およびＹ軸は、水平面と平行な平面上に位置する直交ベクトルであり、Ｚ軸は、ＸＹ平面に対して直交し、鉛直下方を正方向にとった軸である。
【0082】
そして、位置・姿勢推定部７７は、図９に示すように、内部基準点Ｏ８が物体Ｗの内部にある場合、内部基準点Ｏ８の位置ベクトルと同組に含まれる外部基準点Ｏ５、Ｏ６、Ｏ７の位置ベクトルを、物体Ｗの位置および姿勢の推定材料として適切な基準点と判定する。
【0083】
一方、内部基準点Ｏ９のように、物体Ｗの外部にある場合、位置・姿勢推定部７７は、内部基準点Ｏ９の位置ベクトルと同組に含まれる外部基準点を、物体Ｗの位置および姿勢の推定材料として不適切な基準点と判定し、推定材料から除外する。
【0084】
具体的には、位置・姿勢推定部７７は、内部基準点Ｏ８の実空間における座標値を（ｘｋ、ｙｋ、ｚｋ）とし、下記式（８）によって、内部基準点Ｏ８が物体Ｗの内部にあるか否かを検証し、外部基準点Ｏ５、Ｏ６、Ｏ７の可否を判定する。
【0085】
【数6】
【0086】
ただし、上記式（８）におけるｓｃａｎＺ（ｘ，ｙ）は、実空間におけるｘ座標値およびｙ座標値が、内部基準点Ｏ８のｘ座標値およびｙ座標値と一致するサンプル点のｚ座標値である。
【0087】
このように、位置・姿勢推定部７７は、物体Ｗの位置および姿勢の推定材料として不適切な基準点Ｏを、物体Ｗの位置および姿勢の推定材料から除外する。これにより、位置・姿勢推定部７７は、後に行われる外部基準点Ｏ５、Ｏ６、Ｏ７の投票処理量を低減しつつ、物体Ｗの位置および姿勢の推定精度を向上させることができる。
【0088】
なお、本実施形態では、モデルＭの内部空間に内部基準点Ｏ４を設定したが、モデルＭの内部空間以外であれば、例えば、モデルＭの表面に内部基準点Ｏ４を設定してもよい。ただし、かかる場合、内部基準点Ｏ４に対応する内部基準点Ｏ１０が物体Ｗの表面にあるか否かを、外部基準点Ｏ５、Ｏ６、Ｏ７の可否判定基準とすると、可否判定基準が高くなり過ぎて、適切な基準点Ｏを不適切と誤判定するおそれがある。
【0089】
そこで、位置・姿勢推定部７７では、上記式（８）に所定の閾値ｔｈを設け、外部基準点Ｏ５、Ｏ６、Ｏ７の可否判定基準に一定の幅を持たせることにより、適切な基準点を不適切とする誤判定の発生を抑制する。
【0090】
続いて、位置・姿勢推定部７７は、物体Ｗの位置および姿勢の判定材料として適切と判定した外部基準点Ｏ５、Ｏ６、Ｏ７の実空間における位置と一致する実空間内の投票点へそれぞれ投票する処理を行う。
【0091】
例えば、位置・姿勢推定部７７は、図１０の上図に示す始点Ｐ（ｉ）を原点とする局所座標系における外部基準点Ｏ５、Ｏ６、Ｏ７の位置を、図１０の下図に示す実空間で対応する３点Ａ１、Ｂ１、Ｃ１へそれぞれ投票する。位置・姿勢推定部７７は、推定材料として適切と判定した全ての外部基準点について、順次実空間への投票を行う。
【0092】
これにより、例えば、図１０に示す３点Ａ１、Ｂ１、Ｃ１の投票点の組は、徐々に得票数が増加する。また、実空間では、３点Ａ１、Ｂ１、Ｃ１以外の投票点にも、３点の外部基準点の組毎に、それぞれ実空間内の他の３投票点の組へ順次投票が行われる。位置・姿勢推定部７７は、かかる各３投票点の組毎に、各投票点の得票数を監視する。
【0093】
ここで、例えば、３点の外部基準点の組に含まれる各外部基準点の各ｘ、ｙ、ｘ座標値、つまり９個の座標値で１点が決定する９次元空間内の投票点へ投票を行った場合、得票数が最多の投票点に基づいて、最も確からしい外部基準点を検出することができる。しかし、かかる場合、投票空間が膨大となり、投票に要する計算量も膨大となる。
【0094】
このため、位置・姿勢推定部７７は、３点の外部基準点の組に含まれる各外部基準点の位置を３次元の実空間における独立した３投票点の組へそれぞれ投票する。これにより、投票に要する計算量を大幅に低減することができる。ただし、位置・姿勢推定部７７では、本来一つの３投票点の組へ投票されるべき３票のうち、いずれかが別の投票点の組へ投票される所謂干渉現象が発生する。
【0095】
そこで、位置・姿勢推定部７７は、３投票点の組に含まれる各投票点のうち、投票数が最小の投票点の得票数が所定の閾値Ｖｔｈに達した場合に、その３投票点の組をリストアップする。そして、位置・姿勢推定部７７は、リストアップした３投票点の組毎に総得票数を集計し、得票数が多い組の順に、その組に含まれる投票点の実空間における位置に基づいて、物体Ｗの位置および姿勢を推定する。
【0096】
例えば、位置・姿勢推定部７７は、図１０に示す３点Ａ１、Ｂ１、Ｃ１の組の中で、点Ａ１の得票数が最も少ないと、点Ａ１の得票数が閾値Ｖｔｈに達した場合に、３点Ａ１、Ｂ１、Ｃ１の組をリストアップする。これにより、例えば、干渉によって、２点Ｂ１、Ｃ１の得票数が増大しても、点Ａ１の得票数が閾値Ｖｔｈに達しない限り、適切か否かが未定の３点Ａ１、Ｂ１、Ｃ１の組を早まって適切な得票点の組と判定することを抑制することができる。
【0097】
続いて、位置・姿勢推定部７７は、推定した複数の物体Ｗの位置および姿勢を示す情報を把持対象情報生成部７８へ出力する。把持対象情報生成部７８は、位置・姿勢推定部７７から入力される情報に基づいて、ロボット２の把持候補となる物体Ｗの有無を判定し、把持候補がある場合、把持候補の位置および姿勢を示す情報を、ない場合、その旨を示す情報をロボット制御装置５へ出力する。
【0098】
次に、図１１〜図１３を参照し、物体検出装置４の処理部７が実行する処理について説明する。図１１は、実施形態に係るテーブル８１の作成処理を示すフローチャートである。図１２は、実施形態に係る物体Ｗの位置および姿勢の検出処理を示すフローチャートである。図１３は、実施形態に係る把持候補に関する情報の作成処理を示すフローチャートである。
【0099】
物体検出装置４の処理部７は、物体Ｗの位置および姿勢を検出する以前に、予め図１１に示す処理を実行することによってテーブル８１を作成する。具体的には、処理部７は、図１１に示すように、まず、モデルデータを取得し（ステップＳ１０１）、モデルデータからモデルＭのエッジＥｄを抽出する（ステップＳ１０２）。
【0100】
続いて、処理部７は、モデルＭの表面から選択する始点ＰｉのモデルＭ表面における位置に対応する変数ｉへ１を設定する（ステップＳ１０３）。その後、処理部７は、モデルＭの表面から選択する終点ＰｊのモデルＭ表面における位置に対応する変数ｊへ１を設定する（ステップＳ１０４）。
【0101】
続いて、処理部７は、変数ｉの値と変数ｊの値とが等しくないか否かを判定する（ステップＳ１０５）。つまり、処理部７は、モデルＭの表面から選択する始点Ｐｉと終点Ｐｊとが同一点か否かを判定する。なお、ここでは、変数ｉの値および変数ｊの値は、ともに１である。このため、処理部７は、始点Ｐｉと終点Ｐｊとが同一点であると判定し（ステップＳ１０５，Ｎｏ）、処理をステップＳ１０９へ移す。
【0102】
ステップＳ１０９において、処理部７は、処理を変数ｊのループの先頭、つまり、ステップＳ１０４へ移す。ステップＳ１０４において、処理部７は、変数ｊの値へ１を加算し、処理をステップＳ１０５へ移す。以後、処理部７は、処理がステップＳ１０９まで進む度に、ステップＳ１０４で変数ｊの値へ１を加算し、変数ｊの値がＭｊに達した後に、処理がステップＳ１０９まで進むと、処理をステップＳ１１０へ移す。つまり、処理部７は、モデルＭの表面からＭｊ個の終点Ｐｊを選択する。
【0103】
また、処理部７は、ステップＳ１０５において、変数ｉの値と変数ｊの値とが等しくないと判定した場合（ステップＳ１０５，Ｙｅｓ）、処理をステップＳ１０６へ移す。ステップＳ１０６において、処理部７は、始点Ｐｉの位置ベクトルｐｉと、始点Ｐｉの法線ベクトルｎｉと、終点Ｐｊの位置ベクトルｐｊとから、始点Ｐｉを原点とする局所座標系を算出する。
【0104】
続いて、処理部７は、算出した局所座標系における各基準点Ｏの位置ベクトルを算出する（ステップＳ１０７）。ここで、処理部７は、始点Ｐｉおよび終点Ｐｊを含むモデルＭの局所表面の特徴量であるサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉の特徴量についても算出する。
【0105】
続いて、処理部７は、サーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉の特徴量をキーとしたテーブル８１に算出した基準点Ｏの位置ベクトルを格納し（ステップＳ１０８）、処理をステップＳ１０９へ移す。そして、処理部７は、一つの始点Ｐｉと、Ｍｊ個の終点Ｐｊとについて、ステップＳ１０４〜ステップＳ１０９の処理を繰り返し、その後、処理をステップＳ１１０へ移す。
【0106】
ステップＳ１１０において、処理部７は、処理を変数ｉのループの先頭、つまり、ステップＳ１０３へ移す。ステップＳ１０３において、処理部７は、変数ｉの値へ１を加算し、処理をステップＳ１０４へ移す。以後、処理部７は、処理がステップＳ１１０まで進む度に、ステップＳ１０３で変数ｉの値へ１を加算し、変数ｉの値がＭｉに達した後に、処理がステップＳ１１０まで進むと、処理をステップＳ１１１へ移す。
【0107】
つまり、処理部７は、モデルＭの表面からＭｉ個の始点Ｐｉを選択し、Ｍｉ個の各始点Ｐｉと、Ｍｊ個の終点Ｐｊとについて、ステップＳ１０３〜ステップＳ１１０の処理を繰り返した後、処理をステップＳ１１１へ移す。最後に、処理部７は、作成したテーブル８１を記憶部８に保存して、処理を終了する。
【0108】
なお、テーブル８１は、物体検出装置４以外の装置によって作成されてもよい。かかる場合、物体検出装置４は、他の装置から作成されたテーブル８１を取得して、記憶部８に保存する。
【0109】
また、処理部７は、図１２に示す処理を実行することによって、物体Ｗの位置および姿勢を検出するサーフェスマッチングを行う。具体的には、処理部７は、図１２に示すように、まず、センサ３からバラ積みされた物体Ｗのシーンデータを取得し（ステップＳ２０１）、シーンデータから物体Ｗのエッジを抽出する（ステップＳ２０２）。
【0110】
続いて、処理部７は、シーンデータから各サンプル点の法線ベクトルを算出する。ここで、処理部７は、エッジを形成するサンプル点については、勾配ベクトルも算出する（ステップＳ２０３）。続いて、処理部７は、物体Ｗの表面から選択する始点Ｐ（ｉ）の物体Ｗ表面における位置に対応する変数ｉへ１を設定し、（ステップＳ２０４）、シーンデータから始点Ｐ（ｉ）のサーフレットΔ（ｐｉ）を取得する（ステップＳ２０５）。
【0111】
その後、処理部７は、物体Ｗの表面から選択する終点Ｐ（ｊ）の物体Ｗ表面における位置に対応する変数ｊへ１を設定し（ステップＳ２０６）、シーンデータから終点Ｐ（ｊ）のサーフレットΔ（ｐｊ）を取得する（ステップＳ２０７）。
【0112】
続いて、処理部７は、取得したサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉が制約条件を満たすか否かを判定する（ステップＳ２０８）。ここで、処理部７は、例えば、始点Ｐ（ｉ）および終点Ｐ（ｊ）がいずれも面上の点である場合、始点Ｐ（ｉ）の法線ベクトルｎ（ｉ）と終点Ｐ（ｊ）の法線ベクトルｎ（ｊ）とのなす角度が所定の閾値Ｔｆより大きい場合に、制約条件を満たすと判定する。一方、始点Ｐ（ｉ）の法線ベクトルｎ（ｉ）と終点Ｐ（ｊ）の法線ベクトルｎ（ｊ）とのなす角度が所定の閾値Ｔｆ以下の場合、処理部７は、制約条件を満たさないと判定する。
【0113】
また、処理部７は、例えば、始点Ｐ（ｉ）が面上の点であり、終点Ｐ（ｊ）がエッジ上の点である場合、始点Ｐ（ｉ）の法線ベクトルｎ（ｉ）と終点Ｐ（ｊ）の勾配ベクトルｇ（ｊ）とのなす角度が所定の閾値Ｔｅより大きい場合に、制約条件を満たすと判定する。一方、始点Ｐ（ｉ）の法線ベクトルｎ（ｉ）と終点Ｐ（ｊ）の勾配ベクトルｇ（ｊ）とのなす角度が所定の閾値Ｔｅ以下の場合、処理部７は、制約条件を満たさないと判定する。
【0114】
かかる、制約条件を設けることにより、処理部７は、際立った特徴が表れ難い始点Ｐ（ｉ）およびＰ（ｊ）のペアについて、無駄にサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉を取得することを防止することができるので、処理量を低減することができる。
【0115】
そして、処理部７は、取得したサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉が制約条件を満たすと判定した場合（ステップ２０８，Ｙｅｓ）、処理をステップＳ２０９へ移す。一方、制約条件を満たさないと判定した場合（ステップＳ２０８，Ｎｏ）、処理部７は、処理をステップＳ２１８へ移す。
【0116】
ステップＳ２０９において、処理部７は、テーブル８１から、ステップＳ２０５およびステップＳ２０７で取得したサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉と特徴量が一致する全サーフレットペア（Ｎｖ個）の基準点Ｏの位置ベクトルの組を取得する。
【0117】
続いて、処理部７は、ステップＳ２０５およびステップＳ２０７で取得したサーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉の局所座標系を算出する（ステップＳ２１０）。つまり、処理部７は、サーフレットペア〈Δ（ｐｊ）、Δ（ｐｉ）〉に基づいて、始点Ｐ（ｉ）を原点とする局所座標系を算出する。
【0118】
その後、処理部７は、ステップＳ２０９で取得した位置ベクトルの組の取得順番を示す変数ｒに１を設定する（ステップＳ２１１）。続いて、処理部７は、ステップＳ２１０で算出した局所座標系と、ｒ番目の基準点Ｏの位置ベクトルの組とに基づいて、各基準点Ｏの実空間における位置ベクトルの組を算出する（ステップＳ２１２）。
【0119】
ここで、処理部７は、内部基準点Ｏ８が物体Ｗの内部にあるか否かを判定する（ステップＳ２１３）。そして、処理部７は、内部基準点Ｏ８が物体Ｗの内部にあると判定した場合（ステップＳ２１３，Ｙｅｓ）、処理をステップＳ２１４へ移す。一方、内部基準点Ｏ８が物体Ｗの外部にあると判定した場合（ステップＳ２１３，Ｎｏ）、処理をステップＳ２１７へ移す。
【0120】
ステップＳ２１４において、処理部７は、ステップＳ２１３で物体Ｗの内部にあると判定された内部基準点Ｏ８と同じ組に含まれる外部基準点Ｏ５、Ｏ６、Ｏ７の位置ベクトルと一致する実空間内の投票点へ投票を行う。そして、処理部７は、ステップＳ２１４で投票した３点の投票点の組の中で、投票数が最小の投票点の得票数が閾値Ｖｔｈに達したか否かの判定を行う（ステップＳ２１５）。
【0121】
ここで、処理部７は、閾値Ｖｔｈに達したと判定した場合（ステップＳ２１５，Ｙｅｓ）、処理をステップＳ２１６へ移し、ステップＳ２１４で投票した３点の投票点の組をリストアップして（ステップＳ２１６）、処理をステップＳ２１７へ移す。一方、閾値Ｖｔｈに達していないと判定した場合（ステップ２１５，Ｎｏ）、処理をステップＳ２１７へ移す。
【0122】
ステップＳ２１７において、処理部７は、処理を変数ｒのループの先頭、つまり、ステップＳ２１１へ移す。ステップＳ２１１において、処理部７は、変数ｒの値へ１を加算する。以後、処理部７は、処理がステップＳ２１７まで進む度に、ステップＳ２１１で変数ｒの値へ１を加算し、変数ｒの値がＮｖに達した後に、処理がステップＳ２１７まで進むと、処理をステップＳ２１８へ移す。つまり、処理部７は、ステップＳ２０９で取得した基準点Ｏの位置ベクトルの全組について、ステップＳ２１２〜ステップＳ２１６の処理を行う。
【0123】
ステップＳ２１８において、処理部７は、処理を変数ｊのループの先頭、つまり、ステップＳ２０６へ移す。ステップＳ２０６において、処理部７は、変数ｊの値へ１を加算する。以後、処理部７は、処理がステップＳ２１８まで進む度に、ステップＳ２０６で変数ｊの値へ１を加算し、変数ｊの値がＮｈに達した後に、処理がステップＳ２１８まで進むと、処理をステップＳ２１９へ移す。
【0124】
ステップＳ２１９において、処理部７は、処理を変数ｉのループの先頭、つまり、ステップＳ２０４へ移す。ステップＳ２０４において、処理部７は、変数ｉの値へ１を加算する。以後、処理部７は、処理がステップＳ２１９まで進む度に、ステップＳ２０４で変数ｉの値へ１を加算し、変数ｉの値がＮｔに達した後に、処理がステップＳ２１９まで進むと、処理をステップＳ２２０へ移す。
【0125】
ステップＳ２２０において、処理部７は、リストアップした投票点の組毎に総得票数を集計し、総得票数が高い順に投票点の組をソートする。続いて、処理部７は、総得票数の高い上位Ｇ個の投票点の組から物体Ｗの位置および姿勢を算出し、（ステップＳ２２１）、処理を終了する。
【0126】
また、処理部７は、図１３に示す処理を実行することによって、ロボット２の把持候補となる物体Ｗに関する情報を生成し、ロボット制御装置５へ出力する。具体的には、処理部７は、図１３に示すように、まず、シーンデータを計測する（ステップＳ３０１）。
【0127】
続いて、図１２に示す処理（サーフェスマッチング）によって算出したＧ個の把持候補を検出する（ステップＳ３０２）。そして、処理部７は、把持候補に対する後述の詳細マッチングを行う順番を示す変数ｔに１を設定し（ステップＳ３０３）、処理をステップＳ３０４へ移す。ステップＳ３０４において、処理部７は、変数ｔの値がＧ未満であるか否かを判定する（ステップＳ３０４）。
【0128】
ここで、処理部７は、変数ｔの値がＧ未満であると判定した場合（ステップＳ３０４，Ｙｅｓ）、処理をステップＳ３０５へ移す。一方、変数ｔの値がＧに達したと判定した場合（ステップＳ３０４，Ｎｏ）、把持候補なしフラグをロボット制御装置５へ設定し（ステップＳ３１０）、処理を終了する。
【0129】
ステップＳ３０５において、処理部７は、ｔ番目の把持候補に対して詳細マッチングを行う。例えば、処理部７は、詳細マッチングとして、ＩＣＰ(Iterative Closest Point)アルゴリズムによる詳細なパターンマッチングを行う。そして、処理部７は、詳細マッチングに成功したか否かを判定する（ステップＳ３０６）。
【0130】
ステップＳ３０６において、処理部７は、詳細マッチングに成功したと判定した場合（ステップＳ３０６，Ｙｅｓ）、処理をステップＳ３０７へ移す。一方、詳細マッチングに失敗したと判定した場合（ステップＳ３０６，Ｎｏ）、処理をステップＳ３１１へ移す。
【0131】
ステップＳ３０７において、処理部７は、把持候補に対してロボット２がピッキング動作を行う際にハンド２５が干渉するかどうかのハンド干渉チェックを行う。そして、処理部７は、干渉がないか否かを判定し（ステップＳ３０８）、干渉がないと判定した場合（ステップＳ３０８，Ｙｅｓ）、処理をステップＳ３０９へ移す。
【0132】
一方、干渉があると判定した場合（ステップＳ３０８，Ｎｏ）、処理をステップＳ３１１へ移す。ステップＳ３１１において、処理部７は、変数ｔの値へ１を加算し、処理をステップＳ３０４へ移す。また、ステップＳ３０９において、処理部７は、ステップＳ３０８で干渉なしと判定した把持候補の位置および姿勢（ピッキング位置および姿勢）をロボット制御装置５へ設定して、処理を終了する。
【0133】
上述したように、実施形態に係る物体検出方法では、物体のモデルの外部空間に、物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、モデルの内部空間に、推定材料の可否判定材料となる内部基準点を設定する。そして、モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、始点に対する外部基準点および内部基準点の位置の組とを対応付けたテーブルを記憶する。
【0134】
さらに、実施形態に係る物体検出方法では、実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、始点および終点の対を含む局所表面における物体の特徴量を算出する。
【0135】
そして、テーブルから物体の特徴量と一致する特徴量が対応付けられた位置の組を取得して実空間における位置の組へ変換し、位置の組に含まれる内部基準点の位置が物体の外部にある場合に、位置の組に含まれる外部基準点の位置を推定材料から除外して物体の位置および姿勢を推定する。かかる物体検出方法によれば、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる。
【0136】
また、実施形態に係る物体検出方法では、変換後の位置の組に含まれる内部基準点の位置が物体の内部にある場合に、位置の組に含まれる複数の外部基準点の位置と一致する実空間内の投票点へそれぞれ投票する。そして、一つの位置の組に含まれる複数の外部基準点の位置に基づいて投票が行われた投票点の組の中で、投票数が最小の投票点の得票数が所定の閾値に達した場合に、投票点の組をリストアップする。
【0137】
その後、リストアップした投票点の組毎に総得票数を集計し、総得票数が多い組の順に組に含まれる投票点の実空間における位置に基づいて、物体の位置および姿勢を推定する。これにより、本来投票されるべきでない投票点へ投票される所謂干渉が発生しても、物体の位置および姿勢に関する推定精度が低下することを抑制することができる。
【0138】
また、実施形態に係る物体検出方法では、物体およびモデルのエッジを検出し、モデルの表面に位置する点群および物体の表面に位置するサンプル点群をエッジ上の点と、面上の点とに分類する。
【0139】
そして、物体の形状に応じて、始点として選択する点と終点として選択する点とをエッジ上の点および面上の点から選択する。これにより、実施形態に係る物体検出方法では、物体の形状に応じた、より特徴的な局所表面の特徴量に基づいて、物体の位置および姿勢を推定することができるので汎用性を向上させることができる。
【0140】
また、実施形態に係る物体検出方法では、サンプル点から選択する始点として、面上の点を選択し、サンプル点から選択する終点として、エッジ上の点を選択することにより、物体に占める平面部分の面積が比較的大きな物体の検出精度を向上させることができる。
【0141】
また、実施形態に係る物体検出方法では、サンプル点から選択する始点および終点として、始点における法線ベクトルと、終点における法線ベクトルまたは勾配ベクトルとのなす角度が所定の閾値より大きい２点を選択する。これにより、物体の比較的特徴的でない部分の局所表面については、位置および姿勢の推定材料から除外することができるので、位置および姿勢の検出に要する処理量を低減することができる。
【0142】
さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。
【符号の説明】
【0143】
１ ロボットシステム２ ロボット
３ センサ
４ 物体検出装置
５ ロボット制御装置
７ 処理部
７１ モデルデータ取得部
７２ シーンデータ取得部
７３ 基準点設定部
７４ エッジ検出部
７５ サーフレット選択部
７６ 演算部
７７ 位置・姿勢推定部
７８ 把持対象情報生成部
８ 記憶部
８１ テーブル
Ｗ 物体
Ｍ モデル