JP5788461B2 - バラ積みされた物品をロボットで取出す装置及び方法 - Google Patents

Description

本発明は、三次元空間にバラ積みされた物品の位置及び姿勢を認識し、認識した物品を、ロボットを用いて取出す物品取出装置及び物品取出方法に関する。

この種の装置として、従来、バラ積みされた物品をカメラで撮像して得られた二次元画像や三次元測定機で測定して得られた三次元点集合に対し、パターンマッチングを用いて物品の位置を認識するようにした装置が知られている（例えば特許文献１参照）。

この特許文献１記載の装置では、事前にCADモデルなどから物品の三次元モデルパターンを取得する一方、三次元空間における物品の表面を三次元測定機で測定して三次元点集合（距離画像）を取得し、三次元点集合から抽出されたエッジによって囲まれた部分領域に三次元点集合を分割する。そして、初めに部分領域の一つを物品領域として設定し、この物品領域に対する三次元モデルパターンのマッチング処理と、他の部分領域を物品領域に加える更新処理の二つの処理を繰り返すことで、物品の位置及び姿勢を計測する。

特開２０１１−１７９９０９号公報

しかしながら、特許文献１に記載の装置は、予め物品一品種毎に三次元モデルパターンを作成する必要があり、手間を要する。とくに、物品が多品種の場合には、品種数分のモデルパターンを作成する必要があり、多大な手間が必要となる。また、不定形の物品の場合には、そもそもモデルパターンを作成することができず、適用不可能である。さらに、物品がバラ積みされた状態では、パターンマッチングにより物品の三次元姿勢を決定できるほど十分な三次元点集合を得ることは困難である。

本発明の一態様である物品取出装置は、物品を保持可能なハンドを有するロボットと、三次元空間にバラ積みされた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、三次元測定機により測定された位置情報に基づき、三次元空間における複数の三次元点の分布の程度を表す密度分布を演算する密度演算部と、密度演算部により演算された密度分布に基づき、密度が極大となる密度極大位置を演算する極大位置演算部と、極大位置演算部により演算された密度極大位置に基づき、密度極大位置またはその近傍にある物品を取出し可能なハンドの位置及び姿勢であるハンド位置姿勢を演算するハンド位置姿勢演算部と、ハンド位置姿勢演算部により演算されたハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御するロボット制御部と、を備えることを特徴とする。

また、本発明の別の態様は、物品を保持可能なハンドを有するロボットを用いて、三次元空間にバラ積みされた複数の物品を取出す物品取出方法であって、バラ積みされた複数の物品の表面位置を三次元測定機で測定して、複数の三次元点の位置情報を取得し、三次元測定機により測定された位置情報に基づき、三次元空間における複数の三次元点の分布の程度を表す密度分布を演算し、演算された密度分布に基づき、密度が極大となる密度極大位置を演算し、演算された密度極大位置に基づき、密度極大位置またはその近傍にある物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢を演算し、演算されたハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御することを特徴とする。

本発明によれば、三次元点測定機によって取得された物品表面の複数の三次元点の位置情報から、三次元点の分布の程度を表す密度分布を演算し、密度が極大となる密度極大位置に基づきハンドの位置姿勢を制御する。したがって、物品のモデルパターンを作成することなく物品の位置及び姿勢を認識することができ、多品種や不定形の物品であっても容易にその位置及び姿勢を認識して、物品を保持することができる。密度極大位置は物品の表面上に存在するので、物品の表面に確実にハンドを当接させることができ、物品取出作業の信頼性が高まる。

本発明の一実施形態に係る物品取出装置の概略構成を示す図。 図１のロボット制御装置で実行される処理の一例を示すフローチャート。 図１の三次元測定機で取得した三次元点集合の一例を示す図。 図３の三次元点集合から求めた連結集合の一例を示す図。 連結集合を説明する概念図。 連結集合を求める処理の詳細を示すフローチャート。 連結集合に属する三次元点を近似した平面と平面上の法線ベクトルとを示す図。 連結集合に属する三次元点を平面に正射影した投影点の一例を示す図。 投影点の密度を求める計測点の一例を示す図。 密度極大点に対応するハンド位置姿勢の一例を示す図。 図１０のハンド位置姿勢の番号付けの一例を示す図。 本発明の実施形態に係る物品取出装置の動作の一例を示す図。 図１２に続く動作の一例を示す図。 図１３に続く動作の一例を示す図。 図１のロボット制御装置の内部構成を示すブロック図。

以下、図１〜図１５を参照して本発明の実施形態に係る物品取出装置について説明する。図１は、本発明の一実施形態に係る物品取出装置１０の概略構成を示す図である。物品取出装置１０は、三次元測定機１１と、ロボット１２と、三次元測定機１１とロボット１２とに接続してロボット１２を制御するロボット制御装置１３とを有する。ロボット１２は、アーム１２ａの先端部に取り付けられたハンド１４を有する。ロボット１２の側方にはコンテナ１６が配置されている。なお、図１には、ＸＹＺの直交３軸座標系を併せて示している。Ｚ方向は鉛直方向、Ｘ方向及びＹ方向は水平方向であり、コンテナ１６はＸＺ平面上に示されている。

