JP2015089588A - 三次元空間に置かれた物品をロボットで取出す装置及び方法 - Google Patents

Abstract

【課題】パターンマッチングやユーザの教示作業等によらずに、三次元空間に置かれた物品の位置及び姿勢を認識して物品を取出すことが可能な物品取出装置を提供する。【解決手段】物品取出装置１０は、連結集合３２を用いて物品２０の位置及び姿勢を認識する。この場合、三次元測定機１１によって取得された複数の三次元点３１の位置情報及びカメラ１２によって取得された画像データに基づき、複数の三次元点３１の中から互いに近傍にある第１および第２三次元点３１を選択し、これら三次元点３１の位置情報と画像データとに基づき、三次元点３１に対応する画像上の点を含む部分画像領域における画像勾配情報を取得し、三次元点３１の位置情報と画像勾配情報とに基づき第１および第２三次元点３１が同一物品上にあるか否かを判定し、同一物品上にあると判定されると、第１および第２三次元点３１を同一の連結集合に含める。【選択図】図１３

Description

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

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

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

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

特許文献３記載の装置では、カメラの撮像動作により物品を含む領域のカラー画像と距離画像とを取得し、取得した画像を表示部に表示する。この表示画像中の物品の一部をユーザが前景領域として教示すると、カラー画像の色情報と距離画像から得られる距離情報とに基づき、物品以外の領域を背景領域として設定し、画像中の物品を抽出する。

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

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

また、特許文献３に記載の装置は、画像中の物品を抽出するためにユーザの教示作業が必要であるため、手間を要する。とくに、物品が多品種の場合には、品種数分の教示作業が必要であり、多大な手間が必要となる。

本発明の一態様である物品取出装置は、物品を保持可能なハンドを有するロボットと、三次元空間に置かれた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、複数の物品を含む領域を撮像し、画像データを取得するカメラと、三次元測定機によって取得された複数の三次元点の位置情報及びカメラによって取得された画像データに基づき、複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求める連結集合演算部と、連結集合に属する三次元点の位置情報に基づき、物品の位置及び姿勢を特定する物品特定部と、物品特定部により特定された物品を取出し可能なハンドの位置及び姿勢であるハンド位置姿勢を求めるハンド位置姿勢演算部と、ハンド位置姿勢演算部により求められたハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御するロボット制御部と、を備える。連結集合演算部は、三次元測定機によって取得された複数の三次元点の中から、互いに近傍にある第１三次元点と第２三次元点とを選択する三次元点選択部と、カメラで取得された画像データに基づき、第１三次元点及び第２三次元点にそれぞれ対応する画像上の点を含む部分画像領域における、物品の表面の勾配状態を表す画像勾配情報を取得する勾配情報取得部と、第１三次元点の位置情報、第２三次元点の位置情報及び勾配情報取得部により取得された画像勾配情報に基づき、第１三次元点と第２三次元点とが同一物品上にあるか否かを判定する判定部とを有し、判定部により第１三次元点と第２三次元点とが同一物品上にあると判定されると、第１三次元点と第２三次元点とを同一の連結集合に含める。

また、本発明の別の態様は、物品を保持可能なハンドを有するロボットを用いて、三次元空間に置かれた物品を取出す物品取出方法であって、三次元空間に置かれた複数の物品の表面位置を三次元測定機で測定して複数の三次元点の位置情報を取得し、複数の物品を含む領域をカメラで撮像して画像データを取得し、三次元測定機によって取得された三次元点の位置情報及びカメラで取得された画像データに基づき、複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求め、連結集合に属する三次元点の位置情報に基づき、物品の位置及び姿勢を特定し、位置及び姿勢が特定された物品を取出し可能なハンドの位置及び姿勢であるハンド位置姿勢を求め、ハンド位置姿勢へハンドを移動して物品を取出すようにロボットを制御する。連結集合を求めるときには、三次元測定機によって取得された複数の三次元点の中から、互いに近傍にある第１三次元点と第２三次元点とを選択し、カメラで取得された画像データに基づき、第１三次元点及び第２三次元点にそれぞれ対応する画像上の点を含む部分画像領域における、物品の表面の勾配状態を表す画像勾配情報を取得し、第１三次元点の位置情報、第２三次元点の位置情報及び画像勾配情報に基づき、第１三次元点と第２三次元点とが同一物品上にあるか否かを判定し、第１三次元点と第２三次元点とが同一物品上にあると判定されると、第１三次元点と第２三次元点とを同一の連結集合に含める。

