JP6836628B2 - ピッキングまたはデバンニング用対象物認識装置、ピッキングまたはデバンニング用対象物認識方法、ならびにプログラム - Google Patents

Description

本発明は、ピッキングまたはデバンニング用対象物認識装置、ピッキングまたはデバンニング用対象物認識方法、ならびにプログラムに関する。

保管場所から荷物や部品等（ワークとも称される）を取り出す作業をピッキングという。従来、物流におけるピッキング処理を、人に代わってロボットが実行するピッキング装置が広く用いられている。すなわち、ピッキング装置とは、ある場所に置かれたピッキングの対象物である荷物や部品等から、個別にピッキング対象物の位置・姿勢を認識し、ロボットにより１つずつ取出す装置である。

また、コンテナから荷物や部品等を取り出す（荷下ろしする）作業を、デバンニングという。デバンニング処理においても、人に代わってロボットが実行するデバンニング装置が広く用いられている。

従来、ピッキング装置においては、バラ積みされたワークを距離センサで３次元計測し、得られた計測結果とワークの３ＤＣＡＤモデルとを照合することによって、個別のワークの３次元位置と姿勢を認識した後、ロボットハンドをハンド待機位置へ移動する技術がある。（例えば、特許文献１）。

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

上述した特許文献１に記載の技術においては、ワーク（荷物や部品等）を距離センサで３次元計測し、得られた計測結果とワークの３ＤＣＡＤモデルとを照合することによって、個別のワークの３次元位置と姿勢が認識される。すなわち、ピッキングの対象となる荷物や部品等が複数の形状を有するものである場合、上述した特許文献１に記載の技術においては、それぞれのピックングの対象の３ＤＣＡＤモデルを全てシステムに入力する必要があった。

また、上述した特許文献１に記載の技術では、例えば、紛体等が封入された袋等、３ＤＣＡＤモデルが存在しない物品等を対象物としてピッキングすることができなかった。

すなわち、デバンニング装置において、荷物や部品等のデバンニング位置の認識に、上述した特許文献１に記載の技術を応用しても、デバンニングの対象となる荷物や部品等が複数の形状を有するものである場合、それぞれのデバンニングの対象となる物品等の３ＤＣＡＤモデルを全てシステムに入力する必要があり、また、紛体等が封入された袋等、３ＤＣＡＤモデルが存在しない対象物である場合、デバンニングの対象を荷下ろしすることはできない。

そこで、本発明は、前記課題を解決すること、すなわち、ピッキングまたはデバンニングの対象となる荷物や部品等が複数の形状を有する場合や、対象物が、例えば、紛体等が封入された袋等の３ＤＣＡＤモデルが存在しない場合でも、ピッキング処理またはデバンニング処理をロボット等の装置に実行させることができる、ピッキングまたはデバンニング用対象物認識装置、ピッキングまたはデバンニング用対象物認識方法、ならびにプログラムを提供することを目的とする。

本発明の一側面は、対象物に対してピッキングまたはデバンニングを行う装置を制御するための情報を生成するピッキングまたはデバンニング用対象物認識装置において、対象物の距離画像および撮像画像を取得する画像データ取得手段と、画像データ取得手段により取得された距離画像および撮像画像の少なくとも一方を用いて、画像内の対象物の領域を検出する対象物領域検出手段と、
画像データ取得手段により取得された距離画像および撮像画像、ならびに、対象物領域検出手段による画像内の対象物の領域の検出結果に基づいて、対象物に該当する３次元点群を取得する３次元点群取得手段と、３次元点群取得手段により取得された対象物に該当する３次元点群に基づいて、対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出手段と、平面方程式算出手段により算出された平面方程式を用いて、対象物の姿勢を検出する姿勢検出手段と、撮像画像、３次元点群、および、平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、ピッキングまたはデバンニングを行う装置が対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出手段とを有し、位置検出手段は、撮像画像における対象物の重心位置の座標（Ｘｇ，Ｙｇ）、位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、位置情報（ｘ，ｙ，ｚ）を求めることを特徴とする。

本発明のピッキングまたはデバンニング用対象物認識装置の他の側面は、位置情報（ｘ，ｙ，ｚ）の内、位置情報ｘがＸｇ・ｚ／ｆとして算出され、位置情報ｙがＹｇ・ｚ／ｆとして算出され、位置情報ｚが、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、位置情報ｘおよび位置情報ｙからｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））として算出されることを特徴とする。

本発明の他の側面は、姿勢検出手段が、Ｘ−Ｙ−Ｚオイラー角を、Ｚ＝Ｒｚ（α）、Ｙ＝Ｒｙ（β）、Ｘ＝Ｒｘ（γ）としたとき、（ｘ，ｙ，ｚ）＝（０，０，１）の点を、Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ）で回転させた場合、（ｘ，ｙ，ｚ）＝（ａ，ｂ，ｃ）となるようなβおよびγの値を求めた後、前記対象物の姿勢（Ｒｚ（α・Ｒｙ（β）・Ｒｘ（γ））を算出することを特徴とする。

