JP6572687B2

JP6572687B2 - 把持可否判定方法

Info

Publication number: JP6572687B2
Application number: JP2015172824A
Authority: JP
Inventors: 宏平菊地; 原田　研介; 研介原田; 偉偉万; 徳生辻; 弘音田; 和之永田
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2015-09-02
Filing date: 2015-09-02
Publication date: 2019-09-11
Anticipated expiration: 2035-09-02
Also published as: JP2017047505A

Description

本発明は、箱の中にバラ積みされた複数の把持対象物をロボットアームのハンドによって把持する際に、把持対象物がハンドにより把持可能か否かを判定する把持可否判定方法に関するものである。

バラ積みされた把持対象物をロボットアームのハンドによって把持する際に、ロボットアームが他の物体と干渉する場合、当該ロボットアームの動作を停止させ、あるいは、その物体を回避する装置が知られている（特許文献１参照）。

特開２００２−３３１４８０号公報

上記装置においては、ロボットアームが他の物体と干渉する場合、当該ロボットアームの動作停止などさせるため、把持可能な部品数が少なくなる虞がある。一方で、仮に、その他の物体を無視して把持を実行すれば、把持可能と判断する部品数は多くなるが、例えば、他の物体と強く衝突し把持成功率が低下する虞がある。
本発明は、このような問題点に鑑みてなされたものであり、把持可能な部品数を増加させつつ、把持成功率は高めることができる把持可否判定方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、バラ積みされた複数の把持対象物をロボットアームのハンドによって把持する際に、前記ハンドが前記把持対象物を把持可能か否かを判定する把持可否判定方法であって、前記把持対象物から所定距離離れたアプローチ点から該把持対象物を前記ハンドにより把持する把持点まで前記ハンドを移動させ、該把持点で前記把持対象物を前記ハンドにより把持するまでに、前記ハンドが通過する領域をハンド通過領域とし、該ハンド通過領域内に含まれる３次元点群データと、前記ハンドによる前記把持対象物の把持の成否と、を対応付けた複数のサンプルデータを取得するステップと、前記ハンド通過領域内に含まる３次元点群のｉ番目の点をｐ_ｉ（ｉ＝１、２、・・・ｎ）とし、前記点ｐ_ｉをアプローチ方向に伸ばした直線が前記ハンド通過領域の境界と交わる点の内、前記点ｐ_ｉから最も遠い点との距離をｈ（ｐ_ｉ）とし、前記点ｐ_ｉから前記ハンドの把持方向に伸ばした直線が前記ハンド通過領域の境界と交わる点の内、前記点ｐ_ｉから最も近い点との距離をｄ（ｐ_ｉ）とし、前記ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）及びｎの値が小さい方が把持成功率が高いと仮定し、前記サンプルデータと前記距離ｈ（ｐ_ｉ）及びｄ（ｐ_ｉ）とに基づき算出した特徴ベクトルから、前記把持対象物の把持可否を判定するための識別器を生成するステップと、前記生成した識別器を用いて前記ハンドによる前記把持対象物の把持可否を判定するステップと、を含む、ことを特徴とする把持可否判定方法である。
この一態様によれば、ハンド通過領域に障害物が存在する場合でも、直ちに、その把持対象物を把持不可能と判断するのではなく、識別器を用いて把持可否の判定を行う。このため、把持可能な部品数を増加させることができる。この把持可否の判定を行う際に、ハンドと障害物との強干渉点が少なく、把持成功率が高いハンドの把持パターンを学習した識別器を用いることで、把持成功率は高めることができる。すなわち、把持可能な部品数を増加させつつ、把持成功率は高めることができる。
この一態様において、前記識別器はランダムフォレストで構成されており、前記特徴ベクトルは、下記式を用いて算出してもよい。この一態様によれば、ランダムフォレストで構成された識別器を用いて、把持可能な部品数を増加させつつ、把持成功率は高めることができる。
この一態様において、前記識別器は、線形ＳＶＭ（support vector machine）で構成されており、前記特徴ベクトルｆ_ｓは、下記式を用いて算出してもよい。この一態様によれば、線形ＳＶＭで構成された識別器を用いて、把持可能な部品数を増加させつつ、把持成功率は高めることができる。

本発明によれば、把持可能な部品数を増加させつつ、把持成功率は高めることができる把持可否判定方法を提供することができる。

本発明の一実施形態に係る搬送ロボットの概略的なシステム構成を示すブロック図である。ロボットアームのハンドのピッキング動作を示す図である。ハンド通過領域を示す図である。距離ｈ（ｐ_ｉ）およびｄ（ｐ_ｉ）を示す図である。学習フェーズ及び実行フェーズを示す図である。学習フェーズにおける把持動作軌道の生成方法のフローを示すフローチャートである。実行フェーズにおける把持動作軌道の生成方法のフローを示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。
本実施の形態の搬送ロボットは、複数の箱内に収容されたピッキング対象物（把持対象物）である部品を当該箱から夫々ピッキング（把持）して搬送する。