上方が開放されたコンテナ１６内には、複数の物品２０がバラ積みされている。本実施形態の物品取出装置１０は、この複数の物品２０がバラ積みされた状態から取出すべき物品２０の位置及び姿勢を認識し、認識された物品２０をコンテナ１６の中からハンド１４で取出して保持し、ロボット１２の動作によりコンテナ１６外の所定位置へ搬送する。複数の物品２０は、図１では互いに同一形状（Ｌ字型形状）として示しているが、Ｌ字型以外でもよく、不定形や複数の品種を含むものでもよい。なお、以下では、ハンド１４で保持された物品２０を、コンテナ１６内の他の物品と区別するために符号２１（図１３参照）で表すことがある。

三次元測定機１１は、コンテナ１６の中央部上方に配置され、コンテナ１６内にバラ積みされた物品２０のうち、露出した物品２０の表面を測定して、複数の三次元点の位置情報（三次元情報）を取得する。三次元測定機１１の測定範囲は、コンテナ１６を含む必要があるが、測定範囲が大き過ぎると測定分解能の低下を招く。したがって、測定範囲は、コンテナ１６の占有範囲と同等、例えばコンテナ１６の占有範囲に一致させることが好ましい。なお、図１では、三次元測定機１１が専用の架台１５に固定されているが、ロボット１２の先端部に三次元測定機１１を取り付けてもよい。三次元測定機１１とロボット制御装置１３とは通信ケーブル等の通信手段によって互いに接続されており、互いに通信できるようになっている。

三次元測定機１１としては、種々の非接触方式のものを利用することができる。例えば、カメラ２台のステレオ方式、レーザスリット光を走査する方式、レーザスポット光を走査する方式、プロジェクタ等の装置を用いてパターン光を物品に投影する方式、光が投光器から出射されてから物品表面で反射し受光器に入射するまでの飛行時間を利用する方式などが挙げられる。

三次元測定機１１は、取得した三次元情報を距離画像または三次元マップといった形式で表現する。距離画像とは、画像形式で三次元情報を表現したものであり、画像の各画素の明るさや色により、その画像上の位置の高さまたは三次元測定機１１からの距離を表す。一方、三次元マップとは、測定された三次元座標値（ｘ，ｙ，ｚ）の集合として三次元情報を表現したものである。本実施形態では、距離画像における各画素や三次元マップにおける三次元座標値を有する点を、三次元点と称し、複数の三次元点からなる集合を、三次元点集合と称する。三次元点集合は、三次元測定機１１で測定した三次元点全体の集合であり、三次元測定機１１により取得できる。

本実施形態では、三次元測定機１１により、露出した物品２０の表面の三次元位置を測定し、物品２０の表面上の位置姿勢を検出する。ハンド１４は、位置姿勢が検出された物品２０の表面に接触し、物品２０を保持する。このようなハンド１４としては、例えば吸着パッド、吸引ノズル、吸着用磁石などが挙げられる。本実施形態では、吸着パッドをハンド１４として用いている。

吸着パッド１４は、長手方向に延在する軸部１４ａと、軸部１４ａの先端に設けられたパッド部１４ｂとを有する。パッド部１４ｂの下端部には吸着面１４ｃが設けられ、吸着面１４ｃの中心は、軸部１４ａの中心を通る軸線Ｌ０上に位置する。この吸着面１４ｃの中心から所定距離Δｄだけ上方における軸線Ｌ０上の位置に、ハンド１４の基準点１４ｄが設定されている。ハンド１４は、ロボット１２の動作により、その位置姿勢が制御される。すなわち、三次元点空間における基準点１４ｄの位置および軸線Ｌ０の向きが制御される。なお、本実施形態では、１つの物品２０を保持するようにハンド１４を構成しているが、同時に複数の物品を保持するようにハンド１４を構成してもよい。

図２は、ロボット制御装置１３で実行される処理、特に物品取出しに係る処理の一例を示すフローチャートである。以下、物品取出装置１０による動作を、図２のフローチャート及び関連する図面を参照しつつ説明する。

図２の処理は、例えば図示しない操作スイッチの操作により、物品２０の取出開始指令が入力されると開始される。まず、ステップＳ１で、三次元空間にバラ積みされた複数の物品２０の表面を三次元測定機１１で測定して三次元点集合３０を取得する。図３は、三次元測定機１１で取得した三次元点集合３０と、三次元点集合３０を構成する三次元点３１の一例を示す図である。図では、三次元点３１が黒丸で示されており、三次元点集合３０は、黒丸全体を含む点線で囲まれた領域として示されている。

次に、ステップＳ２で、三次元点集合３０から１以上の連結集合３２を求める（ステップＳ２）。図４は、三次元点集合３０から求めた連結集合３２の一例を示す図である。図では、連結集合３２は点線で囲まれた領域として示されている。すなわち、図４には、４つの連結集合３２が示されている。

