JP6192088B2 - 物体検出方法および物体検出装置 - Google Patents

Description

開示の実施形態は、物体検出方法および物体検出装置に関する。

従来、距離センサによって取得される物体までの距離に基づいて、物体の位置および姿勢を検出する物体検出方法がある。例えば、特許文献１には、距離センサから物体のエッジ上に位置する点までの距離をハフ変換してハフ空間へ投票し、ハフ空間における得票数の多い点に基づいて物体の位置および姿勢を検出する物体検出方法が開示されている。

特許第５０８８２７８号公報

しかしながら、ハフ変換を用いた物体の検出方法では、ハフ空間への投票数が膨大であるため、物体の位置および姿勢の検出に要する処理量が嵩むという問題がある。

実施形態の一態様は、上記に鑑みてなされたものであって、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる物体検出方法および物体検出装置を提供することを目的とする。

実施形態の一態様に係る物体検出方法では、物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する。前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する。実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する。前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する。

実施形態の一態様によれば、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる。

図１Ａは、実施形態に係る物体検出方法を示す説明図である。 図１Ｂは、実施形態に係る物体検出方法を示す説明図である。 図２は、実施形態に係るロボットシステムを示す説明図である。 図３は、実施形態に係る物体検出装置の構成の一例を示す説明図である。 図４Ａは、実施形態に係るテーブルの作成手順を示す説明図である。 図４Ｂは、実施形態に係るテーブルの作成手順を示す説明図である。 図５Ａは、実施形態に係るテーブルの作成手順を示す説明図である。 図５Ｂは、実施形態に係るテーブルの作成手順を示す説明図である。 図６は、実施形態に係るテーブルの作成手順を示す説明図である。 図７は、実施形態に係るテーブルの一例を示す説明図である。 図８Ａは、実施形態に係る物体の位置および姿勢を検出する手順を示す説明図である。 図８Ｂは、実施形態に係る物体の位置および姿勢を検出する手順を示す説明図である。 図９は、実施形態に係る物体の位置および姿勢を検出する手順を示す説明図である。 図１０は、実施形態に係る物体の位置および姿勢を検出する手順を示す説明図である。 図１１は、実施形態に係るテーブルの作成処理を示すフローチャートである。 図１２は、実施形態に係る物体の位置および姿勢の検出処理を示すフローチャートである。 図１３は、実施形態に係る把持候補に関する情報の作成処理を示すフローチャートである。

以下、添付図面を参照して、本願の開示する物体検出方法および物体検出装置の実施形態を詳細に説明する。なお、以下に示す実施形態によりこの発明が限定されるものではない。また、以下では、位置および姿勢の検出対象となる物体の形状が円筒形状の場合を例に挙げて説明するが、検出対象の物体の形状は円筒形状に限定されるものではない。

図１Ａおよび図１Ｂは、実施形態に係る物体検出方法を示す説明図である。図１Ａに示すように、実施形態に係る物体検出方法では、検出対象の物体と同一形状で、実空間における位置および姿勢が既知のモデルＭの表面形状を示す情報を取得する。

具体的には、モデルＭの表面に位置する点群の各点に関する情報を取得する。各点に関する情報には、各点の実空間における３次元の位置情報（位置ベクトル）、各点におけるモデルＭの表面に対する法線ベクトルの情報が含まれる。

続いて、モデルＭの外部空間に、物体の位置および姿勢の推定材料となる複数の外部基準点Ｏ_１、Ｏ_２、Ｏ_３を設定し、モデルＭの内部空間に、物体の位置および姿勢の推定材料の可否判定材料となる内部基準点Ｏ_４を設定する。なお、設定する基準点の個数はこれに限定されるものではない。

そして、モデルＭの表面に位置する点群から順次選択する始点Ｐ_ｉおよび終点Ｐ_ｊの対を含むモデルＭの局所表面の特徴量を算出する。ここでの特徴量は、例えば、始点Ｐ_ｉおよび終点Ｐ_ｊ間の距離、始点Ｐ_ｉおよび終点Ｐ_ｊの法線の内積、始点Ｐ_ｉの法線と始点Ｐ_ｉおよび終点Ｐ_ｊを結ぶベクトルとの内積などである。

続いて、算出した特徴量と、始点Ｐ_ｉに対する外部基準点Ｏ_１、Ｏ_２、Ｏ_３および内部基準点Ｏ_４の位置ベクトル（始点Ｐ_ｉから外部基準点Ｏ_１、Ｏ_２、Ｏ_３、内部基準点Ｏ_４へ向かう矢印参照）の組とを対応付けたテーブルを作成して記憶する。なお、これまでの工程は、実空間に存在する物体の位置および姿勢を検出する以前に、予めオフラインにて行っておく。

その後、図１Ｂに示すように、実空間に存在する物体Ｗの表面の表面形状を示す情報を取得する。ここでは、図示を省略したが、物体Ｗの表面の表面形状を示す情報には、物体Ｗの表面に位置するサンプル点群の実空間における３次元の位置情報（位置ベクトル）が含まれる。

続いて、サンプル点群から始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の対を順次選択し、始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の対を含む局所表面における物体Ｗの特徴量を算出し、先に記憶したテーブルから物体Ｗの特徴量と一致する特徴量が対応付けられた複数の位置ベクトルの組を取得する。

ここで取得される位置ベクトルの始点Ｐ_ｉのモデルＭにおける位置が、サンプル群から選択した始点Ｐ_（ｉ）の物体Ｗにおける位置と一致していれば、位置ベクトルの組を実空間の位置ベクトルへ変換することで、物体Ｗの正しい基準点位置を取得することができる。したがって、かかる変換した３点の位置ベクトルに基づいて、物体Ｗの正しい位置および姿勢を推定して検出することができる。

ただし、テーブルから取得される位置ベクトルの始点Ｐ_ｉのモデルＭにおける位置は、サンプル群から選択した始点Ｐ_（ｉ）の物体Ｗにおける位置と一致しない場合がある。例えば、物体Ｗの位置や姿勢によっては、物体Ｗにおける始点Ｐ_（ｉ）の位置と、モデルＭにおける始点Ｐ_ｉの位置とが異なる場合にも、物体Ｗにおける局所表面の特徴量とモデルＭにおける局所表面の特徴量とが一致することがある。

かかる場合、テーブルから取得した位置ベクトルの組を実空間の位置ベクトルへ変換しても、物体Ｗの正しい基準点位置を取得することができず、物体Ｗの正しい位置および姿勢を検出することができない。

このため、実施形態に係る物体検出方法では、ハフ変換を利用して、テーブルから取得した複数の位置ベクトルの組を実空間における位置ベクトルの組へ変換し、変換後の位置ベクトルが示す３次元位置をハフ空間となる実空間の投票点へ投票する。そして、３点ずつ投票される投票点の組のなかで、得票数の多い投票点の組に含まれる位置ベクトルを物体Ｗの正しい基準点の位置と判定し、かかる位置に基づいて物体Ｗの位置および姿勢を判定する。

