JP6725587B2 - バラ積みされたワークを取り出すロボットシステムおよびロボットシステムの制御方法 - Google Patents

Description

本発明は、バラ積みされたワークを取り出すロボットシステムおよびロボットシステムの制御方法に関する。

多数のワークをコンテナなどの容器に収容する場合に、ワークの間隔または向きが不規則に配置されている場合がある。すなわち、容器の内部でワークがバラ積みされている場合がある。従来の技術においては、この容器からワークを取り出すロボットを備えるロボットシステムが知られている（例えば、特開２０１０−６９５４２号公報）。

バラ積みされたワークは、様々な間隔または様々な向きにて収容されている。このために、ワークの位置および姿勢を検出して、ワークを取り出すことが好ましい。従来の技術においては、ワークの位置および姿勢を検出するために、３次元センサを用いることが知られている（例えば、特開２０１０−１２０１４１号公報および特開２０１７−６４８１７号公報）。この方法では、３次元センサにて容器に配置されたワークを撮像する。３次元センサにより取得した画像により、バラ積みされたワークの３次元点群などの情報を生成することができる。そして、ワークの位置および姿勢を検出し、ワークの位置および姿勢に基づいてロボットの位置および姿勢を制御することができる。

ワークを把持するエンドエフェクタとしては、ワークを挟むための複数の爪部を有するハンドを用いることができる。ところが、このハンドは、ワークの決められた部分しか把持することができずに、バラ積みされたワークを効率よく取出すことができない場合がある。例えば、ワークの表側の部分を把持するハンドでは、ワークが裏返っているときに、ワークの裏面の部分を把持することができない場合がある。この結果、全てのワークを取り出せない場合がある。

エンドエフェクタとしては、様々な方向に向いているワークを把持できることが好ましい。すなわち、ワークに対するハンドの相対的な向きを定めずにワークを把持することができるハンドを用いることが好ましい。従来の技術においては、半球状の先端部を有し、電磁石にてワークを吸着する磁石ハンドを用いることが知られている（例えば、特開２０１５−１７１７４９号公報）。

特開２０１０−６９５４２号公報 特開２０１０−１２０１４１号公報 特開２０１７−６４８１７号公報 特開２０１５−１７１７４９号公報

容器の内部にワークがバラ積みされた状態において、ロボットが容器からワークを取り出す動作を実施した時に、ハンドがワークの把持に失敗する場合がある。または、ロボットが所望のワークと異なるワークを取り出す場合がある。このような場合には、サイクルタイムが長くなる。例えば、ハンドがワークを把持していない状態で、ロボットが次の工程までワークを搬送する動作を実施すると、ワークを搬送する動作および元の位置まで戻る動作を行う時間が無駄になる。このために、ハンドにてワークを把持して次の工程にワークを搬送する前に、所望のワークを把持しているか否かを確認することが好ましい。すなわち、ワークが把持されている状態を確認することが好ましい。

ワークが把持されている状態を確認するためには、ロボットのリストとハンドとの間に、質量の変化を検出する装置を配置することができる（例えば、上記の特開２０１５−１７１７４９号公報）。ワークを把持する前のハンドの質量とワークを把持した後のハンドの質量との差を測定することにより、ワークが把持されている状態を確認することができる。または、ハンドなどに光電センサを取り付けることができる。ワークが把持されている場合には、光電センサから出力される光がワークの表面にて反射される。光電センサが反射した光を受信した場合に、ワークが把持されていると判定することができる。ところが、これらの方法では、ワークの把持されている状態を確認するために新たな装置が必要になるという問題がある。また、質量の変化を検出する装置では、可動する部分の摩擦が大きい場合には、質量が小さなワークを検出することができない場合がある。

また、ワークに対するハンドの相対的な向きを定めずにワークを把持することができるハンドを用いた場合には、ハンドに対して様々な向きにてワークが把持される。このために、光電センサではワークの検出が難しいという問題がある。

例えば、ハンドとして磁石ハンドを採用して、ワークを検出する装置として光電センサを採用することができる。ところが、ワークを把持する時のハンドに対するワークの向きは定められていない。このために、ハンドに把持されたワークの向きに依存して、光電センサが出射する光が反射されない場合がある。この場合には、ハンドにてワークが把持されているにも関わらず、ハンドにてワークが把持されていないと判定される。または、光電センサを用いた場合には、１個のワークが把持されているか、又は２個以上のワークが把持されているかを判定することができないという問題がある。

本開示の第１の態様は、容器の内部にバラ積みされたワークを取り出すロボットシステムである。ロボットシステムは、ワークを把持するハンドと、ハンドを移動するロボットと、ワークまでの距離の情報を取得する３次元センサと、ハンドおよびロボットを制御する制御装置とを備える。制御装置は、ハンドおよびロボットに動作指令を送出する動作制御部と、３次元センサの出力に基づいて、ワークに設定された測定点の位置の情報を含む３次元情報を生成する生成部とを含む。制御装置は、ワークの３次元形状データを記憶する記憶部と、ワークの３次元情報を３次元形状データと比較するモデルマッチングを行うことにより、ワークの位置および姿勢を検出する検出部とを含む。制御装置は、ワークの位置および姿勢に基づいてロボットが取り出す目標のワークを選定する選定部を含む。制御装置は、３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除部と、ロボットがワークを持ち上げる動作を実施した後の状態を判定する判定部とを備える。生成部は、ロボットが目標のワークを把持する動作を実施する前にワークを撮像した３次元センサの出力に基づいて第１の３次元情報を生成する。動作制御部は、第１の３次元情報に基づいて選定された目標のワークを把持して持ち上げる動作を実施した後に停止するようにロボットを制御する。生成部は、ロボットが目標のワークを持ち上げる動作を実施した後にワークを撮像した３次元センサの出力に基づいて第２の３次元情報を生成する。削除部は、第１の３次元情報の測定点の位置と第２の３次元情報の測定点の位置とを比較する。削除部は、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出する。削除部は、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成する。検出部は、第３の３次元情報に含まれるワークの位置および姿勢を検出する。判定部は、第３の３次元情報に含まれるワークが目標のワークと同一であるか否かを判定する。動作制御部は、第３の３次元情報に含まれるワークが目標のワークと同一である場合に、ハンドにて把持したワークを予め定められた搬送位置まで搬送するようにロボットを制御する。

本開示の第２の態様は、ロボットおよびハンドを備え、容器の内部にバラ積みされたワークを取り出すロボットシステムの制御方法である。制御方法は、ロボットの位置および姿勢を変更する動作制御工程を含む。制御方法は、ワークまでの距離の情報を取得する３次元センサにてワークを撮像する撮像工程と、３次元センサの出力に基づいて、ワークに設定された測定点の位置の情報を含む３次元情報を生成する生成工程とを含む。制御方法は、ワークの３次元情報をワークの３次元形状データと比較するモデルマッチングを行うことにより、ワークの位置および姿勢を検出する検出工程を含む。制御方法は、ワークの位置および姿勢に基づいてロボットが取り出す目標のワークを選定する選定工程を含む。制御方法は、３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除工程と、ロボットがワークを持ち上げる動作を実施した後の状態を判定する判定工程とを含む。生成工程は、ロボットが目標のワークを把持する動作を実施する前にワークを撮像した３次元センサの出力に基づいて第１の３次元情報を生成する工程を含む。動作制御工程は、第１の３次元情報に基づいて選定された目標のワークを把持して持ち上げる動作を実施した後に停止するようにロボットを制御する工程を含む。生成工程は、ロボットが目標のワークを持ち上げる動作を実施した後にワークを撮像した３次元センサの出力に基づいて第２の３次元情報を生成する工程を含む。削除工程は、第１の３次元情報の測定点の位置と第２の３次元情報の測定点の位置とを比較する工程を含む。削除工程は、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出する工程を含む。削除工程は、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成する工程を含む。検出工程は、第３の３次元情報に含まれるワークの位置および姿勢を検出する工程を含む。判定工程は、第３の３次元情報に含まれるワークが目標のワークと同一であるか否かを判定する工程を含む。動作制御工程は、第３の３次元情報に含まれるワークが目標のワークと同一である場合に、ハンドにて把持したワークを予め定められた搬送位置まで搬送するようにロボットを制御する工程を含む。

