JP6335460B2 - ロボットシステムの制御装置及び指令値生成方法、並びにロボットシステムの制御方法 - Google Patents

Description

本発明は、複数の関節が制御されることにより駆動する多関節アーム及び視覚センサを利用するロボット装置におけるロボット校正装置及びロボット校正方法、並びにロボット装置及びロボット装置の制御方法に関する。

従来、垂直多関節アーム及びエンドエフェクタ（以下、ロボット本体という）と、これらを制御する制御装置とを備え、アームの先端部に視覚センサ（以下、カメラともいう）を取り付けたロボット装置が普及している。このロボット装置では、カメラによりワーク等の対象物を計測し、計測結果から制御装置が対象物の位置姿勢を演算し、演算して得られた対象物の位置姿勢に基づいてロボット本体の位置姿勢を制御するようになっている。

一般に、カメラが基準とする計測座標系と、ロボット本体が基準とする動作座標系とは異なっているので、カメラの計測値からロボット本体の動作目標値（指令値）を得るためには、カメラの計測値を計測座標系から動作座標系に座標変換する必要がある。この座標変換を行うためには、事前に計測座標系と動作座標系との校正作業を行い、校正値（座標変換式）を求めておく必要がある。

この校正作業の例としては、例えば、ロボット本体に複数の位置姿勢を順次取らせながら、各々の位置姿勢においてカメラにより校正用基準物を計測する所謂ハンドアイキャリブレーション方法が知られている（特許文献１及び非特許文献１参照）。この方法では、ロボット本体の各位置姿勢におけるロボット本体の先端部への指令値と、カメラからの校正用基準物の計測値との関係から、残差が最小となるように動作座標系と計測座標系との関係を求め、校正値を得ている。

特開平１０−０６３３１７号公報

Hand-Eye Calibration (Radu Horaud and Fadi Dornaika, 1995)

特許文献１に記載された座標校正方法はロボット本体の動作誤差を考慮したものではなかったが、実際にはアームでの機械誤差や撓み等に起因して動作誤差が発生する。また、動作誤差以外にも、組立誤差やレンズ歪みといったロボット本体ごとに異なる固有の誤差もある。これらの誤差は、従来の座標校正方法では十分に抑制することができず、ロボット本体の位置姿勢の制御において精度向上の妨げとなっていた。

本発明は、動作誤差等を含めたロボット本体の位置姿勢を演算することで、ロボット本体の位置姿勢の制御の精度を向上できるロボット校正装置及びロボット校正方法、並びにロボット装置及びロボット装置の制御方法を提供することを目的とする。

本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を有するロボットシステムの制御装置において、複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う処理装置を備える、ことを特徴とするロボットシステムの制御装置である。
また、本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を有するロボットシステムの制御装置において、複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う処理装置を備える、ことを特徴とするロボットシステムの制御装置である。

また、本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を備えるロボットシステムに対する指令値を生成する指令値生成方法であって、制御装置が、複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う、ことを特徴とする指令値生成方法である。
また、本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を備えるロボットシステムに対する指令値を生成する指令値生成方法であって、制御装置が、複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う、ことを特徴とする指令値生成方法である。

また、本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を備えるロボットシステムの制御方法であって、制御装置が、複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う、ことを特徴とするロボットシステムの制御方法である。

また、本発明は、複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を備えるロボットシステムの制御方法であって、制御装置が、複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、前記複数の校正用指令値に基づく前記ロボット本体の位置姿勢と、前記計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う、ことを特徴とするロボットシステムの制御方法である。

本発明では、校正用の指令値又は該指令値に基づいて制御された結果の制御結果値に基づくロボット本体の第一の位置姿勢と、その指令値に対応するロボット本体の第二の位置姿勢との差分に基づいて、校正関数を算出する。そして、本発明では、校正関数に基づいて仮の指令値を校正し、得られた校正後指令値によりロボット本体の位置姿勢を制御している。従って、本発明によれば、従来のハンドアイキャリブレーション方法と異なり、ロボット本体の動作誤差や固有の誤差を打ち消すことができるので、ロボット本体の位置姿勢の制御において精度向上を図ることができる。

本発明の第１実施形態に係るロボットシステムの概略構成を示す説明図である。 本発明の第１実施形態に係るロボットシステムで座標校正を行い、ロボット本体を制御する際の処理手順を示すフローチャートである。 本発明の第１実施形態に係るロボットシステムで校正用データを取得する際の処理手順を示すフローチャートである。 本発明の第１実施形態に係るロボットシステムで校正関数を算出する際の処理手順を示すフローチャートである。 本発明の第１実施形態に係るロボットシステムにより校正値を算出する際のロボット本体の位置姿勢を示す説明図である。 本発明の第１実施形態に係るロボットシステムにより動作誤差を算出する際のロボット本体の位置姿勢を示す説明図である。 本発明の第２実施形態に係るロボットシステムの概略構成を示す説明図である。 本発明の第２実施形態に係るロボットシステムにより校正値を算出する際のロボット本体の位置姿勢を示す説明図である。 本発明の第１実施形態に係るロボットシステムの制御装置とロボット校正装置とが別個に構成される変形例の概略構成を示す説明図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
本実施形態のロボットシステム１の説明に先立って、本明細書中での数式表現の定義について説明する。本明細書中では、ロボットシステム１を構成する多関節アーム２０、ハンド２１、カメラ３、ワーク６等について、６自由度の位置姿勢を３次元の座標系により表すと共に、任意の２つの座標系の相対位置姿勢を座標変換行列（同次変換行列）によって表現する。

まず、任意の座標系Ａから任意の座標系Ｂに向かう相対位置姿勢を表す座標変換行列を^ＡＨ_Ｂと記述し、その座標変換行列^ＡＨ_Ｂが示す相対位置姿勢を相対位置姿勢^ＡＨ_Ｂとも記述する。例えば、本実施形態では、図１に示すように、後述するロボット座標系Ｒを基準とする手先座標系Ｔの相対位置姿勢を^ＲＨ_Ｔで示し、また後述するカメラ座標系Ｖを基準とするワーク座標系Ｗの相対位置姿勢を^ＶＨ_Ｗで示す。

相対位置姿勢^ＡＨ_Ｂは、回転行列Ｒｏｔ及び並進ベクトルｔを用いて、数式１のように定義される。

数式１において、回転行列Ｒｏｔは３次元の回転を表す３×３の行列であり、並進ベクトルｔは３×１の３次元ベクトルである。並進の移動量のＸ，Ｙ，Ｚ成分をそれぞれｔ_ｘ，ｔ_ｙ，ｔ_ｚと表し、ＸＹＺ各軸回りの回転成分をθ_ｘ，θ_ｙ，θ_ｚで表すと、回転行列Ｒｏｔは数式２のように定義されると共に、並進ベクトルｔは数式３のように定義される。

なお、数式２では、ＸＹＺ各軸回りの回転成分を表す回転行列を、それぞれＲｏｔｘ，Ｒｏｔｙ，Ｒｏｔｚとしている。