しかしながら、テーブルから取得した全ての位置ベクトルの組を投票の対象とした場合、物体Ｗの位置および姿勢を検出する処理量が嵩むという問題が生じる。また、前述したように、テーブルから取得した複数の位置ベクトルの組の中には、物体Ｗの位置および姿勢の推定材料として不適切な位置ベクトルも含まれている。

さらに、物体Ｗの位置や姿勢等によっては、位置および姿勢の推定材料として不適切な位置ベクトルの投票点が、適切な位置ベクトルの投票点よりも多くの票を獲得することもあり、物体Ｗの位置および姿勢の検出精度が低下する恐れがある。

そこで、実施形態に係る物体検出方法では、投票を行う前に、位置および姿勢の推定材料として不適切な位置ベクトルの組を投票の対象から除外することで、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させる。

具体的には、テーブルから取得した位置ベクトルの組を実空間の位置ベクトルへ変換した場合に、図１Ｂに示すように、モデルＭに設定された外部基準点Ｏ_１、Ｏ_２、Ｏ_３が実空間で３点Ａ、Ｂ、Ｃに、内部基準点Ｏ_４が物体Ｗ外部の点Ｄに位置していたとする。

このように、本来であれば物体Ｗの内部に存在するはずの内部基準点Ｏ_４が物体Ｗ外部の点Ｄに存在した場合、かかる点Ｄと同じ組に含まれる実空間における３点Ａ、Ｂ、Ｃを投票対象、つまり、物体Ｗの位置および姿勢の推定材料から排除する。

したがって、実施形態に係る物体検出方法によれば、投票を行う前に、位置および姿勢の推定材料として不適切な位置ベクトルの組を投票の対象から除外することができるので、物体Ｗの位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させる。

なお、モデルＭに設定された内部基準点Ｏ_４に対応する実空間の点Ｄが適切に物体Ｗの内部に存在していた場合、外部基準点Ｏ_１、Ｏ_２、Ｏ_３に対応する実空間の３点Ａ、Ｂ、Ｃの位置を投票し、投票結果に基づいて物体Ｗの位置および姿勢を検出する。なお、かかる物体検出方法の詳細については、実施形態に係る物体検出装置の動作と合わせて後述する。

次に、図２を参照して、実施形態に係る物体検出装置を備えたロボットシステムについて説明する。図２は、実施形態に係るロボットシステム１を示す説明図である。図２に示すように、ロボットシステム１は、ロボット２と、センサ３と、物体検出装置４と、ロボット制御装置５とを備える。

ロボット２は、床面などに設置される胴部２１と、胴部２１から延伸する右アーム２２および左アーム２３とを備える。右アーム２２および左アーム２３は、いずれも７軸の自由度を有するロボットアームである。また、右アーム２２の先端には、内部に円筒状の物体Ｗがバラ積みされた箱体６を把持するハンド２４が設けられ、左アーム２３の先端には、箱体６の内部から物体Ｗをピックアップするハンド２５が設けられる。

センサ３は、箱体６の内部にバラ積みされた物体Ｗの３次元形状を検出するものであり、例えば、三次元スキャナである。かかるセンサ３は、支持体３１によって支持されて箱体６の鉛直上方に配置される。

センサ３は、レーザ光によって物体Ｗを走査し、物体Ｗから反射される反射光に基づいて物体Ｗの３次元形状を検出する。そして、センサ３は、物体Ｗの３次元形状を示す情報（以下、「シーンデータ」と記載する）を物体検出装置４へ出力する。シーンデータには、前述した物体Ｗの表面に位置するサンプル点群の実空間における３次元の位置情報（位置ベクトル）が含まれる。

物体検出装置４は、センサ３から入力されるシーンデータと前述したテーブルとに基づいて、箱体６の内部にバラ積みされた物体Ｗの位置および姿勢を検出し、ロボット２の把持対象となる物体Ｗの位置および姿勢をロボット制御装置５へ出力する。なお、物体検出装置４の構成の一例については、図３を参照して後述する。

ロボット制御装置５は、物体検出装置４から入力される把持対象の物体Ｗの位置および姿勢に基づいてロボット２の制御信号を生成し、制御信号をロボット２へ出力することによってロボット２に物体Ｗのピックアップ動作を行わせる。

次に、図３を参照して、物体検出装置４の構成の一例について説明する。図３は、実施形態に係る物体検出装置４の構成の一例を示す説明図である。図３に示すように、物体検出装置４は、処理部７と記憶部８とを備える。記憶部８は、前述のテーブル８１を記憶する。なお、テーブル８１の一例については、図７を参照して後述する。

処理部７は、モデルデータ取得部７１と、シーンデータ取得部７２と、基準点設定部７３と、エッジ検出部７４と、サーフレット選択部７５と、演算部７６と、位置・姿勢推定部７７と、把持対象情報生成部７８とを備える。

物体検出装置４では、モデルデータ取得部７１と、基準点設定部７３と、エッジ検出部７４と、サーフレット選択部７５と、演算部７６とによって、前述したテーブル８１を作成して記憶部８へ記憶させる。

ここで、図４Ａ〜図７を合わせて参照しながら、物体検出装置４によるテーブル８１の作成手順およびテーブル８１の一例について説明する。図４Ａ〜図６は、実施形態に係るテーブル８１の作成手順を示す説明図であり、図７は、実施形態に係るテーブル８１の一例を示す説明図である。

モデルデータ取得部７１は、ロボット２の把持対象となる物体Ｗとサイズおよび形状が等しい図４Ａに示すモデルＭのＣＡＤデータを所定の情報処理装置（図示略）から読み込む。そして、モデルデータ取得部７１は、ＣＡＤデータからモデルＭの表面形状を示すモデルデータを抽出して取得する。

具体的には、モデルデータ取得部７１は、図４Ｂに示すように、モデルＭ表面に位置する点群の各点の実空間における３次元の位置情報（位置ベクトル）、各点におけるモデルＭの表面に対する法線ベクトルの情報等をモデルデータとして取得する。そして、モデルデータ取得部７１は、取得したモデルデータを基準点設定部７３へ出力する。

基準点設定部７３は、図４Ｂに示すように、モデルデータに対してモデルＭの外部空間に、３点の外部基準点Ｏ_１、Ｏ_２、Ｏ_３を設定するとともに、モデルＭの内部空間に、内部基準点Ｏ_４を設定する。なお、外部基準点Ｏ_１、Ｏ_２、Ｏ_３および内部基準点Ｏ_４を区別しない場合、これらを基準点Ｏと総称する。かかる基準点設定部７３は、基準点Ｏを設定したモデルデータをエッジ検出部７４へ出力する。