本発明の他の側面は、姿勢検出手段により検出された姿勢、および、位置検出手段により検出された位置情報を、ピッキングまたはデバンニングを行う装置が認識可能な座標系に変換する座標系変換処理手段と、座標系変換処理手段により座標系が変換された姿勢および位置情報の出力処理を実行する出力処理手段とをさらに備えることを特徴とする。

本発明のピッキングまたはデバンニング用対象物認識方法の一側面は、対象物の距離画像および撮像画像を取得する画像データ取得ステップと、画像データ取得ステップの処理により取得された距離画像および撮像画像の少なくとも一方を用いて、画像内の前記対象物の領域を検出する対象物領域検出ステップと、画像データ取得ステップの処理により取得された距離画像および撮像画像、ならびに、対象物領域検出ステップの処理による画像内の前記対象物の領域の検出結果に基づいて、対象物に該当する３次元点群を取得する３次元点群取得ステップと、３次元点群取得ステップの処理により取得された対象物に該当する３次元点群に基づいて、対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出ステップと、平面方程式算出ステップの処理により算出された平面方程式を用いて、対象物の姿勢を検出する姿勢検出ステップと、距離画像、撮像画像、３次元点群、および、平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、ピッキングまたはデバンニングを行う装置が対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出ステップとを有し、位置検出ステップが、撮像画像における対象物の重心位置の座標（Ｘｇ，Ｙｇ）、位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、位置情報（ｘ，ｙ，ｚ）を求めることを特徴とする。

本発明の他の側面は、Ｘｇ・ｚ／ｆに基づいて位置情報ｘを算出し、Ｙｇ・ｚ／ｆに基づいて位置情報ｙを算出し、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、位置情報ｘおよび位置情報ｙから導き出されるｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））に基づいて位置情報ｚを算出することを特徴とする。

本発明のプログラムの一側面は、ピッキングまたはデバンニングを行う対象物を認識するコンピュータに、対象物の距離画像および撮像画像を取得する画像データ取得ステップと、画像データ取得ステップの処理により取得された距離画像および撮像画像の少なくとも一方を用いて、画像内の対象物の領域を検出する対象物領域検出ステップと、画像データ取得ステップの処理により取得された距離画像および撮像画像、ならびに、対象物領域検出ステップの処理による画像内の対象物の領域の検出結果に基づいて、対象物に該当する３次元点群を取得する３次元点群取得ステップと、３次元点群取得手段により取得された対象物に該当する３次元点群に基づいて、対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出ステップと、平面方程式算出ステップの処理により算出された平面方程式を用いて、対象物の姿勢を検出する姿勢検出ステップと、撮像画像、３次元点群、および、平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、ピッキングまたはデバンニングを行う装置が対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出ステップとを含む処理を実行させるとともに、位置検出ステップの処理として、撮像画像における対象物の重心位置の座標（Ｘｇ，Ｙｇ）、位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、位置情報（ｘ，ｙ，ｚ）を求める処理を実行させる。

本発明の他の側面は、位置情報（ｘ，ｙ，ｚ）を求める処理は、Ｘｇ・ｚ／ｆに基づいて位置情報ｘを算出する処理と、Ｙｇ・ｚ／ｆに基づいて位置情報ｙを算出する処理と、平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、位置情報ｘおよび位置情報ｙから導き出されるｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））に基づいて位置情報ｚを算出する処理を含むことを特徴とする。

本発明によれば、ピッキングまたはデバンニングの対象となる荷物や部品等が複数の形状を有する場合や、対象物が、例えば、紛体等が封入された袋等の３ＤＣＡＤモデルが存在しない場合でも、ピッキング処理またはデバンニング処理をロボット等の装置に実行させることができる。

ピッキングまたはデバンニングシステム１の概要を示す図である。 ３次元画像解析装置１２が有する機能を示す機能ブロック図である。 画像データ取得部２２が３次元画像撮像システム１１から取得する距離画像について説明する図である。 画像における対象物２の領域の検出について説明する図である。 ３次元画像解析装置１２の処理について説明するためのフローチャートである。

以下、本発明の一実施の形態のピッキングまたはデバンニングシステム１について、図１〜図５を参照しながら説明する。

図１を参照して、本発明の一実施例であるピッキングまたはデバンニングシステム１について説明する。

ピッキングまたはデバンニングシステム１は、３次元画像撮像システム１１、３次元画像解析装置１２、ロボット制御装置１３、および、ロボット１４を有している。対象物２は、ピッキングまたはデバンニングシステム１によるピッキングまたはデバンニングの対象となる部品や荷物など（ワークとも称される）である。ピッキングまたはデバンニングシステム１は、対象物２の位置および姿勢を検出し、その検出結果に基づいて、ロボット１４によってピッキングまたはデバンニングする処理を実行する。３次元画像解析装置１２は、ピッキングまたはデバンニング用対象物認識装置に対応する。

