JP5616478B1 - ワークを搬送するロボットを備えるロボットシステム - Google Patents

Abstract

【課題】ワークを安定的に取出せるロボットシステムを提供する。【解決手段】ロボットシステムは、ワークの位置情報を取得する三次元測定機と、ワークを保持可能なツールを備えたロボットと、ロボットを制御する制御装置と、を具備する。制御装置には、ワークの位置情報と、ワークの重心位置とを関連付けた関連付けデータが格納される。取出動作の対象のワークは、三次元測定機によって取得される位置情報と、関連付けデータと、に基づいて、その重心位置が推定される。制御装置は、推定された重心位置に従って、ワークの保持位置、取出方向およびツールの位置および姿勢のうち少なくとも１つを決定する。【選択図】図３

Description

本発明は、ワークを取出すとともに取出されたワークを搬送するのに使用されるロボットを備えるロボットシステムに関する。

コンテナにバラ積みにされたワークのように、所定の位置に置かれていないワークをツールによって取出すとともに取出されたワークを所定の場所まで搬送するのに使用されるロボットを備えたシステムが公知である。このようなロボットシステムにおいて、取出されるべきワークの位置および姿勢を正確に認識していない場合、ワークの位置および姿勢は不定であるので、ワークがツールによって保持されるときに、ワークとツールとの間の位置関係が定まらない。より具体的には、ワークを適当に保持したり、バラ積みにされたワーク上の或る取出し易い部分を保持したりする場合などでは、ツールによって保持されるワークの部位は毎回変化し得る。また、ツールによって保持されたときのツールに対するワークの姿勢も同様に毎回変化し得る。このような場合、またワークの位置および姿勢を認識していてもワークの重心位置が分からない場合、ワークを保持するときにツールと、ワークの重心位置との位置関係を、所望の位置関係とすることができない。

そのため、ワークの重心から離間した位置にある部位においてワークが保持されたり、ワークが不安定な姿勢で保持されたりする場合がある。そのような場合、ワークが安定的に保持されないので、ツールによって保持されるワークの部位が相対的に移動して保持状態が搬送中に変化したり、ワークが搬送中に落下して周囲の装置を損傷させたりし、これによってシステムの稼動を中断させるだけではなく、周囲の作業者に危険を及ぼしたりする虞がある。

特許文献１には、ワークの保持状態を検査して、必要に応じてワークを別の姿勢で保持し直すことが開示されている。特許文献２〜４には、視覚センサなどを利用して、ツールに対するワークの姿勢を補正することが開示されている。特許文献５には、複数の方向から取得されるワークの画像データと、教示モデルとを予め関連付けるとともに、ロボットおよびワークの間の相対方向の情報と、教示モデルとの関連付けを予め関連付けることによって、ワークを取出す際に適切な教示モデルに従って、ロボットの動作を決定することが開示されている。特許文献６には、ワークの取出しに失敗したときに、ワークを箱の中央に近付けるようにロボットの動作を制御することが開示されている。特許文献７には、ワークを取出す際に、異常を検知したら取出工程を中断してロボットを退避させることが開示されている。特許文献８〜１０には、ツールとワークとの間に作用する力に対する重力の影響を補償する目的でワークの重量および重心位置、重力方向を算出する方法が開示されている。

特開２０１１−１８３５３７号公報 特開平５−２４１６２６号公報 特開２００４−２４９３９１号公報 特開２０１１−２０１００７号公報 特許第３３００６８２号公報 特許第３９２５０２０号公報 特開２０００−２６３４８１号公報 特開平５−１１６０８１号公報 特開２０１２−４０６３４号公報 特開平７−２０５０７５号公報

前述したような公知の関連技術においては、ワークの保持状態が不安定である場合に、ワークを保持し直すための追加の工程が要求されるので、サイクルタイムの増大につながる。 そこで、取出した後にワークを保持し直す必要がないようにワークを安定的に取出せるロボットシステムが望まれている。

本願の１番目の発明によれば、三次元空間に置かれたワークを取出して搬送するロボットシステムにおいて、前記ワークの表面位置を測定し、前記ワークの表面上の複数の点の、三次元空間における位置情報を取得する三次元測定機と、指定される位置および姿勢に従って動作可能なロボットと、前記ロボットの先端部に取付けられていて前記ワークを保持可能なツールと、前記ワークが前記ツールによって保持されているときに前記ワークが前記ツールに作用する力を計測する力計測部と、前記ロボットを制御する制御装置と、を具備しており、前記制御装置は、前記三次元測定機によって取得される前記複数の点の位置情報に基づいて、取出すワークおよび該ワークの位置および姿勢を特定するワーク特定部と、前記ワーク特定部によって特定された前記ワークが取出されるように、前記ワークの保持位置、前記ワークの取出方向、および前記ツールの位置および姿勢を決定する取出位置姿勢決定部と、前記ワークを保持したロボットの複数の姿勢において前記力計測部によって計測される力データに基づいて、前記ツールによって保持された状態の前記ワークの重心位置を算出する重心位置算出部と、前記ツールによって保持された状態の前記ワークについて、前記三次元測定機によって取得される該ワークの位置情報と、前記重心位置算出部によって算出される該ワークの重心位置と、を関連付ける関連付け部と、前記関連付け部による関連付けの結果を記憶する関連付け記憶部と、前記関連付け記憶部によって記憶された関連付けの結果に基づいて、前記ワーク特定部によって特定された前記ワークの重心位置を推定する重心位置推定部と、を備えており、前記取出位置姿勢決定部は、前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワーク特定部によって特定された前記ワークを取出す際の前記ワークの保持位置、前記ワークの取出方向および前記ツールの位置および姿勢のうちの少なくとも１つを決定するように形成される、ロボットシステムが提供される。
本願の２番目の発明によれば、１番目の発明のロボットシステムにおいて、前記取出位置姿勢決定部は、前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワークの保持位置を決定するように形成されるとともに、前記ワークが保持され得る前記ワークの保持位置のうち、前記重心位置推定部によって推定される前記ワークの重心位置から最短距離にある保持位置から所定閾値の範囲内に含まれる保持位置を、前記ワークの実際の保持位置として決定するように形成される。
本願の３番目の発明によれば、１番目の発明のロボットシステムにおいて、前記取出位置姿勢決定部は、前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワークの保持位置を決定するように形成されるとともに、前記ワークが保持され得る前記ワークの保持位置のうち、前記ワークの重心位置と、前記ワークの取出方向または前記ワークを取出す際の前記ツールの姿勢と、に基づいて、前記ワークの実際の保持位置を決定するように形成される。
本願の４番目の発明によれば、１番目の発明のロボットシステムにおいて、前記取出位置姿勢決定部は、前記ワークを取出す際の移動方向が、前記重心位置から、前記保持位置または該保持位置から所定の距離だけ離間した位置に向けられた方向に一致するように、前記ワークを取出す際の前記ツールの位置および姿勢を決定するように形成される。
本願の５番目の発明によれば、１番目から４番目のいずれかの発明のロボットシステムにおいて、前記ワーク特定部は、前記取出位置姿勢決定部によって決定される前記保持位置または該保持位置から所定の距離だけ離間した位置と、前記重心位置との間の距離が、所定の閾値を超える場合に、別のワークを代替的に特定するように形成される。
本願の６番目の発明によれば、１番目から５番目のいずれかの発明のロボットシステムにおいて、前記ワーク特定部は、前記ワーク特定部によって特定されたワークが別のワークと重なっている場合、前記取出位置姿勢決定部によって決定される前記ワークの保持位置または該保持位置から所定の距離だけ離間した位置と、ワークどうしが重なり合う領域において定められる或る位置との間の距離が、所定の閾値以下である場合、別のワークを代わりに特定するように形成される。
本願の７番目の発明によれば、１番目から６番目のいずれかの発明のロボットシステムにおいて、当該ロボットシステムは、前記ワークを取出す際の力データに基づいて、前記ワークの取出し時に発生する力の大きさおよび振動を検出する検出部と、前記検出部による検出結果に基づいて、前記ワークの取出し時の安定性を判定する安定性判定部と、をさらに備えており、前記ワーク特定部は、前記安定性判定部による判定結果に基づいて、ワークを特定する際の優先順位を変更するように形成される。
本願の８番目の発明によれば、１番目から６番目のいずれかの発明のロボットシステムにおいて、当該ロボットシステムは、前記ワークを取出す際の力データに基づいて、前記ワークの取出し時に発生する力の大きさおよび振動を検出する検出部と、前記検出部による検出結果に基づいて、前記ワークの取出し時の安定性を判定する安定性判定部と、をさらに備えており、前記取出位置姿勢決定部は、前記安定性判定部による判定結果に基づいて、ワークの保持位置、ワークの取出方向またはワークを取出す際の前記ツールの姿勢を変更するように形成される。

