JP7724164B2 - ロボット制御システム - Google Patents

Description

本発明は、３次元視覚センサを利用してロボットをフィードバック制御するロボット制御システムに関する。

ワークを把持する作業やワークを所定の位置に接続する作業を、ロボットを利用して自動化することが行われている。一般的なロボット制御システムでは、ロボットのハンドを目標となる対象物まで移動させるために、カメラ等の視覚センサで対象物の位置情報を取得した後、この位置情報をカメラ座標系から世界座標系に変換し、さらにロボット座標系に変換してロボットに伝える。しかし、カメラ座標系－世界座標系間の変換行列を決定するには、カメラのキャリブレーションを行う必要があり、このキャリブレーション作業は極めて手間とコストのかかる作業であった。

これに対して近年、ビジュアルフィードバック制御と呼ばれる技術の開発が進んでいる。この制御方法は、視覚センサが撮像した画像から得られる特徴量に基づいてロボットをフィードバック制御する方法である。例えば、対象物とロボットのハンドとの距離を特徴量として、この距離が減少するようにロボットの動作をフィードバック制御することにより、対象物の位置の座標変換を行うことなく、ハンドを対象物に到達させることができる。

特許文献１には、ワークの把持等を行うロボットシステムにおいて、ロボットアームの位置姿勢の視覚フィードバック制御を行うロボット制御装置が記載されている。特許文献１に記載されたロボット制御装置は、まず、ロボットアームを予めワークに対する教示目標位置に位置させた状態でロボットアームに取付けられたＣＣＤカメラによりワークを撮影し、得られた画像上の特徴量を目標データとして視覚認識装置に記憶する。そして、作業中の現在データと目標データとの誤差を求め、誤差が０に収束するまでロボットアームの位置姿勢を制御することを繰返す。特徴量は、ワークに設定された複数個の特徴点を画像から抽出して、その位置や向きに基づいて視覚認識装置によって求められ、ワークの位置及び姿勢を認識するものである。

特許文献２には、高齢者等の介護を支援する生活支援ロボットアーム及びハンドをビジュアルフィードバック制御する制御装置であって、ロボットアームのハンドに把持対象物を把持させた後、把持対象物を目標位置に移動させる制御装置が記載されている。特許文献２には、把持対象物を把持する直前、直後に、把持対象物自体がカメラの視界を阻害し、目標位置の特定ができないという問題を解決するために、ハンドの把持部に前方カメラを設けるとともに側方にも側方カメラを設置し、ハンドが把持対象物を把持する直前あるいは直後において、ビジュアルフィードバック制御の基礎となるカメラを前方カメラから側方カメラに切り換えることが記載されている。

特開２００３－２１１３８１号公報 特開２０１１－２３５３８６号公報

しかし、特許文献１に記載された制御装置は、ワークが線状物などであって変形しやすく、特徴点相互の位置関係が一定しない場合には、ワーク毎に目標データとすべき特徴量が異なるため、かかるワークを対象とする作業には適用できないという問題があった。また、特許文献２に記載された制御装置では、把持対象物や目標位置が動いてカメラの視野から外れると作業を完了させることができない。

本発明は、上記を考慮してなされたものであり、ビジュアルフィードバック制御によってロボットに作業を行わせるロボット制御システムであって、ワークが変形しやすい場合にも適用可能で、作業の対象が３次元視覚センサの視野から外れた場合にも作業を継続可能なロボット制御システムを提供することを課題とする。

本発明のロボット制御システムは、ロボットハンドの動作に連動して移動する第１の対象を、静止しているまたは前記ロボットハンドの動作によらず移動する第２の対象まで移動させるロボットを制御するシステムであって、前記第２の対象の３次元情報を取得する３次元視覚センサと、前記３次元視覚センサを移動させるセンサ移動手段と、制御装置とを有する。そして、前記制御装置は、前記３次元情報に基づいて前記第２の対象と前記第１の対象の距離を算出し、前記第２の対象と前記第１の対象の距離を減少させるように前記ロボットハンドの位置をフィードバック制御し、前記第２の対象が前記３次元視覚センサの視野内にあるか否かを判定し、視野外であると判定した場合は、前記３次元視覚センサの視野を広げるか移動させることにより前記第２の対象を探索し、前記第２の対象が前記３次元視覚センサの視野に含まれるように前記センサ移動手段を制御する。

本発明のロボット制御システムの第１の態様において、前記第１の対象が前記ロボットハンド自体であり、前記第２の対象がワーク上の作業目標である。
すなわち、第１の態様のロボット制御システムは、ロボットハンドを、静止しているまたは前記ロボットハンドの動作によらず移動するワーク上の作業目標まで移動させるロボットを制御するシステムであって、前記作業目標の３次元情報を取得する３次元視覚センサと、前記３次元視覚センサを移動させるセンサ移動手段と、制御装置とを有する。そして、前記制御装置は、前記３次元情報に基づいて前記作業目標と前記ロボットハンドの距離を算出し、前記作業目標と前記ロボットハンドの距離を減少させるように前記ロボットハンドの位置をフィードバック制御し、前記作業目標が前記３次元視覚センサの視野内にあるか否かを判定し、視野外であると判定した場合は、前記３次元視覚センサの視野を広げるか移動させることにより前記作業目標を探索し、前記作業目標が前記３次元視覚センサの視野に含まれるように前記センサ移動手段を制御する。

ここで、作業とは、ロボットハンドによってワーク上の作業目標に対して何らかの作用を及ぼすことを広く意味し、例えば、ロボットハンドによって作業目標を挟持や吸着によって把持することや、作業目標に切削、研削、放電、はんだ付け等の各種加工を施すことなどが含まれる。

上記第１の態様のロボット制御システムにおいて、好ましくは、前記制御装置はさらに、前記３次元情報に基づいて前記ロボットハンドが前記作業目標に対して作業するための作業方向を算出し、該作業方向と前記ロボットハンドの向きのずれを減少させるように前記ロボットハンドの向きをフィードバック制御する。

