(57)【特許請求の範囲】
【請求項１】
ロボット制御装置により制御されて動作するロボットであって、
前記ロボット制御装置は、
ロボットの可動可能なアームに設けられ、撮像した画像データを出力する撮像装置と、
第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、
前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定部と、
前記アームを制御するアーム制御部と、
を備え、
前記画像推定部は、
前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、
前記アーム制御部は、
前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
ことを特徴とするロボット。
【請求項２】
前記画像取得部は、
前記第１の時刻に前記撮像装置が出力する第１の画像データを取得し、前記第１の時刻より後の第２の時刻に前記撮像装置が出力する第２の画像データを取得し、前記第２の時刻より後の第３の時刻に前記撮像装置が出力する第３の画像データを取得し、前記第３の時刻より後の第４の時刻に前記撮像装置が出力する第４の画像データを取得し、
前記画像推定部は、
前記画像取得部が取得した第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを用いて、前記第３の時刻に撮像される画像データと前記第４の時刻に撮像される画像データとを推定する
ことを特徴とする請求項１に記載のロボット。
【請求項３】
前記アームの位置を検出し、前記検出されたアームの手先位置と姿勢を示す情報を前記画像推定部に出力するエンコーダー
を備え、
前記画像推定部は、
前記第１の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、
前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第３の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第２の画像データと、前記第３の画像データとに基づき前記第４の時刻に撮像される画像データを推定し、
前記算出した画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正する
ことを特徴とする請求項１または請求項２に記載のロボット。
【請求項４】
前記画像推定部は、
前記第１の時刻から前記第３の時刻までの前記アームの位置と姿勢に基づく第１の回転を表す行列及び第１の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第１の基礎行列を算出し、前記第２の時刻から前記第３の時刻における前記アームの位置と姿勢に基づく第２の回転を表す行列及び第２の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第２の基礎行列とを算出し、
前記算出した前記第１の基礎行列、前記第２の基礎行列、前記第１の画像データ及び前記第２の画像データを用いて前記第３の時刻の画像データを推定し、前記第３の時刻において、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データとの画像データ間の誤差を算出する
ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット。
【請求項５】
前記画像推定部は、
前記誤差が時間や前記アームの手先位置と姿勢に依存しない場合、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データとの間の誤差を、前記第４の時刻における画像データに含まれる誤差として算出し、前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正する
ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット。
【請求項６】
前記画像推定部は、
前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記第４の時刻の画像データを推定し、
前記アーム制御部は、
前記画像推定部が推定した前記第４の時刻の画像データを用いて、前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記アームを制御する
ことを特徴とする請求項１から請求項５のいずれか１項に記載のロボット。
【請求項７】
前記画像推定部は、
前記エンコーダーが前記アームの位置を示す情報を前記画像推定部に出力する周期で前記第４の時刻の画像データを推定し、
前記アーム制御部は、
前記画像推定部が、前記第４の時刻の画像データを用いて、前記エンコーダーが前記アームの位置を示す情報を前記画像推定部に出力する周期で前記アームを制御する
ことを特徴とする請求項３に記載のロボット。
【請求項８】
前記画像推定部は、
前記第１の画像データが撮像された時刻を、前記第１の画像データが取得された第１の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出し、前記第２の画像データが撮像された時刻を、該第２の画像データが取得された第２の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出する
ことを特徴とする請求項１から請求項７のいずれか１項に記載のロボット。
【請求項９】
可動可能なアームに設けられ、撮像した画像データを出力する撮像装置を有するロボットのアームを制御するロボット制御装置であり、
第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、
前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻に撮像される画像データに含まれる誤差を算出する画像推定部と、
前記アームを制御するアーム制御部と、
を備え、
前記画像推定部は、
前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻の画像データを補正し、
前記アーム制御部は、
前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
ことを特徴とするロボット制御装置。
【請求項１０】
ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御するロボット制御方法であり、
撮像装置が、撮像して画像データを出力する撮像工程と、
画像取得部が、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得工程と、
画像推定部が、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定工程と、
アーム制御部が、前記アームを制御するアーム制御工程と、
を含み、
前記画像推定工程は、
前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、
前記アーム制御工程は、
前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
ことを特徴とするロボット制御方法。
【請求項１１】
ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御する処理をコンピューターに実行させるためのプログラムであり、
前記撮像装置が撮像して画像データを出力する撮像手順と、
第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得手順と、
前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定する第１画像推定手順と、
前記第１画像推定手順により推定された前記第３の時刻の画像データと、前記画像取得手順により取得された前記第３の時刻の画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する誤差算出手順と、
前記第２の画像データと、前記第３の画像データとに基づき前記第４の時刻に撮像される画像データを推定し、前記誤差算出手順が算出した前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正する第２画像推定手順と、
前記第２画像推定手順により補正された前記第４の時刻の画像データを用いて前記アームを制御するアーム制御手順と、
をコンピューターに実行させるためのロボット制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット、ロボット制御装置、ロボット制御方法およびロボット制御プログラムに関する。
【背景技術】
【０００２】
近年、製造現場等では、スカラロボットや多軸ロボット等が製品の組み立てや検査等に用いられている。製品の組み立てや検査を行なう際のロボットの制御方法としてビジュアルサーボがある。ビジュアルサーボは、目標物との相対的な位置の変化を視覚情報として計測し、それをフィードバック情報として用いることによって目標物を追跡するサーボ系の一種である。
【０００３】
画像取得には、露光、アナログ−デジタル変換、カメラからパソコンのインターフェースへの画像データ転送などが含まれる。したがって、通常は露光開始の瞬間からＣＰＵが画像を読み出し可能となるまでには、最低でも１フレーム分の遅れが発生する。さらにＣＰＵでは、画像処理に時間がかかるため、画像処理が終了するまで最低でももう１フレームの遅れが発生する。
このため、ビジュアルサーボでは、画像を露光した時の情報を使って、ロボットに次に移動する指令値を生成するが、ロボットに指令値を送る時は露光した時間から最低でも２フレーム分以上の時間が経過してしまう。
【０００４】
これに対して、ロボットのアームの手先位置・姿勢を出力するエンコーダーの情報は、高速取り込みが可能である。このため、非特許文献１では、このエンコーダーの情報と過去の画像情報を用いて現在の予測画像を生成することで、画像転送にかかる時間（遅れ時間）を待たずに、ロボットの指令値を生成している。
【０００５】
また、特許文献１では、画像転送処理時間のタイムラグを誤差として予測し補正するために、メモリーにセンサー情報を記憶させて、記憶させたセンサー情報を用いて、取得時刻が同じになるようにそれぞれのセンサー情報を近似補正している。近似補正は、メモリーに記憶されている算出したい時刻の前後の位置情報を用いて算出している。また、特許文献１では、センサー情報の取り込み間隔が微小な場合、算出したい時刻に最も近い時刻に取得された位置情報を用いて近似している。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平９−３００２６４号公報
【非特許文献】
【０００７】
【非特許文献１】Abdul Hafez, Piyush Janawadkar and C.V. Jawahar, "Novel view prediction for improved visual servoing", NCC 2006, New Delhi, India
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、非特許文献１に記載の発明では、実際は、エンコーダー情報から計算されるロボット手先位置・姿勢と実際のロボットの手先位置・姿勢には誤差がある。この誤差は、例えば、重力の影響やモーターの回転誤差により発生する。このため、現在の画像を精度良く予測できないため、この画像を用いた場合、精度の良いビジュアルサーボが行えない。
【０００９】
また、特許文献１に記載の発明では、算出したい時刻の位置情報を、その時刻の前後の位置情報を用いて算出しているため、現在時刻における位置を算出できない。また、センサー情報の取り込み間隔が微小な場合、算出したい時刻に最も近い時刻に取得された位置情報を用いて近似するためには、画像情報も位置情報と同じ間隔で取得する必要があり、高価なカメラが必要である。
この場合においても、エンコーダー情報から計算されるロボット手先位置・姿勢と実際のロボットの手先位置・姿勢には、重力の影響等によるロボットキャリブレーション誤差が残る。このため、現在の画像を精度良く予測できないため、この画像を用いた場合、精度の良いビジュアルサーボが行えない。
【００１０】
また、ビジュアルサーボを行うために、予めキャリブレーションを行っても、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等が残り、これらが誤差要因となる。この結果、精度の良いビジュアルサーボが行えない。
【００１１】
本発明は、上記の課題に鑑みてなされたものであって、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボット、ロボット制御装置、ロボット制御方法、およびロボット制御プログラムを提供することを目的としている。
【課題を解決するための手段】
【００１２】
上記目的を達成するため、本発明に係るロボットは、ロボット制御装置により制御されて動作するロボットであって、前記ロボット制御装置は、ロボットの可動可能なアームに設けられ、撮像した画像データを出力する撮像装置と、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定部と、前記アームを制御するアーム制御部と、を備え、前記画像推定部は、前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、前記アーム制御部は、前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御することを特徴とする。
【００１３】
本発明によれば、画像取得部は、第４時刻より前の異なる３つの時刻である第１〜第３の時刻と第４の時刻に、それぞれ撮像装置が出力する画像データを取得する。第４の時刻は、例えば現在時刻である。そして、画像推定部は、第４の時刻より前の２つの異なる時刻の各画像データに基づいて、第３の時刻の画像データを推定する。そして、画像推定部は、推定した第３の時刻の画像データと、第３の時刻に取得された画像データ間の誤差を算出し、算出した画像データ間の誤差を用いて、第４の時刻の画像データを推定する。そして、アーム制御部は、現在時刻である第４の時刻の推定された画像を用いてアームを制御する。
この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現在時刻である第４の時刻に撮像された画像データが取得されていない状態であっても、現在時刻である第４の時刻の画像データの誤差を考慮して推定することができるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボットを提供できる。
【００１４】
また、本発明のロボットにおいて、前記画像取得部は、前記第１の時刻に前記撮像装置が出力する第１の画像データを取得し、前記第１の時刻より後の第２の時刻に前記撮像装置が出力する第２の画像データを取得し、前記第２の時刻より後の第３の時刻に前記撮像装置が出力する第３の画像データを取得し、前記第３の時刻より後の第４の時刻に前記撮像装置が出力する第４の画像データを取得し、前記画像推定部は、前記画像取得部が取得した第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを用いて、前記第３の時刻に撮像される画像データと前記第４の時刻に撮像される画像データとを推定するようにしてもよい。
【００１５】
また、本発明のロボットにおいて、前記アームの位置を検出し、前記検出されたアームの手先位置と姿勢を示す情報を前記画像推定部に出力するエンコーダーを備え、前記画像推定部は、前記第１の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第３の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記算出した画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正するようにしてもよい。
【００１６】
本発明によれば、画像推定部は、アームが備えるエンコーダーが出力するアームの手先位置と姿勢を示す情報と、撮像装置の焦点距離を含む内部パラメーターを表す行列と、第１の画像データと、第２の画像データとを用いて第３の時刻の画像データを推定する。そして、画像推定部は、推定した第３の時刻の画像データと、第３の時刻に取得された画像データ間の誤差を算出し、算出した画像データ間の誤差を算出する。さらに、画像推定部は、エンコーダーが出力するアームの手先位置と姿勢を示す情報と、撮像装置の焦点距離を含む内部パラメーターを表す行列と、第２の画像データと、第３の画像データと、算出した誤差に基づき第４の時刻の画像データを推定する。そして、アーム制御部は、現在時刻である第４の時刻の推定された画像を用いてアームを制御する。
この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現在時刻である第４の時刻に撮像された画像データが取得されていない状態であっても、現在時刻である第４の時刻の画像データの誤差を考慮して推定することができるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボットを提供できる。
【００１７】
また、本発明のロボットにおいて、前記画像推定部は、前記第１の時刻から前記第３の時刻までの前記アームの位置と姿勢に基づく第１の回転を表す行列及び第１の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第１の基礎行列を算出し、前記第２の時刻から前記第３の時刻における前記アームの位置と姿勢に基づく第２の回転を表す行列及び第２の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第２の基礎行列とを算出し、前記算出した前記第１の基礎行列、前記第２の基礎行列、前記第１の画像データ及び前記第２の画像データを用いて前記第３の時刻の画像データを推定し、前記第３の時刻において、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データとの画像データ間の誤差を算出するようにしてもよい。
【００１８】
本発明によれば、画像推定部は、第１の時刻から第３の時刻及び第２の時刻から第３の時刻におけるアームが備えるエンコーダーが出力するアームの手先位置と姿勢に基づく各回転を表す行列及び各並進を表す行列と、撮像装置の焦点距離を含む内部パラメーターを表す行列とを用いて各基礎行列を算出する。また、各基礎行列と、第１の画像データと、第２の画像データとに基づき第３の時刻の画像データを推定する。そして、画像推定部は、推定した第３の時刻の画像データと、第３の時刻に取得された画像データ間の誤差を算出し、算出した画像データ間の誤差を算出する。
この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現在時刻である第４の時刻に撮像された画像データが取得されていない状態であっても、第４の時刻の画像データを補正するための第３の時刻の画像データの誤差を算出することができるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボットを提供できる。
【００１９】
また、本発明のロボットにおいて、前記画像推定部は、前記誤差が時間や前記アームの手先位置と姿勢に依存しない場合、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データ間との誤差を、前記第４の時刻における画像データに含まれる誤差として算出し、前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正するようにしてもよい。
【００２０】
本発明によれば、画像データ間の誤差が時間や前記アームの手先位置と姿勢に依存しない場合、画像推定部は、第３の時刻の推定した画像データと取得された画像データ間の誤差を現在時刻である第４の時刻の画像データの誤差として算出する。そして、画像推定部は、この誤差を用いて、現在時刻である第４の時刻の推定した画像データを補正する。
この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現在時刻である第４の時刻に撮像された画像データが取得されていない状態であっても、第３の時刻の画像データの誤差を第４の時刻の画像データの補正値に用いることができるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボットを提供できる。
【００２１】
また、本発明のロボットにおいて、前記画像推定部は、前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記第４の時刻の画像データを推定し、前記アーム制御部は、前記画像推定部が推定した前記第４の時刻の画像データを用いて、前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記アームを制御するようにしてもよい。
【００２２】
また、本発明のロボットにおいて、前記画像推定部は、前記エンコーダーが前記アームの位置を示す情報を前記画像推定部に出力する周期で前記第４の時刻の画像データを推定し、前記アーム制御部は、前記画像推定部が、前記第４の時刻の画像データを用いて、前記エンコーダーが前記アームの位置を示す情報を前記画像推定部に出力する周期で前記アームを制御するようにしてもよい。
【００２３】
本発明によれば、画像推定部が、撮像装置のフレームレートより速い周期で、第４の時刻の画像を推定する。撮像装置のフレームレートより速い周期は、例えば、アームが備えるエンコーダーが検出したアームの実測位置を出力する周期である１［ｍｓｅｃ］である。このため、撮像装置のフレームレートが、例えば３３［ｍｓｅｃ］であっても、画像指定部は１［ｍｓｅｃ］毎に撮像された画像を推定する。そして、アーム制御部は、この推定された画像を用いてアームを制御するので、低速な撮像装置を用いても、速いフレームレートの撮像装置を使った場合と同様に、アームを高速で制御することができる。
【００２４】
上記目的を達成するため、本発明のロボットにおいて、前記画像推定部は、前記第１の画像データが撮像された時刻を、前記第１の画像データが取得された第１の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出し、前記第２の画像データが撮像された時刻を、該第２の画像データが取得された第２の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出するようにしてもよい。
【００２５】
本発明によれば、誤差算出部は、取得済みの画像データが撮像された各時刻を、画像が取得された時刻から撮像装置が撮像してから画像データを出力するまでの遅延時間を減算して算出する。この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現時刻である第３の時刻の画像が取得されていない状態であっても、取得済みの各画像データが撮像された各時刻を算出できるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボットを提供できる。
【００２６】
上記目的を達成するため、本発明のロボット制御装置は、可動可能なアームに設けられ、撮像した画像データを出力する撮像装置を有するロボットのアームを制御するロボット制御装置であり、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻に撮像される画像データに含まれる誤差を算出する画像推定部と、前記アームを制御するアーム制御部と、を備え、前記画像推定部は、前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻の画像データを補正し、前記アーム制御部は、前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御することを特徴とする。
【００２７】
上記目的を達成するため、本発明のロボット制御方法は、ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御するロボット制御方法であり、撮像装置が、撮像して画像データを出力する撮像工程と、画像取得部が、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得工程と、画像推定部が、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定工程と、アーム制御部が、前記アームを制御するアーム制御工程と、を含み、前記画像推定工程は、前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、前記アーム制御工程は、前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御することを特徴とする。
【００２８】
上記目的を達成するため、本発明のロボット制御プログラムは、ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御する処理をコンピューターに実行させるためのプログラムであり、前記撮像装置が撮像して画像データを出力する撮像手順と、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の時刻との３つの時刻と前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得手順と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定する第１画像推定手順と、前記第１画像推定手順により推定された前記第３の時刻の画像データと、前記画像取得手順により取得された前記第３の時刻の画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する誤差算出手順と、前記第２の画像データと、前記第３の画像データとに基づき前記第４の時刻に撮像される画像データを推定し、前記誤差算出手順が算出した前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正する第２画像推定手順と、前記第２画像推定手順により補正された前記第４の時刻の画像データを用いて前記アームを制御するアーム制御手順と、をコンピューターに実行させる。
【００２９】
本発明によれば、画像取得部は、第４時刻より前の異なる３つの時刻である第１〜第３の時刻と第４の時刻に、それぞれ撮像装置が出力する画像データを取得する。第４の時刻は、例えば現在時刻である。そして、画像推定部は、第４の時刻より前の時刻の第１の時刻の画像データと第２の時刻の各画像データに基づいて、第３の時刻の画像データを推定する。そして、画像推定部は、推定した第３の時刻の画像データと、第３の時刻に取得された画像データ間の誤差を算出し、算出した画像データ間の誤差を用いて、第４の時刻の画像データを推定する。そして、アーム制御部は、現在時刻である第４の時刻の推定された画像を用いてアームを制御する。
この結果、撮像装置が撮像してから画像取得部に出力するまでに遅れ時間があり、現在時刻である第４の時刻に撮像された画像データが取得されていない状態であっても、現在時刻である第４の時刻の画像データの誤差を考慮して推定することができるので、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボット制御装置、ロボット制御方法、及びロボット制御プログラムを提供できる。
【図面の簡単な説明】
【００３０】
【図１】第１実施形態に係るロボット１の概略構成を示す斜視図である。
【図２】同実施形態に係るアーム部２０とエンコーダー９０の概念図である。
【図３】同実施形態に係る制御部１００のブロック図である。
【図４】過去の２つの時刻における画像とカメラ位置、現在の時刻における画像とカメラ位置を説明する図である。
【図５】同実施形態に係る撮像装置６０が撮像するタイミングとエンコーダー情報が取得されるタイミングを説明する図である。
【図６】同実施形態に係るアーム２の手先位置・姿勢を説明する図である。
【図７】同実施形態に係る各時刻におけるパラメーター、アーム２の相対移動量、画像の関係を説明する図である。
【図８】同実施形態に係る現在時刻ｄ０の画像Ｉ０を推定する手順のフローチャートである。
【図９】第２実施形態に係る現在時刻ｄ０の画像Ｉ０を推定する手順のフローチャートである。
【図１０】第３実施形態に係るロボット１ａの概略構成を示す斜視図である。
【図１１】同実施形態に係るアーム部２０−１及び２０−２と力センサー、エンコーダーの概念図である。
【発明を実施するための形態】
【００３１】
［第１実施形態］
図１は、本実施形態に係るロボット１の概略構成を示す斜視図である。図１に示すように、ロボット１は、１本のアーム２を備えている。アーム２は、固定部１０、アーム部２０、及びエンドエフェクター４０を備える。また、ロボット１は、制御部１００の制御により動作する。また、対象物７０は、ステージ７２の上面に配置されている。
【００３２】
固定部１０は、例えば床、壁、天井、移動可能な台車の上などに固定される。固定部１０は、内部に、制御部１００（破線にて図示）を備える。
制御部１００は、アーム部２０、エンドエフェクター４０を制御する。制御部１００の詳細は後述する。なお、制御部１００は、固定部１０の内部ではなく、固定部１０の外部に設けるようにしてもよい。
【００３３】
撮像装置６０は、エンドエフェクター４０の先端部に取り付けられている。撮像装置６０は、例えばＣＣＤ（Ｃｈａｒｇｅ  Ｃｏｕｐｌｅｄ  Ｄｅｖｉｃｅ）カメラである。撮像装置６０は、対象物を撮影し、撮影したアナログ値の画像データを制御装置１００に出力する。すなわち、本実施形態では、ロボット１の手先位置に、撮像装置６０が固定設置されているハンドアイ・ロボットである。また、撮像される対象物７０は固定されている。
なお、このようなハンドアイではなく、撮像装置６０をステージ７２に固定し、ロボット１のエンドエフェクター４０に非図示のハンドを取り付け、このハンドが対象物７２を把持して移動させるようにしてもよい。
【００３４】
制御部１００は、ロボット１のアーム２を後述するように制御する。また、制御部１００は、撮像装置６０が出力した画像データとアーム２の手先位置・姿勢情報とに基づき、後述するように、現在時刻の画像情報を推定する。また、制御部１００は、推定した現在時刻の画像情報を、画像表示装置６５に出力する。
【００３５】
図２は、本実施形態に係るアーム部２０とエンコーダー９０の概念図である。
図２に示すように、アーム２のアーム部２０は、第１リンク２１、第２リンク２２、第３リンク２３、第４リンク２４および第５リンク２５がこの順に連結されたものである。また、アーム部２０は、回転部８２、８３及び８５と、屈折部８１、８４及び８６とを有している。
【００３６】
屈折部８１は、固定部１０と第１リンク２１との間に設けられている。回転部８２は、第１リンク２１と第２リンク２２との間に設けられている。回転部８３は、第２リンク２２と第３リンク２３との間に設けられている。屈折部８４は、第３リンク２３と第４リンク２４との間に設けられている。回転部８５は、第４リンク２４と第５リンク２５との間に設けられている。屈折部８４は、第５リンク２５のうち第４リンク２４が設けられた他方に設けられている。
【００３７】
図２に示すように、アーム部２０は、６自由度を有する６軸アームである。すなわち、アーム部２０の先端のエンドエフェクター４０は、これらの各軸の回転部と屈折部の動作により、移動する。アーム部２０が６軸アームであるため、制御部１００は、互いに独立な６つの変数で制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）の３つの変数（成分）、姿勢を表す角度（α、β、γ）の３つの成分（変数）に対応して制御することができる。
【００３８】
また、図２に示すように、回転部８２、８３及び８５と、屈折部８１、８４及び８６は、各々エンコーダー９１〜９６を備えている。例えば、回転部８２が備えるエンコーダー９２は、回転角度、速度を検出し、検出した回転角度および速度を示す情報を制御部１００に出力する。以下、エンコーダー９１〜９６を、エンコーダー９０という。エンコーダー９０は、例えば光学式エンコーダーであり、アブソリュート（絶対値）エンコーダーまたはインクリメンタル（相対値）エンコーダーである。
【００３９】
図３は、本実施形態に係る制御部１００のブロック図である。
図３に示すように、制御部１００は、アーム制御部１０１、駆動部１０２、画像取得部１０３、クロック発生部１０４、画像処理部１０５、記憶部１０６、位置情報取得部１０７、位置姿勢算出部１０８、および、画像推定部１０９を備えている。また、制御部１００には、撮像装置６０、エンコーダー９０、ロボット１（アーム２含む）、および、画像表示装置６５が接続されている。なお、画像推定部とは、位置姿勢算出部１０８と画像推定部１０９である。
【００４０】
制御部１００のアーム制御部１０１は、撮像装置６０の画像中心（アーム２の手先ともいう）を移動させる駆動信号を生成し、駆動部１０２に出力する。また、アーム制御部１０１は、画像推定部１０９が推定した画像データを用いて、アーム２をビジュアルサーボにより制御する。
【００４１】
駆動部１０２は、アーム制御部１０１が出力する駆動信号に応じて、ロボット１のアーム２を駆動する。
【００４２】
画像取得部１０３は、撮像装置６０が出力する画像データを取得し、取得した画像データをデジタルデータに変換する。例えば、撮像装置６０の撮影タイミングが３０ｆｐｓ（フレーム／秒）の場合、画像取得部１０３は、撮像装置６０が撮像した画像を３３［ｍｓｅｃ］毎に取得する。画像取得部１０３は、撮像された画像データを取得した時刻を、クロック発生部１０４が出力するクロック信号から取得する。また、画像取得部１０３は、変換した画像データと、取得した時刻を示す情報とを画像処理部１０５に出力する。
なお、撮像装置６０が出力する画像情報がデジタル信号の場合、画像取得部１０３は、取得した画像情報を、変換せずにそのまま画像処理部１０５に出力する。
【００４３】
クロック発生部１０４は、一定周期のクロック信号を発生させ、発生させたクロック信号を、アーム制御部１０１、画像取得部１０３、および位置情報取得部１０７に出力する。
【００４４】
画像処理部１０５は、画像取得部１０３が出力する画像データに対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、例えば、２値化処理により対象物７０の輪郭を抽出し、エッジ検出により特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻を示す情報を、実際の画像を撮像した時刻に補正する。例えば、取得した画像を取得した時刻と、その画像が実際に撮像された時刻が１フレーム分ずれている場合、撮像した時刻に対して取得した時刻のずれを遅れ時間という。そして、遅れ時間が１フレームの場合、取得した時刻から１フレーム分の時間を減算することで取得した時刻を算出する。画像処理部１０５は、抽出した特徴量を示す情報と算出した時刻を示す情報とを関連づけて、記憶部１０６に記憶させる。
【００４５】
記憶部１０６には、撮像された画像の特徴量を示す情報と、撮像された時刻を示す情報とが関連付けられて記憶されている。
【００４６】
位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得する。また、位置情報取得部１０７は、アーム２の手先位置・姿勢を示す情報を取得した時の時刻を、クロック発生部１０４が出力するクロック信号から取得する。また、位置情報取得部１０７は、取得したアーム２の手先位置・姿勢を示す情報と、取得した時刻を示す情報とを位置姿勢算出部１０８に出力する。
【００４７】
位置姿勢算出部１０８は、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報を用いて、後述するように基礎行列を算出し、算出した基礎行列を画像推定部１０９に出力する。また、位置姿勢算出部１０８は、画像推定部１０９が出力する実測された画像と推定された推定画像を用いて、例えば最小化問題を解いて、実測画像と推定画像間の誤差を算出し、算出した実測画像と推定画像間の誤差を画像推定部１０９に出力する。
【００４８】
画像推定部１０９は、記憶部１０６に記憶されている各時刻の画像データを読み出す。また、画像推定部１０９は、後述するように、読み出した画像データの各座標の情報と、位置姿勢算出部１０８が出力する基礎行列を用いて、現在時刻の画像を推定し、推定した現在時刻の画像データをアーム制御部１０１に出力する。
【００４９】
ビジュアルサーボ、カメラの内部パラメーター（透視変換行列）、及び基礎行列等については、公知であるため（例えば、橋本浩一. ビジュアルサーボ-ii- コンピュータビジョンの基礎、計測自動制御学会、システム／制御／情報（講座）, Vol. 53, No. 11, pp. 476-483, 2009.参照）、導出過程などの説明は省略する。
【００５０】
図４は、過去の２つの時刻における画像とカメラ位置、現在の時刻における画像とカメラ位置を説明する図である。図４に示すように、撮像装置６０は、過去の時刻ｄ２（第１の時刻）において、座標ｍ２を含む画像を撮像し、過去のある時刻ｄ１（第２の時刻）において、座標ｍ１を含む画像を撮像し、現在時刻ｄ０（第３の時刻）において、座標ｍ０を含む画像を撮像する。なお、図４において、例えば、時刻ｄ２の実際の画像は、撮像装置６０が撮像して出力するまでに１フレームの遅延がある場合、時刻ｄ３の時に撮像された画像である。
そして、過去の時刻ｄ２から現在時刻ｄ０における回転分がＲ２０、並進分がｔ２０である。同様に、過去の時刻ｄ１から現在時刻ｄ０における回転分がＲ１０、並進分がベクトルｔ１０である。同様に、過去の時刻ｄ２から過去の時刻ｄ１における回転分がＲ２１、並進分がｔ２１である。
【００５１】
また、過去の時刻ｄ２で撮像された画像Ｉ２は、座標ｍ２ｉ（ｉ＝１、・・・、Ｎ）の集まりである。同様に、過去の時刻ｄ１で撮像された画像Ｉ１は、座標ｍ１ｉ（ｉ＝１、・・・、Ｎ）の集まりである。同様に、現在時刻ｄ０で撮像された画像Ｉ０は、座標ｍ１ｉ（ｉ＝１、・・・、Ｎ）の集まりである。
時刻ｄ２とｄ１におけるエピポーラ拘束の関係は、次式（１）のように表される。同様に、時刻ｄ１とｄ０におけるエピポーラ拘束の関係は、次式（２）のように表される。同様に、時刻ｄ２とｄ０におけるエピポーラ拘束の関係は、次式（３）のように表される。なお、エピポーラ拘束とは、基準カメラに写ったある点がもう一方の画像上のある直線（エピポーラライン）上に射影され、異なる視点から得られた２つの画像の対応する点同士が満たす条件である。
【００５２】
【数１】
0005834545.tif 000002
【００５３】
【数２】
0005834545.tif 000003
【００５４】
【数３】
0005834545.tif 000004
【００５５】
現在時刻ｄ０の画像Ｉ０の座標ｍ０は、次式（４）により推定できる。
【００５６】
【数４】
0005834545.tif 000005
【００５７】
図５は、本実施形態に係る撮像装置６０が撮像するタイミングとエンコーダー情報が取得されるタイミングを説明する図である。
図５に示すように、撮像装置６０の撮影タイミングが３０ｆｐｓ（フレーム／秒）の場合、撮像装置６０が撮像した画像は、３３［ｍｓｅｃ］毎に更新される。図５において、時刻ｄ２、ｄ１、ｄ０は、撮像装置６０が撮像するタイミングであり、３３［ｍｓｅｃ］間隔である。一方、エンコーダー９０が生成するロボット１の手先位置・姿勢を示すエンコーダー情報は、例えば１［ｍｓｅｃ］毎に取得される。図５において、時刻ｄｅ１、ｄｅ２、・・・は、エンコーダー情報が取得されるタイミングであり、１［ｍｓｅｃ］間隔である。また、時刻ｄ０は現在時刻であり、時刻ｄ１は時刻ｔ０より３３［ｍｓｅｃ］前、時刻ｄ２は、時刻ｄ１より３３［ｍｓｅｃ］前である。
【００５８】
次に、現在時刻ｄ０の画像を予測する手順の概略について、図面を用いて説明する。
図６は、本実施形態に係るアーム２の手先位置・姿勢を説明する図である。図７は、本実施形態に係る各時刻におけるパラメーター、アーム２の相対移動量、画像の関係を説明する図である。
図６において、アーム部２０を簡略化して示している。図６、図７に示すように、時刻ｄ２において、撮像装置６０を備えるアーム２の手先位置・姿勢は、ロボット座標（ｘ２，ｙ２，ｚ２，α２，β２，γ２）の位置・姿勢にある。この時刻ｄ２において、撮像装置６０は、対象物体７０の画像Ｉ２を撮像する。なお、推定した画像Ｉ０を、以下、推定画像Ｉ０’という。同様に、推定した画像Ｉ１を、推定画像Ｉ１'、推定した画像Ｉ２を、推定画像Ｉ２'、推定した画像Ｉ３を、推定画像Ｉ３'という。
【００５９】
次に、時刻ｄ１において、アーム２の手先位置・姿勢は、ロボット座標（ｘ２，ｙ２，ｚ２，α２，β２，γ２） から（ｘ１，ｙ１，ｚ１，α１，β１，γ１）の位置・姿勢に移動する。時刻ｄ２からｄ１において、アーム２の手先位置・姿勢の相対移動量は、回転分がＲ２１、並進分がｔ２１である。この場合、時刻ｄ２からｄ１における基礎行列Ｆ２１は、次式（５）のように表される。
【００６０】
【数５】
0005834545.tif 000006
【００６１】
この時刻ｄ１において、撮像装置６０は、対象物体７０の画像Ｉ１を撮像する。
次に、時刻ｄ０において、アーム２の手先位置・姿勢は、ロボット座標（ｘ１，ｙ１，ｚ１，α１，β１，γ１） から（ｘ０，ｙ０，ｚ０，α０，β０，γ０）の位置・姿勢に移動する。時刻ｄ１からｄ０において、アーム部２０の手先位置・姿勢の相対移動量は、回転分がＲ１０、並進分がｔ１０である。この場合、時刻ｄ１からｄ０における基礎行列Ｆ１０は、次式（６）のように表される。
【００６２】
【数６】
0005834545.tif 000007
【００６３】
式（５）と（６）において、符号Ｔは、並進を表す行列であり、次式（７）のように表される。なお、並進ベクトルｔは、［ｔｘ  ｔｙ  ｔｚ］Ｔ（Ｔは転置行列を表す）である。また、行列Ａは、撮像装置６０の内部パラメーターを表す行列であり、画像中心座標、直交性、縦軸と横軸のスケール、焦点距離などにより表される、例えば３行３列の行列である。
【００６４】
【数７】
0005834545.tif 000008
【００６５】
そして、制御部１００の画像推定部１０９は、これらの基礎行列（ｆｕｎｄａｍｅｎｔａｌ  ｍａｔｒｉｘ）Ｆ２１（式（５））、Ｆ１０（式（６））、撮像された画像Ｉ２、およびＩ１を用いて、現在時刻ｄ０の画像Ｉ０を推定する。なお、基礎行列とは、２つの画像の対応する点の座標上の関係（写像関係）を示す行列である。
【００６６】
なお、従来技術では、各時刻におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ、並進を表す行列Ｔ、及び撮像装置６０の内部パラメーターを表す行列Ａにキャリブレーション誤差がなく、正確な場合を想定している。しかしながら、実際には、各行列には、ロボット１のロボットキャリブレーション誤差や、カメラキャリブレーション誤差等を有している場合がある。なお、ロボットキャリブレーション誤差とは、ロボットが有する幾何学的な誤差要因と非幾何学的な誤差要素に対してキャリブレーションを行った後の誤差である。幾何学的な誤差要因とは、ロボットの機構の要素加工時の製造誤差等であり、非幾何学的な誤差要素とは、負荷時の重力による各関節のたわみや歯車間の伝達誤差等である。また、カメラキャリブレーション誤差とは、撮像装置６０の内部パラメーターを表す行列Ａと、撮像装置６０の外部パラメーターを表す行列を決定することである。なお、撮像装置６０の外部パラメーターとは、並進ベクトルｔ、回転を表す行列Ｒを要素に持つ、カメラ座標系でのベクトルをワールド座標系に変換する同次行列である。また、撮像装置６０が撮像してから、出力されるまでの遅れ時間による誤差がある。このような場合、誤差を考慮せずに現在時刻の画像データを推定してしまうと、推定した画像データに誤差を含んでしまうため、精度良くアーム２の制御ができない。これらの誤差を考慮して、現在時刻の画像データを推定するには、回転を表す行列Ｒ、並進を表す行列Ｔ、及び撮像装置６０の内部パラメーターを表す行列Ａの各行列の誤差（ΔＲ、ΔＴ、ΔＡ）を算出することで補正できる。誤差は、例えば、現在時刻ｄ０より前の過去の時刻ｄ１に取得された画像データと過去の時刻ｄ１の、推定された画像データ間の誤差を算出することで求められる。この場合、推定された画像データと、実際に取得された画像データとが一致するような最小化問題を、例えばニュートン法などを用いて解くことで、誤差情報を算出する。
【００６７】
次に、本実施形態における時刻ｄ０における画像Ｉ０に含まれる誤差ε０の算出方法について説明する。誤差ε０は、時刻ｄ０における（回転を表す行列Ｒ、並進を表す行列Ｔ、撮像装置６０の内部パラメーターを表す行列Ａ）の誤差として、次式（８）のように表される。
なお、式（８）において、ベクトルｘ１は、ワールド座標における時刻ｄ１におけるアーム２の手先位置・姿勢、ベクトルｘ０は、時刻ｄ０におけるアーム２の手先位置・姿勢、誤差ε１は、時刻ｄ１における画像Ｉ１の誤差、誤差ε０は、時刻ｄ０における画像Ｉ０の誤差である。
【００６８】
【数８】
0005834545.tif 000009
【００６９】
式（８）において、一定の誤差がある場合、すなわち、誤差が時間や位置に依存しない場合の誤差は一定値とみなせる。このため、誤差の偏微分成分である次式（９）は、０と見なせるため、時刻ｄ０における誤差ε０は、次式（１０）のように、時刻ｄ１における誤差ε１と等しい。
【００７０】
【数９】
0005834545.tif 000010
【００７１】
【数１０】
0005834545.tif 000011
【００７２】
また、式（８）において、誤差が時間や位置に依存する場合、式（９）を、過去の時系列データから求めるようにしてもよい。
時刻ｄ１における画像Ｉ１の誤差ε１は、時刻ｄ１に取得した画像Ｉ１と、時刻ｄ１に取得される画像を推定した推定画像（以下、時刻ｄ１の推定画像という）Ｉ１’との差を、ニュートン法などを用いて最小化問題を解くことで算出する。例えば、画像Ｉ１から、推定した推定画像Ｉ１’にΔＲ、ΔＴ、ΔＡを加えて、推定画像Ｉ１’を更新した推定画像を減算し、減算結果が最小になるΔＲ、ΔＴ、ΔＡを算出することで、誤差ε１（ΔＲ１、ΔＴ１、ΔＡ１）を算出する。
時刻ｄ１の推定画像Ｉ１’は、上述したように式（４）に基づき、次式（１１）を用いて、各画素を算出することで推定する。ただし、この場合、推定された推定画像Ｉ１’は、誤差を有している。なお、撮像装置６０の内部パラメーターを表す行列Ａの各パラメーターは、予めキャリブレーション処理により算出しておく。
【００７３】
【数１１】
0005834545.tif 000012
【００７４】
式（１１）において、座標ｍ２は、画像Ｉ２の各座標、座標ｍ３は、画像Ｉ３の各座標である。また、基礎行列Ｆ２１は、時刻ｄ２から時刻ｄ１に移動した場合の基礎行列、基礎行列Ｆ３１は、時刻ｄ３から時刻ｄ１に移動した場合の基礎行列である。すなわち、推定画像Ｉ１’は、画像Ｉ３、Ｉ２、基礎行列Ｆ３１、及び基礎行列Ｆ２１を用いて推定する。
【００７５】
次に、現在時刻ｄ０の画像Ｉ０を推定する手順について、図８のフローチャートを用いて説明する。なお、本実施形態では、撮像装置６０が画像データを撮像してから、撮像された画像データを画像取得部１０３が取得するまで、１フレームの遅延量があるとして説明する。
【００７６】
（ステップＳ１）制御部１００のアーム制御部１０１は、撮像装置６０が取り付けられているアーム２の手先位置・姿勢を（ｘ３，ｙ３，ｚ３，α３，β３，γ３）から（ｘ２，ｙ２，ｚ２，α２，β２，γ２）に移動させる駆動信号を生成し、駆動部１０２を介してロボット１のアーム２を駆動する。また、アーム制御部１０１は、生成した駆動信号に基づく情報を位置姿勢算出部１０８に出力する。なお、駆動信号に基づく情報とは、例えば、アーム２の手先位置・姿勢（ｘ３，ｙ３，ｚ３，α３，β３，γ３）とアーム２の手先位置・姿勢（ｘ２，ｙ２，ｚ２，α２，β２，γ２）を示す情報である。
この駆動信号により、アーム２の手先位置・姿勢は、時刻ｄ３の時に（ｘ３，ｙ３，ｚ３，α３，β３，γ３）に移動し、時刻ｄ２の時に（ｘ２，ｙ２，ｚ２，α２，β２，γ２）に移動する。また、アーム２の手先位置・姿勢は、時刻ｄ１の時に（ｘ１，ｙ１，ｚ１，α１，β１，γ１）に移動し、時刻ｄ０の時に（ｘ０，ｙ０，ｚ０，α０，β０，γ０）に移動する。そして、時刻ｄ３〜ｄ０の各間隔は、撮像装置６０のフレームレートであり、例えば３３［ｍｓｅｃ］である。ステップＳ１終了後、ステップＳ２に進む。
【００７７】
（ステップＳ２）次に、時刻ｄ２（第２の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ３を撮像装置６０から取得する。取得した画像Ｉ３は、上述したように内部処理による遅れにより、画像を取得した時刻ｄ２に対して、１フレーム分の３３［ｍｓｅｃ］遅れている。このため、取得した画像Ｉ３は、時刻ｄ２より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ３（第１の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ３を取得したときの時刻ｄ２をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ３をアナログ信号からデジタル信号に変換し、変換した画像Ｉ３と取得した時刻ｄ２とを画像処理部１０５に出力する。ステップＳ２終了後、ステップＳ３に進む。
【００７８】
（ステップＳ３）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ３に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、例えば、２値化処理により対象物７０の輪郭を抽出し、エッジ検出により特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ２に基づき、実際の画像を撮像した時刻ｄ３（＝ｄ２−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ３とを関連づけて、記憶部１０６に記憶させる。ステップＳ３終了後、ステップＳ４に進む。
【００７９】
（ステップＳ４）時刻ｄ２の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ４終了後、ステップＳ５に進む。
【００８０】
（ステップＳ５）次に、時刻ｄ１（第３の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ２を撮像装置６０から取得する。取得した画像Ｉ２は、時刻ｄ１より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ２（第２の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ２を取得したときの時刻ｄ１をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ２をアナログ信号からデジタル信号に変換し、変換した画像Ｉ２と取得した時刻ｄ１とを画像処理部１０５に出力する。ステップＳ５終了後、ステップＳ６に進む。
【００８１】
（ステップＳ６）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ２に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ１に基づき、実際の画像を撮像した時刻ｄ２（＝ｄ１−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ２とを関連づけて、記憶部１０６に記憶させる。ステップＳ６終了後、ステップＳ７に進む。
【００８２】
（ステップＳ７）時刻ｄ１の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ７終了後、ステップＳ８に進む。
【００８３】
（ステップＳ８）次に、時刻ｄ０（第４の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ１を撮像装置６０から取得する。取得した画像Ｉ１は、時刻ｄ０より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ１（第３の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ１を取得したときの時刻ｄ０をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ２をアナログ信号からデジタル信号に変換し、変換した画像Ｉ１と取得した時刻ｄ０とを画像処理部１０５に出力する。ステップＳ８終了後、ステップＳ９に進む。
【００８４】
（ステップＳ９）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ１に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ０から画像の遅れに基づく時間３３［ｍｓｅｃ］を減算して、実際の画像を撮像した時刻ｄ１（＝ｄ０−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ１とを関連づけて、記憶部１０６に記憶させる。ステップＳ９終了後、ステップＳ１０に進む。
【００８５】
（ステップＳ１０）時刻ｄ０の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ１０終了後、ステップＳ１１に進む。
【００８６】
（ステップＳ１１）次に、位置姿勢算出部１０８は、時刻ｄ３から時刻ｄ１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ３１と並進を表す行列Ｔ３１を、時刻ｄ３とｄ１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ３１と並進を表す行列Ｔ３１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ３１を上述した式（５）においてＲ２１をＲ３１、Ｔ２１をＴ３１に置き換えて算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ３１を画像推定部１０９に出力する。
次に、位置姿勢算出部１０８は、時刻ｄ２から時刻ｄ１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２１と並進を表す行列Ｔ２１を、時刻ｄ２とｄ１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ２１と並進を表す行列Ｔ２１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ２１を、上述した式（５）により算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ２１を画像推定部１０９に出力する。
ステップＳ１１終了後、ステップＳ１２に進む。
【００８７】
（ステップＳ１２；第１画像推定手順）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ３とＩ２の画像データを読み出す。画像推定部１０９は、位置姿勢算出部１０８が出力する基礎行列Ｆ３１と基礎行列Ｆ２１、記憶部１０６から読み出した画像Ｉ３とＩ２の画像データを用いて、上述した式（１１）により時刻ｄ１の推定画像Ｉ１’を推定する。なお、式（１１）において、画像情報ｍ１は、時刻ｄ１の推定画像Ｉ１’の画像情報であり、画像情報ｍ２は、時刻ｄ１で取得された画像Ｉ２の画像情報であり、画像情報ｍ３は、時刻ｄ２で取得された画像Ｉ３の画像情報である。画像推定部１０９は、時刻ｄ０で実測された画像Ｉ１と式（１１）で推定した推定画像Ｉ１’を位置姿勢算出部１０８に出力する。ステップＳ１２終了後、ステップＳ１３に進む。
【００８８】
（ステップＳ１３）次に、位置姿勢算出部１０８は、画像推定部１０９が出力する実測された画像Ｉ１と推定された推定画像Ｉ１’を用いて、例えば最小化問題を解いて誤差ε１である（ΔＲ１、ΔＴ１、ΔＡ１）を算出する。ステップＳ１３終了後、ステップＳ１４に進む。
【００８９】
（ステップＳ１４）次に、位置姿勢算出部１０８は、時刻ｄ１から時刻ｄ０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ１０と並進を表す行列Ｔ１０を、時刻ｄ１とｄ０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ１０と並進を表す行列Ｔ１０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ１０を、上述した式（６）により算出する。
ステップＳ１４終了後、ステップＳ１５に進む。
【００９０】
（ステップＳ１５）次に、位置姿勢算出部１０８は、時刻ｄ２から時刻ｄ０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２０と並進を表す行列Ｔ２０を、時刻ｄ２とｄ０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。
次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ２０と並進を表す行列Ｔ２０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ２０を、次式（１２）により算出する。
ステップＳ１５終了後、ステップＳ１６に進む。
【００９１】
【数１２】
0005834545.tif 000013
【００９２】
（ステップＳ１６）次に、誤差が時間や位置に依存しない場合、位置姿勢算出部１０８は、上述した式（１０）により、誤差ε０をステップＳ１２で算出した誤差ε１として算出し、算出した誤差ε０（ΔＲ０、ΔＴ０、ΔＡ０）を画像推定部１０９に出力する。ステップＳ１６終了後、ステップＳ１７に進む。
【００９３】
（ステップＳ１７；第２画像推定手順）次に、画像推定部１０９は、記憶部１０６に記憶されている時刻ｄ１の時の画像Ｉ２と、時刻ｄ０の時のＩ１を読み出す。なお、画像情報ｍ０は、時刻ｄ０の推定される推定画像Ｉ０’の画像情報であり、画像情報ｍ１は、時刻ｄ０で取得された画像Ｉ１の画像情報であり、画像情報ｍ２は、時刻ｄ１で取得された画像Ｉ２の画像情報である。
次に、画像推定部１０９は、ステップＳ１６で算出した誤差ε０（ΔＲ０、ΔＴ０、ΔＡ０）を各基礎行列Ｆ１０、Ｆ２０に加算して、各基礎行列Ｆ１０、Ｆ２０を更新する。なお、回転行列Ｒについては、誤差ΔＲ０を乗じることで更新する。ステップＳ１７終了後、ステップＳ１８に進む。
【００９４】
（ステップＳ１８；第２画像推定手順）次に、画像推定部１０９は、読み出した画像Ｉ１、Ｉ２の各画素の情報ｍ１、ｍ２と、更新した基礎行列Ｆ１０、Ｆ２０を用いて、次式（１３）により、時刻ｄ０の画像ｍ０を推定する。
【００９５】
【数１３】
0005834545.tif 000014
【００９６】
全ての画素の情報について算出することで、画像推定部１０９は、時刻ｄ０の推定画像Ｉ０’を推定する。
以上により、時刻ｄ０の画像Ｉ０の推定を終了する。
このようにして推定された現在時刻ｄ０の推定画像Ｉ０’を用いて、制御部１００が、ビジュアルサーボを行う。
【００９７】
なお、ステップＳ１８では、時刻ｄ２、ｄ１に撮像された画像Ｉ３、Ｉ２を用いて、現在時刻ｄ０の画像Ｉ０を推定する方法を説明した。撮像装置６０の遅れ時間が長く、画像処理により最低でも２フレームの遅れ時間が発生する場合は、例えば、時刻ｄ３（現在時刻ｄ０より３フレーム前の時刻）に撮像された画像Ｉ４と、時刻ｄ２（現在時刻ｄ０より２フレーム前の時刻）に撮像された画像Ｉ３とを用いて、現在時刻ｄ０の画像Ｉ０を推定する。この結果、現在時刻ｄ０における画像Ｉ０が直接得られなくても、誤差を含めて精度良く画像Ｉ０を推定できるので、遅れ時間があっても精度良く制御できる。
また、遅れ時間が１フレームの場合であっても、時刻ｄ３（現在時刻ｄ０より３フレーム前の時刻）、ｄ２（現在時刻ｄ０より２フレーム前の時刻）に撮像された画像Ｉ４、Ｉ３を用いて、現在時刻ｄ０の画像Ｉ０を推定するようにしてもよい。
また、なお、本実施形態では、ステップＳ１８とＳ１９で、時刻ｄ０の画像ｍ０を推定した後、推定した推定画像Ｉ０’に誤差を加算して推定画像Ｉ０’を更新する例を説明した。しかしながら、ステップＳ１７で算出した誤差ε０（ΔＲ０、ΔＴ０、ΔＡ０）を基礎行列Ｆ１０に加算して基礎行列Ｆ１０を更新し、更新したＦ１０を用いて式（１３）により時刻ｄ０の推定画像Ｉ０’を算出するようにしてもよい。
【００９８】
以上のように、本実施形態では、画像推定部１０９は、現在時刻ｄ０より１フレーム前の時刻ｄ１の推定画像Ｉ１’を推定する。そして、位置姿勢算出部１０８は、現在時刻ｄ０より１フレーム前の時刻ｄ１に取得された画像Ｉ０と推定画像Ｉ１’との誤差ε０を誤差ε１として算出する。そして、位置姿勢算出部１０８は、時刻ｄ２から時刻ｄ０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２０、並進を表す行列Ｔ２０、撮像装置６０の内部パラメーターを表す行列Ａを用いて、基礎行列Ｆ２０を算出する。また、位置姿勢算出部１０８は、時刻ｄ１から時刻ｄ０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２０、並進を表す行列Ｔ２０、撮像装置６０の内部パラメーターを表す行列Ａを用いて、基礎行列Ｆ２０を算出する。そして、画像推定部１０９は、現在時刻ｄ０より２フレーム前の時刻ｄ２に撮像された画像Ｉ３、現在時刻ｄ０より１フレーム前の時刻ｄ１に撮像された画像Ｉ２、誤差を考慮した基礎行列Ｆ２０、基礎行列Ｆ１０を用いて、現在時刻ｄ０の推定画像Ｉ０’を推定する。そして、推定した推定画像Ｉ０’に算出した誤差ε０を加算して、推定画像Ｉ０’を更新する。
この結果、撮像装置６０による遅れ誤差、ロボットキャリブレーション誤差、及びカメラキャリブレーション誤差を考慮した現在時刻ｄ０における推定画像Ｉ０’を精度良く推測できる。そして、この推定された画像を用いてロボット１を制御できるため、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度の良いビジュアルサーボを行うことができる。
【００９９】
［第２実施形態］
本実施形態では、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報が取得できるタイミング毎に画像を推定する。
本実施形態に係る現在時刻ｄ０の画像Ｉ０を推定する手順について、図９のフローチャートを用いて説明する。エンコーダー９０からの出力は、１［ｍｓｅｃ］毎に取得できるとして、以下の説明をする。
【０１００】
（ステップＳ１０１）まず、制御部１００のアーム制御部１０１は、アーム２の手先位置・姿勢を（ｘ３，ｙ３，ｚ３，α３，β３，γ３）から（ｘ２，ｙ２，ｚ２，α２，β２，γ２）に移動させる駆動信号を生成し、駆動部１０２を介してロボット１のアーム２を駆動する。また、アーム制御部１０１は、生成した駆動信号に基づく情報を位置姿勢算出部１０８に出力する。
この駆動信号により、アーム２の手先位置・姿勢は、時刻ｄ３の時に（ｘ３，ｙ３，ｚ３，α３，β３，γ３）に移動し、時刻ｄ２の時に（ｘ２，ｙ２，ｚ２，α２，β２，γ２）に移動する。また、アーム２の手先位置・姿勢は、時刻ｄ１の時に（ｘ１，ｙ１，ｚ１，α１，β１，γ１）に移動し、時刻ｄ０の時に（ｘ０，ｙ０，ｚ０，α０，β０，γ０）に移動する。そして、時刻ｄ３〜ｄ０の間隔は、撮像装置６０のフレームレートであり、例えば３３［ｍｓｅｃ］である。ステップＳ１０１終了後、ステップＳ１０２に進む。
【０１０１】
（ステップＳ１０２）次に、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を１［ｍｓｅｃ］毎に取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ１０２終了後、ステップＳ１０３に進む。
【０１０２】
（ステップＳ１０３）次に、画像取得部１０３は、クロック発生部１０４が出力するクロック信号に基づき、３３［ｍｓｅｃ］経過したか否かを判定する。
３３［ｍｓｅｃ］経過した場合（ステップＳ１０３；Ｙｅｓ）、ステップＳ１０４に進む。３３［ｍｓｅｃ］経過していない場合（ステップＳ１０３；Ｎｏ）、ステップＳ１０６に進む。
【０１０３】
（ステップＳ１０４）画像取得部１０３は、時刻ｄ２に画像Ｉ３を撮像装置６０から取得する。また、画像取得部１０３は、画像Ｉ３を取得したときの時刻ｄ２をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ３をアナログ信号からデジタル信号に変換し、変換した画像Ｉ３と取得した時刻ｄ２とを画像処理部１０５に出力する。ステップＳ１０４終了後、ステップＳ１０５に進む。
【０１０４】
（ステップＳ１０５）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ３に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、例えば、２値化処理により対象物７０の輪郭を抽出し、エッジ検出により特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ２から画像の遅れに基づく時間３３［ｍｓｅｃ］を減算して、実際の画像を撮像した時刻ｄ３（＝ｄ２−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ３とを関連づけて、記憶部１０６に記憶させる。ステップＳ１０５終了後、ステップＳ１０６に進む。
【０１０５】
（ステップＳ１０６）次に、画像推定部１０９は、記憶部１０６に画像データが３つ以上記憶されているか否かを判別する。
記憶部１０６に画像データが３つ以上記憶されている場合（ステップＳ１０６；Ｙｅｓ）、ステップＳ１０７に進む。記憶部１０６に画像データが３つ以上記憶されていない場合（ステップＳ１０６；Ｎｏ）、ステップＳ１０２に戻る。
【０１０６】
この場合、記憶部１０６には、まだ画像Ｉ３しか記憶されていないため、ステップＳ１０２に戻り、ステップＳ１０２〜Ｓ１０６を繰り返す。そして、ステップＳ１０４において、画像取得部１０３は、対象物７０を撮像した時刻ｄ１における画像Ｉ２を撮像装置６０から取得する。ステップＳ１０５において、画像処理部１０５は、抽出した特徴量と算出した時刻ｄ２とを関連づけて、記憶部１０６に記憶させる。さらに、ステップＳ１０４において、画像取得部１０３は、対象物７０を撮像した時刻ｄ０における画像Ｉ１を撮像装置６０から取得する。ステップＳ１０５において、画像処理部１０５は、抽出した特徴量と算出した時刻ｄ０とを関連づけて、記憶部１０６に記憶させる。
【０１０７】
（ステップＳ１０７）次に、位置姿勢算出部１０８は、時刻ｄ３から時刻ｄ１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ３１と並進を表す行列Ｔ３１を、時刻ｄ３とｄ１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ３１と並進を表す行列Ｔ３１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ３１を上述した式（５）においてＲ２１をＲ３１、Ｔ２１をＴ３１に置き換えて算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ３１を画像推定部１０９に出力する。
次に、位置姿勢算出部１０８は、時刻ｄ２から時刻ｄ１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２１と並進を表す行列Ｔ２１を、時刻ｄ２とｄ１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ２１と並進を表す行列Ｔ２１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ２１を、上述した式（５）により算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ２１を画像推定部１０９に出力する。
ステップＳ１０７終了後、ステップＳ１０８に進む。
【０１０８】
（ステップＳ１０８）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ３とＩ２の画像データを読み出す。画像推定部１０９は、位置姿勢算出部１０８が出力する基礎行列Ｆ２１と基礎行列Ｆ３１、記憶部１０６から読み出した画像Ｉ３とＩ２の画像データを用いて、上述した式（１１）により時刻ｄ１の推定画像Ｉ１’を推定する。画像推定部１０９は、実測された画像Ｉ１と推定した推定画像Ｉ１’を位置姿勢算出部１０８に出力する。ステップＳ１０終了後、ステップＳ１０９に進む。
【０１０９】
（ステップＳ１０９）次に、位置姿勢算出部１０８は、画像推定部１０９が出力する実測された画像Ｉ１と推定された推定画像Ｉ１’を用いて、例えば最小化問題を解いて誤差ε１である（ΔＲ１、ΔＴ１、ΔＡ１）を算出する。ステップＳ１０９終了後、ステップＳ１１０に進む。
【０１１０】
（ステップＳ１１０）次に、位置姿勢算出部１０８は、時刻ｄ１から時刻ｄ０におけるアーム２の手先位置・姿勢の回転Ｒ１０と並進を表す行列Ｔ１０を、時刻ｄ１とｄ０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ１０と並進を表す行列Ｔ１０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ１０を、上述した式（６）により算出する。
ステップＳ１１０終了後、ステップＳ１１１に進む。
【０１１１】
（ステップＳ１１１）次に、位置姿勢算出部１０８は、時刻ｄ２から時刻ｄ０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ２０と並進を表す行列Ｔ２０を、時刻ｄ２とｄ０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ２０と並進を表す行列Ｔ２０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ２０を、上述した式（１２）により算出する。
ステップＳ１１１終了後、ステップＳ１１２に進む。
【０１１２】
（ステップＳ１１２）次に、誤差が時間や位置に依存しない場合、位置姿勢算出部１０８は、上述した式（１０）により、誤差ε０（ΔＲ０、ΔＴ０、ΔＡ０）をステップＳ１０９で算出した誤差ε１（ΔＲ１、ΔＴ１、ΔＡ１）として算出する。ステップＳ１１２終了後、ステップＳ１１３に進む。
【０１１３】
（ステップＳ１１３）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ２、Ｉ１を読み出す。
次に、画像推定部１０９は、ステップＳ１１２で算出した誤差ε０（ΔＲ０、ΔＴ０、ΔＡ０）を各基礎行列Ｆ１０、Ｆ２０に加算して、各基礎行列Ｆ１０、Ｆ２０を更新する。なお、回転行列Ｒについては、誤差ΔＲ０を乗じることで更新する。ステップＳ１１３終了後、ステップＳ１１４に進む。
【０１１４】
（ステップＳ１１４）次に、画像推定部１０９は、読み出した画像Ｉ２、Ｉ１の各画素の情報ｍ２、ｍ１と、更新した基礎行列Ｆ１０、Ｆ２０を用いて、上述した式（１３）により、時刻ｄ０の画像ｍ０を推定する。
全ての画素の情報について算出することで、画像推定部１０９は、時刻ｄ０の推定画像Ｉ０’を推定する。
【０１１５】
ステップＳ１１４終了後、ステップＳ１０２に戻る。
このようにして推定された現在時刻ｄ０の推定画像Ｉ０’を用いて、制御部１００が、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎に、ビジュアルサーボを行う。
【０１１６】
本実施形態によれば、位置姿勢算出部１０８は、撮像装置６０のフレームレートが３３［ｍｓｅｃ］の場合、撮像装置６０から取得できる画像データを３３［ｍｓｅｃ］でしか得られなくても、現在時刻より前に取得した画像データ、アーム２の手先位置・姿勢を示す情報に基づき、アーム２の手先位置・姿勢の誤差を算出できる。そして、画像推定部１０９は、現在時刻より前の時刻ｄ１に取得した画像データと時刻ｄ１の推定画像Ｉ１’との誤差ε１を現在時刻ｄ０の誤差ε０として算出する。そして、算出した誤差を用いて、現在時刻の画像を推定するようにしたので、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎（例えば、１［ｍｓｅｃ］毎）に、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良くビジュアルサーボを行うことができる。
例えば、低いフレームレートの撮像装置６０を用いたロボット１では、ロボット１の制御におけるサーボループのゲインを上げないと高速に制御できない。しかしながら、低いフレームレートの撮像装置６０を用いたロボット１では、サーボループのゲインを上げると振動が発生するため、サーボループのゲインを上げられず、このためロボット１を高速に制御することができない。また、高速な制御を行うために高いフレームレートの撮像装置６０を用いた場合、撮像装置６０が高価であるため、ロボット１のコストアップになってしまう。
本実施形態によれば、低いフレームレートの撮像措置６０を用いた場合でも、撮像装置６０のフレームレートより速い間隔で、予測した画像を用いてロボット１を制御することができる効果がある。
【０１１７】
なお、本実施形態では、ステップＳ１０３で、画像取得部１０３が、クロック発生部１０４が出力するクロック信号に基づき、３３［ｍｓｅｃ］毎に撮像された画像データを取得する例を説明したが、撮像装置６０が撮像した画像データを出力するタイミング毎に画像データを取得するようにしてもよい。
また、本実施形態では、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎（例えば、１［ｍｓｅｃ］毎）に現在時刻の画像を推定する例を説明したが、画像を推定する間隔は、制御する用途に応じて任意でもよい。
【０１１８】
［第３実施形態］
図１０は、本実施形態に係るロボット１ａの概略構成を示す斜視図である。図１０に示すように、ロボット１ａは、２本のアーム２−１と２−２を備えている。アーム２−１と２−２は、本体部５００に連結されている。また、本体部５００は、固定部１０ａに連結されている。
アーム２−１は、アーム部２０−１、及びエンドエフェクター４０−１を備える。アーム２−２は、アーム部２０−２、力センサー３０−２、及びエンドエフェクター４０−２を備える。また、ロボット１は、制御部１００ａの制御により動作する。
【０１１９】
撮像装置６０は、エンドエフェクター４０−１の先端部に取り付けられている。撮像装置６０は、対象物を撮影し、撮影したアナログ値の画像データを制御装置１００ａに出力する。
【０１２０】
アーム部２０−１は、第１実施形態の構成と同様である。
アーム部２０−２は、第１実施形態と同様に第１リンク２１−２、第２リンク２２−２、第３リンク２３−２、第４リンク２４−２および第５リンク２５−２がこの順に連結されたものである。アーム部２０−２の第５リンク２５−２の他方端には、エンドエフェクター４０−２が設けられている。アーム部２０−２の第５リンク２５−２とエンドエフェクター４０−２の間には、力センサー３０−２が介挿されている。力センサー３０−２は、検出したアーム２−２のエンドエフェクター４０−２に加わる力成分とトルク成分を含む情報を制御部１００ａに出力する。なお、エンドエフェクター４０−２は、例えば把持部（ハンド部）である。
【０１２１】
図１１は、本実施形態に係るアーム部２０−１及び２０−２と力センサーとエンコーダーの概念図である。
図１１に示すように、ロボット１ａは、各アーム２−１、２−２がそれぞれ６自由度を有し、さらに本体部５００が１自由度を有している。図１１に示すように、アーム２−１と同様に、アーム２−１のアーム部２０−１は、回転部８２−１、８３−１及び８５−１と、屈折部８１−１、８４−１及び８６−１とを有している。そして、アーム２−１のアーム部２０−１は、エンドエフェクター４０−１との間に力センサー３０を有している。
【０１２２】
また、図１１に示すように、第１実施形態と同様に回転部８２−１、８３−１及び８５−１と、屈折部８１−１、８４−１及び８６−１は、各々エンコーダー９１−１〜９６−１を備えている。また、図１１に示すように、アーム２−１と同様に、アーム２−２のアーム部２０−２は、回転部８２−２、８３−２及び８５−２と、屈折部８１−２、８４−２及び８６−２は、各々エンコーダー９１−２〜９６−２を備えている。
【０１２３】
本実施形態では、アーム２−１に取り付けられている撮像装置６０により、ワーク７２の上に置かれている対象物７０を撮像して、制御部１００ａが、第１実施形態と同様に現在時刻の画像データを推定する。そして、制御部１００ａは、推定した画像データを用いて、アーム２−２のエンドエフェクター４０−２のハンドが把持している物体の位置を制御する。制御部１００ａは、例えば、対象物７０に把持されている部品を組み付けるように、推定された画像データを用いてアーム２−２を制御する。
この結果、本実施形態のロボット１は、第１実施形態と同様に、撮像装置６０が出力する画像データに遅れ時間があり現在時刻の画像データが取得できず、さらにロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良くアームを制御して例えば部品の取り付け等の作業を、精度良く行うことができる。また、第２実施形態の処理方法を組み合わせることで、本実施形態のロボット１は、撮像装置６０のフレームレートより高いレートで画像データの推定を行うことで、撮像装置６０が低いフレームレートであっても、高速でアームを精度良く制御することができる。
【０１２４】
なお、本実施形態では、撮像装置が撮像してから撮像した画像を出力するまで、１フレームの遅れがある例を説明した。しかしながら、撮像装置のフレームレートが高速であり、撮像してから撮像された画像が出力されるまでの遅れ時間による誤差がほとんど無い場合でも、上述したようにロボットキャリブレーション誤差やカメラキャリブレーション誤差が存在する。本実施形態のロボットによれば、このような場合でも、異なる時刻に撮像された画像間の誤差を算出して、算出した誤差を用いて、現在時刻の画像を推定することで、ロボットキャリブレーション誤差やカメラキャリブレーション誤差があっても、それらを補正して現在時刻の画像を精度良く推定することができる。この結果、推定された画像を用いて精度良くビジュアルサーボを行うことができる。
【０１２５】
なお、実施形態の図３または図１０の制御部の各部の機能を実現するためのプログラムをコンピューター読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピューターシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピューターシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＣＤ−ＲＯＭ等の可搬媒体、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）  Ｉ／Ｆ（インタフェース）を介して接続されるＵＳＢメモリー、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、サーバーやクライアントとなるコンピューターシステム内部の揮発性メモリーのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
【符号の説明】
【０１２６】
１・・・ロボット  １０・・・固定部  ２０・・・アーム部
３０・・・力センサー  ４０・・・エンドエフェクター  ６０・・・撮像装置
７０・・・対象物  １００・・・制御部  １０１・・・アーム制御部
１０２・・・駆動部  １０３・・・画像取得部  １０４・・・クロック発生部
１０５・・・画像処理部  １０６・・・記憶部  １０７・・・位置情報取得部
１０８・・・位置姿勢算出部（誤差算出部）  １０９・・・画像推定部
Ｆ０１、Ｆ０２、Ｆ１２・・・基礎行列
Ｒ・・・回転を表す行列  Ｔ・・・並進を表す行列  ｔ・・・並進
Ａ・・・カメラ（撮像装置６０）の内部パラメーターを表す行列
ベクトルｘ１、ベクトルｘ２、ベクトルｘ０・・・アームの手先位置・姿勢
ε・・・実際に取得された画像と推定された画像との誤差
Ｉ１、Ｉ２・・・撮像された画像  Ｉ０’・・・現在時刻ｄ０の推定画像
ｍ１、ｍ２・・・Ｉ１、Ｉ２の各画素
【図１】
0005834545.tif 000015
【図２】
0005834545.tif 000016
【図３】
0005834545.tif 000017
【図４】
0005834545.tif 000018
【図５】
0005834545.tif 000019
【図６】
0005834545.tif 000020
【図７】
0005834545.tif 000021
【図８】
0005834545.tif 000022
【図９】
0005834545.tif 000023
【図１０】
0005834545.tif 000024
【図１１】
0005834545.tif 000025