JP5500926B2 - 物体把持制御方法及び装置 - Google Patents

Description

本発明は、無作為に置かれた複数の被把持部材を把持する順序を自律的に設定するための物体把持制御方法及び装置に関するものである。

従来、無作為に置かれた複数の被把持部材を、把持手段を持つマニピュレータによって把持、移送する作業において、被把持部材を把持する順序は作業者が判断して、マニピュレータに指示する方法を採っているため作業効率が悪い。

これに対し特許文献１において、被把持部材間の重心位置の平均値である平均重心位置を計算し、被把持部材の重心位置と平均重心位置との距離を当該被把持部材の離散度として定義し、これを指標として把持順序を自律的に決定する方法が提案されている。

即ち、離散度の値が最大となる被把持部材が１つであれば、予め設定された安定把持位置で把持できるかをチェックしてから把持を実行する。また、離散度の値が最大となる被把持部材が複数存在すれば、各被把持部材を把持する際のマニピュレータの可操作度を計算し、可操作度が最大となる被把持部材について、予め設定された安定把持位置で把持できるかをチェックしてから把持を実行する。従って、この技術によればロボットが自律的に把持順序を決定することができる。

特開平４−１４６０９０号公報

無作為に置かれた複数の被把持部材から把持を試みる場合に、把持手段の位置決め誤差などによって被把持部材の周囲に置かれた他の被把持部材と干渉（接触）する虞れがある。これを低減するには、周囲がクリアな被把持部材から順番に把持、移送することが望ましい。しかし、上述の従来技術においては、被把持部材間の重心位置関係或いはマニピュレータの可操作度を基に把持順序が決定されるため、周囲がクリアな被把持部材から順番に把持、移送することができない場合がある。

本発明の目的は、上述の問題点を解消し、周囲がクリアな被把持部材から把持を行うように、把持順序を自律的に設定する物体把持制御方法及び装置を提供することにある。

上記目的を達成するための本発明に係る物体把持制御装置は、複数の被把持部材の形状・位置・姿勢を認識する認識手段と、前記認識手段により認識された被把持部材に基づいて、被把持部材リストを生成する生成手段と、前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段が干渉しない前記把持手段の姿勢範囲を取得する取得手段と、前記被把持部材のそれぞれについて、前記取得された姿勢範囲が所定の閾値よりも大きいかを判定する第１の判定手段と、前記第１の判定手段において、所定の閾値よりも大きいと判定された被把持部材を把持対象部材として選択する選択手段と、前記選択手段で前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加手段と、前記選択手段で前記把持対象部材として選択された前記被把持部材を前記被把持部材リストから削除する削除手段とを備えることを特徴とする物体把持制御装置。
ことを特徴とする。

また、本発明に係る物体把持制御方法は、複数の被把持部材の形状・位置・姿勢を認識する認識ステップと、前記認識手段により認識された被把持部材に基づいて、被把持部材リストを生成する生成ステップと、前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段とが干渉し得る範囲を把持禁止範囲として取得する取得ステップと、前記被把持部材のそれぞれについて、前記設定された把持禁止範囲が所定の閾値よりも小さいかを判定する第１の判定ステップと、前記第１の判定ステップにおいて所定の閾値よりも小さいと判定された被把持部材を把持対象部材として選択する選択ステップと、前記選択ステップにおいて前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加ステップと、前記選択ステップにおいて前記把持対象部材として選択された被把持部材を前記被把持部材リストから削除する削除ステップとを備えることを特徴とする。

本発明に係る物体把持制御方法及び装置によれば、周囲の被把持部材に把持手段を介して干渉しない被把持部材から順次に把持を行うように、把持順序を自律的に設定することが可能となる。

実施例１の把持装置の斜視図である。 把持手段の斜視図である。 実施例１のフローチャート図である。 被把持部材近似方法の説明図である。 被把持部材近似方法の説明図である。 干渉可能性判定円の説明図である。 被把持部材近似円同士が干渉する場合の説明図である。 干渉可能性判定円と被把持部材近似円が干渉する場合の説明図である。 干渉可能性判定円と被把持部材近似円が干渉しない場合の説明図である。 被把持部材近似円同士が干渉する場合の説明図である。 干渉可能性判定円と被把持部材近似円が干渉する場合の説明図である。 干渉可能性判定円と被把持部材近似円が干渉する場合の説明図である。 被把持部材占有範囲と把持指占有範囲の説明図である。 非干渉姿勢範囲の計算についてのグラフ図である。 非干渉姿勢範囲が複数存在する場合の説明図である。 設定された把持姿勢を説明図である。 実施例１を適用する別の形態の斜視図である。 実施例２の把持装置の斜視図である。 把持手段の側面図である。 フローチャート図である。 被把持部材近似方法の説明図である。 被把持部材近似方法の説明図である。 把持手段可動範囲の説明図である。 被把持部材近似球同士が干渉する場合の説明図である。 干渉可能性判定球と被把持部材近似球が干渉する場合の説明図である。 干渉可能性判定球と被把持部材近似球が干渉する場合の説明図である。 或る把持姿勢により干渉が生ずる場合の説明図である。 或る把持姿勢により干渉が生じない場合の説明図である。 実施例２を適用する別のフローチャート図である。