次に、座標変換行列の性質として、任意の座標系Ａ，Ｂ，Ｃに対して、数式４が成立する。

即ち、座標系Ａに対する座標系Ｂの相対位置姿勢^ＡＨ_Ｂと座標系Ｂに対する座標系Ｃの相対位置姿勢^ＢＨ_Ｃとを乗算することにより、座標系Ａに対する座標系Ｃの相対位置姿勢^ＡＨ_Ｃを求めることができる。

また、座標系Ｂに対する座標系Ａの相対位置姿勢^ＢＨ_Ａを求めるためには、数式５のように逆行列を算出する。

また、並進の移動量及び各軸回りの回転成分からなるベクトル（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ，θ_ｘ，θ_ｙ，θ_ｚ）から座標変換行列^ＡＨ_Ｂを求めるには、本来は数式１〜数式３を用いて表示する必要がある。これに対し、本明細書中では、簡易にＨ（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ，θ_ｘ，θ_ｙ，θ_ｚ）を用いて数式６に表すようにする。

同様に、座標変換行列^ＡＨ_Ｂから並進の移動量及び各軸回りの回転成分からなるベクトル（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ，θ_ｘ，θ_ｙ，θ_ｚ）を求める逆変換式についても、数式１〜数式３を用いた表示にせずに、ｐｏｓｅ（^ＡＨ_Ｂ）として数式７に表すようにする。

（但し、−１８０［ｄｅｇ］＜θ_ｘ，θ_ｙ，θ_ｚ＜１８０［ｄｅｇ］）

ｐｏｓｅ（^ＡＨ_Ｂ）の変換において、θ_ｙ＝±９０［ｄｅｇ］の場合は解が一意に求まらないが、その場合には複数存在する解のうち１つが算出されるものとする。尚、本実施形態では、座標変換行列の対象範囲はロボット本体２の動作誤差程度の小さい角度範囲であるので、θ_ｙ＝±９０［ｄｅｇ］になる場合は稀であり、上述した解が一意に求まらない場合の影響は極めて小さい。

次に、本実施形態に係るロボットシステム１について説明する。

図１に示すように、ロボットシステム１は、ロボット装置１ａと、ロボット校正部（制御部）４７とを備えている。ロボット装置１ａは、多関節アーム２０及びハンド２１を有するロボット本体２と、該ロボット本体２に設けられた単眼のカメラ（視覚センサ）３と、ロボット本体２の位置姿勢を制御する制御装置（ロボット校正装置）４と、を備えている。

多関節アーム２０（以下、アームという）は、ロボット本体２の基端部に固定された架台５に固定されており、架台５には該架台５を基準にしてアーム２０の位置姿勢を表すロボット座標系Ｒが設定されている。また、架台５には、ワーク（対象物）６が載置されるようになっており、ワーク６には該ワーク６を基準にしてワーク座標系Ｗが設定されている。

アーム２０としては、７つのリンクと、各リンクを揺動又は回動可能に連結する６つの関節とを備える６軸の垂直多関節アーム２０を適用している。本実施形態では６軸の垂直多関節アーム２０を適用しているが、これに限らず、軸数は用途や目的に応じて適宜変更してもよい。

各関節には、各関節を各々駆動するモータあるいは必要に応じて直動アクチュエータが、出力機器として設けられている。各関節には、モータの回転角度を検知するエンコーダと、各モータに供給する電流を検知する電流センサと、各関節のトルクを検知するトルクセンサとが、入力機器として設けられている。

アーム２０は、制御装置４のロボット制御部（制御部）４４から出力される指令値により各関節を駆動して、ハンド２１の位置姿勢を調整するようになっている。ロボット制御部４４は、ロボット座標系Ｒを基準とする後述する手先座標系Ｔの相対位置姿勢（指令値^ＲＨ_Ｔ）の目標値に対して、アーム２０の各関節の取るべき角度を計算し、各関節に対して指令値^ＲＨ_Ｔを出力するようになっている。また、ロボット制御部４４は、各エンコーダから各関節の現在角度情報を取得し、後述する現在の手先座標系Ｔの相対位置姿勢の指令値^ＲＨ_Ｔに基づいて制御された結果の制御結果値を算出可能になっている。そして、後述するロボット校正部４７は、ロボット制御部４４で得られた制御結果値を、後述する校正用データを取得する際に、指令値^ＲＨ_Ｔの代わりに利用可能になっている。

アーム２０の先端部にはハンド２１が支持されており、該ハンド２１は、アーム２０の動作により位置姿勢が調整されると共に、ワーク６を把持可能な例えば３本の指を備えている。ハンド２１には、該ハンド２１を基準にして手先座標系Ｔが設定されている。本実施形態では、アーム２０にはワーク６を把持可能なハンド２１が取り付けられている。しかしながら、これには限られず、例えば把持以外の手段でワーク６を保持する機構や、ワーク６に加工を施す工具等、ワーク６に対して作業可能なエンドエフェクタの全般が取り付けられるようにしてもよい。

また、アーム２０の先端部あるいはハンド２１にはカメラ３が支持されており、該カメラ３は制御装置４のカメラ制御部４５からの指令を受けて撮像（計測）し、画像データをカメラ制御部４５に送信するようになっている。カメラ３には、光軸方向及び撮像視野の縦横方向を基準にしてカメラ座標系Ｖが設定されている。本実施形態では、カメラ３は、ロボット本体２に対する所定の位置姿勢関係として、光軸がハンド２１の長軸方向に対して約４５度を形成してハンド２１の先端を向くように設けられている。

ロボット本体２に接続された制御装置４は、コンピュータにより構成され、ロボット本体２を制御するようになっている。制御装置４を構成するコンピュータは、例えばＣＰＵ４０と、各部を制御するためのプログラムを記憶するＲＯＭ４１と、データを一時的に記憶するＲＡＭ４２と、ロボット本体２に通信可能にする入出力インターフェース回路（Ｉ／Ｆ）４３とを備えている。

ＣＰＵ４０は、上述のロボット制御部４４と、カメラ制御部４５と、ワーク位置姿勢計算部４６とを備えている。尚、本実施形態では、制御装置４はロボット校正装置と共用されており、制御装置４のＣＰＵ４０はロボット校正装置の制御部であるロボット校正部４７を含んで構成されている。

カメラ制御部４５は、カメラ３が取得した画像データを制御装置４のワーク位置姿勢計算部４６に送信し、該ワーク位置姿勢計算部４６は、受信した画像データに基づき画像処理を行ってワーク６の位置姿勢を算出するようになっている。ここで算出される出力値は、カメラ座標系Ｖを基準とするワーク座標系Ｗの位置姿勢^ＶＨ_Ｗに相当するデータである。ワーク位置姿勢計算部４６は、処理結果をロボット校正部４７に送信する。

