JP2013013950A - ロボット、ロボット制御装置、ロボット制御方法、およびロボット制御プログラム - Google Patents

Abstract

【課題】ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良く制御可能なロボット、ロボット制御装置、ロボット制御方法、およびロボット制御プログラムを提供することを目的としている。
【解決手段】撮像装置６０と、第１〜第４の時刻にロボットのアームに設けられた撮像装置が出力する第１〜第４の画像データを取得する画像取得部１０３と、第１と第２の画像データとに基づき第３の時刻の画像データを推定し、推定した第３の時刻の画像データと取得された第３の時刻の画像データとの誤差を算出し、算出した誤差に基づき第４の時刻の画像データに含まれる誤差を算出する画像推定部１０９と、アームを制御するアーム制御部１０１とを備え、画像推定部は算出した第４の時刻における画像データに含まれる誤差を補正して第４の時刻の画像データを推定し、アーム制御部は推定された第４の時刻の画像データを用いてアームを制御する。
【選択図】図３

Description

本発明は、ロボット、ロボット制御装置、ロボット制御方法およびロボット制御プログラムに関する。

近年、製造現場等では、スカラロボットや多軸ロボット等が製品の組み立てや検査等に用いられている。製品の組み立てや検査を行なう際のロボットの制御方法としてビジュアルサーボがある。ビジュアルサーボは、目標物との相対的な位置の変化を視覚情報として計測し、それをフィードバック情報として用いることによって目標物を追跡するサーボ系の一種である。

画像取得には、露光、アナログ−デジタル変換、カメラからパソコンのインターフェースへの画像データ転送などが含まれる。したがって、通常は露光開始の瞬間からＣＰＵが画像を読み出し可能となるまでには、最低でも１フレーム分の遅れが発生する。さらにＣＰＵでは、画像処理に時間がかかるため、画像処理が終了するまで最低でももう１フレームの遅れが発生する。
このため、ビジュアルサーボでは、画像を露光した時の情報を使って、ロボットに次に移動する指令値を生成するが、ロボットに指令値を送る時は露光した時間から最低でも２フレーム分以上の時間が経過してしまう。

これに対して、ロボットのアームの手先位置・姿勢を出力するエンコーダーの情報は、高速取り込みが可能である。このため、非特許文献１では、このエンコーダーの情報と過去の画像情報を用いて現在の予測画像を生成することで、画像転送にかかる時間（遅れ時間）を待たずに、ロボットの指令値を生成している。

また、特許文献１では、画像転送処理時間のタイムラグを誤差として予測し補正するために、メモリーにセンサー情報を記憶させて、記憶させたセンサー情報を用いて、取得時刻が同じになるようにそれぞれのセンサー情報を近似補正している。近似補正は、メモリーに記憶されている算出したい時刻の前後の位置情報を用いて算出している。また、特許文献１では、センサー情報の取り込み間隔が微小な場合、算出したい時刻に最も近い時刻に取得された位置情報を用いて近似している。

特開平９−３００２６４号公報

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.参照）、導出過程などの説明は省略する。

図４は、過去の２つの時刻における画像とカメラ位置、現在の時刻における画像とカメラ位置を説明する図である。図４に示すように、撮像装置６０は、過去の時刻ｄ_２（第１の時刻）において、座標ｍ_２を含む画像を撮像し、過去のある時刻ｄ_１（第２の時刻）において、座標ｍ_１を含む画像を撮像し、現在時刻ｄ_０（第３の時刻）において、座標ｍ_０を含む画像を撮像する。なお、図４において、例えば、時刻ｄ_２の実際の画像は、撮像装置６０が撮像して出力するまでに１フレームの遅延がある場合、時刻ｄ_３の時に撮像された画像である。
そして、過去の時刻ｄ_２から現在時刻ｄ_０における回転分がＲ_２０、並進分がｔ_２０である。同様に、過去の時刻ｄ_１から現在時刻ｄ_０における回転分がＲ_１０、並進分がベクトルｔ_１０である。同様に、過去の時刻ｄ_２から過去の時刻ｄ_１における回転分がＲ_２１、並進分がｔ_２１である。

また、過去の時刻ｄ_２で撮像された画像Ｉ_２は、座標ｍ_２ｉ（ｉ＝１、・・・、Ｎ）の集まりである。同様に、過去の時刻ｄ_１で撮像された画像Ｉ_１は、座標ｍ_１ｉ（ｉ＝１、・・・、Ｎ）の集まりである。同様に、現在時刻ｄ_０で撮像された画像Ｉ_０は、座標ｍ_１ｉ（ｉ＝１、・・・、Ｎ）の集まりである。
時刻ｄ_２とｄ_１におけるエピポーラ拘束の関係は、次式（１）のように表される。同様に、時刻ｄ_１とｄ_０におけるエピポーラ拘束の関係は、次式（２）のように表される。同様に、時刻ｄ_２とｄ_０におけるエピポーラ拘束の関係は、次式（３）のように表される。なお、エピポーラ拘束とは、基準カメラに写ったある点がもう一方の画像上のある直線（エピポーラライン）上に射影され、異なる視点から得られた２つの画像の対応する点同士が満たす条件である。

現在時刻ｄ_０の画像Ｉ_０の座標ｍ_０は、次式（４）により推定できる。

図５は、本実施形態に係る撮像装置６０が撮像するタイミングとエンコーダー情報が取得されるタイミングを説明する図である。
図５に示すように、撮像装置６０の撮影タイミングが３０ｆｐｓ（フレーム／秒）の場合、撮像装置６０が撮像した画像は、３３［ｍｓｅｃ］毎に更新される。図５において、時刻ｄ_２、ｄ_１、ｄ_０は、撮像装置６０が撮像するタイミングであり、３３［ｍｓｅｃ］間隔である。一方、エンコーダー９０が生成するロボット１の手先位置・姿勢を示すエンコーダー情報は、例えば１［ｍｓｅｃ］毎に取得される。図５において、時刻ｄｅ_１、ｄｅ_２、・・・は、エンコーダー情報が取得されるタイミングであり、１［ｍｓｅｃ］間隔である。また、時刻ｄ_０は現在時刻であり、時刻ｄ_１は時刻ｔ_０より３３［ｍｓｅｃ］前、時刻ｄ_２は、時刻ｄ_１より３３［ｍｓｅｃ］前である。