ここで言う連結集合３２とは、三次元点集合３０の部分集合であり、任意の三次元点（第１の三次元点）３１の近傍にその三次元点３１とは異なる他の三次元点（第２の三次元点）３１が存在する場合、第１の三次元点３１と第２の三次元点３１とが連結されてなるものである。すなわち、連結集合３２とは、互いに近傍に存在する三次元点３１を連結してなる集合である。図５は、連結集合３２の概念を説明する図である。図５では、隣り合う第１三次元点３１と第２三次元点３１との間の距離が所定値以内であるときに、第１三次元点３１と第２三次元点３１とを互いに連結している。

例えば図５に示すように複数の三次元点３１（３１１〜３１７で表す）が三次元測定機１１により測定され、このうち、３１１と３１２、３１２と３１３、３１３と３１４、及び３１５と３１６がそれぞれ所定距離内に存在するとき、これらは互いに連結される。この場合、３１２と３１３を介して３１１と３１４も連結されるため、３１１〜３１４は同一の連結集合３２１を構成する。一方、３１５と３１６は、３１１〜３１４のいずれにも連結されないため、別の連結集合３２２を構成する。３１７は、３１１〜３１６のいずれにも連結されないため、３１７のみで連結集合３２３を構成する。

三次元的測定機１１によりバラ積みされた物品２０の表面が測定される場合、同一物品２０上における隣り合う三次元点３１（例えば図５の３１３，３１４）は互いに近距離に位置する。これに対し、物品２０の境界部では隣り合う三次元点（例えば図５の３１４，３１５）の位置が大きく変化する。したがって、三次元点３１３，３１４は同一の連結集合３２に属するのに対し、三次元点３１４，３１５は互いに異なる連結集合３２に属する。このため、連結集合３２を構成する３次元点間の最大距離や、連結集合３２の最小点数、最大点数等を適宜設定することで、連結集合３２を、単一の物品２０の表面形状とみなすことができる。すなわち、単一の物品２０に対して単一の連結集合３２が１対１で対応し、連結集合３２によって物品２０を特定することができる。

図６は、連結集合３２を求めるための処理（連結集合演算処理）、すなわち図２のステップＳ２の処理をより詳細に示すフローチャートである。まず、ステップＳ２１で、三次元点集合３０に属する全ての三次元点３１に、初期のラベル番号として、どの連結集合３２にも属していないことを表すラベル番号０を割り振る。以下では、自然数であるラベル番号ｊが割り振られた三次元点３１を、３１（ｊ）で表す。ラベル番号ｊは、連結集合３２に対応して割り振られる番号であり、０ではない同一のラベル番号ｊが割り振られていれば、同一の連結集合３２に属することになる。次に、ステップＳ２２で、１番目の連結集合３２を求めるため、ラベル番号ｊを１とする（j←1）。

次に、ステップＳ２３で、三次元点集合３０に属する三次元点３１であって、ラベル番号が０である任意の三次元点３１（０）を選択する。ステップＳ２４では、ラベル番号が０である三次元点３１（０）が選択されたか否かを判定し、肯定されるとステップＳ２５に進む。三次元点３１（０）が選択されなかった場合は、三次元点集合３０に属する全ての三次元点３１が何れかの連結集合３２に属している。この場合は、ステップＳ２４が否定され、連結集合演算処理が終了し、図２のステップＳ３に進む。

ステップＳ２５では、ラベル番号がｊである三次元点３１（ｊ）を格納するためのリストＬｊを準備する。ステップＳ２６では、ステップＳ２４で選択された三次元点３１（０）にラベル番号ｊを割り振った後に、その三次元点３１（ｊ）をリストＬｊに追加する。ステップＳ２７では、自然数値を取る変数ｋに対して、初期値１を与える（ｋ←１）。ｋは、リストＬｊに含まれる三次元点３１（ｊ）を指定する番号である。なお、リストＬｊには、追加された三次元点３１（ｊ）が追加された順番に並んでいるものとする。

ステップＳ２８では、リストＬｊのｋ番目の三次元点３１（ｊ）の近傍に、ラベル番号が０である三次元点３１（０）が存在するか否かを判定する。ステップＳ２８が肯定されるとステップＳ２９に進み、否定されるとステップＳ２９をパスしてステップＳ３０に進む。ステップＳ２９では、リストＬｊのｋ番目の三次元点３１（ｊ）の近傍に存在すると判定された全ての三次元点３１（０）にラベル番号ｊを割り振った後、これら三次元点３１（ｊ）をリストＬｊの最後に追加する。ステップＳ３０では、変数ｋに１を追加する（ｋ←ｋ＋１）。

ステップＳ３１では、ｋの値がリストＬｊに格納されている三次元点３１（ｊ）の数（要素数Ｎ）より大きいか否かを判定する。ｋが要素数Ｎより大きい場合、リストＬｊに格納されたＮ個の全ての三次元点３１（ｊ）に対する近傍判定の処理が終了しており、リストＬｊ内の三次元点３１（ｊ）の近傍にある三次元点が既に同一のリストＬｊ内に格納されている。このため、リストＬｊに三次元点３１（ｊ）を追加する処理を終了し、ステップＳ３２に進む。それ以外の場合は、リストＬｊ内の全ての三次元点３１（ｊ）に対し近傍判定の処理が終了していないため、ステップＳ２８に戻り、リストＬｊへ三次元点３１（ｊ）を追加する処理を繰り返す。