上記第１の態様のロボット制御システムにおいて、好ましくは、前記制御装置はさらに、前記３次元情報に基づいて前記ロボットハンドが前記作業目標に対して作業するための作業姿勢を算出し、該作業姿勢と前記ロボットハンドの姿勢のずれを減少させるように前記ロボットハンドの姿勢をフィードバック制御する。

本発明のロボット制御システムの第２の態様において、前記第１の対象が前記ロボットハンドの把持したワークの特定部位であり、前記第２の対象が前記特定部位を接続する接続目標である。
すなわち、第２の態様のロボット制御システムは、ロボットハンドが把持したワークの特定部位を、前記特定部位を接続する接続目標であって、静止しているまたは前記ロボットハンドの動作によらず移動する接続目標まで移動させるロボットを制御するシステムであって、前記接続目標の３次元情報を取得する３次元視覚センサと、前記３次元視覚センサを移動させるセンサ移動手段と、制御装置とを有する。そして、前記制御装置は、前記３次元情報に基づいて前記接続目標と前記特定部位の距離を算出し、該接続目標と該特定部位の距離を減少させるように前記ロボットハンドの位置をフィードバック制御し、前記接続目標が前記３次元視覚センサの視野内にあるか否かを判定し、視野外であると判定した場合は、前記３次元視覚センサの視野を広げるか移動させることにより前記接続目標を探索して、前記接続目標が前記３次元視覚センサの視野に含まれるように前記センサ移動手段を制御する。

ここで、接続とは、ワークの特定部位を移動させて接続目標に一致させることを広く意味し、例えば、ワークが備えるコネクタ（特定部位）を対となる他のコネクタ（接続目標）に接続することや、線状や帯状のワークの先端（特定部位）を穴やスリット（接続目標）に挿入することなどが含まれる。

上記第２の態様のロボット制御システムにおいて、好ましくは、前記制御装置はさらに、前記３次元情報に基づいて前記特定部位を前記接続目標に接続するための接続方向を算出し、該接続方向と前記特定部位の向きのずれを減少させるように前記ロボットハンドの姿勢をフィードバック制御する。

上記第２の態様のロボット制御システムにおいて、好ましくは、前記制御装置はさらに、前記３次元情報に基づいて前記特定部位を前記接続目標に接続するための接続姿勢を算出し、該接続姿勢と前記特定部位の姿勢のずれを減少させるように前記ロボットハンドの姿勢をフィードバック制御する。

本発明のロボット制御システムの第３の態様において、前記第１の対象が前記ロボットハンドの把持したワークであり、前記第２の対象が前記ワークを配置する配置目標であり、前記制御装置はさらに、前記３次元情報に基づいて前記ワークを前記配置目標に配置するための配置姿勢を算出し、該配置姿勢と前記ワークの姿勢のずれを減少させるように前記ロボットハンドの姿勢をフィードバック制御する。
すなわち、第３の態様のロボット制御システムは、ロボットハンドが把持したワークを、前記ワークを配置する配置目標であって、静止しているまたは前記ロボットハンドの動作によらず移動する配置目標まで移動させるロボットを制御するシステムであって、前記配置目標の３次元情報を取得する３次元視覚センサと、前記３次元視覚センサを移動させるセンサ移動手段と、制御装置とを有する。そして、前記制御装置は、前記３次元情報に基づいて前記配置目標と前記ワークの距離を算出し、前記配置目標の姿勢を算出し、前記配置目標と前記ワークの距離、および前記配置目標の姿勢と前記ワークの姿勢のずれを減少させるように前記ロボットハンドの位置をフィードバック制御し、前記配置目標が前記３次元視覚センサの視野内にあるか否かを判定し、視野外であると判定した場合は、前記３次元視覚センサの視野を広げるか移動させることにより前記配置目標を探索して、前記配置目標が前記３次元視覚センサの視野に含まれるように前記センサ移動手段を制御する。

ここで、配置目標は、ワークを配置するために十分な大きさを有する領域であり、配置とはワークを配置目標内に置くことを意味する。

本発明のロボット制御システムによれば、ビジュアルフィードバック制御によってロボットに作業を行わせるにあたり、ワークが変形しやすいものであっても適用可能であり、作業の対象が３次元視覚センサの視野から外れた場合にも作業が継続可能である。

第１実施形態のロボット制御システムの機器構成を示す図である。 第１実施形態のロボット制御システムを用いたワーク把持作業の工程フロー図である。 線状のワークの把持位置および把持姿勢を説明するための図である。 第２実施形態のロボット制御システムの機器構成を示す図である。 第２実施形態のロボット制御システムを用いたワーク把持作業の工程フロー図である。 第３実施形態のロボット制御システムの機器構成を示す図である。 第３実施形態のロボット制御システムを用いた接続作業の工程フロー図である。 接続目標の接続姿勢を説明するための図である。 第４実施形態のロボット制御システムの機器構成を示す図である。 第５実施形態のロボット制御システムの機器構成を示す図である。 第５実施形態または第６実施形態のロボット制御システムを用いた配置作業の工程フロー図である。 第６実施形態のロボット制御システムの機器構成を示す図である。

本発明のロボット制御システムの第１実施形態を図１～３に基づいて説明する。本実施形態では、ワークをロボットハンドで把持する作業において、ロボットハンドに固定された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。

本実施形態のロボット制御システム１１は、ロボットハンド２４を備えたロボット２０と、ロボットハンドに固定されたステレオカメラ３０と、ロボット制御システム１１の全体を制御する制御装置４０とを有する。ロボット制御システム１１は、ワークであるケーブル５０上の把持位置５２をロボットハンド２４に把持させる。把持位置５２はこの把持作業における作業目標である。ロボットハンド２４の位置姿勢は、ステレオカメラ３０が取得する３次元情報に基づいて、制御装置４０によってフィードバック制御される。なお、本明細書において、ロボットハンドを単に「ハンド」ということがある。また、ハンドの位置は、特に断りのない限り、ハンドのツールセンターポイント（ＴＣＰ）の位置を意味する。