本開示の態様のロボットシステムおよびロボットシステムの制御方法によれば、バラ積みされたワークを取り出すときに、ハンドにてワークが把持されている状態を精度よく判定することができる。

実施の形態におけるロボットシステムの斜視図である。 実施の形態におけるロボットシステムのブロック図である。 実施の形態におけるワークが内部に配置されたコンテナの平面図を示す。 実施の形態におけるレンジセンサおよびコンテナの部分断面図である。 実施の形態におけるコンテナおよびワークの部分断面図である。 ワークの表面に設定される測定点の説明図である。 ワークを取り出す前の第１の距離画像である。 ワークを取り出した後の第２の距離画像である。 第１の距離画像から削除する特定の測定点を説明する図である。 第１の距離画像から特定の測定点の情報を削除することにより生成された第３の距離画像である。 第１の比較例のハンド、ワーク、および光電センサの第１の側面図である。 第１の比較例のハンド、ワーク、および光電センサの第２の側面図である。 第２の比較例のハンド、ワーク、および光電センサの第１の側面図である。 第２の比較例のハンド、ワーク、および光電センサの第２の側面図である。 第２の比較例のハンド、ワーク、および光電センサの第３の側面図である。 実施の形態における制御のフローチャートである。

図１から図１６を参照して、実施の形態におけるロボットシステムおよびロボットシステムの制御方法について説明する。本実施の形態のロボットシステムは、容器の内部に山の様に積まれているワークを取り出して、予め定められた搬送位置までワークを搬送する作業を実施する。

図１は、本実施の形態におけるロボットシステムの斜視図である。ロボットシステム５は、ロボット１およびハンド２を備える。ロボットシステム５は、ロボット１およびハンド２を制御する制御装置４を備える。本実施の形態のロボット１は、複数の関節部を含む多関節ロボットである。ロボット１は、上部アーム１１と下部アーム１２とを含む。下部アーム１２は、旋回ベース１３に支持されている。旋回ベース１３は、ベース１４に支持されている。ロボット１は、上部アーム１１の端部に連結されているリスト１５を含む。これらのロボット１の構成部材は、予め定められた回転軸の周りに回転するように形成される。ロボットとしては、この形態に限られず、ハンドの位置および姿勢を変更可能な任意のロボットを採用することができる。

ハンド２は、ワークＷを把持したり解放したりする作業ツールである。ハンド２は、磁力による吸着力を発生させる電磁石２ａと、電磁石２ａの磁力によってワークＷを吸着する吸着部材２ｂとを含む。吸着部材２ｂは、棒状に形成されている。吸着部材２ｂは、先端が球状に形成されている。本実施の形態のハンド２は、ワークＷに対するハンド２の相対的な位置および姿勢を定めずにワークＷを把持することができる。

ワークＷは、容器としてのコンテナ９の内部に配置されている。容器としては、箱、袋、または籠などのワークＷを収容できる任意の部材を採用することができる。コンテナ９の内部には、ワークＷがバラ積みされている。コンテナ９は、架台８１に載置されている。本実施の形態のロボットシステム５は、コンテナ９からワークＷを１個ずつ取り出して、予め定められた搬送位置まで搬送する。

ロボットシステム５は、ワークＷの位置および姿勢を検出するための３次元センサとしてのレンジセンサ６を備える。レンジセンサ６は、ワークＷまでの距離の情報を取得する。レンジセンサ６は、ワークＷを撮像できる位置に配置される。本実施の形態では、レンジセンサ６は、コンテナ９の上方に配置されている。レンジセンサ６は、支持部材８３により固定されている。レンジセンサ６は、制御装置４により制御されている。

本実施の形態のレンジセンサ６は、２台のカメラ６１，６２を含むステレオカメラである。カメラ６１，６２は、２次元の画像を撮像することができる２次元カメラである。カメラ６１，６２としては、ＣＣＤ（Charge-Coupled Device）センサまたはＣＭＯＳ（Complementary Metal-Oxide Semiconductor）センサなどの撮像素子を備えた任意のカメラを採用することができる。２台のカメラ６１，６２の相対的な位置は予め定められている。本実施の形態のレンジセンサ６は、ワークＷに向かって縞模様などのパターン光を投影するプロジェクタ６３を含む。

本実施の形態のロボットシステム５には、ロボット１の位置および姿勢が変化した時に不動の基準座標系３７が設定されている。図１の例では、ロボット１のベース１４に、基準座標系３７の原点が配置される。基準座標系３７はワールド座標系とも称される。基準座標系３７には、座標軸として、互いに直行するＸ軸、Ｙ軸、およびＺ軸が設定される。例えば、基準座標系３７の座標値（ｘb，ｙb，ｚb）を設定することができる。また、Ｘ軸の周りの座標軸としてＷ軸、Ｙ軸の周りの座標軸としてＰ軸、および、Ｚ軸の周りの座標軸としてＲ軸が設定されても構わない。この場合に、基準座標系３７の座標値（ｘb，ｙb，ｚb，ｗb，ｐb，ｒb）を設定することができる。

図２に、本実施の形態におけるロボットシステムのブロック図を示す。図１および図２を参照して、ロボット１は、ロボット１の位置および姿勢を変化させるロボット駆動装置を含む。ロボット駆動装置は、アームおよびリスト等の構成部材を駆動するロボット駆動モータ２２を含む。ロボット駆動モータ２２が駆動することにより、それぞれの構成部材の向きが変化する。

ロボットシステム５は、ハンド２を駆動するハンド駆動装置を備える。本実施の形態のワークＷは、鉄などの磁性体にて形成されている。ハンド２の電磁石２ａが駆動することによりワークＷが吸着部材２ｂに吸着される。

制御装置４は、ＣＰＵ（Central Processing Unit）と、ＣＰＵにバスを介して接続されたＲＡＭ（Random Access Memory）およびＲＯＭ（Read Only Memory）等を有する演算処理装置（コンピュータ）を含む。制御装置４には、ロボット１の動作を行うために予め作成された動作プログラム４１が入力される。動作プログラム４１は、記憶部４２に記憶される。本実施の形態のロボットシステム５は、動作プログラム４１に基づいてワークＷを搬送する。ロボット１は、自動的に予め定められた搬送位置までワークＷを搬送することができる。

動作制御部４３は、動作プログラム４１に基づいてロボット１を駆動するための動作指令をロボット駆動部４５に送出する。ロボット駆動部４５は、ロボット駆動モータ２２を駆動する電気回路を含む。ロボット駆動部４５は、動作指令に基づいてロボット駆動モータ２２に電気を供給する。また、動作制御部４３は、動作プログラム４１に基づいてハンド２を駆動する動作指令をハンド駆動部４４に送出する。ハンド駆動部４４は、電磁石２ａを駆動する電気回路を含む。ハンド駆動部４４は、動作指令に基づいて電磁石２ａに電気を供給する。更に、動作制御部４３は、動作プログラム４１に基づいて、レンジセンサ６を駆動する動作指令をレンジセンサ６に送出する。

