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

Description

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

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

特許文献１記載の装置では、予め基準三次元相対姿勢にある物品を撮像して得られた二次元画像から二次元モデルパターンを作成し、二次元モデルパターンに二次元の幾何学的変換を施して複数の変換二次元モデルパターンを作成し、複数の変換二次元モデルパターンを使用して物品の二次元画像に対して二次元パターンマッチングを行なう。

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

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

しかしながら、特許文献１、２に記載の装置は、予め物品一品種毎に二次元モデルパターンまたは三次元モデルパターンを作成する必要があり、手間を要する。とくに、物品が多品種の場合には、品種数分のモデルパターンを作成する必要があり、多大な手間が必要となる。また、不定形の物品の場合には、そもそもモデルパターンを作成することができず、適用不可能である。

本発明による物品取出装置は、物品を保持可能なハンドを有するロボットと、三次元空間にバラ積みされた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、三次元点測定機によって取得された複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求める連結集合演算手段と、連結集合に属する三次元点の位置情報に基づき、連結集合を代表する位置及び姿勢である代表位置姿勢を求めて物品の位置及び姿勢を特定する物品特定手段と、物品特定手段により位置及び姿勢が特定された物品を取出し可能なハンドの位置及び姿勢であるハンド位置姿勢であって、代表位置姿勢に対応するハンド位置姿勢を求めるハンド位置姿勢演算手段と、ハンド位置姿勢演算手段により求められたハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御するロボット制御手段と、を備えることを特徴とする。

また、本発明は、物品を保持可能なハンドを有するロボットを用いて、三次元空間にバラ積みされた物品を取出す物品取出方法であって、バラ積みされた複数の物品の表面位置を三次元測定機で測定して複数の三次元点の位置情報を取得し、三次元点測定機によって取得された複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求め、連結集合に属する三次元点の位置情報に基づき、連結集合を代表する位置及び姿勢である代表位置姿勢を求めて物品の位置及び姿勢を特定し、位置及び姿勢が特定された物品を取出し可能なハンドの位置及び姿勢であるハンド位置姿勢であって、代表位置姿勢に対応するハンド位置姿勢を求め、ハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御する、ことを特徴とする。

本発明によれば、三次元空間にバラ積みされた物品の位置及び姿勢を認識する際に、三次元点測定機によって取得された複数の三次元点の位置情報から連結集合を求め、連結集合を用いて物品の位置及び姿勢を特定する。したがって、物品のモデルパターンを作成することなく物品の位置及び姿勢を認識することができ、多品種や不定形の物品であっても容易にその位置及び姿勢を認識して、物品を保持することができる。特に、平面状或いは緩やかな曲面状の面を持つ物品の位置及び姿勢の認識と保持に適する。

本発明の一実施形態に係る物品取出装置の概略構成を示す図である。 図１のロボット制御装置で実行される処理の一例を示すフローチャートである。 図１の三次元測定機で取得した三次元点集合の一例を示す図である。 図３の三次元点集合から求めた連結集合の一例を示す図である。 図４の各連結点集合に属する三次元点の位置に基づく代表位置姿勢の一例を示す図である。 図５の代表位置姿勢に対応するハンド位置姿勢の一例を示す図である。 図６のハンド位置姿勢の番号付けの一例を示す図である。 図１の物品取出装置を構成するハンドの保持部に対する番号付けの一例を示す図である。 本発明の実施形態に係る物品取出装置の動作の一例を示す図である。 図９に続く動作の一例を示す図である。 図１０に続く動作の一例を示す図である。 図１１に続く動作の一例を示す図である。 図１２に続く動作の一例を示す図である。 図２の連結集合を求める処理の詳細を示すフローチャートである。 連結集合を説明する概念図である。

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

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

ハンド１４は、不図示の昇降機構により昇降可能な一対の保持部１４ａを有する。保持部１４ａは、通常は図１に示すように上昇状態にある。この上昇状態から下降して物品２０を保持した後に、再度上昇する。これにより、保持物品２１が他の物品２０よりも上方（保持部１４ａの上昇する方向）に取出され、ロボット１２の動作により保持物品２１を搬送する際、保持物品２１やハンド１４とコンテナ１６内の他の物品２０との衝突を避けることができる。

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

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

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