エッジ検出部７４は、図５Ａに示すように、基準点設定部７３から入力されるモデルデータに基づいて、モデルＭのエッジＥｄを検出する。さらに、エッジ検出部７４は、モデルＭのエッジＥｄを形成する点群に対してエッジＥｄ上の点であることを示す情報（以下、「ラベルＥ」と記載する）を付加する。また、エッジ検出部７４は、モデルＭのエッジＥｄ以外の部分を形成する点群に対して面上の点であることを示す情報（以下、「ラベルＦ」と記載する）を付加する。そして、エッジ検出部７４は、ラベルＥとラベルＦとを付加したモデルデータをサーフレット選択部７５へ出力する。

サーフレット選択部７５は、エッジ検出部７４から入力されるモデルデータから、モデルＭの表面に位置する２点の対を始点Ｐ_ｉおよび終点Ｐ_ｊの対として順次選択し、選択した点に関する情報（以下、「サーフレット」と記載する）を取得する。

例えば、サーフレット選択部７５は、図５Ｂに示すように、円筒形状のモデルＭの周面に位置する始点Ｐ_ｉを選択する場合、始点Ｐ_ｉの実空間における位置ベクトルｐ_ｉ、ラベルＦ、法線ベクトルｎ_ｉの組を始点Ｐ_ｉのサーフレットΔ（ｐ_ｉ）として取得する。

また、サーフレット選択部７５は、モデルＭのエッジＥｄ（図５Ａ参照）に位置する終点Ｐ_ｊを選択する場合、終点Ｐ_ｊの実空間における位置ベクトルｐ_ｊ、ラベルＥ、終点Ｐ_ｊの勾配ベクトルｇ_ｊを終点Ｐ_ｊのサーフレットΔ（ｐ_ｊ）として取得する。

ここでの勾配ベクトルｇ_ｊは、終点Ｐ_ｊにおけるモデルＭの周面と端面とのなす角度である。なお、面上の点を終点として選択する場合、サーフレット選択部７５は、終点のサーフレットとして、終点の位置ベクトル、ラベルＦ、終点の法線ベクトルの組を取得する。

サーフレット選択部７５は、選択する始点Ｐ_ｉおよび終点Ｐ_ｊを変更しながら、複数のサーフレットΔ（ｐ_ｉ）とサーフレットΔ（ｐ_ｊ）との対（以下、「サーフレットペア」と記載する）を取得し、演算部７６へ出力する。

かかるサーフレットペアは、演算部７６が始点Ｐ_ｉおよび終点Ｐ_ｊの対を含むモデルＭの局所表面の特徴量を演算する処理などで使用される情報である。このため、サーフレット選択部７５は、モデルＭの表面に位置する全ての点を始点Ｐ_ｉおよび終点Ｐ_ｊとして選択してサーフレットペアを取得する構成でもよく、終点Ｐ_ｊとしてラベルＥが付加された点、または、ラベルＦが付加された点を選択する構成でもよい。

モデルＭの表面に位置する全ての点を始点Ｐ_ｉおよび終点Ｐ_ｊとして選択する構成によれば、モデルＭにおける局所表面の特徴量をより多くテーブル８１に蓄積させることができるため、物体Ｗの検出精度を向上させることができる。

また、終点Ｐ_ｊとしてラベルＥが付加された点を限定して選択する構成は、モデルに占める平面部分の領域が比較的広い場合に有効である。具体的には、平面部分の占める領域が広いモデルでは、平面部分のいずれの点を始点Ｐ_ｉおよび終点Ｐ_ｊとして選択しても、算出される局所表面の特徴量に大差がなく、エッジＥｄ部分を終点Ｐ_ｊとして選択した方が、よりモデルの特徴を表す特徴量を得ることができる。しかも、ラベルＦが付加された面上の点を終点Ｐ_ｊの選択肢から除外することができるので、特徴量を算出する処理量を低減することができる。

したがって、終点Ｐ_ｊとしてラベルＥが付加された点を限定して選択する構成によれば、平面部分の占める領域が広いモデルについて、処理量を低減しつつ、物体の位置および姿勢の検出精度を向上させることができる。

一方、終点Ｐ_ｊとしてラベルＦが付加された点を限定して選択する構成は、モデルに占める曲面部分の領域が比較的広い場合に有効である。具体的には、曲面部分の占める領域が広いモデルでは、モデルの面状における局所平面のそれぞれにモデルの特徴がある。

したがって、終点Ｐ_ｊとしてラベルＦが付加された点を限定して選択する構成によれば、モデルの面状における局所平面のそれぞれについて特徴量を取得することで、曲面部分の占める領域が広い物体の位置および姿勢の検出精度を向上させることができる。

演算部７６は、サーフレット選択部７５から入力されるサーフレットペアを使用して、始点Ｐ_ｉおよび終点Ｐ_ｊの対を含むモデルＭの局所表面の特徴量を算出する。例えば、演算部７６は、始点Ｐ_ｉおよび終点Ｐ_ｊ間の距離、始点Ｐ_ｉおよび終点Ｐ_ｊの法線ベクトルｎ_ｉの内積、始点Ｐ_ｉの法線ベクトルｎ_ｉと始点Ｐ_ｉおよび終点Ｐ_ｊを結ぶベクトルｆ_ｉ，ｊとの内積を特徴量として算出する。

さらに、演算部７６は、始点Ｐ_ｉを原点とする局所座標系を算出し、局所座標系における基準点Ｏの位置ベクトル（図５Ｂに示す一点鎖線矢印参照）を算出する。基準点Ｏの位置ベクトルの算出には、始点Ｐ_ｉの位置ベクトルｐ_ｉ、終点Ｐ_ｊの位置ベクトルｐ_ｊ、位置ベクトルｐ_ｉと位置ベクトルｐ_ｊとの差分ベクトルｆ_ｉ，ｊが用いられる。

具体的には、実空間における各基準点Ｏの位置ベクトルは、下記の式（１）によって表すことができる。

この実空間における各基準点Ｏの位置ベクトルを〈ｐ_ｉ、ｎ_ｉ、ｆ_ｉ，ｊ〉により定まる局所座標系を用いて表す。局所座標系は、以下のように定める。

上記式（２）、式（３）、式（４）によって算出される直交基底を用いて、各基準点Ｏの局所座標系における位置ベクトルは、下記の式（５）によって算出する。

こうして、図６に示すように、始点Ｐ_ｉを原点とした局所座標系における外部基準点Ｏ_１、Ｏ_２、Ｏ_３、および、内部基準点Ｏ_４の各位置ベクトル（図６に示す一点鎖線矢印参照）を算出する。

続いて、演算部７６は、図７に示すように、先に算出した始点Ｐ_ｉおよび終点Ｐ_ｊの対を含む局所表面の特徴量（Ｈ１、Ｈ２など）と、始点Ｐ_ｉを原点とした局所座標系における基準点Ｏの位置ベクトル（ここでは、外部基準点Ｏ_１、Ｏ_２、Ｏ_３、内部基準点Ｏ_４の４つ位置ベクトル）の組とを対応付けたテーブル８１を作成する。