本発明を図示の実施例に基づいて詳細に説明する。

図１は実施例の把持装置の斜視図を示し、作業台１の上方にマニピュレータ２によって操作される把持手段３が設けられている。また、作業台１の上方には、被把持部材認識手段としてカメラ等の視覚センサ４が設けられている。

作業台１の上には、複数の被把持部材Ｗとして、円板状の被把持部材Ｗａと長方形状の被把持部材Ｗｂが無作為に置かれている。

本実施例では、作業台１上において、マニピュレータ２によって把持手段３を操作し、被把持部材Ｗの側面を把持して、１個ずつ作業台１から取り出し、図示しない別の作業台に移送することを繰り返すようになっている。

図２は把持手段３の斜視図である。把持手段３はマニピュレータ２の持つ自由度によって、把持中心Ｏを軸に作業台１に対して水平方向に回動可能とされている。把持手段３は２本の把持指５を備え、これらの把持指５は把持中心Ｏに対称的に開閉して、被把持部材Ｗを把持することができる。把持対象となる被把持部材Ｗの周囲に、他の被把持部材Ｗが近接している場合でも、この回動角度を適切に制御することによって、把持対象に近接した他の被把持部材Ｗと把持手段３とは、干渉することなく、つまり相互に接触せずに把持可能となる場合がある。

図３は被把持部材Ｗを把持する順序を設定する方法のフローチャート図である。

ステップＳ１１０：視覚センサ４を用い、全ての被把持部材Ｗの形状・位置・姿勢を認識し、被把持部材識別子ｉを割り当てる。本実施例では、二次元平面内で考えれば十分であるので、被把持部材Ｗの形状は円及び長方形として認識される。

ステップＳ１２０：把持可能姿勢範囲設定はステップＳ１２１〜Ｓ１２４から成る。

ステップＳ１２１：ステップＳ１１０での認識結果を基に、被把持部材Ｗを内包する被把持部材モデルを作成する。被把持部材モデルは少なくとも１つの凸モデルの和集合とする。被把持部材モデルの要素として凸モデルを使用することで、把持手段３と被把持部材Ｗが干渉する可能性を判定することが容易になる。凸モデルには様々なものがあるが、本実施例では円を用い被把持部材近似円と呼ぶ。

被把持部材Ｗが円板状物体である場合は、認識された円そのものを被把持部材近似円とし、この被把持部材近似円を被把持部材モデルとすればよい。被把持部材Ｗが長方形状物体である場合には、図４に示すように１つの外接円を被把持部材近似円Ｓとし、被把持部材モデルとすることもできるが、被把持部材Ｗの大きさを大きく見積もりすぎてしまうことになる。このため、図５に示すように複数の被把持部材近似円Ｓ１、Ｓ２、・・・を設定し、その和集合を被把持部材モデルとしてもよい。

把持を行う際には、被把持部材Ｗに設定された何れかの被把持部材近似円Ｓに対し、その中心に把持手段３の把持中心Ｏを位置決めし、把持手段３の姿勢を適切に制御して把持を行う。

それぞれの凸モデルの被把持部材近似円Ｓ、Ｓ’、・・・に被把持部材近似円識別子ｊを割り当てる。各被把持部材近似円Ｓ、Ｓ’、・・・は被把持部材識別子ｉと被把持部材近似円識別子ｊの組（ｉ，ｊ）で識別される。識別子（ｉ，ｊ）の被把持部材近似円Ｓに対して、中心の座標をｐ_(i,j)（ｘ_(i,j)，ｙ_(i,j)）とし、半径をｒ_(i,j)とする。これらはステップＳ１１０で得られた被把持部材Ｗの形状・位置・姿勢から算出することができる。なお、半径方向の認識誤差や位置決め誤差に対して確実な把持ができるように、被把持部材近似円Ｓの半径として、把持手段３を開閉させる掴み代を加味しておくことが好ましい。

ステップＳ１２２：把持手段可動範囲のモデルを設定するために、把持手段３の把持手段モデルを設定する。本実施例では、平面上で考えればよいので、形状については把持指５が作業台１上で占める形状を考えれば十分である。