ハンド１４は、物品２０を取出し、かつ、保持することができるが、それが可能なハンドの形態としては、例えば吸引ノズル、吸着用磁石、吸着パッドまたはチャックなどが挙げられる。ハンド１４は、ロボット１２の動作により、その位置姿勢が制御される。

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

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

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

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

三次元的測定機１１によりバラ積みされた物品２０の平面が測定される場合、同一物品２０上における隣り合う三次元点３１（例えば図４の３１ａ，３１ｂ）は互いに近距離に位置する。これに対し、物品２０の境界部では隣り合う三次元点（例えば図４の３１ｂ，３１ｃ）の位置が大きく変化する。したがって、三次元点３１ａ，３１ｂは同一の連結集合３２に属するのに対し、三次元点３１ｂ，３１ｃは互いに異なる連結集合３２に属することになり、連結集合３２により物品２０を特定することができる。なお、連結集合３２を求める具体的処理については、後述する（図１４）。

次に、同一の連結集合３２に属する三次元点３１の位置に基づき、各々の連結集合３２を代表する代表位置姿勢３３を求める（ステップＳ３）。連結集合３２は物品２０の露出する表面を特定するものであるので、代表位置姿勢３３は、物品２０を代表する位置及び姿勢となる。すなわち、ステップＳ３は、連結集合３２を用いて物品２０の位置及び姿勢を特定する処理であり、これにより物品２０の位置及び姿勢を認識することができる。なお、物品２０の位置及び姿勢は、三次元測定機１１によって取得した三次元点３１の位置情報に基づいて特定される。すなわち、物品２０の露出部を計測して得られる三次元点３１に基づき、計算により求められる。このため、物品２０の配置が同じでも、露出状況が異なると、特定される物品２０の位置及び姿勢は異なる。つまり、物品２０の位置及び姿勢は、物品２０の配置に対応して一義的に特定されるのではなく、物品２０の表面の露出状況に応じて特定される。

図５は、連結集合３２に属する三次元点３１の位置に基づいて計算した代表位置姿勢３３の一例を表す示す図である。代表位置姿勢３３は、直角に交差する一対の矢印３３ａ，３３ｂで示されているが、これは代表位置姿勢３３が直交座標系で表されるためである。図５では、代表位置姿勢３３が２つの矢印３３ａ，３３ｂで示されているが、代表位置姿勢３３は二次元空間におけるものではなく、三次元空間におけるものである。

代表位置姿勢３３の求め方には、幾通りかの方法がある。まず、１つ目の例として、連結集合３２に属する三次元点３１の重心位置と所定の姿勢（例えば矢印３３ａを鉛直方向上方に向けた姿勢）とを組合せて代表位置姿勢３３とする方法がある。重心位置の計算には、連結集合３２に属する全ての三次元点３１を利用しても良いし、別途、外れ値対策等の処理を導入して、選別した三次元点３１を利用しても良い。外れ値対策としては、例えば、始めに連結集合３２に属する全ての三次元点３１を重心計算に利用して重心位置を求め、重心計算に利用した三次元点３１で重心位置との距離が所定距離以上である三次元点３１が存在する場合、重心計算に利用した三次元点３１から重心位置との距離が大きい順に所定割合の三次元点３１を除去する。そして残った三次元点３１を重心計算に利用して重心位置を再計算する。この処理を、重心計算に利用した全ての三次元点３１が重心位置から所定距離内に収まるまで繰り返せば良い。

２つ目の例として、連結集合３２に属する三次元点３１の位置に基づいて平面を求め、その平面上の１つの点（例えば重心となる点）の位置とその平面の法線方向に基づく姿勢とを組合せて代表位置姿勢３３とする方法がある。平面は、連結集合３２に属する全ての三次元点３１を用いて最小二乗法で求めてもよいし、別途、何らかの外れ値対策の処理を導入して求めてもよい。外れ値対策の方法としては、M推定法、RANSAC、LMedS、ハフ変換など、幾通りかの方法がある。ハフ変換などの処理を入れることで、連結集合３２が、複数の物品２０の表面に跨っている場合でも、その中から、一つの平面を抽出して認識することが可能になる。