次に、現在時刻ｄ_０の画像を予測する手順の概略について、図面を用いて説明する。
図６は、本実施形態に係るアーム２の手先位置・姿勢を説明する図である。図７は、本実施形態に係る各時刻におけるパラメーター、アーム２の相対移動量、画像の関係を説明する図である。
図６において、アーム部２０を簡略化して示している。図６、図７に示すように、時刻ｄ_２において、撮像装置６０を備えるアーム２の手先位置・姿勢は、ロボット座標（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）の位置・姿勢にある。この時刻ｄ_２において、撮像装置６０は、対象物体７０の画像Ｉ_２を撮像する。なお、推定した画像Ｉ_０を、以下、推定画像Ｉ_０’という。同様に、推定した画像Ｉ_１を、推定画像Ｉ_１'、推定した画像Ｉ_２を、推定画像Ｉ_２'、推定した画像Ｉ_３を、推定画像Ｉ_３'という。

次に、時刻ｄ_１において、アーム２の手先位置・姿勢は、ロボット座標（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２） から（ｘ_１，ｙ_１，ｚ_１，α_１，β_１，γ_１）の位置・姿勢に移動する。時刻ｄ_２からｄ_１において、アーム２の手先位置・姿勢の相対移動量は、回転分がＲ_２１、並進分がｔ_２１である。この場合、時刻ｄ_２からｄ_１における基礎行列Ｆ_２１は、次式（５）のように表される。

この時刻ｄ_１において、撮像装置６０は、対象物体７０の画像Ｉ_１を撮像する。
次に、時刻ｄ_０において、アーム２の手先位置・姿勢は、ロボット座標（ｘ_１，ｙ_１，ｚ_１，α_１，β_１，γ_１） から（ｘ_０，ｙ_０，ｚ_０，α_０，β_０，γ_０）の位置・姿勢に移動する。時刻ｄ_１からｄ_０において、アーム部２０の手先位置・姿勢の相対移動量は、回転分がＲ_１０、並進分がｔ_１０である。この場合、時刻ｄ_１からｄ_０における基礎行列Ｆ_１０は、次式（６）のように表される。

式（５）と（６）において、符号Ｔは、並進を表す行列であり、次式（７）のように表される。なお、並進ベクトルｔは、［ｔ_ｘ ｔ_ｙ ｔ_ｚ］^Ｔ（Ｔは転置行列を表す）である。また、行列Ａは、撮像装置６０の内部パラメーターを表す行列であり、画像中心座標、直交性、縦軸と横軸のスケール、焦点距離などにより表される、例えば３行３列の行列である。

そして、制御部１００の画像推定部１０９は、これらの基礎行列（ｆｕｎｄａｍｅｎｔａｌ ｍａｔｒｉｘ）Ｆ_２１（式（５））、Ｆ_１０（式（６））、撮像された画像Ｉ_２、およびＩ_１を用いて、現在時刻ｄ_０の画像Ｉ_０を推定する。なお、基礎行列とは、２つの画像の対応する点の座標上の関係（写像関係）を示す行列である。

なお、従来技術では、各時刻におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ、並進を表す行列Ｔ、及び撮像装置６０の内部パラメーターを表す行列Ａにキャリブレーション誤差がなく、正確な場合を想定している。しかしながら、実際には、各行列には、ロボット１のロボットキャリブレーション誤差や、カメラキャリブレーション誤差等を有している場合がある。なお、ロボットキャリブレーション誤差とは、ロボットが有する幾何学的な誤差要因と非幾何学的な誤差要素に対してキャリブレーションを行った後の誤差である。幾何学的な誤差要因とは、ロボットの機構の要素加工時の製造誤差等であり、非幾何学的な誤差要素とは、負荷時の重力による各関節のたわみや歯車間の伝達誤差等である。また、カメラキャリブレーション誤差とは、撮像装置６０の内部パラメーターを表す行列Ａと、撮像装置６０の外部パラメーターを表す行列を決定することである。なお、撮像装置６０の外部パラメーターとは、並進ベクトルｔ、回転を表す行列Ｒを要素に持つ、カメラ座標系でのベクトルをワールド座標系に変換する同次行列である。また、撮像装置６０が撮像してから、出力されるまでの遅れ時間による誤差がある。このような場合、誤差を考慮せずに現在時刻の画像データを推定してしまうと、推定した画像データに誤差を含んでしまうため、精度良くアーム２の制御ができない。これらの誤差を考慮して、現在時刻の画像データを推定するには、回転を表す行列Ｒ、並進を表す行列Ｔ、及び撮像装置６０の内部パラメーターを表す行列Ａの各行列の誤差（ΔＲ、ΔＴ、ΔＡ）を算出することで補正できる。誤差は、例えば、現在時刻ｄ_０より前の過去の時刻ｄ_１に取得された画像データと過去の時刻ｄ_１の、推定された画像データ間の誤差を算出することで求められる。この場合、推定された画像データと、実際に取得された画像データとが一致するような最小化問題を、例えばニュートン法などを用いて解くことで、誤差情報を算出する。

次に、本実施形態における時刻ｄ_０における画像Ｉ_０に含まれる誤差ε_０の算出方法について説明する。誤差ε_０は、時刻ｄ_０における（回転を表す行列Ｒ、並進を表す行列Ｔ、撮像装置６０の内部パラメーターを表す行列Ａ）の誤差として、次式（８）のように表される。
なお、式（８）において、ベクトルｘ_１は、ワールド座標における時刻ｄ_１におけるアーム２の手先位置・姿勢、ベクトルｘ_０は、時刻ｄ_０におけるアーム２の手先位置・姿勢、誤差ε_１は、時刻ｄ_１における画像Ｉ_１の誤差、誤差ε_０は、時刻ｄ_０における画像Ｉ_０の誤差である。

式（８）において、一定の誤差がある場合、すなわち、誤差が時間や位置に依存しない場合の誤差は一定値とみなせる。このため、誤差の偏微分成分である次式（９）は、０と見なせるため、時刻ｄ_０における誤差ε_０は、次式（１０）のように、時刻ｄ_１における誤差ε_１と等しい。

