JP5850962B2 - ビジュアルフィードバックを利用したロボットシステム - Google Patents

Description

本発明は、カメラを使ってロボットの動作を補正するロボットシステムに関し、特に、ロボットの座標系とカメラの座標系の位置合わせを行っておく必要をなくすために、ビジュアルフィードバックを利用したロボットシステムに関する。

多くの産業用ロボットはティーチングプレイバックと呼ばれる方式に基づいて動作している。すなわち、ロボットに行わせたい動作や作業をあらかじめプログラムとして教示しておき、プログラムを再生することでロボットに所望の作業を行わせる。ロボットは、教示された動作や作業を正確に繰り返すことができるが、プログラムを教示した時とは異なる位置に対象物が置かれていると、その対象物に対して正確に作業を行うことができなくなる。

それに対して、カメラなどのビジョンセンサを使って対象物の位置を認識し、ビジョンセンサが認識した対象物の位置情報を用いてロボットの動作を補正するということが行われている。このようにすることで、対象物がプログラム教示時と異なる位置に置かれていても、対象物に対して正確に作業を行うことができるようになる。

ビジョンセンサで見つけた対象物の位置は、ビジョンセンサの計測の基準となっている座標系（以下、「ビジョン座標系」と呼ぶ）で表現されているが、ビジョンセンサで見つけた位置情報に基づいてロボットの動作を補正するためには、ビジョン座標系で表現された位置情報を、ロボットの動作制御の基準となっている座標系（以下、「ロボット座標系」と呼ぶ）での位置情報に変換する必要がある。ビジョン座標系で表された位置情報をロボット座標系で表された位置情報に変換するためのデータを「カメラのキャリブレーションデータ」と呼び、カメラのキャリブレーションデータを取得するための作業を「カメラをキャリブレーションする」という（特許文献１）。

一方で、カメラのキャリブレーションをせずに、ビジョンセンサを使ったロボットの位置補正を可能にする手段のひとつとして、ビジュアルフィードバックと呼ばれている手法が知られている（例えば、特許文献２）。例えば、ロボットを対象物の把持位置に移動させ、ロボットに取り付けられたカメラで対象物を撮影し、その対象物の見え方を目標データとして記憶しておく。そして、実作業時においては、ロボットを移動するごとにカメラで対象物を撮影し、目標データと同一の見え方となるようにロボットの位置姿勢を制御することを繰返し、誤差が０に収束したところで、ハンドによる把持を実行する。

特許文献２に記載の方法は、誤差がゼロになった位置でハンドによる把持を行う。つまり、対象物を把持する位置にロボットが位置決めされている状態で、カメラに写った対象物の見え方を目標データとして記憶する必要がある。そのため、ロボットのアーム先端部に取り付けるハンドとカメラの位置関係には制約があり、自由に設計することができない。

また、この方法の場合、誤差がゼロになった位置でのみ対象物に対して作業を行うことができるので、対象物を把持するといった簡単な作業をロボットに行わせる場合には問題にならないが、例えば、対象物に対して所定の経路で接着剤を塗布するといったような、複雑な動作経路を必要とする作業をロボットに行わせたい場合には利用できない。

一方で、基準対象物に対して目標データを設定した際のロボットの位置と、基準対象物を把持する動作を行う際のロボットの位置との差をあらかじめ位置変換行列として記憶しておき、誤差がゼロになるようにロボットに収束動作をさせた後、記憶している位置変換行列で示される動作を行ってから把持動作を行う方法も提案されている（特許文献３）。この発明では、特許文献２に記載の方法に比べると、ロボットのアーム先端部に取り付けるハンドとカメラの位置関係には制約がなくなっているが、複雑な動作経路を必要とする作業をロボットに行わせたい場合には利用できないという問題は解消されていない。

ところで、対象物がテーブルなどの平らな台の上に載置されている場合、目標データと同一の見え方となるようにロボットの位置姿勢を制御する際は、ロボットのアーム先端部に取り付けられたカメラと対象物が載置されたテーブルの距離が変わらないように制御するのが望ましい。また、対象物の向きが変わる場合には、カメラと対象物の相対的な向きが同じになるようにロボットを制御する必要があるが、カメラとテーブルの相対的な傾きが変わらないように、テーブル面に垂直な軸回りにカメラが回転するように制御するのが望ましい。

しかしながら、通常のビジュアルフィードバックでは、カメラとテーブルの距離が変わらないことや、カメラとテーブルの相対的な傾きが変わらないことは保証されず、その結果、目標データと同一の見え方になるようにロボットを制御するのに必要以上に時間がかかることがある。

この問題を回避するには、テーブル面と平行な座標系をロボットに設定しておいて、それを拠り所としてカメラの移動を行わせるようにすればよい。たとえば、ロボットのハンド上の任意の１点をＴＣＰ（Ｔｏｏｌ Ｃｅｎｔｅｒ Ｐｏｉｎｔ）として設定し、テーブル面上の一直線上にない３箇所にロボットのＴＣＰを接触させて、それぞれの箇所におけるロボット座標系からみたＴＣＰの座標値を読み取ることにより、テーブル面上にロボットの座標系を設定することができる（特許文献４）。

特開平１０−４９２１８号公報 特開２００３−２１１３８１号公報 特許第４２６５０８８号公報 特開２０１０−０７６０５４号公報

しかしながら、このような方法で座標系を設定する作業は、作業者の技量により設定精度に差が出るといった問題や、ロボットのハンドとテーブルを物理的に接触させるため、操作ミスによりハンドやテーブルに損傷を与える危険があるといった問題がある。

本発明は、ビジュアルフィードバックを用いてロボットの動作を補正するシステムにおいて、対象物に複雑な経路の作業をさせることを可能とすると共に、作業者がロボットの座標系について意識をしないでも、カメラをテーブル面に平行に移動し、テーブル面に垂直な軸回りに回転させることで、効率的にカメラを所定の位置に移動させる方法を提供することを目的とする。

