(57)【特許請求の範囲】
【請求項１】
把持対象物を把持するロボットハンドを備えるロボット装置を制御するロボット制御装置であって、
前記把持対象物の視覚情報を取得する第１の取得手段と、
前記ロボットハンドにより前記把持対象物に作用する力覚情報を取得する第２の取得手段と、
前記第１の取得手段により取得された前記視覚情報から前記把持対象物の位置および姿勢を算出する算出手段と、
前記第２の取得手段により取得された前記力覚情報に基づいて、前記把持対象物の把持状態変動性を導出する導出手段と、
前記導出手段により導出された前記把持対象物の前記把持状態変動性に基づいて、前記第１の取得手段および前記算出手段の少なくとも１つの処理実行を制御する制御手段と、
を具備し、
前記制御手段は、前記導出手段により導出された前記把持状態変動性に基づいて、前記第１の取得手段および前記算出手段の少なくとも１つが実行する画像処理の画像サイズ、画像解像度、およびフレームレートの少なくとも１つを決定する
ことを特徴とするロボット制御装置。
【請求項２】
前記第２の取得手段は、前記把持対象物と前記ロボットハンドとの接触点または接触面における力またはトルクの少なくとも１つを、前記力覚情報として取得することを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記導出手段は、前記把持対象物の前記力覚情報の単位時間当たりの変化量に基づいて、前記把持対象物の前記把持状態変動性を導出することを特徴とする請求項１または２に記載のロボット制御装置。
【請求項４】
前記制御手段は、前記導出手段により導出された前記把持状態変動性に基づいて、前記第１の取得手段および前記算出手段の処理実行を抑制することを特徴とする請求項１から３のいずれか１項に記載のロボット制御装置。
【請求項５】
前記導出手段は、前記把持対象物を把持運搬する前記ロボットハンドの運動のモーメントと、前記把持対象物に作用する重力とに基づいて、前記第２の取得手段により取得された前記力覚情報を補正し、補正された力覚情報に基づいて、前記把持対象物の前記把持状態変動性を導出することを特徴とする請求項１から４のいずれか１項に記載のロボット制御装置。
【請求項６】
前記把持状態変動性は、前記把持対象物の前記力覚情報の単位時間当たりの変化量を示す指標であることを特徴とする請求項１から５のいずれか１項に記載のロボット制御装置。
【請求項７】
請求項１から６のいずれか１項に記載のロボット制御装置と、
前記ロボット装置のロボットアームおよび前記ロボットハンドの軌道情報、および前記ロボット装置の各関節の駆動情報を生成する計画手段と、
前記ロボットアームおよび前記ロボットハンドの駆動装置を制御する第２の制御手段と、
を具備することを特徴とするロボット駆動制御装置。
【請求項８】
請求項７記載のロボット駆動制御装置と、
前記ロボット駆動制御装置により駆動制御される前記ロボット装置と、
前記把持対象物を撮像して、前記第１の取得手段へ前記視覚情報を供給する視覚センサと、
前記力覚情報を検知して、前記第２の取得手段へ前記力覚情報を供給する力覚センサと、
を備えることを特徴とするロボットシステム。
【請求項９】
把持対象物を把持するロボットハンドを備えるロボット装置を制御するロボット制御方法であって、
前記把持対象物の視覚情報を取得するステップと、
前記ロボットハンドにより前記把持対象物に作用する力覚情報を取得するステップと、
前記取得された前記視覚情報から前記把持対象物の位置および姿勢を算出するステップと、
前記取得された前記力覚情報に基づいて、前記把持対象物の把持状態変動性を導出するステップと、
前記導出された前記把持対象物の前記把持状態変動性に基づいて、前記視覚情報を取得するステップおよび前記把持対象物の位置および姿勢を算出するステップの少なくとも１つのステップの処理実行を制御するステップと、
を含み、
前記制御するステップにおいて、導出された前記把持状態変動性に基づいて、前記視覚情報を取得するステップおよび前記算出するステップの少なくとも１つにおいて実行される画像処理の画像サイズ、画像解像度、およびフレームレートの少なくとも１つを決定する
ことを特徴とするロボット制御方法。
【請求項１０】
コンピュータを、請求項１から６のいずれか１項に記載のロボット制御装置の各手段として機能させるためのコンピュータプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、把持対象物を把持および運搬し、組み付け等のタスクの実行を行うロボットを制御するロボット制御装置、ロボット制御方法、ロボット制御システムおよびコンピュータプログラムに関する。
【背景技術】
【０００２】
従来、ロボットが搬送すべき把持対象物（ワーク）の把持を行う際の把持状態を、視覚センサを用いて検出することにより、ロボット制御の方法を変更する手法がある。
例えば、特許文献１は、ロボットのハンドが把持するワークを固定カメラで撮像し、撮像された画像からワークの特徴部の位置および姿勢を求めるワーク搬送装置を開示する。具体的には、このワーク搬送装置は、搬送中のワークの特徴箇所の位置と、撮像時のロボットの位置とから、ハンドがワークを把持する状態（把持状態）を認識し、予め記憶された把持状態と比較して所定誤差を超えた場合にワーク搬送先位置を補正する。
特許文献２は、ロボットアームの先端に設けられた多指ハンド部の指先力センサにより、多指ハンド部の対象物に対する実際の接触位置を検出し、視覚センサにより検出された対象物の位置情報・姿勢情報を修正するロボット装置を開示する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４１７４３４２号公報
【特許文献２】特許第５５０５１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、ロボットが把持対象物を把持する際に把持ずれが発生した場合、カメラ等の視覚センサで把持対象物の位置・姿勢の変化として検出することができる。
しかしながら、視覚センサの撮像だけで、把持対象物の把持ずれを判断したのでは、リアルタイムに把持ずれを検出することが困難であり、把持ずれが進行している際に迅速かつ正確に把持ずれ後の把持対象物の位置・姿勢を求めることが困難であった。
また、常時、把持対象物やロボットの画像撮像、認識、及び位置・姿勢算出等の高負荷の画像処理を動作させなければならず、処理負荷が増大していた。
そこで、本発明の目的は、ロボットが把持対象物を把持する際に発生する把持ずれを、迅速かつ正確に検知すると共に、その処理負荷を低減することができるロボット制御装置、ロボット制御方法、ロボット制御システムおよびコンピュータプログラムを提供することにある。
【課題を解決するための手段】
【０００５】
上記課題を解決するために、本願発明に係るロボット制御装置の一態様は、把持対象物を把持するロボットハンドを備えるロボット装置を制御するロボット制御装置であって、前記把持対象物の視覚情報を取得する第１の取得手段と、前記ロボットハンドにより前記把持対象物に作用する力覚情報を取得する第２の取得手段と、前記第１の取得手段により取得された前記視覚情報から前記把持対象物の位置および姿勢を算出する算出手段と、前記第２の取得手段により取得された前記力覚情報に基づいて、前記把持対象物の把持状態変動性を導出する導出手段と、前記導出手段により導出された前記把持対象物の前記把持状態変動性に基づいて、前記第１の取得手段および前記算出手段の少なくとも１つの処理実行を制御する制御手段と、を具備し、前記制御手段は、前記導出手段により導出された前記把持状態変動性に基づいて、前記第１の取得手段および前記算出手段の少なくとも１つが実行する画像処理の画像サイズ、画像解像度、およびフレームレートの少なくとも１つを決定する。
【発明の効果】
【０００６】
本発明によれば、ロボットが把持対象物を把持する際に発生する把持ずれを、迅速かつ正確に検知すると共に、その処理負荷を低減することができる。
【図面の簡単な説明】
【０００７】
【図１】実施形態１のロボット制御システムのブロック構成を示す図である。
【図２】把持ずれ現象の状態を説明するための図である。
【図３】実施形態１の処理の流れを示すフローチャートである。
【図４】実施形態１の処理の流れを示すフローチャートである。
【図５】実施形態１の座標系を説明するための図である。
【図６】実施形態１における把持対象物にかかるモーメントを説明する図である。
【図７】実施形態１の把持変動性値の説明をするための図である。
【図８】実施形態１の把持変動性値の説明をするための図である。
【図９】実施形態２の把持変動性値の説明をするための図である。
【図１０】実施形態２の処理の流れを示すフローチャートである。
【図１１】実施形態２の処理の流れを示すフローチャートである。
【図１２】実施形態３のロボット制御システムのブロック構成を示す図である。
【図１３】実施形態で説明した各部のハードウエア構成の一例を示す図である。
【発明を実施するための形態】
【０００８】
以下、添付図面を参照して、本発明を実施するための形態について詳細に説明する。
なお、以下に説明する実施の形態は、本発明の実現手段としての一例であり、本発明が適用される装置の構成や各種条件によって適宜修正又は変更されるべきものであり、本発明は以下の実施の形態に限定されるものではない。
【０００９】
実施形態１
（概要）
本実施形態では、力覚センサと視覚センサとを用いて、把持対象物の把持ずれ状態を判断し、判断された把持ずれ状態に基づき視覚制御を行う。これにより、本実施形態は、把持ずれ状態を正確に測定し、把持ずれが進行していても正確に組み付けタスクを実行できるように構成される。
以下の説明においては、ロボット動作の対象とするタスクとして、部品（把持対象物）を所定の位置に組み付けるジョブ（ロボットシステムによる把持対象物への操作）を想定する。
把持対象物の組み付けジョブの対象動作とは、把持対象物の認識、位置および姿勢取得動作、把持対象物の把持動作、把持対象物の運搬動作、把持対象物の組み付け動作、の一連の動作をいう。組み付けジョブは、これら一連の動作を、位置および姿勢精度を保証するようにロボットアームやロボットハンドのロボット行動計画を生成し、実行することである。
【００１０】
（ロボット制御システムの構成）
本実施形態のロボット制御システムの構成について述べる。
図１は、本実施形態のロボット制御システム８（ロボットシステム）のブロック構成を示す図である。図１に示すように、ロボット制御システム８は、ロボット制御装置１０（ロボット駆動制御装置）と、ロボット装置１１と、把持対象物情報取得装置１２と、力覚測定装置１３と、から構成される。また、ロボット装置１１は、把持対象物１４を把持して、組み付け先部位１７に取り付けるジョブを実行するものとする。
ロボット装置１１は、ロボットアーム１５を備えている。更にロボット装置１１は、ロボットアーム１５の先端にロボットハンド１６を備えている。
ロボットアーム１５は、空間内において有形の運動および方向の変更ができ、この運動および変更によって、ロボットアーム１５の先端に備えられているロボットハンド１６の位置および姿勢を任意に変更することができる。
【００１１】
把持対象物情報取得装置１２は、視覚センサであり、例えば、把持対象物１４を撮像するカメラなどを利用することができる。また、図１では、把持対象物情報取得装置１２は、ロボットアーム１５もしくはロボットハンド１６に取り付けられているがこれに限定されず、把持対象物１４を撮像できれば、どこに取り付けられていてもよい。この視覚センサは、把持対象物１４の２次元画像および３次元情報（距離画像や点群データ）の少なくともいずれか一方の視覚情報を取得することができる。
力覚測定装置１３は、力覚センサであり、ロボットハンド１６の把持対象物１４と接触する部位（ロボットハンド１６のフィンガー先端やハンド手掌など）に備えられている。この力覚センサによって、ロボットハンド１６と把持対象物１４との間に発生する力の大きさ（量）を検出することができる。力覚センサは、力の大きさだけでなくトルクの大きさを検出してもよい。
本実施形態のロボット装置１１は、把持対象物１４を、組み付け先部位１７に設置するよう動作する。本実施形態において、ロボット装置１１のジョブの一例は、以下の通りである。まず、ロボットハンド１６で把持対象物１４を把持する動作を実行する。次に、ロボットアーム１５を動作させて把持対象物１４を運搬する。さらに、把持対象物１４を組み付け先部位１７に組み付ける動作を実行する。
【００１２】
（ロボット制御装置）
ロボット制御装置１０は、プログラムを実行可能なコンピュータで構成され、ＣＰＵ、メモリなどの基本的なＰＣの構成を有する。図１に示すロボット制御装置１０の各部は、ＣＰＵが所定のプログラムを実行して、メモリに格納されている各種データを処理することによって実現される。
また、ロボット制御装置１０は、ロボット装置１１、把持対象物情報取得装置１２および力覚情報取得装置１６を接続するためのインターフェースを備えている。ロボット制御装置１０は、これらのインターフェースを介して、ロボット装置１１、把持対象物情報取得装置１２および力覚情報取得装置１６を制御することができる。
把持対象物情報取得装置１２は、把持対象物１４の３次元情報もしくは２次元画像を取得する視覚センサである。
【００１３】
ここでは、例えば、所定のプロジェクタが把持対象物１４に対して所定の像をパターン投影する。把持対象物情報取得装置１２である視覚センサは、把持対象物１４が反射した光パターンを撮像して、把持対象物１４の２次元画像を取得する。さらに、把持対象物情報取得装置１２は、取得した２次元画像に基づき、視覚センサとプロジェクタとの配置関係から、三角測量法により取得した２次元画像の各画素の３次元形状情報を取得する方法を用いる。但し、把持対象物情報取得装置１２は把持対象物１４の３次元情報および２次元画像（以下、「２次元情報」として参照する。）を取得できればよく、これらに限定されない。
【００１４】
次に、ロボット制御装置１０（ロボット駆動制御装置）の各部の構成について説明する。図１に示すように、ロボット制御装置１０は、把持ずれ算出部９と、行動計画部１１２と、ロボット制御部１１３とを備える。この把持ずれ算出部９は、力覚情報取得部１０７と、把持変動性値評価部１０８と、視覚制御部１０９と、把持対象物情報取得部１１０と、位置・姿勢算出部１１１とを備える。
力覚情報取得部１０７（第２の取得手段）は、ロボット制御装置１０に接続された力覚測定装置１３からの力覚情報を取得する。この力覚情報とは、把持対象物１４とロボットハンド１６との接触点または接触面における力情報又はトルク情報の少なくともいずれか一方を含む情報である。この力覚情報は、ロボットハンド１６により把持対象物１４に作用する力覚情報であり、接触面の情報であっても、接触点の情報であってもよい(接触面及び接触点を、以下、単に「接触点」とする)。
【００１５】
把持変動性値評価部１０８（導出手段）は、把持対象物１４に作用する力覚情報から、把持状態の変動性を判断し、判断した把持の変動性を、把持変動性値（把持対象物１４の把持状態変動性）として算出する。
この把持変動性値は、ロボットハンド１６に備えられた力覚測定装置１３である力覚センサの出力する力覚情報の時間的な変動状態を示す数値である。力覚センサが出力する力覚情報の値の変動（変動量）が大きいほど、把持変動性値が大きく変化をする。また、力覚センサの力覚情報の値の変動（変動量）が小さいほど、把持変動性値は小さく変化をする。把持変動性値評価部１０８が把持変動性値を算出することによって、ロボットハンド１６が把持対象物１４を把持している際の、把持の状態を評価することができる。
【００１６】
視覚制御部１０９（制御手段）は、把持変動性評価部１０８が出力する把持変動性値（把持状態の変動性）に基づき、把持対象物情報取得部１１０および位置・姿勢算出部１１１のうち、少なくとも一方または双方の処理の実行を制御する。
把持対象物情報取得部１１０（第1の取得手段）は、ロボット制御装置１０に接続された把持対象物情報取得装置１２から、把持対象物１４の３次元情報および２次元情報（視覚情報）を取得する。
位置・姿勢算出部１１１（算出手段）は、把持対象物情報取得部１１０が出力する把持対象物１４の視覚情報（３次元情報および２次元情報）から、把持対象物１４の位置および姿勢を算出する。
【００１７】
次に、行動計画部１１２およびロボット制御部１１３について説明する。
行動計画部１１２（計画手段）は、把持対象物１４の運搬先の目的場所とする位置および姿勢の状態まで把持対象物１４を運搬できるように、ロボットアーム１５およびロボットハンド１６の各関節角度の時系列変化であるロボット行動計画情報を算出する。すなわち、行動計画部１１２は、把持ずれ算出部９が出力するロボット装置１１及び把持対象物１４の情報に基づき、所望の動作を実現するためのロボットアーム１５およびロボットハンド１６の軌道情報、ロボット装置１１の各関節の駆動情報等を生成する。
ロボット制御部１１３（第２の制御手段）は、行動計画部１１２で算出されたロボット行動計画情報に従って、ロボット装置１１の動作（運動）を制御する。すなわち、ロボット制御部１１３（第２の制御手段）は、ロボット装置１１のロボットアーム１５及びロボットハンド１６の駆動装置を制御する。
このようにして、ロボット制御装置１０は、ロボット装置１１を制御して、把持対象物１４を目的の運搬先に運ぶことができる。
【００１８】
（把持ずれ現象）
次に、把持ずれ現象について説明する。図２は、本発明の実施形態１の把持ずれ現象の状態を説明する図である。
把持ずれ現象とは、把持対象物１４を目的地に運搬する際に、ロボットハンド１６の把持圧力と把持対象物１４にかかるモーメントの状態によって発生する意図しない把持対象物１４の姿勢の変化のことをいう。
把持対象物１４の運搬動作等を実行する際に、把持圧力と把持対象物１４にかかるモーメントの状態によっては、図２（ａ）における２０で示される方向に把持対象物１４が回動し、本来の位置から回転してずれてしまうことが考えられる。このずれの結果、把持対象物１４の位置が、図２に示す把持位置１４ａから把持位置１４ｂに変化してしまう。
【００１９】
さらに、把持ずれが進行し、最終的に図２（ｂ）に示すように、把持対象物１４が、把持位置１４ｃの位置まで回転しずれた状態で停止する。このような把持ずれ現象の発生と、把持ずれ後の把持対象物１４の位置および姿勢の正確な予測は困難である。
このような把持ずれ現象が進行すると、把持対象物１４が把持位置１４ａの状態にあることを前提としたロボット行動計画では、把持対象物１４の運搬動作や把持対象物１４の組み付け動作の精度の保証が困難になる。このため、ロボット装置１１が実行するジョブの成功の保証が困難となる。
【００２０】
（本実施形態における処理の概要）
次に、ロボット制御装置１０の処理の流れの説明を行う。図３、図４は、本実施形態に係るロボット制御装置１０の処理の流れを示すフローチャートである。図３、図４に示される処理動作は、ロボット制御装置１０の備えるＣＰＵがプログラムを実行することによって達成される。
最初に、ロボット制御装置１０の処理の基本的な流れを述べる。
まず、把持対象物情報取得部１１０が、把持対象物１４の把持対象物情報（視覚情報）の取得を行い、位置・姿勢算出部１１１が、把持対象物１４の位置および姿勢の値を算出する。次に、行動計画部１１２が、その把持対象物１４の位置および姿勢に合わせて、把持対象物１４の組み付けジョブのためのロボット行動計画情報を算出する。
ロボット制御部１１３は、算出されたロボット行動計画情報に従って、ロボット装置１１の制御を実行する。
【００２１】
本実施形態においては、ロボット制御部１１３が上記のロボット制御を実行している間に、力覚情報取得部１０７が、力覚測定装置１３の出力する力覚情報に基づき、把持対象物１４の把持の状態を取得する。そして、把持変動性値評価部１０８が、取得された把持の状態に基づき、把持変動性値の算出を行う。このように算出された把持変動性値を評価することにより、視覚制御部１０９が、把持対象物情報取得部１１０（第1の取得手段）と、位置・姿勢算出部１１１（算出手段）と、の実行の制御を行う。
このような処理によって、把持対象物１４の把持ずれに対する実行制御を行うことができる。
把持ずれに対する上記の実行制御を実行した後で、さらに把持ずれが生じた場合には、その把持ずれの状態に合わせて、再度、ロボットの行動計画を立て直す。把持ずれが許容範囲以下の状態であり、ロボット装置１１のジョブの実行（動作継続）に問題が無ければ、ロボット制御部１１３によるロボット制御が引き続き実行される。最終的に、ロボットの行動計画情報の通りに、ロボット制御が実行され完了すれば、把持対象物１４の組み付けジョブは終了する。
【００２２】
（ロボット制御装置１０の処理フロー詳細）
次に、前述したロボット制御装置１０が実行する処理の詳細なフローを説明する。図３の処理が開始されると、Ｓ２において、把持ずれ算出部９（視覚制御部１０９）は、最初に、把持ずれフラグをＯＦＦにする。
この把持ずれフラグとは、視覚制御部１０９によって管理され参照・更新可能な、把持の変動性評価の際に用いられるフラグである。
この把持ずれフラグは、把持対象物１４の把持が安定している時に、把持を行ってから以降継続している安定状態なのか、それとも一度把持ずれが発生した後の安定状態なのかを示すフラグであって、把持ずれが発生した履歴を示すフラグである。
具体的には、把持ずれフラグがＯＮの場合は、その把持対象物１４に関して把持ずれが発生したことがあることを表す。一方、把持ずれフラグがＯＦＦの場合は、その把持対象物１４において把持ずれが発生したことがないことを表す。
【００２３】
次に、Ｓ３において、視覚制御部１０９は、把持対象物情報取得部１１０もしくは位置・姿勢算出部１１１の実行を制御するための実行制御フラグをＯＦＦにする。この実行制御フラグとは、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との実行を制御するフラグである。
具体的には、実行制御フラグがＯＮの場合は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との実行を抑制する。一方、実行制御フラグがＯＦＦの場合は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との処理の実行は抑制されず、通常実行される。視覚制御部１０９は、この実行制御フラグを保持し管理を行う。
【００２４】
次に、Ｓ４において、把持対象物情報取得部１１０は、把持対象物１４の３次元情報と２次元情報とのいずれか一方または双方の情報の取得を行う。
次に、Ｓ５において、位置・姿勢算出部１１１は、Ｓ４で取得された把持対象物１４の把持対象物情報から、把持対象物１４の種類の認識と、把持対象物１４の位置および姿勢の算出とを行う。ここで、把持対象物情報とは、把持対象物１４の３次元情報と２次元情報とのいずれか一方または双方の情報（視覚情報）をいう。
具体的には、位置・姿勢算出部１１１は、把持対象物１４の位置および姿勢の算出を、把持対象物情報が３次元点群の場合はＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）アルゴリズムを用いて実行する。また、把持対象物情報が２次元情報であれば、２次元モデル画像とのマッチングを利用して実行することができる。
【００２５】
次に、Ｓ６において、行動計画部１１２は、Ｓ５で算出された把持対象物１４の位置および姿勢から、把持対象物１４の組み付けジョブ中の一連の動作に対応するロボット行動計画情報を生成する。組み付けジョブ中の一連の動作には、例えば、把持対象物１４に対しての認識、位置および姿勢の取得動作（撮像）、把持動作、運搬動作、組付け動作等が含まれる。具体的には、行動計画部１１２は、ロボット行動計画情報の作成の際、ＲＲＴ（Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅ）アルゴリズムを用いた経路計画アルゴリズムを利用できる。
ロボット行動計画情報の一種である把持行動プランや運搬行動プランなどは、ロボット装置１１のロボットアーム１５やロボットハンド１６の各関節の値の時系列変化情報であり、ロボットの動作軌道を示す情報である。
把持対象物１４の把持動作についてのロボット行動計画情報の生成には、ＦＯＲＣＥ−ＣＬＯＳＵＲＥ  ＦＵＮＣＴＩＯＮによる把持姿勢評価を基にした経路計画アルゴリズムを利用できる。これらの手法を用いることで、行動計画部１１２は、既知の障害物やオンラインで検出された障害物などを回避したロボット装置１１のロボット行動計画を生成できる。なお、同様の結果が得られるのであれば、その他の装置やアルゴリズムを用いても構わない。
【００２６】
次に、行動計画部１１２が作成したロボット行動計画情報に従って、ロボット制御装置１０は、把持対象物１４に対するロボット装置１１の把持動作を実行させる。これらの把持動作は、Ｓ７〜Ｓ１９において以下のとおり実行される。
Ｓ７において、ロボット制御部１１３は、ロボット行動計画情報に従ってロボット１１の制御（ロボットの動作制御）を行う。ここで、ロボットの動作制御とは、Ｓ６において行動計画部１１２が生成したロボット行動計画情報である時系列の各関節角度情報から、ロボット装置１１への動作指令コマンドを生成して、実際にロボット装置１１を動作させることをいう。具体的には、ロボット制御部１１３は、時系列の各関節角度情報を、ロボット装置１１に含まれる各ロボットコントローラの仕様に合わせて動作指令コマンドに変換し、ロボット装置１１に送信する。この動作指令コマンドにより、ロボット装置１１に含まれるロボットアーム１５やロボットハンド１６の各関節のモータの値が変更されて、ロボット装置１１が動作する。
このようなロボット装置１１の動作制御は、一度にすべての動作軌道を動作させるのではなく、ロボット制御部１１３が、ごく短い時間間隔でロボット装置１１への軌道制御を実行することで実現できる。これによって、ロボット装置１１は、所望のジョブを実行することができるとともに、急なジョブの変更等にも対応することができる。
【００２７】
(把持ずれ制御処理詳細)
このロボット装置１１の動作中に、上述したような把持ずれ等の意図しない現象が発生する。例えば、把持ずれ現象が進行している過程で、把持対象物情報取得部１１０や位置・姿勢算出部１１１が通常の動作を実行すると、把持対象物１４の把持対象物情報である３次元情報や２次元情報に撮影ぶれが生じてしまう。本実施形態では、このような問題に対処できるよう、以下の把持ずれ検出処理を行っている。把持ずれ検出処理（制御処理）は、図３に示すＳ８〜図４に示すＳ１８までの処理である。
図３のＳ８において、力覚情報取得部１０７は、まず、力覚測定装置１３から力覚情報の取得を行う。この力覚情報は、把持対象物１４とロボットハンド１６との間にかかる力やトルクの値である。
Ｓ９において、把持変動性値評価部１０８は、把持対象物１４の把持の状態を求めて、把持変動性値の算出を行う。
ここで、力覚取得部１０７で取得された力覚情報は、ロボット装置１１の運動のモーメント等の影響を含んでいるため、ロボット装置１１の運動のモーメントや重力の影響を排除した把持対象物１４自体のモーメントによる力覚値に変換する必要がある。把持変動性値評価部１０８は、この補正された力覚情報を算出する。この補正された力覚情報の算出方法については、図６を用いて後に詳細を説明する。
【００２８】
Ｓ９において、把持変動性値評価部１０８は、この補正された力覚情報から、把持状態の変動性を示す値である把持変動性値を算出する。
ここで算出される把持変動性値（把持状態変動性）とは、ロボットハンド１６と把持対象物１４の接触部（接触点または接触面）に作用する力の大きさの単位時間あたりの変化量（変動量）を表す指標である。換言すれば、把持変動性値評価部１０８は、力覚情報の単位時間当たりの変化量に基づいて、その変化量を示す指標として把持変動特性値（把持状態変動性）を算出する（導出する）。把持状態が安定している場合には、ロボットハンド１６と把持対象物１４との接触面に対して法線方向に安定的に一定の力がかかる。このため、この力の大きさや方向が安定しない場合に、把持対象物１４に対する把持ずれが進行していると判断することができる。このような力覚情報に基づく把持ずれ（ないし把持安定）検出（把持変動性値算出）の詳細に関しては、図７から図９を用いて後に詳細を説明する。
【００２９】
次に、図４のＳ１０において、把持変動性値評価部１０８は、算出された把持変動性値の絶対値がある一定の閾値より大きい値かどうかを判定する。把持変動性値が、閾値より大きい場合は、把持状態は安定していないと判断して、Ｓ１１に進む。一方、把持変動性値が閾値と同一か小さい場合は、把持状態は安定していると判断して、次のＳ１２に進む。
更に、Ｓ１２において、把持変動性値評価部１０８は、把持ずれフラグがＯＮであるかＯＦＦであるかを判定し、把持ずれフラグがＯＮである場合はＳ１３に進み、把持ずれフラグがＯＦＦである場合はＳ１５に進む。
【００３０】
この一連のＳ１０からＳ１２を経て判断された把持ずれ現象は、以下の３種類の状態に分類される。
第１の状態は、Ｓ１０（Ｙｅｓ）から分岐しＳ１１からＳ１４に至る処理の状態である。この場合は、把持対象物１４に対する把持状態が安定せず、把持ずれ現象が進行している過程であると判断する。Ｓ１１において、把持変動性値評価部１０８は、前述の把持ずれフラグをＯＮにしてから、Ｓ１４に進む。Ｓ１４において、視覚制御部１０９は、把持対象物情報取得部１１０および位置・姿勢算出部１１１のうち、いずれか一方または双方の処理の実行を抑制するために、視覚制御部１０９が保持する実行制御フラグをＯＮにする。すなわち、把持変動性値評価部１０８は、把持変動性値の算出を行った結果、所定の閾値より大きく、把持ずれが生じていると判断した場合、視覚制御部１０９（制御手段）は、次のような処理を実行する。その処理は、把持対象物情報取得部１１０（第１の取得手段）および位置・姿勢算出部１１１（算出手段）のうち、いずれか一方または双方の処理の実行を抑制する処理である。
【００３１】
第２の状態は、Ｓ１２（Ｙｅｓ）から分岐しＳ１３に至る処理の状態である。この場合は、把持対象物１４に対する把持状態が一度不安定になり把持ずれ現象が発生した後に、把持状態が安定した状態に移行したと判断する。Ｓ１３において、視覚制御部１０９は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との処理を実行させて把持ずれ後の把持対象物１４の位置および姿勢を算出させるために、視覚制御部１０９が保持する実行制御フラグをＯＦＦにする。
また、視覚制御部１０９は、把持対象物取得部１０１と位置・姿勢算出部１１１とにおける画像処理等が実行されている最中である場合は、これらの処理を中断し、画像処理を再スタートするように制御を行う。このような制御を行う理由は、後述する図８（ｂ）の把持変動性判定タイミング７３により迅速に同期させるためである。視覚制御部１０９は、このような制御を実行することによって、特に画像処理がマルチタスクで実行されている場合、より応答性の高い画像処理を行わせることができる。その結果、把持ずれ現象からの復帰に対してより迅速に対応することができる。
【００３２】
第３の状態は、Ｓ１２（Ｎｏ）から分岐しＳ１５に至る処理の状態である。この場合は、過去に把持ずれ現象が発生したことがない、つまり、把持対象物１４の把持が正常に行われ、把持ロボット行動計画や運搬ロボット行動計画が正常に実行できていると判断する。この状態では特に問題は生じないので、Ｓ１５において、視覚制御部１０９は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との処理を実行させるために、視覚制御部１０９が保持する実行制御フラグをＯＦＦにする。
視覚制御部１０９がＳ１３もしくはＳ１５もしくはＳ１４を実行した後、Ｓ１６に進む。
Ｓ１６において、視覚制御部１０９は、視覚制御部１０９が保持する実行制御フラグがＯＦＦであるか否か判定する。視覚制御部１０９は、実行制御フラグがＯＦＦである場合は、把持対象物１４の把持ずれの位置を確認するために、再度、把持対象物情報取得部１１０に把持対象物１４の３次元情報や２次元情報の取得を行わせる。続いて、Ｓ１７において、視覚制御部１０９は、実行制御フラグがＯＦＦである場合は、Ｓ１６で取得された把持対象物１４の３次元情報や２次元情報から、位置・姿勢算出部１１１に把持対象物１４の位置および姿勢の算出を行わせる。
一方、視覚制御部１０９が保持する実行制御フラグがＯＮの場合、Ｓ１６およびＳ１７における把持対象物情報取得部１１０と位置・姿勢算出部１１１との処理は実行されない。
【００３３】
次に、把持ずれが発生したかどうかの判定を行う。Ｓ１８において、視覚制御部１０９または行動計画部１１２は、現在得られている把持対象物１４の位置および姿勢の値と、図３のＳ６においてロボット行動計画情報を算出した時に用いた把持対象物１４の位置および姿勢の値とを比較する。そして、両者の差が所定の閾値より大きいか否かを判定する。ここで、Ｓ１７において把持対象物１４の位置および姿勢の値が算出されている場合は、Ｓ１７において算出された値と、Ｓ６においてロボット行動計画情報を算出した時に用いた把持対象物１４の位置および姿勢の値とを比較して所定の閾値内か判定する。
両者の差が所定の閾値より大きい場合は、行動計画部１１２は、把持ずれ現象による把持対象物１４の把持ずれ量が、ロボット行動計画情報の実行に影響があると判断をして、図３のＳ２に戻る。そして、把持ずれ後の把持対象物１４の位置および姿勢（把持対象物１４の把持ずれ量（すべり量））に基づいて、ロボット装置１１の復帰動作の行動計画のプランニングが実行される。
【００３４】
上記第１の状態の場合、Ｓ１１で把持ずれフラグがＯＮ、Ｓ１４で実行制御フラグがＯＮとされ、Ｓ１６およびＳ１７の処理はバイパスされるので、Ｓ１８の比較結果はＮｏとなり、計画が終了するまでＳ７に戻って処理が繰り返される。このため、把持状態が変動する間、把持対象物１４の位置および姿勢の算出に係る画像処理は抑制される。次以降のループで把持状態が安定したと判断された際に（Ｓ１０Ｙｅｓ）、上記第２の状態に移行し、Ｓ１３で実行処理フラグがＯＦＦとされて、把持ずれ後の把持対象物１４の位置および姿勢が算出される（Ｓ１６、Ｓ１７）。Ｓ１８で把持ずれ量が許容範囲外であれば（Ｓ１８Ｙｅｓ）、Ｓ２に戻ってロボット装置１１の復帰動作の行動計画が算出される。
Ｓ１８Ｙｅｓで処理がＳ２に戻った後にプランニングされるロボット装置１１の復帰動作の行動計画とは、把持対象物１４が、把持ずれ状態から、組み付け可能タスクを実行可能な把持状態に変更する行動計画を示す。例えば、把持ずれした把持対象物１４を台に一度置いて持ち直すなどの手法を用いて把持ずれ状態の解消を行うロボット行動計画情報である。このロボット装置１１の復帰動作の行動計画によって、把持ずれ現象によって生じた把持の位置および姿勢に関する不具合を解消することができる。
【００３５】
また、図４のＳ１８において、把持ずれ量が許容範囲内であり、持ち替えなどの動作をせずに組み付けジョブがそのまま実行可能と判断することができる場合もある（Ｓ１８Ｎｏ）。この場合は、行動計画部１１２は、そのまま組み付けジョブなどの次工程のロボット行動計画を実行するように、復帰動作の行動計画をプランニングしてもよい。
これに替えて、Ｓ１８において、行動計画部１１２は、把持ずれが発生していると判断する場合でも、把持ずれの量に基づき把持ずれ量を補償するようにロボット装置１１の動作を制御するような復帰動作の行動計画をプランニングしてもよい。これらの処理は、行動計画部１１２が、通常の動作プランニング処理と同様に備えており、Ｓ６で実行することができる。
【００３６】
次に、Ｓ１９において、行動計画部１１２は、ロボット行動計画の実行が終了したかどうかの確認を行い、終了していない場合は、図３のＳ７に戻り、ロボット制御部１１３が、ロボット装置１１の動作の制御を実行する。一方、終了している場合は、処理を完了し、ロボット装置１１の組み付けジョブの動作処理を終了する。
Ｓ１９において、行動計画部１１２は、ロボット行動計画の実行の終了判定を実行する。具体的には、行動計画部１１２は、図３のＳ６において作成されたロボット行動計画の時系列中のロボット装置１１の各関節の値と、実際のロボットの各関節の値が同一（許容誤差より小さい誤差）になったか否かを判定する。そして、行動計画部１１２は、これらの値が同一になった場合に、ロボット行動計画の実行の終了と判定する。
以上説明した図３および図４のフローチャートに従い、ロボット行動計画の情報の算出およびロボット制御を行うことによって、把持ずれ現象が発生した状態においても、把持ずれがない状態への復帰動作をより迅速に行うことができる。したがって、組み付け動作などのジョブをより円滑かつ高精度にロボット装置１１に実行させることができる。
【００３７】
（ロボット制御システムの座標系）
次に、ロボット制御装置１０を含むシステム全体の座標系に関して説明する。図５は、本実施形態の座標系を説明する図である。
ロボット装置１１の行動計画におけるロボット装置１１の位置および姿勢の算出や、把持対象物１４の位置および姿勢の制御の演算、組み付け先部位１７の位置および姿勢の算出等は、図５に示される座標系によって演算が行われる。
図５に示すように、ロボット装置１１の作業空間内にある一点を原点座標５０２とする。この原点座標５０２と、ロボット装置１１の座標との変位を、ベクトル５０３（ＲＸ、ＲＹ、ＲＺ）とする。ロボット装置１１の姿勢を示す３×３の回転行列を、ＲＭとする。
【００３８】
また、ロボット装置１１の座標と、ロボットアーム１５先端との変位を、ベクトル５０５（ＦＸ、ＦＹ、ＦＺ）とする。ロボットアーム１５の姿勢を示す３×３回転行列を、ＦＭとする。ロボットアーム１５の先端とロボットハンド１６の先端（把持対象物１４の把持ポイント）との変位は、ベクトル５０６（ＥＸ、ＥＹ、ＥＺ）とする。また、ロボットハンド１６の姿勢を示す３×３の回転行列は、ＥＭとする。ロボットアーム１５の先端と、把持対象物情報取得装置１２の焦点位置との変位は、ベクトル５０７（ＣＸ、ＣＹ、ＣＺ）とする。また、把持対象物情報取得装置１２である視覚センサの姿勢を示す３×３の回転行列を、ＣＭとする。把持対象物情報取得装置１２の焦点位置と把持対象物１４（ワーク）との変位を、ベクトル５０８（ＶＸ、ＶＹ、ＶＺ）とする。また、視覚センサから見た把持対象物１４の位置および姿勢を示す３×３の回転行列を、ＶＭとする。
ここで、原点座標５０２から見た把持対象物１４の座標値を、座標５０４（ＷＸ、ＷＹ、ＷＺ）とする。また、把持対象物１４の姿勢を示す３×３の回転行列をＷＭとすると、このＷＭについては、以下の２式で表すことができる。
【００３９】
【数１】
0006700726.tif 000002
【００４０】
【数２】
0006700726.tif 000003
【００４１】
式１は、ロボット装置１１のロボットアーム１５のある位置および姿勢と、力覚的な予測から推定する把持対象物１４の位置および姿勢を示す式である。力覚的な予測からＥＭ、ＥＸ、ＥＹ、ＥＺを生成するためには、把持対象物１４の重心位置と力覚情報とから算出できる。把持対象物１４の形状情報が既知である場合、ＥＭ、ＥＸ、ＥＹ、ＥＺは、以下のように算出できる。
まず、力覚センサ（力覚測定装置１３）が取得した力覚情報の値から把持対象物１４の重心位置を推定する。この力覚情報から推定される重心位置と、既知の（把持対象物１４の）形状情報から算出される重心位置との差に基づき、ＥＭ、ＥＸ、ＥＹ、ＥＺを算出することができる。その詳細な算出方法として、例えば、「中島明、早川義一、「視覚と力覚情報の融合に基づく多指ハンドによる把握・操り制御」、計測自動制御学会第６回制御部門大会予稿集、名古屋大学、pp.309-312、２００６年」が利用できる。
【００４２】
力覚情報を取得する位置が少なければ少ないほど、力覚情報量が少なくなり正確な把持対象物１４の位置および姿勢を求めることは難しくなる。一方、力覚センサが面の計測可能である場合は、この力覚センサからより多くの力覚情報が得られる。したがって、このような多くの力覚情報を用いれば、把持対象物１４の位置および姿勢をより正確に求められる。
式２は、ロボット装置１１のロボットアーム１５が存在する位置および姿勢と、視覚情報から測定される把持対象物１４の位置および姿勢を示す式である。視覚情報に基づき、把持対象物１４の把持状態を計測することは、把持対象物情報（３次元情報および２次元情報）から算出することができる。この算出には、例えば、「五十嵐克司、細田耕、浅田稔（大阪大学工学部）、「未知環境における視覚と力覚のハイブリッド制御」、ロボティクスシンポジア予稿集、pp87-92、１９９６年」が利用できる。
【００４３】
これら２式（式１、式２）から算出される把持対象物１４の位置および姿勢の情報を重ね合わせること等によって３次元再構成処理を行うことができる。この３次元再構成処理は、例えば、特許文献２で開示されている手法を利用することができる。また同様に、位置合わせを行う際に用いられるＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ)等のフィッティングアルゴリズムも、例えば特許文献２で開示されている手法を用いることができる。このようにして、把持対象物１４の位置および姿勢の算出を行うことができる。
式１で算出された把持対象物１４の位置および姿勢は、精度が低い可能性がある。そのため、本実施形態では、力覚センサの形態等を考慮し、式２で算出された把持対象物１４の位置および姿勢の値と、式１で算出された把持対象物１４の位置および姿勢の値とを、所定の重み付けで重ね合わせている。
【００４４】
次に式１と式２の関係について説明をする。
式１と式２は、それぞれ把持対象物１４の位置および姿勢であるＷＭ、ＷＸ、ＷＹ、ＷＺを求める式であるため、式１と式２のＷＭ、ＷＸ、ＷＹ、ＷＺの値は、理想的には一致する。
しかし、把持ずれ現象等が発生すると、力覚情報取得部１０７が取得する力覚情報や、把持対象物情報取得部１１０が取得する把持対象物情報（把持対象物１４の情報）などに急激な変化が発生する。そのため、式１および式２からそれぞれ求めた把持対象部１４の位置および姿勢の値の相対的な差（誤差値）が大きくなる。この把持対象物１４の位置および姿勢の誤差値を用いることによって、視覚情報による把持対象物情報と力覚情報による把持対象物情報とを統合して、図４のＳ１０の把持状態の変動性値の算出に用いてもよい。
【００４５】
次に、行動計画部１１２のロボット行動計画情報の算出に関して説明する。
まず、図５に示すように、組み付け先部位１７の組み付け先部位位置５０１に組み付けた状態の把持対象物１４の位置を座標５０９（ＴＸ、ＴＹ、ＴＺ）、組み付け先部位位置５０１における把持対象物１４の姿勢を示す３×３回転行列をＴＭとする。ここでロボット行動計画情報の生成（算出）とは、他の障害物との衝突を避けながら、座標５０９と座標５０４が一致し、更に回転行列ＷＭと回転行列ＴＭが一致するように、ロボットの関節値を変化させることである。
本実施形態は、このように装置間の座標を定義し算出しているので、位置・姿勢算出部１１１は、把持対象物１４を認識し、把持対象物１４の位置および姿勢を算出し、行動計画部１１２は、ロボット装置１１の行動計画等を算出することができる。
【００４６】
（把持対象物にかかるモーメントと把持変動性値）
次に、把持対象物１４にかかる力覚の値（力覚値）について説明をする。図６は、本実施形態における把持対象物１４にかかるモーメントを説明する図である。
図３のＳ６において生成したロボット行動計画情報に基づき、ロボット制御部１１３はロボット装置１１を制御している。そのロボット装置１１の制御の実行中に、図３のＳ９において、把持変動性値評価部１０８は、把持対象物１４の把持変動性値の算出を行っている。ここでは、この把持変動性値の算出方法を詳細に述べる。
本実施形態において、把持変動性値とは、ロボットハンド１６と把持対象物１４の接触部（接触面または接触点）にかかる力の大きさの単位時間あたりの変化量（変動量）のことをいい、把持対象物の把持状態の変動値を意味する。把持対象物１４に対する把持状態が安定している場合、ロボットハンド１６と把持対象物１４の接触面に対して法線方向に安定的に一定の力がかかる。ただし、ロボットアーム１５が運搬動作等を行っている場合、把持対象物１４には、次の３種類の力やモーメントがかかる。
【００４７】
第１に、ロボットハンド１６のフィンガーからかかる把持圧力である。第２に、ロボットアーム１５が運搬動作をしている際に発生するロボットハンド１６へのモーメント６２である。第３に、把持対象物１４の重量に由来する重力の力６１である。これら３つの力が、ロボットハンド１６のフィンガーの先端に付けられている力覚測定装置１３と把持対象物１４の接触点または接触面６４にかかる。そのため、力覚情報取得部１０７が取得する力覚情報は、この３つの力の合算値として得られる。
図６では、便宜上、モーメント６２と重力の力６１の合力６３を示すが、さらにこれらに把持圧力も加えられて、３つの力の合算値が得られる。ロボットアーム１５による運搬動作によって発生するモーメント６２は、ロボットアーム１５の形状とロボット装置１１の運搬動作によるロボット行動計画情報（時系列の各関節の値）から、算出することができる。また、把持対象物１４の重量や形状が既知であれば、重力の値６１も算出することができる。
【００４８】
本実施形態においては、把持変動性値評価部１０８は、ロボット装置１１の運搬動作を知るために、ロボット制御部１１３（第２の制御手段）から、ロボット装置１１の位置および姿勢を検出する。さらに、把持変動性値評価部１０８は、ロボット装置１１のロボットアーム１５およびロボットハンド１６の位置および姿勢も検出する。これによって、把持変動性値評価部１０８は、ロボットアーム１５およびロボットハンド１６の運動を知ることができ、モーメント６２を計算することができる。ロボット制御部１１３は、既に説明したように、ロボット装置１１の関節の動きを制御する動作指令コマンドを逐次ロボット装置１１に送信しているので、現在のロボット装置１１の位置および姿勢を把握し、検出することができる。
その結果、以下の式３を用いて、力覚測定装置１３と把持対象物１４の接触面における把持圧力値を算出することができる。
ＳＦ＝ＧＦ＋ＡＦ＋ＦＦ          （式３）
この式３において、ＳＦは、力覚測定装置１３（力覚センサ）が取得する力覚値（力覚情報）、ＧＦは、把持対象物１４にかかる重力値、ＡＦは、ロボットのアームの動作のモーメントによる力である。ＦＦは、ロボットハンド１６のフィンガー先端と把持対象物１４との間に発生する把持圧力を示している。
【００４９】
把持変動性値評価部１０８が用いる力覚情報（力覚値）は、ＦＦで示されるロボットハンド１６のフィンガー先端と把持対象物１４との間に発生する把持圧力を用いることができる。そこで、式３からＦＦを求める。式３を変形して、下記式４を得る。
ＦＦ＝ＳＦ−ＧＦ−ＡＦ          （式４）
この式４によって得られたＦＦの値を、把持対象物１４にかかる力覚情報の値（力覚値）として用いる。このようにＦＦのみを抽出することによって、把持対象物１４を把持したロボットハンド１６が取り付けられたロボットアーム１５が動作していたとしても、ロボットアーム１５の動きによる影響を取り除くことができる。このように、力覚測定装置１３（力覚センサ）が取得する力覚値を、重力（ＧＦ）、モーメント（ＡＦ）を用いて補正して得られたＦＦの値を、力覚情報の値として用いることができる。このような補正は、把持変動性値評価部１０８が実行する。
把持変動性値は、この（補正後の）力覚情報の値の単位時間当たりの変動量として得られる。本実施形態においては、この変動量を把持変動性値ＨＳＳ（式５）として表す。ここで、単位時間ｔは、ロボット装置１１の稼働可能な速度に対して、十分短い時間を用いる。この計算も把持変動性値評価部１０８が実行する。
【００５０】
【数３】
0006700726.tif 000004
【００５１】
この式５で表される把持変動性値ＨＳＳの最大値もしくは平均値が所定の閾値より大きい場合、把持変動性値評価部１０８は、把持対象物１４に対する把持状態が不安定だと判断し、把持ずれが発生・進行していると判断できる。一方、把持変動性値ＨＳＳの最大値もしくは平均値が所定の閾値より小さい場合、把持対象物１４に対する把持状態が安定であると判断し、把持ずれが発生・進行していないと判断できる。
【００５２】
（把持変動性検出の実例：実施形態１の画像処理制御）
次に、実際に発生する把持ずれ現象に対して、力覚情報と、画像処理の連動動作に関して実例を説明する。図７、図８は、本実施形態の把持対象物１４に対する把持の変動性検出と、視覚制御部１０９の動作の関連を説明する図である。
図７（ａ）は、把持対象物１４が把持対象物１４ａの位置から把持対象物１４ｂの位置に向かって、矢印２０の方向にずれるように把持ずれ現象が発生し、把持状態が不安定になった状態を示す図である。図７（ｂ）は、図７（ａ）で示した把持ずれ現象が発生している状況において、力覚情報取得部１０７が取得する力覚情報を表したグラフと、時間経過に伴う画像処理単位の遷移を模式的に表したタイムチャートとを示す。図７（ｂ）に示すように、把持ずれ現象が発生している状況において、力覚情報は、例えばグラフ７１ａのような波形になり、不規則な高周波成分を含む状態となる。このときに、図３および図４に示す処理フローにより、把持対象物情報取得部１１０や位置・姿勢算出部１１１等における画像処理は、視覚制御部１０９による制御によって実行が抑制されないよう制御される。
【００５３】
図７（ｂ）において画像処理単位７２で示される画像処理は、把持対象物情報取得部１１０や位置・姿勢算出部１１１等における画像取得および認識処理、および付随する把持対象物１４の位置および姿勢算出処理等の画像処理の単位を表す。図７（ｂ）の画像処理単位７２と同様に、図８（ｂ）においても画像処理単位７４、７５が示されている。ここで、画像処理単位７２および７４（点線の画像処理単位）は画像処理が行われていないことを示すのに対して、画像処理単位７５（実線の画像処理単位）は画像処理が行われていることを示す。図７（ｂ）に示すように、力覚情報に不規則な高周波成分が含まれている間は、視覚制御部１０９が画像処理等の実行を抑制しているので、撮影ぶれ等の発生する不正確な把持対象物取得情報を元にした位置および姿勢算出処理等の実行は抑制される。
【００５４】
次に、図８（ａ）は、把持対象物１４が把持対象物１４ｃの位置で把持ずれ現象を起こしている不安定な状態から、把持対象物１４ｄの位置へ移動し、把持が安定して把持ずれが終了した状態を示す図である。このように把持ずれ現象を起こしている状態から、把持ずれ状態が終了した状態において、力覚情報取得部１０７によって得られる力覚情報の変動は、図８（ｂ）のグラフ７１ｂに示されている。このグラフ７１ｂに示すように、力覚情報のグラフは、高周波な変動状態もしくは高振幅な変動状態から、把持変動性判定タイミング７３位置を境界として、低周波な変動状態もしくは低振幅な変動状態に、状態が変化する。この段階での画像処理は、図３および図４のフローチャートにより、把持変動性判定タイミング７３経過前は、視覚制御部１０９の制御によって、画像処理の実行が抑制された状態である（図７（ｂ）の破線の画像処理単位７４）。
【００５５】
一方、把持変動性判定タイミング７３経過後は、把持安定状態と判定され、画像取得等である画像処理単位７５（実線で示されている）から実行が開始される。
このように、力覚情報取得部１０７が取得した力覚情報によって、把持変動性評価部１０８が把持変動性（把持安定性）の評価を行う。この把持変動性の評価に応じて、視覚制御部１０９が、把持対象物取得部１１０等の画像処理実行ユニットを動作させるか否か（またはどのように動作させるか）の制御を可変に行う。このため、把持対象物情報取得部１１０や位置・姿勢算出部１１１の不要な画像処理実行を有効に抑制することができ、画像処理に係る処理負荷が軽減される。
【００５６】
（本実施形態の効果）
以上述べたように、本実施形態によれば、把持対象物１４の把持ずれ現象が発生している状態での、各種演算の抑制と、把持ずれの状態の検出を実現できる。
さらに、本実施形態によれば、把持ずれが解消した後の安定した状態での把持対象物１４の正確な位置および姿勢を得ることができる。したがって、把持ずれが発生しても正確に把持対象物１４を運搬でき、把持ずれが発生しても組み付け等のタスクを実現することができる。
【００５７】
実施形態２
（概要）
本実施形態は、実施形態１で説明した構成および処理に加え、把持ずれ現象が進行している過程での把持対象物１４の位置および姿勢の算出を行い、把持ずれの状態把握を行うように構成したものである。このような構成および処理によって、実施形態２は、ロボット行動計画を作成して復帰の対処ができる。以下、実施形態２を実施形態１と異なる点につき説明する。
（把持変動性検出の実例：実施形態２の画像処理制御）
本実施形態における視覚制御部１０９の動作に関して説明する。実施形態２において、実際に発生する把持ずれに対して力覚情報取得部１０７が取得する力覚情報と、画像処理の連動性に関しての実例を図９に示す。図９（ａ）は、把持対象物１４に対する把持状態が不安定になっている状態（図７（ａ））における力覚情報を表すグラフと、画像処理の流れを模式的に表したタイムチャートとを示す図であり、実施形態１において説明した図７（ｂ）に相当する図である。
【００５８】
図７（ａ）に示すように１４ａの位置から１４ｂの位置へ把持ずれが発生した状態において、力覚情報取得部１０７が取得する力覚情報は、図９（ａ）のグラフ８１ａのように、不規則な高周波成分を含む状態もしくは高振幅な信号を含む状態となる。このような状態の場合、本実施形態では、実施形態１と異なり、把持対象物情報取得部１１０や位置・姿勢算出部１１１等が実行する画像処理は、視覚制御部１０９によって次のように制御される。すなわち、把持ずれが発生している間、画像処理等は、画像サイズ等の画像処理単位の値を小さく設定され、また、画像処理のフレームレートを上げて実行されるよう制御される。ここでは、画像サイズとフレームレートを調整する例を示したが、画像解像度や他のパラメータを調整するのでもよい。このように、視覚制御部１０９（制御手段）は、把持対象物情報取得部１１０（第１の取得手段）及び位置・姿勢算出部１１１（算出手段）の少なくとも１つが実行する画像処理の画像サイズ及び画像解像度のうち少なくとも１つを決定する。また、画像サイズや画像解像度以外の、画像処理のフレームレート等を決定してもよい。
【００５９】
本実施形態の図９（ａ）に示す動作が、実施形態１の図７（ｂ）との動作と異なる点は、把持ずれ現象が進行している過程において、画像サイズが小さいが高フレームレートに画像処理を実行し、把持対象物１４の位置および姿勢を算出している点である。このように構成することで、本実施形態においては、把持ずれ現象の進行状態を低精度ではあるがより迅速に把握することができる。
次に、図９（ｂ）は、把持対象物１４に対する把持状態が不安定な状態から安定な状態へ遷移している状態を示す。
図８（ａ）に示す把持対象物１４が把持ずれ現象を起こしている不安定な状態から、把持対象物１４ｄの位置へ移動し、把持が安定して把持ずれが終了した状態において、力覚情報取得部１０７が取得する力覚情報が図９（ｂ）の力覚情報に示されている。グラフ８１ｂに示すように、把持変動性判定タイミング位置８３の経過前、力覚情報値は、高周波成分を含むもしくは高振幅な変動を伴う値である。一方、把持変動性判定タイミング位置８３の経過後、力覚情報値は、低周波成分を主として含む状態に変位する、もしくは低振幅な状態に変位する。
【００６０】
この場合の画像処理等は、把持変動性判定タイミング８３を境にして次のように変化する。すなわち、把持変動性判定タイミング８３の経過前は、画像サイズが小さく高フレームレートで画像処理等が実行される（図９（ｂ）の画像処理単位８４）。一方、把持変動性判定タイミング８３の経過後は、画像サイズが大きく、低フレームレートで画像処理等が実行される（図９（ｂ）の画像処理単位８５）。図９（ｂ）において、画像処理単位８２、８４と比べて画像処理単位８５は、より大きいサイズもしくは解像度によって画像処理が行われる。このため、把持状態が安定した後には、低フレームレートではあるが、精度が高い位置および姿勢の計測値の算出が実行される。
【００６１】
上記のように構成することで、本実施形態では、視覚制御部１０９は、把持ずれ中の把持対象物１４の位置および姿勢を位置・姿勢算出部１１１に算出させることができる。この際、上述した視覚制御部１０９の制御により、位置・姿勢算出部１１１は、把持ずれ進行中においても、把持対象物１４の位置および姿勢算出処理の精度を下げる替わりにより高速に位置および姿勢を算出することができる。また、把持対象物１４に対する把持ずれがなくなり、安定した把持状態になると、視覚制御部１０９は、画像処理単位を大きくし、フレームレートを下げて行わせるように画像処理実行を制御する。その結果、より高精度な把持対象物１４の位置および姿勢を算出することが可能になる。
【００６２】
（実施形態２の処理フローチャート）
次に、本実施形態におけるロボット制御装置１０の動作を、図１０、図１１のフローチャートに基づき説明する。図１０、図１１で示される処理動作は、ロボット制御装置１０の備えるＣＰＵがプログラムを実行することによって達成される。ここでは、実施形態１の図３および図４のフローチャートと異なる点につき説明する。
実施形態１において把持ずれ現象が進行中のとき、視覚制御部１０９は把持対象物情報取得部１１０と、位置・姿勢算出部１１１の処理を停止（抑制）していた。これに対して、本実施形態においては、把持変動性に応じて画像処理の処理量を変化させ、把持ずれ現象が進行している途中でも、把持対象物１４の位置および姿勢を算出する。
本実施形態の基本的な処理の流れ（図１０のＳ１から図１１のＳ２０まで）は、実施形態１の図３、図４と同じであるため全体の処理の説明は割愛する。
【００６３】
本実施形態が、実施形態１から変更されている点は、視覚制御部１０９の制御に関わる処理である図１０のＳ３ｂ、図１１のＳ１３ｂ、Ｓ１４ｂ、Ｓ１５ｂである。また、この視覚制御部１０９の制御の変更に伴って、視覚制御部１０９の影響を受ける把持対象物情報取得部１１０と、位置・姿勢算出部１１１とに関わる処理である、図１０のＳ４ｂ、Ｓ５ｂ、図１１のＳ１６ｂ、Ｓ１７ｂも変更されている。
まず、Ｓ３ｂにおいて、視覚制御部１０９は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１とに対する実行制御量を少なくする。実行制御量が少なく設定されたことにより、把持ずれ算出部９は、より高精度な画像処理（把持対象物１４の位置および姿勢の算出）を実行する。
【００６４】
本実施形態において、実行制御量は、視覚制御部１０９が把持対象物情報取得部１１０と、位置・姿勢算出部１１１とに対して出力する。この実行制御量とは、把持対象物情報取得部１１０と、位置・姿勢算出部１１１の処理方法と量を決定する値であり、実行制御量を少なくすると、処理対象画像は高解像度（画像サイズ大）で処理される。従って、画像処理のフレームレートは低下する。逆に実行制御量を多くすると、処理対象画像は低解像度（画像サイズ小）で処理されるため、画像処理のフレームレートは向上する。また、低解像度で処理する代わりに、解像度は変えずに画像をトリミングしてサイズを小さくした画像を使っても、画像処理のフレームレートは向上する。
したがって、Ｓ３ｂにおいて、視覚制御部１０９が実行制御量を少なく設定したことは、把持対象物情報取得部１１０と、位置・姿勢算出部１１１において、低フレームレートであるが、高解像度で処理が行われる。
【００６５】
次に、本実施形態は、Ｓ１０およびＳ１２を経て得られる把持ずれ現象の３状態に対する処理の内容が実施形態１と異なる。
第１の状態は、図１１のＳ１０（Ｙｅｓ）から分岐しＳ１１からＳ１４ｂに至る処理の状態である。この場合は、把持対象物１４に対する把持状態が安定せず、把持ずれ現象が進行している過程であると判断する。図１１のＳ１１において、把持変動性値評価部１０８は、前述の把持ずれフラグをＯＮにし、Ｓ１４ｂに進む。
Ｓ１４ｂにおいて、視覚制御部１０９は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１とに対する上述した実行制御量を多くした設定を把持ずれ算出部９ｂへ行う。
第２の状態は、Ｓ１２（Ｙｅｓ）から分岐しＳ１３ｂに至る処理の状態である。この場合は、把持状態が一度不安定になり把持ずれ現象が発生した後に、把持が安定した状態に移行したと判断する。Ｓ１３ｂにおいて、視覚制御部１０９は、把持対象物情報取得部１１０と位置・姿勢算出部１１１との実行制御量を少なくした設定を把持ずれ算出部９へ行う。
【００６６】
第３の状態は、Ｓ１２（Ｎｏ）から分岐しＳ１５ｂに至る処理の状態である。この場合は、過去に把持ずれ現象が発生したことがない状態、つまり、把持対象物１４に対する把持ロボット行動計画や、運搬ロボット行動計画等がこれまで正常に実行できていると判断する。この状態では、特に問題は生じないので、Ｓ１５ｂにおいて、視覚制御部１０９は、把持対象物情報取得部１１０と、位置・姿勢算出部１１１との実行制御量を少なくした設定を把持ずれ算出部９へ行う。
Ｓ１３ｂ、Ｓ１５ｂもしくはＳ１４ｂを実行した後、Ｓ１６ｂにおいて、把持対象物情報取得部１１０は、把持ずれ後の把持対象物１４の位置の確認のために、再度、把持対象物１４の３次元情報や２次元情報の取得を行う。
【００６７】
次に、Ｓ１７ｂにおいて、位置・姿勢算出部１１１は、Ｓ１６ｂで取得された把持対象物１４の３次元情報や２次元情報から、把持ずれ後の把持対象物１４の位置および姿勢の算出を行う。
次に、Ｓ１８において、Ｓ１７ｂで取得された把持対象物１４の位置および姿勢と、図１０のＳ５ｂでロボット行動計画情報算出の際に算出された把持対象物１４の位置および姿勢とを比較し、把持ずれが発生したか否かを判定する。両者の差が所定の閾値より大きい場合は、把持ずれ現象による把持対象物１４の把持ずれ量が、ロボット行動計画情報の実行に影響があると判断をして、図１０のＳ２に戻る。図１０のＳ２において、行動計画部１１２は、把持ずれ後の把持対象物１４の位置および姿勢に基づいて、ロボット装置１１の復帰動作のプランニングを実行する。
なお、Ｓ１５ｂを経て、Ｓ１８ｂが実行される場合、すなわち把持ずれが生じていないと判断される場合でも、実施形態１と異なり、Ｓ１６ｂおよびＳ１７ｂの実行は抑制されずに実行されるため、把持ずれ発生有無がＳ１８で判断される。
【００６８】
本実施形態においては、把持変動性がない場合（Ｓ１０Ｎｏ）、Ｓ１６ｂにおいて、視覚制御手段１０９は、把持対象物情報取得部１１０から得られる把持対象物取得情報の画像サイズを大きくすることにより、画像サイズとフレームレートの制御を行える。同様に、把持変動性がない場合、Ｓ１７ｂにおいて、視覚制御手段１０９は、位置・姿勢算出部１１１で処理される画像サイズを大きくすることにより、画像サイズとフレームレートの制御を行える。これにより、視覚制御手段１０９は、図９において説明した制御を実現することができる。例えば、画像サイズを大きくし、フレームレートを下げる制御を行うことができる。
把持ずれ現象が生じている間でも、Ｓ１７ｂにおいて、視覚制御手段１０９は、位置・姿勢算出部１１１の画像サイズやフレームレートを制御し、把持対象物１４の位置および姿勢を、制御された画像サイズやフレームレートで出力させることができる。例えば、画像サイズを小さくし、フレームレートを上げる制御を行うことができる。
【００６９】
（変形例）
また、本実施形態の変形例として、把持対象物情報取得装置１２の視覚センサは、把持変動性値に応じて、シャッター速度を変更してもよい。把持変動性値と、被写体（把持対象物１４）の撮影ぶれには関係性があるため、把持変動性値に応じてシャッター速度を可変とすることで、被写体の撮影ぶれの少ないより正確な把持対象物情報を取得することができる。なお、撮影ぶれとは、例えば２次元情報（２次元画像）上で被写体の動きによって被写体の像が多重に露光される現象である。
更に、把持対象物情報取得装置１２の視覚センサのシャッタータイミングを、図９（ｂ）で示した把持変動性判定タイミング８３に合わせてもよい。把持対象物情報取得装置１２の視覚センサのシャッタータイミングを合わせることによって、被写体の撮影ぶれの少ないより正確な把持対象物情報を取得することができる。
【００７０】
このように、把持変動性値に基づいて、ロボット制御装置１０の各部の処理内容を変更することによって、把持ずれ現象が発生している状況において、把持対象物１４の位置および姿勢を取得することができる。したがって、本実施形態によれば、把持ずれ現象が発生している状況下でも、行動計画部１１２は、把持対象物１４の位置および姿勢の状態を考慮しながら、ロボット行動計画情報を生成し実行することができる。その結果、行動計画部１１２は、例えば、ロボットハンド１６の把持圧力を高めるなどのロボット行動計画情報を生成し実行することができる。これにより、把持ずれ現象が進行している状態においても、それに対応した行動をロボット装置１１に行わせることができる。
【００７１】
（本実施形態の効果）
以上説明したように、本実施形態に係るロボット制御装置１０によれば、把持対象物１４の把持状態の変動性を効率よく取得して、把持ずれ後の安定した状態での把持対象物１４の正確な位置および姿勢を得ることができる。さらに、把持ずれ現象が進行している過程での把持対象物１４の位置および姿勢も得ることができる。したがって、本実施形態に係るロボット制御装置１０は、把持ずれが発生しても、ロボット装置１１に正確に把持対象物１４を運搬し、組み付け等のタスクを実行させることができる。
また、本実施形態に係るロボット制御装置１０は、把持ずれ現象が進行中の間も視覚センサの撮影パラメータを連動させて撮像を行わせることができるので、把持対象物１４の位置および姿勢をより正確に得ることができる。把持ずれ現象が進行中の間に、画像処理を行う際の画像サイズやフレームレート、画像の解像度を可変に制御することで、処理負荷を低減することができる。
【００７２】
実施形態３
（概要）
本実施形態は、実施形態１で説明した構成に加え、力覚センサと視覚センサ（固定カラ等）を用いて把持ずれ現象の状態を判断し、視覚制御を行う。このような構成によって、本実施形態のロボット制御装置１０ｂは、把持ずれ現象が発生しても正確に組み付けタスクをロボット装置１１に実行させることができる。
ここで、対象とするタスクとして部品（把持対象物１４）を所定の位置（組み付け先部位１７）に組み付けるジョブ（ロボットシステムによる把持対象物１４への操作）は、実施形態１と同様であるとの前提で説明を行う。
【００７３】
（本実施形態の構成）
図１２は、本実施形態に係るロボット制御システム８ｂの構成を示す図である。
本実施形態において、把持対象物情報取得装置１２ｂは、ロボット装置１１以外の箇所に固定されている視覚センサである。図１２に示された構成と、実施形態１の図１との相違点は、把持対象物情報取得装置１２ｂと、位置・姿勢算出部１１１ｂとである。
本実施形態において、把持対象物情報取得装置１２ｂは、ロボット装置１２のロボットアーム１５に装備されたロボットアームカメラではなく、ロボット装置１１とは別個の所定の箇所に固定された視覚センサである。このように、把持対象物情報取得装置１２ｂとして、所定の箇所に固定された視覚センサを用いることで、位置・姿勢算出部１１１ｂの処理が一部変更されている。
本実施形態にかかる位置・姿勢算出部１１１ｂの処理が、実施形態１の位置・姿勢算出部１１１の処理と異なる点は、把持対象物１４の位置および姿勢に関しての算出式の変更である。すなわち、実施形態１で説明した式２が、以下の式６のように変更される。
【００７４】
【数４】
0006700726.tif 000005
このように固定視覚センサによる把持対象物１４の位置および姿勢の算出方法の詳細は、例えば、「井口征士、佐藤宏介、「３次元画像計測」、昭晃堂、１９９０年」に記載されている。
【００７５】
（本実施形態の効果）
以上述べたように、本実施形態によれば、固定の視覚センサを用いた装置構成であったとしても、把持ずれ後の安定した状態での把持対象物の正確な位置および姿勢を得ることができる。そして、把持ずれが発生しても正確に把持対象物を運搬し、組み付け等のタスクを実現することができる。
（他の構成例）
図１３は、上記各実施形態の各部を構成することのできるコンピュータ２１０の構成の例を示す。例えば、図１に示すロボット制御装置１０（１０ｂ）を、コンピュータ２１０で構成することができる。また、ロボット制御装置１０（１０ｂ）に含まれる各部を、それぞれコンピュータ（例えばコンピュータ２１０）で構成することもできる。また、例えば、把持ずれ算出部９（９ｂ）をコンピュータ２１０で構成してもよい。行動計画部１１２やロボット制御部１１３は、把持ずれ算出部９（９ｂ）とは別のコンピュータ２１０で構成することもできる。
ＣＰＵ２１１は、ＲＯＭ２１２、ＲＡＭ２１３、外部メモリ２１４等に格納されたプログラムを実行することによって、上記各実施形態の各部を実現する。ＲＯＭ２１２は上記ＣＰＵが実行するプログラムや各種データを保持することができる。ＲＡＭ２１３は、把持対象物情報取得装置１２が撮像した画像情報や、力覚測定装置１３が取得した力覚情報を保持することができる。
【００７６】
また、外部メモリ２１４は、ハードディスク、光学式ディスクや半導体記憶装置等で構成してよく、力覚情報（値）の時系列データを格納してもよい。また、撮像部２１５は、把持対象物情報取得装置１２の視覚センサを構成してもよい。すなわち、ロボット制御装置１０や把持ずれ算出部９をコンピュータ２１０で構成し、把持対象物情報取得装置１２を、撮像部２１５で構成することができる。
入力部２１６は、ロボット制御装置１０の操作者が操作をする操作部を構成することができる。キーボードやタッチパネルで構成することができるが、マウス等のポインティングデバイスや各種スイッチで構成してもよい。
表示部２１７は、各種ディスプレイで構成することができる。そして、操作者に対して、ロボット制御装置１０のロボットの制御の様子や、把持ずれの状態等を表示させることもできる。通信Ｉ／Ｆ２１８は、外部と通信を行うインターフェースであり、インターネット等を通じてロボット制御部１０を操作して、ロボット装置１１を遠隔操作することができる。また、コンピュータ２１０の上記説明した各部はバス２１９によって相互に接続されている。
【００７７】
また、上記各実施形態は、以下の処理を実行することによっても実現される。即ち、上述した各実施形態の１以上の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給することによって実現してよい。その場合、当該システム或いは装置のコンピュータ（またはＣＰＵやＭＰＵまたは１つ以上のプロセッサ等）がプログラムを読み出して処理を実行する。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【００７８】
８・・・ロボット制御システム、９・・・把持ずれ算出部、１０・・・ロボット制御装置、１１・・・ロボット装置、１２・・・把持対象物情報取得装置、１３・・・力覚測定装置、１４・・・把持対象物、１５・・・ロボットアーム、１６・・・ロボットハンド、１７・・・組み付け先部位、１１０・・・把持対象物情報取得部、１１１・・・位置・姿勢算出部、１１２・・・行動計画部、１１３・・・ロボット制御部、１０７・・・力覚情報取得部、１０８・・・把持変動性値評価部、１０９・・・視覚制御部
【図１】
0006700726.tif 000006
【図２】
0006700726.tif 000007
【図３】
0006700726.tif 000008
【図４】
0006700726.tif 000009
【図５】
0006700726.tif 000010
【図６】
0006700726.tif 000011
【図７】
0006700726.tif 000012
【図８】
0006700726.tif 000013
【図９】
0006700726.tif 000014
【図１０】
0006700726.tif 000015
【図１１】
0006700726.tif 000016
【図１２】
0006700726.tif 000017
【図１３】
0006700726.tif 000018