以下、ピッキングまたはデバンニングシステム１の各部が認識する３次元空間座標系については、３次元空間における 右手系 （正系）座標、すなわち、右手の親指、人指し指、中指を直交するように曲げたときに、親指を x 軸、人指し指を y 軸、中指を z 軸とする座標系を用いて説明する。

対象物２自体の形状を表すための座標系は、モデリング座標系（ローカル座標系とも称する）と称される。一般的にコンピュータ等が３元空間全体を定義する場合に用いられるのは、ワールド座標系である。３次元画像解析装置１２も、基本的には、ワールド座標系を基準として処理を実行するが、ここで、対象物２の形状をワールド座標系で与えると、物体が移動するたびに再定義が必要となるため、物体の形状を与える(モデリングする)ためにモデリング座標系が用いられる。ここでは、モデリング座標系のｚ軸は、対象物２のピッキングまたはデバンニングにおいて、姿勢を検出するために用いられる平面（この平面の詳細については後述する）に対して垂直とされる。また、ｚ軸の正の方向は、後述する３次元画像撮像システム１１、および、３次元画像解析装置１２が認識しているワールド座標系の正の方向と同一であるものとする。

３次元画像撮像システム１１は、例えば、プロジェクタおよびカメラ、または、TOF（Time of Flight）カメラを含んで構成され、距離画像および撮像画像を取得し、３次元画像解析装置１２に供給する。３次元画像撮像システム１１が認識している座標系は、一般的にコンピュータ等が３元空間全体を定義する場合に用いられるワールド座標系である。

距離画像とは、３次元画像撮像システム１１からの距離を画像座標で表した画像であり、３次元座標のzのみを画素値としている。３次元画像撮像システム１１は、例えば、倍精度浮動小数点画像などを用いて距離を表すことができる。また、撮像画像とは、通常の8bit等のカメラ画像である。

３次元画像解析装置１２は、３次元画像撮像システム１１から供給されたワールド座標系の距離画像および撮像画像に基づいて、対象物２の位置、すなわち、撮像画像内に含まれる領域を検出し、対象物２をピッキングまたはデバンニングするための位置情報（例えば、ピッキングであれば、対象物２に対して、ピッキングパット等を接する位置に関する情報）および対象物２の姿勢（傾き）を算出する。そして、３次元画像解析装置１２は、算出したワールド座標系における対象物２の位置情報および姿勢を、ロボット制御装置１３が認識可能な、すなわち、ロボット１４の視点での座標系であるロボット座標系に変換し、ロボット座標系に変換された情報を、ロボット制御装置１３に供給する。

ロボット制御装置１３は、３次元画像解析装置１２から供給された、ロボット座標系で表されている対象物２の位置情報および姿勢の情報に基づいて、ロボット１４によって対象物２をピッキングまたはデバンニングする処理を制御する。

ロボット１４は、ロボット制御装置１３の制御に基づいて、対象物２をピッキングまたはデバンニングする。

ロボット制御装置１３とロボット１４は、個別に構成されていても、同一装置として構成されていてもよい。また、ロボット制御装置１３とロボット１４には、従来用いられている装置を利用することが可能である。

次に、図２の機能ブロック図を用いて、３次元画像解析装置１２について説明する。

３次元画像解析装置１２は、記憶部２１、画像データ取得部２２、対象物領域検出部２３、３次元点群取得部２４、平面方程式算出部２５、姿勢検出部２６、ピッキング位置検出部２７、ロボット座標系変換処理部２８、および、出力処理部２９を含んで構成されている。

記憶部２１は、３次元画像解析装置１２の各部の処理に必要なデータを記憶しているとともに、各部が取得した情報、および、各部が生成した情報を記憶する。記憶部２１は、３次元画像解析装置１２の各部と情報を授受可能であるが、図中、データの流れを示す矢印は省略する。

画像データ取得部２２は、３次元画像撮像システム１１から、距離画像および撮像画像を取得し、対象物領域検出部２３に供給する。画像データ取得部２２が３次元画像撮像システム１１から取得する距離画像は、例えば、図３のように表される。図３においては、濃度が高いほど、３次元画像撮像システム１１と撮像対象点との距離が長く、濃度が低いほど、３次元画像撮像システム１１と撮像対象点との距離が短い。

対象物領域検出部２３は、画像データ取得部２２から供給された距離画像および撮像画像の少なくとも一方を用いて、画像内の対象物２の領域を検出する。

対象物領域検出部２３による画像内の対象物２の領域の検出方法は、いかなるものであってもよい。対象物領域検出部２３は、例えば、距離画像のみを使用して、 Mean-Shift法の関数を用いて２値化を行い、ラプラシアン等のエッジ検出を行うことにより高さエッジを求め、Mean-Shift法の関数を用いた２値化結果と、ラプラシアン等のエッジ検出を行うことにより求められた高さエッジの検出結果との論理積を取り、ブロブ解析により対象物２の領域を検出することができる。