連結集合３２が、複数の物品２０の表面に跨る場合とは、物品２０の境界部における三次元点３１（例えば図４の３１ｂ，３１ｃ）が互いに近距離に位置する場合であり、この場合には三次元点３１ｂ，３１ｃが同一の連結集合３２に属する。なお、ハフ変換を行って連結集合３２内の複数の三次元点３１が同一平面上にあるか否かを判定し、互いに異なる平面上にあると判定されると、連結集合３２を各平面に対応した連結集合（例えば三次元点３１ｂを含む連結集合３２と三次元点３１ｃを含む連結集合３２）に分割するようにしてもよい。これにより連結集合３２が修正され、物品２０毎に精度よく連結集合３２を求めることができる。

３つ目の例として、連結集合３２に属する三次元点３１の位置に基づいて曲面を求め、その曲面上の１つの点（例えば重心に最も近い曲面上の点）の位置とその位置における曲面の法線方向とに基づく姿勢とを組合せて代表位置姿勢３３とする方法がある。曲面は、連結集合３２に属する全ての三次元点３１を用いて最小二乗法で求めてもよいし、別途、何らかの外れ値対策の処理を導入して求めてもよい。外れ値対策の方法としては、M推定法、RANSAC、LMedS、ハフ変換など、幾通りかの方法がある。ハフ変換などの処理を用いることで、連結集合３２が、複数の物品の表面に跨っている場合でも、その中から、一つの曲面を抽出して認識することが可能になる。

４つ目の例として、連結集合３２に属する各三次元点３１の位置に基づいて穴を求め、その穴の位置及び姿勢（穴が穿設される方向）を代表位置姿勢３３とする方法がある。この方法は、物品２０に穴が設けられている場合に有効である。一例を示せば、まず、連結集合３２内に、所定面積以上の三次元点３１が存在しない閉じた領域（三次元点不存在領域）があるか否かを判定し、三次元点不存在領域があると判定されると、その三次元点不存在領域が穴であると仮定する。次に、穴の縁にある三次元点３１の重心位置を求め、これを穴の位置とする。さらに、穴の縁にある三次元点３１の位置に基づいて平面を求め、その平面の法線方向に基づく姿勢を、穴の姿勢とする。

図５は、上記４つの例のうち、２つ目の例によって代表位置姿勢３３を求めた場合を示している。ステップＳ３では、連結集合３２に属する三次元点３１が存在する領域の面積を演算し、面積が所定値より小さい連結集合３２（図４の３２ａ）からは、代表位置姿勢３３を取得しない。すなわち、表面が十分に露出していると判断される連結集合３２に対してのみ、代表位置姿勢３３を求める。代表位置姿勢３３を求めていない連結集合３２ａは、物品取出しの対象としない。これにより、連結集合３２ａによって表される物品２０、すなわち上面の一部が他の物品２０で覆われている奥側の物品２０にハンド１４が接近することを防止でき、ハンド１４と取出し対象以外の物品２０とが衝突することを抑制できる。

次に、各々の代表位置姿勢３３に対応するハンド位置姿勢３４を求める（ステップＳ４）。図６は、代表位置姿勢３３に対応するハンド位置姿勢３４の一例を表す示す図である。ハンド位置姿勢３４は、代表位置姿勢３３と同様に、直角に交差する一対の矢印３４ａ，３４ｂで示されている。

ハンド位置姿勢３４の位置（矢印３４ａ，３４ｂの交点）及び姿勢（矢印３４ａ，３４ｂの方向）の求め方には、それぞれ幾通りずつかの方法がある。位置に関しては、例えば、代表位置姿勢３３の位置をそのままハンド位置姿勢３４の位置とする方法がある。別の例として、代表位置姿勢３３の位置よりも所定の座標軸３５（例えばＺ軸）の向きに沿って所定長移動した位置を、ハンド位置姿勢３４の位置とする方法もある。図６は、後者の例で位置を示している。姿勢に関しては、例えば、代表位置姿勢３３の姿勢をそのままハンド位置姿勢３４の姿勢とする方法がある。別の例として、代表位置姿勢３３の位置がコンテナ１６の壁に近い場合、壁とハンドとの衝突を避ける目的で壁から離れる方向にハンドを傾けるようにする方法もある。図６は、前者の例で姿勢を示している。