本実施形態では、ケーブル５０をワークの例として説明するが、ハンド２４が把持するワークの種類は特に限定されない。ワークはケーブル以外の線状物であってもよいし、線状物以外の、例えばフラットケーブル等の帯状物であってもよい。また、ワークはコネクタ付きハーネス、電子部品、食品、文房具などであってもよい。本実施形態のロボット制御システム１１は、柔軟で変形しやすいワークに対しても適用できるので、ワークが線状物や帯状物である場合に特にメリットがある。ケーブル５０の供給形態は特に限定されない。ケーブル５０はトレイ等の上に載置されていてもよいし、筒状の容器に、一端を容器の外に出して収容されていてもよい。また、ケーブル５０は静止していてもよいし、コンベアベルト５５などに載置されて、ハンド２４の動作とは無関係に移動していてもよい。

また、本実施形態では、ケーブルの把持作業を例として説明するが、ハンド２４が作業目標に対して行う作業は、把持作業には限定されない。例えば、ハンドとして適当なエンドエフェクタを選択することによって、ワークの作業目標に切削、研削、放電、はんだ付け等の各種加工を施してもよい。

ロボット２０は多関節ロボットであって、複数のリンク２２と関節２３からなるアーム２１と、アームの先端に装着されたハンド２４を備える。ハンド２４の形式は特に限定されず、ワークの種類に応じて、その把持に適したものを用いることができる。ハンドは、例えばワークを一対の指で挟持するものであってもよいし、ワークをノズルで真空吸着して把持するものであってもよい。なお、本実施形態では、ハンド２４に３次元視覚センサであるステレオカメラ３０が固定されており、ロボット２０がステレオカメラの位置および姿勢を変化させるセンサ移動手段を兼ねている。

ステレオカメラ３０は３次元視覚センサであって、ケーブル５０の３次元情報を取得する。ここで、ケーブルの３次元情報とは、ケーブル上の把持位置の３次元座標を導出可能とする情報である。ステレオカメラ３０が取得する３次元情報は、具体的には、異なる視点から撮像された２枚の画像である。以下において、ステレオカメラ３０が取得する画像を「ステレオ画像」または単に「画像」という。

本実施形態のステレオカメラ３０はハンド２４に固定されている。ステレオカメラは、ハンドの基端部に、ハンドの先端方向に向けて設置することが好ましい。ただし、ステレオカメラとハンドの相対的な位置関係は設計図等から既知であるため、ステレオカメラの視野にハンドが入っていなくても差し支えない。

３次元視覚センサの種類はステレオカメラに限定されず、ワークの３次元情報を含む画像が取得可能であれば特に限定されない。３次元視覚センサの例としては、ステレオ方式のカメラの１台を光を投影するプロジェクタに置き換えた光切断法などのアクティブステレオ方式、ＴｏＦ方式などの機器が挙げられる。３次元視覚センサとしては、好ましくはステレオカメラを用いる。ステレオカメラは、近距離にある対象物を精度良く３次元計測するのに適しており、ハンド２４がケーブル５０を把持する際に、ステレオカメラ３０がケーブル５０に近付くにしたがって、把持位置５２の３次元座標等をより高精度で求めることができるからである。なお、ステレオカメラは３台以上のカメラによって、同時に３枚以上の画像を撮像するものであってもよい。

制御装置４０は、各種演算を行うとともに、ロボット制御システム１１の全体を制御する。制御装置４０は、ステレオカメラ３０が取得した画像の画像処理を行い、ケーブル５０上の把持位置５２の３次元座標を求め、把持位置５２とハンド２４の距離を算出するなどの各種演算を行う。制御装置４０は、把持位置５２とハンド２４との距離を減少させるように、ロボット２０をフィードバック制御してハンド２４を移動させることによって、ハンドの位置をフィードバック制御する。また、制御装置４０は、把持位置がステレオカメラ３０の取得した画像内にあるか否かを判定する。そして、把持位置５２がステレオカメラ３０の視野外にあって、画像に写っていない場合は、ステレオカメラの位置、姿勢またはその両方を変えることにより把持位置を探索して、把持位置がステレオカメラの視野に含まれるようにセンサ移動手段、本実施形態ではロボット２０の動作を制御する。制御装置による演算や制御の内容の詳細は後述する。

制御装置４０の物理的な構成は特に限定されない。制御装置４０は、物理的に１台の装置として構成されていてもよいし、２つ以上の部分に分けて構成されていてもよい。また、制御装置４０は、ロボット２０の関節２３やハンド２４の動作を制御するロボットの制御部（図示せず）と一体に構成されていてもよい。

次に、本実施形態のロボット制御システム１１を用いて、ハンド２４でケーブル５０を把持する作業を図２の工程フローに沿って説明する。

（Ｓ１１）ステレオカメラ３０でケーブル５０を撮像して、ケーブルの３次元情報を取得する。この３次元情報は、制御装置４０がケーブル５０上の把持位置５２の３次元座標等を算出する基礎となるので、ステレオカメラは、少なくとも把持位置の座標を算出可能な範囲を撮像する。把持位置の座標を算出可能な範囲とは、例えば、把持位置がマークで示されている場合はそのマークが含まれていれば足り、把持位置がマークやケーブルの先端など何らかの目印からの距離で決められている場合は、当該目印から把持位置までの部分をいう。図３では、把持位置５２はケーブル５０の先端５１から距離ｄの位置に設定されている。