ロボット１は、ロボット１の位置および姿勢を検出するための状態検出器を含む。本実施の形態における状態検出器は、アーム等の構成部材の駆動軸に対応するロボット駆動モータ２２に取り付けられた位置検出器１８を含む。位置検出器１８の出力により、それぞれの駆動軸における構成部材の向きを取得することができる。例えば、位置検出器１８は、ロボット駆動モータ２２が駆動するときの回転角を検出する。また、位置検出器１８の出力に基づいて、ハンド２の位置および姿勢が検出される。

制御装置４は、動作プログラム４１に基づいてロボット１の動作を設定する動作設定部５２を含む。本実施の形態の動作設定部５２は、コンテナ９から取り出すワークＷを選定し、ワークＷをハンド２にて把持する制御を実施する。また、動作設定部５２は、ハンド２にて把持されたワークＷの状態を検出して、ロボット１の動作を制御する。

図３に、ワークが内部に配置されたコンテナの平面図を示す。図４に、本実施の形態におけるレンジセンサとワークとの関係を説明する部分断面図を示す。図４では、説明のためにレンジセンサ６とコンテナ９との距離が近くなるように記載している。コンテナ９の内部には、複数のワークＷが配置されている。本実施の形態のワークＷは、円筒状に形成されている。本実施の形態においては、コンテナ９の少なくとも１つの方向において、ワークＷ同士の間隔またはワークＷの姿勢が同一でない状態をバラ積みされた状態と称する。本実施の形態では、複数のワークＷは、ワークＷの軸線の向きが互いに異なるようにコンテナ９に配置されている。すなわち、複数のワークＷの向きが不規則になるようにワークＷが積まれている。

レンジセンサ６は、撮像が可能な範囲である撮像範囲６４を有する。カメラ６１，６２は、撮像範囲６４の内部にコンテナ９が収容されように配置されることが好ましい。すなわち、カメラ６１，６２は、コンテナ９の上方から見える全てのワークＷを撮像できるように配置されることが好ましい。

図５に、ワークを取り出すときのハンドおよびコンテナの部分断面図を示す。ハンド２の電磁石２ａが駆動することにより、吸着部材２ｂにワークＷが吸着する。そして、ハンド２が移動することにより、矢印９１に示すようにワークＷを持ち上げることができる。本実施の形態においては、ワークＷは不規則な向きにてハンド２に把持される。

図２および図４を参照して、制御装置４は、レンジセンサ６の出力に基づいて、ワークＷの３次元情報を生成する生成部５８を含む。生成部５８は、カメラ６１，６２にて撮像した画像を処理する。生成部５８は、ステレオ法によりワークＷの３次元情報を生成することができる。３次元情報には、ワークＷの表面に設定された測定点の位置の情報が含まれる。

図６に、ワークの表面上に設定される測定点の説明図を示す。生成部５８は、任意の方法にて、レンジセンサ６から見えるワークＷの表面に複数の測定点ＭＰを設定することができる。例えば、図４を参照して、生成部５８は、プロジェクタ６３の光軸６３ａに垂直な平面６５を設定することができる。生成部５８は、平面６５に格子状の線を設定することができる。例えば、生成部５８は、互いに直交するように、Ｘ軸方向に延びる直線およびＹ軸方向に延びる直線を設定することができる。生成部５８は、これらの複数の直線の交点とプロジェクタ６３のレンズ中心点とを結ぶ直線６６を設定する。そして、生成部５８は、直線６６の方向において、ワークＷの表面に交わる点を測定点ＭＰに設定することができる。

生成部５８は、２台のカメラ６１，６２にて撮像される２つの画像の視差に基づいて、レンジセンサ６から測定点ＭＰまでの距離を算出する。生成部５８は、測定点ＭＰまでの距離および直線６６の向きに基づいて、測定点ＭＰの位置を検出することができる。例えば、生成部５８は、基準座標系３７における測定点ＭＰの座標値を算出して、３次元情報を生成することができる。

３次元情報は、複数の測定点ＭＰの位置の情報を含む。本実施の形態の３次元情報は、基準座標系３７における測定点ＭＰの座標値を含む。または、３次元情報は、レンジセンサ６から測定点ＭＰまでの距離およびレンジセンサ６から測定点ＭＰに向かう方向の情報を含んでいても構わない。３次元情報は、例えば、距離画像または３次元マップである。距離画像は、レンジセンサ６からの距離に応じて画像の色または画像の濃さを変化させた画像である。３次元マップは、予め定められた座標系における測定点の座標値、またはレンジセンサからの測定点までの距離および測定点の方向の情報を含む。なお、本実施の形態の生成部５８は、ロボット１を制御する制御装置４に含まれているが、この形態に限られない。カメラ６１，６２にて取得した画像を処理する演算処理装置が制御装置４とは別に配置されていても構わない。

図２を参照して、検出部５３は、ワークＷの３次元情報を３次元形状データと比較するモデルマッチングを行うことにより、ワークＷの位置および姿勢を検出する。本実施の形態においては、予め作成されたワークＷの３次元形状データ４６が記憶部４２に記憶されている。３次元形状データ４６としては、例えば、ＣＡＤ（Computer Aided Design）装置にて生成される形状の３次元データを用いることができる。生成部５８は、ＣＡＤ装置により生成された３次元形状データ４６に基づいて、ワークＷの向きを検出することができる。

または、作業者は、予め定められた方向から撮像したときのワークの距離画像を３次元形状データ４６として採用することができる。作業者は、形状が大きく異なって見える複数の方向から撮像したワークの距離画像を３次元形状データ４６として採用することができる。例えば、本実施の形態のワークＷは、円筒の形状を有する。このため、ワークＷの軸線に垂直な方向から撮像した距離画像と、ワークＷの軸線の方向から見たときの距離画像とを、３次元形状データ４６に採用することができる。ワークＷを見る方向が少し異なっていても、検出部５３は、これらの３次元形状データ４６に基づいてワークＷの位置および姿勢を検出することができる。

なお、複数の種類のワークが容器に配置されている場合に、作業者は、それぞれのワークの種類ごとに３次元形状データ４６を作成する。記憶部４２は、複数の種類の３次元形状データ４６を記憶することができる。動作設定部５２は、それぞれの種類のワークごとに判定を行うことができる。

動作設定部５２は、ロボット１にて取り出すための目標のワークＷを選定する選定部５９を含む。選定部５９は、検出部５３にて検出されたワークＷの位置および姿勢に基づいて目標のワークＷを選定する。選定部５９は、任意の制御により目標のワークＷを選定することができる。例えば、レンジセンサ６から最も近いワークＷを目標のワークＷに設定することができる。すなわち、ワークＷの位置が高いものから順番にワークＷを選定することができる。または、所定の条件においては、ワークＷの位置が低いものを選定しても構わない。例えば、選定部５９は、ハンド２にて把持可能な部分が大きく露出しており、容易に把持することができるワークＷを選定しても構わない。

また、選定部５９は、ハンド２によりワークＷの把持が可能か否かを判定しても構わない。例えば、ワークＷがコンテナ９の壁面の近くに存在する場合には、ハンド２の吸着部材２ｂをワークＷに接触させることができない場合がある。このような場合には、選定部５９は、ワークＷの把持が不可能であると判定しても構わない。