図１は、本発明の一実施形態に係る搬送ロボットの概略的なシステム構成を示すブロック図である。本実施形態に係る搬送ロボット１は、ロボットアーム２と移動台車３と、ロボットアーム２及び移動台車３を制御する制御装置４と、周辺の環境情報を検出する検出部５と、を備えている。

ロボットアーム２は、移動台車３に固定されている。ロボットアーム２は、例えば、手首関節、肘関節、肩関節などの複数の関節部を有する多関節型アームとして構成されている。ロボットアーム２の先端部には、一対の指部（グリッパ）を有するハンド（以下、単にハンドと称す）が設けられている。各関節部に各関節部を駆動するモータなどのアクチュエータが設けられている。各関節部には各関節部の回転角を検出するエンコーダ、ポテンショメータ、レゾルバーなどの回転センサが設けられている。制御装置４は、各関節部のアクチュエータに制御信号を送信することで各関節部のアクチュエータを制御する。また、制御装置４は、一対の指部のアクチュエータを制御することで、指部の開閉動作を制御する。これにより、ハンドの指部は部品を把持する。なお、上記ロボットアーム２の構成は一例であり、これに限定されず、任意の構成が適用可能である。

移動台車３は、例えば、左右の駆動輪、駆動輪を駆動するモータ、減速機及びエンコーダなどを備えている。また、移動台車３は、部品を載置する通箱が設けられている。搬送ロボット１は、ロボットアーム２により把持した部品を通箱に載置し、搬送を行う。エンコーダは、検出したモータの回転情報を制御装置４に出力する。制御装置４は、エンコーダから出力される回転情報に基づいて、各モータに制御信号を送信することで移動台車３の駆動輪を制御する。

制御装置４は、検出部５から出力される周辺の環境情報（以下、周辺環境情報）に基づいて、ロボットアーム２及び移動台車３の駆動を制御する。
制御装置４は、例えば、制御演算、演算処理等を行うＣＰＵ（Central Processing Unit）４ａ、ＣＰＵ４ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ４ｂ、外部と信号の入出力を行う複数のインターフェイス部（Ｉ／Ｆ）４ｃなどからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ４ａ、メモリ４ｂ及びインターフェイス部４ｃは、データバスなどを介して相互に接続されている。

検出部５は、搬送ロボット１の周辺の環境情報を検出する。検出部５は視覚センサ５１を有している。視覚センサ５１としては、例えば、Ｋｉｎｅｃｔ（登録商標）センサなどを用いることができる。但し、検出部５は、搬送ロボット１の周辺の環境情報を検出することができるセンサであれば、特に限定されない。検出部５は、検出した周辺の環境情報を制御装置４に出力する。

搬送ロボット１は、例えば、ロボットアーム２を用いて複数の箱内に収容された把持対象物の部品を当該箱から夫々把持し、移動台車３の通箱に載置して搬送を行う。ここで、特定の箱の中にバラ積みされた複数の部品をロボットアーム２のハンドによって把持する際に、その部品がハンドにより把持可能（ピッキング可能）か否かを判定する必要がある。

図２は、ロボットアームのハンドのピッキング動作を示す図である。ロボットアーム２が各部品を把持する際に、まず、制御装置４はロボットアーム２の各関節部のアクチュエータを制御して、ハンド２１を部品から一定距離離れたアプローチ点に移動させる（ａ）。その後、制御装置４は、ロボットアーム２の各関節部のアクチュエータを制御して、ハンド２１を、そのアプローチ点から把持点に移動させる（ｂ）。そして、制御装置４は、その把持点でハンド２１の指部２２が部品を把持するように、ハンド２１のアクチュエータを制御する（ｃ）。

ところで、上述したハンドがアプローチ点から把持点まで移動する際にハンドが通過する第１通過領域、および、ハンドが把持点で把持を完了するまでに通過する第２通過領域（第１及び第２通過領域をSweeping Volumeとし、以下、ハンド通過領域（斜線部分）Ｓと称す）内に障害物が存在することがある（図３）。

このようにハンド通過領域Ｓに障害物が存在する場合（ハンド２１が障害物と干渉する場合）、単に、その部品を把持不可能と判断すれば、実際に把持を実行した際の把持成功率は高まるが把持可能な部品数は少なくなる虞がある。
一方で、そのハンド通過領域Ｓの障害物を無視して把持を実行すれば、把持可能と判断する部品数は多くなる。しかし、例えば、ハンド２１が障害物と強く衝突し、実際に把持を実行した際の把持成功率が低下する虞がある。