ステップＳ３２では、ラベル番号ｊに１を追加し（ｊ←ｊ＋１）、ステップＳ２３に戻る。以降、ステップＳ２３〜ステップＳ３２と同様の処理を繰り返し、次のラベル番号ｊに対応する連結集合３２を求める。

以上の連結集合演算処理を、図５を参照して具体的に説明する。連結集合演算処理の開始時には、全ての三次元点３１１〜３１７が連結集合３２に属しておらず、三次元点３１１〜３１７のラベル番号は０である（ステップＳ２１）。この状態から、ラベル番号１の連結集合３２を作成するため、例えば三次元点３１３を選択すると（ステップＳ２３）、その三次元点３１３にラベル番号１を割り振った後（３１３（１））、三次元点３１３をラベル番号１のリストＬ１の１番目に格納する（ステップＳ２６）。

次いで、リストＬ１の１番目の三次元点３１３の近傍に、ラベル番号０の三次元点３１（０）が存在するか否かを判定する（ステップＳ２８）。この場合、ラベル番号０の三次元点３１２，３１４が存在するため、これら三次元点３１２，３１４にラベル番号１をそれぞれ割り振り（３１２（１），３１４（１））、リストＬ１の２番目及び３番目にそれぞれ追加する（ステップＳ２９）。これによりリストＬ１の要素数Ｎは３となる。

その後、変数ｋが２（＜Ｎ）となり（ステップＳ３０）、リストＬ１の２番目の三次元点３１２の近傍に、ラベル番号０の三次元点３１（０）が存在するか否かを判定する（ステップＳ２８）。この場合、ラベル番号０の三次元点３１１が存在するため、この三次元点３１１にラベル番号１を割り振り（３１１（１））、リストＬ１の４番目に追加する（ステップＳ２９）。これによりリストＬ１の要素数Ｎは４となる。

その後、変数ｋが３（＜Ｎ）となり（ステップＳ３０）、リストＬ１の３番目の三次元点３１４の近傍に、ラベル番号０の三次元点３１が存在するか否かを判定する（ステップＳ２８）。この場合、三次元点３１４の近傍にラベル番号０の三次元点３１は存在しないため、要素数Ｎが４のままｋが４となり（ステップＳ３０）、リストＬ１の４番目の三次元点３１１の近傍に、ラベル番号０の三次元点３１（０）が存在するか否かを判定する（ステップＳ２８）。この場合、三次元点３１１の近傍にラベル番号０の三次元点３１は存在しないため、要素数Ｎが４のままｋが５となる（ステップＳ３０）。

このとき、変数ｋは要素数Ｎよりも大きくなるため、ラベル番号１のリストＬ１の作成を終了し、ラベル番号を２として（ステップＳ３２）、同様の処理を繰り返す。繰り返しの処理では、例えばラベル番号０の三次元点３１５，３１６にラベル番号２を割り振って、三次元点３１５（２），３１６（２）をリストＬ２に追加し、ラベル番号０の三次元点３１７にラベル番号３を割り振って、三次元点３１７（３）をリストＬ３に追加する。これにより、ラベル番号０の三次元点３１が不存在となるため、ステップＳ２４が否定され、連結集合演算処理を終了し、図２のステップＳ３に進む。

ステップＳ３では、各々の連結集合３２に対し、連結集合３２に属する三次元点３１の位置に基づき、三次元点３１を近似した平面３３を求める。平面３３は、三次元点３１の存在する範囲にわたって延在する平面、すなわち有限な平面である。図７は、平面３３と平面３３上の点から平面３３に対して垂直に延びる法線ベクトル３３ａとを示す図である。法線ベクトル３３ａにより各平面３３の姿勢を特定することができる。三次元点３１が各物体２０の単一の表面のみで計測されたと仮定すると、図７に示すように、その物体２０の表面が平面３３となる。

平面３３は、例えば連結集合３２に含まれる全ての三次元点３１を用い、最小二乗法により演算することができる。なお、別途何らかの外れ値対策を行って平面３３を求めてもよい。外れ値対策の方法としては、M推定法、RANSAC、LMedS、ハフ変換など、幾通りかの方法がある。ハフ変換などの処理を導入することで、連結集合３２が、複数の物品２０の表面に跨っている場合でも、その中から、一つの平面３３を抽出して認識することが可能になる。平面３３から所定距離以上離れている三次元点３１を、以降の処理において、その三次元点３１が含まれる連結集合３２から除去するようにしてもよい。

ステップＳ４では、連結集合３２に属する三次元点３１を、その連結集合３２に対応する平面３３に対して垂直に投影した投影点３４の位置を演算する。図８は、三次元点３１を平面３３に正射影した投影点３４の一例を示す図である。図８では、投影点３４は白丸で示されている。なお、連結集合３２に属する三次元点３１の個数が所定値よりも少ない、すなわち物品２０の露出した面積が所定値より小さい連結集合３２（例えば図７の３２ａ）に対しては、ステップＳ４以降の処理を行わず、物品２０の表面が十分に露出している場合にのみ、ステップＳ４以降の処理を行うようにしてもよい。これにより物品２０の取出し時に、ハンド１４が物品２０を取り損ねる可能性を低減し、物品取出装置１０により効率的な物品２０の取出しを実現できる。