本発明によれば、ワークを取出す際に、ワークの重心位置を推定し、推定された重心位置に基づいて、ワークを安定的に取出せるように、ワークの保持位置、ワークの取出方向またはツールの位置および姿勢が決定される。それにより、ワークの取出し搬送時に、ワークが大きくずれたり、ワークを落としたりすることを減らし、ワークを安定して取出し搬送することができるので、サイクルタイムの短縮ひいてはシステムの稼働コストの削減が可能になる。

一実施形態に係るロボットシステムの構成例を示す概略図である。 一実施形態に係る制御装置の構成を機能的に示す図である。 ロボットの制御装置による処理を説明するフローチャートである。 一実施形態に従って決定されるワークの保持位置の例を示す図である。 一実施形態に従って決定されるワークの保持位置の別の例を示す図である。 一実施形態に従って決定されるワークの保持位置の別の例を示す図である。 一実施形態に従ってワークを取出す態様を示す図である。 ワークが重なり合って配列される例を示す図である。 一実施形態に係る制御装置の構成を機能的に示す図である。 一実施形態に従って、ワークの保持位置および取出方向を変更する場合の例を示す図である。 一実施形態に従って、ワークの保持位置を変更する場合の例を示す図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下、力は、特に断らない限り、力および力のモーメントを含むものとする。図１は、一実施形態に係るロボットシステム１１の構成例を示す概略図である。ロボットシステム１１は、指定された位置および姿勢に従って動作可能なロボット４０と、ロボット４０を制御する制御装置１０と、ワーク５０の表面の三次元位置を測定する三次元測定機４６と、を具備している。ロボットシステム１１では、コンテナ５２内にバラ積みされたワーク５０に対して三次元測定機４６によりワーク５０上の三次元点を計測した情報を基に、ツール４４を用いてワーク５０を取出して搬送する。図示されるロボット４０は６軸多関節型ロボットであるものの、本発明は、他の形態を有する任意の公知のロボットにも同様に適用可能である。ロボット４０の各駆動軸は制御装置１０によって制御され、それによりロボットアームの先端が三次元空間の指定された場所に位置決めされ得るようになっている。制御装置１０は、ＣＰＵ、ＲＯＭ、ＲＡＭなどを含むハードウェア構成を有していて、後述する種々の機能を実行する。

ロボット４０は、コンテナ５２内にバラ積みされたワーク５０を取出して所定の場所、例えばコンベヤまたは作業台（図示せず）まで順次移動させるのに使用される。ロボット４０のアームの先端には、力計測部である６軸の力センサ４２が設けられている。力センサ４２には、空気圧式吸着パッドを具備するツール４４が取付けられている。ツール４４は、負圧を利用してワーク５０に吸引力を付与することによってワーク５０を保持できるように形成される。力センサ４２は、ワーク５０がツール４４によって保持された状態において、ツール４４に作用する力を計測するように形成されている。

力センサ４２の代わりに、ロボット４０の各関節を駆動させているモータの電流値を基にツール４４に作用する力を推定するように、間接的に取得するようにしてもよい。また、ツール４４に力センサを取付けるなどして、ツール４４に作用する力を計測するようにしてもよい。また、バラ積みされたワークを取出すシステムでは、ロボット４０の手先に取付けられたツール４４がワーク５０やコンテナ５２の壁と衝突、接触したり、またワーク取出し時に別のワークに引っかかったりするなどして過負荷がかかることがあるため、衝突や接触、過負荷の検出のために、力計測部を備えていることがある。こういった、衝突防止、過負荷の検出、力監視または力制御の目的でロボット４０が力計測部を備えている場合は、その力計測部を力センサ４２の代わりに使用してもよい。

ツール４４は、ワーク５０を保持可能であれば他の形態を有し得る。例えば、２本の爪を開閉することによってワーク５０を挟持するように形成されるツールであってもよい。或いは、ワーク５０に対して吸引力を発生する電磁石を備えたツールであってもよい。また、ツールによってワーク５０を保持するときに、ツールとワークとの位置関係が変わらないように、ワーク５０に接触するツールの部位を柔軟な構造で形成されるようにし、ワーク５０に接触する際にその形状が柔軟に変化するようにしてもよい。例えば、ばねや蛇腹機構や、空気を通す穴を複数有し柔軟な材料で作られた機構、などで形成され、吸引することによってワーク５０を引付けその状態を固定する、空気圧式吸引機構部を有するツールにしてもよい。

図１に示されるように、ロボットシステム１１は、コンテナ５２に収容された複数のワーク５０の表面の三次元位置を測定する三次元測定機４６を備えている。三次元測定機４６は、支持台４８によってコンテナ５２内の複数のワーク５０を測定できるように所定の位置に設けられている。三次元測定機４６は、三次元空間における複数のワーク５０の表面の位置を測定できるように形成されていればよく、特定のタイプの測定機には限定されない。例えば、三次元測定機４６は、レーザを利用して三次元位置情報を取得するタイプの測定機でもよいし、または撮影された画像データを画像処理することによって三次元位置情報を取得するタイプの測定機でもよい。三次元測定機４６が設置される態様も特に限定されるものではなく、例えば床、壁など周辺の場所に固定されていてもよいし、ロボット４０のロボットアームに取付けられていてもよい。

三次元測定機４６は、コンテナ５２内に置かれたワーク５０の表面上の複数の三次元点の位置を測定することによって得られるワーク５０の三次元の位置情報をロボット４０の制御装置１０に送出するように形成される。制御装置１０に入力されたワーク５０の三次元位置情報は、制御装置１０の、後述する記憶部２４に記憶される。このとき、三次元測定機４６で取得した画像データを制御装置１０に送出して、制御装置１０においてワーク５０上の三次元点の位置を算出し記憶部２４に記憶するようにしてもよい。三次元測定機４６によって取得された複数の三次元点の位置情報を基に、あるワークと別のワークとの境界や、ワークとそれ以外の物体との境界が推定され、ワーク毎の位置情報が取得される。このとき、前記三次元点によって成される面の変化や、面領域が滑らかにつながっているか否か、三次元点の位置が所定閾値を超えて変化する箇所、または予め用意されたワークの三次元形状など、を基に、あるワークと別のワークとの境界や、ワークとそれ以外の物体との境界が推定される。三次元測定機４６によって取得された複数の三次元点の位置情報に対してはこの処理がおこなわれ、ワークの位置情報として、このようなワーク毎の位置情報が取得される。ワークの位置情報とは、ワークの表面上の複数の点の位置から、ワークの存在位置や保持可能な位置が分かり、ワークの位置および姿勢が推定され得る位置データとする。

図２は、本発明の実施形態に係る制御装置１０の構成を機能的に示す図である。図示されるように、制御装置１０は、重心位置算出部１２と、ワーク特定部１４と、取出位置姿勢決定部１６と、関連付け部１８と、関連付け記憶部２０と、重心位置推定部２２と、記憶部２４と、を具備している。さらに、三次元測定機４６で取得した画像などのデータを基に、ワーク５０上の三次元点の位置を算出する三次元点算出部を具備していてもよい。