ここで、モデルＭには、特徴量が一致する複数の局所表面が存在する。このため、テーブル８１では、一つの特徴量（Ｈ１、Ｈ２など）に対して、それぞれ局所座標系における複数の基準点Ｏの組が対応付けられる。つまり、テーブル８１は、各特徴量（Ｈ１、Ｈ２など）をキーとするハッシュテーブルのデータ構造となる。演算部７６は、こうして形成したテーブル８１を記憶部８へ記憶させる。

また、物体検出装置４では、シーンデータ取得部７２と、エッジ検出部７４と、サーフレット選択部７５と、演算部７６と、位置・姿勢推定部７７と、把持対象情報生成部７８とによって、物体Ｗの位置および姿勢を推定して検出する。

ここで、図８Ａ〜図１０を合わせて参照しながら、物体検出装置４が物体Ｗの位置および姿勢を検出する手順の一例について説明する。図８Ａ〜図１０は、実施形態に係る物体Ｗの位置および姿勢を検出する手順を示す説明図である。

シーンデータ取得部７２は、センサ３から、図８Ａに示すバラ積みされた物体Ｗの３次元形状を示すシーンデータを取得する。具体的には、シーンデータ取得部７２は、図８Ｂに示すように、物体Ｗ表面に位置するサンプル点群の各点の実空間における３次元の位置情報（位置ベクトル）、各点における物体Ｗの表面に対する法線ベクトルｎ_（ｉ）の情報等をシーンデータとして取得する。そして、シーンデータ取得部７２は、取得したシーンデータをエッジ検出部７４へ出力する。

エッジ検出部７４は、シーンデータ取得部７２から入力されるシーンデータに基づいて、物体Ｗのエッジを検出する。さらに、エッジ検出部７４は、物体Ｗのエッジを形成するサンプル点群に対してエッジ上の点であることを示すラベルＥを付加し、ラベルＥを付加した各サンプル点の勾配ベクトルｇ_（ｊ）を算出する。

また、エッジ検出部７４は、物体Ｗのエッジ以外の部分を形成するサンプル点群に対して面上の点であることを示すラベルＦを付加する。そして、エッジ検出部７４は、ラベルＥとラベルＦとを付加したモデルデータをサーフレット選択部７５へ出力する。なお、エッジ検出部７４は、エッジ上のサンプル点の勾配ベクトルｇ_（ｊ）についてもサーフレット選択部７５へ出力する。

サーフレット選択部７５は、エッジ検出部７４から入力されるシーンデータから、物体Ｗの表面に位置する２点の対を始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の対として順次選択し、選択したサンプル点に関するサーフレットを取得する。

例えば、サーフレット選択部７５は、円筒形状の物体Ｗの周面に位置する始点Ｐ_（ｉ）を選択する場合、始点Ｐ_（ｉ）の実空間における位置ベクトルｐ_（ｉ）、ラベルＦ、法線ベクトルｎ_（ｉ）の組を始点Ｐ_（ｉ）のサーフレットΔ（ｐ_ｉ）として取得する。

また、サーフレット選択部７５は、物体Ｗのエッジに位置する終点Ｐ_（ｊ）を選択する場合、終点Ｐ_（ｊ）の実空間における位置ベクトルｐ_（ｊ）、ラベルＥ、終点Ｐ_（ｊ）の勾配ベクトルｇ_（ｊ）を終点Ｐ_（ｊ）のサーフレットΔ（ｐ_ｊ）として取得する。

ここでの勾配ベクトルｇ_（ｊ）は、終点Ｐ_（ｊ）における物体Ｗの周面と端面とのなす角度である。なお、面上の点を終点として選択する場合、サーフレット選択部７５は、終点のサーフレットとして、終点の位置ベクトル、ラベルＦ、終点の法線ベクトルの組を取得する。

サーフレット選択部７５は、選択する始点Ｐ_（ｉ）および終点Ｐ_（ｊ）を変更しながら、複数のサーフレットΔ（ｐ_ｉ）とサーフレットΔ（ｐ_ｊ）との対であるサーフレットペアを取得し、演算部７６へ出力する。

かかるサーフレットペアは、演算部７６が始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の対を含む物体Ｗの局所表面の特徴量を演算する処理などで使用される情報である。したがって、モデルＭのサーフレットペアを取得する場合と同様に、サーフレット選択部７５によって、全ての点を始点Ｐ_（ｉ）および終点Ｐ_（ｊ）として選択してサーフレットペアを取得することで、物体Ｗの検出精度を向上させることができる。

また、ラベルＥが付加された終点Ｐ_ｊをサーフレット選択部７５によって限定して選択させることにより、平面部分の占める領域が広い物体について、処理量を低減しつつ、物体の位置および姿勢の検出精度を向上させることができる。一方、ラベルＦが付加された終点Ｐ_ｊをサーフレット選択部７５によって限定して選択させることにより、曲面部分の占める領域が広い物体について、位置および姿勢の検出精度を向上させることができる。

演算部７６は、サーフレット選択部７５から入力されるサーフレットペアを使用して、始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の対を含む物体Ｗの局所表面の特徴量を算出する。例えば、演算部７６は、始点Ｐ_（ｉ）および終点Ｐ_（ｊ）間の距離、始点Ｐ_（ｉ）および終点Ｐ_（ｊ）の法線ベクトルｎ_（ｉ）の内積、始点Ｐ_（ｉ）の法線ベクトルｎ_（ｉ）と始点Ｐ_（ｉ）および終点Ｐ_（ｊ）を結ぶベクトルｆ_{（ｉ，ｊ）}との内積を特徴量として算出する。そして、演算部７６は、特徴量の算出に使用したサーフレットペアと、算出した物体Ｗの局所表面の特徴量とを位置・姿勢推定部７７へ出力する。

位置・姿勢推定部７７は、演算部７６から入力される物体Ｗの局所表面の特徴量と一致する特徴量が対応付けられた基準点Ｏの位置ベクトルの組をテーブル８１から複数取得する。さらに、位置・姿勢推定部７７は、始点Ｐ_（ｉ）を原点とする局所座標系を算出する。そして、位置・姿勢推定部７７は、テーブル８１から取得した位置ベクトルの組を、下記式（６）によって、始点Ｐ_（ｉ）を原点とする局所座標系における位置ベクトルの組へ変換する。

続いて、位置・姿勢推定部７７は、始点Ｐ_（ｉ）を原点とする局所座標系における変換後の位置ベクトルの組を、下記式（７）によって、実空間における位置ベクトルの組へと変換する。

これにより、図８Ｂに一点鎖線で示すように、外部基準点Ｏ_５、Ｏ_６、Ｏ_７、および、内部基準点Ｏ_８の実空間における位置ベクトルが算出される。ここで、位置・姿勢推定部７７は、図９に示すように、黒丸で示す物体Ｗ表面のサンプル点と、白丸で示す内部基準点Ｏ_８との位置関係に基づいて、外部基準点Ｏ_５、Ｏ_６、Ｏ_７の可否を判定する。なお、図９に示すＸ軸およびＹ軸は、水平面と平行な平面上に位置する直交ベクトルであり、Ｚ軸は、ＸＹ平面に対して直交し、鉛直下方を正方向にとった軸である。