本発明によれば、三次元空間に置かれた物品の位置及び姿勢を認識する際に、カメラによって取得された画像データに基づき画像勾配情報を取得し、三次元測定機で測定された三次元点の三次元位置情報及び画像勾配情報を用いて複数の三次元点の連結集合を求め、連結集合を用いて物品の位置及び姿勢を特定する。したがって、物品のモデルパターンを作成することなく、かつ、ユーザの教示作業を要することなく、物品の位置及び姿勢を容易に認識することができる。

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

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

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

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

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

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

カメラ１２は、ＣＣＤ等の撮像素子を有し、パレット１６の中央部上方に配置され、パレット１６上に置かれた物品２０を撮像する。カメラ１２の撮影領域は、パレット１６を含む必要があるが、撮影領域が大き過ぎると撮像分解能の低下を招く。したがって、撮影領域は、パレット１６の占有範囲と同等、例えばパレット１６の占有範囲に一致させることが好ましい。なお、図１では、カメラ１２が専用の架台１７に固定されているが、ロボット１３の先端部にカメラ１２を取り付けてもよい。カメラ１２とロボット制御装置１４とは通信ケーブル等の通信部によって互いに接続されており、互いに通信できるようになっている。

カメラ１２は、予めキャリブレーションされており、キャリブレーションデータを用いることにより、三次元測定機１１によって測定された三次元点とカメラ１２によって撮像された画像上の点（二次元点）との対応関係を求めることができる。すなわち、三次元点がカメラ画像のどの点に当たるのかを求めることができ、これにより特定の三次元点に対応した画像データを得ることができる。

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

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

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

次に、複数の物品２０を含む領域をカメラ１２で撮像して画像４０を取得する（ステップＳ２）。図４は、カメラ１２で撮像された画像４０の一例を示す模式図である。図４には、パレット１６を表すパレット画像４１と物品２０を表す物品画像４２とが示されている。

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

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

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

本実施形態では、単一の物品２０に対し単一の連結集合３２が対応するように、すなわち、物品２０と連結集合３２とが１対１で対応するように連結集合３２を構築する。これにより、連結集合３２によって物品２０を特定することができる。この場合、三次元測定機１１からの測定データだけでなく、カメラ１２からの画像データを用いて連結集合３２を求めるが、この連結集合３２を求める具体的処理については、後述する（図１３）。

次に、同一の連結集合３２に属する三次元点３１の位置に基づき、各々の連結集合３２を代表する代表位置姿勢３３を求める（ステップＳ４）。連結集合３２は物品２０の露出する表面を特定するものであるので、代表位置姿勢３３は、物品２０を代表する位置及び姿勢となる。図７は、連結集合３２に属する三次元点３１の位置に基づいて計算した代表位置姿勢３３の一例を表す示す図である。代表位置姿勢３３は、直角に交差する一対の矢印３３ａ，３３ｂで示されているが、これは代表位置姿勢３３が直交座標系で表されるためである。なお、図７では、代表位置姿勢３３が２つの矢印３３ａ，３３ｂで示されているが、代表位置姿勢３３は二次元空間におけるものではなく、三次元空間におけるものである。

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

２つ目の例として、連結集合３２に属する三次元点３１に外接する長方形（外接長方形）を求め、外接長方形の中心の位置姿勢を代表位置姿勢３３とする方法がある。外接長方形を求めるためには、まず、連結集合３２に含まれる全ての三次元点３１を用いて三次元空間内において平面を求め、連結集合３２に含まれる全ての三次元点３１をこの平面上に投影する。次いで、投影された全ての三次元点３１を内部に含む凸多角形を計算し、計算された凸多角形に外接する長方形を求める。平面は、連結集合３２に属する全ての三次元点３１を用いて最小二乗法で求めてもよいし、別途、何らかの外れ値対策の処理を導入して求めてもよい。外れ値対策の方法としては、M推定法、RANSAC、LMedS、ハフ変換など、幾通りかの方法がある。凸多角形を計算する方法としては、Andrewのアルゴリズムなどの方法がある。凸多角形に外接する長方形を計算する方法としては、Rotating calipers法などがある。図７は、１つ目の例によって代表位置姿勢３３を求めた場合を示している。

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