本発明の特徴部であるロボット校正部４７は、本発明のロボット校正装置及びその制御部に相当する。ロボット校正部４７は、ロボット制御部４０から取得した手先座標系Ｔにおけるハンド２１の位置姿勢の指令値^ＲＨ_Ｔと、ワーク位置姿勢計算部４６から取得したワーク座標系Ｗにおけるワーク６の位置姿勢^ＶＨ_Ｗとに基づき、校正値を算出する。更に、ロボット校正部４７は、校正用の指令値^ＲＨ_Ｔ ^comに基づくロボット本体２の第一の位置姿勢（以下、理想位置姿勢という）と、それに相当する校正用の計測値^ＲＨ_Ｔ’ ^measに基づくロボット本体２の第二の位置姿勢（以下、実位置姿勢という）と、の差分に基づいて、指令値を校正する校正関数Ｆを算出する。

上述したロボットシステム１のロボット校正部４７により校正値を算出し校正関数Ｆを求める手順を、図２に示すフローチャートのステップＳ１〜Ｓ３により説明する。尚、図２に示すフローチャートでは、ステップＳ１〜Ｓ３はロボットシステム１の立上時又はメンテナンス時等に行う校正作業の手順を示し、ステップＳ４〜Ｓ７は生産ラインで順次供給されたワーク６に対して繰返し行う位置姿勢制御の手順を示す。

まず、ロボット校正部４７は、ロボット制御部４４によりロボット本体２を複数の位置姿勢に位置決めしながら、図５及び図６に示すように、カメラ制御部４５によりカメラ３で校正プレート（校正用基準物）１０を順次計測する。これにより、ロボット校正部４７は、校正に必要な複数のデータを取得して記憶する（ステップＳ１）。

校正プレート１０は、例えば平板状で、表面に所定の寸法の円形パターン１０ａ，１０ｂが複数（３個以上）配置されて表示されており、カメラ３により６自由度の位置姿勢を計測できるようになっている。また、校正プレート１０は、カメラ３から計測できる視野内に設置されていると共に、ロボット本体２が移動した時でも移動しないように架台５に対して相対的に固定されて配置されている。

校正プレート１０の複数の円形パターン１０ａ，１０ｂのうち角部の１つだけは中空円形パターン１０ａとし、他は塗りつぶされた中実円形パターン１０ｂとしている。複数の円形パターン１０ａ，１０ｂは、中空円形パターン１０ａの存在により全体として非点対称の配置になっており、校正プレート１０の６自由度の位置姿勢を一意に特定できるようになっている。また、校正プレート１０には、該校正プレート１０を基準にして校正プレート座標系Ｐが設定されている。

校正プレート１０の有するパターンとしては、円形パターンには限らず、例えば直線的な格子パターン等を用いてもよい。また、例えばステレオカメラ等、カメラ３により３次元の計測を行うことができる場合は、平板状の校正プレート１０に限らず、立体形状を有する物体を校正用の基準部材としてもよい。更には、例えば作業の対象とする実際のワーク６を校正用基準物として用いることもできる。

また、校正用データを取得する際の前提として、予め校正用のＮ個の位置姿勢を制御装置４のＲＯＭ４１又はＲＡＭ４２に記憶させておく。ここでの校正用の位置姿勢の決め方としては、ロボット本体２が作業時に必要とし得る動作範囲の中で、なるべく多様な姿勢を取らせることが好ましい。後述する校正関数Ｆの種類にもよるが、一般的には姿勢の数が多いほど精度が向上するため、例えば１０〜数１０以上の位置姿勢を利用することが好ましい。また、ワーク６を設置する位置姿勢に実際にはばらつきが想定されるので、ばらつきを含むようにロボット本体２の可動範囲を広めに設定することが精度向上のために好ましい。

ここで、ステップＳ１の校正用データを取得する手順を、図３に示すフローチャートに沿って詳細に説明する。

まず、ロボット校正部４７は、繰返し処理のカウンタｉを１に初期化し（ステップＳ１１）、ロボット制御部４４によりロボット本体２を最初の校正位置姿勢（ｉ＝１）に位置決めする（ステップＳ１２）。そして、ロボット校正部４７は、カメラ制御部４５によりカメラ３で校正プレート１０を計測する（ステップＳ１３）。更に、ロボット校正部４７は、ワーク位置姿勢計算部４６によりカメラ３から取得したデータに基づいて校正プレート１０の位置姿勢を計算する。即ち、ワーク位置姿勢計算部４６は、カメラ座標系Ｖを基準とした校正プレート座標系Ｐの位置姿勢^ＶＨ_Ｐ［ｉ］を算出する。

ロボット校正部４７は、ロボット本体２を位置決めした際のハンド２１の先端部の理想位置姿勢に対応する指令値^ＲＨ_Ｔ［ｉ］を取得して、ＲＡＭ４２に保存し（ステップＳ１４）、また、その計測値^ＶＨ_Ｐ［ｉ］をＲＡＭ４２に記憶する（ステップＳ１５）。ハンド２１の先端部の位置姿勢の指令値^ＲＨ_Ｔ［ｉ］としては、予め校正用の位置姿勢として保存されていたデータを用いるようにできる。あるいは、ロボット制御部４４により、指令値^ＲＨ_Ｔ［ｉ］に基づいてロボット本体２の位置姿勢を決定した後に、各関節のエンコーダから現在角度情報の値を取得して、指令値^ＲＨ_Ｔ［ｉ］に基づいて制御された結果の制御結果値を算出してもよい。この場合、ロボット校正部４７は、得られた制御結果値を指令値^ＲＨ_Ｔ［ｉ］の代わりに用いることができる。尚、一般にはロボット本体２の動作においては、制御の目標値に対して少量の偏差が残るため、校正用データとして指令値^ＲＨ_Ｔ［ｉ］よりも制御結果値を用いた方がより高精度になりやすい。

そして、ロボット校正部４７は、カウンタｉを１つ進め（ステップＳ１６）、カウンタｉが校正位置姿勢の個数Ｎを超えているか否かを判断する（ステップＳ１７）。ロボット校正部４７が、カウンタｉはＮを超えていないと判断した場合は、ロボット本体２を次の校正位置姿勢に位置決めし、上述と同様の処理を実行する（ステップＳ１２〜Ｓ１６）。

ロボット校正部４７が、カウンタｉはＮを超えていると判断した場合は、校正用データの取得の処理を終了し、元の処理に戻る。このため、校正用データの取得処理（図２のステップＳ１）が終了した時には、ロボット本体２のＮ個の位置姿勢の指令値^ＲＨ_Ｔ ^com［ｉ］（ｉ＝１，２，…，Ｎ）がＲＡＭ４２に記憶される。同様に、校正プレート１０のＮ個の計測値^ＶＨ_Ｐ ^meas［ｉ］（ｉ＝１，２，…，Ｎ）も、ＲＡＭ４２に記憶される。

次に、図５に示すように、ロボット校正部４７は、カメラ座標系Ｖと手先座標系Ｔの位置姿勢関係（第１の校正値）^ＶＨ_Ｔと、ロボット座標系Ｒと校正プレート座標系Ｐの位置姿勢関係（第２の校正値）^ＲＨ_Ｐとを演算して取得する（図２のステップＳ２）。ここでは、ロボット校正部４７は、先に取得した指令値^ＲＨ_Ｔ［ｉ］と、計測値^ＶＨ_Ｐ［ｉ］とに基づき、例えば公知のハンドアイキャリブレーション手法（非特許文献１参照）を用いて、^ＶＨ_Ｔ及び^ＲＨ_Ｐを演算して取得する。