また、ケーブル５０を把持するときのハンド２４の向きＸが制限される場合には、ステレオカメラ３０は、少なくともハンドが把持位置５２を把持するための把持方向Ａを算出可能な範囲を撮像する。把持方向Ａを算出可能な範囲とは、例えば、ケーブルを側方から把持する場合であれば、把持位置を含む領域であって、把持位置５２におけるケーブルの接線方向Ｔが算出可能な程度に広い範囲である。接線方向Ｔが求まれば、ケーブルに対する把持方向Ａを算出することができる。

さらに、本実施形態のように、ハンド２４がその向きＸの周りに等方的でなく、２本の指でケーブル５０を挟持する場合は、ツールセンターポイントＴＣＰを把持位置５２に一致させたときに指がケーブルと干渉することを避ける必要があるので、ハンドがケーブルを把持する際に、ハンドの向きＸに加えて、Ｘの周りの回転角度θも制限される。このような場合、ステレオカメラ３０は、少なくともハンドが把持位置５２を把持するための把持姿勢を算出可能な範囲を撮像する。把持姿勢は、ハンドがケーブルを把持するのに所要の姿勢である。ハンドの姿勢は、ハンドの向きＸとＸの周りの回転角度θで表され、把持姿勢は、把持方向Ａと把持位置を把持するハンドのＡの周りの回転角度αで表される。把持姿勢を算出可能な範囲は、上記の把持方向Ａを算出可能な範囲と同様に、例えば、ケーブルを側方から把持する場合であれば、把持位置を含む領域であって、把持位置５２におけるケーブルの接線方向Ｔが算出可能な程度に広い範囲である。

（Ｓ１２）制御装置４０は、ステレオカメラ３０が取得したステレオ画像に把持位置５２が含まれているか否かを判定する。把持位置がステレオカメラ３０の視野内になく、画像に含まれていない場合は、把持位置を探索する。探索工程（Ｓ１７）については後述する。なお、把持位置の近傍に目印がなく、把持位置の３次元座標を求めてみなければ画像に把持位置が含まれるか否かの判定ができない場合は、次工程（Ｓ１３）の後に本判定工程（Ｓ１２）を実施する。次工程（Ｓ１３）で把持位置５２の３次元座標が算出できれば把持位置は画像に含まれていたことになり、把持位置５２の３次元座標が算出できなければ把持位置は画像に含まれていなかったと判定できる。

（Ｓ１３）制御装置４０は、ステレオ画像を処理して、ケーブル５０の立体形状を認識し、把持位置５２の３次元座標と把持姿勢を算出する。これらの値は、ある程度の幅を持った許容範囲として算出してもよい。制御装置４０が算出する３次元座標や姿勢は、すべてカメラ座標系における値である。ケーブルの立体形状の認識、把持位置５２の３次元座標と把持姿勢の算出方法は、公知の方法によって行うことができる。例えば、ワークが帯状物である場合について把持位置および把持方向を算出する方法が、本出願人による特開２０２０－０３７１４７に開示されている。

（Ｓ１４）制御装置４０は、把持位置５２とハンド２４との距離、および把持姿勢とハンドの姿勢とのずれを算出する。ハンド２４の位置および姿勢は、ハンドがステレオカメラ３０の視野内にあって画像に写っていれば、画像に基づいて算出してもよいが、本実施形態では、ハンド２４にステレオカメラ３０が固定されているためステレオカメラとハンドの相対的な位置関係が既知であるので、ハンドが画像に写っていない場合であっても、カメラ座標系でのハンドの位置および姿勢を知ることができる。

（Ｓ１５）制御装置４０は、把持位置５２とハンド２４の位置の座標、および把持姿勢とハンド２４の姿勢が一致しているか否かを判定する。把持位置とハンドの位置、および把持姿勢とハンドの姿勢が一致した場合は、ハンド２４で把持位置を把持する（工程Ｓ１８）。なお、把持姿勢としてその許容範囲が与えられている場合は、ハンドの姿勢がその範囲内にあれば、両者は一致したとみなす。把持位置の３次元座標についても同様である。

（Ｓ１６）把持位置５２とハンド２４の位置、または把持姿勢とハンド２４の姿勢が一致していない場合は、制御装置４０がロボット２０に指示して、把持位置とハンドの距離、および把持姿勢とハンド２４の姿勢のずれを減少させるようにハンド２４の位置姿勢をフィードバック制御する。この過程で、ハンド２４に固定されたステレオカメラ３０も把持位置に近づいていくので、把持位置の３次元座標等の算出精度も次第に高まることとなる。

（Ｓ１７）工程Ｓ１２で、ステレオカメラ３０が取得した画像に把持位置５２が含まれていないと判定された場合は、ステレオカメラの視野を広げるか移動させて、ステレオカメラの視野から外れた把持位置を探索する。

ステレオカメラの視野を広げる一つの方法は、ステレオカメラが固定されたハンド２４をケーブル５０から遠ざかるように移動させる方法である。把持位置５２が見つかったら、工程Ｓ１１以降を繰り返す。または、ステレオカメラ３０の向きを変えて把持位置を視野の中心に寄せた後、工程Ｓ１１以降を繰り返してもよい。

ステレオカメラの視野を広げる他の方法は、ステレオカメラ３０の焦点距離が可変である場合に、ステレオカメラをズームアウトする方法である。把持位置５２が見つかったら、ステレオカメラ３０の向きを変えて把持位置を視野の中心に寄せた後、工程Ｓ１１以降を繰り返す。ステレオカメラの焦点距離は、把持位置を視野の中心に寄せた後、または工程Ｓ１１以降を繰り返す過程のどこかの段階で元に戻す。

ステレオカメラの視野を移動させる一つの方法は、ステレオカメラが固定されたハンド２４の姿勢を変えて、例えば、視線中心が螺旋を描くように視野を移動させる方法である。把持位置５２が見つかったら、ステレオカメラ３０の向きを把持位置５２が見つかった向きのままにして、あるいは好ましくは把持位置を視野の中心に寄せた後、工程Ｓ１１以降を繰り返す。