ハンド位置姿勢３４の位置（矢印３４ａ，３４ｂの交点）及び姿勢（矢印３４ａ，３４ｂの方向）の求め方には、それぞれ幾通りずつかの方法がある。位置に関しては、例えば、代表位置姿勢３３の位置をそのままハンド位置姿勢３４の位置とする方法がある。別の例として、代表位置姿勢３３の位置よりも所定の座標軸３５（例えばＺ軸）の向きに沿って所定長さだけ移動した位置を、ハンド位置姿勢３４の位置とする方法もある。図８は、後者の例で位置を示している。姿勢に関しては、例えば、代表位置姿勢３３の姿勢をそのままハンド位置姿勢３４の姿勢とする方法がある。別の例として、代表位置姿勢３３の所定の座標軸３５周りに所定角度回転した姿勢をハンド位置姿勢３４の姿勢とする方法もある。図８は、前者の例で姿勢を示している。

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

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

次に、図１０に示すように、ロボット駆動用のアクチュエータ（電動モータ）に制御信号を出力し、ロボット１３の動作によりハンド１５をハンド位置姿勢Ｐｋへ移動させる（ステップＳ８）。変数の初期値ｋ＝１に対しては、Ｐｋ＝Ｐ１である。

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

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

ステップＳ１２では、変数ｋがｎより小さいか否かを判定する。この判定は、ｎ個（図１１では２個）のハンド位置姿勢３４の中にハンド１５が未だ到達していないものが存在するか否かの判定である。ｋ＜ｎと判定されると、ハンド位置姿勢Ｐｋ＋１にハンド１５が未だ到達していないので、ｋ←ｋ＋１とし（ステップＳ１３）、ステップＳ８へ戻る。ステップＳ１２でｋ＜ｎでないと判定されると、ｎ個のハンド位置姿勢３４の全てに保持部１４ａが到達したので、ステップＳ１に戻る。

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

以上では、ロボット制御装置１４での処理により、複数の物品２０の表面位置を三次元測定機１１で測定して複数の三次元点３１からなる三次元点集合３０を取得し（ステップＳ１）、三次元点集合３０から互いに近傍にある三次元点３１を連結してなる連結集合３２を求め（ステップＳ３）、連結集合３２に属する三次元点３１の位置情報に基づき、物品２０を取出可能なハンド１５の位置姿勢（ハンド位置姿勢３４）を求め（ステップＳ４、ステップＳ５）、さらに求められたハンド位置姿勢３４にハンド１５を移動してパレット１６上の物品２０を取出すようにロボット１２を制御する（ステップＳ８〜ステップＳ１１）。

ここで、連結集合３２は、物品表面の位置及び姿勢（傾き）を反映するので、連結集合３２を用いることで、パターンマッチングやユーザの教示作業等によらず物品２０の位置及び姿勢を特定することができる。このため、物品２０のモデルパターンを作成する必要がなく、形状の異なる多品種の物品２０であっても容易にその位置及び姿勢を認識して、物品２０を保持することができる。また、追加された新品種の物品２０に対しても、モデルパターンの追加やユーザの教示作業等を要することなく、その位置及び姿勢を容易に認識することができる。

ところで、複数の同一形状の物品２０が隣接して配置されている場合、第１三次元点３１と第２三次元点３１とが互いに異なる物品２０上で測定されるおそれがある。このとき、第１三次元点３１と第２三次元点３１間の距離を算出し、この距離が所定値内にある場合に、第１三次元点３１と第２三次元点３１とが互いに近傍にあるとして第１三次元点３１と第２三次元点３１とが同一の連結集合３２に属するように構成すると、複数の物品２０にまたがって連結集合３２が構成されることになる。これを避けるため、本実施形態では、以下のように連結集合３２を演算する。