また、対象物領域検出部２３は、例えば、距離画像および撮像画像を使用して、 距離画像からMean-Shift法の関数を用いて2値化を行い、 撮像画像を適切な手法を用いて２値化し、距離画像からの２値化結果と、撮像画像からの２値化結果との論理積を取り、ブロブ解析により対象物２の領域を検出することができる。

また、対象物領域検出部２３は、例えば、複数の同一形状の対象物２を検出する場合、先行して得た撮像画像からマスターパターンを登録し、ＦＰＭ（Feature Point application Matching：特徴点応用マッチング）を用いてサーチすることにより、対象物２の領域を検出することができる。

このとき、対象物領域検出部２３は、撮像画像中に、対象物２が複数含まれている場合、対象物２のうちのいずれか１つのみを検出する。対象物領域検出部２３は、例えば、複数の対象物２が積み重なって置かれていた場合においては、距離画像に基づいて、ロボット１４が、一番上の対象物２からピッキングまたはデバンニングを行うことができるように、距離が３次元画像撮像システム１１から一番近いものから順番に、対象物２として検出する。

また、対象物領域検出部２３は、撮像画像中に対象物２が複数含まれている場合、それらの配置や、対象物２の１つをピッキングまたはデバンニングすることによって、残った対象物２の位置が変わるか否かなどに基づいて、３次元画像撮像システム１１から取得された１回分の距離画像および撮像画像に基づいて複数の対象物２の領域を検出したり、対象物２の１つをピッキングまたはデバンニングするごとに、３次元画像撮像システム１１から、新たな距離画像および撮像画像を取得するものとしてもよい。３次元画像解析装置１２が図示しない操作入力部を備えることにより、対象物領域検出部２３は、３次元画像撮像システム１１から取得された１回分の距離画像および撮像画像に基づいて複数の対象物２の領域を検出するか、対象物２の１つをピッキングまたはデバンニングするごとに、３次元画像撮像システム１１から、新たな距離画像および撮像画像を取得するかを、ユーザにより選択可能とすることができる。

対象物領域検出部２３は、上述したようにして、取得した画像データから、例えば、図４に示されるような、対象物２の領域を検出する。対象物領域検出部２３は、対象物２の領域の検出結果、ならびに、距離画像および撮像画像を、３次元点群取得部２４に供給する。

３次元点群取得部２４は、対象物領域検出部２３から供給された、対象物２の領域の検出結果、ならびに、距離画像および撮像画像を基に、対象物２に該当する３次元点群を取得し、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像を、平面方程式算出部２５、および、ピッキング位置検出部２７に供給する。

平面方程式算出部２５は、３次元点群取得部２４から供給された、対象物２に該当する３次元点群のデータに基づいて、対象物２のピッキングまたはデバンニングにおいて、姿勢を検出するために用いられる平面を表す平面方程式を算出する。

３次元空間内の平面は、次の式（１）で示される。

ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０ ・・・（１）

平面方程式算出部２５は、３次元点群取得部２４から供給された、対象物２に該当する３次元点群の各座標を、上述した式（１）に代入し、式（１）が成立するａ，ｂ，ｃ，ｄの値を求める。この時、平面方程式算出部２５は、最小二乗法、または、ロバスト推定法を用いて、式（１）が成立するａ，ｂ，ｃ，ｄの値を求める。なお、３次元画像解析装置１２が図示しない操作入力部を備えることにより、平面方程式算出部２５は、最小二乗法、または、ロバスト推定法のいずれを用いて、式（１）が成立するａ，ｂ，ｃ，ｄの値を求めるかを、ユーザにより選択可能なようにしてもよい。

平面方程式算出部２５は、式（１）が成立するａ，ｂ，ｃ，ｄの値の算出結果、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像を、姿勢検出部２６およびピッキング位置検出部２７に供給する。

姿勢検出部２６は、平面方程式算出部２５から供給された式（１）が成立するａ，ｂ，ｃ，ｄの値の算出結果、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像に基づいて、対象物２の姿勢、すなわち、傾きを検出する。

上述した式（１）で表される平面の法線ベクトルは、上述したモデリング座標系のｚ軸に相当する。ここで、Ｘ−Ｙ−Ｚオイラー角を、Ｚ＝Ｒｚ（α）、Ｙ＝Ｒｙ（β）、Ｘ＝Ｒｘ（γ）としたとき、姿勢検出部２６は、（ｘ，ｙ，ｚ）＝（０，０，１）の点を、Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ）で回転させた場合、（ｘ，ｙ，ｚ）＝（ａ，ｂ，ｃ）となるようなα、β、γを、次の式（２）から求める。

ここで、Ｒｚ（α）の回転は、ｚ軸周りとなるため、任意の値を設定することができるから、 姿勢検出部２６は、βおよびγの値を求めればよい。

Ｒｚ（α）、Ｒｙ（β）、および、Ｒｘ（γ）は、下の式（３）から（５）で表される。