以上のとおり、制御装置４０がセンサ移動手段であるロボット２０を制御し、あるいはロボット２０およびステレオカメラ３０を制御して、把持位置５２を探索できる。なお、本工程（Ｓ１７）において把持位置をステレオカメラの視野の中心に寄せるために、ビジュアルフィードバック制御を用いることができる。また、本工程（Ｓ１７）では、把持位置５２がステレオカメラ３０の視野内にあるか否かが分かればよく、把持位置の３次元座標は利用しない。したがって、本工程はステレオ画像のうち１枚の画像だけを利用して実施することもできる。

以上の工程を繰り返すことで、前述のとおり、工程Ｓ１５において把持位置５２とハンド２４の位置、および把持姿勢とハンドの姿勢が一致していると判定された場合は、ハンド２４で把持位置５２を把持して（Ｓ１８）、作業が完了する。

なお、ワークを把持する際のハンドの姿勢が問題とならない場合は、把持姿勢の算出や把持姿勢とハンドの姿勢のずれの算出、一致判定は不要である。上記各工程は、把持位置とハンド２４の位置だけに着目して実施することができる。同様に、ワークを把持する際のハンドの回転角度θが問題とならない場合は、上記各工程は、把持位置、把持方向Ａ、ハンドの位置およびハンドの向きＸだけに着目して実施することができる。また、ハンドの向きＸや回転角度θの修正は、ハンドの位置の修正とは独立して、上記工程を繰り返す過程のどこかの段階でまとめて実施してもよい。ハンドの姿勢や回転角度が問題とならない場合にハンドの位置や向きＸだけに着目すればよいことは、以降の第２～第４実施形態においても同様である。また、ハンドの姿勢の修正を位置の修正と独立して実施可能なことは、以降の第２～第６実施形態においても同様である。

本発明のロボット制御システムの第２実施形態を図４～５および図３に基づいて説明する。本実施形態では、ワークをロボットハンドで把持する作業において、ロボットとは離れて設置された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。以下においては、第１実施形態と異なる部分を説明し、第１実施形態と同様の部分については説明を省略する。

本実施形態のロボット制御システム１２は、ロボットハンド２４を備えたロボット２０と、ロボットとは離れて設置されたステレオカメラ３０と、ステレオカメラを移動させるセンサ移動手段３２と、ロボット制御システム１２の全体を制御する制御装置４０とを有する。ロボット制御システム１２は、ワークであるケーブル５０上の把持位置５２をロボットハンド２４に把持させる。この把持作業において、ロボットハンド２４の位置姿勢は、ステレオカメラ３０が取得する３次元情報に基づいて、制御装置４０によってフィードバック制御される。

ステレオカメラ３０は、ケーブル５０の３次元情報を取得するとともに、本実施形態ではハンド２４の３次元情報も取得する。

センサ移動手段３２は３次元視覚センサであるステレオカメラ３０の位置および姿勢を変化させる。センサ移動手段の構造は、ロボット２０と同様の多関節ロボットであってもよいが、作業の内容に応じて、ステレオカメラの位置および姿勢を所定の範囲で変化させられればよいので、ロボット２０より単純な構造とすることができる。例えば、ケーブル５０が移動しない場合は、ハンド２４の移動にしたがって、ケーブルに向けたステレオカメラとケーブルとの距離を変化させるだけでよいので、シリンダーを伸縮させるリニアアクチュエータを用いることができる。ケーブル５０が移動する場合で、関節を有するアームを用いる場合であっても、関節の数をロボット２０より少なくできることが多い。

制御装置４０は、ステレオカメラ３０が取得した画像の画像処理を行い、ケーブル５０上の把持位置５２の３次元座標とともにハンド２４の位置の３次元座標を算出し、ステレオカメラ３０が取得したステレオ画像に把持位置５２およびハンド２４が含まれているか否かを判定する。制御装置による演算や制御の内容の詳細は後述する。

次に、本実施形態のロボット制御システム１２を用いて、ハンド２４でケーブル５０を把持する作業を図５の工程フローに沿って説明する。

（Ｓ２１）ステレオカメラ３０でケーブル５０およびハンド２４を撮像して、ケーブルおよびハンドの３次元情報を取得する。

（Ｓ２２）制御装置４０は、ステレオカメラ３０が取得したステレオ画像に把持位置５２およびハンド２４が含まれているか否かを判定する。把持位置およびハンドのいずれかがステレオカメラ３０の視野内になく、画像に含まれていない場合は、探索工程（Ｓ２７）によって把持位置またはハンドを探索する。

（Ｓ２３）制御装置４０は、ステレオ画像を処理して、ケーブル５０の立体形状を認識して把持位置５２の３次元座標と把持姿勢を算出し、同時に、ハンド２４の立体形状を認識してハンドの位置および姿勢を算出する。

（Ｓ２４）制御装置４０は、把持位置５２とハンド２４との距離、および把持姿勢とハンドの姿勢とのずれを算出する。

（Ｓ２５）制御装置４０は、把持位置５２とハンド２４の位置の座標、および把持姿勢とハンド２４の姿勢が一致しているか否かを判定する。把持位置とハンドの位置、および把持姿勢とハンドの姿勢が一致した場合は、ハンド２４で把持位置を把持する（工程Ｓ２８）。

（Ｓ２６）把持位置５２とハンド２４の位置、または把持姿勢とハンド２４の姿勢が一致していない場合は、制御装置４０がロボット２０に指示して、把持位置とハンドの距離、および把持姿勢とハンド２４の姿勢のずれを減少させるようにハンド２４の位置姿勢をフィードバック制御する。また、制御装置４０は、センサ移動手段３２に指示して、ステレオカメラ３０の位置姿勢を制御する。具体的には、把持位置５２またはハンド２４が画像の周縁部にあるときは、両者を画像の中心に向けて寄せるように、ステレオカメラの視野を移動させる。また、ハンドがケーブルに近づくのに合わせて、把持位置およびハンドの位置姿勢の計測精度を高めるために、ステレオカメラと把持位置との距離を縮める。