本発明の一実施形態に係るロボットシステムは、ロボットのアーム先端部に装着された装置を用いて、平面上の第１の対象物位置に置かれた対象物に対して、所定の作業を行う動作指令を含むプログラムによって制御されるロボットと、アーム先端部に装着され、対象物を撮像するカメラと、第１の対象物位置に対して、所定の相対的位置関係にあるアーム先端部の位置を第１のロボット位置として記憶する第１のロボット位置記憶部と、対象物が第１の対象物位置に置かれ、アーム先端部が第１のロボット位置に位置決めされた状態である目標到達状態において、カメラの画像上での対象物の位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を目標到達状態データとして記憶する目標到達状態データ記憶部と、平面上の第２の対象物位置に置かれた対象物を、ロボットのアーム先端部が任意の初期位置に位置決めされた状態において、カメラで撮像した際における、対象物のカメラの画像上での位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を目標到達状態データのうちの対応する特徴量と一致させるための、アーム先端部の任意の初期位置からの移動量を求めるロボット移動量計算部と、アーム先端部を移動量に基づいて移動させた際における、アーム先端部の位置である第２のロボット位置と、第１のロボット位置との差異に基づいてプログラムを補正するための補正データを計算する補正データ計算部と、を備えることを特徴とする。

カメラの画像に写る対象物の画像上での特徴量が目標到達状態データと同じになるようにロボットを制御することによって、第２の対象物位置に置かれた対象物と第２のロボット位置の相対的な位置関係は、第１の対象物位置に置かれた対象物と第１のロボット位置の相対的な位置関係と同一になっていると期待できる。したがって、第１の対象物位置に置かれた対象物に対して作業を行うように教示されたプログラムを、第１のロボット位置と第２のロボット位置の差異に基づいて補正して再生することで、第２の対象物位置に置かれた対象物に対して作業を行うことができる。これは、プログラムに教示されているロボットの動作が複雑な経路であっても、第２の対象物位置に置かれた対象物に対して所望の作業を行うことが可能になることを意味する。

本発明の他の実施形態に係るロボットシステムは、ロボットのアーム先端部が第２のロボット位置に位置決めされた状態において、カメラが撮像した、第２の対象物位置に置かれた対象物の画像上での位置、姿勢、及び大きさのうちの少なくとも１つの特徴量と、目標到達状態データ記憶部に記憶された目標到達状態データのうち対応する特徴量との差異が所定の値よりも大きい場合に、ロボット移動量計算部は、第２のロボット位置をあらためて初期位置として移動量を求め、アーム先端部を該移動量に基づいて移動させた際における、アーム先端部の位置をあらためて第２のロボット位置とすることを特徴とする。

目標到達状態に一致させるようなロボットの移動量を求める方法としては、いくつかの方法が提案されており、本願発明はどのような方法でロボットの移動量を求めるかについては限定しない。しかしながら、簡便な方法を採用した場合には、最初に求めた移動量に基づいてロボットを１回移動させただけでは目標到達状態にならないことがある。そのような場合は、第２のロボット位置を改めて初期位置として同様の処理を繰り返すことで、目標到達状態に徐々に近づけていくことができる。

本発明のさらに他の実施形態に係るロボットシステムは、ロボットのアーム先端部が第２のロボット位置に位置決めされた状態において、ロボット移動量計算部は、第２のロボット位置をあらためて初期位置として移動量を求め、該移動量が所定の値よりも大きい場合に、アーム先端部を該移動量に基づいて移動させた際における、アーム先端部の位置をあらためて第２のロボット位置とすることを特徴とする。

本発明のさらに他の実施形態に係るロボットシステムは、互いに異なる複数の第２の対象物位置を少なくとも３つ設け、該複数の第２の対象物位置に対象物を置き、それぞれの対象物位置に対応した複数の第２のロボット位置を取得し記憶する第２のロボット位置記憶部と、第２のロボット位置記憶部に記憶された複数の第２のロボット位置に基づいて、対象物が置かれている平面の法線ベクトルを算出する法線ベクトル計算部を更に備え、ロボット移動量計算部は、法線ベクトルに垂直な平面内の並進移動、及び該法線ベクトルに平行な軸回りの回転移動のみを移動量として算出することを特徴とする。

本発明のさらに他の実施形態に係るロボットシステムは、互いに異なる複数の第２の対象物位置の１つが第１の対象物位置と同一であり、複数の第２のロボット位置の１つが第１のロボット位置であることを特徴とする。

対象物が置かれているテーブル面に垂直な軸の方向ベクトル（法線ベクトル）は、第１のロボット位置と２つの第２のロボット位置、あるいは３つの第２のロボット位置があれば計算できる。たとえば第１のロボット位置から１つ目の第２のロボット位置に向かうベクトルと、第１のロボット位置から２つ目の第２のロボット位置に向かうベクトルとの外積を計算することで法線ベクトルを求めることができる。また、３つ以上の第２のロボット位置が得られている場合には、たとえば、最小二乗法などを用いて法線ベクトルを算出することもできる。

この際、計算に使用するすべての第２のロボット位置が直線上に位置していると平面が求まらないため、第１のロボット位置と２つの第２のロボット位置、あるいは３つの第２のロボット位置が、たとえば正三角形の頂点の位置となるように第２の対象物位置を設けるとよい。また、３つの第２のロボット位置がほぼ同じ位置である場合も、平面の法線ベクトルが正しく求まらない可能性がある。このため、第２のロボット位置記憶部に記憶する第２のロボット位置は、既に記憶されている第２のロボット位置と一定距離以上離れていることを確認してから記憶することが好ましい。