ステップＳ５では、各平面３３の全範囲にわたって各平面３３上に計測点を設定し、各計測点から所定距離内にある投影点３４の個数を求める。この個数は、各計測点における投影点３４の密度に対応するものであり、ステップＳ５では、平面３３上における投影点３４の密度分布を演算する。投影点３４は三次元点３１に対応するため、投影点３４の密度分布は三次元点３１の密度分布とみなすことができる。

図９は、計測点３５と投影点３４の一例を示す図である。図９では、平面３３上に、格子状に等間隔に計測点３５を設定している。図９において、計測点３５ａから所定距離ｒ内にある投影点３４の数、すなわち計測点３５ａを中心とした半径ｒの円３７内に含まれる投影点３４の個数は９個である。この場合、例えば投影点３５ａの密度を９とする。なお、投影点３４の個数を円３７の面積で除算した値を密度としてもよい。このような密度の計算を、全ての計測点３５に対して行うことで、平面３３上の投影点３４の密度分布を求めることができる。平面３３上に等間隔に計測点３５を設定するのではなく、例えば各投影点３４に計測点３５を設定してもよい。各計測点３５間における密度は、例えば補間処理によって求めることができ、これにより平面３３全体の密度分布を求めることができる。

ステップＳ６では、ステップＳ５で演算した密度分布に基づき、投影点３４の密度が極大となる平面３３上の点（密度極大点）の位置（密度極大位置）を演算する。なお、投影点３４の密度分布は三次元点３１の密度分布に対応するため、ステップＳ６の処理は、三次元点３１の密度が極大となる密度極大位置を演算することに相当する。密度極大位置は、平面３３上の所定範囲内における密度の大小を比較し、密度が最大となる平面３３上の位置を演算することにより求めることができる。密度極大点が平面３３上に複数求められた場合、例えば密度が大きい方を選択し、密度極大位置を演算すればよい。密度極大位置は、連結集合３２を代表する代表位置である。

図１０は、密度極大点３９の一例を示す図である。密度極大点３９は、物品２０の表面の三次元点３１が密集した点である。したがって、物品２０がＬ字型形状や他の複雑形状であっても、あるいは物品２０の中央（重心位置）に孔が開口されている場合であっても、密度極大点３９は、常に物品２０の露出した表面上に存在する。なお、図１０では、密度極大点３９における平面３３に対する法線ベクトル３９ａを示している。

ステップＳ７では、密度極大位置にある物品２０を取出し可能なハンド１４の位置及び姿勢（ハンド位置姿勢３８）を演算する。図１０に示すように、ハンド位置姿勢３８は、例えば直角に交差する一対の矢印３８ａ，３８ｂで示され、矢印３８ａ，３８ｂの交点がハンド位置姿勢３８の位置を表し、矢印３８ａ，３８ｂの向きがハンド位置姿勢３８の姿勢を表す。なお、図１０では、便宜上、ハンド位置姿勢３８が２つの矢印３８ａ，３８ｂで示されているが、ハンド位置姿勢３８は、二次元空間におけるものではなく三次元空間におけるものであり、実際には三次元の直交座標系で表される。直交座標系に限らず、種々の座標系でハンド位置姿勢３８を表すこともできる。

一対の矢印３８ａ，３８ｂの交点は、密度極大点３９から法線ベクトル３９ａに沿って所定距離（例えば図１のΔｄ）だけ離れた位置に設定される。矢印３８ｂは、法線ベクトル３９ａと平行であり、矢印３８ａは、三次元点３１によって求められた平面３３と平行である。

ステップＳ８では、各々のハンド位置姿勢３８にＰ１，Ｐ２，・・・，Ｐｎとナンバリングする。但し、ｎはハンド位置姿勢３８の個数である。図１１は、ナンバリングされたハンド位置姿勢３８を示す図であり、所定の座標軸４０に対する座標値の降順、つまり、高い位置にある（ｚ座標が大きい）ものから順番にナンバリングされている。ナンバリングの順序は、座標値の降順の他、連結集合３２をなす面の面積の大きい順など、取得した三次元点集合に基づいて任意の方法で決定付けられる順番としてもよい。なお、図１１では、ｎ＝３である。

ステップＳ９では、自然数値を取る変数ｊに対して初期値を与える。すなわち、ｊ←１とする。変数ｊは、ハンド位置姿勢３８の番号の指定に用いる。

ステップＳ１０では、ロボット駆動用のアクチュエータ（電動モータ）に制御信号を出力し、図１２に示すように、ハンド１４をハンド位置姿勢Ｐｊ（例えばＰ１）に移動する。すなわち、ハンド１４の基準点１４ｄを、ハンド位置姿勢Ｐ１の矢印３８ａ，３８ｂの交点に一致させるとともに、ハンド１４の軸線Ｌ０を矢印３８ｂに一致させる。これによりハンド１４の吸着面１４ｃが、取出すべき物品２０の表面に平行に配置される。ハンド位置姿勢Ｐｊは、所定の座標軸４０に対する降順にナンバリングしているので、取出すべき物品２０はコンテナ１６内の最も高い位置にある物品２０となる。