これに対し、本実施形態に係る把持可否判定方法においては、ハンド通過領域Ｓのどの位置に障害物があると、ハンド２１による部品の把持が失敗（あるいは成功）するかを学習した識別器を生成し、この識別器を用いて、ハンド２１による部品の把持可否を判定する。これにより、ハンド通過領域Ｓに障害物が存在する場合でも、直ちに、その部品を把持不可能と判断するのではなく、識別器を用いて把持可否の判定を行う。このため、把持可能な部品数を増加させることができる。この把持可否の判定を行う際に、ハンド通過領域Ｓのどの位置に障害物があると、ハンド２１による部品の把持が失敗（あるいは成功）するかを学習した識別器を用いる。このため、たとえハンド通過領域Ｓ内に障害物がある場合でも、その把持成功率を高めることができる。すなわち、把持可能な部品数を増加させつつ、把持成功率は高めることができる。

例えば、ハンド２１がバラ積みされた他の部品に掠るあるいは軽く押す程度に接触するのであれば（このような場合は把持成功率が高いと推定される）、その部品を押しのけて把持対象の部品を把持できる場合がある。したがって、このような、従来、ハンド２１が他の部品と干渉し把持不可能と判断されていた部品が把持可能となる。
本実施形態に係る把持可否判定方法は、上述の如く、ロボットアーム２によるバラ積みされた部品の把持作業において、ハンド２１と、把持対象部品と隣接する部品と、の接触を適度に許容することで、把持可能な部品数を増加させつつ、把持成功率は高めるものである。

本実施形態に係る把持可否判定方法においては、ハンド通過領域Ｓ内に障害物がある場合の把持成功条件（ハンド通過領域Ｓのどの位置に障害物があると、ハンド２１による部品の把持が成功かの条件）を、以下の学習によって導出した識別器を生成する。すなわち、把持成功率の高い把持パターンを学習した識別器を生成する。

制御装置４は、過去のハンドの把持動作におけるハンド通過領域Ｓ内に含まれる障害物を表すポイントクラウド（３次元点群データ）と、ハンド２１による把持対象の部品の把持成否と、を対応付けたサンプルデータを学習データとして取得する。そして、制御装置４は、取得した学習データを用いて、把持成否を判定するための識別器を生成する。なお、上記ポイントクラウドは、把持対象の部品付近の点を除いたものである。

本実施形態において、障害物（ポイントクラウド）がハンド２１からどれ位離れているか、換言すると、障害物がハンド２１にどれ位干渉するかを、特徴ベクトルで表現した識別器を生成する。ここで、ハンド通過領域内に含まれるｉ番目の点をｐ_ｉ（ｉ＝１、２、・・・、ｎ）とし、下記変数を定義する（図４）。

ｈ（ｐ_ｉ）：点ｐ_ｉからアプローチ点方向に伸ばした直線がハンド通過領域境界と交わる点の内、ｐ_ｉから最も遠い点との距離。その点ｐ_ｉをアプローチ方向に押しのける距離。
ｄ（ｐ_ｉ）：点ｐ_ｉから指部の開閉方向（把持方向）に伸ばした直線がハンド通過領域境界と交わる点の内、ｐ_ｉから最も近い点との距離。その点ｐ_ｉがハンド通過領域の内側（指部２２の閉方向）に入り込んだ距離。

上記ｈ（ｐ_ｉ）及びｄ（ｐ_ｉ）が大きい場合、障害物がハンド２１に強干渉することを示している。
ここで、ハンド２１と障害物との強干渉する点を少なくすると、ハンド２１による部品把持の成功率が高まると仮定する。すなわち、ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）及びｎの全ての値が小さい方が把持成功率が高いと仮定する。そして、この仮定の基で把持成功率が高いときに生成された特徴ベクトルを用いて識別器を生成している。

以上のように、本実施形態においては、上述したように、点ｐｉの距離ｈ（ｐ_ｉ）および距離ｄ（ｐ_ｉ）を定義し、ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）及びｎの値が小さい方が把持成功率が高いと仮定し、サンプルデータおよび距離をｈ（ｐ_ｉ）およびｄ（ｐ_ｉ）に基づいて算出した特徴ベクトルから、把持可能か否かを判定するための識別器を生成する。この識別器を用いることで、ハンド通過領域Ｓに障害物が存在する場合（ハンド２１が障害物と接触する場合）でも、ハンド２１と障害物との強干渉点が少なく、把持成功率が高いハンド２１の把持パターンを選択できるため、把持成功率は高めることができる。

識別器は、例えば、線形ＳＶＭ（support vector machine）又はランダムフォレスト（Random Forest）で構成されている。なお、識別器は、上記一例に限定されず、任意の学習アルゴリズムで構成できる。