そして、選定部５９は、目標のワークＷの位置および姿勢に応じて、ロボット１がワークＷを把持できる目標位置および目標姿勢を算出する。選定部５９は、動作制御部４３にロボット１の目標位置および目標姿勢を送信する。ロボット１は、目標位置および目標姿勢に向かって位置および姿勢を変更する。ロボット１が目標位置および目標姿勢に到達した後に、ハンド２の吸着部材２ｂが励磁されることにより、ワークＷを把持することができる。次に、動作制御部４３は、ロボット１の位置および姿勢を変更してワークＷを持ち上げる動作を実施する。

動作設定部５２は、ロボット１がワークＷを持ち上げた後の状態を判定する判定部５５を含む。判定部５５は、所望の状態にてワークＷを把持しているか否かを判定する。判定部５５は、ロボット１が所望の状態にてワークＷを把持している場合には、搬送位置までワークを搬送する動作指令を動作制御部４３に送信する。動作制御部４３は、搬送位置までワークを搬送するようにロボット１を制御する。

一方で、判定部５５は、ロボット１が所望の状態にてワークＷを把持していない場合には、ハンド２にて把持したワークＷをハンド２にて把持する前に配置されていた元の位置の近傍に戻す動作指令を動作制御部４３に送信する。または、判定部５５は、ハンド２がワークＷを把持していないと判定した場合に、ワークの撮像から制御を実施する動作指令を送出する。

次に、判定部５５にてワークＷが把持されている状態を判定し、ワークＷが把持されている状態に応じて、ロボット１の動作を変化させる制御について詳しく説明する。

図７に、生成部にて生成された第１の３次元情報の例を示す。図７は、ロボット１が駆動する前にレンジセンサ６にて撮像した第１の距離画像３１である。ここでの例では、レンジセンサ６からの距離に応じて濃さが変化する距離画像が示されている。この距離画像では、レンジセンサ６からの距離が近いほど部品の濃さが薄くなる。第１の距離画像３１には、複数のワークの画像７２ａ，７２ｂ，７２ｃと、コンテナの画像７１が含まれている。

本実施の形態では、ロボット１が目標のワークＷを把持する動作を実施する前に、レンジセンサ６にてワークＷを撮像する。生成部５８は、レンジセンサ６の出力に基づいて、第１の距離画像３１を生成する。第１の距離画像３１は、ワークＷを撮像したレンジセンサ６の出力に基づいて生成された第１の３次元情報に相当する。本実施の形態の３次元情報には、測定点ＭＰの座標値の情報が含まれている。

検出部５３は、第１の距離画像３１に基づいて、それぞれのワークＷの位置および姿勢を検出する。選定部５９は、ワークＷの位置および姿勢に基づいて、ロボット１にて取り出すワークＷを選定する。動作制御部４３は、ロボット１が目標のワークＷを把持するように制御する。動作制御部４３は、目標のワークＷを持ち上げる動作を実施した後に停止するようにロボット１を制御する。この時に、ロボット１は、目標のワークＷをコンテナ９から取り出して、レンジセンサ６の撮像範囲６４の外側に待避する。すなわち、レンジセンサ６が撮像する距離画像に、ロボット１、ハンド２、およびハンド２に把持されたワークＷが写らないように、ロボット１の位置および姿勢を変更する。ロボット１は、ワークＷを目標の搬送位置まで搬送せずに、コンテナ９の近傍にワークＷを配置する。

ここでの例では、複数のワークの画像７２ａ，７２ｂ，７２ｃのうち、画像７２ａに対応するワークが目標のワークＷであるとする。また、ロボット１は、目標のワークＷのみを持ち上げている。すなわち、ロボット１は、目標のワーク以外のワークＷは持ち上げていない。

次に、レンジセンサ６は、ロボット１がワークＷを持ち上げる動作を実施した後に、コンテナ９に配置されたワークＷを撮像する。生成部５８は、レンジセンサ６の出力に基づいて、第２の３次元情報としての第２の距離画像を生成する。

図８に、目標のワークを持ち上げる動作を実施した後の距離画像の例を示す。第２の距離画像３２では、図７に示す画像７２ａに対応するワークＷがコンテナ９から取り出されることにより、画像７２ａが消失している。

図２を参照して、本実施の形態における動作設定部５２は、３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除部５４を含む。削除部５４は、第１の３次元情報の測定点ＭＰの位置と第２の３次元情報の測定点ＭＰの位置とを比較する。削除部５４は、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出する。削除部５４は、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成する。

図９に、ワークを把持する前に生成した第１の３次元情報の測定点と、ワークを持ち上げる動作を行った後に生成した第２の３次元情報の測定点とを説明する概略図を示す。図９に示す例では、目標のワーク以外のワークＷが示されている。特に、目標のワークＷを持ち上げる前の位置と目標のワークＷを持ち上げた後の位置が変わらないワークＷが示されている。

ロボット１が目標のワークＷを持ち上げる動作を実施したときに、ワークＷが動かなければ、第１の３次元情報に含まれる測定点ＭＰａの位置と、第２の３次元情報に含まれる測定点ＭＰｂの位置とは同一になる。しかしながら、測定誤差などにより、測定点ＭＰａの位置と測定点ＭＰｂの位置とが僅かに異なる場合がある。または、ロボット１が目標のワークＷを持ち上げる動作を実施したときに、他のワークＷが僅かに移動したとしても、次の工程におけるワークＷの取出しに影響を与えない場合がある。

本実施の形態における削除部５４は、第１の３次元情報に含まれる測定点ＭＰａの近傍に、第２の３次元情報に含まれる測定点ＭＰｂが存在する場合に、その測定点ＭＰａを特定の測定点に設定する。そして、削除部５４は、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成する。

第１の３次元情報に含まれる測定点ＭＰａを削除するための判定範囲は、予め定められている。本実施の形態においては、基準座標系におけるＸ軸方向、Ｙ軸方向、およびＺ軸方向の判定範囲が定められている。図９に示す例では、Ｘ軸方向およびＹ軸方向について判定範囲６７が示されている。第１の３次元情報に含まれる測定点ＭＰａに対して、判定範囲６７の内部に、第２の３次元情報に含まれる測定点ＭＰｂが存在する。このために、削除部５４は、測定点ＭＰａを特定の測定点として検出する。削除部５４は、特定の測定点を第１の３次元情報から削除する。例えば、第１の距離画像３１に含まれている測定点ＭＰａの座標値を削除する。このように、削除部５４は、特定の測定点を第１の３次元情報から削除した第３の３次元情報を生成する。

図９に示す例では、全ての第１の３次元情報の測定点ＭＰａの近傍に、第２の３次元情報の測定点ＭＰｂが存在している。このために、削除部５４は、全ての測定点ＭＰａを特定の測定点に指定して削除している。

図１０に、削除部にて生成された第３の距離画像の例を示す。図７、図８、および図１０を参照して、第３の３次元情報としての第３の距離画像３３には、位置および姿勢が変化しない複数のワークの画像７２ｂ，７２ｃが第１の距離画像３１から削除されている。第３の距離画像３３には、ロボット１にてコンテナ９から取り出されたワークの画像７２ａが残存する。

検出部５３は、第３の距離画像３３と３次元形状データ４６とを比較するモデルマッチングを行うことにより、画像７２ａに対応するワークＷの位置および姿勢を検出する。