図６は被把持部材近似円Ｓと把持指近似円Ｆと干渉可能性判定円Ｊの位置関係を示している。ここでは把持指５の断面に外接する円を用い、把持指近似円Ｆと呼ぶ。２本の把持指５の大きさは同じであるとし、その把持指近似円Ｆの半径をｒ_f、直径をＲ_fとする。

また、把持指５は２本あるので、その位置関係も把持手段モデルのパラメータとなる。把持手段３の回動角度の原点を基準とし、１本目の把持指５が配置される角度をψ₁、２本目の把持指５が配置される角度をψ₂とする。半径ｒ_f、直径Ｒ_f、角度ψ₁、ψ₂が把持手段モデルを表現するパラメータとなり、本実施例ではψ₁＝０°、ψ₂＝１８０°とする。

把持を行う際には、各被把持部材近似円Ｓの中心を回転中心として様々な姿勢で把持指５を配置できるので、把持手段可動範囲モデルとして、各被把持部材近似円Ｓの中心から半径（ｒ_(i,j)＋Ｒ_f）の円を設定する。この円の内側に他の被把持部材Ｗが差しかかっていると、把持手段３の姿勢によっては把持指５が他の被把持部材Ｗに触れて干渉する可能性があるので、これを干渉可能性判定円Ｊと呼ぶ。

ステップＳ１２３：識別子（ｉ１，ｊ１）の被把持部材近似円Ｓに対して、様々な姿勢で把持を試みる際に、別の識別子（ｉ２，ｊ１）を持つ被把持部材近似円Ｓ’と把持手段３とが干渉し得るかを判定する。

先ず、被把持部材近似円Ｓ、Ｓ’同士が干渉しているかを調べる。同じ被把持部材Ｗに割り当てられ、隣り合っている被把持部材近似円Ｓ、Ｓ’同士は干渉していることが多い。

識別子（ｉ１，ｊ１）の被把持部材近似円Ｓの半径ｒ_(i1,j1)と識別子（ｉ２，ｊ１）の被把持部材近似円Ｓ’の半径ｒ_(i2,j1)との和を求める。この和が中心間距離Ｌ_{(i1,j1),(i2,j1)}よりも大きければ、前者の被把持部材近似円Ｓ（ｉ１，ｊ１）に対して後者の被把持部材近似円Ｓ’を干渉可能性ありと判定する。図７の状態では、被把持部材近似円Ｓと被把持部材近似円Ｓ’について干渉がある。

被把持部材近似円ＳとＳ’の干渉がなければ、次に干渉可能性判定円Ｊと干渉があるかを調べる。識別子（ｉ１，ｊ１）の被把持部材近似円Ｓに設定した干渉可能性判定円Ｊの半径（ｒ_(i1,j1)＋Ｒ_f）と、識別子（ｉ２，ｊ１）の被把持部材近似円Ｓ’の半径ｒ_(i2,j1)との和を求める。これらの和が中心間距離Ｌ_{(i1,j1),(i2,j1)}よりも大きければ、識別子（ｉ１，ｊ１）の被把持部材近似円Ｓに設定された干渉可能性判定円Ｊに対して、識別子（ｉ２，ｊ１）の被把持部材近似円Ｓ’を干渉可能性ありと判定する。図８の状態では、被把持部材近似円Ｓの干渉可能性判定円Ｊと被把持部材近似円Ｓ’について干渉がある。

このような２種の干渉がなければ、識別子（ｉ１，ｊ１）の被把持部材近似円Ｓを把持する際に、識別子（ｉ２，ｊ１）の被把持部材近似円Ｓ’は干渉しないものとする。

図９に示す状態では、被把持部材近似円Ｓの干渉可能性判定円Ｊと被把持部材近似円Ｓ’について干渉がないので、被把持部材近似円Ｓを把持する際に、被把持部材近似円Ｓ’の影響を考慮する必要はない。

全ての被把持部材近似円Ｓについて上述の処理を行い、干渉可能性のある被把持部材近似円Ｓ’をリストアップする。

ステップＳ１２４：干渉判定結果を基に、個々の被把持部材近似円Ｓについて、把持手段３が周囲の被把持部材Ｗに干渉しないような把持手段３の姿勢範囲を算出する。これを把持可能姿勢範囲と呼ぶ。本実施例においては、把持手段３は回動の１自由度を持つので、把持可能姿勢範囲は一次元の角度範囲を意味する。

図１０に示すように、或る被把持部材近似円Ｓの中心から、干渉相手の被把持部材近似円Ｓ’との交点に向かう２本の直線Ｌのなす角を求める。また、図１１に示すように或る被把持部材近似円Ｓの中心から、干渉相手の被把持部材近似円Ｓ’に引いた２本の接線Ｔのなす角を求める。