図１２、図１３は、本実施形態の特徴的構成である連結集合３２を求めるための処理（連結集合演算処理）の一例、すなわち図２のステップＳ３における処理を具体的に示すフローチャートである。

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

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

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

ステップＳ３０では、リストＬｊのｍ番目の三次元点３１（ｊ）の近傍に、同一物品２０上に存在するラベル番号０の三次元点３１（０）が存在するか否かを判定し、存在すると判定された三次元点３１（０）をリストＬｊに追加する処理（近傍判定処理）を行う。

図１３は、ステップＳ３０の近傍判定処理を詳細に示すフローチャートである。ステップＳ３０Ａでは、リストＬｊのｍ番目の三次元点３１（ｊ）を選択する。ステップＳ３０Ｂでは、ステップＳ２で取得した画像４０を用いて、ステップＳ３０Ａで選択した三次元点３１（ｊ）に対応する画像４０上の二次元点５１（ｊ）を算出する。この場合、予め設定したカメラ１２のキャリブレーションデータを用いることで、三次元点３１（ｊ）に対応する二次元点５１（ｊ）を算出することができる。

ステップＳ３０Ｃでは、ラベル番号が０である三次元点３１（０）であって、ステップＳ３０Ａで選択した三次元点３１（ｊ）の近傍にある三次元点（近傍三次元点３１０（０））を全て集めた集合である近傍三次元点集合を演算する。近傍三次元点３１０（０）は、例えば、選択した三次元点３１（ｊ）のｘ座標及びｙ座標との差が所定値以下であるｘ座標及びｙ座標を有する点である。なお、選択した三次元点３１（ｊ）のｘ座標、ｙ座標及びｚ座標との差が所定値以下であるｘ座標、ｙ座標及びｚ座標を有する点を近傍三次元点３１０（０）としてもよく、ユークリッド距離が所定値内である点を近傍三次元点３１０（０）としてもよい。近傍三次元点３１０（０）は、同一物品上に存在すると予想される三次元点３１（ｊ）であり、ラベル番号がｊの三次元点３１（ｊ）の候補となる。

ステップＳ３０Ｄでは、近傍三次元点集合に属する全ての近傍三次元点３１０（０）を３１０（１），３１０（２），・・・，３１０（ｎ）とナンバリングする。ステップＳ３０Ｅでは、自然数値をとる変数ｉに対して、初期値１を与える。すなわち、ｉ←１とする。ステップＳ３０Ｆでは、ナンバリングした近傍三次元点３１０（ｉ）を選択する。ステップＳ３０Ｇでは、カメラ１２のキャリブレーションデータを用いて、近傍三次元点３１０（ｉ）に対応する画像４０上の点である近傍二次元点５１０（ｉ）を算出する。

ステップＳ３０Ｈでは、ステップＳ３０Ｂで算出した画像４０上の二次元点５１（ｊ）とステップＳ３０Ｇで算出した画像４０上の二次元点５１０（ｉ）とを含む領域（部分画像領域）の画像勾配情報を取得する。部分画像領域は、例えば二次元点５１（ｊ）と二次元点５１０（ｉ）とを結ぶ線分から所定距離以内にある領域である。画像勾配情報は、例えば、部分画像領域をソーベルフィルタなどでフィルタリングを行い、得られたフィルタリング画像のうち、一定コントラスト（明るさ）以上の画素値の情報である。すなわち、隣接する画素の明るさの急峻な変化を表す物理量を、画像勾配情報として取得する。部分画像領域内における明るさが最小である画素値と明るさが最大である画素値との差を、画像勾配情報として用いることもできる。このような画像の明暗の変化量により、物品２０の表面の勾配状態を特定できる。