以下に、具体的な手順を示す。カメラ３及びハンド２１の間と、ロボット本体２及び校正プレート１０の間とは、それぞれ固定されているため、^ＶＨ_Ｔ及び^ＲＨ_Ｐは、ロボット本体２の位置姿勢によらず一定である。カメラ３の計測誤差とロボット本体２の動作誤差が無い場合、各校正位置姿勢について数式８が成り立つ。

（ｉ＝１，２，…，Ｎ）

校正用の位置姿勢はＮ個あるので、それぞれについて各値を取得すると、数式８をＮ本連立することができる。実際に得られるデータには誤差があるため、誤差最小化計算によりＮ組のデータに対する誤差（残差）が最小となる^ＶＨ_Ｔ及び^ＲＨ_Ｐの値を算出し、校正値とする。ここでの算出方法については、公知のハンドアイキャリブレーション手法と同様であるため、詳細な説明は省略する。以降、このステップＳ２で算出した校正値を、^ＶＨ_Ｔ ^cal及び^ＲＨ_Ｐ ^calと表示する（図６参照）。

尚、予め各座標系の関係が設計値として既知である場合は、ステップＳ１で取得したデータを用いずに設計値を用いるようにしてもよい。また、カメラ座標系Ｖと手先座標系Ｔとの関係^ＶＨ_Ｔに関しては、カメラ３からハンド２１等を直接計測することによって取得することも可能である。

次に、本実施形態の特徴である校正関数Ｆのパラメータを算出する処理（ステップＳ３、校正関数算出工程）について説明する。ここでの校正関数Ｆとは、ロボット本体２に対する指令値と、その指令値におけるロボット本体２の実際の位置姿勢との関係を表す関数である。これは、校正時にステップＳ１で取得した校正用データの実績値から指令値と実際の位置姿勢との関係を求め、実作業時にはその関係を用いて指令値を与えた時のハンド２１の実際の位置姿勢を推定する。このように、誤差分を補正することによって、ロボット本体２を高精度に動作させることができる。

そこで、手先位置指令値を^ＲＨ_Ｔ ^comとし、その指令値^ＲＨ_Ｔ ^comをロボット本体２に与えることにより実現する手先位置姿勢の推定値を^ＲＨ_Ｔ ^predとした場合に、校正関数Ｆを数式９のように定める。

このステップＳ３では、数式９に示す校正関数Ｆを近似的に同定するためのパラメータを算出する。校正関数Ｆとしては様々な関数を用いることができるが、本実施形態においては多項式を用いる場合について説明する。

以下、多項式である校正関数Ｆのパラメータを算出する手順について、図４に示すフローチャートに沿って説明する。

まず、ロボット校正部４７は、繰返し処理のカウンタｉを１に初期化する（ステップＳ３１）。このカウンタｉは、ステップＳ１で取得したＮ個の校正位置姿勢に対応させる。そして、ロボット校正部４７は、ｉ番目の計測値^ＶＨ_Ｐ ^meas［ｉ］に対して、手先実現位置姿勢の計測値^ＲＨ_Ｔ’ ^meas［ｉ］を数式１０により算出する（ステップＳ３２）。

（ｉ＝１，２，…，Ｎ）

計測値^ＲＨ_Ｔ’ ^meas［ｉ］は、ｉ番目の校正位置姿勢に対して、カメラ３による計測値^ＶＨ_Ｐ ^meas［ｉ］を反映して、ロボット座標系Ｒ及び実現された手先座標系Ｔ’の関係を求めた指標である。カメラ３の計測精度がロボット本体２の動作精度よりも高い場合、計測値^ＲＨ_Ｔ’ ^meas［ｉ］は、ロボット本体２の理想位置姿勢の指令値^ＲＨ_Ｔ ^com［ｉ］に対して、現実世界で実際にロボット本体２の位置決めされた位置姿勢を反映したものとなる。即ち、手先実現位置姿勢の計測値^ＲＨ_Ｔ’ ^meas［ｉ］は、理想位置姿勢の指令値^ＲＨ_Ｔ ^com［ｉ］に対して、差分（動作誤差）^ＴＨ_Ｔ’ ^err［ｉ］が乗った値と考えることができる。

次に、ロボット校正部４７は、校正関数Ｆのパラメータ計算の前処理として、ｉ番目の校正位置姿勢に対する指令値^ＲＨ_Ｔ ^com［ｉ］及び計測値^ＲＨ_Ｔ’ ^meas［ｉ］について、基準位置姿勢の指令値^ＲＨ_Ｔ０で相対化する（ステップＳ３３、相対化工程）。即ち、ロボット校正部４７は、ｉ番目の校正位置姿勢における理想位置姿勢及び実位置姿勢を、基準位置姿勢で相対化する。ｉ番目の校正位置姿勢に対する手先位置姿勢の指令値^ＲＨ_Ｔ ^com［ｉ］については、数式１１のように、予め定めたロボット本体２の基準位置姿勢の指令値（基準指令値）^ＲＨ_Ｔ０によって相対化する。

（ｉ＝１，２，…，Ｎ）

ここでの相対化とは、指令値^ＲＨ_Ｔ ^com［ｉ］及び計測値^ＲＨ_Ｔ’ ^meas［ｉ］を、基準位置姿勢の指令値^ＲＨ_Ｔ０との相対的な位置姿勢の値に変換することを意味する。尚、基準位置姿勢の指令値^ＲＨ_Ｔ０としては、校正位置姿勢を取得した際の代表的な位置姿勢の指令値を用いることができる。

ｉ番目の校正位置姿勢に対する手先実現位置姿勢の計測値^ＲＨ_Ｔ’ ^meas［ｉ］については、数式１２のように、基準位置姿勢の指令値^ＲＨ_Ｔ０によって相対化する。

（ｉ＝１，２，…，Ｎ）

このステップＳ３３の処理により、後述する処理で６自由度の各成分（ｘ，ｙ，ｚ，α，β，γ）を入力する多項式関数ｆを定義する際に、回転の成分が大きいと±１８０°付近で関数が不連続となり近似の精度が低下することを抑制できる。尚、このステップＳ３３は近似の精度の低下を抑制するための処理であるので、校正する位置姿勢の条件等によっては省略してもよい。

そして、ロボット校正部４７は、カウンタｉを１つ進め（ステップＳ３４）、カウンタｉが校正位置姿勢の個数Ｎを超えているか否かを判断する（ステップＳ３５）。ロボット校正部４７が、カウンタｉはＮを超えていないと判断した場合は、次の校正位置姿勢について上述と同様の処理を実行する（ステップＳ３２〜Ｓ３４）。