次に、判定部５５は、検出部５３にて検出されたワークＷが目標のワークＷと同一であるか否かを判定する。判定部５５は、選定部５９から目標のワークＷの位置および姿勢を取得する。判定部５５は、第３の距離画像３３に含まれるワークＷの位置および姿勢と、目標のワークＷの位置および姿勢との差が、予め定められた判定範囲内であれば、第３の距離画像３３に含まれるワークは、目標のワークと同一であると判定する。なお、検出部５３にて複数のワークが検出され、目標のワークと異なるワークが含まれている場合に、判定部５５は、第３の３次元情報に含まれるワークは目標のワークと同一でないと判定する。

図１０に示す例では、判定部５５は、目標のワークＷが取り出されたと判定する。次に、判定部５５は、ハンド２にて把持したワークＷを予め定められた搬送位置まで搬送する指令を動作制御部４３に送出する。動作制御部４３は、ハンド２にて把持したワークＷを搬送位置まで搬送するようにロボット１を制御する。

このように、本実施の形態のロボットシステム５では、ワークを持ち上げる動作を実施する前の第１の３次元情報と、ワークを持ち上げる動作を実施した後の第２の３次元情報とを比較することにより、ハンド２が目標のワークを把持しているか否かを判定することができる。

ロボット１が目標のワークＷを持ち上げる動作を実施した時に、ハンド２が目標のワークＷと異なる１個のワークＷを把持している場合には、第３の３次元情報には、目標のワークＷとは異なるワークの画像が残存する。検出部５３は、目標のワークとは異なるワークの位置および姿勢を検出する。判定部５５は、検出部５３にて検出されたワークは目標のワークと異なると判定する。または、ハンド２が目標のワークに加えて、目標のワーク以外のワークを把持している場合がある。すなわち、ハンド２が２個以上のワークＷを把持している場合がある。この場合に、第３の３次元情報には、複数のワークＷの情報が含まれる。検出部５３は、複数のワークＷの位置および姿勢を検出する。判定部５５は、検出部５３にて検出された複数のワークＷは、目標のワークと同一でないと判定する。

ハンド２が目標のワークとは異なるワークを把持している場合または２個以上のワークを把持している場合に、判定部５５は、ハンド２にて把持したワークＷを元の位置の近傍に戻す動作指令を動作制御部４３に送出する。動作制御部４３は、ハンド２にて把持したワークをハンド２にて把持する前に配置されていた元の位置の近傍に戻すようにロボット１を制御する。すなわち、動作制御部４３は、第１の３次元情報における目標のワークの位置の近傍にワークＷを戻すように制御する。

ロボット１がワークＷを元の位置の近傍に戻した後には、ワークＷを搬送する制御をワークＷの撮像から実施する。すなわち、レンジセンサ６はコンテナ９に配置されているワークＷを撮像し、検出部５３はワークＷの位置および姿勢を検出し、選定部５９は、ロボット１が取り出す次の目標のワークＷを選定する。

ワークＷをコンテナ９に戻す場合に、ハンド２にて把持したワークＷを元の位置とは異なる位置に戻す制御を実施することができる。しかしながら、この制御では、コンテナ９の内部において荷崩れが生じる虞がある。ハンド２にて把持したワークＷを元の位置の近傍に戻す制御を実施することにより、ワークＷを戻したときに荷崩れが生じることを抑制することができる。なお、ハンド２にて把持したワークＷを元の位置と同一の位置に戻す制御を実施しても構わない。

また、検出部５３は、第３の３次元情報において、ワークＷの位置および姿勢を検出できない場合がある。すなわち、図１０に示す第３の距離画像３３において、第３の３次元情報と３次元形状データ４６とを比較するモデルマッチングを行っても、ワークＷの位置および姿勢を検出できない場合がある。例えば、ハンド２がワークＷの把持に失敗して、ワークＷを把持していない場合がある。更に、ロボット１が目標のワークＷを持ち上げる動作を実施した時に、コンテナ９の内部のワークＷに荷崩れが生じずに、全てのワークＷの位置および姿勢が変化していない場合がある。または、ワークＷの位置の移動量が非常に小さく、削除部５４によって測定点の情報が削除されている場合がある。この場合には、第３の距離画像３３には、１つもワークの画像が含まれていないために、検出部５３はワークを検出することができない。

判定部５５は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数を検出することができる。判定部５５は、特定の測定点以外の測定点の個数が予め定められた把持の失敗に関する判定値よりも小さい場合に、ハンド２がワークＷを把持していないと判定することができる。すなわち、削除部５４にて殆ど全ての測定点が削除されている場合に、判定部５５は、コンテナ９に配置された全てのワークＷの位置および姿勢が変化していないと判定することができる。また、判定部５５は、ワークＷを取り出す動作を実施した時に、ワークＷの荷崩れが生じていないと判定することができる。ワークの把持の失敗に関する判定値は、小さな値を採用することができる。例えば、１つのワークに設定される測定点の個数の下限値よりも小さな値を採用することができる。

判定部５５がハンド２にてワークＷが把持されていないと判定した後に、制御装置４は、ワークＷを搬送する制御をワークＷの撮像から実施する。すなわち、レンジセンサ６はコンテナ９に配置されているワークＷを撮像し、検出部５３はワークＷの位置および姿勢を検出し、選定部５９は、ロボット１が取り出す次の目標のワークＷを選定する。

また、ロボット１がワークＷを取り出す動作を実施したときに、荷崩れが生じる場合がある。すなわち、ロボット１が取り出さなかったワークＷの位置および姿勢が変化する場合がある。荷崩れが生じた場合に、第２の３次元情報には、第１の３次元情報に含まれる測定点とは離れた測定点が多く存在する。第１の３次元情報の測定点について、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する特定の測定点の個数は少なくなる。すなわち、第１の３次元情報の測定点に含まれる特定の測定点の個数は少なくなる。

判定部５５は、荷崩れの発生を判定するために、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数を検出する。判定部５５は、特定の測定点以外の測定点の個数が予め定められた荷崩れに関する判定値よりも大きいか否かを判定する。判定部５５は、特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きい場合に、荷崩れが生じていると判定することができる。荷崩れに関する判定値は、例えば、１つのワークに設定される測定点の個数の上限値よりも大きな値を採用することができる。

または、判定部５５は、荷崩れの発生を判定するために、第２の３次元情報の測定点について、第１の３次元情報の測定点から予め定められた距離の範囲内に存在しない測定点の個数を算出しても構わない。この測定点の個数が荷崩れに関する判定値よりも大きければ、判定部５５は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きいと判定しても構わない。

判定部５５は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が予め定められた荷崩れに関する判定値よりも大きいと判定した場合に、ハンド２にて把持したワークＷを元の位置の近傍に戻す指令を動作制御部４３に送出する。動作制御部４３は、ハンド２にて把持したワークＷをハンド２にて把持する前に配置されていた元の位置の近傍に戻すようにロボット１を制御する。

ロボット１がワークＷを元の位置の近傍に戻した後に、制御装置４は、ワークＷを取り出す制御をワークＷの撮像から実施する。すなわち、レンジセンサ６はコンテナ９に配置されているワークＷを撮像し、検出部５３はワークＷの位置および姿勢を検出し、選定部５９は、ロボット１が取り出す次の目標のワークＷを選定する。

本実施の形態におけるロボットシステム５は、ロボット１がワークＷを持ち上げた動作を実施した時に、ワークＷが把持されている状態を判別する。そして、ワークＷの把持の状態に応じてロボット１を制御することができる。このために、ロボットシステム５は、ワークＷを搬送するサイクルタイムを短くすることができる。例えば、ワークＷが把持されていないにも関わらず、ロボット１が目標の搬送位置までワークＷを搬送する動作を回避することができる。または、ハンド２が目標のワーク以外のワークＷを把持していたり、目標のワークＷに加えて他のワークＷを把持していたりする場合に、ワークＷを目標の搬送位置まで搬送することを回避できる。例えば、目標のワーク以外のワークまたは２個以上のワークを搬送した後に、ロボット１がワークを元の位置に戻す動作を排除することができる。