ステップ３０Ｉでは、ステップＳ３０Ｈで取得した画像勾配情報に基づき、ステップＳ３０Ａで選択した三次元点３１（ｊ）とステップＳ３０Ｆで選択した近傍三次元点３１０（ｉ）とが互いに同一の物品２０上に存在するか否かを判定する。この判定は、例えば、三次元点３１（ｊ）と近傍三次元点３１０（ｉ）との間の距離が所定値Δｄ以下であり、かつ、部分画像領域内における画像の明暗の変化量が所定値以下であるか否かを判定することにより行われる。三次元点３１（ｊ）と近傍三次元点３１０（ｉ）との距離に関しては、例えば一次元以上三次元以下の所定座標系に関してそれぞれ予め所定距離Δｄを定めておき、全ての所定座標系において三次元点３１（ｊ）と近傍三次元点３１０（ｉ）との間の距離が所定値Δｄ以下であるか否かを判定するようにしてもよい。

ステップＳ３０Ｉが肯定されるとステップＳ３０Ｊに進み、否定されるとステップＳ３０ＪをパスしてステップＳ３０Ｋに進む。ステップＳ３０Ｊでは、ステップＳ３０Ｆで選択された近傍三次元点３１０（ｉ）にラベル番号ｊを割り振り、近傍三次元点３１０（ｉ）を三次元点３１（ｊ）としてリストＬｊの最後に追加する。ステップＳ３０Ｋでは、変数ｉに１を追加する（ｉ←ｉ＋１）。ステップＳ３０Ｌでは、全ての近傍三次元点３１０（１）〜３１０（ｎ）に対しステップＳ３０Ｉの判定が行われた否か、すなわち変数ｉがｎより大きいか否かを判定する。ステップＳ３０Ｌが否定されるとステップＳ３０Ｆに戻り、上述したのと同様の処理を繰り返す。ステップＳ３０が肯定されると、近傍判定処理（ステップＳ３０）を終了し、図１２のステップＳ３１に進む。

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

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

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

次いで、リストＬ１の１番目の三次元点３１４の近傍であり、三次元点３１４が測定された物品２０と同一物品上にラベル番号０の三次元点３１（０）が存在するか否かを判定する（ステップＳ３０）。例えば、図１４に示すように、物品２０の表面上に三次元点３１３〜３１５が測定され、３１３と３１４、及び３１４と３１５がそれぞれ所定距離Δｄ以下の同一距離だけ離れていると仮定する。このとき、画像４０上の二次元点５１として、図１５に示すように各三次元点３１３〜３１５に対応する二次元点５１３〜５１５が算出される（ステップＳ３０Ｇ）。このうち、二次元点５１３，５１４はそれぞれ同一物品上の三次元点３１３，３１４に対応するため、二次元点５１３，５１４を含む部分画像領域５２１中における画像の明暗の変化量は所定値以内である。したがって、三次元点３１３，３１４が同一物品上に存在すると判定され（ステップＳ３０Ｉ）、三次元点３１３にラベル番号１を割り振り（３１３（１））、リストＬ１の２番目に追加する（ステップＳ３０Ｊ）。これにより、三次元点３１４と三次元点３１５とが連結され、リストＬ１の要素数Ｎは２となる。

一方、二次元点５１４，５１５はそれぞれ異なる物品上の三次元点３１４，３１５に対応するため、図１６に示すように、二次元点５１４，５１５を含む部分画像領域５２２中における画像の明暗の変化量は所定値を超える。したがって、三次元点３１４，３１５が同一物品上にないと判定され、三次元点３１５は三次元点３１４と同一のリストＬ１に追加されない。三次元点３１４の近傍にある全ての近傍三次元点３１０（０）に対して近傍判定処理が終了すると、ｍが２（＜Ｎ）となり（ステップＳ３１）、上述したのと同様にして、リストＬ１の２番目の三次元点３１３の近傍であり、三次元点３１３が測定された物品２０と同一物品上に、ラベル番号０の三次元点３１（０）が存在するか否かを判定する（ステップＳ３０）。

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