まず、上記ランダムフォレストの特徴ベクトルの定義方法について説明する。
特徴ベクトルとして、ハンド通過領域内のポイント分布を用いる。このため、ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）を用いた２次元ヒストグラムを下記のように定義する。なお、下記定義において、binwidthはbin幅であり、bin_h及びbin_dは各軸のbin上限値（無条件で把持失敗と判定する閾値）である。
特徴ベクトル＝（底面からの距離：min（（h（p_i）/binwidth）,bin_h）,
側面からの距離：min（（d（p_i）/binwidth）,bin_d））

目的変数を列挙型（把持成功、把持失敗）する。制御装置４は、サンプルデータから各葉ノード到達時の把持成功率を算出することで、任意の特徴ベクトルに対して把持成功率を算出する。なお、識別器が、ランダムフォレストで構成されている場合、上記サンプルデータは領域の偏りのないランダムなデータを用いるのが好ましい。制御装置４は、状況に応じて、把持成功率の閾値を設定することで、把持可否判定を行ってもよい。把持成功率を高めたい場合は、この閾値を大きく設定する。

続いて線形ＳＶＭの特徴ベクトルの定義方法について説明する。
特徴ベクトルを、下記２次元ベクトルｆ_ｓとして定義する。

制御装置４は、サンプルデータに基づいて各領域での把持成功率を算出する。制御装置４は、任意の特徴ベクトルに対して属する領域の識別を行うことで、把持成功率を算出する。また、制御装置４は、状況に応じて、閾値を設定することで、把持可否判定を行う。把持成功率を高めたい場合は、この閾値を大きく設定する。

本実施形態に係る制御装置４は、図５に示す如く、上述した識別器を生成する学習フェーズと、学習フェーズで生成した識別器を用いて、実際の把持動作を行う実行フェーズと、を実行する。

学習フェーズにおいて、制御装置４は、部品情報、搬送ロボット１の幾何情報などの前提条件の取得を行う（１）。次に、制御装置４は、部品毎の把持可能位置・姿勢を導出して、把持位置姿勢データベースの構築を行う（２）。その後、検出部５は部品の位置・姿勢を検出する（３）。さらに、制御装置４は、把持位置姿勢データベースおよび検出された部品の位置・姿勢に基づいて、実際に部品をピッキングするようにロボットアーム２を制御する（４）。このとき、制御装置４は、上述したように、ハンド通過領域内に含まれるポイントクラウドと、ハンド２１による把持対象の部品の把持成否と、を対応付けたサンプリングデータを学習データとして、把持成否を学習し識別器を生成する（５）。

続いて、実行フェーズにおいて、検出部５は部品の位置・姿勢を検出する（１）。次に、制御装置４は、上記識別器を用いて把持成功率の高い把持位置姿勢を選択し、検出した部品の位置・姿勢に基づいて、部品をピッキングするようにロボットアーム２を制御する（２）。以下、上述した学習フェーズ及び実行フェーズについて、詳細に説明する。

＜＜学習フェーズ＞＞
（１）前提条件の取得
まず、上述した学習フェーズの前提条件について説明する。
（前提１）ロボットアーム２が把持する部品、ロボットアーム２自体の幾何モデル、ならびに、部品の重量は予め与えられているものとする。
（前提２）各箱の中に入っている部品の種類は既知であるが、部品の箱中での位置及び部品の数は未知とする。
例えば、メモリ４ｂには、上述した、ロボットアーム２が把持する部品の幾何モデル、ロボットアーム２自体の幾何モデル、部品の重量、各箱の中に入っている部品の種類、などの情報が予め記憶されている。

（２）把持位置姿勢のデータベースの構築
続いて、ロボットアーム２が把持動作を開始する前に、ハンド２１が各部品を把持可能な位置及び姿勢（以下、把持位置姿勢）のデータベースを構築する。制御装置４は、部品毎の把持位置姿勢のデータベースを、例えば、下記＜フェーズ１〜６＞に従って構築する。

＜フェーズ１＞
先ず、制御装置４は、部品の形状（幾何モデル）に対して最適な分割数を決めるための評価指標を導き出す。詳細には、制御装置４は、領域分割数決定方法によって、以下の条件１を満たすまでクラスタをマージする。

＜条件１＞
誤差（error）＞０．０１、且つ、クラスタの重なり度＜０．２であって、クラスタの重なり度は、下記式により導き出す。

但し、vol_maxは最大クラスタ領域のバウンディングボックスの体積、overlap_voliは最大クラスタ領域とクラスタｉのバウンディングボックスが重なった領域の体積である。

＜フェーズ２＞
制御装置４は、＜フェーズ１＞で導き出した評価指標を最小化するように、部品の幾何モデルを一般的な方法（例えば、非特許文献：辻、原田、山野辺、永田、中村、長谷川、”一般物体把持のための把持姿勢変換”、ロボティクスシンポジア講演稿集、pp.219-224、2013）に基づいて分割（クラスタリング）する。これにより、部品の幾何モデルに複数の２次曲面が割り当てられる。