（Ｓ２７）工程Ｓ２２で、ステレオカメラ３０が取得した画像に把持位置５２またはハンド２４が含まれていないと判定された場合は、把持位置を探索する。把持位置を探索する方法は、第１実施形態と同様に、ステレオカメラ３０の視野を広げるか移動させることによって行うことができる。具体的には、ステレオカメラをケーブル５０から遠ざけることにより、またはステレオカメラ３０の焦点距離が可変である場合はステレオカメラを制御してズームアウトすることによりカメラの視野を広げて、ステレオカメラの視野から外れた把持位置を探索する。あるいは、ステレオカメラの視野を移動させて、ステレオカメラの視野から外れた把持位置を探索する。把持位置５２またはハンド２４が見つかったら、把持位置およびハンドをステレオカメラの視野に収めた状態にした後、工程Ｓ２１以降を繰り返す。

以上の工程を繰り返すことで、前述のとおり、工程Ｓ２５において把持位置５２とハンド２４の位置、および把持姿勢とハンドの姿勢が一致していると判定された場合は、ハンド２４で把持位置５２を把持して（Ｓ２８）、作業が完了する。

本発明のロボット制御システムの第３実施形態を図６～８に基づいて説明する。本実施形態では、ロボットハンドが把持したワーク上の特性部位を所定の接続目標に接続する作業において、ロボットハンドに固定された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。以下においては、第１実施形態と異なる部分を説明し、第１実施形態と同様の部分については説明を省略する。

本実施形態のロボット制御システム１３は、ロボットハンド２４を備えたロボット２０と、ロボットハンドに固定されたステレオカメラ３０と、ロボット制御システム１３の全体を制御する制御装置４０とを有する。ロボット制御システム１３は、ロボットハンド２４が把持したケーブル６０（ワーク）の先端に装着されたプラグ６１（特定部位）を、基板６２上のソケット６３（接続目標）に接続する。この接続作業において、ロボットハンド２４の位置姿勢は、ステレオカメラ３０が取得する３次元情報に基づいて、制御装置４０によってフィードバック制御される。

本実施形態では、ケーブル６０をワーク、プラグ６１を特定部位、ソケット６３を接続目標の例として説明するが、これらの種類は特に限定されない。本実施形態のロボット制御システム１３は、例えば、ケーブルやチューブの先端を加工装置等の開口部やスリットに通す、各種部品の所定の部位を機器内の所定の位置に嵌める、カバーや外装パネルの所定の部位を機器本体等の所定の位置に固定するなどの接続作業にも適用できる。また、ソケット６３は静止していてもよいし、コンベアベルト５５などに載置されて、ハンド２４の動作とは無関係に移動していてもよい。

本実施形態において、ワークが変形しない場合（ロボットハンド２４がプラグ６１を直接把持している場合などを含む）は、ワークを特定の位置および向きで把持していれば、ハンド２４に固定されたステレオカメラ３０とプラグ６１の相対的な位置関係は既知である。一方、ワークが柔軟であるとか、変形しやすいまたはワークの把持位置や向きが定まっていない場合には、ステレオカメラとプラグの相対的な位置関係が一定せず、未知である。以下においては、ステレオカメラとプラグの相対的な位置関係が未知である場合について説明する。

ステレオカメラ３０はソケット６３およびプラグ６１の３次元情報を取得する。なお、本実施形態のロボット２０はステレオカメラの位置および姿勢を変化させるセンサ移動手段を兼ねている。

制御装置４０は、ステレオカメラ３０が取得した画像の画像処理、各種演算、ロボット２０のフィードバック制御、ソケット６３の探索などを行う。制御装置による演算や制御の内容は後述する。

次に、本実施形態のロボット制御システム１３による接続作業を、図７の工程フローに沿って説明する。

（Ｓ３１）ステレオカメラ３０でソケット６３およびプラグ６１を撮像して、それぞれの位置の３次元情報を取得する。また、プラグをソケットに接続するときのプラグの向きＹおよびＹの周りの回転角度φが制限される場合には、ステレオカメラ３０は、少なくともソケットにプラグを接続するための接続方向ＢおよびＢの周りの回転角度βを算出可能な範囲を撮像する。以下において、プラグをソケットに接続するのに所要の接続方向Ｂおよび回転角度βを総称して「接続姿勢」という。

（Ｓ３２）制御装置４０は、ステレオカメラ３０が取得したステレオ画像にソケット６３およびプラグ６１が含まれているか否かを判定し、ソケットまたはプラグがステレオカメラ３０の視野内になく、画像に含まれていない場合は、探索工程（Ｓ３７）を実施する。

（Ｓ３３）制御装置４０は、ステレオ画像を処理して、ソケット６３およびプラグ６１の立体形状を認識し、ソケットの位置の３次元座標と接続姿勢を算出するとともに、プラグの位置の３次元座標および姿勢を算出する。

（Ｓ３４）制御装置４０は、ソケット６３とプラグ６１との距離、および接続姿勢とプラグの姿勢とのずれを算出する。

（Ｓ３５）制御装置４０は、ソケット６３とプラグ６１の位置、および接続姿勢とプラグの姿勢が一致しているか否かを判定する。ソケットとプラグの位置、および接続姿勢とプラグ姿勢が一致した場合は、プラグをソケットに接続する（工程Ｓ３８）。