ロボット移動量計算部は、ロボットが第１のロボット位置に位置決めされた状態で、第１の対象物位置に置かれた対象物の中心位置を通り法線ベクトルに平行な直線の位置を、ロボットのアーム先端部に対する相対的な直線として定義し、ロボット移動量計算部による回転移動は、該定義されたアーム先端部に対する相対的な直線を回転軸として回転移動させるものであってもよい。

カメラの画像に写る対象物の画像上での特徴量が目標到達状態データと同じになるようにロボットを移動する際の回転軸を、対象物が第１の対象物位置に置かれ、ロボットが第１のロボット位置に位置決めされた状態の時のロボットのアーム先端部に対する相対的な直線として定義される、対象物の中心位置を通る法線ベクトルに平行な軸とすることで、ロボットを回転移動させたときにも、対象物が画像外にはみ出してしまうことがなくなる。

カメラを用いて作業対象物の位置姿勢を認識し、ロボットに所定の作業をさせるロボットシステムにおいて、カメラのキャリブレーションをする必要がないというビジュアルフィードバックの特徴を維持しつつ、使用する際の制約であった、複雑な経路を伴う作業を行うシステムには適用できないという制約を解消することができる。

さらに、ロボットが、作業対象物が載置してあるテーブルの面に対して近づいたり遠ざかったり、もしくは傾いたりすることにより効率的に目標到達状態に到達できないことがあるという問題を解消し、安定して、確実に、短時間で目標到達状態を実現することができる。

発明の第１の実施形態におけるロボットシステムの全体構成を示す図である。 本発明において用いられるロボット制御装置のブロック図である。 本発明のロボットシステム立ち上げ時の流れを示すフローチャートである。 本発明のロボットシステムの作業実行時の処理を示すフローチャートである。 第１及び第２の対象物位置に置いた対象物の位置を表す平面図である。 シフトされた仮想的なロボット座標系を表す図である。 ツール座標系の定義を説明するための図である。 ツール座標系とカメラとの位置関係を表す図である。 ツール座標系と第２のツール座標系との位置関係を表す図である。 ツール座標系をＺ軸回りに回転移動する様子を上から見た図である。

以下、図面を参照して、本発明に係るロボットシステムについて説明する。ただし、本発明の技術的範囲はそれらの実施の形態には限定されず、特許請求の範囲に記載された発明とその均等物に及ぶ点に留意されたい。

図１は本発明の実施形態におけるロボットシステムの全体構成を示す図である。本発明の実施形態に係るロボットシステムは、ロボット１のアーム先端部に設けられたメカニカルインターフェイス部３２に装着された装置であるツール３０を用いて、平面５上の第１の対象物位置に置かれた対象物４に対して、所定の作業を行う動作指令を含むプログラムによって制御されるロボット１と、メカニカルインターフェイス部３２に装着され、対象物４を撮像するカメラ３と、第１の対象物位置に対して、所定の相対的位置関係にあるメカニカルインターフェイス部３２の位置を第１のロボット位置として記憶する第１のロボット位置記憶部１２ａ（図２）と、対象物４が第１の対象物位置に置かれ、メカニカルインターフェイス部３２が第１のロボット位置に位置決めされた状態である目標到達状態において、カメラ３の画像上での対象物４の位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を目標到達状態データとして記憶する目標到達状態データ記憶部１２ｂと、平面上の第２の対象物位置に置かれた対象物を、ロボットのアーム先端部が任意の初期位置に位置決めされた状態において、カメラで撮像した際における、対象物のカメラの画像上での位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を目標到達状態データのうちの対応する特徴量と一致させるための、メカニカルインターフェイス部３２の任意の初期位置からの移動量を求めるロボット移動量計算部１１ｂと、メカニカルインターフェイス部３２を移動量に基づいて移動させた際における、メカニカルインターフェイス部３２の位置である第２のロボット位置と、第１のロボット位置との差異に基づいてプログラムを補正するための補正データを計算する補正データ計算部１１ｃと、を備えることを特徴とする。

ロボット１は公知の典型的なロボットであり、例えば６軸多関節ロボットである。ロボット１は、アーム部１ａ及びベース部１ｂを有しており、また、ロボット１を制御するロボット制御装置２に接続されている。

アーム部１ａのアーム先端部に設けられたメカニカルインターフェイス部３２には、ツール３０が取り付けられている。図１に示されるツール３０は、例えば、接着剤塗布用のノズル３１など、対象物に対して作業を行うための機構である。メカニカルインターフェイス部３２には、さらに、対象物４の画像を撮像するためのカメラ３が取り付けられている。

ロボット１には、ベース部１ｂに固定されたロボット座標系Σｂと、メカニカルインターフェイス部３２に固定されたメカニカルインターフェイス座標系Σｆとが定義されている。ロボット制御装置２は、ロボット座標系Σｂから見たメカニカルインターフェイス座標系Σｆの位置及び姿勢Ｒをロボット現在位置として制御する。ここで、Ｒは４×４の同次変換行列である。以下、特に断わりのない限り、「ロボット位置」とは、ロボット座標系Σｂから見たメカニカルインターフェイス座標系Σｆの位置姿勢Ｒを指すものとする。

カメラ３は、例えば産業用のＣＣＤカメラであり、画像を撮像する機能を持つ周知の受光デバイスである。本実施例では、ロボット制御装置２が画像処理機能を有しているものとして、カメラ３はロボット制御装置２に接続されているが、パソコンなど別途画像処理用の装置を設け、カメラ３は画像処理装置に接続し、画像処理装置とロボット制御装置２を通信ケーブルで接続するようにしてもよい。

対象物４は、平坦なテーブル５に載置されており、ロボット１を移動することでカメラ３の視野内に対象物４が入るように移動し、カメラ３で対象物４の画像を撮像することができる。

図２は本発明において用いられるロボット制御装置のブロック図である。ロボット制御装置２は、メインＣＰＵ（以下、単に「ＣＰＵ」と称する）１１を含んでいる。