ステップＳ１１では、ハンド駆動用のアクチュエータに物品２０を保持するための制御信号を出力する。これにより、物品２０が、ハンド１４の吸着面１４ｃに吸着され、保持される。吸着面１４ｃは、物品２０の現実に存在する表面に対向して配置されるため、ハンド１４で物品２０を安定的に保持することができる。この場合、例えば、ハンド１４が吸引ノズルを有する場合、真空ポンプを作動し、吸引力で物品２０を吸引、保持する。把持部１４ａが吸着用磁石を有する場合、電磁コイルに電流を流して磁石を作動し、磁力で物品２０を吸着、保持する。

次に、ステップＳ１２で、ロボット駆動用のアクチュエータに制御信号を出力し、図１３に示すように、物品２１を保持したハンド１４を所定方向、例えば所定の座標軸４０（図１１）の方向、あるいはハンド位置姿勢３８の矢印３８ｂ方向（図１２）に沿って上昇させる。ハンド１４を所定方向に上昇させることで、ロボット１２の動作によりハンド１４を移動させる際、ハンド１４や物品２１が他の物品２０と衝突することを避けることができる。

ステップＳ１３では、ハンド１４による物品２１の保持が成功したか否かを判定する。例えば、ハンド１４に重量を検出する重量検出器を設け、検出値が所定値以上となった場合に、保持が成功したと判定する。なお、ハンド１４が吸着用磁石を有する場合、近接センサで物品２１が存在するか否かを判定し、判定結果に応じて保持が成功したか否かを判定すればよい。ハンド１４が吸引ノズルを有する場合、吸引時のエアの流量や圧力の変化に応じて保持が成功したか否かを判定すればよい。保持が成功したと判定されると、ステップＳ１４へ進む。保持が成功していないと判定されると、ステップＳ１５へ進む。

ステップＳ１５では、ｊ＜ｎか否かを判定する。この判定は、ｎ個（図１２では３個）のハンド位置姿勢３８の中にハンド１４が未だ到達していないものが存在するか否かの判定である。ｊ＜ｎと判定されると、ハンド位置姿勢Ｐｊ＋１にハンド１４が未だ到達していないので、ステップＳ１６でｊ←ｊ＋１とし、ステップＳ１０へ戻る。そして、図１４に示すようにロボット１２の動作により、ハンド１４を次のハンド位置姿勢Ｐｊ（例えばＰ２）へ移動させる。次いで、ハンド駆動用のアクチュエータに物品２０を保持するための制御信号を出力し、物品２１を保持する（ステップＳ１１）。ステップＳ１５でｊ≧ｎと判定されると、ｎ個のハンド位置姿勢３８の全てにハンド１４が到達したので、ステップＳ１４へ進む。

ステップＳ１４では、ロボット駆動用アクチュエータに制御信号を出力し、物品２１をロボット１２の動作により所定位置に搬送し、物品２１をハンド１４から取り外す。以上で、１サイクルの処理が終了となる。

本実施形態によれば以下のような作用効果を奏することができる。
（１）本実施形態では、三次元点空間にバラ積みされた複数の物品２０の表面位置を三次元測定機１１で測定して複数の三次元点３１の位置情報を取得し（ステップＳ１）、測定された三次元点１１の位置情報に基づき、三次元空間における三次元点１１の分布の程度を表す密度分布を演算し（ステップＳ５）、演算された密度分布に基づき、密度が極大となる密度極大位置を演算し（ステップＳ６）、この密度極大位置に基づき、密度極大位置にある物品２０を取出し可能なハンド位置姿勢３８を演算し（ステップＳ７）、ハンド位置姿勢３８へハンド１４を移動して物品２０を取出すようにロボット１２を制御する（ステップＳ１０〜ステップＳ１２）。

これにより、パターンマッチング等によらず物品２０の位置及び姿勢を特定することができる。このため、物品２０のモデルパターンを作成する必要がなく、多品種や不定形の物品２０であっても容易にその位置及び姿勢を認識して、物品２０を保持することができる。また、追加された新品種の物品２０に対してもモデルパターンを追加することなく、その位置及び姿勢を認識することができ、物品２０の位置姿勢の認識失敗や誤認識あるいは物品２０の取損ねや衝突等の問題を回避し、上方にある取出し易い物品２０へハンド１４を高速に移動させて物品２０を効率良く取出すことができる。さらに、三次元点３１の密度が高い密度極大位置に基づきハンド位置姿勢３８を演算するため、物品２０の表面にハンド１４を確実に接触させることができる。したがって、物品２０がＬ字形状を呈する場合や、物品２０の中央に穴が開口されている場合等、連結集合３２の重心が物品２０の表面上にない場合であっても、物品２０を安定的に保持することができる。

