(57)【要約】【課題】移動するワークのピッキングに対応することのできるピッキング装置を提供する。【解決手段】第一のステレオカメラ６を用いた初期段階の検出操作でワーク２の位置および姿勢を正確に認識し、ワーク２の移動位置に追従させてハンド４を移動させながら第二のステレオカメラ７を繰り返し作動させてワーク２の位置および姿勢のズレ量を求め、位置および姿勢のズレ量を補正して最終的なピッキング動作の目標位置とハンド４の姿勢を制御する。【選択図】　　　　図１
2004001122.tif 000002
【特許請求の範囲】
【請求項１】
ワークを把持するハンドを有する多関節型の産業用ロボットと、前記ワークの位置および姿勢を把握するためのカメラと、前記産業用ロボットおよびカメラを駆動制御するコンピュータシステムとを備えたピッキング装置において、
前記ワークの載置位置の近傍に大きな視差を有する第一のステレオカメラを固定的に設置する一方、前記産業用ロボットにおける前記ハンドの近傍に相対的に視差の小さな第二のステレオカメラを配備し、
前記コンピュータシステムには、前記第一のステレオカメラによって撮影された画像に基いて前記ワークの位置および姿勢を求める位置姿勢演算手段と、
前記位置姿勢演算手段によって求められたワーク位置の近傍まで前記ハンドを移動させ前記第二のステレオカメラを作動させて画像を撮影し、前記位置姿勢演算手段によって求められたワークの位置および姿勢に対するワークの実位置および実姿勢のズレ量を求めるズレ量演算手段と、
前記位置姿勢演算手段によって求められたワークの位置および姿勢と前記ズレ量演算手段によって求められたワークの実位置および実姿勢のズレ量とに基いて最終的なワークの位置および姿勢を求める位置姿勢補正手段と、
前記位置姿勢補正手段によって求められたワークの位置および姿勢に基いてピッキング動作の目標位置とハンドの姿勢を制御するピッキング動作制御手段とを設けたことを特徴とするピッキング装置。
【請求項２】
ワークを搬送するコンベアと、ワークを把持するハンドを有する多関節型の産業用ロボットと、前記ワークの位置および姿勢を把握するためのカメラと、前記産業用ロボットおよびカメラを駆動制御するコンピュータシステムとを備えたピッキング装置において、
前記コンベア上を搬送されるワークを所定位置で撮影する大きな視差を有する第一のステレオカメラを固定的に設置する一方、前記産業用ロボットにおける前記ハンドの近傍に相対的に視差の小さな第二のステレオカメラを配備し、
前記コンピュータシステムには、前記第一のステレオカメラによって撮影された画像に基いて前記所定位置におけるワークの位置および姿勢を求める位置姿勢演算手段と、
前記位置姿勢演算手段によって求められたワーク位置と前記コンベアの搬送速度と経過時間とに基いてワークの現在位置を求め、前記ワークの現在位置に追従させて前記ハンドを移動させる追跡動作制御手段と、
前記追跡動作制御手段の作動中に前記第二のステレオカメラを繰り返し作動させ前記ハンドおよびコンベアと共に併進する座標系内で画像を撮影し、前記位置姿勢演算手段によって求められたワークの位置および姿勢に対するワークの実位置および実姿勢のズレ量を求めるズレ量演算手段と、
前記位置姿勢演算手段によって求められたワークの位置および姿勢と前記ズレ量演算手段によって求められたワークの実位置および実姿勢のズレ量とに基いて前記ハンドおよびコンベアと共に併進する座標系内における最終的なワークの位置および姿勢を求める位置姿勢補正手段と、
前記位置姿勢補正手段によって求められたワークの位置および姿勢に基いてピッキング動作の目標位置とハンドの姿勢を制御するピッキング動作制御手段とを設けたことを特徴とするピッキング装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ワークの位置および姿勢を把握してワークを把持するピッキング装置の改良、特に、位置および姿勢の検出に関わる精度と処理速度を向上させるための改良に関する。
【０００２】
【従来の技術】
２つのカメラシステムを利用してワークの位置および姿勢の把握精度を向上させる技術としては、例えば、特開２０００−２８８９７４に開示されるようなロボット装置が既に提案されている。
【０００３】
特開２０００−２８８９７４のロボット装置は、ロボットの手首の先端に２次元視覚センサからなる第一の視覚センサと、２次元視覚センサまたは３次元視覚センサからなる第二の視覚センサとを設け、第一の視覚センサでワークの大まかな位置および姿勢を検出し、更に、第一の視覚センサで求められたワーク位置の近傍にロボットの手首を移動させ、第二の視覚センサを作動させてワークの位置および姿勢を厳密に計測して産業用ロボットによるピッキング動作を行わせるようにしたものである。
【０００４】
【発明が解決しようとする課題】
しかし、特開２０００−２８８９７４のロボット装置では、第一の視覚センサを２次元視覚センサで構成しているため、第一の視覚センサを用いた初期段階の検出操作で精度の高い認識を行うことが難しく、この結果、第二の視覚センサを利用して行う精密な測定操作に際して撮影対象となる領域を効果的に絞り込むことが困難であり、画像の取り込みや演算処理に必要とされる所要時間が増大するといった弊害が発生する。
【０００５】
また、画像の取り込みや演算処理の所要時間が増大する結果、移動するワークの位置や姿勢を検出してピッキング動作を行うことは事実上不可能である。
【０００６】
【発明の目的】
そこで、本発明の目的は、前記従来技術の欠点を解消し、画像の取り込みや演算処理に必要とされる所要時間を短縮し、高速かつ高精度でワークの位置および姿勢を認識することが可能であって、移動するワークのピッキングにも対応することのできるピッキング装置を提供することにある。
【０００７】
【課題を解決するための手段】
本発明は、ワークを把持するハンドを有する多関節型の産業用ロボットと、ワークの位置および姿勢を把握するためのカメラと、産業用ロボットおよびカメラを駆動制御するコンピュータシステムとを備えたピッキング装置であり、前記目的を達成するため、特に、
ワークの載置位置の近傍に大きな視差を有する第一のステレオカメラを固定的に設置する一方、産業用ロボットにおけるハンドの近傍に相対的に視差の小さな第二のステレオカメラを配備し、
前記コンピュータシステムには、第一のステレオカメラによって撮影された画像に基いてワークの位置および姿勢を求める位置姿勢演算手段と、
前記位置姿勢演算手段によって求められたワーク位置の近傍までハンドを移動させ第二のステレオカメラを作動させて画像を撮影し、前記位置姿勢演算手段によって求められたワークの位置および姿勢に対するワークの実位置および実姿勢のズレ量を求めるズレ量演算手段と、
前記位置姿勢演算手段によって求められたワークの位置および姿勢と前記ズレ量演算手段によって求められたワークの実位置および実姿勢のズレ量とに基いて最終的なワークの位置および姿勢を求める位置姿勢補正手段と、
前記位置姿勢補正手段によって求められたワークの位置および姿勢に基いてピッキング動作の目標位置とハンドの姿勢を制御するピッキング動作制御手段とを設けたことを特徴とする構成を有する。
【０００８】
以上の構成において、まず、ワークの載置位置の近傍に設置された第一のステレオカメラがワーク周辺の画像を撮影し、この画像を位置姿勢演算手段が解析してワークの位置および姿勢を求める。第一のステレオカメラは産業用ロボットとは独立して固定的に設置されているので、ステレオカメラを構成するカメラ間の視差を大きく取ることが可能であり、第一のステレオカメラを用いた初期段階の検出操作におけるワークの位置および姿勢の認識精度が向上する。
次いで、ズレ量演算手段が、前記位置姿勢演算手段によって求められたワーク位置の近傍までハンドを移動させ、第二のステレオカメラを作動させて画像を撮影し、前記位置姿勢演算手段によって求められたワークの位置および姿勢に対するワークの実位置および実姿勢のズレ量を求める。第一のステレオカメラによるワークの位置および姿勢の認識精度が高いので、第二のステレオカメラは撮影対象となる領域を狭く絞り込んで撮影するだけで確実にワークを捕捉することができる。つまり、解析の対象となる画像データの容量が軽減されることにより、ワークの実位置および実姿勢のズレ量の演算に必要とされる所要時間が短縮され、高速の演算処理が実現されることになる。
第二のステレオカメラは産業用ロボット上でハンドの近傍に取り付けられるので、ステレオカメラを構成するカメラ間の視差を第一のステレオカメラのように大きく取ることはできないが、ワークに接近した状態でワークを撮影することができるので、十分な精度でワークの位置および姿勢のズレを検出することができる。
そして、位置姿勢演算手段によって求められたワークの位置および姿勢とズレ量演算手段によって求められたワークの実位置および実姿勢のズレ量とに基いて位置姿勢補正手段が最終的なワークの位置および姿勢を求め、ピッキング動作制御手段が、位置姿勢補正手段によって求められたワークの位置および姿勢に基いてピッキング動作の目標位置とハンドの姿勢を制御する。
【０００９】
また、ワークを搬送するコンベアと、ワークを把持するハンドを有する多関節型の産業用ロボットと、ワークの位置および姿勢を把握するためのカメラと、産業用ロボットおよびカメラを駆動制御するコンピュータシステムとを備えたピッキング装置の場合においては、
コンベア上を搬送されるワークを所定位置で撮影する大きな視差を有する第一のステレオカメラを固定的に設置する一方、産業用ロボットにおけるハンドの近傍に相対的に視差の小さな第二のステレオカメラを配備し、
前記コンピュータシステムには、第一のステレオカメラによって撮影された画像に基いて前記所定位置におけるワークの位置および姿勢を求める位置姿勢演算手段と、
前記位置姿勢演算手段によって求められたワーク位置とコンベアの搬送速度と経過時間とに基いてワークの現在位置を求め、ワークの現在位置に追従させてハンドを移動させる追跡動作制御手段と、
前記追跡動作制御手段の作動中に第二のステレオカメラを繰り返し作動させハンドおよびコンベアと共に併進する座標系内で画像を撮影し、前記位置姿勢演算手段によって求められたワークの位置および姿勢に対するワークの実位置および実姿勢のズレ量を求めるズレ量演算手段と、
前記位置姿勢演算手段によって求められたワークの位置および姿勢と前記ズレ量演算手段によって求められたワークの実位置および実姿勢のズレ量とに基いて前記ハンドおよびコンベアと共に併進する座標系内における最終的なワークの位置および姿勢を求める位置姿勢補正手段と、
前記位置姿勢補正手段によって求められたワークの位置および姿勢に基いてピッキング動作の目標位置とハンドの姿勢を制御するピッキング動作制御手段とを設けるようにする。
【００１０】
このような構成を適用した場合、位置姿勢演算手段によって求められたワーク位置とコンベアの搬送速度と経過時間とに基いて追跡動作制御手段がワークの現在位置を求め、ワークの現在位置に追従させてハンドを移動させるので、ワークが移動した場合であっても、ワークとハンドとの相対的な位置関係は、コンベア上のワークの位置および姿勢に変化がない限り常に一定の状態に保たれる。
従って、第一のステレオカメラによる撮影の開始からピッキング動作制御手段によるハンドの駆動制御までのタイムラグによってハンドとワークの位置関係に変化が生じることはなく、定位置にワークをおいて第二のステレオカメラでワークの実位置および実姿勢のズレ量を算出してピッキング動作の目標位置とハンドの姿勢を制御する場合と同様、確実なピッキング動作を行うことができる。
また、ワークと第二のステレオカメラとの相対的な位置関係に変化が生じた場合には、荷崩れ等によってコンベア上のワークの位置や姿勢に変化が生じたことを意味し、この位置や姿勢のズレ量に応じて位置姿勢補正手段がピッキング動作の目標位置やハンドの姿勢を補正するので、コンベアで搬送されるワークに荷崩れ等が生じた場合であっても確実なピッキング動作を行うことが可能となる。
また、ズレ量演算手段は、追跡動作制御手段が作動する間、第二のステレオカメラを繰り返し作動させて、ハンドおよびコンベアと共に併進する座標系内で画像を撮影してワークの実位置および実姿勢のズレ量を求めるようにしているので、荷崩れ等によってコンベア上のワークの位置や姿勢に変化が生じたような場合であっても、直前の処理で検出されたワークの位置および姿勢を初期位置としてズレ量の演算処理を実行することができ、ワークの実位置および実姿勢のズレ量の演算に必要とされる所要時間を大幅に短縮することができる。また、コンベア上のワークの位置や姿勢に結果として大きな変化が生じた場合であっても、一撮影周期間の位置および姿勢のズレ量は大きくないので、位置および姿勢の不連続的な変化に伴うデータ探索の負荷の増大によるエラーを発生することなく、ワークの実位置および実姿勢のズレ量を追跡することができる。
【００１１】
【発明の実施の形態】
以下、図面を参照して本発明の実施形態について詳細に説明する。図１は本発明を適用した一実施形態のピッキング装置１の構成を簡略化して示した模式図である。
【００１２】
この実施形態のピッキング装置１は、ワーク２を搬送するコンベア３と、ワーク２を把持するハンド４を備えた多関節型の産業用ロボット５、ならびに、ワーク２の位置と姿勢を把握するための第一のステレオカメラ６および第二のステレオカメラ７と、産業用ロボット５および第一，第二のステレオカメラ６，７を駆動制御するコンピュータシステム８によって構成される。
【００１３】
第一のステレオカメラ６は、コンベア３上を搬送されるワーク２が所定位置を通過する際にワーク２を撮影するためのもので、視点を離して視差を大きくした３つのカメラ６ａ，６ｂ，６ｃによって構成され、コンベア３の一側に固定的に設置されている。第一のステレオカメラ６は、コンピュータシステム８の一部を構成する数値制御装置９からの指令で画像を取り込み、撮影した画像のデータを数値制御装置９に転送する。
【００１４】
第二のステレオカメラ７も第一のステレオカメラ６と同様に３つのカメラ７ａ，７ｂ，７ｃによって構成されるが、産業用ロボット５上でハンド４の近傍に取り付けてハンド４と一体的に移動させる関係上、カメラ７ａ，７ｂ，７ｃ間の視差は相対的に小さく、図２に示されるようにコンパクトに纏められている。
【００１５】
コンピュータシステム８は、産業用ロボット５に付属するロボット制御装置１０と汎用の数値制御装置９とによって構成され、このうち、ロボット制御装置１０は専ら産業用ロボット５の駆動制御に、また、数値制御装置９は、画像処理等を始めとする付加機能の実現のために使用され、両者間はデータ転送可能に接続されている。
【００１６】
この実施形態では市販のロボット制御装置１０に改造を加えずに利用することを前提とし、画像処理等の付加機能を実現するための数値制御装置９をロボット制御装置１０に追加してコンピュータシステム８としているが、画像処理等の付加機能を実装することを含めてロボット制御装置１０を始めから設計し直すような場合には、単体のロボット制御装置１０それ自体をコンピュータシステム８とすることも可能である。また、これとは逆に数値制御装置９に産業用ロボット５の駆動制御機能を実装し、単体の数値制御装置９をコンピュータシステム８としてもよい。
【００１７】
このように、コンピュータシステム８に構成上の格別の制限はないが、ここでは、一例としてロボット制御装置１０と数値制御装置９でコンピュータシステム８を構成した場合の構造について簡単に説明する。
【００１８】
コンピュータシステム８の一部を構成するロボット制御装置１０は、図３に示されるように、基本的な制御プログラムを格納したＲＯＭ１１と、ユーザが作成した動作プログラム等を記憶するための不揮発性メモリ１２、および、不揮発性メモリ１２に記憶された動作プログラムとＲＯＭ１２に格納されている制御プログラムとに基いて各種の演算処理を実行するＣＰＵ１３と、演算過程のデータの一時記憶等に用いられるＲＡＭ１４、ならびに、ＣＰＵ１３からの指令に応じて産業用ロボット５の各軸のサーボモータ（図示せず）を駆動制御する軸制御回路１５とを備える。
【００１９】
軸制御回路１５はＣＰＵ１３から与えられる移動指令に基いて各軸のサーボモータを駆動し、産業用ロボット５のハンド４の位置および姿勢と移動速度ならびに駆動トルクを制御する。
【００２０】
表示装置付手動データ入力装置１６は動作プログラムの編集作業等に使用され、また、入出力インターフェイス１７はロボット制御装置１０と数値制御装置９との間のデータ転送に使用される。
【００２１】
数値制御装置９は、図４に示されるように、システムプログラムを格納したＲＯＭ１８と、画像の解析に用いられるアプリケーションプログラム等を記憶した不揮発性メモリ１９、および、演算処理用のＣＰＵ２０と、演算過程のデータの一時記憶等に用いられるＲＡＭ２１、ならびに、表示装置付手動データ入力装置２２と、入出力インターフェイス２３、更に、画像取り込み用のフレームメモリ２４によって構成される。フレームメモリ２４はＲＡＭ２１の一部によって構成してもよい。
【００２２】
入出力インターフェイス２３は数値制御装置９とロボット制御装置１０とを接続してデータ転送を行うためのもので、第一のステレオカメラ６が有する３つのカメラ６ａ，６ｂ，６ｃで撮影された画像データと第二のステレオカメラ７が有する３つのカメラ７ａ，７ｂ，７ｃで撮影された画像データも、入出力インターフェイス２３を介してフレームメモリ２４に送られるようになっている。
【００２３】
フレームメモリ２４内のデータ配列の一例を図１０に示す。この実施形態では、６４０ドット×４８０ドットの３組の画素データを２５６階調の濃度で記憶するようにしているが、解像度や階調等の画素の記憶条件については、必要とされる分解能等に応じて適宜に決めるものとする。
【００２４】
また、図１に示されるように、コンベア３の近傍には、このコンベア３で搬送されるワーク２が所定の撮影位置に到達したことを検出するための定位置検出センサ２５が設けられており、この定位置検出センサ２５からの信号も入出力インターフェイス２３を介して数値制御装置９のＣＰＵ２０に入力されるようになっている。
【００２５】
不揮発性メモリ１９には、取り扱いの対象となる可能性のある様々な形状のワーク２に対応した３次元形状データが予め幾つか登録されている。不揮発性メモリ１９に記憶されている３次元形状データの一例を図１１に示す。
【００２６】
図５〜図７は数値制御装置９側のＣＰＵ２０が実行するピッキング制御の概略を示したフローチャート、また、図８〜図９は数値制御装置９からの指令を受けたロボット制御装置１０のＣＰＵ１３が実行するパルス分配処理の概略を示したフローチャートである。
【００２７】
次に、図５〜図９を参照して位置姿勢演算手段，ズレ量演算手段，位置姿勢補正手段としてのＣＰＵ２０の処理動作、および、追跡動作制御手段としてのＣＰＵ２０およびＣＰＵ１３の処理動作、ならびに、ピッキング動作制御手段としてのＣＰＵ１３の処理動作について説明する。
【００２８】
ピッキング制御を開始したＣＰＵ２０は、まず、定位置検出センサ２５からの定位置検出信号が入力されているか否か、つまり、コンベア３で搬送されるワーク２が所定の撮影位置に到達したか否かを判定し（ステップａ１）、定位置検出信号が検出されていなければ、そのまま定位置検出信号の入力を待つ待機状態を保持する。
【００２９】
そして、ワーク２が所定の撮影位置に到達して定位置検出センサ２５からの定位置検出信号が確認されると、ＣＰＵ２０は経過時間計測タイマｔをリスタートさせてワーク２が所定の撮影位置に到達してからの経過時間の計測を開始すると共に（ステップａ２）、第一のステレオカメラ６を作動させ、所定の撮影位置にあるワーク２を撮影してカメラ６ａ，６ｂ，６ｃから画像を取り込み（ステップａ３）、これらの画像データをフレームメモリ２４に記憶する（ステップａ４）。図１２に取り込まれた画像の一例を示す。
【００３０】
次いで、位置姿勢演算手段として機能するＣＰＵ２０が、画像の解析に用いられるアプリケーションプログラムを起動し、フレームメモリ２４に記憶されているワーク２の画像データを解析して３次元情報を求め（ステップａ５）、この３次元情報と不揮発性メモリ１９に登録されている複数の３次元形状データとを照合して現時点で作業対象となっているワーク２の形状を特定した後（ステップａ６）、撮影時点つまりステップａ３の実行時点におけるワーク２の位置Ｘ，Ｙ，Ｚとワーク２の姿勢α，β，γを求める（ステップａ７）。フレームメモリ２４に記憶されているワーク２の画像データを解析して得られる３次元情報の一例を図１３に、また、ワーク２の３次元情報と不揮発性メモリ１９に登録されている３次元形状データとの照合例を図１４に示す。
【００３１】
但し、ワーク２の位置Ｘ，Ｙ，Ｚは固定式の第一のステレオカメラ６を基準とした座標系であり、各座標系の向きは図１に示される通りである。また、ワーク２の姿勢α，β，γは、αがＸ軸回りの姿勢の傾き、βがＹ軸周りの姿勢の傾き、また、γがＺ軸回りの姿勢の傾きである。
【００３２】
前述した通り、第一のステレオカメラ６は産業用ロボット５とは独立してコンベア３の側方に固定的に設置されているので、ステレオカメラ６を構成するカメラ６ａ，６ｂ，６ｃ間の視差を大きく取ることができ、第一のステレオカメラ６を用いた検出操作におけるワーク２の位置および姿勢の認識精度を大幅に向上させることができる。
【００３３】
次いで、追跡動作制御手段の一部として機能するＣＰＵ２０は、コンベア３の搬送速度Ｖ（設定値）と、経過時間計測タイマｔの現在値、ならびに、撮影時点つまりステップａ３の実行時点におけるワーク２のＸ軸座標上の位置Ｘとに基いて、現時点におけるワーク２のＸ軸座標上の位置Ｘを更新して求める（ステップａ８）。
【００３４】
そして、ＣＰＵ２０は、入出力インターフェイス２３を介してロボット制御装置１０にアクセスし、軸制御回路１５に設けられた各軸の現在位置記憶レジスタからハンド４の現在位置ｘｎ，ｙｎ，ｚｎの値を読み込み（ステップａ９）、ワーク２の現在位置Ｘ，Ｙ，Ｚの各軸の要素からハンド４の現在位置ｘｎ，ｙｎ，ｚｎの値を減じ、更に、Ｚ軸成分に関してはオフセット量ｚ０を加算して、ハンド４をハンド４の現在位置ｘｎ，ｙｎ，ｚｎからワーク２の現在位置Ｘ，Ｙ，Ｚよりもｚ０だけ上方に移動させるために必要とされる移動量Ｄｘ，Ｄｙ，Ｄｚを求める（ステップａ１０）。そして、Ｄｘ，Ｄｙ，Ｄｚをインクリメンタル移動指令としてロボット制御装置１０に送信する（ステップａ１１）。
【００３５】
また、ＣＰＵ２０は、コンベア３の搬送速度Ｖに相当する速度でハンド４にＸ軸方向の送りを掛ける際に必要とされる単位時間当たりの分配パルス量Ｄ０ｘの値を求め、この値をロボット制御装置１０に送信する（ステップａ１２）。
【００３６】
次いで、ＣＰＵ２０は、撮影回数積算カウンタＣの値を零に初期化し（ステップａ１３）、入出力インターフェイス２３を介してロボット制御装置１０にアクセスして、軸制御回路１５に設けられた分配処理用の各軸の位置偏差記憶レジスタの値を読み込み（ステップａ１４）、その値がインポジション幅の範囲に収まっているか否かを判定する（ステップａ１５）。
【００３７】
ステップａ１１の処理でロボット制御装置１０にインクリメンタル移動指令を送信した直後の現段階では各軸の位置偏差は増大しており、その値がインポジション幅の範囲に収まることはないので、ＣＰＵ２０は、ステップａ１４〜ステップａ１５の処理を繰り返し実行して、各軸の位置偏差記憶レジスタの値がインポジション幅の範囲に収まるのを待機することになる。
【００３８】
一方、ステップａ１１の処理でロボット制御装置１０に送信されたインクリメンタル移動指令Ｄｘ，Ｄｙ，Ｄｚは、ロボット制御装置１０側のＣＰＵ１３によりステップｂ１の処理で検出され、これを検出したＣＰＵ１３は、各インクリメンタル移動指令Ｄｘ，Ｄｙ，Ｄｚを各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚにセットする（ステップｂ２）。そして、更に、数値制御装置９から送信された分配パルス量Ｄ０ｘの値を読み込み（ステップｂ３）、この値を記憶した後（ステップｂ４）、軸制御回路１５のトルクリミット回路にトルクリミットを設定して（ステップｂ５）、公知のパルス分配処理を開始する（ステップｂ６）。
【００３９】
ステップｂ６のパルス分配処理では、各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚに記憶されたインクリメンタル移動指令Ｄｘ，Ｄｙ，ＤｚをＤｘ：Ｄｙ：Ｄｚの比率で単位時間当たりの微小な移動指令ΔＤｘ，ΔＤｙ，ΔＤｚとして各軸のサーボモータにパルス分配周期毎の移動指令として送出する処理が繰り返され、その都度、各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚから分配済みのパルス量ΔＤｘ，ΔＤｙ，ΔＤｚ（パルスコーダからのフィードバックパルス）が減算される。
【００４０】
そして、１周期分のパルス分配処理がステップｂ６で完了する度、追跡動作制御手段の一部として機能するＣＰＵ１３が、Ｘ軸に関する位置偏差記憶レジスタＥｒｘに分配パルスＤ０ｘの値を加算する（ステップｂ７）。
【００４１】
次いで、ＣＰＵ１３は、数値制御装置９からの補正指令ｄｘ，ｄｙ，ｄｚ，Ｄα，Ｄβ，Ｄγが入力されているか否か（ステップｂ８）、および、数値制御装置９からのピッキング指令やプレイス指令が入力されているか否か（ステップｂ１０）を判定することになるが、この段階では何れの指令も入力されないので、追跡動作制御手段の一部として機能するＣＰＵ１３は、ステップステップｂ６〜ステップｂ８とステップｂ１０の処理のみを繰り返し実行することになる。
【００４２】
まず、ステップｂ６およびステップｂ７の繰り返し処理が開始された直後の段階では、各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚの値は実質的にＤｘ，Ｄｙ，Ｄｚであって位置偏差が十分に大きいため、ハンド４はトルクリミットを越えない範囲の強い力つまり速い速度で駆動制御され、コンベア３と共に移動するワーク２を追って其の上方ｚ０の位置まで直線補間によって移動されることになる。このとき、ワーク２はコンベア３の搬送によってＸ軸方向に移動するが、ステップｂ７の処理によりＸ軸の位置偏差記憶レジスタＥｒｘの値が分配周期毎にＤ０ｘずつ加算されるので、Ｄｘ，Ｄｙ，Ｄｚの位置偏差が解消された場合であっても位置偏差記憶レジスタＥｒｘにはＤ０ｘの位置偏差が定常的に滞留することになり、この位置偏差Ｄ０ｘを解消しようとする軸制御回路１５の働きにより、ハンド４がコンベア３上のワーク２と同様にしてＸ軸方向に移動速度Ｖで送られることになる。
【００４３】
このようにして、位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚの位置偏差が概ね解消され、ハンド４がワーク２の上方ｚ０の目標位置に追い付き、位置偏差記憶レジスタＥｒｘに滞留する位置偏差Ｄ０ｘのみによってハンド４がワーク２と一定の位置関係を保ってＸ軸方向に移動するようになると、各軸の位置偏差記憶レジスタの値がインポジション幅の範囲に収まるようになり、数値制御装置９側のＣＰＵ２０がステップａ１５の処理でこれを検出する。
【００４４】
そこで、ズレ量演算手段として機能するＣＰＵ２０は、ワーク２と一定の位置関係を保って移動する第二のステレオカメラ７を作動させ、ワーク２を撮影してカメラ７ａ，７ｂ，７ｃから画像を取り込み（ステップａ１６）、これらの画像データをフレームメモリ２４に記憶する（ステップａ１７）。
【００４５】
次いで、ズレ量演算手段として機能するＣＰＵ２０が、画像の解析に用いられるアプリケーションプログラムを起動し、例えば、フレームメモリ２４に記憶されている画像の中央部近傍を最初の探索領域としてワーク２の画像を探索し、ワーク２の各軸方向の位置のズレ量（Δｘ，Δｙ，Δｚ）と、ステップａ７の処理で求められたワーク２の姿勢（α，β，γ）を基準とする各軸の回りのワーク２の姿勢のズレ量（Δα，Δβ，Δγ）を求める（ステップａ１８）。
【００４６】
この実施形態では、図８に示したパルス分配処理により、ステップａ７の処理で求められたワーク２の位置（Ｘ，Ｙ，Ｚ）を基準として、その上方ｚ０の位置にハンド４を移動させるように駆動制御しているので、ハンド４に対する第二のステレオカメラ７のオフセット量を差し引くことを前提として、基本的には、第二のステレオカメラ７の座標原点（０，０，０）がステップａ７の処理で求められたワーク２の位置（Ｘ，Ｙ，Ｚ）と一致することになる。但し、ここでのＸの値は搬送過程における変動的な値であり、実質的には、Ｘ＝Ｘ（ステップａ７の初期値）＋Ｖ・ｔである。つまり、第二のステレオカメラ７の座標系はハンド４およびコンベア３と共にＸ軸方向に併進する座標系であり、ハンド４に対するステレオカメラ７のオフセット量を差し引くことを前提とした場合、途中で荷崩れ等が生じてコンベア３に対してワーク２が相対移動しない限り、ワーク２は常に第二のステレオカメラ７の画像の中央部に捕捉されることになる。従って、第一のステレオカメラ６による撮影完了後、コンベア３に対するワーク２の位置に変動がない限り、（Δｘ，Δｙ，Δｚ）の値も（Δα，Δβ，Δγ）の値も基本的に零である。
【００４７】
一方、途中で荷崩れ等が生じてコンベア３に対してワーク２が相対移動したり姿勢が崩れたりした場合には、そのズレ量がステップａ１８の処理によって（Δｘ，Δｙ，Δｚ）や（Δα，Δβ，Δγ）として求められる。この場合、ＣＰＵ２０は、（Δｘ，Δｙ，Δｚ）および（Δα，Δβ，Δγ）の値をワーク２の次の探索領域として記憶した後（ステップａ１９）、撮影回数積算カウンタＣの値を１インクリメントし（ステップａ２０）、撮影の繰り返し回数Ｃが設定回数Ｎに達しているか否かを判定する（ステップａ２１）。
【００４８】
そして、撮影回数積算カウンタＣの値が設定回数Ｎに達していなければ、ズレ量演算手段として機能するＣＰＵ２０は、前記と同様にしてステップａ１６〜ステップａ２１の処理を繰り返し実行し、その都度、ワーク２に関する各軸方向の位置のズレ量（Δｘ，Δｙ，Δｚ）と各軸の回りの姿勢のズレ量（Δα，Δβ，Δγ）を求めると共に、これらの値をワーク２の新たな探索領域として更新して記憶していく。
【００４９】
この間、荷崩れ等が生じてコンベア３に対してワーク２が相対移動したり姿勢が崩れたりする場合もあるが、一撮影周期間の位置および姿勢のズレ量は大きくはなく、しかも、直前の処理周期で記憶されたワーク２の位置や姿勢のズレ量（ステップａ１９参照）に基いて、次の処理周期で、其の近傍でワーク２の画像が探索されるので（ステップａ１８参照）、ワーク２の位置および姿勢の不連続的な変化に伴うデータ探索の負荷の増大によってエラーが発生することなく、ワーク２の実位置および実姿勢のズレ量を的確に追跡することができる。
【００５０】
また、第一のステレオカメラ６によるワーク２の位置および姿勢の認識精度が高いため、第二のステレオカメラ７による第１回目の撮影時点でワーク２を的確に視野に納めることができ、しかも、画像データの探索領域を絞り込むことも可能となるため、全体としての処理速度の短縮に有益である。
【００５１】
第二のステレオカメラ７の視差は第一のステレオカメラ６の視差と比べて相対的に小さいが、ワーク２に対して十分に接近して撮影を行うことができるので、十分な精度でワーク２の位置および姿勢のズレを検出することができる。
【００５２】
そして、撮影回数積算カウンタＣの値が設定回数Ｎに達したことがステップａ２１の判定処理によって検出されると、位置姿勢補正手段として機能するＣＰＵ２０は、最後に求められた位置のズレ量、つまり、ハンド４およびコンベア３と共にＸ軸方向に併進する座標系内におけるワーク２の最終的な位置のズレ量Δｘ，Δｙ，Δｚを位置ズレ補正のためのインクリメンタル移動指令ｄｘ，ｄｙ，ｄｚとして記憶し、Ｚ軸方向の補正量に関しては上方へのオフセットｚ０を解消するための補正値−ｚ０を更に加算すると共に、最後に求められた姿勢のズレ量Δα，Δβ，Δγをステップａ７の処理で求められたα，β，γに加算してワーク２の絶対的な姿勢α，β，γを求める（ステップａ２２）。
【００５３】
次いで、位置姿勢補正手段として機能するＣＰＵ２０は、入出力インターフェイス２３を介してロボット制御装置１０にアクセスし、軸制御回路１５に設けられた各軸の現在姿勢記憶レジスタからハンド４の現在姿勢αｎ，βｎ，γｎを読み込み（ステップａ２３）、ワーク２を把持するためにハンド４に必要とされる姿勢α，β，γの各軸の要素からハンド４の現在姿勢αｎ，βｎ，γｎの各軸の要素の値を減じ、ハンド４を現在の姿勢αｎ，βｎ，γｎからワーク２の把持に適した姿勢α，β，γに変化させるために必要とされる姿勢変化量Ｄα，Ｄβ，Ｄγを求め（ステップａ２４）、位置ズレ補正のためのインクリメンタル移動指令ｄｘ，ｄｙ，ｄｚとハンド４の姿勢制御のためのインクリメンタル姿勢変更指令Ｄα，Ｄβ，Ｄγをロボット制御装置１０に送信する（ステップａ２５）。
【００５４】
次いで、ＣＰＵ２０は、入出力インターフェイス２３を介してロボット制御装置１０にアクセスして、軸制御回路１５に設けられた分配処理用の各軸の位置偏差記憶レジスタおよび姿勢偏差記憶レジスタの値を読み込み（ステップａ２６）、その値がインポジション幅の範囲に収まっているか否かを判定する（ステップａ２７）。ステップａ２５の処理でロボット制御装置１０にインクリメンタル移動指令とインクリメンタル姿勢変更指令を送信した直後の現段階では各軸の位置偏差および姿勢偏差は増大しており、その値がインポジション幅の範囲に収まることはないので、ＣＰＵ２０は、ステップａ２６〜ステップａ２７の処理を繰り返し実行して、各軸の位置偏差記憶レジスタおよび姿勢偏差記憶レジスタの値がインポジション幅の範囲に収まるのを待機することになる。
【００５５】
一方、ステップａ２５の処理でロボット制御装置１０に送信されたインクリメンタル移動指令ｄｘ，ｄｙ，ｄｚとインクリメンタル姿勢変更指令Ｄα，Ｄβ，Ｄγは、ロボット制御装置１０側のＣＰＵ１３によりステップｂ８の処理で検出され、これを検出したＣＰＵ１３は、各インクリメンタル移動指令ｄｘ，ｄｙ，ｄｚを各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚに加算すると共に、各軸の姿勢偏差記憶レジスタＥｒα，Ｅｒβ，Ｅｒγにインクリメンタル姿勢変更指令Ｄα，Ｄβ，Ｄγをセットする（ステップｂ９）。
【００５６】
そして、このようにして更新された各軸の位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚと各軸の姿勢偏差記憶レジスタＥｒα，Ｅｒβ，Ｅｒγの値に基いて、ピッキング動作制御手段として機能するＣＰＵ１３が前記と同様にしてステップｂ６〜ステップｂ８とステップｂ１０の処理のみを繰り返し実行する。
【００５７】
この結果、ステップｂ６のパルス分配処理では、ハンド４に対してＸ軸の方向に速度Ｖの送りを掛ける処理と重畳して、Ｘ，Ｙ，Ｚの各軸方向に対してｄｘ，ｄｙ，ｄｚの位置ズレ補正の送りを掛ける処理と、ハンド４の姿勢をＸ，Ｙ，Ｚの各軸の回りにＤα，Ｄβ，Ｄγだけ姿勢変化（回転）させる処理とが並行して行われることになる。
【００５８】
そして、位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚの位置偏差が概ね解消され、ハンド４がワーク２を把持できる位置に達し、また、姿勢偏差記憶レジスタＥｒα，Ｅｒβ，Ｅｒγの姿勢偏差が解消されてハンド４がワーク２の表裏を法線方向から把持できる姿勢に達すると、ハンド４は、前述した位置偏差Ｄ０ｘの働きにより、この状態を保持したまま速度Ｖでワーク２と共にＸ軸方向にのみ移動する。
【００５９】
一方、数値制御装置９側のＣＰＵ２０は、ステップａ２７の判定処理によって位置偏差記憶レジスタＥｒｘ，Ｅｒｙ，Ｅｒｚおよび姿勢偏差記憶レジスタＥｒα，Ｅｒβ，Ｅｒγの偏差がインポジション幅に入ったことを検出し、ロボット制御装置１０に対してピッキング指令とプレイス指令を送信する（ステップａ２８）。
【００６０】
そして、ステップｂ１０の処理でこれらの指令を検出したＣＰＵ１３が、ハンド４を閉鎖させてワーク２を把持させ（ステップｂ１１）、各軸のエラーレジスタをクリアした後（ステップｂ１２）、予め設定されているワーク受け渡し位置にハンド４を移動させてワーク２を置き（ステップｂ１３）、数値制御装置９に完了信号を送信して（ステップｂ１４）、数値制御装置９からの原位置復帰指令を待つ待機状態に入る（ステップｂ１５）。
【００６１】
そして、ステップａ２９の処理で完了信号を検出したＣＰＵ２０は、ロボット制御装置１０に現位置復帰指令を出力した後（ステップａ３０）、定位置検出センサ２５からの定位置検出信号を待つ初期の待機状態に復帰し（ステップａ１）、また、原位置復帰指令を検出したＣＰＵ１３は、ハンド４を初期設定位置のアプローチポイントに戻した後（ステップｂ１６）、数値制御装置９からのインクリメンタル移動指令Ｄｘ，Ｄｙ，Ｄｚの入力を待つ初期の待機状態に復帰する（ステップｂ１）。
【００６２】
以上、一実施形態として、コンベア３によって搬送されるワーク２に対してハンド４を同期移動させながらワーク２を把持する場合について説明したが、定位置検出センサ２５がワーク２を検出した時点でコンベア３を停止させてワーク２を把持するような構成、つまり、ワーク２の載置位置が略一定となるようなピッキング装置に対しても、各種の設定値を変更することにより、図５〜図７に示した数値制御装置９側のＣＰＵ２０の処理や図８〜図９に示したロボット制御装置１０側のＣＰＵ１３の処理をそのまま適用することが可能である。
【００６３】
具体的には、ステップａ８における変数Ｖとステップａ１２における変数Ｄ０ｘの値を共に零として設定し、ステップａ２１で判定基準となるＮの値を１に設定すればよい。
【００６４】
この場合、ズレ量演算手段は、第一のステレオカメラ６で撮影されたワーク位置の近傍までハンド４を移動させて一度だけ第二のステレオカメラ７を作動させてワーク２の初期位置および初期姿勢に対するワーク２の実位置および実姿勢のズレ量を求めるように機能し（Ｎ＝１）、追跡動作制御手段は、事実上、非作動の状態となる（Ｖ＝Ｄ０ｘ＝０）。
【００６５】
また、最初に述べた実施形態においてワーク２に対してハンド４を同期移動させる際に、第二のステレオカメラ７を様々な方向に移動させて撮影条件を変えることで全体としての位置や姿勢のズレの検出精度を向上させることも可能である。第二のステレオカメラ７の位置や姿勢を変化させる方法としては、ハンド４の送り動作を調整する方法と、ハンド４と第二のステレオカメラ７との間に第二のステレオカメラ７の位置や姿勢を変えるためのアクチュエータを実装する方法とが考えられる。
【００６６】
前者に関しては、基本的に、図８のステップｂ６〜ステップ１０に示されるようなパルス分配処理において追跡動作の途中で強制的にハンド４を移動させる適当な移動指令（オフセット指令）を入力することにより達成可能である。当然、ハンド４の位置の変化によって第二のステレオカメラ７によるワーク２の見え方つまり位置や姿勢に変化が生じるが、この場合、この位置や姿勢のズレ自体がハンド４の位置や姿勢の補正量と同等にして機能することになるので駆動制御上の問題は発生しない。例えば、ハンド４を通常の位置からオフセット指令で５センチだけ＋Ｙの方向に移動させて第二のステレオカメラ７による撮影を行ったとすると、結果として、ワーク２が最初の位置から−Ｙの方向に５センチ位置ズレしたものと判定されることになるので、Δｙの値は−５センチとなり、結果的に、ハンド４は通常の位置からオフセット指令で強制的に５センチだけ＋Ｙの方向に移動された位置から−Ｙの方向に５センチだけ位置を補正され、最終的に、ピッキングに最適の位置に戻されることになる。
【００６７】
また、アクチュエータを使用してハンド４に対する第二のステレオカメラ７の位置や姿勢を変える場合には、ステップａ７の処理で求められたワーク２の位置（Ｘ，Ｙ，Ｚ）にハンド４を移動させてハンド４に対する第二のステレオカメラ７のオフセット量を差し引くことで第二のステレオカメラ７の座標原点を実質的に第一のステレオカメラ６の座標位置（Ｘ，Ｙ，Ｚ）に一致させる場合と同様、アクチュエータの作動状況に応じてハンド４に対する第二のステレオカメラ７のオフセット量を差し引くことで対処することが可能である。
【００６８】
【発明の効果】
本発明のピッキング装置は、最初にワークの位置および姿勢を検出するために使用される第一のステレオカメラを産業用ロボットとは独立して固定的に設置することで第一のステレオカメラを構成するカメラ間の視差を大きく取るようにしたので、第一のステレオカメラを用いた初期段階の検出操作におけるワークの位置および姿勢の認識精度が向上し、第二のステレオカメラは撮影対象となる領域を狭く絞り込んで撮影するだけで確実にワークを撮影することが可能となり、解析の対象となる画像データの容量が軽減されるので、ワークの実位置および実姿勢のズレ量の演算に必要とされる所要時間が短縮されて高速の演算処理が実現される。
第二のステレオカメラは産業用ロボット上でハンドの近傍に取り付ける必要上、カメラ間の視差を短くしたコンパクトな構成とされるが、ワークに接近した状態でワークを撮影することができるので、十分な精度でワークの位置および姿勢のズレを検出することができる。
【００６９】
また、ワークの移動位置に追従させてハンドを移動させながら第二のステレオカメラを繰り返し作動させてワークの位置および姿勢のズレ量を求めるようにしたので、荷崩れ等によってコンベア上のワークの位置や姿勢に変化が生じた場合であっても、位置や姿勢のズレ量に応じてピッキング動作の目標位置やハンドの姿勢を適切に補正して確実なピッキング動作を行うことができる。
【図面の簡単な説明】
【図１】本発明を適用した一実施形態のピッキング装置の構成を簡略化して示した模式図である。
【図２】第二のステレオカメラの外観を簡略化して示した斜視図である。
【図３】コンピュータシステムの一部を構成するロボット制御装置の要部を示した機能ブロック図である。
【図４】コンピュータシステムの一部を構成する数値制御装置の要部を示した機能ブロック図である。
【図５】数値制御装置側のＣＰＵが実行するピッキング制御の概略を示したフローチャートである。
【図６】ピッキング制御の概略を示したフローチャートの続きである。
【図７】ピッキング制御の概略を示したフローチャートの続きである。
【図８】ロボット制御装置のＣＰＵが実行するパルス分配処理の概略を示したフローチャートである。
【図９】パルス分配処理の概略を示したフローチャートの続きである。
【図１０】フレームメモリ内のデータ配列の一例を示した概念図である。
【図１１】不揮発性メモリに記憶されている３次元形状データの一例を示した概念図である。
【図１２】第一のステレオカメラで撮影された画像の一例を示した概念図である。
【図１３】ワークの画像データを解析して得られる３次元情報の一例を示した概念図である。
【図１４】ワークの３次元情報と不揮発性メモリに登録されている３次元形状データとの照合例を示した概念図である。
【符号の説明】
１　ピッキング装置
２　ワーク
３　コンベア
４　ハンド
５　産業用ロボット
６　第一のステレオカメラ
６ａ，６ｂ，６ｃ　カメラ
７　第二のステレオカメラ
７ａ，７ｂ，７ｃ　カメラ
８　コンピュータシステム
９　数値制御装置（コンピュータシステムの一部）
１０　ロボット制御装置（コンピュータシステムの一部）
１１　ＲＯＭ
１２　不揮発性メモリ
１３　ＣＰＵ（追跡動作制御手段の一部，ピッキング動作制御手段）
１４　ＲＡＭ
１５　軸制御回路
１６　表示装置付手動データ入力装置
１７　入出力インターフェイス
１８　ＲＯＭ
１９　不揮発性メモリ
２０　ＣＰＵ（位置姿勢演算手段，ズレ量演算手段，位置姿勢補正手段，追跡動作制御手段の一部）
２１　ＲＡＭ
２２　表示装置付手動データ入力装置
２３　入出力インターフェイス
２４　フレームメモリ
２５　定位置検出センサ
【図１】
2004001122.tif 000003
【図２】
2004001122.tif 000004
【図３】
2004001122.tif 000005
【図４】
2004001122.tif 000006
【図５】
2004001122.tif 000007
【図６】
2004001122.tif 000008
【図７】
2004001122.tif 000009
【図８】
2004001122.tif 000010
【図９】
2004001122.tif 000011
【図１０】
2004001122.tif 000012
【図１１】
2004001122.tif 000013
【図１２】
2004001122.tif 000014
【図１３】
2004001122.tif 000015
【図１４】
2004001122.tif 000016