＜フェーズ３＞
制御装置４は、２次曲面として楕円や円が割り当てられたものに対しては、これらのバウンディングボックスを導き出す。なお、バウンディングボックスの３辺長が部品全体のバウンディングボックスの３辺長の０．１５倍（但し、倍数は適宜変更することができる。）より小さいクラスタ領域は、把持点探索の対象外とする。

＜フェーズ４＞
制御装置４は、＜フェーズ３＞で導き出したバウンディングボックスを、例えば、非特許文献：原田、辻、金子、金広、丸山、”直方体モデルに基づく多指ハンドの把握計画”、日本機械学会誌（Ｃ編）、vol.76、no.762、pp.331-339、2010で定義したＧＲＣ（Grasping Rectangular Convex）とみなし、ＧＲＣの曲面に離散化した点を仮定し、この点にアプローチするように仮想的にハンド２１を動かす。

＜フェーズ５＞
制御装置４は、＜フェーズ４＞におけるハンド２１の姿勢から当該ハンド２１の指部２２が部品と接触するまで仮想的に当該指部２２を閉じる。

＜フェーズ６＞
制御装置４は、仮想的に全ての指部２２が部品と接触した時点で、例えば、非特許文献：原田、辻、宇都、山野辺、永田、”ソフトフィンガ型把持の安定性解析”、計測自動制御学会システムインテグレーション部門講演会予稿集、2013に開示された手法で把持安定性をチェックする。制御装置４は、各把持位置姿勢のチェックを行って、把持安定性の条件を満たさないと判断した把持位置姿勢を対象外とする。

制御装置４は、上述の離散化した点に対して＜フェーズ５＞及び＜フェーズ６＞を繰り返すことで、各部品に対して把持位置姿勢のデータベースを構築することができる。なお、把持位置姿勢のデータベースにおける各データは、部品の座標系からみたロボットアーム２の手首関節の位置データ、部品の座標系からみたロボットアーム２の手首関節の姿勢行列データ、及びハンド２１の指部２２の関節の関節角度ベクトルデータを含む。

制御装置４は、上述の＜フェーズ１＞〜＜フェーズ６＞を部品とハンド２１との全てのペアに対して行って、部品とハンド２１との全てのペアに対して当該ハンド２１の把持位置姿勢のデータベースを構築し、当該データベースをメモリ４ｂに格納する。なお、ハンド２１の把持位置姿勢のデータベースは、予めメモリ４ｂに格納されていてもよい。

（３）部品の位置姿勢の計測
把持対象物である箱内の部品の位置及び姿勢（以下、位置姿勢）の計測を行う。
搬送ロボット１が動作を開始し、制御装置４は、モータを制御して上記構築したデータベースの把持位置姿勢に含まれるロボットアーム２の位置に移動台車３を移動させる。そして、検出部５は、その位置で、視覚センサ５１を用いて、部品の位置姿勢を計測する。

なお、箱中には複数の部品が入っているが、このうちの１つの部品の位置姿勢を計測するものとする。検出部５は、例えば、ＰＣＬ（Point Cloud Library）のアルゴリズムを用いて部品の位置姿勢を取得する。なお、ＰＣＬのアルゴリズムは、例えば、非特許文献：A.Aldoma,F.Tombari,R.B.Rusu,and M.Vincze,”OUR-CVFH-Oriented, Unique and Repeatable Clustered Viewpoint Feature Histogram for Object Recognition and 6DOF Pose Estimation”,A.Pinz et al.(Eds.):DAGM/OAGM 2012, LNCS,pp.113-122,2012に詳細に記載されておりこれを援用できるものとする。

検出部５は、視覚センサ５１を用いて、箱中の部品のポイントクラウド（３次元点群データ）を検出する。検出部５は、ＰＣＬのアルゴリズを用いて、検出したポイントクラウドに対して部品の幾何モデルをマッチングさせることで、ポイントクラウドの中から部品の位置姿勢を検出する。なお、箱内の部品の位置姿勢は、予めメモリ４ｂに記憶されていてもよい。

（４）把持動作（ピッキング動作）
制御装置４は、実際にロボットアーム２を動作させ、そのときのロボットアーム２の把持動作軌道を、下記のように生成する。
上記の如く、視覚センサ５１は、箱に入っている部品の位置姿勢を計測する。このとき、絶対座標系から見た部品の位置を示す３次元位置ベクトルをＰｏとし、部品の位置姿勢を示す３×３の回転行列をＲｏとし、認識された部品のうち第ｉ把持対象物を夫々Ｐｏ_ｉ、Ｒｏ_ｉと定義する。