また、式（８）において、誤差が時間や位置に依存する場合、式（９）を、過去の時系列データから求めるようにしてもよい。
時刻ｄ_１における画像Ｉ_１の誤差ε_１は、時刻ｄ_１に取得した画像Ｉ_１と、時刻ｄ_１に取得される画像を推定した推定画像（以下、時刻ｄ_１の推定画像という）Ｉ_１’との差を、ニュートン法などを用いて最小化問題を解くことで算出する。例えば、画像Ｉ_１から、推定した推定画像Ｉ_１ ^’にΔＲ、ΔＴ、ΔＡを加えて、推定画像Ｉ_１’を更新した推定画像を減算し、減算結果が最小になるΔＲ、ΔＴ、ΔＡを算出することで、誤差ε_１（ΔＲ_１、ΔＴ_１、ΔＡ_１）を算出する。
時刻ｄ_１の推定画像Ｉ_１’は、上述したように式（４）に基づき、次式（１１）を用いて、各画素を算出することで推定する。ただし、この場合、推定された推定画像Ｉ_１’は、誤差を有している。なお、撮像装置６０の内部パラメーターを表す行列Ａの各パラメーターは、予めキャリブレーション処理により算出しておく。

式（１１）において、座標ｍ_２は、画像Ｉ_２の各座標、座標ｍ_３は、画像Ｉ_３の各座標である。また、基礎行列Ｆ_２１は、時刻ｄ_２から時刻ｄ_１に移動した場合の基礎行列、基礎行列Ｆ_３１は、時刻ｄ_３から時刻ｄ_１に移動した場合の基礎行列である。すなわち、推定画像Ｉ_１’は、画像Ｉ_３、Ｉ_２、基礎行列Ｆ_３１、及び基礎行列Ｆ_２１を用いて推定する。

次に、現在時刻ｄ_０の画像Ｉ_０を推定する手順について、図８のフローチャートを用いて説明する。なお、本実施形態では、撮像装置６０が画像データを撮像してから、撮像された画像データを画像取得部１０３が取得するまで、１フレームの遅延量があるとして説明する。

（ステップＳ１）制御部１００のアーム制御部１０１は、撮像装置６０が取り付けられているアーム２の手先位置・姿勢を（ｘ_３，ｙ_３，ｚ_３，α_３，β_３，γ_３）から（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）に移動させる駆動信号を生成し、駆動部１０２を介してロボット１のアーム２を駆動する。また、アーム制御部１０１は、生成した駆動信号に基づく情報を位置姿勢算出部１０８に出力する。なお、駆動信号に基づく情報とは、例えば、アーム２の手先位置・姿勢（ｘ_３，ｙ_３，ｚ_３，α_３，β_３，γ_３）とアーム２の手先位置・姿勢（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）を示す情報である。
この駆動信号により、アーム２の手先位置・姿勢は、時刻ｄ_３の時に（ｘ_３，ｙ_３，ｚ_３，α_３，β_３，γ_３）に移動し、時刻ｄ_２の時に（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）に移動する。また、アーム２の手先位置・姿勢は、時刻ｄ_１の時に（ｘ_１，ｙ_１，ｚ_１，α_１，β_１，γ_１）に移動し、時刻ｄ_０の時に（ｘ_０，ｙ_０，ｚ_０，α_０，β_０，γ_０）に移動する。そして、時刻ｄ_３〜ｄ_０の各間隔は、撮像装置６０のフレームレートであり、例えば３３［ｍｓｅｃ］である。ステップＳ１終了後、ステップＳ２に進む。

（ステップＳ２）次に、時刻ｄ_２（第２の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ_３を撮像装置６０から取得する。取得した画像Ｉ_３は、上述したように内部処理による遅れにより、画像を取得した時刻ｄ_２に対して、１フレーム分の３３［ｍｓｅｃ］遅れている。このため、取得した画像Ｉ_３は、時刻ｄ_２より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ_３（第３の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ_３を取得したときの時刻ｄ_２をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ_３をアナログ信号からデジタル信号に変換し、変換した画像Ｉ_３と取得した時刻ｄ_２とを画像処理部１０５に出力する。ステップＳ２終了後、ステップＳ３に進む。

（ステップＳ３）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ_３に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、例えば、２値化処理により対象物７０の輪郭を抽出し、エッジ検出により特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ_２に基づき、実際の画像を撮像した時刻ｄ_３（＝ｄ_２−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_３とを関連づけて、記憶部１０６に記憶させる。ステップＳ３終了後、ステップＳ４に進む。

（ステップＳ４）時刻ｄ_２の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ４終了後、ステップＳ５に進む。

（ステップＳ５）次に、時刻ｄ_１（第３の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ_２を撮像装置６０から取得する。取得した画像Ｉ_２は、時刻ｄ_１より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ_２（第２の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ_２を取得したときの時刻ｄ_１をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ_２をアナログ信号からデジタル信号に変換し、変換した画像Ｉ_２と取得した時刻ｄ_１とを画像処理部１０５に出力する。ステップＳ５終了後、ステップＳ６に進む。

（ステップＳ６）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ_２に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ_１に基づき、実際の画像を撮像した時刻ｄ_２（＝ｄ_１−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_２とを関連づけて、記憶部１０６に記憶させる。ステップＳ６終了後、ステップＳ７に進む。

（ステップＳ７）時刻ｄ_１の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ７終了後、ステップＳ８に進む。

（ステップＳ８）次に、時刻ｄ_０（第４の時刻）の時、画像取得部１０３は、対象物７０を撮像した画像Ｉ_１を撮像装置６０から取得する。取得した画像Ｉ_１は、時刻ｄ_０より３３［ｍｓｅｃ］前の位置の画像、すなわち、時刻ｄ_１（第３の時刻）の時の画像である。
また、画像取得部１０３は、画像Ｉ_１を取得したときの時刻ｄ_０をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ_２をアナログ信号からデジタル信号に変換し、変換した画像Ｉ_１と取得した時刻ｄ_０とを画像処理部１０５に出力する。ステップＳ８終了後、ステップＳ９に進む。

（ステップＳ９）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ_１に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ_０から画像の遅れに基づく時間３３［ｍｓｅｃ］を減算して、実際の画像を撮像した時刻ｄ_１（＝ｄ_０−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_１とを関連づけて、記憶部１０６に記憶させる。ステップＳ９終了後、ステップＳ１０に進む。

（ステップＳ１０）時刻ｄ_０の時、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ１０終了後、ステップＳ１１に進む。

（ステップＳ１１）次に、位置姿勢算出部１０８は、時刻ｄ_３から時刻ｄ_１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_３１と並進を表す行列Ｔ_３１を、時刻ｄ_３とｄ_１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_３１と並進を表す行列Ｔ_３１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_３１を上述した式（５）においてＲ_２１をＲ_３１、Ｔ_２１をＴ_３１に置き換えて算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ_３１を画像推定部１０９に出力する。
次に、位置姿勢算出部１０８は、時刻ｄ_２から時刻ｄ_１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２１と並進を表す行列Ｔ_２１を、時刻ｄ_２とｄ_１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_２１と並進を表す行列Ｔ_２１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_２１を、上述した式（５）により算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ_２１を画像推定部１０９に出力する。
ステップＳ１１終了後、ステップＳ１２に進む。