次に、各々のハンド位置姿勢３４にＰ１，Ｐ２，・・・，Ｐｎとナンバリングする（ステップＳ５）。但し、ｎはハンド位置姿勢３４の個数である。図７は、ナンバリングされたハンド位置姿勢３４を示す図であり、所定の座標軸３５に対する座標値の降順、つまり、高い位置にあるものから順番にナンバリングされている。なお、図７では、ｎ＝３である。

次に、ハンド１４の保持部１４ａにＨ１，Ｈ２，・・・，Ｈｍとナンバリングする（ステップＳ６）。但し、ｍは保持部１４ａの個数である。図８は、ナンバリングされた保持部１４ａを示しており、図８では、ｍ＝２である。

次に、自然数値を取る変数ｊ，ｋに対して初期値を与える。すなわち、ｊ←１，ｋ←１とする（ステップＳ７）。変数ｊは保持部１４ａの番号の指定に用い、変数ｋはハンド位置姿勢３４の番号の指定に用いる。

次に、図９に示すように、保持部１４ａ駆動用のアクチュエータ（電動モータあるいはシリンダ）に制御信号を出力し、保持部Ｈｊを下降させるとともに、ロボット駆動用のアクチュエータ（電動モータ）に制御信号を出力し、ロボット１２の動作により保持部Ｈｊをハンド位置姿勢Ｐｋへ移動させる（ステップＳ８）。変数の初期値ｊ＝１，ｋ＝１に対しては、Ｈｊ＝Ｈ１，Ｐｋ＝Ｐ１である。

次に、保持部Ｈｊ駆動用のアクチュエータに物品２０を保持するための制御信号を出力し、図１０に示すように保持部Ｈｊの下端面で保持物品２１を保持する（ステップＳ９）。例えば、保持部Ｈｊが吸引ノズルを有する場合、真空ポンプを作動し、吸引力で保持物品２１を吸引、保持する。保持部Ｈｊが吸着用磁石を有する場合、電磁コイルに電流を流して磁石を作動し、磁力で保持物品２１を吸着、保持する。保持部Ｈｊがチャックである場合、チャックを開くあるいは閉じることで、保持物品２１を保持する。

次に、図１１に示すように、保持部Ｈｊ駆動用のアクチュエータに制御信号を出力し、保持物品２１を保持した保持部Ｈｊを上昇させる（ステップＳ１０）。保持部Ｈｊを上昇させることで、ロボット１２の動作によりハンド１４を移動させる際、保持物品２１や保持部Ｈｊと他の物品２０との衝突を避けることができる。

次に、保持部１４ａによる保持物品２１の保持が成功したか否かを判定する（ステップＳ１１）。この判定は、例えば、保持部１４ａが吸引ノズルを有する場合、吸引時のエアの流量や圧力の変化に応じて保持が成功したか否かを判定すればよい。保持部Ｈｊが吸着用磁石を有する場合、近接センサで保持物品２１が存在するか否かを判定し、その存否に応じて保持が成功したか否かを判定すればよい。保持部Ｈｊがチャックを有する場合、開閉確認センサでチャックの開閉状態を確認し、保持が成功したか否かを判定すればよい。保持が成功したと判定されると、ステップＳ１２へ進む。保持が成功していないと判定されると、ステップＳ１４へ進む。

ステップＳ１２では、ｊ＜ｍか否かを判定する。この判定は、ｍ個（図１１では２個）の保持部１４ａの中に保持物品２１を未だ保持していないものが存在するか否かの判定である。ｊ＜ｍと判定されると、保持部Ｈｊ＋１が保持物品２１を未だ保持していないので、ｊ←ｊ＋１とし(ステップＳ１３)、ステップＳ１４へ進む。ｊ＜ｍでないと判定されると、全ての保持部１４ａが保持物品２１を保持しているので、ステップＳ１６へ進む。