図１２に示すように或る被把持部材近似円Ｓの中心から、その干渉可能性判定円Ｊと干渉相手の被把持部材近似円Ｓ’との交点に向かう２本の直線Ｌのなす角を求め、これらの角を比較し、最も大きいものを被把持部材占有範囲と定義する。

なお、干渉可能性判定円Ｊと被把持部材近似円Ｓ’との干渉である場合には、図１０に示すような或る被把持部材近似円Ｓの中心から、干渉相手の被把持部材近似円Ｓ’との交点に向かう２本の直線Ｌのなす角は考えなくともよい。

或る被把持部材近似円Ｓについて、被把持部材占有範囲に把持指５が進入しない把持姿勢があれば、その被把持部材Ｗは把持可能である。この範囲に把持指５が進入しないための姿勢範囲を非干渉姿勢範囲と定義し、次のように算出する。

図１３における横軸を姿勢の原点とし、反時計方向をプラス方向とする。或る被把持部材近似円Ｓの中心から、把持指近似円Ｆに引いた２本の接線がなす角を把持指占有範囲と定義する。被把持部材Ｗの占有範囲を２θ₂、把持指５の占有範囲を２θ₁とすると、１本の把持指５が他の被把持部材Ｗに干渉してしまう姿勢範囲は、角度座標上で図１４に示すグラフ図に示すようになる。この範囲内の把持姿勢では、姿勢の原点方向に配置された把持指５が被把持部材Ｗに干渉してしまうので、これを干渉姿勢範囲と定義する。

２本の把持指５の両方が干渉しない範囲を探すために、干渉姿勢範囲をそれぞれ角度ψ₁＝０、ψ₂＝１８０゜移動させた図１４に示すグラフ図を用意し、重ね合わせる。把持姿勢がこの範囲外にあるとき、把持指５は２本とも被把持部材Ｗの占有範囲外にあることが分かる。把持指５の数が増えても同様の処理が可能である。把持指５の回動角度が干渉姿勢範囲以外の非干渉姿勢範囲に納まっていれば、隣接する把持部材近似円Ｓ’に干渉することはない。

干渉のある被把持部材近似円Ｓ’が複数存在する場合など、非干渉姿勢範囲が複数存在する場合がある。この場合に、最大幅となる非干渉姿勢範囲を採用する。図１５に示す場合は非干渉姿勢範囲Ａが採用される。

それぞれの被把持部材Ｗに対して、少なくとも１つの被把持部材近似円Ｓが設定されているので、それらの中で最大の非干渉姿勢範囲を、その被把持部材Ｗの把持可能姿勢範囲とする。これは、把持姿勢の誤差に対して最も確実な個所を掴むということを意味している。

ステップＳ１３０：把持対象設定は図３のステップＳ１３１〜Ｓ１３６から成る。

ステップＳ１３１：ステップＳ１２０で得られた結果を基に、把持可能姿勢範囲が空でない被把持部材Ｗを把持可能として判定する。

ステップＳ１３２：把持可能な被把持部材Ｗが存在するかを判断し、存在すればステップＳ１３３に移行し、存在しなければ把持対象なしとしてステップＳ１３０を終了し、ステップＳ１４０に移行する。

ステップＳ１３３：把持可能姿勢範囲の大きさが所定の閾値以上の被把持部材Ｗを把持候補として設定する。ここでは、被把持部材Ｗ間で最大の把持可能姿勢範囲を持つものとする。

ステップＳ１３４：把持候補として設定された被把持部材Ｗについて、把持可能姿勢範囲の中心を把持姿勢として設定する。例えば、図１６に示すように把持姿勢つまり把持指５の配置角度つまり把持指近似円Ｆが設定される。これにより、把持姿勢の誤差によって被把持部材近似円Ｓ’に干渉してしまう可能性を低減できる。孤立した被把持部材Ｗなど、全く干渉の可能性がない把持候補については、把持手段３の現在姿勢をそのまま使用してもよい。

ステップＳ１３５：把持候補は複数あるかどうかを判断し、把持候補が１つであれば、その被把持部材Ｗを把持対象として設定し、ステップＳ１３０を終了する。把持候補が複数ある場合はステップＳ１３６に移行する。

ステップＳ１３６：複数の把持候補について、把持可能姿勢範囲が同じ値である場合には、それぞれ設定された把持姿勢と、把持手段３の現在姿勢を比較し、差が最小の被把持部材Ｗを把持対象として物体把持制御を行う。これにより、把持を実行する際の把持手段３の姿勢変化量が小さくなり、作業効率の向上が見込める。複数の把持対象が存在する場合には、移送先への距離などを指標に一意的に定めればよい。

ステップＳ１４０：把持対象の被把持部材Ｗが存在するか判断し、存在すればステップＳ１５０に移行し、存在しなければ作業を終了する。