（ステップＳ１２；第１画像推定手順）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ_３とＩ_２の画像データを読み出す。画像推定部１０９は、位置姿勢算出部１０８が出力する基礎行列Ｆ_３１と基礎行列Ｆ_２１、記憶部１０６から読み出した画像Ｉ_３とＩ_２の画像データを用いて、上述した式（１１）により時刻ｄ_１の推定画像Ｉ_１’を推定する。なお、式（１１）において、画像情報ｍ_１は、時刻ｄ_１の推定画像Ｉ_１’の画像情報であり、画像情報ｍ_２は、時刻ｄ_１で取得された画像Ｉ_２の画像情報であり、画像情報ｍ_３は、時刻ｄ_２で取得された画像Ｉ_３の画像情報である。画像推定部１０９は、時刻ｄ_０で実測された画像Ｉ_１と式（１１）で推定した推定画像Ｉ_１’を位置姿勢算出部１０８に出力する。ステップＳ１２終了後、ステップＳ１３に進む。

（ステップＳ１３）次に、位置姿勢算出部１０８は、画像推定部１０９が出力する実測された画像Ｉ_１と推定された推定画像Ｉ_１’を用いて、例えば最小化問題を解いて誤差ε_１である（ΔＲ_１、ΔＴ_１、ΔＡ_１）を算出する。ステップＳ１３終了後、ステップＳ１４に進む。

（ステップＳ１４）次に、位置姿勢算出部１０８は、時刻ｄ_１から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_１０と並進を表す行列Ｔ_１０を、時刻ｄ_１とｄ_０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_１０と並進を表す行列Ｔ_１０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_１０を、上述した式（６）により算出する。
ステップＳ１４終了後、ステップＳ１５に進む。

（ステップＳ１５）次に、位置姿勢算出部１０８は、時刻ｄ_２から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２０と並進を表す行列Ｔ_２０を、時刻ｄ_２とｄ_０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。
次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_２０と並進を表す行列Ｔ_２０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_２０を、次式（１２）により算出する。
ステップＳ１５終了後、ステップＳ１６に進む。

（ステップＳ１６）次に、誤差が時間や位置に依存しない場合、位置姿勢算出部１０８は、上述した式（１０）により、誤差ε_０をステップＳ１２で算出した誤差ε_１として算出し、算出した誤差ε_０（ΔＲ_０、ΔＴ_０、ΔＡ_０）を画像推定部１０９に出力する。ステップＳ１６終了後、ステップＳ１７に進む。

（ステップＳ１７；第２画像推定手順）次に、画像推定部１０９は、記憶部１０６に記憶されている時刻ｄ_１の時の画像Ｉ_２と、時刻ｄ_０の時のＩ_１を読み出す。なお、画像情報ｍ_０は、時刻ｄ_０の推定される推定画像Ｉ_０’の画像情報であり、画像情報ｍ_１は、時刻ｄ_０で取得された画像Ｉ_１の画像情報であり、画像情報ｍ_２は、時刻ｄ_１で取得された画像Ｉ_２の画像情報である。
次に、画像推定部１０９は、ステップＳ１６で算出した誤差ε_０（ΔＲ_０、ΔＴ_０、ΔＡ_０）を各基礎行列Ｆ_１０、Ｆ_２０に加算して、各基礎行列Ｆ_１０、Ｆ_２０を更新する。なお、回転行列Ｒについては、誤差ΔＲ_０を乗じることで更新する。ステップＳ１７終了後、ステップＳ１８に進む。

（ステップＳ１８；第２画像推定手順）次に、画像推定部１０９は、読み出した画像Ｉ_１、Ｉ_２の各画素の情報ｍ_１、ｍ_２と、更新した基礎行列Ｆ_１０、Ｆ_２０を用いて、次式（１３）により、時刻ｄ_０の画像ｍ_０を推定する。

全ての画素の情報について算出することで、画像推定部１０９は、時刻ｄ_０の推定画像Ｉ_０’を推定する。
以上により、時刻ｄ_０の画像Ｉ_０の推定を終了する。
このようにして推定された現在時刻ｄ_０の推定画像Ｉ_０’を用いて、制御部１００が、ビジュアルサーボを行う。

なお、ステップＳ１８では、時刻ｄ_２、ｄ_１に撮像された画像Ｉ_３、Ｉ_２を用いて、現在時刻ｄ_０の画像Ｉ_０を推定する方法を説明した。撮像装置６０の遅れ時間が長く、画像処理により最低でも２フレームの遅れ時間が発生する場合は、例えば、時刻ｄ_３（現在時刻ｄ_０より３フレーム前の時刻）に撮像された画像Ｉ_４と、時刻ｄ_２（現在時刻ｄ_０より２フレーム前の時刻）に撮像された画像Ｉ_３とを用いて、現在時刻ｄ_０の画像Ｉ_０を推定する。この結果、現在時刻ｄ_０における画像Ｉ_０が直接得られなくても、誤差を含めて精度良く画像Ｉ_０を推定できるので、遅れ時間があっても精度良く制御できる。
また、遅れ時間が１フレームの場合であっても、時刻ｄ_３（現在時刻ｄ_０より３フレーム前の時刻）、ｄ_２（現在時刻ｄ_０より２フレーム前の時刻）に撮像された画像Ｉ_４、Ｉ_３を用いて、現在時刻ｄ_０の画像Ｉ_０を推定するようにしてもよい。
また、なお、本実施形態では、ステップＳ１８とＳ１９で、時刻ｄ_０の画像ｍ_０を推定した後、推定した推定画像Ｉ_０’に誤差を加算して推定画像Ｉ_０’を更新する例を説明した。しかしながら、ステップＳ１７で算出した誤差ε_０（ΔＲ_０、ΔＴ_０、ΔＡ_０）を基礎行列Ｆ_１０に加算して基礎行列Ｆ_１０を更新し、更新したＦ_１０を用いて式（１３）により時刻ｄ_０の推定画像Ｉ_０’を算出するようにしてもよい。