重心位置算出部１２は、ロボット４０の複数の姿勢において力センサ４２によって計測される力データに基づいて、ツール４４によって保持された状態のワーク５０の重心位置を算出するように構成される。また、力データは、力および力のモーメントに関するデータを含んでいる。また、本明細書において、特に断らない限り、重心位置は質量中心の位置を意味する。

関連付け部１８は、ワーク５０の位置情報と、重心位置算出部１２によって算出されるワーク５０の重心位置と、を関連付けるように構成される。このときのワーク５０の位置情報は、ワーク５０の重心位置と関連付けるため、ツール４４によって保持された状態で、三次元測定機４６によって取得される。関連付け部１８では、ある座標系上における、ワーク５０の位置情報である、ワーク５０の表面上に存在する三次元点の分布と、ワーク５０の重心位置との位置関係を取得する。これによって、ワーク５０上の三次元点の分布がある程度、得られたとき、そのワーク５０の位置情報を基に、パターン認識などの公知の方法に従って照合などをして、ワーク５０の重心位置を推定することができる。または、次のように関連付け、推定をしてもよい。ワーク５０の表面上に存在する三次元点の分布とワーク５０の重心位置を基に、ワーク５０の位置および姿勢を表し原点を重心とする座標系を定め、その座標系と、ワーク５０の表面上に存在する三次元点の分布とを対応付ける。ワーク５０のある位置情報が取得されたときには、その三次元点の分布から、その座標系の位置および姿勢を算出し、ワークの重心位置や、位置および姿勢を推定する。関連付け部１８による関連付けの工程の例は後述する。関連付け記憶部２０は、関連付け部１８による関連付けの結果を記憶するように構成される。

ワーク特定部１４は、三次元測定機４６によって取得されるワーク５０の表面上の複数の点の三次元の位置情報に基づいて、取出されるべきワーク５０および該ワーク５０の位置および姿勢を特定するように構成される。ここで、取出されるべきワーク５０とは、安定的にまたは容易に取出し可能と判断されるワーク５０を意味する。例えば、上方から認識可能な面積が大きいワーク、置かれたワークのうち高い位置にあるワーク、別のワークの上に載っているワークなどを、そのようなワークと判断する。ワーク特定部１４が特定するワーク５０の位置および姿勢情報は、ワーク５０を保持することを可能とするような、ワークの表面上の複数の三次元点の位置を基にした、おおまかなワークの位置および姿勢であってもよい。つまり、ワークの表面上の複数の点の位置から、ワークのおおまかな存在位置や保持可能な位置が分かり、その位置情報を基にワークの位置および姿勢が推定され、ワークを保持することが可能となる。ワークの位置および姿勢は、そのワークの表面上の複数の三次元点の位置を基に、より正確に推定されることが好ましい。重心位置推定部２２は、ワーク特定部１４によって特定されたワーク５０に対して、ワーク５０の重心位置を推定するように構成される。具体的には、特定されたワーク５０のそのときの位置情報と、ワーク５０の位置情報と、ワーク５０の重心位置との関係を示す、関連付け記憶部２０に記憶された関連付けのデータに従って、ワーク５０の重心位置が導き出される。

取出位置姿勢決定部１６は、ワーク特定部１４によって特定されたワーク５０を取出せるように、ワークの保持位置、ワークの取出方向およびツールの位置および姿勢を決定するように構成される。このとき、ワーク５０が置かれているコンテナ５２とツール４４との干渉を避けるように考慮することが好ましい。本実施形態によれば、取出位置姿勢決定部１６は、さらに重心位置推定部２２によって推定されるワーク５０の重心位置に基づいて、ワーク５０の保持位置、ワークの取出方向およびツール４４の位置および姿勢のうちの少なくとも１つを決定する。

本発明において、ロボット４０に対する、ワーク５０の保持位置は、ロボット４０のロボットアームに設定された制御点を基準として、所定のルールに従って定められる位置を表す。すなわち、ロボット４０に対する、ワーク５０の保持位置は便宜的に設定される。例えば、ワーク保持位置は、（ａ）ＴＣＰ（ｔｏｏｌ ｃｅｎｔｅｒ ｐｏｉｎｔ：ロボットアームに対して設定される制御点）の位置やＴＣＰから所定方向に所定量だけシフトさせた位置、（ｂ）ツール４４がワーク５０に実際に接触する位置などであり得る。（ｂ）の場合において、ツール４４とワーク５０の接触位置が複数存在する場合、それら接触位置を代表する位置であり得る。例えば、ワーク５０が保持された状態において、ツール４４がワーク５０の面の３箇所においてワーク６０に接触する場合、ワーク保持位置は、（ｂ１）３つの接触箇所の代表的位置、例えばそれら接触箇所の中心に設定され得る。また、代替的に２つの爪を開閉させてワーク５０を保持するツールが使用される場合、ワーク保持位置は、（ｂ２）それら爪の中間、（ｂ３）爪の中間から所定量だけ離間した位置、（ｂ４）それら爪のいずれか一方に設定されるＴＣＰの位置に設定され得る。

記憶部２４には、ロボット４０のための教示プログラム、動作プログラム、算出されるワーク５０の重心位置、ワーク５０の重量、三次元測定機４６から得られるワーク５０の三次元位置情報、各種計算に必要なパラメータおよび計算結果などが記憶される。

図３は、ロボット４０の制御装置１０による処理を説明するフローチャートである。本実施形態によれば、ワーク５０の取出工程を開始する前の準備工程（ステップＳ１〜Ｓ４）として、ワーク５０の位置情報と、ワーク５０の重心位置とを関連付ける関連付けデータが取得される。

先ず、ロボット４０が所定の教示プログラムに従って動作され、ツール４４によってワーク５０が保持される（ステップＳ１）。続いて、重心位置算出部１２が起動され、保持されたワーク５０の重心位置が算出される（ステップＳ２）。ステップＳ２によって、ツール４４によって保持された状態でのワーク５０の重心位置が求まり、力センサ４２に設定した座標系上の、またはロボット４０の手先に設定した座標系上の、ワーク５０の重心位置が求まる。ワーク５０の重心位置は、ステップＳ１においてワーク５０が保持された後、ロボット４０を動作させ、ワーク５０の姿勢を変化させたときに力センサ４２によって検出される力データに基づいて、重心位置算出部１２によって算出される。

重心位置算出部１２は、例えば特許文献８〜１０に開示されるように、ロボット４０の停止時の力データに基づいて、重力が作用する力を基にワーク５０の重心位置および重量をそれぞれ算出する。或いは、ワーク５０の重心位置および重量は、ロボット４０の動作中の力データに基づいて、重力および慣性力などが作用する力を基に算出されてもよい。いずれの場合も、ツール４４の重心位置および重量が予め取得される。ツール４４の重心位置および重量は、ワークを保持しない状態のツールに対して、重心位置算出部１２を用いて算出する。そして、ワーク５０を保持した状態におけるツール４４とワーク５０を合わせた重心位置および重量と、予め取得しておいたツール４４の重心位置および重量とを基に、ワーク５０の重心位置および重量が算出される。このとき、ツール４４とワーク５０の両方から力センサ４２に作用する力を基に重心位置を算出し、その値を本発明におけるワークの重心位置と置き換えても同様の効果がある場合には簡単のためにその値を使用してもよい。また、ワーク５０の重量を算出する代わりに、予め測定または算出された重量の値を使用するように形成されてもよい。ワーク５０の重量の個体差が大きくなく、既知の値を使用できる場合は、ワーク５０の重量の算出処理を省略することができる。また、重心位置の計算結果の誤差を低減できる。