ＣＰＵ１１は、カメラ３により撮像された画像に写る対象物４の位置、姿勢、大きさなどの特徴量を算出するための画像処理部１１ａ、算出した特徴量と記憶している目標到達状態データに基づいてロボットの移動量を求めるロボット移動量計算部１１ｂ、第１のロボット位置と第２のロボット位置に基づいてロボットの作業を補正するための補正データを算出する補正データ計算部１１ｃ、複数の第２のロボット位置に基づいて対象物４が載置されているテーブル５の面の法線ベクトルを求める法線ベクトル計算部１１ｄを備える。

ＣＰＵ１１から延びるバス１７には、ＲＡＭ、ＲＯＭ、不揮発性メモリ等からなるメモリ１２、教示操作盤用インターフェイス１３、カメラインターフェイス１４、サーボ制御部１５、及び外部装置用の入出力インターフェイス１６が接続されている。

教示操作盤用インターフェイス１３には、教示操作盤１８が接続されている。作業者は、教示操作盤１８を操作して、ロボットプログラムの作成、修正、登録、あるいは各種パラメータの設定の他、教示されたロボットプログラムの再生運転、ロボット位置の手動操作等を実行する。また、本実施例ではロボット制御装置２は画像処理機能を有しており、作業者は、教示操作盤１８を操作して、画像処理プログラムの作成、修正、登録、あるいは各種パラメータの設定の他、教示された画像処理プログラムの再生運転、検出テスト等を実行する。

ロボット１を制御するシステムソフトウェアや画像処理を行うシステムソフトウェアは、メモリ１２のＲＯＭに格納されている。また、アプリケーションに応じてロボットの作業内容が教示されるロボットプログラムやそれに関連する設定データは、メモリ１２の不揮発性メモリに格納されている。メモリ１２のＲＡＭは、ＣＰＵ１１が行なう各種演算処理におけるデータの一時記憶の記憶領域に使用される。メモリ１２は、ロボット位置を記憶するロボット位置記憶部１２ａ、画像処理部１１ｂにより算出された目標到達状態データを記憶する目標到達状態データ記憶部１２ｂ、を含んでいる。

サーボ制御部１５はサーボ制御器＃１〜＃ｎ（ｎはロボットの総軸数であり、ここではｎ＝６とする）を備え、ＣＰＵ１１からの動作指令を受け、各軸に付属したパルスコーダ（図示せず）から受け取るフィードバック信号と併せてサーボアンプＡ１〜Ａｎにトルク指令を出力する。各サーボアンプＡ１〜Ａｎは、各トルク指令に基づいて各軸のサーボモータＭ１〜Ｍｎに電流を供給してそれらを駆動する。

カメラインターフェイス１４には、撮像手段であるカメラ３が接続される。ＣＰＵ１１から撮影指令がカメラインターフェイス１４を介して送られると、カメラ３は電子シャッタ機能により撮影を実行し、映像信号がグレイスケール信号の形でカメラインターフェイス１４を介してメモリ１２に格納される。

本願のロボットシステムでは、テーブル５上の任意のひとつの位置に対象物４を載置した状態で、対象物４に対して所定の作業を行うロボットプログラムを教示しておく。この位置を「基準位置」と呼ぶ。対象物４が基準位置に載置されている場合は、そのロボットプログラムを再生するだけで、ロボット１は対象物４に対して適切な作業を行うことができる。

その上で、対象物４が基準位置とは異なる位置に載置されていた場合は、後述する本発明が開示する手法によって補正データを計算し、補正データに基づいてロボットプログラムを補正しながら再生することで、基準位置とは異なる位置に載置されている対象物４に対してロボット１が適切に作業を行えるようにする。

図３は、本発明のロボットシステム立ち上げ時の流れを示すフローチャートである。以下、図３を参照しつつ、ロボットシステムの立ち上げ手順を説明する。

はじめに、対象物４を、テーブル５上の基準位置（以下、「第１の対象物位置」と呼ぶ）に載置する（Ｓ１０１）。

次に、第１の対象物位置に置かれた対象物４がカメラ３の視野に写るような第１のロボット位置にロボット１を移動する（Ｓ１０２）。第１のロボット位置は、カメラ３の光軸がテーブル５の面に対しておおよそ垂直になるような位置が好ましいが、この条件に限定するものではない。たとえば、カメラ３の光軸がテーブル５の面に対して多少傾いていても問題ない。また、第１のロボット位置は、画像に写った対象物４の像が受けるレンズひずみ等の影響を最小限に抑えるため、対象物４がカメラ視野のおおよそ中心に写る位置とすることが好ましい。

次に、カメラ３で対象物４の画像を撮像し、対象物４の画像をモデルデータとして教示する（Ｓ１０３）。ここで、どのような情報をモデルデータとして記憶するかは、画像から対象物４を検出するのに用いる画像処理のアルゴリズムに依存する。本発明では、どのような画像処理アルゴリズムを用いても構わないが、たとえば、正規化相関を用いたテンプレートマッチングや、エッジ情報を用いた一般化ハフ変換などの手法が知られており、そういった手法を用いることができる。正規化相関によるテンプレートマッチングの場合は、テンプレートがモデルデータに相当する。

次に、教示したモデルデータを用いて、画像から対象物４を検出し、検出した対象物４の画像上での位置、姿勢、大きさなどの特徴量を目標到達状態データとして目標到達データ記憶部１２ｂに記憶すると共に、第１のロボット位置をロボット位置記憶部１１ａに記憶する（Ｓ１０４）。たとえば、画像処理アルゴリズムとしてテンプレートマッチングを採用した場合、テンプレートが最も一致した際の、テンプレートの中心位置、回転角度、拡大縮小した割合をそれぞれ、位置、姿勢、大きさの特徴量とすることができる。