そして、位置・姿勢推定部７７は、図９に示すように、内部基準点Ｏ_８が物体Ｗの内部にある場合、内部基準点Ｏ_８の位置ベクトルと同組に含まれる外部基準点Ｏ_５、Ｏ_６、Ｏ_７の位置ベクトルを、物体Ｗの位置および姿勢の推定材料として適切な基準点と判定する。

一方、内部基準点Ｏ_９のように、物体Ｗの外部にある場合、位置・姿勢推定部７７は、内部基準点Ｏ_９の位置ベクトルと同組に含まれる外部基準点を、物体Ｗの位置および姿勢の推定材料として不適切な基準点と判定し、推定材料から除外する。

具体的には、位置・姿勢推定部７７は、内部基準点Ｏ_８の実空間における座標値を（ｘ_ｋ、ｙ_ｋ、ｚ_ｋ）とし、下記式（８）によって、内部基準点Ｏ_８が物体Ｗの内部にあるか否かを検証し、外部基準点Ｏ_５、Ｏ_６、Ｏ_７の可否を判定する。

ただし、上記式（８）におけるｓｃａｎＺ（ｘ，ｙ）は、実空間におけるｘ座標値およびｙ座標値が、内部基準点Ｏ_８のｘ座標値およびｙ座標値と一致するサンプル点のｚ座標値である。

このように、位置・姿勢推定部７７は、物体Ｗの位置および姿勢の推定材料として不適切な基準点Ｏを、物体Ｗの位置および姿勢の推定材料から除外する。これにより、位置・姿勢推定部７７は、後に行われる外部基準点Ｏ_５、Ｏ_６、Ｏ_７の投票処理量を低減しつつ、物体Ｗの位置および姿勢の推定精度を向上させることができる。

なお、本実施形態では、モデルＭの内部空間に内部基準点Ｏ_４を設定したが、モデルＭの内部空間以外であれば、例えば、モデルＭの表面に内部基準点Ｏ_４を設定してもよい。ただし、かかる場合、内部基準点Ｏ_４に対応する内部基準点Ｏ_１０が物体Ｗの表面にあるか否かを、外部基準点Ｏ_５、Ｏ_６、Ｏ_７の可否判定基準とすると、可否判定基準が高くなり過ぎて、適切な基準点Ｏを不適切と誤判定するおそれがある。

そこで、位置・姿勢推定部７７では、上記式（８）に所定の閾値ｔｈを設け、外部基準点Ｏ_５、Ｏ_６、Ｏ_７の可否判定基準に一定の幅を持たせることにより、適切な基準点を不適切とする誤判定の発生を抑制する。

続いて、位置・姿勢推定部７７は、物体Ｗの位置および姿勢の判定材料として適切と判定した外部基準点Ｏ_５、Ｏ_６、Ｏ_７の実空間における位置と一致する実空間内の投票点へそれぞれ投票する処理を行う。

例えば、位置・姿勢推定部７７は、図１０の上図に示す始点Ｐ_（ｉ）を原点とする局所座標系における外部基準点Ｏ_５、Ｏ_６、Ｏ_７の位置を、図１０の下図に示す実空間で対応する３点Ａ_１、Ｂ_１、Ｃ_１へそれぞれ投票する。位置・姿勢推定部７７は、推定材料として適切と判定した全ての外部基準点について、順次実空間への投票を行う。

これにより、例えば、図１０に示す３点Ａ_１、Ｂ_１、Ｃ_１の投票点の組は、徐々に得票数が増加する。また、実空間では、３点Ａ_１、Ｂ_１、Ｃ_１以外の投票点にも、３点の外部基準点の組毎に、それぞれ実空間内の他の３投票点の組へ順次投票が行われる。位置・姿勢推定部７７は、かかる各３投票点の組毎に、各投票点の得票数を監視する。

ここで、例えば、３点の外部基準点の組に含まれる各外部基準点の各ｘ、ｙ、ｘ座標値、つまり９個の座標値で１点が決定する９次元空間内の投票点へ投票を行った場合、得票数が最多の投票点に基づいて、最も確からしい外部基準点を検出することができる。しかし、かかる場合、投票空間が膨大となり、投票に要する計算量も膨大となる。

このため、位置・姿勢推定部７７は、３点の外部基準点の組に含まれる各外部基準点の位置を３次元の実空間における独立した３投票点の組へそれぞれ投票する。これにより、投票に要する計算量を大幅に低減することができる。ただし、位置・姿勢推定部７７では、本来一つの３投票点の組へ投票されるべき３票のうち、いずれかが別の投票点の組へ投票される所謂干渉現象が発生する。

そこで、位置・姿勢推定部７７は、３投票点の組に含まれる各投票点のうち、投票数が最小の投票点の得票数が所定の閾値Ｖｔｈに達した場合に、その３投票点の組をリストアップする。そして、位置・姿勢推定部７７は、リストアップした３投票点の組毎に総得票数を集計し、得票数が多い組の順に、その組に含まれる投票点の実空間における位置に基づいて、物体Ｗの位置および姿勢を推定する。

例えば、位置・姿勢推定部７７は、図１０に示す３点Ａ_１、Ｂ_１、Ｃ_１の組の中で、点Ａ_１の得票数が最も少ないと、点Ａ_１の得票数が閾値Ｖｔｈに達した場合に、３点Ａ_１、Ｂ_１、Ｃ_１の組をリストアップする。これにより、例えば、干渉によって、２点Ｂ_１、Ｃ_１の得票数が増大しても、点Ａ_１の得票数が閾値Ｖｔｈに達しない限り、適切か否かが未定の３点Ａ_１、Ｂ_１、Ｃ_１の組を早まって適切な得票点の組と判定することを抑制することができる。

続いて、位置・姿勢推定部７７は、推定した複数の物体Ｗの位置および姿勢を示す情報を把持対象情報生成部７８へ出力する。把持対象情報生成部７８は、位置・姿勢推定部７７から入力される情報に基づいて、ロボット２の把持候補となる物体Ｗの有無を判定し、把持候補がある場合、把持候補の位置および姿勢を示す情報を、ない場合、その旨を示す情報をロボット制御装置５へ出力する。

次に、図１１〜図１３を参照し、物体検出装置４の処理部７が実行する処理について説明する。図１１は、実施形態に係るテーブル８１の作成処理を示すフローチャートである。図１２は、実施形態に係る物体Ｗの位置および姿勢の検出処理を示すフローチャートである。図１３は、実施形態に係る把持候補に関する情報の作成処理を示すフローチャートである。