ステップＳ１４では、ｋ＜ｎか否かを判定する。この判定は、ｎ個（図１１では３個）のハンド位置姿勢３４の中に保持部１４ａが未だ到達していないものが存在するか否かの判定である。ｋ＜ｎと判定されると、ハンド位置姿勢Ｐｋ＋１に保持部１４ａが未だ到達していないので、ｋ←ｋ＋１とし（ステップＳ１５）、ステップＳ８へ戻る。そして、図１２に示すように次の保持部Ｈｊを下降させながらロボット１２の動作により次のハンド位置姿勢Ｐｋへ移動させる。なお、図１２は、Ｈｊ＝Ｈ２，Ｐｋ＝Ｐ２の例である。次いで、この保持部Ｈｊに物品吸着用の制御信号を出力し、図１３に示すように、次の保持物品２１を保持する（ステップＳ９）。ステップＳ１４でｋ＜ｎでないと判定されると、ｎ個のハンド位置姿勢３４の全てに保持部１４ａが到達したので、ステップＳ１６へ進む。

ステップＳ１６では、ロボット駆動用アクチュエータに制御信号を出力し、ハンド１４を所定位置に移動する。これにより保持物品２１がロボット１２の動作により所定位置に搬送される。その後、保持部駆動用アクチュエータに制御信号を出力し、保持物品２１を保持部１４ａから取外す。以上で、１サイクルの処理が終了となる。なお、未到達のハンド位置姿勢３４があるにも拘わらず（ｋ＜ｎ）、全ての保持部１４ａで保持物品２１を保持している場合（ｊ≧ｍ）、ステップＳ１６で保持物品２１を所定位置に搬送して保持部１４ａから取外した後に、保持部１４ａの番号ｊを初期値１にして、ステップＳ８以降の処理が繰り返される。

ここで、連結集合３２（ステップＳ２）を求めるための具体的処理について説明する。図１４は、連結集合３２を求めるための処理（連結集合演算処理）の一例を示すフローチャートである。

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

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

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

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

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

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

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

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

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

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

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

以上の処理では、連結集合３２を求める際に、近傍判定部としての機能を有するロボット制御装置１３内での処理により、三次元点３１の近傍に他の三次元点３１が存在するか否かを判定するようにした（ステップＳ２８）。近傍判定部では、例えば、予めＸＹＺの三次元の座標系に対応して所定距離ａを定め、三次元点集合３０に属する第１三次元点３１（例えば図１５の３１１）と第２三次元点３１（例えば図１５の３１２）との間の距離Ｄが所定距離ａ以内である場合に、第１三次元点３１１の近傍に第２三次元点３１２が存在すると判定すればよい。ＸＹＺの三次元の座標系ではなく、ＸＹの二次元の座標系とＺの一次元の座標系に対応してそれぞれ所定距離ｘｙａ，ｚａを定め、ＸＹの二次元の座標系とＺの一次元の座標系に関する三次元点３１１，３１２間の距離Ｄｘｙ，Ｄｚを求め、この距離Ｄｘｙ，Ｄｚがそれぞれ予め定めた所定値ｘｙａ，ｚａ以内のときに、三次元点３１１の近傍に三次元点３１２が存在すると判定してもよい。

三次元点集合３０が距離画像の形で表され、距離画像の各画素が縦横等間隔に並んでいるとき、近傍判定部は、例えば以下のようにして三次元点同士の近傍判定を行うようにしてもよい。すなわち、距離画像の各画素の明るさの尺度となる１次元座標系を定義し、１次元座標系に対し、所定距離に対応して明るさの差を表す閾値を定義する。第１三次元点と第２三次元点が距離画像上で隣接する画素であり、かつ、対応する画素間の明るさの差が閾値以内のときに、第１三次元の近傍に第２三次元点があると判定すればよい。

本実施形態によれば、以下のような作用効果を奏することができる。
（１）三次元空間にバラ積みされた物品２０の表面位置を三次元測定機１１で測定して複数の三次元点３１の位置情報を取得するようにした。さらに、ロボット制御装置１３での処理により、三次元点集合３０から互いに近傍にある三次元点３１を連結してなる連結集合３２を求め（ステップＳ２）、連結集合３２に属する三次元点３１の位置情報に基づき、物品２０を取出可能なハンド１４（保持部１４ａ）の位置姿勢（ハンド位置姿勢３４）を求め（ステップＳ３〜ステップＳ４）、求められたハンド位置姿勢３４にハンド１４を移動してコンテナ内から物品２１を取出すようにロボット１２を制御した（ステップＳ８〜ステップＳ１０）。