（２）三次元測定機１１により取得された複数の三次元点３１を近似する平面３３を演算し（ステップＳ３）、その平面３３に複数の三次元点３１を投影した投影点３４の位置を演算し（ステップＳ４）、平面３３上の密度計測点３５から所定距離ｒ内にある投影点３４の個数を求め、この個数に応じて密度計測点３５における投影点３４の密度を求めることで、密度分布を演算するようにした（ステップＳ５）。このように三次元点３１を平面３３上に投影することで、物品２０の表面に密度が極大となる箇所が存在するようになり、物品２０の表面をハンド１４で良好に吸着することができ、物品取出装置１０による物品取出しの信頼性が高まる。これに対し、近似平面３３を導入しない場合、例えば物品２０の角部に密度が極大となる箇所が存在することがあり、この場合には物品２０の角部をハンド１４で吸着することは困難である。

（３）複数の三次元点３１のうち、互いに近傍にある三次元点３１を連結してなる連結集合３２を構成し（ステップＳ２）、各連結集合３２に含まれる三次元点３１を用いて、連結集合３２毎に密度分布を演算し（ステップＳ５）、密度極大位置を求めるようにした（ステップＳ６）。連結集合３２は各物品２０に対応した三次元点３１の集合であるため、物品３１毎に密度極大位置が求められることとなり、互いに姿勢の異なる複数の物品２０をハンド１４で保持することが可能となる。

（４）連結集合３２は、密度極大位置を含む平面３３上の領域内における三次元点３１によって構成され、連結集合３２に含まれる三次元点３１の位置情報に基づき、各物品２０に対応した連結集合３２を代表する位置、すなわち密度極大位置を演算し、この代表位置と代表位置の周囲の三次元点３１の位置情報（例えば平面３３をなす三次元点３１の位置情報）とに基づき、ハンド位置姿勢３８（図１２の矢印３８ａ，３８ｂの交点の位置および矢印３８ａ，３８ｂの向き）を演算するようにした（ステップＳ７）。これにより物品２０毎にハンド位置姿勢３８が演算され、個々の物品２０を安定的に保持することができる。

図１５は、図１のロボット制御装置１３の内部構成を示すブロック図である。ロボット制御装置１３は、密度演算部１３１と、極大位置演算部１３２と、ハンド位置姿勢演算部１３３と、ロボット制御部１３４と、集合設定部１３５とを有する。密度演算部１３１は、面演算部１３１Ａと、投影点演算部１３１Ｂとを有する。

上記実施形態では、三次元測定機１１により取得された複数の三次元点３１を平面３３で近似したが（ステップＳ３）、曲面で近似するようにしてもよく、面演算部１３１Ａの構成は上述したものに限らない。したがって、ステップＳ４において、平面３３に複数の三次元点３３を投影するのではなく、曲面に複数の三次元点３１を投影するようにしてもよく、投影点３４の位置を演算する投影点演算部１３１Ｂの構成も上述したものに限らない。上記実施形態では、平面３３上の密度計測点３５から所定距離ｒ内にある投影点３４の個数を求め、この個数に応じて密度計測点３５における投影点３４の密度を求め、この密度を用いて投影点３４の密度分布を演算するようにしたが（ステップＳ５）、三次元点３１を平面や曲面に投影することなく、三次元空間における複数の三次元点３１の分布の程度を表す密度分布を演算するようにしてもよく、三次元測定機１１により測定された位置情報に基づき密度分布を演算するのであれば、密度演算部１３１の構成はいかなるものでもよい。

上記実施形態では、投影点３４の密度分布に基づき、投影点３４の密度が極大となる密度極大位置を演算したが（ステップＳ６）、投影点３４を用いることなく三次元点３１の密度分布に基づき、三次元点３１の密度が極大となる密度極大位置を演算するようにしてもよく、極大位置演算部１３２の構成は上述したものに限らない。

上記実施形態では、密度極大点３９から平面３３の法線ベクトル３９ａに沿って所定量離れた点にハンド位置姿勢３８の位置を設定し、平面３３および法線ベクトル３９ａに平行な方向（矢印３８ａ，３８ｂ）にハンド位置姿勢３８の姿勢を設定したが（図１０）、極大位置演算部１３２により演算された密度極大位置に基づいてハンド位置姿勢３８を設定するのであれば、上述したのとは別の位置および姿勢にハンド位置姿勢３８を設定してもよく、ハンド位置演算部１３３の構成は上述したものに限らない。上記実施形態では、密度極大位置にある物品２０を取出し可能なハンド１４の位置姿勢をハンド位置姿勢３８としたが、密度極大位置の近傍にある物品２０を取出し可能な位置姿勢をハンド位置姿勢３８としてもよい。

上記実施形態では、ハンド１４の軸線Ｌ０および基準点１４ｄをハンド位置姿勢３８に一致させるようにロボット１４を制御したが（ステップＳ１０）、ハンド位置姿勢演算部１３３により演算されたハンド位置姿勢３８へハンド１４を移動して物品２０を取出すようにロボット１２を制御するのであれば、ロボット制御部１３４の構成はいかなるものでもよい。ハンド１４は物品２０の形状に応じて適宜変更されるものであり、本発明は、上述した以外の種々の形状の物品２０に対し適用可能である。