なお、上述の如く、部品毎のハンド２１の把持位置姿勢のデータベースがメモリ４ｂに格納されている。認識された部品のうち、第ｉ把持対象物の、第ｊ番目の把持位置姿勢データに含まれる対象物座標系から見た手首関節の位置をｏＰｗ_ｉｊ、姿勢を表す行列をｏＲｗ_ｉｊとする。このとき、絶対座標系から見た手首関節の目標位置姿勢は以下の式により定義できる。
Ｐｗ_ｉｊ＝Ｐｏ_ｉ＋Ｒｏ_ｉ＊ｏＰｗ_ｉｊ
Ｒｗ_ｉｊ＝Ｒｏ_ｉ＊ｏＲｗ_ｉｊ

制御装置４は、上記式に示す手首関節の位置姿勢に対して逆運動学演算を行うことにより、ロボットアーム２の把持姿勢における目標関節角度を算出する。制御装置４は、上述した逆運動学演算を、ハンド２１の把持位置姿勢のデータベースの各部品に対して順次行い、各部品に対する目標関節角度を算出する。

制御装置４は、算出した各部品に対する目標関節角度に基づいて、各部品に対する初期姿勢から把持位置姿勢、さらに把持位置姿勢から各部品を通箱に置く姿勢までのハンド２１が障害物を回避しながら移動する把持動作軌道を生成する。なお、この軌道の生成方法は、例えば、非特許文献：G. Sanchez and J.-C. Latombe,”A Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking”, Robotics Research, Springer Tracts in Advanced Robotics, vol.6,pp.403-417,2003に詳細に開示されており、これにを援用できる。

図６は、学習フェーズにおける、上記把持動作軌道の生成方法のフローを示すフローチャートである。
制御装置４は、検出部５から、把持対象の部品の位置Ｐｏ_ｉ、及び姿勢Ｒｏ_ｉを取得する（ステップＳ１０１）。

制御装置４は、部品のポイントクラウドをセグメント化し、各セグメントのバウンディングボックスを計算する。制御装置４は、計算したバウンディングボックスの形状が把持対象の部品のバウンディングボックスの形状に近いものに対し部品の位置姿勢を当てはめる。

制御装置４は、パラメータｉ＝１、ｊ＝１、ｋ＝１と設定し初期化を行う（ステップＳ１０２）。
制御装置４は、検出部５により検出された部品の位置姿勢に対して、把持位置姿勢のデータベースを用いることで、把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊを算出する（ステップＳ１０３）。

制御装置４は、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有するか否かを判定する（ステップＳ１０４）。
制御装置４は、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有すると判定すると（ステップＳ１０４のＹＥＳ）、その目標把持位置姿勢の候補において、ロボットアーム２が障害物に干渉していないか否かを判定する（ステップＳ１０５）。一方、制御装置４は、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有しないと判定すると（ステップＳ１０４のＮＯ）、後述の（ステップＳ１１０）の処理に移行する。

制御装置４は、その目標位置姿勢の候補において、ロボットアーム２が障害物に干渉していないと判定すると（ステップＳ１０５のＹＥＳ）、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できるか否かを判定する（ステップＳ１０６）。一方、制御装置４は、その目標位置姿勢の候補において、ロボットアーム２が障害物に干渉していると判定すると（ステップＳ１０５のＮＯ）、後述の（ステップＳ１１０）の処理に移行する。

制御装置４は、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できると判定すると（ステップＳ１０６のＹＥＳ）、第ｋ把持動作軌道としてメモリ４ｂに記憶し（ステップＳ１０７）、後述の（ステップＳ１１０）の処理に移行する。一方、制御装置４は、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できないと判定すると（ステップＳ１０６のＮＯ）、後述の（ステップＳ１１０）の処理に移行する。

制御装置４は、全把持対象、全把持位置姿勢の判定が完了したか（ｉ、ｊが最大値か）否かを判定する（ステップＳ１０８）。
制御装置は、全把持対象、全把持位置姿勢の判定が完了したと判定すると（ステップＳ１０８のＹＥＳ）、本処理を終了する。一方、制御装置は、全把持対象、全把持位置姿勢の判定が完了していないと判定すると（ステップＳ１０８のＮＯ）、ｋをインクリメントし（ｋ＝ｋ＋１（但し、第ｉ把持対象物のデータベースの数を超えると、ｉ＝ｉ＋１、ｊ＝１とする））（ステップＳ１０９）、後述の（ステップＳ１１０）の処理に移行する。

制御装置は、パラメータｊをインクリメントし（但し、第ｉ把持対象物のデータベースの数を超えると、ｉ＝ｉ＋１、ｊ＝１とする）（ステップＳ１１０）、上記（ステップＳ１０３）処理に戻る。
制御装置４は、上記生成した把持動作軌道を用いて、下記（５）で識別器を生成する。
（５）識別器の生成
上記説明した通りであり、説明が重複するので省略する。