最後に、第１の対象物位置に載置された対象物４に対して、ロボット１が所定の作業を行うロボットプログラムを教示する（Ｓ１０５）。

図４は、本発明のロボットシステムの作業実行時の処理を示すフローチャートである。以下、図４を参照しつつ、本発明のロボットシステムにおけるロボットシステムの動作を説明する。

まず、ロボットを初期位置へ移動する（Ｓ２０１）。通常、ロボットの初期位置は第１のロボット位置と同じでよいが、対象物４がカメラ３の視野に入り、かつ、ロボットの姿勢が第１のロボット位置と同じであれば、異なる位置であってもよい。たとえば、テーブル５が広く、第１のロボット位置だけではテーブル５の全面がカメラ３の視野に収まりきらない場合は、初期位置を変更することにより、テーブル５の全面を複数の視野に分割して計測を行うことができる。

次に、カメラ３で対象物４の画像を撮像し、画像処理による対象物４の検出を実行する（Ｓ２０２）。画像処理アルゴリズムには、先のＳ１０４と同じ手法を用い、対象物４の特徴量（画像上での位置、姿勢、大きさ）の情報を取得する。

次に、Ｓ２０２で取得した特徴量が、記憶している目標到達状態データと一致するような位置へロボットを移動するためのロボット移動量を計算する（Ｓ２０３）。画像から取得した特徴量に基づいて、ロボット移動量を算出する手法は様々なものが提案されており、本発明ではどのような手法を用いるかは限定しないが、一実施例を後述する。

次に、現在のロボット位置が目標到達状態であるか否かを判断する（Ｓ２０４）。これは、Ｓ２０３で算出したロボット移動量の大きさを、あらかじめ設定しておいたしきい値と比較することで判断すればよい。たとえば、しきい値が０．５ｍｍであれば、計算されたロボット移動量が０．５ｍｍ未満であれば、目標到達状態であると判断することができる。また、Ｓ２０４での判断基準を、Ｓ２０２で取得した特徴量と、記憶している目標到達状態データのうち対応する特徴量との差異を、あらかじめ設定しておいたしきい値と比較した値としてもよい。たとえば、しきい値が１画素であれば、計算された位置の際が１画素未満であれば、目標到達状態であると判断することができる。

目標到達状態でない場合は、Ｓ２０３で算出したロボット移動量に基づいてロボット１を移動させる（Ｓ２０５）。１回ロボットを移動させたことで目標到達状態にあるものとして次のステップ（Ｓ２０６）に進んでもよいが、図４のフローチャートではＳ２０４の目標到達条件を満たしている事を再確認するために、Ｓ２０２へ戻るようにしている。

目標到達状態であると判断された場合は、その時のロボット位置を第２のロボット位置として記憶すると共に、記憶している第１のロボット位置と第２のロボット位置に基づいて、検出した対象物４に対しての作業を行うための補正データを計算する（Ｓ２０６）。

ロボットプログラムの補正の原理と、補正データの計算方法については以下の通りである。

前述したように、ロボット位置は、ロボット座標系Σｂから見たメカニカルインターフェイス座標系Σｆの位置及び姿勢Ｒとして表すことができる。ここで、Ｒは４×４の同次変換行列である。ロボットプログラムは、ロボット１を複数のロボット位置に順に移動させるため、複数のロボット位置の集合と考えることができる。ｉ番目のロボット位置を
Ｒ_iとすると、求めたい補正データは、

となるような４×４の同次変換行列Ｘであると考えることができる。すなわち、ロボットプログラムに教示されているロボット位置Ｒ_iを、補正データＸに基づいて、上記の式でＲ_i’に変換しながら再生することでロボットの補正を実現する。この変換行列Ｘには、図６に示すように、仮想的にロボット座標系ΣｂをＸだけシフトしてロボットプログラムを再生するような効果があるので、複雑な経路を伴う作業であっても、対象物４の位置や向きに合わせて適切にロボットのプログラムを補正することができる。

本発明においては、記憶されている第１のロボット位置をＲ₁、第２のロボット位置をＲ₂とすると、補正データＸはＲ₁とＲ₂に基づいて次式で算出することができる。

最後に、計算した補正データに基づいて教示されているロボットプログラムを補正しながら再生する（Ｓ２０７）。

続いて、Ｓ２０３において、画像から得られた特徴量に基づいて、ロボット位置を目標到達状態に一致させるためのロボット移動量の計算方法のひとつとして、ヤコビアン行列を用いた方法を説明する。

まず、目標到達状態に一致させるためのロボットの移動量は、検出された対象物の特徴量と、目標到達状態データとして記憶している特徴量との差分に基づいて算出する。図５は、第１及び第２の対象物位置に置いた対象物の位置を表す平面図である。たとえば、図５では、第１の対象物位置にある対象物４の像を破線で、第２の対象物位置にある対象物４の像を実線で表しているが、ビジョン座標系での位置を（ｕ，ｖ）で表すと、Δｕが画像の縦方向の位置の差分、Δｖが画像の横方向の位置の差分となる。

また、画像上での見た目の大きさの特徴量をｓとする。たとえばテンプレートマッチングの場合は、テンプレートの大きさを１００％＝１．０として、テンプレートより大きく見えていれば１．０より大きな値、小さく見えていれば１．０よりも小さな値で表現するようにすることができる。

目標到達データとして記憶している特徴量を（ｕ₀，ｖ₀，ｓ₀）、Ｓ２０２で得られた特徴量を（ｕ₁，ｖ₁，ｓ₁）とすると、特徴量の差分は次の式で表せる。

なお、画像上での見た目の大きさの特徴量ｓは、対象物が置かれているテーブル面５からカメラ３までの距離に反比例するため、割合のままではなく、距離に比例した値になるように逆数を用いるようにしている。