ステップＳ１５０：設定された把持対象を設定された姿勢で把持するように、把持手段３を制御し被把持部材Ｗの把持を実行し、把持が終了するとステップＳ１１０に戻る。

このように本実施例によれば、把持に際して周辺の被把持部材Ｗと把持指５との干渉が起こり難い被把持部材Ｗを優先的に把持することができる。しかも、周辺の被把持部材Ｗから離れた位置に把持指５が配置されるので、視覚センサ４による姿勢認識の誤差や、把持姿勢の位置決め誤差に対して確実な把持を行うこともできる。

更に、干渉の程度が同じ被把持部材Ｗが多い場合には、把持手段３の姿勢変化を抑えることができる。一般に、把持手段３の姿勢を変化させるアクチュエータは、マニピュレータ２の基部などと比べ、軽量・低出力であることが多く、把持手段３の姿勢変化を抑えることは作業効率の向上に寄与できる。

なお、実施例１では被把持部材Ｗを板状物体としたが、図１７に示すように被把持部材Ｗが高さがほぼ揃った円柱Ｗａ’や直方体Ｗｂ’などである場合でも、同じ手法を適用できることは明らかである。

図１８は実施例２の把持装置の斜視図を示す。作業台１１の上に複数の被把持部材Ｗとして立方体状の被把持部材Ｗｅと四角錐台状の被把持部材Ｗｄが無作為に置かれている状態を示している。本実施例では、マニピュレータ１２により把持手段１３を作業台１１の上方から接近して被把持部材Ｗを把持して、１個ずつ作業台から取り出すようになっている。また、複数個の視覚センサ１４が立体視のために作業台１１の上方に配置されている。

図１９はマニピュレータ１２により作動する把持手段１３の側面図である。把持手段１３はマニピュレータ１２が有する自由度によって、把持中心Ｏを軸に回動できるようになっている。把持手段１３が備える２本の把持指１５は、把持中心Ｏに関して対称に開閉し、被把持部材Ｗを把持することができる。把持手段１３はマニピュレータ１２が持つ自由度によって、被把持部材Ｗを様々な姿勢で把持可能となっており、最終軸の自由度により把持中心Ｏを軸に自在に回動できる。

把持対象となる被把持部材Ｗの周囲に他の被把持部材Ｗが近接している場合でも、この姿勢を適切に制御することによって、把持対象周囲の被把持部材Ｗと把持手段１３の把持指１５が干渉することなく把持可能となる場合がある。

図２０は実施例２の動作フローチャート図である。

ステップＳ２１０：複数の視覚センサ１４を用いて被把持部材Ｗの形状・位置・姿勢を認識し、被把持部材識別子ｉを割り当てる。被把持部材Ｗの形状は立方体及び四角錐台として認識される。

ステップＳ２２０：把持禁止範囲設定はステップＳ２２１〜Ｓ２２４から成る。

ステップＳ２２１：ステップＳ２１０での認識結果を基に、被把持部材Ｗを内包する被把持部材モデルを設定する。被把持部材モデルは少なくとも１つの凸モデルの和集合として表現される。この凸モデルを使用することで、次ステップでの干渉判定が容易になることが期待できる。凸モデルには様々なものがあるが、本実施例では球を用い、被把持部材近似球と呼ぶ。

被把持部材Ｗが立方体状物体である場合には、１つの外接球を被把持部材近似球とし、これを被把持部材モデルとすればよい。被把持部材Ｗが四角錐台状物体である場合に、図２１に示すように１つの被把持部材近似球Ｂで内包すると、物体の大きさを大きく見積もり過ぎてしまうことになる。従って、図２２に示すように大きさが異なる複数の被把持部材近似球Ｂ１、Ｂ２、・・・により内包し、その和集合で表現すればよい。

把持を行う際には、被把持部材Ｗに設定された何れかの被把持部材近似球Ｂに対し、その中心に把持中心Ｏが向かうように位置決めし、把持手段１３の姿勢を適切に制御して把持を行う。

各被把持部材モデルのなかで、各凸モデルの被把持部材近似球Ｂに被把持部材近似球識別子ｊを割り当てる。各凸モデルの被把持部材近似球Ｂは識別子ｉとｊの組で指定される。

識別子（ｉ，ｊ）の被把持部材近似球Ｂに対して、中心の座標をｐ_(i,j)（ｘ_(i,j)，ｙ_(i,j)，ｚ_(i,j)）とし、半径をｒ_(i,j)とする。これらはステップＳ２１０で得られた被把持部材Ｗの形状・位置・姿勢から算出することができる。