物体検出装置４の処理部７は、物体Ｗの位置および姿勢を検出する以前に、予め図１１に示す処理を実行することによってテーブル８１を作成する。具体的には、処理部７は、図１１に示すように、まず、モデルデータを取得し（ステップＳ１０１）、モデルデータからモデルＭのエッジＥｄを抽出する（ステップＳ１０２）。

続いて、処理部７は、モデルＭの表面から選択する始点Ｐ_ｉのモデルＭ表面における位置に対応する変数ｉへ１を設定する（ステップＳ１０３）。その後、処理部７は、モデルＭの表面から選択する終点Ｐ_ｊのモデルＭ表面における位置に対応する変数ｊへ１を設定する（ステップＳ１０４）。

続いて、処理部７は、変数ｉの値と変数ｊの値とが等しくないか否かを判定する（ステップＳ１０５）。つまり、処理部７は、モデルＭの表面から選択する始点Ｐ_ｉと終点Ｐ_ｊとが同一点か否かを判定する。なお、ここでは、変数ｉの値および変数ｊの値は、ともに１である。このため、処理部７は、始点Ｐ_ｉと終点Ｐ_ｊとが同一点であると判定し（ステップＳ１０５，Ｎｏ）、処理をステップＳ１０９へ移す。

ステップＳ１０９において、処理部７は、処理を変数ｊのループの先頭、つまり、ステップＳ１０４へ移す。ステップＳ１０４において、処理部７は、変数ｊの値へ１を加算し、処理をステップＳ１０５へ移す。以後、処理部７は、処理がステップＳ１０９まで進む度に、ステップＳ１０４で変数ｊの値へ１を加算し、変数ｊの値がＭ_ｊに達した後に、処理がステップＳ１０９まで進むと、処理をステップＳ１１０へ移す。つまり、処理部７は、モデルＭの表面からＭ_ｊ個の終点Ｐ_ｊを選択する。

また、処理部７は、ステップＳ１０５において、変数ｉの値と変数ｊの値とが等しくないと判定した場合（ステップＳ１０５，Ｙｅｓ）、処理をステップＳ１０６へ移す。ステップＳ１０６において、処理部７は、始点Ｐ_ｉの位置ベクトルｐ_ｉと、始点Ｐ_ｉの法線ベクトルｎ_ｉと、終点Ｐ_ｊの位置ベクトルｐ_ｊとから、始点Ｐ_ｉを原点とする局所座標系を算出する。

続いて、処理部７は、算出した局所座標系における各基準点Ｏの位置ベクトルを算出する（ステップＳ１０７）。ここで、処理部７は、始点Ｐ_ｉおよび終点Ｐ_ｊを含むモデルＭの局所表面の特徴量であるサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉の特徴量についても算出する。

続いて、処理部７は、サーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉の特徴量をキーとしたテーブル８１に算出した基準点Ｏの位置ベクトルを格納し（ステップＳ１０８）、処理をステップＳ１０９へ移す。そして、処理部７は、一つの始点Ｐ_ｉと、Ｍ_ｊ個の終点Ｐ_ｊとについて、ステップＳ１０４〜ステップＳ１０９の処理を繰り返し、その後、処理をステップＳ１１０へ移す。

ステップＳ１１０において、処理部７は、処理を変数ｉのループの先頭、つまり、ステップＳ１０３へ移す。ステップＳ１０３において、処理部７は、変数ｉの値へ１を加算し、処理をステップＳ１０４へ移す。以後、処理部７は、処理がステップＳ１１０まで進む度に、ステップＳ１０３で変数ｉの値へ１を加算し、変数ｉの値がＭ_ｉに達した後に、処理がステップＳ１１０まで進むと、処理をステップＳ１１１へ移す。

つまり、処理部７は、モデルＭの表面からＭ_ｉ個の始点Ｐ_ｉを選択し、Ｍ_ｉ個の各始点Ｐ_ｉと、Ｍ_ｊ個の終点Ｐ_ｊとについて、ステップＳ１０３〜ステップＳ１１０の処理を繰り返した後、処理をステップＳ１１１へ移す。最後に、処理部７は、作成したテーブル８１を記憶部８に保存して、処理を終了する。

なお、テーブル８１は、物体検出装置４以外の装置によって作成されてもよい。かかる場合、物体検出装置４は、他の装置から作成されたテーブル８１を取得して、記憶部８に保存する。

また、処理部７は、図１２に示す処理を実行することによって、物体Ｗの位置および姿勢を検出するサーフェスマッチングを行う。具体的には、処理部７は、図１２に示すように、まず、センサ３からバラ積みされた物体Ｗのシーンデータを取得し（ステップＳ２０１）、シーンデータから物体Ｗのエッジを抽出する（ステップＳ２０２）。

続いて、処理部７は、シーンデータから各サンプル点の法線ベクトルを算出する。ここで、処理部７は、エッジを形成するサンプル点については、勾配ベクトルも算出する（ステップＳ２０３）。続いて、処理部７は、物体Ｗの表面から選択する始点Ｐ_（ｉ）の物体Ｗ表面における位置に対応する変数ｉへ１を設定し、（ステップＳ２０４）、シーンデータから始点Ｐ_（ｉ）のサーフレットΔ（ｐ_ｉ）を取得する（ステップＳ２０５）。

その後、処理部７は、物体Ｗの表面から選択する終点Ｐ_（ｊ）の物体Ｗ表面における位置に対応する変数ｊへ１を設定し（ステップＳ２０６）、シーンデータから終点Ｐ_（ｊ）のサーフレットΔ（ｐ_ｊ）を取得する（ステップＳ２０７）。

続いて、処理部７は、取得したサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉が制約条件を満たすか否かを判定する（ステップＳ２０８）。ここで、処理部７は、例えば、始点Ｐ_（ｉ）および終点Ｐ_（ｊ）がいずれも面上の点である場合、始点Ｐ_（ｉ）の法線ベクトルｎ_（ｉ）と終点Ｐ_（ｊ）の法線ベクトルｎ_（ｊ）とのなす角度が所定の閾値Ｔ_ｆより大きい場合に、制約条件を満たすと判定する。一方、始点Ｐ_（ｉ）の法線ベクトルｎ_（ｉ）と終点Ｐ_（ｊ）の法線ベクトルｎ_（ｊ）とのなす角度が所定の閾値Ｔ_ｆ以下の場合、処理部７は、制約条件を満たさないと判定する。

また、処理部７は、例えば、始点Ｐ_（ｉ）が面上の点であり、終点Ｐ_（ｊ）がエッジ上の点である場合、始点Ｐ_（ｉ）の法線ベクトルｎ_（ｉ）と終点Ｐ_（ｊ）の勾配ベクトルｇ_（ｊ）とのなす角度が所定の閾値Ｔ_ｅより大きい場合に、制約条件を満たすと判定する。一方、始点Ｐ_（ｉ）の法線ベクトルｎ_（ｉ）と終点Ｐ_（ｊ）の勾配ベクトルｇ_（ｊ）とのなす角度が所定の閾値Ｔ_ｅ以下の場合、処理部７は、制約条件を満たさないと判定する。