ロボット４０の動作中の力データが利用される場合、リンクの慣性パラメータ（質量、重心位置、慣性テンソル）を同定するのに用いられる公知の同定方法が使用される。例えば力センサ４２に取付けられた物体に対するＮｅｗｔｏｎ−Ｅｕｌｅｒ方程式を立て、複数の姿勢についての関係式から、力センサ４２に取付けられた物体（ツール４４およびツール４４によって保持されるワーク５０）の重量および重心位置が、例えば最小二乗推定によって算出される。そして、予め取得しておいたツール４４の重心位置および重量と、前記算出結果と、を基に、ツール４４によって保持されるワーク５０の重心位置および重量が算出される。

動作するのに従って重心位置が変化するツール、例えば開閉する２つの爪から形成されるツールが使用される場合は、ツールの構成部品の重心位置および重量を予め求めておく。ツールの可動部分、例えば爪のストローク長を計測する計測手段が使用される。そして、可動部分のストローク長および重量に基づいて、ツールの重心位置を補正する。それにより、ツールの重心位置、ひいてはワーク５０の重心位置を正確に算出できるようになる。

ワーク５０の重心位置を算出する際のワーク５０の移動位置および移動経路は、予め定められたものであるのが好ましい。例えば、予め重心位置および重量が既知のワークを用いて、ワークの重心位置および重量の算出が可能であることを確認できた移動位置および移動経路が利用される。ワークの重心位置および重量の算出が可能であるか否かの判定処理は、例えば、重心位置および重量の算出値が所定閾値を超えて異なるか否か、算出に使用される関係式の行列が縮退しているか否か、関係式における行列の条件数が所定閾値よりも小さいか否か、などに基づいて実行される。重心位置および重量が算出可能であるか否かの判定処理の結果は、結果に応じて、教示装置の画面に表示したり、教示装置で音や振動を出したり、備えられた表示灯で示したりしてもよい。

続いて、三次元測定機４６によってツール４４によって保持されたワーク５０の表面上の複数の三次元点の位置が測定され、ワーク５０の位置情報が取得される（ステップＳ３）。ステップＳ３によって、ツール４４によって保持された状態でのワーク５０について、力センサ４２に設定した座標系上の、またはロボット４０の手先に設定した座標系上の、ワーク５０の位置情報が求まる。このとき、ワーク５０は任意の姿勢を有し得るが、ワーク５０の姿勢を変えながら、ワーク５０の表面上の複数の三次元点の位置を取得し、ワーク５０の位置情報が取得されてもよい。また、ステップＳ２のワーク５０の重心位置の算出は、ステップＳ２とステップＳ３を同時におこなうことによって算出されてもよい。つまり、ワーク５０の位置情報を取得するために、三次元測定機４６に対してロボット４０に保持されたワーク５０の位置および姿勢を変えるときに、力センサ４２によって取得された力データを基に、ワーク５０の重心位置が算出されてもよい。なお、この際に算出されたワーク５０の重量を基に、ワーク５０の重量が所定閾値を超える場合のみ、本発明を適用してもよい。

そして、関連付け部１８によって、ステップＳ３で得られたワーク５０の位置情報と、ステップＳ２で得られたワーク５０の重心位置を基に、ワーク５０の位置情報からワーク５０の重心位置が推定できるように対応付けられる関連付けデータが得られる（ステップＳ４）。なお、ステップＳ３において、ツール４４によってワーク５０の一部分が保持されていることや、ロボット４０やツール４４との干渉などによってワーク５０の一部分の位置が計測不能である場合、ワーク５０の一部の位置情報が三次元測定機４６によって取得されない。そのため、またワークの位置情報とワークの重心位置との関連付けの精度を向上させるため、保持位置を変更した上で、ステップＳ２、ステップＳ３を複数回おこなって取得した複数のデータを基に、ワークの位置情報とワークの重心位置との関連付けの精度を向上させてもよい。また、コンテナ５２内に置かれているワーク５０の種類が複数ある場合、異なる種類のワーク毎に、予めステップＳ１〜ステップＳ４を実行し関連付け部１８によって、ワークの位置情報とワークの重心位置とを関連付けておくことが好ましい。

なお、ステップＳ１〜ステップＳ４の準備工程においてワーク５０を保持するために使用されるツールは、ワーク５０の取出および搬送を実行するのに実際に使用されるツール４４とは別の構成を有するものであってもよい。例えば、ワーク５０の取出し工程に実際に使用されるツールは、ワーク５０を損傷させないように吸着力や把持力の弱いツール、またはワーク保持時のずれを吸収したり、衝撃を吸収したりする機能を有するばね機構または蛇腹機構を有するツールが使用されることも多い。しかしながら、準備工程では、ワークを単に保持できればよいので、ツールの種類は必要に応じて変更されてもよい。また、重心位置の計測精度を向上させるためにワークの重心付近からなるべく離した位置を保持するようにしたり、関連付けの精度向上のためにツールに対するワーク５０の重心位置および位置が変わるように複数回、ステップＳ２、ステップＳ３をおこなったりしてもよい。

続いて、ワーク５０の取出工程（ステップＳ５〜Ｓ９）が実行される。ワーク５０の取出工程においては、ワーク特定部１４によって安定的にまたは容易に取出し可能と判断されるワーク５０が特定される（ステップＳ５）。取出されるべきワーク５０の特定は、三次元測定機４６によって取得されるワーク５０の表面上の複数の点の三次元の位置情報に基づいて、あるワークと別のワークとの境界や、ワークとそれ以外の物体との境界が推定され、ワーク毎の位置情報が取得され、それによって推定されるコンテナ５２内の複数のワーク５０のコンテナ５２内における配列状態に基づいて実行される。例えば、ワーク表面上の認識可能な面積が大きいワーク、置かれたワークのうち高い位置にあるワーク、別のワークの上に載っているワークなどがステップＳ５において特定される。

そして、ワーク特定部１４によって、取出されるべきワーク５０が特定されるとともに、そのワーク５０の位置および姿勢が求められる（ステップＳ６）。ワーク特定部１４が特定するワーク５０の位置および姿勢は、ワーク５０を保持することを可能とするような、ワークの表面上の複数の三次元点の位置を基にした、おおまかなワークの位置および姿勢であってもよい。つまり、ワークの表面上の複数の点の位置から、ワークのおおまかな存在位置や保持可能な位置が分かり、その位置情報を基にワークの位置および姿勢が推定され、ワークを保持することが可能となる。ワークの位置および姿勢は、そのワークの表面上の複数の三次元点の位置を基に、より正確に推定されることが好ましい。

次に、ワーク重心位置推定部２２が起動され、ステップＳ６においてワーク特定部１４が特定したワーク５０について、三次元測定機４６によって取得されるワーク５０の表面上の複数の点の三次元の位置情報に基づいて取得されるワーク５０の位置情報と、ステップＳ４において予め取得しておいたワークの位置情報と重心位置とを関連付ける関連付けデータと、に基づいて、特定されたワーク５０の重心位置が推定される（ステップＳ７）。ワーク５０の重心位置の推定は、ワーク特定部１４が特定したワーク５０の位置情報と、予め取得しておいたワークの位置情報と重心位置との関連付けデータにおけるワークの位置情報とを、パターン認識などの公知の方法に従って照合などをして、ロボット４０の基準座標系からのワーク５０の重心の位置を推定する。

取出位置姿勢決定部１６は、ステップＳ６で取得されたワーク５０の位置および姿勢と、ステップＳ７で取得されたワーク５０の重心位置と、に基づいて、ワーク５０を安定的ないし容易に取出すための態様を決定する。例えば、ワーク５０の保持位置、ワーク５０の取出方向およびワーク５０を取出す際のツール４４の位置および姿勢のうちの少なくとも１つが決定される（ステップＳ８）。そして、ステップＳ８で決定された態様に従って、ワーク５０が取出される（ステップＳ９）。