ロボット校正部４７が、カウンタｉはＮを超えていると判断した場合は、多項式のパラメータを演算する処理に入る（ステップＳ３６）。この時点で、各校正位置姿勢について、相対化した手先位置姿勢の指令値^Ｔ０Ｈ_Ｔ ^com［ｉ］と相対化した手先実現位置姿勢計測値^Ｔ０Ｈ_Ｔ’ ^meas［ｉ］との組が、合計Ｎ組算出されている。

ステップＳ３６では、前提として、数式９の校正関数Ｆを求めるにあたって、以下の２点の変換を行う。第１に、前述したように回転成分の不連続性を回避するために、基準位置姿勢の指令値^ＲＨ_Ｔ０によって相対化する。第２に、数式９における手先位置指令値^ＲＨ_Ｔ ^com及び手先位置姿勢の推定値^ＲＨ_Ｔ ^predは４×４の同次変換行列であるが、自由度は６であり、そのままの形式だと多項式で扱い難いため、６成分の形式に変換する。即ち、校正関数Ｆを数式１３に示す多項式関数ｆに置き換える。

ここで、相対化した手先位置姿勢の推定値^Ｔ０Ｈ_Ｔ ^predと、相対化した手先位置姿勢の指令値^Ｔ０Ｈ_Ｔ ^comとは、数式１４により算出される。

数式１３に数式１４を代入して式変形すると、数式１５で表される。

従って、校正関数Ｆと多項式関数ｆとの関係は、数式１６で表される。

数式１６において、基準位置姿勢の指令値^ＲＨ_Ｔ０は既知であるので、多項式関数ｆのパラメータを求めることにより、校正関数Ｆを定めることができる。このため、まず多項式関数ｆへの入力値となる相対化した手先位置姿勢の指令値^Ｔ０Ｈ_Ｔ ^comを６自由度に変換し、変換後の各成分を数式１７のように（ｘ，ｙ，ｚ，α，β，γ）とする。

そして、多項式関数ｆの出力値となる相対化した手先位置姿勢の推定値^Ｔ０Ｈ_Ｔ ^predを６自由度に変換し、変換後の各成分を数式１８のように（ｘ’，ｙ’，ｚ’，α’，β’，γ’）とする。

更に、多項式関数ｆの各成分をｆｘ，ｆｙ，ｆｚ，ｆα，ｆβ，ｆγとすると、数式１９で表される。

本実施形態では、多項式関数ｆを６変数の多項式としている。これにより、多項式関数ｆのｘ’，ｙ’，ｚ’の各成分は、一般的に数式２０のように表される。

同様に、多項式関数ｆのα’，β’，γ’の各成分は、一般的に数式２１のように表される。

数式２０及び数式２１において、Ｄは多項式関数ｆの次数である。また、ａ_{ｘ＿ｌｍｎｐｑｒ}，ａ_{ｙ＿ｌｍｎｐｑｒ}，ａ_{ｚ＿ｌｍｎｐｑｒ}，ａ_{α＿ｌｍｎｐｑｒ}，ａ_{β＿ｌｍｎｐｑｒ}，ａ_{γ＿ｌｍｎｐｑｒ}は多項式関数ｆの係数であり、添字ｌ，ｍ，ｎ，ｏ，ｐ，ｑは０以上の整数である。Ｎ個の校正位置姿勢により、これらの多項式関数ｆを算出することで、校正関数Ｆを定めることができる。本実施形態においては、説明を簡単にするために次数Ｄ＝１の場合について説明する。

数２０のｘ’の式について、次数Ｄ＝１の場合は数式２２となる。

ここで、記述を簡略化するため、数式２３のように文字を置き換えるものとする。

同様に、ｙ’，ｚ’，α’，β’，γ’の各成分についても、Ｄ=１として文字を置き換えると、数式２０及び数式２１は数式２４のようになる。

次に、先に算出した相対化した手先位置姿勢の指令値^Ｔ０Ｈ_Ｔ ^com［ｉ］と、相対化した手先実現位置姿勢の計測値^Ｔ０Ｈ_Ｔ’ ^meas［ｉ］とを、数式２４に代入可能とするために６自由度の形式に変換する。即ち、相対化した手先位置姿勢の指令値^Ｔ０Ｈ_Ｔ ^com［ｉ］の変換結果の６成分を、（ｘ［ｉ］，ｙ［ｉ］，ｚ［ｉ］，α［ｉ］，β［ｉ］，γ［ｉ］）とする。また、相対化した手先実現位置姿勢の計測値^Ｔ０Ｈ_Ｔ’ ^meas［ｉ］の変換結果の６成分を、（ｘ’［ｉ］，ｙ’［ｉ］，ｚ’［ｉ］，α’［ｉ］，β’［ｉ］，γ’［ｉ］）とする。その結果、数式２５が得られる。

（ｉ＝１，２，…，Ｎ）

数式２５を数式２４に代入すると、数式２６が得られる。

（ｉ＝１，２，…，Ｎ）

ここで、例えばｘ’の式に注目して、Ｎ組のデータ（ｉ＝１，２，…，Ｎ）を代入すると、数式２７のようにＮ個の連立方程式を立てることができる。

数式２７の連立方程式を行列の形で整理すると、数式２８が得られる。

数式２８において、ｘ’［ｉ］と、ｘ［ｉ］，ｙ［ｉ］，ｚ［ｉ］，α［ｉ］，β［ｉ］，γ［ｉ］とは既知であり、ａ_ｘ０〜ａ_ｘ６は未知である。Ｎ組のデータのうち、線形独立なものが７組以上存在すれば、公知の最小二乗法によって係数の組ａ_ｘ０〜ａ_ｘ６を求めることができる。

同様に、ｙ’，ｚ’，α’，β’，γ’の各成分についても、最小二乗解を求めることで、数式２０、数式２１、数式２４の定義によって多項式関数ｆを求めることができる。即ち、数式１６の関係によって校正関数Ｆを求めることができる。以上、図２に示すステップＳ１〜Ｓ３が終了することにより、校正関数Ｆを算出する処理が完了する。即ち、ロボット校正部４７は、校正時に用いられる校正用の指令値^ＲＨ_Ｔ ^com［ｉ］に基づいて演算されるロボット本体２の理想位置姿勢を算出する。また、ロボット校正部４７は、ロボット本体２を校正用の指令値^ＲＨ_Ｔ ^com［ｉ］に基づき制御した際にカメラ３により得られた校正用の計測値^ＶＨ_Ｐ ^meas［ｉ］に基づいて演算されるロボット本体２の実位置姿勢を算出する。そして、ロボット校正部４７は、ロボット本体２の理想位置姿勢と、実位置姿勢との差分に基づいて校正関数Ｆを算出している。

次に、上述したロボットシステム１により、得られた校正関数Ｆを利用して、ロボット本体２に指令値を出力してハンド２１をワーク６に移動させ把持させる手順を、図２に示すフローチャートのステップＳ４〜Ｓ７により説明する。