図１１に、第１の比較例のロボットシステムのハンドの部分の第１の側面図を示す。第１の比較例においては、ハンド２がワークＷＣを把持しているか否かを判定するために、光電センサ８５が配置されている。光電センサ８５は、ハンド２に固定されている。光電センサ８５は、矢印９５に示すように光を発する。光電センサ８５がワークＷＣの表面にて反射する光を検出することにより、ハンド２はワークＷＣを把持していると判定することができる。光電センサ８５が反射光を検出しない場合には、ハンド２はワークＷＣを把持していないと判定することができる。

図１２に、第１の比較例のロボットシステムのハンドの部分の第２の側面図を示す。比較例におけるハンド２は、電磁石ハンドである。このために、ワークＷＣが把持される姿勢は定まっていない。図１２に示すように、ワークＷＣが傾いた状態で把持された場合には、光電センサ８５は、反射光を受光することができない。このために、ワークＷＣが把持されているにも関わらず、ワークＷＣが把持されていないと判定される場合がある。第１の比較例においては、光電センサ８５がハンド２に固定されているために、光電センサ８５の向きを定めることが難しいという問題がある。または、ワークＷＣが把持される全ての向きに対して、ワークＷＣを検出できる光電センサ８５の向きが存在しない場合がある。

図１３に、第２の比較例のロボットシステムのハンドの部分の第１の側面図を示す。第２の比較例においては、ハンド２がワークＷＣを把持しているか否かを判定するために、光電センサ８６が配置されている。光電センサ８６は、発光素子８６ａと受光素子８６ｂとを含む。発光素子８６ａおよび受光素子８６ｂは、例えば支持部材などに固定することができる。発光素子８６ａと受光素子８６ｂとの間にワークＷＣが配置されるように、ロボット１の位置および姿勢を制御することができる。発光素子８６ａは、矢印９６に示すように光を出射する。受光素子８６ｂにて受光されない場合に、ハンド２がワークＷＣを把持していると判定することができる。

図１４に、第２の比較例のロボットシステムのハンドの部分の第２の側面図を示す。図１４に示されているように、ワークＷＣの向きが所望の向きと異なる向きにてハンド２に把持される場合には、発光素子８６ａから出射される光は受光素子８６ｂに到達する場合がある。このために、ワークＷＣが把持されているにも関わらず、ワークＷＣが把持されていないと判定される場合がある。

図１５に、第２の比較例のロボットシステムのハンドの部分の第３の側面図を示す。図１５に示すように、ハンド２は、２個以上のワークＷＣを把持する場合がある。この場合に、発光素子８６ａから出射される光は、ワークＷＣにて遮ることができる。しかしながら、この制御では、複数のワークＷＣが把持された状態を検出することはできない。第１の比較例においても同様に、複数のワークＷＣが把持された状態を検出することができない。

このように、光電センサを用いてワークが把持されている状態を判定する方法では、ハンドにてワークが把持されている状態を正確に判定できない場合がある。これに対して、本実施の形態におけるロボットシステム５では、ハンド２に把持されたワークＷの状態を正確に判定することができる。ハンド２にて把持されるワークＷの向きに関わらず、把持したワークＷを検出することができる。または、２個以上のワークＷを把持した状態を検出することができる。この結果、過剰なロボット１の動作を回避することができる。

なお、第２の比較例においては、ワークが把持される複数の状態を考慮して、ハンドを複数の位置および姿勢に移動してワークの検出を行うことができる。しかしながら、この制御では、サイクルタイムが長くなる問題がある。本実施の形態におけるロボットシステム５は、第１の３次元情報と第２の３次元情報とを比較することにより、ワークが把持されている状態を検出することができるために、短時間にてワークが把持されている状態を検出することができる。

更に、本実施の形態のロボットシステム５では、光電センサ等のワークの把持の状態を検出するための装置は不要である。ロボットシステム５は、ワークの位置および姿勢を検出するための３次元センサの出力を用いてワークが把持されている状態を検出することができる。このように、本実施の形態のロボットシステム５は、簡易な構成にて精度よくワークＷが把持されている状態を確認することができる。

図１６に、本実施の形態のロボットシステムの制御方法のフローチャートを示す。図１６に示す制御は、繰り返して実施することができる。

図２および図１６を参照して、ステップ１００において、動作制御部４３は、レンジセンサ６にてワークＷを撮像する第１の撮像工程を実施する。生成部５８は、レンジセンサ６の出力に基づいてワークＷに測定点を設定し、測定点の座標値を含む第１の３次元情報を生成する第１の生成工程を実施する。生成部５８は、ロボット１が目標のワークＷを把持する動作を実施する前にワークＷを撮像したレンジセンサ６の出力に基づいて第１の３次元情報を生成する。

ステップ１０１において、検出部５３は、ワークの第１の３次元情報を予め定められたワークＷの３次元形状データ４６と比較するモデルマッチングを行うことにより、ワークＷの位置および姿勢を検出する第１の検出工程を実施する。

ステップ１０２において、選定部５９は、検出部５３により検出されたワークＷの位置および姿勢に基づいて、ロボット１が取り出す目標のワークを選定する選定工程を実施する。

ステップ１０３において、動作制御部４３は、ロボット１の位置および姿勢を変更する動作制御工程を実施する。ここでの動作制御工程では、ロボット１は、目標のワークＷを把持して持ち上げる動作を実施する。その後に、ロボット１は停止する。

ステップ１０４において、レンジセンサ６にてワークＷを撮像する第２の撮像工程を実施する。生成部５８は、ロボット１が目標のワークＷを持ち上げる動作を実施した後にワークＷを撮像したレンジセンサ６の出力に基づいて第２の３次元情報を生成する第２の生成工程を実施する。

ステップ１０５において、削除部５４は、第１の３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除工程を実施する。削除部５４は、第１の３次元情報の測定点の位置と第２の３次元情報の測定点の位置とを比較する工程を実施する。削除部５４は、第１の３次元情報の測定点に対して予め定められた距離の範囲内に第２の３次元情報の測定点が存在するか否かを判定する。削除部５４は、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出する工程を実施する。削除部５４は、第１の３次元情報から特定の測定点の座標値などの情報を削除した第３の３次元情報を生成する工程を実施する。

ステップ１０６において、検出部５３は、削除工程にて処理が行われた後の第３の３次元情報に含まれるワークの位置および姿勢を検出する第２の検出工程を実施する。検出部５３は、ワークの第３の３次元情報を予め定められたワークの３次元形状データ４６と比較するモデルマッチングを行う。検出部５３は、第３の３次元情報に含まれる全てのワークＷの位置および姿勢を検出する。

ステップ１０８において、判定部５５は、ロボットにてワークＷを持ち上げる動作を実施した後の状態を判定する判定工程を実施する。判定工程では、第２の検出工程にて検出されたワークＷが目標のワークＷと同一であるか否かを判定する。ここでは、判定部５５は、目標のワークＷのみが取り出された否かを判定する。ステップ１０８において、取り出されたワークＷが目標のワークＷと同一でない場合には、制御はステップ１２１に移行する。例えば、ロボット１にて持ち上げられたワークＷが目標のワークＷと異なる場合、または、２個以上のワークＷが持ち上げられている場合に、制御はステップ１２１に移行する。