ステップＳ２２２：把持手段可動範囲のモデルを設定するため、把持手段１３の把持手段モデルを設定する。図２３に示すように、把持手段１３を被把持部材近似球Ｂに接近させた際に把持手段１３を包み込む球の半径を（ｒ_(i,j)＋Ｒ_f）、被把持部材近似球Ｂの中心から把持手段１３を見込む角を２α_fとする。把持手段１３の把持指の直径Ｒ_f及び角α
_fは固定値ではなく、被把持部材近似球Ｂの半径ｒ_(i,j)の関数としてもよい。直径Ｒ_f及び角α_fが、把持手段モデルを表現するパラメータとなる。

把持手段可動範囲モデルとして、被把持部材近似球Ｂと中心を同じくし、各被把持部材近似球Ｂの中心から半径（ｒ_(i,j)＋Ｒ_f）の球を設定する。これを干渉可能性判定球Ｋと呼ぶ。

ステップＳ２２３：識別子（ｉ１，ｊ１）の被把持部材近似球Ｂに対して把持を試みる際に、把持手段１３と識別子（ｉ２，ｊ１）の被把持部材近似球Ｂ’とが干渉し得るかを判定する。

先ず、図２４に示すように隣接する被把持部材近似球Ｂ、Ｂ’同士が干渉しているかを調べる。同じ被把持部材Ｗに割り当てられ、隣り合っている被把持部材近似球Ｂ、Ｂ’同士は干渉していることが多い。

識別子（ｉ，ｊ）の被把持部材近似球Ｂの半径ｒ_(i1,j1)と識別子（ｉ２，ｊ１）被把持部材近似球Ｂ’の半径ｒ_(i2,j1)との和を求める。この和が中心間距離Ｌ_{(i1,j1),(i2,j1)}よりも大きければ、判定対象の被把持部材近似球Ｂを干渉可能性ありと判定する。

被把持部材近似球Ｂ、Ｂ’同士の干渉がなければ、次に干渉可能性判定球Ｋと干渉があるかを調べる。干渉可能性判定球Ｋの半径（ｒ_(i1,j1)＋Ｒ_f）と被把持部材近似球Ｂの半径ｒ_(i2,j1)との和が、中心間距離Ｌ_{(i1,j1),(i2,j1)}よりも大きければ、判定対象の被把持部材近似球Ｂは干渉可能性ありと判定する。

これらの２種の干渉がなければ、識別子（ｉ１，ｊ１）の被把持部材近似球Ｂを把持する際に識別子（ｉ２，ｊ１）の被把持部材近似球Ｂ’は影響はないものとする。全ての被把持部材近似球Ｂについて上述の処理を行い、干渉可能性のある被把持部材近似球Ｂ’をリストアップする。

ステップＳ２２４：干渉判定結果を基に個々の被把持部材Ｗについて、把持手段１３が周囲の被把持部材Ｗに干渉し得る範囲の大きさを算出する。

図２４に示すように、或る被把持部材近似球Ｂの中心から、干渉相手の被把持部材近似球Ｂ’との交差円に向かう直線Ｌが形成する円錐の頂角を求める。また、図２５に示すように、或る被把持部材近似球Ｂの中心から、干渉相手の被把持部材近似球Ｂ’に引いた接線Ｔが形成する円錐の頂角を求める。

更に、図２６に示すように、或る被把持部材近似球Ｂの中心から、その干渉可能性判定球Ｋと干渉相手の被把持部材近似球Ｂ’の交差円に向かう直線Ｌが形成する円錐の頂角とを求める。これらの頂角同士を比較し、最も大きいものを被把持部材占有範囲と定義する。

なお、干渉可能性判定球Ｋと被把持部材近似球Ｂとの干渉がある場合には、図２４に示すような或る被把持部材近似球Ｂの中心から、干渉相手の被把持部材近似球Ｂ’との交差円に向かう直線Ｌが形成する円錐の頂角は考えなくともよい。

各被把持部材近似球Ｂに対して、干渉可能性のある全ての被把持部材近似球Ｂ’からの被把持部材占有範囲の大きさを足し合わせ、被把持部材近似球Ｂの干渉範囲の大きさとする。この値が大きいことは、被把持部材近似球Ｂを把持しようとした際に、周辺に把持手段１３と干渉する他の被把持部材近似球Ｂ’が多く存在する、又はより近くに存在することを示している。

それぞれの被把持部材Ｗに対し、少なくとも１つの被把持部材近似球Ｂが設定されているので、その中で最小となる干渉範囲の大きさを、その被把持部材Ｗの把持禁止範囲の大きさとすればよい。これは、他の被把持部材Ｗが邪魔になり難い個所を掴むということを意味している。

ステップＳ２３０：把持対象設定はステップＳ２３１〜Ｓ２３３から成る。