本実施形態によれば以下のような作用効果を奏することができる。
（１）ロボット制御装置１４での処理により、三次元測定機１１によって取得された複数の三次元点３１の中から、互いに近傍にある第１三次元点３１と第２三次元点３１とを選択し（ステップＳ３０Ａ，ステップＳ３０Ｆ）、カメラ１２で取得された画像データに基づき、第１三次元点３１及び第２三次元点３１にそれぞれ対応する画像上の点（二次元点５１）を含む部分画像領域における、物品２０の表面の勾配状態を表す画像勾配情報を取得し（ステップＳ３０Ｈ）、第１三次元点３１と第２三次元点３１の三次元点情報及び取得された画像勾配情報に基づき、第１三次元点３１と第２三次元点３１とが同一物品上にあるか否かを判定し（ステップＳ３０Ｉ）、さらに第１三次元点３１と第２三次元点３１とが同一物品上にあると判定されると、これら三次元点３１に同一のラベル番号ｊを割り振り、同一の連結集合３２に含めるようにした。

したがって、例えば同一形状の複数の物品２０が隣接して配置されている場合でも、複数の物品２０にまたがって連結集合３２が演算されることがなく、単一の物品２０に単一の連結集合３２を対応付けることができる。よって、連結集合３２により物品２０を精度よく認識することができ、物品取出し作業を正確に行うことができる。

（２）ロボット制御装置１４での処理により、第１三次元点３１と第２三次元点３１とにそれぞれ対応する画像上の二次元点５１を算出し（ステップＳ３０Ｂ，ステップ３０Ｇ）、これら二次元点５１により部分画像領域を特定して、部分画像領域における勾配状態の変化がないと、第１三次元点３１と第２三次元点３１とが同一物品上にあると判定するようにした（ステップＳ３０Ｉ）。したがって、第１三次元点３１と第２三次元点３１とが同一物品上にあるか否かを良好に判定できる。

（３）この場合、カメラ１２のキャリブレーションデータに基づき、各二次元点５１を取得するので、三次元点３１に対応する画像上の二次元点５１を容易に求めることができる。

（４）部分画像領域における画像の明暗の変化量の情報を画像勾配情報として用いるので、物品２０の表面の勾配状態を精度よく判断できる。

（５）三次元測定機１１によって測定された第１三次元点３１と第２三次元点３１との間の距離が所定値Δｄ以下であり、かつ、部分画像領域における画像の明暗の変化量が所定値以下のときに、第１三次元点３１と第２三次元点３１とが同一物品上にあると判定する。これにより、複数の三次元点３１が同一物品上にあるか否かを精度よく判定できる。

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

（７）ロボット制御装置１４での処理により、連結集合３２に属する全ての三次元点３１の重心位置とこの重心位置における所定の姿勢とを組合せて代表位置姿勢３３を求めるようにすると、物品２０を安定してハンド１５で保持することができる。

（８）ロボット制御装置１４での処理により、連結集合３２に属する全ての三次元点３１を包含する外接長方形の中心位置と所定の姿勢とを組み合わせて代表位置姿勢３３を求めるようにすると、三次元測定機１１で計測された物品上の三次元点３１に欠損がある場合であっても、安定して物品２０の中心を代表位置姿勢３３として演算することができ、物品２０を安定してハンド１５で保持することができる。

なお、連結集合３２を求めるときに、三次元測定機１１によって取得された複数の三次元点３１の中から、互いに近傍にある第１三次元点３１と第２三次元点３１とを選択し、カメラ１２で取得された画像データに基づき、第１三次元点３１及び第２三次元点３１にそれぞれ対応する画像上の点５１を含む部分画像領域における、物品２０の表面の勾配状態を表す画像勾配情報を取得し、第１三次元点３１と第２三次元点３１の三次元位置情報及び取得された画像勾配情報に基づき、第１三次元点３１と第２三次元点３１とが同一物品上にあるか否かを判定し、第１三次元点３１と第２三次元点３１とが同一物品上にあると判定されると、第１三次元点３１と第２三次元点３１とを同一の連結集合に含めるのであれば、物品取出方法の構成はいかなるものでもよい。

図１７は、図１のロボット制御装置１４の内部構成を示すブロック図である。ロボット制御装置１４は、連結集合演算部１４１と、物品特定部１４２と、ハンド位置姿勢演算部１４３と、ロボット制御部１４４部とを有する。連結集合演算部１４１は、三次元点選択部１４１Ａと、勾配情報取得部１４１Ｂと、判定部１４１Ｃとを有する。