以上のように、本実施形態では、画像推定部１０９は、現在時刻ｄ_０より１フレーム前の時刻ｄ_１の推定画像Ｉ_１’を推定する。そして、位置姿勢算出部１０８は、現在時刻ｄ_０より１フレーム前の時刻ｄ_１に取得された画像Ｉ_０と推定画像Ｉ_１’との誤差ε_０を誤差ε_１として算出する。そして、位置姿勢算出部１０８は、時刻ｄ_２から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２０、並進を表す行列Ｔ_２０、撮像装置６０の内部パラメーターを表す行列Ａを用いて、基礎行列Ｆ_２０を算出する。また、位置姿勢算出部１０８は、時刻ｄ_１から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２０、並進を表す行列Ｔ_２０、撮像装置６０の内部パラメーターを表す行列Ａを用いて、基礎行列Ｆ_２０を算出する。そして、画像推定部１０９は、現在時刻ｄ_０より２フレーム前の時刻ｄ_２に撮像された画像Ｉ_３、現在時刻ｄ_０より１フレーム前の時刻ｄ_１に撮像された画像Ｉ_２、誤差を考慮した基礎行列Ｆ_２０、基礎行列Ｆ_１０を用いて、現在時刻ｄ_０の推定画像Ｉ_０’を推定する。そして、推定した推定画像Ｉ_０’に算出した誤差ε_０を加算して、推定画像Ｉ_０’を更新する。
この結果、撮像装置６０による遅れ誤差、ロボットキャリブレーション誤差、及びカメラキャリブレーション誤差を考慮した現在時刻ｄ_０における推定画像Ｉ_０’を精度良く推測できる。そして、この推定された画像を用いてロボット１を制御できるため、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度の良いビジュアルサーボを行うことができる。

［第２実施形態］
本実施形態では、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報が取得できるタイミング毎に画像を推定する。
本実施形態に係る現在時刻ｄ_０の画像Ｉ_０を推定する手順について、図９のフローチャートを用いて説明する。エンコーダー９０からの出力は、１［ｍｓｅｃ］毎に取得できるとして、以下の説明をする。

（ステップＳ１０１）まず、制御部１００のアーム制御部１０１は、アーム２の手先位置・姿勢を（ｘ_３，ｙ_３，ｚ_３，α_３，β_３，γ_３）から（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）に移動させる駆動信号を生成し、駆動部１０２を介してロボット１のアーム２を駆動する。また、アーム制御部１０１は、生成した駆動信号に基づく情報を位置姿勢算出部１０８に出力する。
この駆動信号により、アーム２の手先位置・姿勢は、時刻ｄ_３の時に（ｘ_３，ｙ_３，ｚ_３，α_３，β_３，γ_３）に移動し、時刻ｄ_２の時に（ｘ_２，ｙ_２，ｚ_２，α_２，β_２，γ_２）に移動する。また、アーム２の手先位置・姿勢は、時刻ｄ_１の時に（ｘ_１，ｙ_１，ｚ_１，α_１，β_１，γ_１）に移動し、時刻ｄ_０の時に（ｘ_０，ｙ_０，ｚ_０，α_０，β_０，γ_０）に移動する。そして、時刻ｄ_３〜ｄ_０の間隔は、撮像装置６０のフレームレートであり、例えば３３［ｍｓｅｃ］である。ステップＳ１０１終了後、ステップＳ１０２に進む。

（ステップＳ１０２）次に、位置情報取得部１０７は、エンコーダー９０が出力するアーム２の手先位置・姿勢を示す情報を１［ｍｓｅｃ］毎に取得し、取得したアーム２の手先位置・姿勢を示す情報を位置姿勢算出部１０８に出力する。ステップＳ１０２終了後、ステップＳ１０３に進む。

（ステップＳ１０３）次に、画像取得部１０３は、クロック発生部１０４が出力するクロック信号に基づき、３３［ｍｓｅｃ］経過したか否かを判定する。
３３［ｍｓｅｃ］経過した場合（ステップＳ１０３；Ｙｅｓ）、ステップＳ１０４に進む。３３［ｍｓｅｃ］経過していない場合（ステップＳ１０３；Ｎｏ）、ステップＳ１０６に進む。

（ステップＳ１０４）画像取得部１０３は、時刻ｄ_２に画像Ｉ_３を撮像装置６０から取得する。また、画像取得部１０３は、画像Ｉ_３を取得したときの時刻ｄ_２をクロック発生部１０４から取得する。次に、画像取得部１０３は、画像Ｉ_３をアナログ信号からデジタル信号に変換し、変換した画像Ｉ_３と取得した時刻ｄ_２とを画像処理部１０５に出力する。ステップＳ１０４終了後、ステップＳ１０５に進む。

（ステップＳ１０５）画像処理部１０５は、画像取得部１０３が出力する画像Ｉ_３に対して画像処理を行い、特徴量を抽出する。画像処理部１０５は、例えば、２値化処理により対象物７０の輪郭を抽出し、エッジ検出により特徴量を抽出する。画像処理部１０５は、画像取得部１０３が出力する時刻ｄ_２から画像の遅れに基づく時間３３［ｍｓｅｃ］を減算して、実際の画像を撮像した時刻ｄ_３（＝ｄ_２−３３［ｍｓｅｃ］）を算出する。画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_３とを関連づけて、記憶部１０６に記憶させる。ステップＳ１０５終了後、ステップＳ１０６に進む。

（ステップＳ１０６）次に、画像推定部１０９は、記憶部１０６に画像データが３つ以上記憶されているか否かを判別する。
記憶部１０６に画像データが３つ以上記憶されている場合（ステップＳ１０６；Ｙｅｓ）、ステップＳ１０７に進む。記憶部１０６に画像データが３つ以上記憶されていない場合（ステップＳ１０６；Ｎｏ）、ステップＳ１０２に戻る。

この場合、記憶部１０６には、まだ画像Ｉ_３しか記憶されていないため、ステップＳ１０２に戻り、ステップＳ１０２〜Ｓ１０６を繰り返す。そして、ステップＳ１０４において、画像取得部１０３は、対象物７０を撮像した時刻ｄ_１における画像Ｉ_２を撮像装置６０から取得する。ステップＳ１０５において、画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_２とを関連づけて、記憶部１０６に記憶させる。さらに、ステップＳ１０４において、画像取得部１０３は、対象物７０を撮像した時刻ｄ_０における画像Ｉ_１を撮像装置６０から取得する。ステップＳ１０５において、画像処理部１０５は、抽出した特徴量と算出した時刻ｄ_０とを関連づけて、記憶部１０６に記憶させる。