したがって、（ｘ，ｙ，ｚ）＝（０，０，１）の点を、Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ）で回転させると、次の式（６）で示される計算式から、式（７）のように、（ａ，ｂ，ｃ）が求められる。

すなわち、ａ，ｂ，ｃは、次の式（８）〜（１０）で表すことができる。

ここで、式（１０）は、次の式（１１）および（１２）に変形できる。

式（１１）または（１２）を、上述した式（８）に代入した場合の計算式を、次の式（１３）〜（１６）に示す。

そして、得られた式（１６）を、上述した式（９）に代入することにより、次の、式（１７）〜（２０）に示す計算式が導き出される。式（２１）に示されるsin（γ）の値、および、式（２２）に示されるsin（β）の値が得られる。

これにより、式（２１）に示されるsin（γ）の値、および、式（２２）に示されるsin（β）の値が得られる。

このようにして、姿勢検出部２６は、βおよびγの値を求め、姿勢（Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ））を、ピッキング位置検出部２７に供給する。

ピッキング位置検出部２７は、平面方程式算出部２５から供給された、ａ，ｂ，ｃ，ｄの値、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像と、姿勢検出部２６から供給された、姿勢（Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ））に基づいて、ロボット１４がピッキングまたはデバンニングを行う場合の位置情報（ｘ，ｙ，ｚ）を算出する。ピッキング位置検出部２７は、例えば、ロボット１４がピッキングを行う場合、ピッキング位置検出部２７は、ロボット１４が対象物２を吸引等により把持するためのピッキングパットを対象物２に接するための位置情報を算出する。ピッキング位置検出部２７は、位置検出手段に対応する。

ピッキング位置検出部２７が位置情報（ｘ，ｙ，ｚ）を算出する方法は、いずれの方法であってもよいが、例えば、透視投影モデルを利用して、撮像画像の重心点に対応する三次元空間の座標（ｘ，ｙ，ｚ）を検出することが可能である。すなわち、三次元画像撮像システムにより撮像された画像の重心位置の座標を（Xg，Yｇ）とすると、ピッキング位置検出部２７は、三次元画像撮像システムにより撮像された画像における対象物２の重心位置の座標（Xg，Yｇ）から、検出された対象物２の重心点に対応する三次元空間の座標（ｘ，ｙ，ｚ）を求めることができる。

検出された対象物２の重心点を求めるために、透視投影モデルが利用される場合、記憶部２１には、三次元画像撮像システムの焦点距離ｆのデータがあらかじめ記憶される。ピッキング位置検出部２７は、記憶部２１から、三次元画像撮像システムの焦点距離ｆのデータを取得する。

三次元画像撮像システムにより撮像された画像の座標XgおよびYｇは、焦点距離ｆを用いて、次の式（２３）および式（２４）で表される。

ここでも、上述した式（１）が成り立つので、式（１）、式（２３）および、式（２４）ｌら、ｘ，ｙ，ｚの値を求めることができる。

式（２３）および式（２４）を変形すると、次の式（２５）および式（２６）が導き出される。

この式（２５）および式（２６）を式（１）に代入することにより、次の式（２７）が導き出される。

式（２７）より、次の式（２８）で示されるｚの値を求めることができる。

そして、式（２８）で示されるｚの値を、上述した式（２５）および式（２６）に代入することにより、次の式（２９）で示されるｘの値、および、式（３０）で示されるｙの値を求めることができる。

ピッキング位置検出部２７は、三次元画像撮像システムにより撮像された画像における対象物２の重心位置の座標（Xg，Yｇ）に対応する三次元空間の座標（ｘ，ｙ，ｚ）の検出結果を、ａ，ｂ，ｃ，ｄの値、対象物２に該当する３次元点群、距離画像および撮像画像、ならびに、姿勢（Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ））とともに、ロボット座標系変換処理部２８に供給する。

ロボット座標系変換処理部２８は、ピッキング位置検出部２７から供給された、図１を用いて説明したワールド座標系の各情報のうち、ロボット制御装置１３がロボット１４の視点での座標系であるロボット座標系として扱う情報を、図１を用いて説明したロボット座標系に変換し、出力処理部２９に供給する。ロボット座標系変換処理部２８は、座標系変換処理手段に対応する。

出力処理部２９は、ロボット座標系変換処理部２８から供給された情報に基づいて、ロボット制御装置１３に出力する情報を生成し、ロボット制御装置１３に出力する。