このようにして求められた連結集合３２は、三次元測定機１１に向けて露出している物品表面の位置及び姿勢（傾き）を反映するものであり、パターンマッチング等によらず物品２０の位置及び姿勢を特定することができる。このため、物品２０のモデルパターンを作成する必要がなく、多品種や不定形の物品２０であっても容易にその位置及び姿勢を認識して、物品２０を保持することができる。また、追加された新品種の物品２０に対してもモデルパターンを追加無しでその位置及び姿勢を認識することができ、物品２０の位置姿勢の認識失敗や誤認識、或いは物品２０の取損ねや衝突等の問題を回避し、上方にある取出し易い物品２０へハンド１４を高速に移動させて物品２０を効率良く取出すことができる。

これに対し、例えば、物品の二次元モデルパターンを用いて二次元パターンマッチングにより物品の位置を認識する方法では、二次元モデルパターンを作成する必要があり、手間を要する。とくに、物品が多品種の場合には、品種数分のモデルパターンを作成する必要があり、多大な手間が必要となる。さらに、この方法には、本実施形態と比較した場合に以下のような問題点がある。バラ積みされた物品２０は様々な姿勢となり、各姿勢に対応した二次元パターンマッチングを行う場合、例えば二次元の幾何学的変換を施して得られる変換二次元モデルパターンを利用する必要があるが、この方法で対応できるのは、平面的な物品に限られる。通常の立体的な物品に二次元パターンマッチングを適用しようとすると、幾何学的変換では補い切れない見え方の多様性を考慮して、物品一品種についてだけでも表・裏・横・斜めと幾つかの方向から二次元モデルパターンを作成する必要が生じる。さらに、多品種への対応を考慮すると、品種数を乗じた膨大な数の二次元モデルパターンを作成する必要が生じ、多大な手間を要する。また、不定形の物品の場合には、二次元モデルパターンを作成できないので、その位置を認識することはできない。バラ積みされた物品２０は、照明の当たり具合が悪かったり他の物品２０の影が写ったり物品２０が想定以上に傾いたりするため、撮像条件が悪い。したがって、物品２０の位置姿勢の認識失敗や誤認識が発生したり、上方に位置する取出すべき物品の位置の認識に失敗したり、下方に位置する物品の位置が先に認識されたりするおそれがある。誤認識された物品位置姿勢や下方にある物品位置を目掛けてロボット１２のハンド１４の位置姿勢を制御すると、物品２０を取損ねて装置の作業効率が落ちるばかりか、ハンド１４と物品２０とが衝突してそれらを損傷させるおそれがある。損傷を回避しようとすると、ロボット１２の移動速度を遅くせざるを得ず、作業効率が悪化する。

また、例えば、物品の三次元モデルパターンを用いて三次元パターンマッチングにより物品の位置を認識する方法においても、三次元モデルパターンを作成する必要があり、手間を要する。とくに、物品が多品種の場合には、品種数分のモデルパターンを作成する必要があり、多大な手間が必要となる。さらに、この方法には、本実施形態と比較した場合に以下のような問題点がある。不定形の物品は、三次元モデルパターンを作成できないので、その位置を認識することはできない。バラ積みされた物品２０は、三次元測定機１１に向いていない側の三次元点３１を取得でない上に、大きく傾いたり近隣の物品に邪魔されたりするため、撮像条件が悪い。このため、三次元パターンマッチングにより物品の三次元姿勢を決定できるほど量・質ともに十分な三次元点集合を得るのは困難であり、物品の位置姿勢の認識失敗や誤認識が発生したり、上方に位置する取出すべき物品の位置の認識に失敗し、下方に位置する物品の位置が先に認識されたりするおそれがある。誤認識された物品位置姿勢や下方にある物品位置を目掛けてロボット１２のハンド１４の位置姿勢を制御すると、物品２０を取損ねて装置の作業効率が落ちるばかりか、ハンド１４と物品２０とが衝突してそれらを損傷させるおそれがある。損傷を回避しようとすると、ロボット１２の移動速度を遅くせざるを得ず、作業効率が悪化する。