（ステップＳ１０７）次に、位置姿勢算出部１０８は、時刻ｄ_３から時刻ｄ_１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_３１と並進を表す行列Ｔ_３１を、時刻ｄ_３とｄ_１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_３１と並進を表す行列Ｔ_３１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_３１を上述した式（５）においてＲ_２１をＲ_３１、Ｔ_２１をＴ_３１に置き換えて算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ_３１を画像推定部１０９に出力する。
次に、位置姿勢算出部１０８は、時刻ｄ_２から時刻ｄ_１におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２１と並進を表す行列Ｔ_２１を、時刻ｄ_２とｄ_１の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_２１と並進を表す行列Ｔ_２１、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_２１を、上述した式（５）により算出する。位置姿勢算出部１０８は、算出した基礎行列Ｆ_２１を画像推定部１０９に出力する。
ステップＳ１０７終了後、ステップＳ１０８に進む。

（ステップＳ１０８）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ_３とＩ_２の画像データを読み出す。画像推定部１０９は、位置姿勢算出部１０８が出力する基礎行列Ｆ_２１と基礎行列Ｆ_３１、記憶部１０６から読み出した画像Ｉ_３とＩ_２の画像データを用いて、上述した式（１１）により時刻ｄ_１の推定画像Ｉ_１’を推定する。画像推定部１０９は、実測された画像Ｉ_１と推定した推定画像Ｉ_１’を位置姿勢算出部１０８に出力する。ステップＳ１０終了後、ステップＳ１０９に進む。

（ステップＳ１０９）次に、位置姿勢算出部１０８は、画像推定部１０９が出力する実測された画像Ｉ_１と推定された推定画像Ｉ_１’を用いて、例えば最小化問題を解いて誤差ε_１である（ΔＲ_１、ΔＴ_１、ΔＡ_１）を算出する。ステップＳ１０９終了後、ステップＳ１１０に進む。

（ステップＳ１１０）次に、位置姿勢算出部１０８は、時刻ｄ_１から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転Ｒ_１０と並進を表す行列Ｔ_１０を、時刻ｄ_１とｄ_０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_１０と並進を表す行列Ｔ_１０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_１０を、上述した式（６）により算出する。
ステップＳ１１０終了後、ステップＳ１１１に進む。

（ステップＳ１１１）次に、位置姿勢算出部１０８は、時刻ｄ_２から時刻ｄ_０におけるアーム２の手先位置・姿勢の回転を表す行列Ｒ_２０と並進を表す行列Ｔ_２０を、時刻ｄ_２とｄ_０の時のアーム制御部１０１が出力する駆動信号に基づく情報と、位置情報取得部１０７が出力するアーム２の手先位置・姿勢を示す情報とを用いて算出する。次に、位置姿勢算出部１０８は、算出した回転を表す行列Ｒ_２０と並進を表す行列Ｔ_２０、及び撮像装置６０の内部パラメーターを表す行列Ａを用いて基礎行列Ｆ_２０を、上述した式（１２）により算出する。
ステップＳ１１１終了後、ステップＳ１１２に進む。

（ステップＳ１１２）次に、誤差が時間や位置に依存しない場合、位置姿勢算出部１０８は、上述した式（１０）により、誤差ε_０（ΔＲ_０、ΔＴ_０、ΔＡ_０）をステップＳ１０９で算出した誤差ε_１（ΔＲ_１、ΔＴ_１、ΔＡ_１）として算出する。ステップＳ１１２終了後、ステップＳ１１３に進む。

（ステップＳ１１３）次に、画像推定部１０９は、記憶部１０６に記憶されている画像Ｉ_２、Ｉ_１を読み出す。
次に、画像推定部１０９は、ステップＳ１１２で算出した誤差ε_０（ΔＲ_０、ΔＴ_０、ΔＡ_０）を各基礎行列Ｆ_１０、Ｆ_２０に加算して、各基礎行列Ｆ_１０、Ｆ_２０を更新する。なお、回転行列Ｒについては、誤差ΔＲ_０を乗じることで更新する。ステップＳ１１３終了後、ステップＳ１１４に進む。

（ステップＳ１１４）次に、画像推定部１０９は、読み出した画像Ｉ_２、Ｉ_１の各画素の情報ｍ_２、ｍ_１と、更新した基礎行列Ｆ_１０、Ｆ_２０を用いて、上述した式（１３）により、時刻ｄ_０の画像ｍ_０を推定する。
全ての画素の情報について算出することで、画像推定部１０９は、時刻ｄ_０の推定画像Ｉ_０’を推定する。

ステップＳ１１４終了後、ステップＳ１０２に戻る。
このようにして推定された現在時刻ｄ_０の推定画像Ｉ_０’を用いて、制御部１００が、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎に、ビジュアルサーボを行う。

本実施形態によれば、位置姿勢算出部１０８は、撮像装置６０のフレームレートが３３［ｍｓｅｃ］の場合、撮像装置６０から取得できる画像データを３３［ｍｓｅｃ］でしか得られなくても、現在時刻より前に取得した画像データ、アーム２の手先位置・姿勢を示す情報に基づき、アーム２の手先位置・姿勢の誤差を算出できる。そして、画像推定部１０９は、現在時刻より前の時刻ｄ_１に取得した画像データと時刻ｄ_１の推定画像Ｉ_１’との誤差ε_１を現在時刻ｄ_０の誤差ε_０として算出する。そして、算出した誤差を用いて、現在時刻の画像を推定するようにしたので、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎（例えば、１［ｍｓｅｃ］毎）に、ロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良くビジュアルサーボを行うことができる。
例えば、低いフレームレートの撮像装置６０を用いたロボット１では、ロボット１の制御におけるサーボループのゲインを上げないと高速に制御できない。しかしながら、低いフレームレートの撮像装置６０を用いたロボット１では、サーボループのゲインを上げると振動が発生するため、サーボループのゲインを上げられず、このためロボット１を高速に制御することができない。また、高速な制御を行うために高いフレームレートの撮像装置６０を用いた場合、撮像装置６０が高価であるため、ロボット１のコストアップになってしまう。
本実施形態によれば、低いフレームレートの撮像措置６０を用いた場合でも、撮像装置６０のフレームレートより速い間隔で、予測した画像を用いてロボット１を制御することができる効果がある。