ステップＳ２３１：ステップＳ２２０で得られた結果を基に、把持禁止範囲の大きさが所定の閾値以上のものは把持困難と見倣し、閾値以下の被把持部材Ｗを把持候補として設定する。この閾値は実験等で適当に定めておけばよい。

ステップＳ２３２：把持候補に対して、マニピュレータ１２の可動範囲内で、ランダムサンプリングなどの手法を用いて、適当に把持姿勢を設定する。設定された把持姿勢で、干渉可能性のある全ての被把持部材近似球Ｂと、把持手段１３とが干渉しなければ、その把持姿勢で把持可能とする。

図２７は或る把持姿勢での被把持部材近似球Ｂの中心から把持中心Ｏに向かうベクトルと、干渉可能性のある被把持部材近似球Ｂ’の中心に向かうベクトルの張る平面での断面図である。

被把持部材占有範囲α₁と、把持手段１３を見込む角α_fの和が、先の２つのベクトルのなす角よりも大きいとき、その把持姿勢では干渉が起こる。その逆であれば、図２８に示すように干渉は起こらない。この判定を干渉可能性のある全ての被把持部材近似球Ｂに対して行い、全て干渉なしであればその把持姿勢を採用し、１つでも干渉すればランダムな把持姿勢の設定からし直す。また、ランダムに把持姿勢を設定、判定を所定の回数繰り返しても把持可能とならない場合は、把持候補から外す。

ステップＳ２３３：把持可能な把持候補の中から、把持禁止範囲の大きさが最小のものを把持対象として設定する。これにより、周囲の被把持部材Ｗの影響が少ない被把持部材Ｗを優先把持できる。複数の把持対象が存在する場合には、移送先への距離などを指標に、一意的に定めればよい。

ステップＳ２４０：把持対象は存在するかを判断し、存在しなければ作業を終了し、存在すればステップＳ２５０に移行する。

ステップＳ２５０：把持を実行しステップＳ２１０に移行する。

このように本実施例によれば、把持の際に周辺の被把持部材Ｗとの干渉が起こり難い被把持部材Ｗを優先的に把持することができる。

また、図２９のフローチャート図に示すように、一連の把持を行う前に作業終了までの順番を設定しておくこともできる。

ステップＳ３１０：図２のステップＳ２１０と同様に、複数の視覚センサ１４を用いて、被把持部材Ｗの形状・位置・姿勢を認識し、被把持部材識別子ｉを割り当てる。

ステップＳ３２０：被把持部材Ｗのリストを作成し、把持順序リストを初期化する。即ち、リストを空にする。

ステップＳ３３０：ステップＳ２２０と同様に、図２０のステップＳ２２１〜Ｓ２２４から成る。

ステップＳ３４０：ステップＳ２２０と同様に、図２０のステップＳ２３１〜Ｓ２３３から成る。

ステップＳ３５０：把持対象は存在するかを判断し、存在しなければステップＳ３８０に移行し、存在すればステップＳ３６０に移行する。

ステップＳ３６０：把持順序リストの終端に把持対象を追加する。

ステップＳ３７０：ステップＳ３６０で追加した把持対象を被把持部材リストから削除し、ステップＳ３３０に戻る。

ステップＳ３８０：把持順序リストの順番に従って把持を実行し、終了する。

１、１１ 作業台
２、１２ マニピュレータ
３、１３ 把持手段
４、１４ 視覚センサ
５、１５ 把持指
Ｂ、Ｂ’ 被把持部材近似球
Ｆ 把持指近似円
Ｊ 干渉可能性判定円
Ｋ 干渉可能性判定球
Ｓ、Ｓ’ 被把持部材近似円
Ｗ 被把持部材

Claims (14)

1. 複数の被把持部材の形状・位置・姿勢を認識する認識手段と、
   前記認識手段により認識された被把持部材に基づいて、被把持部材リストを生成する生成手段と、
   前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段が干渉しない前記把持手段の姿勢範囲を取得する取得手段と、
   前記被把持部材のそれぞれについて、前記取得された姿勢範囲が所定の閾値よりも大きいかを判定する第１の判定手段と、
   前記第１の判定手段において、所定の閾値よりも大きいと判定された被把持部材を把持対象部材として選択する選択手段と、
   前記選択手段で前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加手段と、
   前記選択手段で前記把持対象部材として選択された被把持部材を前記被把持部材リストから削除する削除手段とを備えることを特徴とする物体把持制御装置。
2. 前記第１の判定手段において、前記取得された姿勢範囲が所定の閾値よりも大きいと判定された被把持部材が複数あるかを判定する第２の判定手段を更に備え、
   前記選択手段は、前記第２の判定手段により前記取得された姿勢範囲が所定の閾値よりも大きいと判定された被把持部材が複数あると判定された場合には、前記把持手段の現在の姿勢と前記被把持部材それぞれを把持する際の姿勢との差に基づいて、前記被把持部材を前記把持対象部材として選択することを特徴とする請求項１に記載の物体把持制御装置。