かかる、制約条件を設けることにより、処理部７は、際立った特徴が表れ難い始点Ｐ_（ｉ）およびＰ_（ｊ）のペアについて、無駄にサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉を取得することを防止することができるので、処理量を低減することができる。

そして、処理部７は、取得したサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉が制約条件を満たすと判定した場合（ステップ２０８，Ｙｅｓ）、処理をステップＳ２０９へ移す。一方、制約条件を満たさないと判定した場合（ステップＳ２０８，Ｎｏ）、処理部７は、処理をステップＳ２１８へ移す。

ステップＳ２０９において、処理部７は、テーブル８１から、ステップＳ２０５およびステップＳ２０７で取得したサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉と特徴量が一致する全サーフレットペア（Ｎｖ個）の基準点Ｏの位置ベクトルの組を取得する。

続いて、処理部７は、ステップＳ２０５およびステップＳ２０７で取得したサーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉の局所座標系を算出する（ステップＳ２１０）。つまり、処理部７は、サーフレットペア〈Δ（ｐ_ｊ）、Δ（ｐ_ｉ）〉に基づいて、始点Ｐ_（ｉ）を原点とする局所座標系を算出する。

その後、処理部７は、ステップＳ２０９で取得した位置ベクトルの組の取得順番を示す変数ｒに１を設定する（ステップＳ２１１）。続いて、処理部７は、ステップＳ２１０で算出した局所座標系と、ｒ番目の基準点Ｏの位置ベクトルの組とに基づいて、各基準点Ｏの実空間における位置ベクトルの組を算出する（ステップＳ２１２）。

ここで、処理部７は、内部基準点Ｏ_８が物体Ｗの内部にあるか否かを判定する（ステップＳ２１３）。そして、処理部７は、内部基準点Ｏ_８が物体Ｗの内部にあると判定した場合（ステップＳ２１３，Ｙｅｓ）、処理をステップＳ２１４へ移す。一方、内部基準点Ｏ_８が物体Ｗの外部にあると判定した場合（ステップＳ２１３，Ｎｏ）、処理をステップＳ２１７へ移す。

ステップＳ２１４において、処理部７は、ステップＳ２１３で物体Ｗの内部にあると判定された内部基準点Ｏ_８と同じ組に含まれる外部基準点Ｏ_５、Ｏ_６、Ｏ_７の位置ベクトルと一致する実空間内の投票点へ投票を行う。そして、処理部７は、ステップＳ２１４で投票した３点の投票点の組の中で、投票数が最小の投票点の得票数が閾値Ｖｔｈに達したか否かの判定を行う（ステップＳ２１５）。

ここで、処理部７は、閾値Ｖｔｈに達したと判定した場合（ステップＳ２１５，Ｙｅｓ）、処理をステップＳ２１６へ移し、ステップＳ２１４で投票した３点の投票点の組をリストアップして（ステップＳ２１６）、処理をステップＳ２１７へ移す。一方、閾値Ｖｔｈに達していないと判定した場合（ステップ２１５，Ｎｏ）、処理をステップＳ２１７へ移す。

ステップＳ２１７において、処理部７は、処理を変数ｒのループの先頭、つまり、ステップＳ２１１へ移す。ステップＳ２１１において、処理部７は、変数ｒの値へ１を加算する。以後、処理部７は、処理がステップＳ２１７まで進む度に、ステップＳ２１１で変数ｒの値へ１を加算し、変数ｒの値がＮｖに達した後に、処理がステップＳ２１７まで進むと、処理をステップＳ２１８へ移す。つまり、処理部７は、ステップＳ２０９で取得した基準点Ｏの位置ベクトルの全組について、ステップＳ２１２〜ステップＳ２１６の処理を行う。

ステップＳ２１８において、処理部７は、処理を変数ｊのループの先頭、つまり、ステップＳ２０６へ移す。ステップＳ２０６において、処理部７は、変数ｊの値へ１を加算する。以後、処理部７は、処理がステップＳ２１８まで進む度に、ステップＳ２０６で変数ｊの値へ１を加算し、変数ｊの値がＮｈに達した後に、処理がステップＳ２１８まで進むと、処理をステップＳ２１９へ移す。

ステップＳ２１９において、処理部７は、処理を変数ｉのループの先頭、つまり、ステップＳ２０４へ移す。ステップＳ２０４において、処理部７は、変数ｉの値へ１を加算する。以後、処理部７は、処理がステップＳ２１９まで進む度に、ステップＳ２０４で変数ｉの値へ１を加算し、変数ｉの値がＮｔに達した後に、処理がステップＳ２１９まで進むと、処理をステップＳ２２０へ移す。

ステップＳ２２０において、処理部７は、リストアップした投票点の組毎に総得票数を集計し、総得票数が高い順に投票点の組をソートする。続いて、処理部７は、総得票数の高い上位Ｇ個の投票点の組から物体Ｗの位置および姿勢を算出し、（ステップＳ２２１）、処理を終了する。

また、処理部７は、図１３に示す処理を実行することによって、ロボット２の把持候補となる物体Ｗに関する情報を生成し、ロボット制御装置５へ出力する。具体的には、処理部７は、図１３に示すように、まず、シーンデータを計測する（ステップＳ３０１）。

続いて、図１２に示す処理（サーフェスマッチング）によって算出したＧ個の把持候補を検出する（ステップＳ３０２）。そして、処理部７は、把持候補に対する後述の詳細マッチングを行う順番を示す変数ｔに１を設定し（ステップＳ３０３）、処理をステップＳ３０４へ移す。ステップＳ３０４において、処理部７は、変数ｔの値がＧ未満であるか否かを判定する（ステップＳ３０４）。

ここで、処理部７は、変数ｔの値がＧ未満であると判定した場合（ステップＳ３０４，Ｙｅｓ）、処理をステップＳ３０５へ移す。一方、変数ｔの値がＧに達したと判定した場合（ステップＳ３０４，Ｎｏ）、把持候補なしフラグをロボット制御装置５へ設定し（ステップＳ３１０）、処理を終了する。

ステップＳ３０５において、処理部７は、ｔ番目の把持候補に対して詳細マッチングを行う。例えば、処理部７は、詳細マッチングとして、ＩＣＰ(Iterative Closest Point)アルゴリズムによる詳細なパターンマッチングを行う。そして、処理部７は、詳細マッチングに成功したか否かを判定する（ステップＳ３０６）。

ステップＳ３０６において、処理部７は、詳細マッチングに成功したと判定した場合（ステップＳ３０６，Ｙｅｓ）、処理をステップＳ３０７へ移す。一方、詳細マッチングに失敗したと判定した場合（ステップＳ３０６，Ｎｏ）、処理をステップＳ３１１へ移す。