まず、ロボット制御部４４は、ロボット本体２を計測用の位置姿勢に位置決めし、カメラ制御部４５によりワーク６をカメラ３で計測する（ステップＳ４、計測値取得工程）。ここで、ロボット本体２への計測用位置姿勢の指令値を^ＲＨ_Ｔ１とし、その指令値^ＲＨ_Ｔ１においてカメラ３により計測したワーク６の位置姿勢の計測値を^ＶＨ_Ｗ ^measとする。

次に、ロボット制御部４４は、計測したワーク６の位置姿勢の計測値^ＶＨ_Ｗ ^measに基づいて、仮の手先指令値（仮の指令値）^ＲＨ_Ｔ ^tempを演算して生成する（ステップＳ５、仮の指令値生成工程）。ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempを、公知の剛体の座標変換モデルを利用して、校正値^ＶＨ_Ｔを用いる数式２９により演算する。

ロボット制御部４４は、数式２９により、ワーク６の位置姿勢の計測値^ＶＨ_Ｗ ^measをロボット座標系Ｒに座標変換し、ロボット本体２の位置姿勢指令値として使用できる値に変換する。但し、数式２９では、ロボット本体２の動作誤差の影響が大きく位置決めの精度は低い。

次に、ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempを校正関数Ｆに入力し、数式３０により演算して、手先位置姿勢の推定値^ＲＨ_Ｔ’ ^predを生成する。

ここで、相対化した仮の手先指令値^Ｔ０Ｈ_Ｔ ^tempからベクトルを算出すると、数式３１で表される。

更に、数式２０、数式２１、数式２４により、数式３２が導かれる。

ロボット制御部４４は、先に算出した多項式関数ｆを用いることで、手先位置姿勢の推定値^ＲＨ_Ｔ’ ^predを算出する。即ち、仮の手先指令値^ＲＨ_Ｔ ^tempを使用してロボット本体２を位置決めした場合、ロボット本体２の動作誤差が加わった実際の手先位置姿勢は手先位置姿勢の推定値^ＲＨ_Ｔ’ ^predになるであろうことが推定される。そこで、ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempと手先位置姿勢の推定値^ＲＨ_Ｔ’ ^predとの差分^ＴＨ_Ｔ’ ^errを数式３３のように算出する。

ロボット制御部４４は、差分^ＴＨ_Ｔ’ ^errを打ち消すように、当該差分^ＴＨ_Ｔ’ ^errの逆行列によって仮の手先指令値^ＲＨ_Ｔ ^tempを補正する。ここで、補正後の最終的な手先指令値（校正後指令値）を^ＲＨ_Ｔ ^compとすると、数式３４で表される。

上述した処理により、ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempを校正関数Ｆにより校正し、最終的な手先指令値^ＲＨ_Ｔ ^compを得ることができる（ステップＳ６、校正工程）。

そして、ロボット制御部４４は、最終的な手先指令値^ＲＨ_Ｔ ^compをロボット本体２に送信し、ロボット本体２を位置決めして作業を行わせる（ステップＳ７、位置姿勢制御工程）。ロボット本体２への指令値は校正処理により動作誤差分が補正されているので、ハンド２１にワーク６へのアプローチを高精度に行わせ、手先座標系Ｔとワーク座標系Ｗとを高精度に一致させ、ハンド２１によるワーク６の把持を高精度に実現することができる。

上述したように本実施形態のロボットシステム１によれば、ロボット本体２の理想位置姿勢と実位置姿勢との差分に基づいて算出した校正関数Ｆを利用して、仮の手先指令値^ＲＨ_Ｔ ^tempを最終的な手先指令値^ＲＨ_Ｔ ^compに校正することができる。このため、従来のハンドアイキャリブレーション方法と異なり、ロボット本体２の動作誤差や固有の誤差を打ち消すことができるので、ロボット本体２の位置姿勢の制御において精度向上を図ることができる。

また、本実施形態のロボットシステム１によれば、カメラ３の計測値を校正関数Ｆに直接入力するのでなく、算出した仮の手先指令値^ＲＨ_Ｔ ^tempを校正関数Ｆに入力する構成としている。これにより、校正時の計測値と実際のワーク６の計測値とに差異があっても好適に誤差を低減できるため、校正プレート１０の計測点を実際のワーク６に対して厳密に対応させる必要はなく、作業性を向上することができる。

また、本実施形態のロボットシステム１によれば、カメラ３がハンド２１に設けられているので、カメラ座標系Ｖと手先座標系Ｔとの誤差が小さく、ロボット本体２の制御を高精度にすることができる。

また、本実施形態のロボットシステム１によれば、校正位置姿勢に対する指令値^ＲＨ_Ｔ ^com及び計測値^ＲＨ_Ｔ’ ^measについて、基準の位置姿勢の指令値^ＲＨ_Ｔ０で相対化している。このため、６自由度の各成分を入力する多項式関数ｆを定義する際に、回転の成分が大きいと±１８０°付近の特異点等で関数が不連続となり近似の精度が低下することを抑制でき、ロボット本体２の制御をより高精度にすることができる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボットシステム１０１について説明する。このロボットシステム１０１は、図７及び図８に示すように、ロボット装置１０１ａとロボット校正部４７とを備えている。このロボット装置１０１ａでは、第１実施形態と異なり、カメラ１０３がロボット本体１０２ではなく、ロボット本体２の基端部に固定された架台５と相対的に位置決めされた上側支持台５０により固定されて支持されている。また、このロボット装置１０１ａでは、第１実施形態と異なり、校正プレート１０が架台５に支持されるのではなく、ハンド２１に相対的に位置決めされた支持部材２２により支持されている。それ以外のハード構成については第１実施形態と同様であるので、第１実施形態と同一符号を付して説明を省略する。

上述したロボットシステム１０１により校正値を算出し校正関数Ｆを求め、該校正関数Ｆを利用してロボット本体１０２を制御する手順を、図２に示すフローチャートにより説明する。尚、図２に示すフローチャートは第１実施形態の処理手順を示すものであるが、本実施形態でも同様の順序で処理を実行するため、援用するものとしている。但し、各手順において第１実施形態と異なる部分があるので、以下、その異なる部分を中心に説明する。

まず、カメラ制御部４５は、ロボット本体１０２を複数の位置姿勢に位置決めしながら、図８に示すように、カメラ１０３で校正プレート１０を順次計測することにより、校正に必要な複数のデータを取得して記憶する（ステップＳ１）。処理の詳細は、図３に示すステップＳ１１〜Ｓ１７と同様であり、カメラ制御部４５は、ロボット本体１０２が動作し得る範囲、かつ校正プレート１０がカメラ１０３の視野に収まる範囲でロボット本体１０２を順次位置決めし、校正プレート１０を計測する。カメラ制御部４５は、Ｎ個の校正位置姿勢の指令値^ＲＨ_Ｔ ^com［ｉ］及びＮ個の計測値^ＶＨ_Ｐ ^meas［ｉ］をＲＡＭ４２に記憶する。