（Ｓ３６）ソケット６３とプラグ６１の位置、または接続姿勢とプラグ姿勢が一致していない場合は、制御装置４０がロボット２０に指示して、ソケットとプラグの距離、および接続姿勢とプラグ姿勢のずれを減少させるようにハンド２４をフィードバック制御する。この過程で、ハンド２４に固定されたステレオカメラ３０もソケットに近づいていくので、ソケットの３次元座標等の算出精度も次第に高まることとなる。

（Ｓ３７）工程Ｓ３２での判定結果により、ソケット６３またはプラグ６１を探索する場合は、第１実施形態と同様に、ステレオカメラの視野を広げ、または移動させて、ステレオカメラの視野から外れたソケットまたはプラグの探索を行う。

以上の工程を繰り返すことで、前述のとおり、工程Ｓ３５においてソケット６３とプラグ６１の位置、および接続姿勢とプラグ姿勢が一致していると判定された場合は、プラグをソケットに接続して（Ｓ３８）、作業が完了する。

なお、ステレオカメラ３０とプラグ６１の相対的な位置関係が既知である場合は、工程Ｓ３１におけるプラグの撮像、工程Ｓ３２におけるプラグが画像に含まれるか否かの判定、工程Ｓ３３におけるプラグの位置姿勢の算出、および工程Ｓ３７におけるプラグの探索は行わなくてもよい。

本発明のロボット制御システムの第４実施形態を図８～９に基づいて説明する。本実施形態では、ロボットハンドが把持したワーク上の特性部位を所定の接続目標に接続する作業において、ロボットとは離れて設置された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。以下においては、第３実施形態と異なる部分を説明し、第３実施形態と同様の部分については説明を省略する。

本実施形態のロボット制御システム１４は、ロボットハンド２４を備えたロボット２０と、ロボットとは離れて設置されたステレオカメラ３０と、ステレオカメラを移動させるセンサ移動手段３２と、ロボット制御システム１４の全体を制御する制御装置４０とを有する。ロボット制御システム１４は、ロボットハンド２４が把持したケーブル６０（ワーク）の先端に装着されたプラグ６１（特定部位）を、基板６２上のソケット６３（接続目標）に接続する。この接続作業において、ロボットハンド２４の位置姿勢は、ステレオカメラ３０が取得する３次元情報に基づいて、制御装置４０によってフィードバック制御される。

本実施形態では、ステレオカメラ３０がロボットとは離れて設置されているため、ワークの種類によらず、ステレオカメラ３０とプラグ６１の相対的な位置関係は未知である。したがって、ステレオカメラ３０はソケット６３およびプラグ６１の３次元情報を取得し、制御装置４０は、ステレオカメラ３０が取得した画像に基づいてソケットおよびプラグの位置の３次元座標を算出する。また、制御装置４０は、ソケットおよびプラグが画像内にあるか否かを判定し、ソケットまたはプラグが画像に写っていない場合はソケットまたはプラグを探索する。センサ移動手段３２は３次元視覚センサであるステレオカメラ３０の位置および姿勢を変化させる。センサ移動手段３２の構造は第２実施形態で説明したものと同じである。

本実施形態のロボット制御システム１４による接続作業方法は、第３実施形態においてステレオカメラ３０とプラグ６１の相対的な位置関係が未知である場合と同様である。ただし、本実施形態では、ステレオカメラ３０がロボットとは離れて設置されているため、図７の工程Ｓ３６において、ハンド２４を移動させるとともに、制御装置４０がセンサ移動手段３２に指示して、ステレオカメラ３０の位置姿勢を制御する。具体的には、ソケット６３またはプラグ６１が画像の周縁部にあるときは、両者を画像の中心に向けて寄せるように、ステレオカメラの視野を移動させる。また、ハンドがソケットに近づくのに合わせて、ソケットおよびプラグの位置姿勢の計測精度を高めるために、ステレオカメラとソケットとの距離を縮める。

本発明のロボット制御システムの第５実施形態を図１０～１１に基づいて説明する。本実施形態では、ロボットハンドが把持したワークを所定の配置目標に配置する作業において、ロボットハンドに固定された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。以下においては、第１および第３実施形態と異なる部分を説明し、第１または第３実施形態と同様の部分については説明を省略する。

本実施形態のロボット制御システム１５は、ロボットハンド２４を備えたロボット２０と、ロボットハンドに固定されたステレオカメラ３０と、ロボット制御システム１５の全体を制御する制御装置４０とを有する。ロボット制御システム１５は、ロボットハンド２４が把持したケーブル７０をトレイ７２上に配置する。この配置作業において、ロボットハンド２４の位置姿勢は、ステレオカメラ３０が取得する３次元情報に基づいて、制御装置４０によってフィードバック制御される。

本実施形態では、ケーブル７０をワーク、トレイ７２を配置目標の例として説明するが、これらの種類は特に限定されない。本実施形態のロボット制御システム１５は、各種のワークを所定の場所に載置したり、所定の容器に収めたりする作業に適用できる。また、トレイ７２は静止していてもよいし、コンベアベルト５５などに載置されて、ハンド２４の動作とは無関係に移動していてもよい。

本実施形態においても第３実施形態と同様に、ワークが変形しない場合は、ハンド２４に固定されたステレオカメラ３０とワークの相対的な位置関係は既知である。一方、ワークが柔軟であるとか、変形しやすい場合には、ステレオカメラとワークの相対的な位置関係が一定せず、未知である。以下においては、ステレオカメラとワークの相対的な位置関係が未知である場合について説明する。

ステレオカメラ３０はトレイ７２およびケーブル７０の３次元情報を取得する。ロボット２０はステレオカメラの位置および姿勢を変化させるセンサ移動手段を兼ねている。制御装置４０は、ステレオカメラ３０が取得した画像の画像処理、各種演算、ロボット２０のフィードバック制御、トレイ７２の探索などを行う。制御装置による演算や制御の内容は後述する。

次に、本実施形態のロボット制御システム１５による接続作業を、図１１の工程フローに沿って説明する。