求めたいロボット移動量を（Δｘ，Δｙ，Δｚ）とすると、ヤコビアン行列Ｊは次式で定義される。

ロボットの移動量（Δｘ，Δｙ，Δｚ）は、どの座標系で表現されてもよいが、カメラ３とメカニカルインターフェイス座標系Σｆの相対的な位置関係が固定されていることを考慮すると、メカニカルインターフェイス座標系Σｆで表現するのが好ましい。すなわち、本実施例では、（Δｘ，Δｙ，Δｚ）は、メカニカルインターフェイス座標系ΣｆのＸ軸、Ｙ軸、Ｚ軸方向にロボット１を移動すべき量を示すものとする。

続いて、ヤコビアン行列Ｊを求める方法を説明する。

まず、対象物４が画像中心付近に写る状態をロボットの原位置として、ロボット１が原位置に位置決めされている状態で、カメラ３で撮像した画像から対象物４を検出し、得られた特徴量を（ｕ₀，ｖ₀，ｓ₀）とする。次に、ロボット１を原位置からメカニカルインターフェイス座標系ΣｆのＸ軸方向に距離ｍだけ移動し、その位置においてカメラ３で撮像した画像から対象物４を検出し、得られた特徴量を（ｕ_X，ｖ_X，ｓ_X）とする。同様に、ロボットを原位置からメカニカルインターフェイス座標系ΣｆのＹ軸方向に距離ｍだけ移動した位置、Ｚ軸方向に距離ｍだけ移動した位置で、それぞれ対象物４を検出し、それぞれの特徴量を（ｕ_Y，ｖ_Y，ｓ_Y）、（ｕ_Z，ｖ_Z，ｓ_Z）とする。

Ｘ軸、Ｙ軸、Ｚ軸方向にロボットを移動したときの対象物の検出結果と原位置での対象物の検出結果から、Δｕ、Δｖ、Δｓをそれぞれ次のように求める。

これを式（１）に代入すると、次のようにヤコビアン行列Ｊを求めることができる。

一旦ヤコビアン行列が求まれば、任意の対象物４の特徴量（ｕ，ｖ，ｓ）に対して、それらを式（１）に代入することで、ロボット１を目標到達状態に一致させるためのロボット移動量（Δｘ，Δｙ，Δｚ）を求めることができるようになる。

以上により、テーブル５上の任意の位置に載置された対象物４に対して、ロボットプログラムを補正しながら、ロボット１に作業を行わせることができるようになる。しかしながら、求められるロボットの移動量（Δｘ，Δｙ，Δｚ）は、カメラ３とテーブル５の距離が変わるような動きをさせてしまう可能性があり、最終的には目標到達状態に到達できるものの、効率が悪い。

そこで、続いて、本発明が開示している、対象物４が載置されているテーブル５の面の法線を算出し、さらに、ロボット１がテーブル５との距離を変えないように、テーブル５の面に平行に移動し、かつ、テーブル５の面の法線に平行な軸回りに回転するようなロボット移動量を算出する方法について説明する。

まず、前述したヤコビアン行列は既に求められており、Ｓ２０１〜Ｓ２０７の手順に従って、テーブル５上の任意の位置に置かれた対象物４に対して、ロボット１に作業をさせられる状態になっているものとする。

一般的に、３次元の任意の平面を同定するには、平面上の同一でない３点が既知であればよいことが知られている。

前述したＳ２０６において記憶した第２のロボット位置について、その位置成分（ｘ，ｙ，ｚ）について考えると、（ｘ，ｙ，ｚ）は、ロボット１が第２のロボット位置に位置決めされている時の、ロボット座標系Σｂから見たメカニカルインターフェイス座標系Σｆの原点の位置である。

第２のロボット位置の位置成分（ｘ，ｙ，ｚ）は、テーブル５の平面上の点ではないが、複数の第２のロボット位置の位置成分（ｘ_i，ｙ_i，ｚ_i）が既知である場合、それらは概略一平面上に乗っているはずで、さらに、複数の（ｘ_i，ｙ_i，ｚ_i）が作る平面とテーブル５の平面は平行になるはずなので、（ｘ_i，ｙ_i，ｚ_i）が作る平面の法線を算出することで、テーブル５の平面の法線を算出することができる。

任意の３次元平面は一般的に次式で表せる。

ここで、ベクトルｎ＝（ａ，ｂ，ｃ）は平面の法線ベクトルであり、スカラ値ｄは座標系原点から平面までの垂線の足の長さを表す。以上のことから、複数の第２のロボット位置の一成分（ｘ_i，ｙ_i，ｚ_i）がある場合、次の関係が成り立つ（ａ，ｂ，ｃ）を求めればよい。

ここから、最小二乗法で法線ベクトルｎを求める。

とすると、

このベクトル（ａ／ｄ，ｂ／ｄ，ｃ／ｄ）を正規化することで、複数の第２のロボット位置が作る平面の法線ベクトル、すなわち、テーブル５の平面の法線ベクトルｎが算出される。

複数の第２のロボット位置の１つを、第１のロボット位置で代用してもよい。

本実施例では、ロボット１に所定の作業をさせる過程で法線ベクトルｎを算出するという形態を取ったため、Ｓ２０７も実施するようにしているが、ロボット１に作業をさせずに先に法線ベクトルｎだけを算出してしまいたい場合は、法線ベクトルｎが算出されるまではＳ２０７は実行しないようにしてもよい。

最後に、算出した法線ベクトルｎに基づいて、Ｓ２０３で算出されるロボットの移動量が、テーブル５の平面に平行な並進移動のみを含むようにする方法について説明する。

ここで、新たな座標系として、ツール座標系Σｔを導入する。ツール座標系Σｔは、メカニカルインターフェイス座標系Σｆに対して相対的に固定されて定義された座標系で、ロボット１を動かした際は、メカニカルインターフェイス座標系Σｆとの相対的な位置関係を維持したまま移動する。