次に、ロボット校正部４７は、第１実施形態とは異なる校正値を演算して取得する。具体的には、ロボット校正部４７は、ロボット座標系Ｒとカメラ座標系Ｖとの位置姿勢関係（第３の校正値）^ＲＨ_Ｖと、手先座標系Ｔと校正プレート座標系Ｐとの位置姿勢関係（第４の校正値）^ＴＨ_Ｐとを演算して取得する（ステップＳ２）。カメラ１０３の計測誤差とロボット本体１０２の動作誤差が無い場合、各校正位置姿勢について数式３５が成り立つ。

（ｉ＝１，２，…，Ｎ）

そして、第１実施形態と同様に、校正用の位置姿勢はＮ個あるので、それぞれについて各値を取得すると、数式３５をＮ個連立することができる。実際に得られるデータには誤差があるため、誤差最小化計算によりＮ組のデータに対する誤差（残差）が最小となる^ＴＨ_Ｐ及び^ＲＨ_Ｖの値を算出し、校正値とする。以降、このステップＳ２で算出した校正値を、^ＴＨ_Ｐ ^cal及び^ＲＨ_Ｖ ^calと表示する。

次に、ロボット校正部４７は、校正関数Ｆのパラメータを算出する（ステップＳ３、校正関数算出工程）。ここでは、ロボット校正部４７は、ｉ番目の計測値^ＶＨ_Ｐ ^meas［ｉ］に対して、手先実現位置姿勢の計測値^ＲＨ_Ｔ’ ^meas［ｉ］を数式３６（第１実施形態の数式１０に相当）により算出する。この数式３６により、計測値^ＲＨ_Ｔ’ ^meas［ｉ］を算出した後の処理は、第１実施形態と同様に進めることができ、校正関数Ｆを特定するためのパラメータを算出することができる。

（ｉ＝１，２，…，Ｎ）

次に、ロボット制御部４４は、ロボット本体１０２を計測用の位置姿勢に位置決めし、カメラ制御部４５がワーク６をカメラ１０３によって計測し、計測値^ＶＨ_Ｗ ^measを得る（ステップＳ４、計測値取得工程）。

次に、ロボット制御部４４は、計測したワーク６の位置姿勢の計測値^ＶＨ_Ｗ ^measに基づいて、仮の手先指令値^ＲＨ_Ｔ ^tempを演算して生成する（ステップＳ５、仮の指令値生成工程）。本実施形態では、ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempを数式３７（第１実施形態の数式２９に相当）により算出する。

仮の手先指令値^ＲＨ_Ｔ ^tempを得た後は、第１実施形態と同様に、ロボット制御部４４は、仮の手先指令値^ＲＨ_Ｔ ^tempを校正関数Ｆにより校正し、最終的な手先指令値^ＲＨ_Ｔ ^compを得る（ステップＳ６、校正工程）。

そして、ロボット制御部４４は、第１実施形態と同様に、最終的な手先指令値^ＲＨ_Ｔ ^compをロボット本体２に送信し、ロボット本体１０２を位置決めして作業を行わせる（ステップＳ７、位置姿勢制御工程）。

上述したように本実施形態のロボットシステム１０１によれば、第１実施形態と同様に、ロボット本体１０２の理想位置姿勢と実位置姿勢との差分に基づいて算出した校正関数Ｆを利用する。このため、仮の手先指令値^ＲＨ_Ｔ ^tempを最終的な手先指令値^ＲＨ_Ｔ ^compに校正することができるので、ロボット本体２の動作誤差や固有の誤差を打ち消すことができるので、ロボット本体２の位置姿勢の制御において精度向上を図ることができる。

また、本実施形態のロボットシステム１によれば、カメラ３が上側支持台５０に設けられているので、カメラ座標系Ｖとロボット座標系Ｒとの誤差が小さく、ロボット本体２の制御を高精度にすることができる。

尚、上述した第１実施形態及び第２実施形態のロボットシステム１，１０１では、多項式の次数Ｄ＝１の場合のみについて説明したが、これには限られない。例えば、次数が２次以上の多項式であっても、上述の実施形態と同様にして校正関数Ｆとして使用することができる。ここで、数式２０及び数式２１に２次以上の次数のＤを代入して展開すると項の個数が多くなるが、多項式係数（ａ_{ｘ＿ｌｍｎｐｑｒ}等）については１次である。このため、校正動作データとして取得した（ｘ［ｉ］，ｙ［ｉ］，ｚ［ｉ］，α［ｉ］，β［ｉ］，γ［ｉ］）の値を代入することで多項式係数に関する一次式となり、線形の最小二乗法で係数を特定することができる。

また、上述した第１実施形態及び第２実施形態のロボットシステム１，１０１では、校正関数Ｆとして多項式のみを説明したが、これには限られず、例えば、スプライン関数等、単純な多項式以外の関数を校正関数Ｆとして用いてもよい。

また、上述した第１実施形態及び第２実施形態のロボットシステム１，１０１では、校正関数Ｆを、手先位置指令値^ＲＨ_Ｔ ^comを入力とし、手先位置姿勢の推定値^ＲＨ_Ｔ ^predを出力する関数として説明したが、これには限られない。例えば、校正関数Ｇをその逆関数として定義しても、同様の効果を得ることができる。即ち、校正関数Ｇを、目標とする手先位置姿勢の推定値^ＲＨ_Ｔ ^predを入力し、その位置姿勢を取るための手先位置指令値^ＲＨ_Ｔ ^comを出力する関数として定義してもよい。

この場合、数式９の代わりに数式３８を定義し、校正関数Ｇのパラメータを同定する処理を実行する。

この場合は、校正関数Ｇによる仮の指令値補正（ステップＳ６）の処理において、数式３３及び数式３４のように差分を求めて適用する処理は不要である。そして、校正関数Ｇに目標位置姿勢の指令値^ＲＨ_Ｔ ^tempを入力して得た結果を、最終的な手先指令値^ＲＨ_Ｔ ^compとして用いることができる。

また、上述した第１実施形態及び第２実施形態のロボットシステム１，１０１では、６自由度の全てを補正する場合について説明したが、これには限られない。即ち、用途によって６自由度の全てを補正する必要がない場合、校正関数Ｆとして算出する係数を適宜制限して使用してもよい。例えば、ｚ方向の補正が不要である場合は、ａ_{ｚ＿ｌｍｎｐｑｒ}を０に固定して、ｚの項に係る係数ａ_{ｚ＿００１０００}のみ１に固定し、その他の成分を補正するようにしてもよい。

また、第１実施形態及び第２実施形態のロボットシステム１，１０１では、ロボット校正装置はロボット校正部４７として制御装置４のＣＰＵ４０により実現された場合について説明したが、これには限られない。例えば、図９に示すように、ロボット校正装置１０４を制御装置４とは別個に設けるようにしてもよい。この場合、ロボット校正装置１０４は、内蔵のＣＰＵ等にロボット校正部（制御部）１４７を備えており、制御装置４の入出力インターフェース回路４３を介してＣＰＵ４０に接続されている。そして、ロボット校正装置１０４において生成された校正値及び校正関数Ｆは、ロボット制御部４４に入力され、ロボット本体２の制御に利用される。これによれば、制御装置４にロボット校正装置１０４を外付けすることができるので、ロボット校正部４７を有しない既存の制御装置４であっても、第１実施形態及び第２実施形態と同様に校正関数Ｆを利用することができる。