なお、上記実施形態では、第１三次元点３１から所定距離内にある近傍三次元点３１０を第２三次元点３１として演算したが（ステップＳ３０Ｃ）、互いに近傍にある第１三次元点３１と第２三次元点３１とを選択するのであれば、三次元点選択部１４１Ａの構成はいかなるものでもよい。上記実施形態では、部分画像領域における画像の明暗の変化量の情報を画像勾配情報として取得したが（ステップＳ３０Ｈ）、物品２０の表面の勾配状態を表すのであれば、画像勾配情報はいかなるものでもよく、勾配情報取得部１４１Ｂの構成は上述したものに限らない。上記実施形態では、部分画像領域内における画像の明暗の変化量が所定値以下であるか否かを判定することにより（ステップＳ３０Ｉ）、第１三次元点３１と第２三次元点３１とが同一物品上にあるか否かを判定するようにしたが、第１三次元点３１と第２三次元点３１の三次元位置情報及び勾配情報取得部１４１Ｂにより取得された画像勾配情報に基づき、第１三次元点３１と第２三次元点３１とが同一物品上にあるか否かを判定するのであれば、判定部１４１Ｃの構成は上述したものに限らない。すなわち、三次元測定機１１によって取得された三次元点の三次元位置情報及びカメラ１２によって取得された画像データに基づき、三次元点測定機１１によって取得された複数の三次元点３１の中から、互いに近傍にある三次元点３１を連結してなる連結集合３２を求めるのであれば、連結集合演算部１４１の構成はいかなるものでもよい。

上記実施形態では、連結集合３２に属する三次元点３１の位置情報に基づき、連結集合３２を代表する代表位置姿勢３３を演算するようにしたが（ステップＳ３）、連結集合３２によって表される物品２０の位置及び姿勢を特定するのであれば、物品特定部１４２の構成は上述したものに限らない。ここで、連結集合３２を代表する位置及び姿勢は、物品２０を代表する位置及び姿勢でもあり、物品２０の位置及び姿勢を特定するとは、物品を代表する位置及び姿勢を求めて物品２０の配置を特定することを意味する。上記実施形態では、代表位置姿勢３３からハンド位置姿勢３４を演算するようにしたが（ステップＳ４）、代表位置姿勢３３として特定された物品２０を取出し可能なハンド位置姿勢３４を求めるのであれば、ハンド位置姿勢演算部１４３の構成は上述したものに限らない。ハンド位置姿勢３４へハンド１４を移動して物品２０を取出すようにロボット１３を制御するのであれば、ロボット制御部１４４の構成はいかなるものでもよい。

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

１０ 物品取出装置
１１ 三次元測定機
１２ カメラ
１３ ロボット
１４ ロボット制御装置
１５ ハンド
１６ パレット
２０ 物品
３０ 三次元点集合
３１ 三次元点
３２ 連結集合
３３ 代表位置姿勢
３４ ハンド位置姿勢
５１ 二次元点
１４１ 連結集合演算部
１４１Ａ 三次元点選択部
１４１Ｂ 勾配情報取得部
１４１Ｃ 判定部
１４２ 物品特定部
１４３ ハンド位置姿勢演算部
１４４ ロボット制御部

Claims (8)

1. 物品を保持可能なハンドを有するロボットと、
   三次元空間に置かれた複数の物品の表面位置を測定し、複数の三次元点の位置情報を取得する三次元測定機と、
   前記複数の物品を含む領域を撮像し、画像データを取得するカメラと、
   前記三次元測定機によって取得された複数の三次元点の位置情報及び前記カメラによって取得された画像データに基づき、該複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求める連結集合演算部と、
   前記連結集合に属する三次元点の位置情報に基づき、物品の位置及び姿勢を特定する物品特定部と、
   前記物品特定部により特定された物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢を求めるハンド位置姿勢演算部と、
   前記ハンド位置姿勢演算部により求められたハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御するロボット制御部と、を備え、
   前記連結集合演算部は、
   前記三次元測定機によって取得された複数の三次元点の中から、互いに近傍にある第１三次元点と第２三次元点とを選択する三次元点選択部と、
   前記カメラで取得された画像データに基づき、前記第１三次元点及び前記第２三次元点にそれぞれ対応する画像上の点を含む部分画像領域における、物品の表面の勾配状態を表す画像勾配情報を取得する勾配情報取得部と、
   前記第１三次元点の位置情報、前記第２三次元点の位置情報及び前記勾配情報取得部により取得された前記画像勾配情報に基づき、前記第１三次元点と前記第２三次元点とが同一物品上にあるか否かを判定する判定部とを有し、
   前記判定部により前記第１三次元点と前記第２三次元点とが同一物品上にあると判定されると、前記第１三次元点と前記第２三次元点とを同一の連結集合に含めることを特徴とする物品取出装置。