（Ｓ４１）ステレオカメラ３０でトレイ７２およびケーブル７０を撮像して、それぞれの位置および姿勢を算出可能な３次元情報を取得する。

（Ｓ４２）制御装置４０は、ステレオカメラ３０が取得したステレオ画像にトレイ７２およびケーブル７０が含まれているか否かを判定し、トレイまたはケーブルがステレオカメラ３０の視野内になく、画像に含まれていない場合は、探索工程（Ｓ４７）を実施する。

（Ｓ４３）制御装置４０は、ステレオ画像を処理して、トレイ７２およびケーブル７０の立体形状を認識し、トレイの位置および姿勢、ならびにケーブルの位置および姿勢を算出する。ここで、トレイの位置および姿勢は、トレイの面の中心位置と姿勢で表すことができ、ケーブルの位置および姿勢は、ケーブルを内包し、かつトレイに納まる大きさの長方形または直方体の中心位置と姿勢で表すことができる。

（Ｓ４４）制御装置４０は、トレイ７２とケーブル７０の距離、および姿勢のずれを算出する。

（Ｓ４５）制御装置４０は、トレイ７２とケーブル７０の位置および姿勢が一致しているか否かを判定する。トレイとケーブルの位置および姿勢が一致した場合は、ケーブルをトレイ上に配置する（工程Ｓ４８）。

（Ｓ４６）トレイ７２とケーブル７０の位置または姿勢が一致していない場合は、制御装置４０がロボット２０に指示して、トレイとケーブルの距離、および姿勢のずれを減少させるようにハンド２４をフィードバック制御する。

（Ｓ４７）工程Ｓ４２での判定結果により、トレイ７２またはケーブル７０を探索する場合は、第１実施形態と同様に、ステレオカメラの視野を広げ、または移動させて、ステレオカメラの視野から外れたトレイまたはケーブルの探索を行う。

以上の工程を繰り返すことで、前述のとおり、工程Ｓ４５においてトレイ７２とケーブル７０の位置および接続が一致していると判定された場合は、ケーブルをトレイ上に配置して（Ｓ４８）、作業が完了する。

本発明のロボット制御システム１６の第６実施形態は、ロボットハンドが把持したワークを所定の配置目標に配置する作業において、ロボットとは離れて設置された３次元視覚センサが取得する情報に基づいてロボットハンドの移動をフィードバック制御する。

本実施形態のロボット制御システム１６は、図１２を参照して、ステレオカメラ３０がロボット２０と離れて設置され、ロボットと独立したセンサ移動手段３２に装着される点で第５実施形態と異なり、その他の構成は第５実施形態と同様である。また、本実施形態のロボット制御システム１６を用いたケーブルの配置作業方法は、図１１に示した工程フローに沿って実施され、ステレオカメラの移動がロボット２０ではなくセンサ移動手段３２を制御して行われる点で第５実施形態と異なり、その他は第５実施形態と同様である。

本発明は、上記の実施形態や実施例に限定されるものではなく、その技術的思想の範囲内で種々の変形が可能である。

１１～１６ ロボット制御システム
２０ ロボット
２１ アーム
２２ リンク
２３ 関節
２４ ロボットハンド
２５ ツールセンターポイント
３０ ステレオカメラ（３次元視覚センサ）
３２ センサ移動手段
４０ 制御装置
５０ ケーブル（ワーク）
５１ ケーブル先端
５２ 把持位置（作業目標）
５５ コンベアベルト
６０ ケーブル（ワーク）
６１ プラグ（特定部位）
６２ 基板
６３ ソケット（接続目標）
７０ ケーブル（ワーク）
７２ トレイ（配置目標）
Ａ 把持方向
Ｂ 接続方向
ｄ ケーブル先端から把持位置までの距離
Ｔ 把持位置におけるケーブルの接線方向
Ｘ ハンドの向き
Ｙ プラグの向き
α 把持姿勢の回転角度
β 接続姿勢の回転角度
θ ロボットハンドの回転角度
φ プラグの回転角度

Claims (3)

1. ロボットハンドを、静止しているまたは前記ロボットハンドの動作によらず移動する柔軟なケーブルの把持位置まで移動させるロボットを制御するシステムであって、
   前記把持位置を撮像してステレオ画像を取得するステレオカメラと、
   前記ステレオカメラを移動させるセンサ移動手段と、
   制御装置とを有し、
   前記ロボットハンドは、前記ケーブルを挟持することにより把持可能であり、
   前記制御装置は、
   前記ステレオ画像に基づきカメラ座標系において、前記把持位置の３次元座標を算出し、前記把持位置と前記ロボットハンドの距離を算出し、前記把持位置と前記ロボットハンドの距離を減少させるように前記ロボットハンドの位置をフィードバック制御するとともに、
   前記ステレオ画像に基づき前記カメラ座標系において、前記把持位置における前記ケーブルの接線方向、前記接線方向と交差する把持方向、および前記ロボットハンドが前記ケーブルを挟持可能な前記把持方向の周りの回転角度を算出することにより、前記ロボットハンドが前記把持位置を把持するための作業姿勢を算出し、前記作業姿勢と前記ロボットハンドの姿勢のずれを減少させるように前記ロボットハンドの姿勢をフィードバック制御し、
   前記把持位置が前記ステレオカメラの視野内にあるか否かを判定し、視野外であると判定した場合は、前記ステレオカメラの視野を広げるか移動させることにより前記把持位置を探索し、前記把持位置が前記ステレオカメラの視野に含まれるように前記センサ移動手段を制御する、
   ロボット制御システム。
2. 前記ステレオカメラが前記ロボットハンドに固定され、
   前記センサ移動手段が前記ロボットである、
   請求項１に記載のロボット制御システム。
3. 前記ステレオカメラが前記ロボットと離れて設置される、
   請求項１に記載のロボット制御システム。