3. 前記取得手段は、少なくとも１つの凸モデルを使用して前記被把持部材及び前記把持手段の可動範囲を設定し、前記姿勢範囲を取得することを特徴とする請求項１又は２に記載の物体把持制御装置。
4. 前記取得手段は、前記凸モデルとして円を用いて前記姿勢範囲を取得することを特徴とする請求項３に記載の物体把持制御装置。
5. 複数の被把持部材の形状・位置・姿勢を認識する認識手段と、
   前記認識手段により認識された被把持部材に基づいて、被把持部材リストを生成する生成手段と、
   前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段とが干渉し得る範囲を把持禁止範囲として取得する取得手段と、
   前記被把持部材のそれぞれについて、前記設定された把持禁止範囲が所定の閾値よりも小さいかを判定する第１の判定手段と、
   前記第１の判定手段において、所定の閾値よりも小さいと判定された被把持部材を把持対象部材として選択する選択手段と、
   前記選択手段で前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加手段と、
   前記選択手段で前記把持対象部材として選択された被把持部材を前記被把持部材リストから削除する削除手段とを備えることを特徴とする物体把持制御装置。
6. 前記第１の判定手段において、前記取得された姿勢範囲が所定の閾値よりも小さいと判定された被把持部材が複数あるかを判定する第２の判定手段を更に備え、
   前記選択手段は、前記第２の判定手段により前記取得された姿勢範囲が所定の閾値よりも小さいと判定された被把持部材が複数あると判定された場合には、前記把持手段の現在の姿勢と前記被把持部材それぞれを把持する際の姿勢との差に基づいて、被把持部材を前記把持対象部材として選択することを特徴とする請求項５に記載の物体把持制御装置。
7. 前記取得手段は、少なくとも１つの凸モデルを使用して前記被把持部材及び前記把持手段の可動範囲を設定し、前記把持禁止範囲を設定することを特徴とする請求項５又は６に記載の物体把持制御装置。
8. 前記取得手段は、凸モデルとして円を用いて前記把持禁止範囲を設定することを特徴とする請求項７に記載の物体把持制御装置。
9. 前記取得手段は、凸モデルとして球を用いて前記把持禁止範囲を設定することを特徴とする請求項７に記載の物体把持制御装置。
10. 更に、前記把持対象部材リストに含まれる前記把持対象部材を把持するように制御する制御手段とを備えることを特徴とする請求項１乃至９のいずれか１項に記載の物体把持制御装置。
11. 前記制御手段は、更に、前記取得手段、前記第１の判定手段、前記選択手段、前記追加手段、前記削除手段とを繰り替えし実行することを特徴とする請求項１０に記載の物体把持制御装置。
12. 更に、前記把持手段を更に備えることを特徴とする請求項１乃至１１のいずれか１項に記載の物体把持装置。
13. 複数の被把持部材の形状・位置・姿勢を認識する認識ステップと、
    前記認識された被把持部材に基づいて、被把持部材リストを生成する生成ステップと、
    前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段が干渉しない前記把持手段の姿勢範囲を取得する取得ステップと、
    前記被把持部材のそれぞれについて、前記取得された姿勢範囲が所定の閾値よりも大きいかを判定する第１の判定ステップと、
    前記第１の判定ステップにおいて所定の閾値よりも大きいと判定された被把持部材を把持対象部材として選択する選択ステップと、
    前記選択ステップにおいて前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加ステップと、
    前記選択ステップにおいて前記把持対象部材として選択された被把持部材を前記被把持部材リストから削除する削除ステップとを備えることを特徴とする物体把持制御方法。
14. 複数の被把持部材の形状・位置・姿勢を認識する認識ステップと、
    前記認識手段により認識された被把持部材に基づいて、被把持部材リストを生成する生成ステップと、
    前記把持手段及び前記被把持部材の形状・位置・姿勢に基づいて、前記被把持部材のそれぞれについて、他の被把持部材と前記把持手段とが干渉し得る範囲を把持禁止範囲として取得する取得ステップと、
    前記被把持部材のそれぞれについて、前記設定された把持禁止範囲が所定の閾値よりも小さいかを判定する第１の判定ステップと、
    前記第１の判定ステップにおいて所定の閾値よりも小さいと判定された被把持部材を把持対象部材として選択する選択ステップと、
    前記選択ステップにおいて前記把持対象部材として選択された被把持部材を把持対象部材リストに追加する追加ステップと、
    前記選択ステップにおいて前記把持対象部材として選択された被把持部材を前記被把持部材リストから削除する削除ステップとを備えることを特徴とする物体把持制御方法。

Images