図７に示すように、ツール座標系Σｔは一例として、原点がメカニカルインターフェイス座標系Σｆと同じ位置にあり、かつ、ロボット１が第１のロボット位置に位置決めされている時にＺ軸がテーブル５の面の法線ベクトルｎと平行になる座標系、と定義する。ツール座標系ΣｔのＸ軸の向きとＹ軸の向きは、Ｚ軸と直交し、かつ、互いに直交するという条件さえ満たせば、どちらを向いていても構わない。

ツール座標系Σｔを定義することは、メカニカルインターフェイス座標系Σｆから見たツール座標系Σｔの位置姿勢Ｔを求めることに等しい。Ｔは４×４の同次変換行列である。

まず、ロボット１が第１のロボット位置に位置決めされているものとして、ロボット座標系Σｂから見たツール座標系Σｔの位置および姿勢Ｔｂを以下の手順に沿って求める。

ツール座標系Σｔの、ロボット座標系Σｂから見たＸ軸、Ｙ軸、Ｚ軸を表す方向ベクトルをそれぞれＶ_x、Ｖ_y、Ｖ_zとすると、Ｚ軸の方向は法線ベクトルｎであるので、

である。Ｘ軸とＹ軸の方向はどちら向きでもよいので、たとえば、Ｘ軸は１つ目の第２のロボット位置から２つ目の第２のロボット位置に向かう方向とし、Ｙ軸はＸ軸とＺ軸に垂直な方向とすることにする。すなわち、

とする。ただし、

である。

ツール座標系Σｔの原点は、ロボット１が第１のロボット位置Ｒ₁に位置決めされているときには、ロボット座標系Σｂから見たΣｆの原点の座標値と同じになるので、それを（ｘ₀，ｙ₀，ｚ₀）とすると、ロボット座標系Σｂから見たツール座標系Σｔの位置と姿勢Ｔｂは、

となる。さらに、ロボット１は第１のロボット位置Ｒ₁に位置決めされているので、

が成り立つから、メカニカルインターフェイス座標系Σｆから見たツール座標系Σｔの位置と姿勢Ｔは次式で算出できる。

メカニカルインターフェイス座標系Σｆから見たツール座標系Σｔの位置姿勢Ｔが求まったことで、メカニカルインターフェイス座標系Σｆで表現されたロボットの移動量
（Δｘ，Δｙ，Δｚ）は、次式でツール座標系Σｔから見たロボットの移動量（Δｘ’，Δｙ’，Δｚ’）に変換できるようになった。

上式では、Ｔが４×４の同次変換行列のため、ロボットの移動量ベクトルも４次元に拡張してあるが、Ｔの回転成分を３×３の回転行列Ｍとして、次式のように表すこともできる。

このようにして求めた、ツール座標系Σｔで表されたロボットの移動量（Δｘ’，
Δｙ’，Δｚ’）は、ツール座標系ΣｔのＺ軸がテーブル５の面と垂直であること、対象物４はテーブル５の上に載置されていることから、Δｚ’は理想的には０になるべきだが、実際には検出誤差や計算機誤差などの様々な要因により完全には０にはならない。そのため、算出されたロボットの移動量通りにロボットを動かすと、微妙にテーブル５に近づいたり遠ざかったりするわけだが、Δｚ’を０に置き換えてしまい、ロボットの移動量を（Δｘ’，Δｙ’，０）とすることで、完全にテーブル５との距離を保ったままロボットを移動させることができる。

ところで、前述の式（１）によると、

であるので、

であることが分かる。したがって、

を計算しておいて、Ｓ２０３ではヤコビアン行列Ｊの代わりにＪ’を用いるようにすることで、（Δｕ，Δｖ，Δｓ）から直接ツール座標系Σｔで表現されたロボット移動量
（Δｘ’，Δｙ’，Δｚ’）を算出するようにしてもよい。

画像上での見た目の回転移動量Δθについては、ツール座標系ΣｔのＺ軸がテーブル５の面に垂直になっているので、ロボット１をツール座標系のＺ軸回りにΔθだけ回転させるようにすればよい。

ただし、この際、図８に示すように、ツール座標系Σｔとカメラ３の位置関係によっては、ツール座標系ΣｔのＺ軸回りにロボット１を回転移動することで、対象物４がカメラ３の視野の外に出てしまう可能性がある。

これについては、図９に示すように、ツール座標系Σｔの原点を並進移動し、対象物４が第１の対象物位置に置かれ、かつ、ロボット１が第１のロボット位置に位置決めされた状態において、対象物４の中心位置を通り法線ベクトルｎに平行な軸上に原点がくるような第２のツール座標系Σｔ’を算出し、第２のツール座標系Σｔ’のＺ軸回りにΔθだけ回転させるようにすることで、対象物４がカメラ３の視野の外に出てしまう問題を解消することができる。以下、第２のツール座標系Σｔ’の算出方法を説明する。

ツール座標系Σｔと第２のツール座標系Σｔ’は平行で、原点位置だけが異なるので、ツール座標系Σｔの原点から第２のツール座標系Σｔ’の原点へ向かうベクトルを、ツール座標系Σｔを基準にして表現したものをＰで表す。

対象物４をテーブル５上の適当な位置、たとえば、第１の対象物位置に載置し、ロボット１を適当な、たとえば第１のロボット位置に位置決めした状態で、画像処理を行って、カメラ３の画像から対象物４を検出する。この時の、ロボット座標系Σｂから見たツール座標系Σｔの位置と姿勢をＴｂ、ロボット座標系Σｂから見たツール座標系Σｔ原点の座標値を（ｘ₁，ｙ₁，ｚ₁）、検出した対象物４のビジョン座標系での座標値を（ｕ₁，ｖ₁）とする。