ステップ１０８において、目標のワークＷのみが取り出されたと判定された場合に、制御はステップ１０９に移行する。ステップ１０９においては、動作制御工程を実施する。ここでの動作制御工程では、動作制御部４３は、ハンド２にて把持したワークＷを予め定められた搬送位置まで搬送するようにロボット１を制御する。

次に、ステップ１１０において、判定部５５は、予め定められた個数のワークが取り出されたか否かを判定する。すなわち、取り出されたワークＷの個数が、個数の判定値に到達しているか否かを判定する。このときの個数の判定値は作業者が予め設定することができる。例えば、作業者は、判定値として５個等の少量の個数を設定することができる。または、作業者は、判定値としてコンテナ９に収容されている全てのワークＷの個数を設定しても構わない。

ステップ１１０において、予め定められた個数のワークＷが取り出されている場合には、この制御を終了する。ステップ１１０において、予め定められた個数のワークＷを取り出していない場合に、制御はステップ１１１に移行する。ステップ１１１において、生成部５８は、現在の第２の３次元情報を第１の３次元情報に設定する。すなわち、ワークＷを取り出した後の３次元情報を第１の３次元情報として設定する。この制御においては、ステップ１０８において目標のワークのみが検出されている。コンテナ９に残存するワークＷに荷崩れが生じていないと判定することができる。このために、目標のワークＷを取り出した後に生成した第２の３次元情報を、第１の３次元情報として使用することができる。

次に、制御はステップ１０１に移行し、ワークＷを取り出す制御を実施する。ステップ１０１では、検出部５３は第１の３次元情報に基づいてワークＷの位置および姿勢を検出する。ステップ１０２では、選定部５９は、次の目標のワークＷを選定する。この制御では、ステップ１００が除外される。３次元カメラにてワークを撮像して、３次元情報を取得する工程を排除することができる。このために、サイクルタイムを短縮することができる。

ステップ１０８において、ロボット１にて取り出されたワークＷが目標のワークＷと同一でない場合には、制御はステップ１２１に移行する。ステップ１２１において、判定部５５は、１個のワークが検出されたか否かを判定する判定工程を実施する。判定部５５は、ハンド２が目標のワークとは異なる１個のワークを把持しているか否かを判定する。また、判定部５５は、第３の３次元情報に含まれるワークが２個か否かを判定する。すなわち、ハンド２が２個のワークを把持しているか否かを判定する。ステップ１２１において、１個または２個のワークが検出された場合に、制御はステップ１２４に移行する。

なお、ステップ１２１においては、２個のワークが検出されたか否かを判定する代わりに、２個以上のワークが検出されたか否かを判定しても構わない。

ステップ１２４においては、判定部５５は、ハンド２にて把持したワークＷをハンド２にて把持する前に配置されていた元の位置の近傍に戻す指令を動作制御部４３に送出する。動作制御部４３は、ワークＷを元の位置の近傍に戻すようにロボット１を制御する。この後に、制御はステップ１００に移行する。ロボット１がワークＷを元の位置の近傍に戻した後に、撮像工程からワークを取り出す制御を実施する。ステップ１００においては、レンジセンサ６にてワークＷを撮像する撮像工程を実施する。ステップ１０２においては、ワークＷの位置および姿勢を検出する検出工程を実施する。ステップ１０３においては、ロボット１が取り出す次の目標のワークＷを選定する選定工程を実施する。そして、前述と同様の制御を実施する。

ステップ１２１において、１個または２個のワークが検出されなかった場合に、制御はステップ１２２に移行する。ステップ１２２において、判定部５５は、荷崩れが生じているか否かを判定する判定工程を実施する。ここでの判定工程では、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きいか否かを判定する。特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きい場合に、判定部５５は、荷崩れが生じていると判定する。この場合に、制御は、ステップ１２４に移行する。

ステップ１２２において、特定の測定点以外の測定点の個数が荷崩れに関する判定値以下の場合に、判定部５５は、荷崩れが生じていないと判定することができる。この場合に、制御はステップ１２３に移行する。

ステップ１２３においては、判定部５５は、ハンド２がワークＷの把持に失敗したか否かの判定を行う。判定部５５は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が把持の失敗に関する判定値よりも小さいか否かを判定する判定工程を実施する。特定の測定点以外の測定点の個数が把持の失敗に関する判定値よりも小さい場合に、判定部５５は、ハンド２がワークＷの把持に失敗していると判定する。この場合に、制御はステップ１００に戻る。ワークＷを撮像する撮像工程からワークＷを取り出す制御を実施する。ステップ１２３において、ハンド２がワークＷの把持に成功していると判定された場合には、制御は、ステップ１２４に移行する。

図１６に示す制御では、ステップ１１０に示すように、予め定められた個数のワークを取り出すまで、ワークを取り出す制御を実施しているが、この形態に限られない。コンテナ９に配置されている全てのワークＷを取り出した時に制御を終了しても構わない。例えば、ステップ１０１において、容器の内部に配置されているワークが検出されない場合には、制御を終了しても構わない。

本実施の形態の３次元センサは、２台の２次元カメラを含むが、この形態に限られない。３次元センサは、３台以上の２次元カメラを含んでいても構わない。３次元センサが３台以上のカメラを含むことにより、一部の画像がハレーション等により不鮮明であっても、他のカメラにて撮像した画像に基づいて３次元情報を生成することができる。また、本実施の形態の３次元センサはプロジェクタを備えるが、プロジェクタを備えなくても構わない。更に、３次元センサは、ワークまでの距離の情報を取得できる任意のセンサを採用することができる。例えば、３次元センサは、光飛行時間方式により距離画像を撮像するＴＯＦ（Time of Flight）カメラであっても構わない。

本実施の形態のレンジセンサ６は、支持部材８３に固定されているが、この形態に限られず、ワークを撮像可能なように配置することができる。例えば、レンジセンサは、ロボットのリストと一体的に移動するように、リストに固定されていても構わない。

本実施の形態のハンド２は、磁石ハンドであるが、この形態に限られない。ハンドは、ワークを把持したり解放したりする任意の作業ツールを採用することができる。例えば、複数の爪部を有し、爪部がワークを挟むことによりワークを把持するハンド、または吸着パッドの内部の空気を吸引ことにより、吸着パッドにてワークを把持するハンド等を採用することができる。

上述のそれぞれの制御においては、機能および作用が変更されない範囲において適宜ステップの順序を変更することができる。

上記の実施の形態は、適宜組み合わせることができる。上述のそれぞれの図において、同一または相等する部分には同一の符号を付している。なお、上記の実施の形態は例示であり発明を限定するものではない。また、実施の形態においては、特許請求の範囲に示される実施の形態の変更が含まれている。

１ ロボット
２ ハンド
４ 制御装置
５ ロボットシステム
６ レンジセンサ
９ コンテナ
３１ 第１の距離画像
３２ 第２の距離画像
３３ 第３の距離画像
３７ 基準座標系
４２ 記憶部
４３ 動作制御部
４６ ３次元形状データ
５３ 検出部
５４ 削除部
５５ 判定部
５８ 生成部
５９ 選定部
６７ 判定範囲
７１ コンテナの画像
７２ａ，７２ｂ 画像
Ｗ ワーク
ＭＰ，ＭＰａ，ＭＰｂ 測定点

Claims (8)