2. 請求項１に記載の物品取出装置において、
   前記勾配情報取得部は、前記第１三次元点及び前記第２三次元点にそれぞれ対応する画像中の点であって、前記部分画像領域を特定する第１二次元点及び第２二次元点を取得し、
   前記判定部は、前記第１三次元点と前記第２三次元点との間の距離が所定値以下であり、かつ、前記部分画像領域における画像の明暗の変化量が所定値以下のときに、前記第１三次元点と前記第２三次元点とが同一物品上にあると判定することを特徴とする物品取出装置。
3. 請求項２に記載の物品取出装置において、
   前記勾配情報取得部は、前記カメラのキャリブレーションデータに基づき、前記第１二次元点及び前記第２二次元点を取得することを特徴とする物品取出装置。
4. 請求項１〜３のいずれか１項に記載の物品取出装置において、
   前記物品特定部は、前記連結集合に属する三次元点の位置情報に基づき、前記連結集合を代表する位置及び姿勢である代表位置姿勢を求め、
   前記ハンド位置姿勢演算部は、この代表位置姿勢に対応して前記ハンド位置姿勢を求めることを特徴とする物品取出装置。
5. 請求項４に記載の物品取出装置において、
   前記物品特定部は、前記連結集合に属する全ての三次元点の重心位置と該重心位置における所定の姿勢とを組合せて前記代表位置姿勢を求めることを特徴とする物品取出装置。
6. 請求項４に記載の物品取出装置において、
   前記物品特定部は、前記連結集合に属する全ての三次元点を包含する外接長方形の中心位置と所定の姿勢とを組み合わせて前記代表位置姿勢を求めることを特徴とする物品取出装置。
7. 請求項１〜６のいずれか１項に記載の物品取出装置において、
   前記三次元測定機は、前記カメラを有することを特徴とする物品取出装置。
8. 物品を保持可能なハンドを有するロボットを用いて、三次元空間に置かれた物品を取出す物品取出方法であって、
   前記三次元空間に置かれた複数の物品の表面位置を三次元測定機で測定して複数の三次元点の位置情報を取得し、
   前記複数の物品を含む領域をカメラで撮像して画像データを取得し、
   前記三次元測定機によって取得された複数の三次元点の位置情報及び前記カメラで取得された画像データに基づき、該複数の三次元点の中から、互いに近傍にある三次元点を連結してなる連結集合を求め、
   前記連結集合に属する三次元点の位置情報に基づき、物品の位置及び姿勢を特定し、
   前記位置及び姿勢が特定された物品を取出し可能な前記ハンドの位置及び姿勢であるハンド位置姿勢を求め、
   前記ハンド位置姿勢へ前記ハンドを移動して前記物品を取出すように前記ロボットを制御し、
   前記連結集合を求めるときには、
   前記三次元測定機によって取得された複数の三次元点の中から、互いに近傍にある第１三次元点と第２三次元点とを選択し、
   前記カメラで取得された画像データに基づき、前記第１三次元点及び前記第２三次元点にそれぞれ対応する画像上の点を含む部分画像領域における、物品の表面の勾配状態を表す画像勾配情報を取得し、
   前記第１三次元点の位置情報、前記第２三次元点の位置情報及び前記画像勾配情報に基づき、前記第１三次元点と前記第２三次元点とが同一物品上にあるか否かを判定し、
   前記第１三次元点と前記第２三次元点とが同一物品上にあると判定されると、前記第１三次元点と前記第２三次元点とを同一の連結集合に含める、ことを特徴とする物品取出方法。

Images