以下、取出位置姿勢決定部１６の作用についてさらに説明する。ワーク５０を取出して移動させる際には、ワーク５０の重心位置に重力および慣性力が作用する。また、ワーク５０の保持位置と重心位置との間の位置関係、特に距離、重力方向および移動方向に応じて保持位置回りに発生する力のモーメントは変化する。ロボット４０によってツール４４で保持されたワーク５０を搬送するとき、ワーク５０の保持位置にかかる力や力のモーメントが大きくなれば、保持方法によっては、ワークの保持状態が不安定になることがある。そこで、ワーク５０の保持位置と重心位置との位置関係や、ワーク５０を取出して移動させる方向を適切に変え、保持位置回りの力のモーメントの大きさを小さくするなどして、ワークの取出し搬送を安定な状態でおこなうようにする。ロボット４０は、ワーク５０を保持するとき、ロボット４０に対して所定のルールによって定められる位置が、ワーク５０の保持位置となるように、ツール４４の位置および姿勢を移動させてワークを保持するようにする。最初に、取出位置姿勢決定部１６によってワーク５０の保持位置が決定される場合について考える。

図４は、本実施形態に従って決定されるワーク５０の保持位置の例を示す図である。図４に示される例において、取出位置姿勢決定部１６は、ワーク５０の表面上の保持され得る位置において、ワーク５０の重心位置Ｇから最短距離に位置する点Ｐを保持位置に設定する。なお、ワーク５０の表面上の保持され得るまたは保持可能な位置とは、ワーク５０の表面上の位置で、ツール４４によって保持可能な位置であるとともに、ロボット４０が到達可能な位置とする。なお、図中の矢印Ａは、ワーク５０の取出方向の例を表している。また、保持位置は、必ずしも重心位置Ｇから最短距離の位置Ｐである必要はなく、最短距離の位置Ｐから所定の閾値範囲内から選定されてもよい。

図５は、本実施形態に従って決定されるワーク５０’の保持位置の別の例を示す図である。この例において使用されるワーク５０’の表面には、ツール４４によって保持され得ない保持不能領域５４が形成されている。この場合、図示されるように、保持不能領域５４から所定距離だけ離間していて、かつ重心位置Ｇにできるだけ近い点が保持位置に設定される。図示された例では、保持不能領域５４の両縁に図示された点Ｐ１およびＰ２のうち、重心位置Ｇにより近い点Ｐ１が保持位置に設定される。図４および図５を用いて説明したように、取出位置姿勢決定部１６は、特定されたワーク５０に対して、ワーク５０の重心位置を基に、ワーク５０を安定して取出して搬送することができる保持位置を決める。また、取出位置姿勢決定部１６は、ワーク５０の位置、コンテナ５２の壁の位置、ツール４４の大きさや形状などによって、ツール４４と周囲との干渉を回避するように、ワーク５０の取出方向やツール４４の位置および姿勢を制限し、その制限内において、ワーク５０の取出方向やツール４４の姿勢を、例えば、ワーク５０の表面に直交する方向、また、重力方向とは逆側の方向、また、ワーク５０の姿勢に応じて設定する任意の方向、また、ワーク５０の長軸方向と平行な方向、などとして、ワーク５０を取出すようにしてもよい。図４および図５の例に従って保持位置がワーク５０の重心位置の近傍に設定されれば、ワーク５０の取出し搬送時に、ワーク５０の移動方向や姿勢を任意の方向に変えるなどしても、ワーク５０の保持状態が安定し、ワーク５０を落下させたり、保持位置がずれたりすることに起因する予期せぬ事故、システムの停止などを防止できる。ここで、ツールが２つ爪で構成され、爪の開閉によってワークを保持するような場合、ワークの重心位置から最短距離から所定閾値内の距離の位置付近に対して、それぞれの爪が接触するように保持してもよい。

図６は、本実施形態に従って決定されるワーク５０の保持位置の別の例を示す図である。図６に示される例においては、保持位置は、ワーク５０の表面上の保持可能な位置において、ワーク５０の重心位置Ｇを通り、ワーク５０の取出方向を表す矢印Ａの直線上に位置する点Ｐが保持位置に設定される。このとき、ワーク５０の重心位置Ｇから取出方向に向かってワーク５０の表面上に存在する位置が、保持できない位置やツール４４が到達できない位置である場合、その位置から最短、または所定閾値内にある、保持可能な位置を保持位置とする。取出方向は任意の方向であるが、例えば次のように決められる。ツール４４の先端に吸着パッドが取付けられている場合、ツール４４の先端部分がワーク５０の表面と面直で接触するようにして面直方向に取出すことによって、安定した取出しが可能となることがある。この場合、面直とした方向に平行な方向を取出方向とする。また、ワーク５０を保持した後に、ワーク５０が置かれた位置から適当な方向にワーク５０を少し引出した後に、ワーク５０を取出し移動させる方向を、取出方向としてもよい。また、特定されたワーク５０付近の複数のワークの位置情報を基にし、ワーク５０に対してツール４４を近付けたり保持した後に取出したりする時に、他のワークと接触しないように、他のワークとの最短距離がなるべく大きくなる方向としてもよい。このように、ワーク５０の取出方向が定められている場合、ワーク５０の取出方向に基づいて適切な保持位置が決定される。また、矢印Ａはツールの姿勢に基づく方向であってもよい。ワーク５０の表面上の保持可能な位置において、ワーク５０の重心位置と、ツール４４の姿勢と、に基づいて保持位置を決定してもよい。ツール４４の姿勢からワーク５０の取出方向が決まる場合、このようにワーク５０の取出方向の代わりにツール４４の姿勢を用いてもよい。また、所望の取出方向が定まっていて、重心位置から所望の取出方向に存在する位置に対して、ツール４４が周辺部との干渉などによって、その位置に移動させられない場合、その位置から所定閾値内の距離にありツール４４が移動可能な位置を位置とする。また、ワーク５０の表面を保持力が強い状態で保持するためや周囲との干渉によってツール４４の姿勢が定まり、取出方向とツール４４の姿勢が一致しない場合に、ワーク５０の重心位置からツール４４の姿勢を基にした方向で、ワーク５０の保持可能な位置を、保持位置としてもよい。

図６を用いて説明したように、取出位置姿勢決定部１６は、特定されたワーク５０に対して、ワーク５０の重心位置と、ワーク５０の取出方向またはツール４４の姿勢を基に、ワーク５０を安定して取出して搬送することができる保持位置を決める。このとき、取出位置姿勢決定部１６は、前記で使用するワーク５０の取出方向またはツール４４の姿勢を求めておく。取出位置姿勢決定部１６は、ワーク５０の位置、コンテナ５２の壁の位置、ツール４４の大きさや形状などによって、ツール４４と周囲との干渉を回避するように、ワーク５０の取出方向やツール４４の位置および姿勢を制限し、その制限内において、ワーク５０の取出方向やツール４４の姿勢を、例えば、ワーク５０の表面に直交する方向、また、重力方向とは逆側の方向、また、ワーク５０の姿勢に応じて設定する任意の方向、また、ワーク５０の長軸方向と平行な方向、などとして、ワーク５０を取出すようにしてもよい。このように、ワーク５０の重心位置とワーク５０の取出方向とに基づいて、保持位置を決めることによって、取出し搬送時に、保持位置にかかる力や力のモーメントを小さくして、なるべく安定的な保持状態とすることができる。