1. 容器の内部にバラ積みされたワークを取り出すロボットシステムであって、
   ワークを把持するハンドと、
   ハンドを移動するロボットと、
   ワークまでの距離の情報を取得する３次元センサと、
   ハンドおよびロボットを制御する制御装置と、を備え、
   前記制御装置は、ハンドおよびロボットに動作指令を送出する動作制御部と、
   ３次元センサの出力に基づいて、ワークに設定された測定点の位置の情報を含む３次元情報を生成する生成部と、
   ワークの３次元形状データを記憶する記憶部と、
   ワークの３次元情報を３次元形状データと比較するモデルマッチングを行うことにより、ワークの位置および姿勢を検出する検出部と、
   ワークの位置および姿勢に基づいてロボットが取り出す目標のワークを選定する選定部と、
   ３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除部と、
   ロボットがワークを持ち上げる動作を実施した後の状態を判定する判定部とを備え、
   前記生成部は、ロボットが目標のワークを把持する動作を実施する前にワークを撮像した３次元センサの出力に基づいて第１の３次元情報を生成し、
   前記動作制御部は、第１の３次元情報に基づいて選定された目標のワークを把持して持ち上げる動作を実施した後に停止するようにロボットを制御し、
   前記生成部は、ロボットが目標のワークを持ち上げる動作を実施した後にワークを撮像した３次元センサの出力に基づいて第２の３次元情報を生成し、
   前記削除部は、第１の３次元情報の測定点の位置と第２の３次元情報の測定点の位置とを比較し、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出し、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成し、
   前記検出部は、第３の３次元情報に含まれるワークの位置および姿勢を検出し、
   前記判定部は、第３の３次元情報に含まれるワークが目標のワークと同一であるか否かを判定し、
   前記動作制御部は、第３の３次元情報に含まれるワークが目標のワークと同一である場合に、ハンドにて把持したワークを予め定められた搬送位置まで搬送するようにロボットを制御する、ロボットシステム。
2. 前記判定部が、第３の３次元情報に含まれるワークが目標のワークと異なると判定した場合、または、第３の３次元情報に含まれるワークが２個以上であると判定した場合に、前記動作制御部は、ハンドにて把持したワークをハンドにて把持する前に配置されていた元の位置の近傍に戻すようにロボットを制御し、
   ロボットがワークを元の位置の近傍に戻した後に、３次元センサは、容器に配置されているワークを撮像し、前記検出部は、ワークの位置および姿勢を検出し、前記選定部は、ロボットが取り出す次の目標のワークを選定する、請求項１に記載のロボットシステム。
3. 前記判定部は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が予め定められた荷崩れに関する判定値よりも大きいか否かを判定し、
   前記判定部が特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きいと判定した場合に、前記動作制御部は、ハンドにて把持したワークをハンドにて把持する前に配置されていた元の位置の近傍に戻すようにロボットを制御し、
   ロボットがワークを元の位置の近傍に戻した後に、３次元センサは、容器に配置されているワークを撮像し、前記検出部は、ワークの位置および姿勢を検出し、前記選定部は、ロボットが取り出す次の目標のワークを選定する、請求項１または２に記載のロボットシステム。
4. 前記判定部は、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が予め定められた把持の失敗に関する判定値よりも小さいか否かを判定し、
   前記判定部が特定の測定点以外の測定点の個数が把持の失敗に関する判定値よりも小さいと判定した場合に、３次元センサは、容器に配置されているワークを撮像し、前記検出部は、ワークの位置および姿勢を検出し、前記選定部は、ロボットが取り出す次の目標のワークを選定する、請求項１から３のいずれか一項に記載のロボットシステム。
5. ロボットおよびハンドを備え、容器の内部にバラ積みされたワークを取り出すロボットシステムの制御方法であって、
   ロボットの位置および姿勢を変更する動作制御工程と、
   ワークまでの距離の情報を取得する３次元センサにてワークを撮像する撮像工程と、
   ３次元センサの出力に基づいて、ワークに設定された測定点の位置の情報を含む３次元情報を生成する生成工程と、
   ワークの３次元情報をワークの３次元形状データと比較するモデルマッチングを行うことにより、ワークの位置および姿勢を検出する検出工程と、
   ワークの位置および姿勢に基づいてロボットが取り出す目標のワークを選定する選定工程と、
   ３次元情報に含まれる少なくとも一部の測定点の情報を削除する削除工程と、
   ロボットがワークを持ち上げる動作を実施した後の状態を判定する判定工程とを備え、
   前記生成工程は、ロボットが目標のワークを把持する動作を実施する前にワークを撮像した３次元センサの出力に基づいて第１の３次元情報を生成する工程を含み、
   前記動作制御工程は、第１の３次元情報に基づいて選定された目標のワークを把持して持ち上げる動作を実施した後に停止するようにロボットを制御する工程を含み、
   前記生成工程は、ロボットが目標のワークを持ち上げる動作を実施した後にワークを撮像した３次元センサの出力に基づいて第２の３次元情報を生成する工程を含み、
   前記削除工程は、第１の３次元情報の測定点の位置と第２の３次元情報の測定点の位置とを比較する工程と、予め定められた距離の範囲内に第２の３次元情報の測定点が存在する第１の３次元情報の測定点を特定の測定点として検出する工程と、第１の３次元情報から特定の測定点の情報を削除した第３の３次元情報を生成する工程とを含み、
   検出工程は、第３の３次元情報に含まれるワークの位置および姿勢を検出する工程を含み、
   前記判定工程は、第３の３次元情報に含まれるワークが目標のワークと同一であるか否かを判定する工程を含み、
   前記動作制御工程は、第３の３次元情報に含まれるワークが目標のワークと同一である場合に、ハンドにて把持したワークを予め定められた搬送位置まで搬送するようにロボットを制御する工程を含む、ロボットシステムの制御方法。
6. 前記判定工程において、第３の３次元情報に含まれるワークが目標のワークと異なると判定された場合、または、第３の３次元情報に含まれるワークが２個以上であると判定された場合に、ハンドにて把持したワークをハンドにて把持する前に配置されていた元の位置の近傍に戻すようにロボットを制御し、
   ロボットがワークを元の位置の近傍に戻した後に、容器に配置されているワークを３次元センサにて撮像する撮像工程を実施し、ワークの位置および姿勢を検出する検出工程を実施し、ロボットが取り出す次の目標のワークを選定する選定工程を実施する、請求項５に記載のロボットシステムの制御方法。
7. 前記判定工程において、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が予め定められた荷崩れに関する判定値よりも大きいか否かを判定し、
   特定の測定点以外の測定点の個数が荷崩れに関する判定値よりも大きい場合に、ハンドにて把持したワークをハンドにて把持する前に配置されていた元の位置の近傍に戻すようにロボットを制御し、
   ロボットがワークを元の位置の近傍に戻した後に、容器に配置されているワークを３次元センサにて撮像する撮像工程を実施し、ワークの位置および姿勢を検出する検出工程を実施し、ロボットが取り出す次の目標のワークを選定する選定工程を実施する、請求項５または６に記載のロボットシステムの制御方法。
8. 前記判定工程において、第１の３次元情報の測定点のうち特定の測定点以外の測定点の個数が予め定められた把持の失敗に関する判定値よりも小さいか否かを判定し、
   特定の測定点以外の測定点の個数が把持の失敗に関する判定値よりも小さい場合に、容器に配置されているワークを３次元センサにて撮像する撮像工程を実施し、ワークの位置および姿勢を検出する検出工程を実施し、ロボットが取り出す次の目標のワークを選定する選定工程を実施する、請求項５から７のいずれか一項に記載のロボットシステムの制御方法。