また、第１実施形態及び第２実施形態のロボットシステム１，１０１では、視覚センサとして単眼のカメラ３，１０３を使用した場合について説明した。しかしながら、これには限らず、例えば、ステレオカメラを利用したり、あるいはレーザスキャナを利用してもよい。レーザスキャナを使用した場合は、例えば、レーザスキャナによりワーク６を計測し、計測結果をカメラ制御部４５にて３次元の計測点群データとして取得する。そして、ワーク位置姿勢計算部４６において、３次元の計測点群データと３次元ＣＡＤモデルとのマッチングを行い、ワーク６の位置姿勢を算出するような構成にする。

尚、以上述べた第１実施形態及び第２実施形態の各処理動作は具体的には制御装置４及びロボット校正装置１０４により実行されるものである。従って、上述した機能を実現するソフトウェアのプログラムを記録した記録媒体を制御装置４等に供給し、制御装置４のＣＰＵ４０等が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した各実施の形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。具体的には、ロボット校正部４７，１４７におけるロボット校正プログラムと、ロボット制御部４４におけるロボット装置の制御プログラムと、ロボットシステム１，１０１におけるロボットシステムの制御プログラムとがある。

また、各実施の形態では、コンピュータ読み取り可能な記録媒体がＲＯＭ４１であり、ＲＯＭ４１にプログラムが格納される場合について説明したが、これに限定するものではない。プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、ＨＤＤ、外部記憶装置、記録ディスク等を用いてもよい。

１，１０１…ロボットシステム、１ａ，１０１ａ…ロボット装置、２，１０２…ロボット本体、３，１０３…カメラ（視覚センサ）、４…制御装置（ロボット校正装置）、５…架台（ロボット本体の基端部）、１０…校正プレート（校正用基準物）、２０…多関節アーム、２１…ハンド（ロボット本体の先端部）、４１…ＲＯＭ（記憶媒体）、４４…ロボット制御部（制御部）、４７，１４７…ロボット校正部（制御部）、５０…上側支持台（ロボット本体の基端部）、１０４…ロボット校正装置

Claims (20)

1. 複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を有するロボットシステムの制御装置において、
   複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
   前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、
   前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
   前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
   前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、
   を行う処理装置を備える、
   ことを特徴とするロボットシステムの制御装置。
2. 前記校正関数が、回転成分を含む、
   ことを特徴とする請求項１に記載のロボットシステムの制御装置。
3. 複数の関節を備えるロボット本体と、
   前記ロボット本体の先端部に固定された視覚センサと、
   請求項１に記載のロボットシステムの制御装置と、を備える、
   ことを特徴とするロボットシステム。
4. 前記ロボットシステムの制御装置が、前記校正した指令値に基づいて前記ロボット本体を制御する、
   ことを特徴とする請求項３に記載のロボットシステム。
5. 複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を有するロボットシステムの制御装置において、
   複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
   前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、
   前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
   前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
   前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、
   を行う処理装置を備える、
   ことを特徴とするロボットシステムの制御装置。
6. 前記校正関数が、回転成分を含む、
   ことを特徴とする請求項５に記載のロボットシステムの制御装置。
7. 複数の関節を備えるロボット本体と、
   前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、請求項５に記載のロボットシステムの制御装置と、
   を備えることを特徴とするロボットシステム。
8. 前記ロボットシステムの制御装置が、前記校正した指令値に基づいて前記ロボット本体を制御する、
   ことを特徴とする請求項７に記載のロボットシステム。
9. 複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を備えるロボットシステムに対する指令値を生成する指令値生成方法であって、
   制御装置が、複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
   前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、
   前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
   前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
   前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、
   を行う、
   ことを特徴とする指令値生成方法。
10. 前記校正関数が、回転成分を含む、
    ことを特徴とする請求項９に記載の指令値生成方法。
11. 複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を備えるロボットシステムに対する指令値を生成する指令値生成方法であって、
    制御装置が、複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
    前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、
    前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
    前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
    前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、を行う、
    ことを特徴とする指令値生成方法。
12. 前記校正関数が、回転成分を含む、
    ことを特徴とする請求項１１に記載の指令値生成方法。
13. 請求項９から１２のいずれか１項に記載の指令値生成方法の各処理をコンピュータに実行させるためのプログラム。
14. 請求項１３に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。
15. 複数の関節を備えるロボット本体と、前記ロボット本体の先端部に固定された視覚センサと、を備えるロボットシステムの制御方法であって、
    制御装置が、複数の校正用指令値に基づいて前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
    前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の手先座標系の位置姿勢関係を表す第一校正値と、前記ロボット本体の基端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第二校正値と、を求める処理と、
    前記複数の校正用指令値に基づく前記ロボット本体の複数の位置姿勢と、前記複数の計測値と前記第一校正値と前記第二校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
    前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
    前記対象物の計測値と前記第一校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、
    を行う、
    ことを特徴とするロボットシステムの制御方法。
16. 前記校正関数が、回転成分を含む、
    ことを特徴とする請求項１５に記載のロボットシステムの制御方法。
17. 複数の関節を備えるロボット本体と、前記ロボット本体の基端部に固定された架台と相対的に位置決めされた視覚センサと、を備えるロボットシステムの制御方法であって、
    制御装置が、複数の校正用指令値に基づいて、校正用基準物を支持する前記ロボット本体を複数の位置姿勢に制御しながら、前記視覚センサにて前記校正用基準物が備える複数のマーカを撮像することにより、前記校正用基準物について複数の計測値を取得する処理と、
    前記複数の校正用指令値と、前記校正用基準物の複数の計測値とから、前記視覚センサの座標系と前記ロボット本体の基端部の座標系の位置姿勢関係を表す第三校正値と、前記ロボット本体の先端部の座標系と前記校正用基準物の座標系の位置姿勢関係をあらわす第四校正値と、を求める処理と、
    前記複数の校正用指令値に基づく前記ロボット本体の位置姿勢と、前記計測値と前記第三校正値と前記第四校正値を用いて演算された前記ロボット本体の実際の位置姿勢の推定値と、の差分に基づいて、前記ロボット本体に対する指令値と実際の位置姿勢の推定値との関係を表す校正関数を求める処理と、
    前記視覚センサにて対象物を撮像することにより前記対象物の計測値を取得する処理と、
    前記対象物の計測値と前記第三校正値と前記校正関数とを用いて、前記対象物に対して前記ロボット本体を動作させる際の前記ロボット本体の位置姿勢の制御に用いる指令値を校正する処理と、
    を行う、
    ことを特徴とするロボットシステムの制御方法。
18. 前記校正関数が、回転成分を含む、
    ことを特徴とする請求項１７に記載のロボットシステムの制御方法。
19. 請求項１５から１８のいずれか１項に記載のロボットシステムの制御方法の各処理をコンピュータに実行させるためのプログラム。
20. 請求項１９に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。

Images