（２）ロボット制御装置１３での処理により、三次元点集合３０に属する任意の三次元点３１（第１三次元点）とこれとは別の三次元点３１（第２三次元点）との間の所定の座標系（例えばＸＹＺ座標系）に関する距離Ｄが、予め定められた所定距離ａ以内にあるか否かを判定し、所定距離ａ以内にあると判定されると、第１三次元点３１と第２三次元点３１に同一のラベル番号ｊを割り振ってこれらを連結するようにした（ステップＳ２８、ステップＳ２９）。したがって、各物品２０の表面の位置姿勢を表す連結集合３２を、容易かつ精度よく作成することができる。また、三次元点集合３０から抽出されたエッジによって囲まれた部分領域に三次元点集合３０を分割する手法に比べ、三次元点集合３０のデータに欠損がある場合においても、安定して物品２０を保持可能な面の位置及び姿勢を認識することができる。

（３）ロボット制御装置１３での処理により、連結集合３２に属する三次元点３１の位置に基づき、連結集合３２を代表する位置及び姿勢である代表位置姿勢３３を求め、この代表位置姿勢３３に対応してハンド位置姿勢３４を求めるようにした（ステップＳ３、ステップＳ４）。これにより、物品２０とハンド１４との位置関係を、ハンド１４の種類等に応じて適切に設定することができる。

（４）ロボット制御装置１３での処理（ステップＳ３）により、連結集合３２に属する三次元点３１の重心位置とこの重心位置における所定の姿勢とを組合せて代表位置姿勢を求めるようにすると、保持物品２１をハンド１４で安定して保持することができる。連結集合３２に属する三次元点３１の位置に基づいて平面または曲面を求め、求めた平面または曲面上の１つの点の位置と平面または曲面の法線方向に基づく姿勢とを組合せて代表位置姿勢３３を求めるようにすると、物品２０の表面が傾いている場合に、その傾きに合わせて代表位置姿勢３３を算出することができる。

（５）表面に穴を有する物品２０に対しては、連結集合３２に属する三次元点３１の位置に基づいて穴を求め、その穴の位置及び姿勢（穴の方向）を代表位置姿勢３３とすることで、保持部１４ａがチャックであるハンド１４を用い、内径把持にて物品２０を保持することができる。

なお、バラ積みされた複数の物品２０の表面位置を三次元測定機１１で測定して複数の三次元点３１の位置情報を取得し、複数の三次元点点３１の中から、互いに近傍にある三次元点３１を連結してなる連結集合３２を求め、連結集合３２に属する三次元点３１の位置情報に基づき、取出し可能な物品２０の位置及び姿勢を特定し、この位置姿勢に対応したハンド位置姿勢３４を求め、ハンド位置姿勢３４にハンド１４を移動して物品２０（保持物品２１）を取出すようにロボット１２を制御するのであれば、物品取出方法の構成はいかなるものでもよい。

上記実施形態では、ハンド１４に２つの保持部１４aを設けるようにしたが、保持部１４ａを３個以上設けてもよく、１つだけ設けるようにしてもよい。三次元点集合３０に属する第１三次元点３１（例えば図１５の３１１）と第２三次元点３１（例えば図１５の３１２）との間の所定の座標系に関する距離が所定距離内にあるときに、第１三次元点３１１と第２三次元点３１２を連結して連結集合３２を演算するようにしたが（ステップＳ２）、互いに近傍にある三次元点３１，３１を連結してなる連結集合３２を求めるのでれば、連結集合演算手段としてのロボット制御装置１３の構成は上述したものに限らない。

連結集合３２に属する三次元点３１の位置情報に基づき、連結集合３２を代表する代表位置姿勢３３を演算するようにしたが（ステップＳ３）、連結集合３２によって表される物品２０の位置及び姿勢を特定するのであれば、物品特定手段としてのロボット制御装置１３の構成は上述したものに限らない。ここで、連結集合３２を代表する位置及び姿勢は、物品２０を代表する位置及び姿勢でもあり、物品２０の位置及び姿勢を特定するとは、物品を代表する位置及び姿勢を求めて物品２０の配置を特定することを意味する。

上記実施形態では、代表位置姿勢３３からハンド位置姿勢３４を演算するようにしたが（ステップＳ４）、代表位置姿勢３３として特定された物品２０を取出し可能なハンド位置姿勢３４を求めるのであれば、ハンド位置姿勢演算手段としてのロボット制御装置１３の構成は上述したものに限らない。ハンド位置姿勢３４へハンド１４を移動して物品２０を取出すようにロボット１２を制御するのであれば、ロボット制御手段としてのロボット制御装置１３の構成はいかなるものでもよい。