また、取出位置姿勢決定部１６は、ワーク５０の重心位置と、任意の保持位置と、に基づいて、ワーク５０の取出方向を決定するように作用し得る。ワーク５０の表面上に定める保持位置は次のように任意の位置とすることができる。例えば、ワークの表面上の保持可能な位置において、三次元測定機４６によって認識されるワーク５０の表面の三次元点を基に、認識される三次元点の中心の位置、またはワーク５０の表面上の面積が大きい面の中心の位置、またはその部分のワークの位置情報を基に凸凹が少なく保持し易いと判断される位置、または保持し易い形状や材質や取出し搬送後の工程を踏まえてワーク５０に対して設定される所定位置、などとすることができる。
取出位置姿勢決定部１６は、ワーク５０の重心位置とワーク５０の保持位置の位置関係を基に、重心位置から保持位置へのベクトルが、ワーク取出し時の移動方向のベクトルと平行となるように、ワークを取出す際の移動方向である、ワークの取出し方向および搬送時の移動方向を決定する。この場合、図６に関連して説明した例と同様に、取出方向に延びる同一直線上に保持位置および重心位置が位置するようになり、ワーク５０を安定的に保持した状態でコンテナ５２から取出せる。また、取出位置姿勢決定部１６は、ワーク５０の保持位置と、ワーク５０の重心位置と、に基づいてツール４４の姿勢を決定するように作用してもよい。ツール４４の姿勢からワーク５０の取出方向が決まる場合、このようにツール４４の姿勢を決定してもよい。また、取出位置姿勢決定部１６は、ワーク５０の位置、コンテナ５２の壁の位置、ツール４４の大きさや形状などによって、ツール４４と周囲との干渉を回避するように、ワーク５０の取出し時および搬送時の移動方向やツール４４の位置および姿勢を制限し、その制限内において、ワーク５０の取出し搬送時の移動方向やツール４４の姿勢を決めることが好ましい。

前述した実施形態によれば、取出位置姿勢決定部１６によって重心位置および保持位置から、ワーク５０を取出す際の移動方向が決定される。これによって決まる移動方向によっては、コンテナ５２との位置関係、他のワーク５０との位置関係、ロボット４０の姿勢またはツール４４の構造上の制約に起因して、周囲のワークやコンテナ５２に衝突したり、移動方向が不適切であったりして、ワーク５０をコンテナ５２からうまく取出せず、取出しおよび搬送時の移動方向を変更すべき場合がある。そのような場合、重心位置と保持位置によって決まる移動方向が、所望の方向となるように、ツール４４によって保持されたワークの姿勢を変えて、または変えながら移動させる。例えば、ワーク５０を取出す態様を例示する図７に示されるように、ワーク５０の重心位置から保持位置に向かうベクトルが、図７（ａ）のように下向きの、コンテナ５２内に向かう成分を含む場合、そのまま移動させれば、他のワークやコンテナ５２に衝突させることになる。そこで、ワーク５０を保持し、保持位置Ｐを維持したまま、ワーク５０の重心位置から保持位置に向かうベクトルが、上向きの、コンテナ５２内から離す方向に向くように、図７（ａ）〜（ｅ）で順次示されるように、ワーク５０の姿勢を変更した後、または変更しながら、ワーク５０の取出し搬送をおこなうようにする。このように、ワーク５０の姿勢を変更することによって、ワーク５０の重心位置から保持位置に向かうベクトルが、所望の取出しおよび搬送時の移動方向（矢印Ａ１〜Ａ５で示される）となる。これに従ってロボット４０によって保持されるワーク５０を移動させれば、ワーク５０の安定的な保持状態を維持しながら、適切な取出し搬送動作を実行できる。

以上、ワーク特定部１４によって特定されたワーク５０を安定的に取出すための取出位置姿勢決定部１６の機能について説明したが、特定されたワーク５０について安定的な取出しおよび搬送動作が困難な場合、そのワーク５０の取出しを実行せず、別のワーク５０を取出した方がよい場合もある。例えば、重心位置と保持位置との間の距離が所定の閾値を超える場合に、制御装置１０は、ワーク特定部１４によるワーク５０の特定処理を再度実行するように構成されてもよい。

別のワーク５０を代替的に特定するように、ワーク５０の特定処理を再実行する必要がある他の例について説明する。例えば、ワーク特定部１４によって特定されたワーク５０が、他のワークと重なっている場合、取出動作が円滑に行えないことがある。

図８は、第１のワーク６０と、第２のワーク６２とが重なり合って配列された場合の例を示している。ワーク特定部１４によってワーク６０が特定されたとする。ワーク特定部１４によって特定されたワーク６０の保持位置Ｐ、ワーク６０を取出す方向は、前記の実施例の通り、取出位置姿勢決定部１６によって決められる。図８は、また、ワーク６０とワーク６２とを、ワーク６０を取出す方向が紙面上方向となるように見た様子を示している。ワークを取出す方向から見て、保持位置と、別のワークが重なっている部分とが近い場合、重なっている領域が狭いとしても、取り出し時に保持位置に掛かる負荷が大きいため、取り出し時に保持状態が変わったり、ワークを落としたりするなど、安定に取り出せない可能性が高い。逆に、別のワークが重なっていたとしても、保持位置と、別のワークが重なっている部分が離れている場合、前記のような問題がないこともある。図８の（ａ）は、保持位置Ｐと、第１のワーク６０および第２のワーク６２が重なり合う重なり領域６４の中心位置Ｍとの間の距離が大きい場合を示している。他方、図８の（ｂ）は、保持位置Ｐと、重なり合う領域６４の中心位置Ｍとの間の距離が小さい場合を示している。図８の（ａ）に示されるように、第１のワーク６０の保持位置Ｐと、重なり領域６４の中心位置Ｐとが十分に離間しているときは、第１のワーク６０の取出動作が比較的円滑に実行され得る。この場合は、ワークの特定を再実行する必要はない。

他方、図８の（ｂ）に示されるように、保持位置Ｐが重なり領域６４に近い場合、第１のワーク６０を取出そうとするときに、安定して取出すためには強い保持力が要求され、取出す際に保持位置Ｐがずれて保持状態が変わったり、ワークを落としたりするなど、安定的に取出せない虞がある。したがって、保持位置Ｐと、重なり領域６４との間の距離が所定の閾値以下であるときに、ワーク特定部１４による特定処理を再実行するように制御装置１０が構成されるのが好ましい。

図８を用いて簡単に説明したように、保持位置と、ワークどうしが重なり合っている領域が近い場合、ワーク特定部１４による特定処理を再実行する。このとき、取出さないようにしたワークは、ワーク特定部によって、次にまたは数回、取出すワークの候補から外す、または別のワーク６２を次の候補とする、などのようにする。保持位置と、ワークどうしが重なり合っている領域が近いか否かは、具体的に、次のように判定する。ワーク６０を取出す方向のベクトルと直交する平面に、三次元測定機４６によって取得されたワーク６０やワーク６２の三次元点を射影し、その平面上で、三次元測定機４６によって取得されたワーク６０とワーク６２の表面上の三次元点の位置情報も基にして、ワーク６０とワーク６２とが重なり合う領域６４を推定する。前記平面における重なり領域６４の位置の重心を算出し、その位置を中心位置Ｍとする。前記平面における、算出した中心位置Ｍと保持位置との間の距離を算出する。このように算出された、ワーク６０とワーク６２とが重なり合う重なり領域６４の中心位置Ｍと、保持位置との距離が所定閾値以下なら、それらは近いと判定する。中心位置Ｍと、保持位置との距離が所定閾値より大きいなら、それらは近い状態ではないと判定する。また、保持位置と、ワークどうしが重なり合っている領域が近いか否かを次のように判定してもよい。前記と同様に、前記平面における、保持位置と、重なり領域６４の位置を算出する。この平面において、保持位置と、重なり領域６４との最短距離を算出する。この最短距離が所定閾値以下なら、それらは近いと判定する。この最短距離が所定閾値より大きいなら、それらは近い状態ではないと判定する。