＜＜実行フェーズ＞＞
（１）部品の位置姿勢の計測
上記学習フェーズの部品の位置姿勢の計測と同一であるため、説明を省略する。
（２）把持動作（ピッキング動作）
図７は、実行フェーズにおける把持動作軌道の生成方法のフローを示すフローチャートである。

制御装置４は、検出部５から把持対象の部品の位置Ｐｏ_ｉ、及び姿勢Ｒｏ_ｉを取得する（ステップＳ２０１）。
制御装置４は、部品のポイントクラウドをセグメント化し、各セグメントのバウンディングボックスを計算する。制御装置４は、計算したバウンディングボックスの形状が把持対象の部品のバウンディングボックスの形状に近いものに対し部品の位置姿勢を当てはめる。

制御装置４は、パラメータｉ＝１、ｊ＝１、ｋ＝１と設定し初期化を行う（ステップＳ２０２）。
制御装置４は、検出部５により検出された部品の位置姿勢に対して、把持位置姿勢のデータベースを用いることで、把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊを算出する（ステップＳ２０３）。

制御装置４は、目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊに対して識別器を用いて把持可能か否かを判定する（ステップＳ２０４）。
制御装置４は、目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊに対して識別器を用いて把持可能と判定したとき（ステップＳ２０４のＹＥＳ）、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有するか否かを判定する（ステップＳ２０５）。一方、制御装置４は、目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊに対して識別器を用いて把持可能でない判定したとき（ステップＳ２０４のＮＯ）、後述の（ステップＳ２１１）の処理に移行する。

制御装置４は、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有すると判定すると（ステップＳ２０５のＹＳＥ）、その目標把持位置姿勢の候補において、ロボットアーム２が障害物に干渉していないか否かを判定する（ステップＳ２０６）。一方、制御装置４は、算出した把持対象の部品の目標位置姿勢の候補Ｐｗ_ｉｊ、Ｒｗ_ｉｊの逆運動学問題が解を有しないと判定すると（ステップＳ２０５のＮＯ）、後述の（ステップＳ２１１）の処理に移行する。

制御装置４は、その目標位置姿勢の候補において、ロボットアーム２が障害物に干渉していないと判定すると（ステップＳ２０６のＹＥＳ）、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できるか否かを判定する（ステップＳ２０７）。一方、制御装置４は、その目標位置姿勢の候補において、ロボットアーム２が障害物に干渉していると判定すると（ステップＳ２０６のＮＯ）、後述の（ステップＳ２１１）の処理に移行する。

制御装置４は、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できると判定すると（ステップＳ２０７のＹＥＳ）、第ｋ把持動作軌道としてメモリ４ｂに記憶し（ステップＳ２０８）、後述の（ステップＳ２１１）の処理に移行する。一方、制御装置４は、各部品に対する初期姿勢から把持位置姿勢、および把持位置姿勢から各部品を通箱に置く姿勢までの軌道を生成できないと判定すると（ステップＳ２０７のＮＯ）、後述の（ステップＳ２１１）の処理に移行する。

制御装置４は、全把持対象、全把持位置姿勢の判定が完了したか（ｉ、ｊが最大値か）否かを判定する（ステップＳ２０９）。
制御装置４は、全把持対象、全把持位置姿勢の判定が完了したと判定すると（ステップＳ２０９のＹＥＳ）、後述の（ステップＳ２１１）の処理に移行する。一方、制御装置４は、全把持対象、全把持位置姿勢の判定が完了していないと判定すると（ステップＳ２０９のＮＯ）、ｋをインクリメントし（ｋ＝ｋ＋１（但し、第ｉ把持対象物のデータベースの数を超えると、ｉ＝ｉ＋１、ｊ＝１とする））（ステップＳ２１０）、後述の（ステップＳ２１１）の処理に移行する。

制御装置４は、パラメータｊをインクリメントし（但し、第ｉ把持対象物のデータベースの数を超えると、ｉ＝ｉ＋１、ｊ＝１とする）（ステップＳ２１１）、上記（ステップＳ２０３）処理に戻る。
制御装置４は、複数の把持動作軌道が得られた場合に、各把持動作軌道の評価指標Ｉを算出する。上述の如く、ハンド２１の把持動作軌道と障害物との強干渉する点を少なくすると、ハンド２１による部品把持の成功率が高まると仮定する。すなわち、ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）及びｎの全ての値が小さい方が把持成功率が高いと仮定する。