なお、本実施形態では、ステップＳ１０３で、画像取得部１０３が、クロック発生部１０４が出力するクロック信号に基づき、３３［ｍｓｅｃ］毎に撮像された画像データを取得する例を説明したが、撮像装置６０が撮像した画像データを出力するタイミング毎に画像データを取得するようにしてもよい。
また、本実施形態では、エンコーダー９０がアーム２の手先位置・姿勢を示す情報を出力するタイミング毎（例えば、１［ｍｓｅｃ］毎）に現在時刻の画像を推定する例を説明したが、画像を推定する間隔は、制御する用途に応じて任意でもよい。

［第３実施形態］
図１０は、本実施形態に係るロボット１ａの概略構成を示す斜視図である。図１０に示すように、ロボット１ａは、２本のアーム２−１と２−２を備えている。アーム２−１と２−２は、本体部５００に連結されている。また、本体部５００は、固定部１０ａに連結されている。
アーム２−１は、アーム部２０−１、及びエンドエフェクター４０−１を備える。アーム２−２は、アーム部２０−２、力センサー３０−２、及びエンドエフェクター４０−２を備える。また、ロボット１は、制御部１００ａの制御により動作する。

撮像装置６０は、エンドエフェクター４０−１の先端部に取り付けられている。撮像装置６０は、対象物を撮影し、撮影したアナログ値の画像データを制御装置１００ａに出力する。

アーム部２０−１は、第１実施形態の構成と同様である。
アーム部２０−２は、第１実施形態と同様に第１リンク２１−２、第２リンク２２−２、第３リンク２３−２、第４リンク２４−２および第５リンク２５−２がこの順に連結されたものである。アーム部２０−２の第５リンク２５−２の他方端には、エンドエフェクター４０−２が設けられている。アーム部２０−２の第５リンク２５−２とエンドエフェクター４０−２の間には、力センサー３０−２が介挿されている。力センサー３０−２は、検出したアーム２−２のエンドエフェクター４０−２に加わる力成分とトルク成分を含む情報を制御部１００ａに出力する。なお、エンドエフェクター４０−２は、例えば把持部（ハンド部）である。

図１１は、本実施形態に係るアーム部２０−１及び２０−２と力センサーとエンコーダーの概念図である。
図１１に示すように、ロボット１ａは、各アーム２−１、２−２がそれぞれ６自由度を有し、さらに本体部５００が１自由度を有している。図１１に示すように、アーム２−１と同様に、アーム２−１のアーム部２０−１は、回転部８２−１、８３−１及び８５−１と、屈折部８１−１、８４−１及び８６−１とを有している。そして、アーム２−１のアーム部２０−１は、エンドエフェクター４０−１との間に力センサー３０を有している。

また、図１１に示すように、第１実施形態と同様に回転部８２−１、８３−１及び８５−１と、屈折部８１−１、８４−１及び８６−１は、各々エンコーダー９１−１〜９６−１を備えている。また、図１１に示すように、アーム２−１と同様に、アーム２−２のアーム部２０−２は、回転部８２−２、８３−２及び８５−２と、屈折部８１−２、８４−２及び８６−２は、各々エンコーダー９１−２〜９６−２を備えている。

本実施形態では、アーム２−１に取り付けられている撮像装置６０により、ワーク７２の上に置かれている対象物７０を撮像して、制御部１００ａが、第１実施形態と同様に現在時刻の画像データを推定する。そして、制御部１００ａは、推定した画像データを用いて、アーム２−２のエンドエフェクター４０−２のハンドが把持している物体の位置を制御する。制御部１００ａは、例えば、対象物７０に把持されている部品を組み付けるように、推定された画像データを用いてアーム２−２を制御する。
この結果、本実施形態のロボット１は、第１実施形態と同様に、撮像装置６０が出力する画像データに遅れ時間があり現在時刻の画像データが取得できず、さらにロボットキャリブレーション誤差やカメラキャリブレーション誤差等があっても精度良くアームを制御して例えば部品の取り付け等の作業を、精度良く行うことができる。また、第２実施形態の処理方法を組み合わせることで、本実施形態のロボット１は、撮像装置６０のフレームレートより高いレートで画像データの推定を行うことで、撮像装置６０が低いフレームレートであっても、高速でアームを精度良く制御することができる。

なお、本実施形態では、撮像装置が撮像してから撮像した画像を出力するまで、１フレームの遅れがある例を説明した。しかしながら、撮像装置のフレームレートが高速であり、撮像してから撮像された画像が出力されるまでの遅れ時間による誤差がほとんど無い場合でも、上述したようにロボットキャリブレーション誤差やカメラキャリブレーション誤差が存在する。本実施形態のロボットによれば、このような場合でも、異なる時刻に撮像された画像間の誤差を算出して、算出した誤差を用いて、現在時刻の画像を推定することで、ロボットキャリブレーション誤差やカメラキャリブレーション誤差があっても、それらを補正して現在時刻の画像を精度良く推定することができる。この結果、推定された画像を用いて精度良くビジュアルサーボを行うことができる。

なお、実施形態の図３または図１０の制御部の各部の機能を実現するためのプログラムをコンピューター読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピューターシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピューターシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ−ＲＯＭ等の可搬媒体、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ） Ｉ／Ｆ（インタフェース）を介して接続されるＵＳＢメモリー、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、サーバーやクライアントとなるコンピューターシステム内部の揮発性メモリーのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。

１・・・ロボット １０・・・固定部 ２０・・・アーム部
３０・・・力センサー ４０・・・エンドエフェクター ６０・・・撮像装置
７０・・・対象物 １００・・・制御部 １０１・・・アーム制御部
１０２・・・駆動部 １０３・・・画像取得部 １０４・・・クロック発生部
１０５・・・画像処理部 １０６・・・記憶部 １０７・・・位置情報取得部
１０８・・・位置姿勢算出部（誤差算出部） １０９・・・画像推定部
Ｆ_０１、Ｆ_０２、Ｆ_１２・・・基礎行列
Ｒ・・・回転を表す行列 Ｔ・・・並進を表す行列 ｔ・・・並進
Ａ・・・カメラ（撮像装置６０）の内部パラメーターを表す行列
ベクトルｘ_１、ベクトルｘ_２、ベクトルｘ_０・・・アームの手先位置・姿勢
ε・・・実際に取得された画像と推定された画像との誤差
Ｉ_１、Ｉ_２・・・撮像された画像 Ｉ_０’・・・現在時刻ｄ_０の推定画像
ｍ_１、ｍ_２・・・Ｉ_１、Ｉ_２の各画素