前述したように、ワーク特定部１４は、三次元測定機４６によって取得されるワーク５０の位置情報に基づいて、安定的ないし容易に取出し可能と判断されるワーク５０を特定する。しかしながら、特定されたワーク５０を実際に取出そうとすると、取出動作が困難な場合がある。このことは、三次元測定機４６によって得られるワーク５０の表面上の三次元点に基づくワーク５０の位置情報において、誤検出や誤差があったり、ワーク５０の測定可能部分の裏側やワークどうしが重なっている部分において測定不能な箇所が存在するなどのワーク５０の配置状況によってワーク５０の情報が欠落していたりして、ワーク５０の状態を完全に把握できない場合があることに起因する。さらに、ワーク５０の材質または表面の形状に起因して、取出動作が困難になることもある。

そこで、ワーク５０の取出工程の際に力センサ４２によって測定される力および力のモーメントの力データに基づいて、ワーク５０の取出し易さに関する情報を収集し、三次元測定機４６によって取得されるワーク５０の位置情報と対応させる。その対応データを利用してワーク特定部１４でワークを特定するようにしてもよい。ワーク５０の取出し易さに関する情報として、例えば、ワーク５０を取出す際に計測される力の最大値、力の平均値、力の微分値の最大値または力の微分値の平均値などが収集される。ワーク５０を取出すとき、力および力のモーメントの平均値や最大値、微分値や微分値の平均値が所定の閾値より大きい場合、ワーク５０は取出しにくい状態であって、まわりのワークを過大な力で無理におしのけたり、場合によっては、周りのワークを傷つけたり、重心から離れた場所を保持して不安定な状態で取出したりしたと推定できる。それらの値が所定の閾値より小さい場合、ワーク５０が取出し易い状態であったために、うまく取出せたと推定できる。このようにワーク５０を取出すときに取得されたワーク５０の表面上の三次元点の位置情報と、ワーク５０の取出し易さとを対応付けて記憶部２４に記憶させておき、この記憶させたデータと、三次元測定機４６で取得されたワークの位置情報を基にすることによって、ワーク特定部１４はより安定して取出せるワークを特定することができるようになる。

図９は、本発明の一実施形態に従って、力センサ４２などの力測定部から得られる力データを利用して取出動作の安定性を判定するように形成された制御装置１０’を機能的に示す図である。制御装置１０’の構成および作用のうち、図２に関連して説明された実施形態と重複するものについては説明を省略する。

制御装置１０’の検出部７０は、ロボット４０に取付けられた力センサ４２と協働するように構成される。そして、ワーク５０を取出す際にツール４４に作用する力の最大値または平均値を検出する。或いは、検出部７０は、ワーク５０を取出す際に発生する振動を検出するように、ツール４４に作用する力の微分値の最大値または平均値を検出するように構成されてもよい。ここで、力は、力および力のモーメント、または力に代替可能な、ロボット４０を駆動させている電流値の値としてもよい。また、空気式吸着パッドの場合、目標圧力からの変化を検出してもよい。

制御装置１０’の安定性判定部７２は、検出部７０によって得られた情報に基づいて、ワーク５０を取出す際の安定性を判定するように構成される。例えば、安定性判定部７２は、ワーク５０を取出す際に、検出部７０によって検出される力が大きい場合、取出動作の安定性が低いと判定する。また、安定性判定部７２は、ワーク５０を取出す際に発生する力の微分値が大きい、すなわち振動が大きい場合も同様に、取出動作の安定性が低いと判定する。

本実施形態において、制御装置１０’のワーク特定部１４は、安定性判定部７２による安定性の判定結果に基づいて、ワーク５０を特定する際の優先順位を変更するように構成される。具体的には次のようにおこなう。ワーク特定部１４は、三次元測定機４６によって取得されるワーク５０の表面上の複数の点の三次元の位置情報に基づいて、ワーク表面上の認識可能な面積が大きい、また積重ねられたワークにおいて高い位置にある、また別のワークの上に載っているなどの状態を判断し、取出し易いと判定されるワークほどスコアが大きくなるように、取出し候補のワークに対してスコアを算出して、前記スコアの大きさに基づいて取出すワークの取出しの優先順位を定め、取出すワークを特定する。このとき、三次元測定機４６によって取得されたワーク５０の位置情報に対して、そのワークの位置情報の特徴と近いワークの位置情報における前記安定性判定部７２の安定性判定の情報が取得されている場合、安定性が低いと推定されるワークに対しては、前記スコアが小さくなるようにし、安定性が高いと推定されるワークに対しては、前記スコアが高くなるようにする。このスコア操作によって、ワーク特定部１４が取出すワークを特定する際の、取出しの優先順位を変更する。

本実施形態において、代替的または付加的に、制御装置１０’の取出位置姿勢決定部１６は、安定性判定部７２による安定性の判定結果に基づいて、ワーク５０の保持位置、取出方向またはワーク５０を取出す際のツール４４の姿勢を変更するように構成されてもよい。

図１０は、本実施形態に従って、ワークの保持位置および取出方向を変更する場合の例を示す図である。ここでは、取出し方向でなくても、ツールの姿勢の変更で対応可能な場合、ワークの保持位置およびツールの姿勢を変更するようにしてもよい。また、ワークの保持位置、またはワークの取出し方向、またはツールの姿勢を変更するようにしてもよい。図１０に示されるように、ワーク特定部１４によって取出される対象に設定された第１のワーク８０に隣接するように、第２のワーク８２が配置されている。このとき、取出し方向は、第１のワーク８０に対して、面直な方向にワークが取出される矢印Ａで示される方向とされ、保持位置は、重心位置Ｇから取出し方向に向かった点Ｐ１とされているとする。または、保持位置は、ワーク８０の重心位置Ｇから最短距離に位置する点Ｐ１とされ、取出し方向は、重心位置Ｇから保持位置Ｐ１に向かう方向とされているとする。この状態から第１のワーク８０を矢印Ａで示される取出方向に取出すと、第２のワーク８２が取出方向の側に位置しているので、比較的大きい力および力のモーメントが検出されたり、比較的大きい力および力のモーメントの振動が検出されたりする。すると、前述した検出部７０によって検出される力および力のモーメント、およびその振動が大きくなり、安定性判定部７２によって、取出動作の安定性が低いと判定される。

このような場合、そのワークに対して、三次元測定機４６によって取得されたワーク８０の位置情報と、前記安定性判定部７２の安定性判定の判定情報と、ワークに対する保持位置、または取出し方向、またはツールの姿勢とを、対応付けて記憶部２４に記憶させておく。そして、別のワークを取出すため、ワーク特定部１４によってワークが特定されたとき、三次元測定機４６によって取得されたそのワークの位置情報の特徴に近い、ワークの位置情報における、前記安定性判定部７２の安定性判定の情報が取得されていたとする。この場合、取出位置姿勢決定部１６は、ワークの保持位置、ワークの取出し方向またはツールの姿勢を変更する。図１０に示される例では、取出位置姿勢決定部１６は、取出し方向を、矢印Ｂで示される矢印Ａとは別の方向、例えば矢印Ａと直交に近い上方向の成分を含む、上方に引出す方向、または、ワーク８０の別の面に直交する方向に変更し、保持位置は、重心位置Ｇから取出し方向に向かった位置Ｐ２とする。または、取出位置姿勢決定部１６は、保持位置を、保持位置Ｐ１から所定距離だけ離れた位置Ｐ２に変更し、取出し方向は、重心位置Ｇから保持位置Ｐ２に向かう方向とする。また、図１０で示される例において、保持位置Ｐ１と矢印Ａで示される取出し方向とに基づいてワーク８０を取出す際、安定性判定部７２によって取出動作の安定性が低いと判定されたとき、その取出し動作を中断して、そのワーク８０に対して、前記のように保持位置、取出し方向またはツールの姿勢を変更してもよい。

図１１は、本実施形態に従って、ワークの保持位置を変更する場合の例を示す図である。図１０の例と比較すると、図１１の例は、取出位置姿勢決定部１６によって保持位置のみが変更され、取出方向やツールの姿勢は変更されない点において相違する。図示される例のように、点Ｐ１の代わりに、第２のワーク８２に対して離間した点Ｐ２を保持位置に設定することによって、矢印Ａで示される取出方向を変更することなく、第１のワーク８０の取出動作がより安定的になるように調整される。