すなわち、上述した物品取出装置１０の構成（図１）は一例であり、連結集合演算手段（ステップＳ２）、物品特定手段（ステップＳ３）、ハンド位置姿勢演算手段（ステップＳ４））、ロボット制御手段（ステップＳ８〜ステップＳ１０）としてのロボット制御装置１３の構成は上述したものに限らない。

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

１０ 物品取出装置
１１ 三次元測定機
１２ ロボット
１３ ロボット制御装置
１４ ハンド
１４ａ 保持部
１６ コンテナ
２０ 物品
２１ 保持物品
３０ 三次元点集合
３１ 三次元点
３２ 連結集合
３３ 代表位置姿勢
３４ ハンド位置姿勢
３５ 座標軸

Claims (8)

1. 物品を保持可能なハンドを有するロボットと、
   三次元空間にバラ積みされた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、
   前記三次元点測定機によって取得された複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求める連結集合演算手段と、
   前記連結集合に属する三次元点の位置情報に基づき、前記連結集合を代表する位置及び姿勢である代表位置姿勢を求めて物品の位置及び姿勢を特定する物品特定手段と、
   前記物品特定手段により位置及び姿勢が特定された物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢であって、前記代表位置姿勢に対応するハンド位置姿勢を求めるハンド位置姿勢演算手段と、
   前記ハンド位置姿勢演算手段により求められたハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御するロボット制御手段と、
   を備えることを特徴とする物品取出装置。
2. 請求項１に記載の物品取出装置において、
   前記三次元測定機によって取得された複数の三次元点は三次元点集合を構成し、
   前記連結集合演算手段は、所定の座標系に関する、前記三次元点集合に属する第１三次元点と第２三次元点との間の距離が、予め定められた所定距離以内にあるか否かを判定する近傍判定部を有し、該近傍判定部により前記第１三次元点と前記第２三次元点との間の距離が前記所定距離以内にあると判定されると、前記第１三次元点と前記第２三次元点とを連結することを特徴とする物品取出装置。
3. 請求項１または２に記載の物品取出装置において、
   前記物品特定手段は、前記連結集合に属する三次元点の重心位置と該重心位置における所定の姿勢とを組合せて前記代表位置姿勢を求めることを特徴とする物品取出装置。
4. 請求項１または２に記載の物品取出装置において、
   前記物品特定手段は、前記連結集合に属する三次元点の位置に基づいて平面を求め、該平面上の１つの点の位置と該平面の法線方向に基づく姿勢とを組合せて前記代表位置姿勢を求めることを特徴とする物品取出装置。
5. 請求項１または２に記載の物品取出装置において、
   前記物品特定手段は、前記連結集合に属する三次元点の位置に基づいて曲面を求め、該曲面上の１つの点の位置と該位置における該曲面の法線方向に基づく姿勢とを組合せて前記代表位置姿勢を求めることを特徴とする物品取出装置。
6. 請求項１または２に記載の物品取出装置において、
   前記物品特定手段は、前記連結集合に属する三次元点の位置に基づいて、所定面積以上の三次元点が存在しない閉じた領域が穴であると仮定して穴を求めるとともに、該穴の位置及び姿勢を求め、これら穴の位置及び姿勢を組合せて前記代表位置姿勢を求めることを特徴とする物品取出装置。
7. 請求項１〜６のいずれか１項に記載の物品取出装置において、
   前記ハンドが、吸引ノズル又は電磁石又は吸着パッド又はチャックのいずれかを有することを特徴とする物品取出装置。
8. 物品を保持可能なハンドを有するロボットを用いて、三次元空間にバラ積みされた物品を取出す物品取出方法であって、
   前記バラ積みされた複数の物品の表面位置を三次元測定機で測定して複数の三次元点の位置情報を取得し、
   前記三次元点測定機によって取得された複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求め、
   前記連結集合に属する三次元点の位置情報に基づき、前記連結集合を代表する位置及び姿勢である代表位置姿勢を求めて物品の位置及び姿勢を特定し、
   前記位置及び姿勢が特定された物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢であって、前記代表位置姿勢に対応する前記ハンド位置姿勢を求め、
   前記ハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御する、ことを特徴とする物品取出方法。

Images