Claims (11)

1. ロボットの可動可能なアームに設けられ、撮像した画像データを出力する撮像装置と、
   第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、
   前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定部と、
   前記アームを制御するアーム制御部と、
   を備え、
   前記画像推定部は、
   前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、
   前記アーム制御部は、
   前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
   ことを特徴とするロボット。
2. 前記画像取得部は、
   前記第１の時刻に前記撮像装置が出力する第１の画像データを取得し、前記第１の時刻より後の第２の時刻に前記撮像装置が出力する第２の画像データを取得し、前記第２の時刻より後の第３の時刻に前記撮像装置が出力する第３の画像データを取得し、前記第３の時刻より後の第４の時刻に前記撮像装置が出力する第４の画像データを取得し、
   前記画像推定部は、
   前記画像取得部が取得した第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを用いて、前記第３の時刻に撮像される画像データと前記第４の時刻に撮像される画像データとを推定する
   ことを特徴とする請求項１に記載のロボット。
3. 前記アームの位置を検出し、前記検出されたアームの手先位置と姿勢を示す情報を前記画像推定部に出力するエンコーダー
   を備え、
   前記画像推定部は、
   前記第１の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、
   前記第２の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記第３の画像データが撮像された時刻における前記エンコーダーが出力する前記アームの手先位置と姿勢を示す情報と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列と、前記第２の画像データと、前記第３の画像データとに基づき前記第４の時刻に撮像される画像データを推定し、
   前記算出した画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正する
   ことを特徴とする請求項１または請求項２に記載のロボット。
4. 前記画像推定部は、
   前記第１の時刻と第２の時刻及び前記第３の時刻における前記アームの位置と姿勢に基づく第１の回転を表す行列及び第１の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第１の基礎行列を算出し、前記第２の時刻から前記第３の時刻における前記アームの位置と姿勢に基づく第２の回転を表す行列及び第２の並進を表す行列と、前記撮像装置の焦点距離を含む内部パラメーターを表す行列を用いて、第２の基礎行列とを算出し、
   前記算出した前記第１の基礎行列、前記第２の基礎行列、前記第１の画像データ及び前記第２の画像データを用いて前記第３の時刻の画像データを推定し、前記第３の時刻において、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データとの画像データ間の誤差を算出する
   ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット。
5. 前記画像推定部は、
   前記誤差が時間や前記アームの手先位置と姿勢に依存しない場合、前記推定した前記第３の時刻の画像データと前記画像取得部が取得した前記第３の時刻の画像データとの間の誤差を、前記第４の時刻における画像データに含まれる誤差として算出し、前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正する
   ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット。
6. 前記画像推定部は、
   前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記第４の時刻の画像データを推定し、
   前記アーム制御部は、
   前記画像推定部が推定した前記第４の時刻の画像データを用いて、前記撮像装置が撮像を行うフレームレートの間隔より短い周期で前記アームを制御する
   ことを特徴とする請求項１から請求項５のいずれか１項に記載のロボット。
7. 前記画像推定部は、
   前記エンコーダーが前記アーム位置を示す情報を前記画像推定部に出力する周期で前記第４の時刻の画像データを推定し、
   前記アーム制御部は、
   前記画像推定部が、前記第４の時刻の画像データを用いて、前記エンコーダーが前記アーム位置を示す情報を前記画像推定部に出力する周期で前記アームを制御する
   ことを特徴とする請求項２から請求項６のいずれか１項に記載のロボット。
8. 前記画像推定部は、
   前記第１の画像が撮像された時刻を、前記第１の画像が取得された第１の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出し、前記第２の画像が撮像された時刻を、該第２の画像が取得された第２の時刻と前記撮像装置が撮像してから画像データを出力するまでの遅延時間を用いて算出する
   ことを特徴とする請求項１から請求項７のいずれか１項に記載のロボット。
9. 可動可能なアームに設けられ、撮像した画像データを出力する撮像装置を有するロボットのアームを制御するロボット制御装置であり、
   第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得部と、
   前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻に撮像される画像データに含まれる誤差を算出する画像推定部と、
   前記アームを制御するアーム制御部と、
   を備え、
   前記画像推定部は、
   前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻の画像データを補正し、
   前記アーム制御部は、
   前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
   ことを特徴とするロボット制御装置。
10. ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御するロボット制御方法であり、
    撮像装置が、撮像して画像データを出力する撮像工程と、
    画像取得部が、第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の３つの時刻と、前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得工程と、
    画像推定部が、前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定し、前記第２の画像データ及び前記第３の画像データに基づき前記第４の時刻に撮像される画像データを推定し、前記推定した前記第３の時刻の画像データと、前記画像取得部が取得した前記第３の時刻に撮像される画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する画像推定工程と、
    アーム制御部が、前記アームを制御するアーム制御工程と、
    を含み、
    前記画像推定工程は、
    前記算出した前記画像データに含まれる誤差を用いて前記推定した第４の時刻に撮像される画像データを補正し、
    前記アーム制御工程は、
    前記画像推定部が補正した前記第４の時刻に撮像される画像データを用いて前記アームを制御する
    ことを特徴とするロボット制御方法。
11. ロボットの可動可能なアームに設けられ画像撮像して画像データを出力する撮像装置を有し、可動するアームを制御する処理をコンピューターに実行させるためのプログラムであり、
    前記撮像装置が撮像して画像データを出力する撮像手順と、
    第４の時刻より前の異なる第１の時刻と第２の時刻及び第３の３つの時刻と前記第４の時刻とに、第１の画像データと第２の画像データと第３の画像データ及び第４の画像データを取得する画像取得手順と、
    前記第１の画像データ及び前記第２の画像データに基づき前記第３の時刻に撮像される画像データを推定する第１画像推定手順と、
    前記第１画像推定手順により推定された前記第３の時刻の画像データと、前記画像取得手順により取得された前記第３の時刻の画像データとの誤差を算出し、前記算出した誤差に基づき前記第４の時刻の画像データに含まれる誤差を算出する誤差算出手順と、
    前記第２の画像データと、前記第３の画像データとに基づき前記第４の時刻に撮像される画像データを推定し、前記誤差算出手順が算出した前記画像データに含まれる誤差を用いて前記第４の時刻に撮像される画像データを補正する第２画像推定手順と、
    前記第２画像推定手順により補正された前記第４の時刻の画像データを用いて前記アームを制御するアーム制御手順と、
    をコンピューターに実行させるためのロボット制御プログラム。

Images