このように、３次元画像解析装置１２は、対象物２の距離画像および撮像画像を取得する画像データ取得部２２と、画像データ取得部２２により取得された距離画像および撮像画像の少なくとも一方を用いて、画像内の対象物２の領域を検出する対象物領域検出部２３と、画像データ取得部２２により取得された距離画像および撮像画像、ならびに、対象物領域検出部２３による画像内の対象物２の領域の検出結果に基づいて、対象物２に該当する３次元点群を取得する３次元点群取得部２４と、３次元点群取得部２４により取得された対象物２に該当する３次元点群に基づいて、対象物２の姿勢を検出するために用いられる平面を表す平面方程式を算出する平面方程式算出部２５と、平面方程式算出部２５により算出された平面方程式を用いて、対象物２の姿勢を検出する姿勢検出部２６と、画像データ取得部２２により取得された撮像画像、３次元点群取得部２４により取得された対象物２に該当する３次元点群、ならびに、平面方程式算出部２５により算出された平面方程式に基づいて、ピッキングまたはデバンニングを行うロボット１４が対象物２を把持するための位置情報を検出するピッキング位置検出部２７とを有するので、ピッキングまたはデバンニングの対象物２が複数の形状を有する場合や、対象物２が、例えば、紛体等が封入された袋等の３ＤＣＡＤモデルが存在しない場合でも、ピッキング処理またはデバンニング処理をロボット１４に実行させることができる。

次に、図５のフローチャートを参照して、３次元画像解析装置１２の処理について説明する。

ステップＳ１において、画像データ取得部２２は、３次元画像撮像システム１１から、距離画像と撮像画像を取得し、対象物領域検出部２３に供給する。画像データ取得部２２が３次元画像撮像システム１１から取得する距離画像は、例えば、図３のようにあらわされる。

ステップＳ２において、対象物領域検出部２３は、画像データ取得部２２から供給された距離画像および撮像画像の少なくとも一方を用いて、例えば、上述したMean-Shift法の関数、２値化、ラプラシアン等のエッジ検出、または、ＦＰＭを用いたサーチなどの方法を用いて、画像内の対象物２の領域を検出する。対象物領域検出部２３は、取得した画像データから、例えば、図４に示されるような、対象物２の領域を検出する。対象物領域検出部２３は、対象物２の領域の検出結果、ならびに、距離画像および撮像画像を、３次元点群取得部２４に供給する。また、このとき、対象物領域検出部２３は、撮像画像中に、対象物２が複数含まれている場合、対象物２のうちのいずれか１つのみを検出する。対象物領域検出部２３は、例えば、複数の対象物２が積み重なって置かれていた場合においては、距離画像に基づいて、ロボット１４が、一番上の対象物２からピッキングまたはデバンニングを行うことができるように、距離が３次元画像撮像システム１１から一番近いものから順番に、対象物２として検出する。

ステップＳ３において、３次元点群取得部２４は、対象物領域検出部２３から供給された、対象物２の領域の検出結果、ならびに、距離画像および撮像画像を基に、検出した対象物２の領域内の３次元点群を取得し、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像を、平面方程式算出部２５、および、ピッキング位置検出部２７に供給する。

ステップＳ４において、平面方程式算出部２５は、３次元点群から、対象物２のピッキングまたはデバンニングにおいて、式（１）を用いて説明した姿勢を検出するために用いられる平面を表す平面方程式ａｘ + ｂｙ + ｃｚ + ｄ = ０のａ，ｂ，ｃ，ｄを求め、式（１）が成立するａ，ｂ，ｃ，ｄの値の算出結果、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像を、姿勢検出部２６およびピッキング位置検出部２７に供給する。この時、平面方程式算出部２５は、最小二乗法、または、ロバスト推定法を用いて、式（１）が成立するａ，ｂ，ｃ，ｄの値を求める。３次元画像解析装置１２が図示しない操作入力部を備えることにより、平面方程式算出部２５は、最小二乗法、または、ロバスト推定法のいずれを用いて、式（１）が成立するａ，ｂ，ｃ，ｄの値を求めるかを、ユーザにより選択可能なようにしてもよい。

ステップＳ５において、姿勢検出部２６は、平面方程式算出部２５から供給された式（１）が成立するａ，ｂ，ｃ，ｄの値の算出結果、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像に基づいて、式（２）〜式（２２）を用いて説明したように、平面の法線（ａ, ｂ, ｃ）から、対象物２の姿勢（Ｒｘ,Ｒｙ,Ｒｚ）を求め、平面方程式算出部２５から供給された、平面方程式のａ，ｂ，ｃ，ｄの値、対象物２に該当する３次元点群、ならびに、距離画像および撮像画像とともに、ピッキング位置検出部２７に供給する。

ステップＳ６において、ピッキング位置検出部２７は、ａ，ｂ，ｃ，ｄの値、対象物２に該当する３次元点群、距離画像および撮像画像、ならびに、姿勢（Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ））の供給を受け、３次元点群から、ロボット１４がピッキングまたはデバンニングを行う場合の位置情報（ｘ，ｙ，ｚ）を算出する。具体的には、ピッキング位置検出部２７は、ロボット１４がピッキングを行う場合、ピッキングパットを対象物２に接するための位置情報であるピッキング位置（ｘ,ｙ,ｚ）を検出し、三次元空間の座標（ｘ，ｙ，ｚ）を、ａ，ｂ，ｃ，ｄの値、対象物２に該当する３次元点群、および、距離画像および撮像画像、ならびに、姿勢（Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ））とともに、ロボット座標系変換処理部２８に供給する。