次に、対象物４は同じ位置に置いたままで、ロボット１をツール座標系ΣｔのＺ軸回りに微小角度φだけ回転移動し、前述した方法を用いて、カメラ３の画像に写る対象物４の特徴量が先ほどと同じ（ｕ₁，ｖ₁）になるようにビジュアルフィードバックを掛ける。この時の収束したロボット位置で、ロボット座標系Σｂから見たツール座標系Σｔ原点の座標値を（ｘ₂，ｙ₂，ｚ₂）とする。

図１０は、この様子を真上から見たものである。角度が異なる２つのロボット位置において、カメラ３の画像上では同じ（ｕ₁，ｖ₁）に対象物４が写っているので、次の式が成り立つ。

ただし、

である。式（２）を展開すると、

となり、Ｐについて解くと次の解が得られる。

ツール座標系Σｔから第２のツール座標系Σｔ’への変換は座標系の回転を伴わないため、第２のツール座標系Σｔ’の原点はツール座標系Σｔの原点をベクトルＰだけ移動することで求められる。

以上により、求めた法線に垂直な面内の平行移動、及び、求めた法線に平行な軸回りの回転移動のみに限定することができるようになった。これにより、収束に必要な移動の方向と回転軸が限定でき、収束時間を短縮することができるようになる。具体的には、対象物を画像上で目標位置に移動する際に高さ方向の収束動作がなくなり、対象物の画像上のサイズが変わらなくなるため、収束判断においてもサイズの差異計算および収束判定を行わずにすみ、処理時間、ひいては収束時間が短縮される。

１ ロボット
２ ロボット制御装置
３ カメラ
４ 対象物
５ テーブル
１２ａ ロボット位置記憶部
１２ｂ 目標到達状態データ記憶部
１５ サーボ制御部

Claims (6)

1. ロボットのアーム先端部に装着された装置を用いて、平面上の第１の対象物位置に置かれた対象物に対して、所定の作業を行う動作指令を含むプログラムによって制御されるロボットと、
   前記アーム先端部に装着され、対象物を撮像するカメラと、
   前記第１の対象物位置に対して、所定の相対的位置関係にある前記アーム先端部の位置を第１のロボット位置として記憶する第１のロボット位置記憶部と、
   対象物が前記第１の対象物位置に置かれ、前記アーム先端部が前記第１のロボット位置に位置決めされた状態である目標到達状態において、前記カメラの画像上での該対象物の位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を目標到達状態データとして記憶する目標到達状態データ記憶部と、
   前記平面上の第２の対象物位置に置かれた対象物を、前記ロボットのアーム先端部が任意の初期位置に位置決めされた状態において、前記カメラで撮像した際における、該対象物の前記カメラの画像上での位置、姿勢、及び大きさのうちの少なくとも１つの特徴量を前記目標到達状態データのうちの対応する特徴量と一致させるための、前記アーム先端部の前記任意の初期位置からの移動量を求めるロボット移動量計算部と、
   前記アーム先端部を前記移動量に基づいて移動させた際における、前記アーム先端部の位置である第２のロボット位置と、前記第１のロボット位置との差異に基づいて前記プログラムに教示されているロボット位置を補正するための補正データを計算する補正データ計算部と、
   を備えることを特徴とするロボットシステム。
2. 前記ロボットの前記アーム先端部が前記第２のロボット位置に位置決めされた状態において、前記カメラが撮像した、前記第２の対象物位置に置かれた対象物の画像上での位置、姿勢、及び大きさのうちの少なくとも１つの特徴量と、前記目標到達状態データ記憶部に記憶された目標到達状態データのうち対応する特徴量との差異が所定の値よりも大きい場合に、前記ロボット移動量計算部は、前記第２のロボット位置をあらためて初期位置として前記移動量を求め、前記アーム先端部を該移動量に基づいて移動させた際における、前記アーム先端部の位置をあらためて第２のロボット位置とする、請求項１に記載のロボットシステム。
3. 前記ロボットの前記アーム先端部が前記第２のロボット位置に位置決めされた状態において、前記ロボット移動量計算部は、前記第２のロボット位置をあらためて初期位置として前記移動量を求め、該移動量が所定の値よりも大きい場合に、前記アーム先端部を該移動量に基づいて移動させた際における、前記アーム先端部の位置をあらためて第２のロボット位置とする、請求項１に記載のロボットシステム。
4. 直線上に位置していない互いに異なる複数の第２の対象物位置を少なくとも３つ設け、該複数の第２の対象物位置に前記対象物を置き、それぞれの対象物位置に対応した複数の第２のロボット位置を取得し記憶する第２のロボット位置記憶部と、
   前記第２のロボット位置記憶部に記憶された複数の第２のロボット位置に基づいて、前記対象物が置かれている平面の法線ベクトルを算出する法線ベクトル計算部を更に備え、
   前記ロボット移動量計算部は、前記法線ベクトルに垂直な平面内の並進移動、及び該法線ベクトルに平行な軸回りの回転移動のみを移動量として算出する、請求項１乃至３のいずれか一項に記載のロボットシステム。
5. 前記互いに異なる複数の第２の対象物位置の１つが第１の対象物位置と同一であり、前記複数の第２のロボット位置の１つが第１のロボット位置である、請求項４に記載のロボットシステム。
6. 前記ロボット移動量計算部による回転移動は、第１の対象物位置に置かれた対象物の中心位置を通り前記法線ベクトルに平行な直線を、前記ロボットが第１のロボット位置に位置決めされた状態で、ロボットのアーム先端部に対する相対的な直線として定義し、該定義されたアーム先端部に対する相対的な直線を回転軸として回転移動させるものであり、
   前記対象物の中心位置は、画像処理アルゴリズムとしてテンプレートマッチングを行い、テンプレートが最も一致した際の、テンプレートの中心位置として求められる、請求項４または５に記載のロボットシステム。

Images