制御装置４は、下記式を用いて、把持動作軌道の評価指標Ｉを算出する。下記式において、α、β、γは正値をとる重み係数であり、Ｉ_ｇは与えられた把持位置姿勢に対する把持安定性の指標値である。なお、この把持安定性の指標値Ｉ_ｇは、例えば、非特許文献：原田、辻、宇都、山野辺、永田、“ソフトフィンガ型把持の安定性解析”、計測自動制御学会システムインテグレーション部門講演会予稿集、２０１３．に開示されており、これを援用できるものとする。

制御装置４は、得られた複数の把持動作軌道のうち、評価指標Ｉが最小となる把持動作軌道を選択する（ステップＳ２１２）。上述の如く、把持動作軌道のハンド通過領域内に障害物が存在する場合に、ハンド２１が障害物に衝突、あるいは僅かに掠る程度の接触など様々な状況が存在する。したがって、複数の把持動作軌道が得られた場合に、最終的に、障害物が把持動作軌道にどれ位干渉するかを示す評価指標が最小（干渉が最小）の把持動作軌道を選択することで、把持成功率をさらに高めることができる。
以上、本実施形態に係る把持可否判定方法おいて、ハンド通過領域内に含まれる３次元点群データと、ハンド２１による把持対象物の把持の成否と、を対応付けた複数のサンプルデータを取得し、距離ｈ（ｐ_ｉ）、距離ｄ（ｐ_ｉ）及びｎの値が小さい方が把持成功率が高いと仮定し、サンプルデータおよび距離ｈ（ｐ_ｉ）およびｄ（ｐ_ｉ）に基づいて算出した特徴ベクトルから、把持対象物の把持可否を判定するための識別器を生成し、生成した識別器を用いてハンドによる把持対象物の把持可否を判定する。
これにより、ハンド通過領域Ｓに障害物が存在する場合でも、直ちに、その部品を把持不可能と判断するのではなく、識別器を用いて把持可否の判定を行う。このため、把持可能な部品数を増加させることができる。この把持可否の判定を行う際に、ハンド２１と障害物との強干渉点が少なく、把持成功率が高いハンド２１の把持パターンを学習した識別器を用いることで、把持成功率は高めることができる。すなわち、把持可能な部品数を増加させつつ、把持成功率は高めることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

また、本発明は、例えば、図６及び図７に示す処理を、ＣＰＵ４ａにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１搬送ロボット、２ロボットアーム、３移動台車、４制御装置、５検出部、２１ハンド、２２指部、５１視覚センサ

Claims

バラ積みされた複数の把持対象物をロボットアームのハンドによって把持する際に、前記ハンドが前記把持対象物を把持可能か否かを判定する把持可否判定方法であって、
前記把持対象物から所定距離離れたアプローチ点から該把持対象物を前記ハンドにより把持する把持点まで前記ハンドを移動させ、該把持点で前記把持対象物を前記ハンドにより把持するまでに、前記ハンドが通過する領域をハンド通過領域とし、該ハンド通過領域内に含まれる３次元点群データと、前記ハンドによる前記把持対象物の把持の成否と、を対応付けた複数のサンプルデータを取得するステップと、
前記ハンド通過領域内に含まる３次元点群のｉ番目の点をｐ_ｉ（ｉ＝１、２、・・・ｎ）とし、前記点ｐ_ｉをアプローチ方向に伸ばした直線が前記ハンド通過領域の境界と交わる点の内、前記点ｐ_ｉから最も遠い点との距離をｈ（ｐ_ｉ）とし、前記点ｐ_ｉから前記ハンドの把持方向に伸ばした直線が前記ハンド通過領域の境界と交わる点の内、前記点ｐ_ｉから最も近い点との距離をｄ（ｐ_ｉ）とし、前記ｈ（ｐ_ｉ）、ｄ（ｐ_ｉ）及びｎの値が小さい方が把持成功率が高いと仮定し、前記サンプルデータと前記距離ｈ（ｐ_ｉ）及びｄ（ｐ_ｉ）とに基づき算出した特徴ベクトルから、前記把持対象物の把持可否を判定するための識別器を生成するステップと、
前記生成した識別器を用いて前記ハンドによる前記把持対象物の把持可否を判定するステップと、
を含む、ことを特徴とする把持可否判定方法。
請求項１記載の把持可否判定方法であって、
前記識別器はランダムフォレストで構成されており、
前記特徴ベクトルは、下記式を用いて算出する、ことを特徴とする把持可否判定方法。
前記特徴ベクトル＝（min（（h（p_i）/binwidth）,bin_h）,
min（（d（p_i）/binwidth）,bin_d））
但し、上記式、binwidthはbin幅であり、bin_h及びbin_dは各軸のbin上限値である。
請求項１記載の把持可否判定方法であって、
前記識別器は、線形ＳＶＭ（support vector machine）で構成されており、
前記特徴ベクトルｆ_ｓは、下記式を用いて算出する、ことを特徴とする把持可否判定方法。