ピッキング位置検出部２７が位置情報（ｘ，ｙ，ｚ）を算出する方法は、いずれの方法であってもよいが、例えば、式（２３）〜式（３０）を用いて説明したように、透視投影モデルを利用することが可能である。

ステップＳ７において、ロボット座標系変換処理部２８は、ピッキング位置検出部２７から供給された、図１を用いて説明したワールド座標系の各情報のうち、ロボット制御装置１３がロボット１４の視点での座標系であるロボット座標系として扱う情報であるピッキング位置（ｘ，ｙ，ｚ）および姿勢（Ｒｘ,Ｒｙ,Ｒｚ）を、ロボット座標系に変換し、出力処理部２９に供給する。

ステップＳ８において、ロボット制御装置１３に出力する情報を生成し、出力処理部２９は、ロボット座標系変換処理部２８から供給された情報に基づいて、ロボット制御装置１３に出力する情報を生成し、ロボット制御装置１３に出力し、処理が終了される。

このような処理により、３次元画像解析装置１２は、ピッキングまたはデバンニングの対象物２が複数の形状を有する場合や、対象物２が、例えば、紛体等が封入された袋等の３ＤＣＡＤモデルが存在しない場合でも、ピッキング処理またはデバンニング処理をロボット１４に実行させることができる。

上述した一連の処理は、ハードウェアにより実行することもできるし、ソフトウェアにより実行することもできる。一連の処理をソフトウェアにより実行する場合には、そのソフトウェアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のパーソナルコンピュータなどに、プログラム記録媒体からインストールされる。

なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。

また、本発明の実施の形態は、上述した実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能である。

１…ピッキングまたはデバンニングシステム、 ２…対象物、 １１…３次元画像撮像システム、 １２…３次元画像解析装置、 １３…ロボット制御装置、 １４…ロボット１４、 ２１…記憶部、 ２２…画像データ取得部、 ２３…対象物領域検出部、 ２４…３次元点群取得部、 ２５…平面方程式算出部、 ２６…姿勢検出部、 ２７…ピッキング位置検出部、 ２８…ロボット座標系変換処理部、 ２９…出力処理部

Claims (8)

1. 対象物に対してピッキングまたはデバンニングを行う装置を制御するための情報を生成するピッキングまたはデバンニング用対象物認識装置において、
   前記対象物の距離画像および撮像画像を取得する画像データ取得手段と、
   前記画像データ取得手段により取得された前記距離画像および前記撮像画像の少なくとも一方を用いて、画像内の前記対象物の領域を検出する対象物領域検出手段と、
   前記画像データ取得手段により取得された前記距離画像および前記撮像画像、ならびに、前記対象物領域検出手段による前記画像内の前記対象物の領域の検出結果に基づいて、前記対象物に該当する３次元点群を取得する３次元点群取得手段と、
   前記３次元点群取得手段により取得された前記対象物に該当する前記３次元点群に基づいて、前記対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出手段と、
   前記平面方程式算出手段により算出された前記平面方程式を用いて、前記対象物の姿勢を検出する姿勢検出手段と、
   前記撮像画像、前記３次元点群、および、前記平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、前記ピッキングまたはデバンニングを行う装置が前記対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出手段と
   を有し、
   前記位置検出手段は、前記撮像画像における前記対象物の重心位置の座標（Ｘｇ，Ｙｇ）、前記位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、前記位置情報（ｘ，ｙ，ｚ）を求める
   ことを特徴とするピッキングまたはデバンニング用対象物認識装置。
2. 請求項１に記載のピッキングまたはデバンニング用対象物認識装置において、前記位置情報（ｘ，ｙ，ｚ）の内、位置情報ｘはＸｇ・ｚ／ｆとして算出され、位置情報ｙはＹｇ・ｚ／ｆとして算出され、位置情報ｚは、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、前記位置情報ｘおよび前記位置情報ｙからｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））として算出される、
   ことを特徴とするピッキングまたはデバンニング用対象物認識装置。
3. 請求項１又は２に記載のピッキングまたはデバンニング用対象物認識装置において、
   前記姿勢検出手段は、Ｘ−Ｙ−Ｚオイラー角を、Ｚ＝Ｒｚ（α）、Ｙ＝Ｒｙ（β）、Ｘ＝Ｒｘ（γ）としたとき、（ｘ，ｙ，ｚ）＝（０，０，１）の点を、Ｒｚ（α）・Ｒｙ（β）・Ｒｘ（γ）で回転させた場合、（ｘ，ｙ，ｚ）＝（ａ，ｂ，ｃ）となるようなβおよびγの値を求めた後、前記対象物の姿勢（Ｒｚ（α・Ｒｙ（β）・Ｒｘ（γ））を算出する
   ことを特徴とするピッキングまたはデバンニング用対象物認識装置。