上記実施形態では、複数の三次元点３１のうち、互いに近傍にある三次元点３１を連結してなる連結集合３２を設定した（ステップＳ２）。この連結集合３２は、極大位置演算部１３２により演算される密度極大位置を含む領域内、すなわち単一の物品３２の表面上の閉じた領域内における三次元点３１によって構成される集合に相当するが、互いに近傍にある三次元点３１によって構成される集合は、連結集合３２以外であってもよく、集合設定部１３５の構成は上述したものに限らない。

なお、バラ積みされた複数の物品３１の表面位置を三次元測定機１１で測定して、複数の三次元点３１の位置情報を取得し、三次元測定機１１により測定された位置情報に基づき、三次元空間における複数の三次元点３１の分布の程度を表す密度分布を演算し、演算された密度分布に基づき、密度が極大となる密度極大位置を演算し、演算された密度極大位置に基づき、密度極大位置またはその近傍にある物品２０を取出し可能なハンド１４の位置及び姿勢であるハンド位置姿勢３８を演算し、演算されたハンド位置姿勢３８へハンド１４を移動して物品２０を取出すようにロボット１２を制御するのであれば、三次元空間にバラ積みされた複数の物品２０を取出す物品取出方法の構成はいかなるものでもよい。

以上の説明はあくまで一例であり、本発明の特徴を損なわない限り、上述した実施形態及び変形例により本発明が限定されるものではない。上記実施形態及び変形例の構成要素には、発明の同一性を維持しつつ置換可能かつ置換自明なものが含まれる。すなわち、本発明の技術的思想の範囲内で考えられる他の形態についても、本発明の範囲内に含まれる。また、上記実施形態と変形例の１つまたは複数を任意に組み合わせることも可能である。

１０ 物品取出装置
１１ 三次元測定機
１２ ロボット
１３ ロボット制御装置
１４ ハンド
３１ 三次元点
３２ 連結集合
３３ 平面
３４ 投影点
３８ ハンド位置姿勢
１３１ 密度演算部
１３１Ａ 面演算部
１３１Ｂ 投影点演算部
１３２ 極大位置演算部
１３３ ハンド位置演算部
１３４ ロボット制御部
１３５ 集合設定部

Claims (6)

1. 物品を保持可能なハンドを有するロボットと、
   三次元空間にバラ積みされた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、
   前記三次元測定機により測定された位置情報に基づき、三次元空間における前記複数の三次元点の分布の程度を表す密度分布を演算する密度演算部と、
   前記密度演算部により演算された密度分布に基づき、密度が極大となる密度極大位置を演算する極大位置演算部と、
   前記極大位置演算部により演算された密度極大位置に基づき、該密度極大位置またはその近傍にある物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢を演算するハンド位置姿勢演算部と、
   前記ハンド位置姿勢演算部により演算されたハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御するロボット制御部と、
   を備えることを特徴とする物品取出装置。
2. 請求項１に記載の物品取出装置において、
   前記密度演算部は、
   前記三次元測定機により取得された複数の三次元点を近似する平面または曲面を演算する面演算部と、
   前記面演算部により演算された平面または曲面上に前記複数の三次元点を投影した投影点の位置を演算する投影点演算部と、を有し、
   前記平面または曲面上の密度計測点から所定距離内にある前記投影点の個数を求め、該個数に応じて前記密度計測点における投影点の密度を求め、該密度を用いて前記密度分布を演算することを特徴とする物品取出装置。
3. 請求項１または２に記載の物品取出装置において、
   前記複数の三次元点のうち、互いに近傍にある三次元点によって構成される集合を設定する集合設定部をさらに備え、
   前記密度演算部は、前記集合に含まれる三次元点を用いて、前記集合毎に前記密度分布を演算し、
   前記極大位置演算部は、前記集合毎に密度極大位置を演算することを特徴とする物品取出装置。
4. 請求項３に記載の物品取出装置において、
   前記集合は、前記複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合であることを特徴とする物品取出装置。
5. 請求項１〜４のいずれか１項に記載の物品取出装置において、
   前記極大位置演算部により演算される密度極大位置を含む領域内における三次元点によって構成される集合を設定する集合設定部をさらに備え、
   前記密度演算部は、前記集合に含まれる三次元点の位置情報に基づき、前記集合を代表する代表位置を演算し、
   前記ハンド位置姿勢演算部は、前記密度演算部で演算された代表位置と該代表位置の周囲の三次元点の位置情報とに基づき、前記ハンド位置姿勢を演算することを特徴とする物品取出装置。
6. 物品を保持可能なハンドを有するロボットを用いて、三次元空間にバラ積みされた複数の物品を取出す物品取出方法であって、
   前記バラ積みされた複数の物品の表面位置を三次元測定機で測定して、複数の三次元点の位置情報を取得し、
   前記三次元測定機により測定された位置情報に基づき、三次元空間における前記複数の三次元点の分布の程度を表す密度分布を演算し、
   演算された密度分布に基づき、密度が極大となる密度極大位置を演算し、
   演算された前記密度極大位置に基づき、該密度極大位置またはその近傍にある物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢を演算し、
   演算された前記ハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御することを特徴とする物品取出方法。

Images