図１０および図１１に関連して述べたように、本実施形態によれば、取出位置姿勢決定部１６は、ワークを取出したときの力データに基づく安定性判定部の判定結果を記憶させておき、その記憶させたデータに基づいて、ワーク取出し時のワークの保持位置、ワークの取出し方向またはワークを取出す際のツールの姿勢を変更することが可能である。また、ワーク取出し時に、取出しが不安定であると判定されたとき、取出し作業を中断し、そのワークに対する、ワーク取出し時のワークの保持位置、ワークの取出し方向またはワークを取出す際のツールの姿勢を変更するようにしてもよい。それにより、三次元測定機４６の検出結果からは分からなかった取り出動作の不安定要素が存在する場合でも、取出動作の態様を適切に変更できるようになり、ワークをより安定した状態で取出すことが可能となる。また、ワークの保持位置および取出方向に加えて、またはそれらに代えて、ツールの位置および姿勢を変更するようにしてもよい。

前述したように、取出位置姿勢決定部１６は、ワークの重心位置に基づいて、ワークの保持位置、ワークの取出方向またはツールの姿勢を決定するように作用するが、ワークの重心位置以外の他の要因を考慮してもよい。例えば、ワークの表面に沿ってワークをスライドさせることで容易に取出せることが分かっている場合は、ワークの表面形状を考慮することができる。さらに、ツールとコンテナとの干渉などを考慮してもよい。また、ワークの材質、ツールの構成などが考慮されてもよい。すなわち、当業者がロボットシステムを使用する上で通常考慮するであろう要因を追加しても本発明を実施できることに留意されたい。

以上、本発明の種々の実施形態および変形例を説明したが、他の実施形態および変形例によっても本発明の意図される作用効果を奏することができることは当業者に自明である。特に、本発明の範囲を逸脱することなく前述した実施形態および変形例の構成要素を削除ないし置換することが可能であるし、公知の手段をさらに付加することが可能である。また、本明細書において明示的または暗示的に開示される複数の実施形態の特徴を任意に組合せることによっても本発明を実施できることは当業者に自明である。

１０ 制御装置
１０’ 制御装置
１１ ロボットシステム
１２ 重心位置算出部
１４ ワーク特定部
１６ 取出位置姿勢決定部
１８ 関連付け部
２０ 関連付け記憶部
２２ 重心位置推定部
２４ 記憶部
４０ ロボット
４２ 力センサ（力計測部）
４４ ツール
４６ 三次元測定機
５０ ワーク
６０ 第１のワーク（ワーク）
６２ 第２のワーク（ワーク）
７０ 検出部
７２ 安定性判定部
８０ 第１のワーク（ワーク）
８２ 第２のワーク（ワーク）
Ｇ 重心位置
Ｐ 保持位置

Claims (8)

1. 三次元空間に置かれたワークを取出して搬送するロボットシステムにおいて、
   前記ワークの表面位置を測定し、前記ワークの表面上の複数の点の、三次元空間における位置情報を取得する三次元測定機と、
   指定される位置および姿勢に従って動作可能なロボットと、
   前記ロボットの先端部に取付けられていて前記ワークを保持可能なツールと、
   前記ワークが前記ツールによって保持されているときに前記ワークが前記ツールに作用する力を計測する力計測部と、
   前記ロボットを制御する制御装置と、を具備しており、
   前記制御装置は、
   前記三次元測定機によって取得される前記複数の点の位置情報に基づいて、取出すワークおよび該ワークの位置および姿勢を特定するワーク特定部と、
   前記ワーク特定部によって特定された前記ワークが取出されるように、前記ワークの保持位置、前記ワークの取出方向、および前記ツールの位置および姿勢を決定する取出位置姿勢決定部と、
   前記ワークを保持したロボットの複数の姿勢において前記力計測部によって計測される力データに基づいて、前記ツールによって保持された状態の前記ワークの重心位置を算出する重心位置算出部と、
   前記ツールによって保持された状態の前記ワークについて、前記三次元測定機によって取得される該ワークの位置情報と、前記重心位置算出部によって算出される該ワークの重心位置と、を関連付ける関連付け部と、
   前記関連付け部による関連付けの結果を記憶する関連付け記憶部と、
   前記関連付け記憶部によって記憶された関連付けの結果に基づいて、前記ワーク特定部によって特定された前記ワークの重心位置を推定する重心位置推定部と、を備えており、
   前記取出位置姿勢決定部は、前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワーク特定部によって特定された前記ワークを取出す際の前記ワークの保持位置、前記ワークの取出方向および前記ツールの位置および姿勢のうちの少なくとも１つを決定するように形成される、ロボットシステム。
2. 前記取出位置姿勢決定部は、
   前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワークの保持位置を決定するように形成されるとともに、
   前記ワークが保持され得る前記ワークの保持位置のうち、前記重心位置推定部によって推定される前記ワークの重心位置から最短距離にある保持位置から所定閾値の範囲内に含まれる保持位置を、前記ワークの実際の保持位置として決定するように形成される、請求項１に記載のロボットシステム。
3. 前記取出位置姿勢決定部は、
   前記重心位置推定部によって推定される前記ワークの重心位置に基づいて、前記ワークの保持位置を決定するように形成されるとともに、
   前記ワークが保持され得る前記ワークの保持位置のうち、前記ワークの重心位置と、前記ワークの取出方向または前記ワークを取出す際の前記ツールの姿勢と、に基づいて、前記ワークの実際の保持位置を決定するように形成される、請求項１に記載のロボットシステム。
4. 前記取出位置姿勢決定部は、
   前記ワークを取出す際の移動方向が、前記重心位置から、前記保持位置または該保持位置から所定の距離だけ離間した位置に向けられた方向に一致するように、前記ワークを取出す際の前記ツールの位置および姿勢を決定するように形成される、請求項１に記載のロボットシステム。
5. 前記ワーク特定部は、
   前記取出位置姿勢決定部によって決定される前記保持位置または該保持位置から所定の距離だけ離間した位置と、前記重心位置との間の距離が、所定の閾値を超える場合に、別のワークを代替的に特定するように形成される、請求項１から４のいずれか１項に記載のロボットシステム。
6. 前記ワーク特定部は、
   前記ワーク特定部によって特定されたワークが別のワークと重なっている場合、
   前記取出位置姿勢決定部によって決定される前記ワークの保持位置または該保持位置から所定の距離だけ離間した位置と、ワークどうしが重なり合う領域において定められる或る位置との間の距離が、所定の閾値以下である場合、別のワークを代わりに特定するように形成される、請求項１から５のいずれか１項に記載のロボットシステム。
7. 当該ロボットシステムは、
   前記ワークを取出す際の力データに基づいて、前記ワークの取出し時に発生する力の大きさおよび振動を検出する検出部と、
   前記検出部による検出結果に基づいて、前記ワークの取出し時の安定性を判定する安定性判定部と、をさらに備えており、
   前記ワーク特定部は、前記安定性判定部による判定結果に基づいて、ワークを特定する際の優先順位を変更するように形成される、請求項１から６のいずれか１項に記載のロボットシステム。
8. 当該ロボットシステムは、
   前記ワークを取出す際の力データに基づいて、前記ワークの取出し時に発生する力の大きさおよび振動を検出する検出部と、
   前記検出部による検出結果に基づいて、前記ワークの取出し時の安定性を判定する安定性判定部と、をさらに備えており、
   前記取出位置姿勢決定部は、前記安定性判定部による判定結果に基づいて、ワークの保持位置、ワークの取出方向またはワークを取出す際の前記ツールの姿勢を変更するように形成される、請求項１から６のいずれか１項に記載のロボットシステム。

Images