4. 請求項１〜３のいずれか一項に記載のピッキングまたはデバンニング用対象物認識装置において、
   前記姿勢検出手段により検出された前記姿勢、および、前記位置検出手段により検出された前記位置情報を、前記ピッキングまたはデバンニングを行う装置が認識可能な座標系に変換する座標系変換処理手段と、
   前記座標系変換処理手段により座標系が変換された前記姿勢および前記位置情報の出力処理を実行する出力処理手段と
   をさらに備えることを特徴とするピッキングまたはデバンニング用対象物認識装置。
5. ピッキングまたはデバンニングの対象物を認識する、ピッキングまたはデバンニング用対象物認識方法において、
   前記対象物の距離画像および撮像画像を取得する画像データ取得ステップと、
   前記画像データ取得ステップの処理により取得された前記距離画像および前記撮像画像の少なくとも一方を用いて、画像内の前記対象物の領域を検出する対象物領域検出ステップと、
   前記画像データ取得ステップの処理により取得された前記距離画像および前記撮像画像、ならびに、前記対象物領域検出ステップの処理による前記画像内の前記対象物の領域の検出結果に基づいて、前記対象物に該当する３次元点群を取得する３次元点群取得ステップと、
   前記３次元点群取得ステップの処理により取得された前記対象物に該当する前記３次元点群に基づいて、前記対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出ステップと、
   前記平面方程式算出ステップの処理により算出された前記平面方程式を用いて、前記対象物の姿勢を検出する姿勢検出ステップと、
   前記距離画像、前記撮像画像、前記３次元点群、および、前記平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、前記ピッキングまたはデバンニングを行う装置が前記対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出ステップと
   を有し、
   前記位置検出ステップは、前記撮像画像における前記対象物の重心位置の座標（Ｘｇ，Ｙｇ）、前記位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、前記位置情報（ｘ，ｙ，ｚ）を求める
   ことを特徴とするピッキングまたはデバンニング用対象物認識方法。
6. 請求項５に記載のピッキングまたはデバンニング用対象物認識方法において、Ｘｇ・ｚ／ｆに基づいて位置情報ｘを算出し、Ｙｇ・ｚ／ｆに基づいて位置情報ｙを算出し、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、前記位置情報ｘおよび前記位置情報ｙから導き出されるｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））に基づいて位置情報ｚを算出する、
   ことを特徴とするピッキングまたはデバンニング用対象物認識方法。
7. ピッキングまたはデバンニングを行う対象物を認識するコンピュータに、
   前記対象物の距離画像および撮像画像を取得する画像データ取得ステップと、
   前記画像データ取得ステップの処理により取得された前記距離画像および前記撮像画像の少なくとも一方を用いて、画像内の前記対象物の領域を検出する対象物領域検出ステップと、
   前記画像データ取得ステップの処理により取得された前記距離画像および前記撮像画像、ならびに、前記対象物領域検出ステップの処理による前記画像内の前記対象物の領域の検出結果に基づいて、前記対象物に該当する３次元点群を取得する３次元点群取得ステップと、
   前記３次元点群取得手段により取得された前記対象物に該当する前記３次元点群に基づいて、前記対象物の姿勢を検出するために用いられる平面を表す平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０（ａ，ｂ，ｃ，ｄ：係数）を求める平面方程式算出ステップと、
   前記平面方程式算出ステップの処理により算出された前記平面方程式を用いて、前記対象物の姿勢を検出する姿勢検出ステップと、
   前記撮像画像、前記３次元点群、および、前記平面方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０に基づいて、前記ピッキングまたはデバンニングを行う装置が前記対象物を把持するための位置情報（ｘ，ｙ，ｚ）を検出する位置検出ステップと
   を含む処理を実行させるとともに、
   前記位置検出ステップの処理として、前記撮像画像における前記対象物の重心位置の座標（Ｘｇ，Ｙｇ）、前記位置情報（ｘ，ｙ，ｚ）及び３次元空間における焦点距離ｆの関係を示すＸｇ＝ｆ・ｘ／ｚ及びＹｇ＝ｆ・ｙ／ｚと、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０とから、前記位置情報（ｘ，ｙ，ｚ）を求める処理を実行させるプログラム。
8. 請求項７に記載のプログラムにおいて、前記位置情報（ｘ，ｙ，ｚ）を求める処理は、Ｘｇ・ｚ／ｆに基づいて位置情報ｘを算出する処理と、Ｙｇ・ｚ／ｆに基づいて位置情報ｙを算出する処理と、前記平面方程式であるａｘ＋ｂｙ＋ｃｚ＋ｄ＝０、前記位置情報ｘおよび前記位置情報ｙから導き出されるｄ・１／（（a・Ｘｇ／ｆ）＋（ｂ・Ｙｇ／ｆ）＋ｃ））に基づいて位置情報ｚを算出する処理を含む、
   ことを特徴とするプログラム。

Images