ステップＳ３０７において、処理部７は、把持候補に対してロボット２がピッキング動作を行う際にハンド２５が干渉するかどうかのハンド干渉チェックを行う。そして、処理部７は、干渉がないか否かを判定し（ステップＳ３０８）、干渉がないと判定した場合（ステップＳ３０８，Ｙｅｓ）、処理をステップＳ３０９へ移す。

一方、干渉があると判定した場合（ステップＳ３０８，Ｎｏ）、処理をステップＳ３１１へ移す。ステップＳ３１１において、処理部７は、変数ｔの値へ１を加算し、処理をステップＳ３０４へ移す。また、ステップＳ３０９において、処理部７は、ステップＳ３０８で干渉なしと判定した把持候補の位置および姿勢（ピッキング位置および姿勢）をロボット制御装置５へ設定して、処理を終了する。

上述したように、実施形態に係る物体検出方法では、物体のモデルの外部空間に、物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、モデルの内部空間に、推定材料の可否判定材料となる内部基準点を設定する。そして、モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、始点に対する外部基準点および内部基準点の位置の組とを対応付けたテーブルを記憶する。

さらに、実施形態に係る物体検出方法では、実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、始点および終点の対を含む局所表面における物体の特徴量を算出する。

そして、テーブルから物体の特徴量と一致する特徴量が対応付けられた位置の組を取得して実空間における位置の組へ変換し、位置の組に含まれる内部基準点の位置が物体の外部にある場合に、位置の組に含まれる外部基準点の位置を推定材料から除外して物体の位置および姿勢を推定する。かかる物体検出方法によれば、物体の位置および姿勢の検出に要する処理量を低減しつつ、検出精度を向上させることができる。

また、実施形態に係る物体検出方法では、変換後の位置の組に含まれる内部基準点の位置が物体の内部にある場合に、位置の組に含まれる複数の外部基準点の位置と一致する実空間内の投票点へそれぞれ投票する。そして、一つの位置の組に含まれる複数の外部基準点の位置に基づいて投票が行われた投票点の組の中で、投票数が最小の投票点の得票数が所定の閾値に達した場合に、投票点の組をリストアップする。

その後、リストアップした投票点の組毎に総得票数を集計し、総得票数が多い組の順に組に含まれる投票点の実空間における位置に基づいて、物体の位置および姿勢を推定する。これにより、本来投票されるべきでない投票点へ投票される所謂干渉が発生しても、物体の位置および姿勢に関する推定精度が低下することを抑制することができる。

また、実施形態に係る物体検出方法では、物体およびモデルのエッジを検出し、モデルの表面に位置する点群および物体の表面に位置するサンプル点群をエッジ上の点と、面上の点とに分類する。

そして、物体の形状に応じて、始点として選択する点と終点として選択する点とをエッジ上の点および面上の点から選択する。これにより、実施形態に係る物体検出方法では、物体の形状に応じた、より特徴的な局所表面の特徴量に基づいて、物体の位置および姿勢を推定することができるので汎用性を向上させることができる。

また、実施形態に係る物体検出方法では、サンプル点から選択する始点として、面上の点を選択し、サンプル点から選択する終点として、エッジ上の点を選択することにより、物体に占める平面部分の面積が比較的大きな物体の検出精度を向上させることができる。

また、実施形態に係る物体検出方法では、サンプル点から選択する始点および終点として、始点における法線ベクトルと、終点における法線ベクトルまたは勾配ベクトルとのなす角度が所定の閾値より大きい２点を選択する。これにより、物体の比較的特徴的でない部分の局所表面については、位置および姿勢の推定材料から除外することができるので、位置および姿勢の検出に要する処理量を低減することができる。

さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。

１ ロボットシステム
２ ロボット
３ センサ
４ 物体検出装置
５ ロボット制御装置
７ 処理部
７１ モデルデータ取得部
７２ シーンデータ取得部
７３ 基準点設定部
７４ エッジ検出部
７５ サーフレット選択部
７６ 演算部
７７ 位置・姿勢推定部
７８ 把持対象情報生成部
８ 記憶部
８１ テーブル
Ｗ 物体
Ｍ モデル

Claims (6)

1. 物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する工程と、
   前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する工程と、
   実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する工程と、
   前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する工程と
   を含むことを特徴とする物体検出方法。
2. 前記変換後の位置の組に含まれる前記内部基準点の位置が物体の内部にある場合に、該位置の組に含まれる複数の前記外部基準点の位置と一致する実空間内の投票点へそれぞれ投票する工程と、
   一つの前記位置の組に含まれる複数の前記外部基準点の位置に基づいて投票が行われた投票点の組の中で、投票数が最小の投票点の得票数が所定の閾値に達した場合に、該投票点の組をリストアップする工程と、
   リストアップした前記投票点の組毎に総得票数を集計し、前記総得票数が多い組の順に該組に含まれる前記投票点の実空間における位置に基づいて、前記物体の位置および姿勢を推定する工程と
   をさらに含むことを特徴とする請求項１に記載の物体検出方法。
3. 前記物体および前記モデルのエッジを検出する工程と、
   前記モデルの表面に位置する点群および前記物体の表面に位置する前記サンプル点群を前記エッジ上の点と、面上の点とに分類する工程と、
   前記物体の形状に応じて、前記始点として選択する点と前記終点として選択する点とを前記エッジ上の点および前記面上の点から選択する工程と
   をさらに含むことを特徴とする請求項１または２に記載の物体検出方法。
4. 前記サンプル点群から選択する前記始点として、前記面上の点を選択する工程と、
   前記サンプル点群から選択する前記終点として、前記エッジ上の点を選択する工程と
   をさらに含むことを特徴とする請求項３に記載の物体検出方法。
5. 前記サンプル点群から選択する前記始点および終点として、前記始点における法線ベクトルと、前記終点における法線ベクトルまたは勾配ベクトルとのなす角度が所定の閾値より大きい２点を選択する工程
   をさらに含むことを特徴とする請求項１〜４のいずれか一つに記載の物体検出方法。
6. 物体のモデルの外部空間に、前記物体の位置および姿勢の推定材料となる複数の外部基準点を設定し、前記モデルの内部空間に、前記推定材料の可否判定材料となる内部基準点を設定する設定部と、
   前記モデルの表面に位置する点群から順次選択する始点および終点の対を含む局所表面の特徴量と、前記始点に対する前記外部基準点および前記内部基準点の位置の組とを対応付けたテーブルを記憶する記憶部と、
   実空間に存在する物体の表面に位置するサンプル点群から始点および終点の対を順次選択し、該始点および終点の対を含む局所表面における前記物体の特徴量を算出する演算部と、
   前記テーブルから前記物体の特徴量と一致する特徴量が対応付けられた前記位置の組を取得して実空間における位置の組へ変換し、該位置の組に含まれる前記内部基準点の位置が前記物体の外部にある場合に、該位置の組に含まれる前記外部基準点の位置を前記推定材料から除外して前記物体の位置および姿勢を推定する推定部と
   を備えることを特徴とする物体検出装置。

Images