JP2009125857A - ロボットのキャリブレーション装置及び方法 - Google Patents

Abstract

【課題】短時間で精度よくロボットの機構パラメータを算出しロボットの位置決め誤差を低減する。
【解決手段】ロボット１のアーム先端に取り付けたキャリブレーション用治具３の位置を計測し、その計測結果と計測時のロボットへの指令値からロボットの機構パラメータを算出し、ロボットのキャリブレーションを行うものであって、治具３を測定装置４の測定可能範囲に複数の位置姿勢で位置決めし、位置決めした位置と測定値とからロボットの機構パラメータ、治具のツールパラメータ及び測定装置の設置位置姿勢を求める第１のキャリブレーション手段、求めた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、測定装置の計測範囲の一点から所定の位置精度の距離以内に複数の姿勢で位置決めし、再度、機構パラメータ、ツールパラメータ及び設置位置姿勢を求める第２のキャリブレーション手段を有するキャリブレーション制御装置６を備える。
【選択図】図１

Description

この発明は、ロボットの位置決め誤差を補正するためのロボットのキャリブレーション装置及び方法に関するものである。

多関節ロボットの機構パラメータとして、関節原点、リンク寸法、ツール寸法などがあるが、これらに誤差があるとロボット手先の絶対位置決め精度が悪化する。ロボットのキャリブレーションは、これらの機構パラメータの誤差を同定、補正することにより、ロボットの絶対位置決め精度を向上させるものである。

従来のロボットのキャリブレーション方法の一つとして、ロボットの手先を同一の位置に複数の姿勢で位置決めしキャリブレーションする方法がある。ロボットの機構パラメータに誤差がなければ、手先を同一の位置に位置決めすれば、そのときのロボット制御装置で管理しているロボットの手先の直交座標系での位置（Ｘ，Ｙ，Ｚ）は同一の値となる。しかし、機構パラメータに誤差がある場合は、同一の値とならない。複数の姿勢で同一の位置に位置決めしたときのロボット制御装置で管理している直交座標系位置のずれから機構パラメータを同定する（例えば、特許文献１参照）。

また、上記のキャリブレーション方法において、同一位置に実際に位置決めする代わりに、ロボット制御装置で管理しているロボットの手先の直交座標系位置が同一の位置になるようにロボットを位置決めし、基準となる姿勢の位置と姿勢を変化させたときの位置との位置ずれを３次元位置測定装置で計測し、その差分を用いてキャリブレーションする方法がある（例えば、特許文献２参照）。

特開昭６１−１３３４０９号公報（第４図） 特開２００１−１８１８２号公報（図１）

しかしながら、上記のような特許文献１によるキャリブレーション方法では、作業者がロボットの手先を複数の姿勢で同一位置に位置決めする必要があり、時間と労力を要していた。また、作業者が目視で同一位置に位置決めするため、精度よく同一位置に位置決めするのが困難であった。

また、特許文献２によるキャリブレーション方法では、ロボットの座標系と３次元位置測定装置の座標系の姿勢が一致していることを前提として、ロボット制御装置で管理しているロボットの手先の直交座標系位置データと、３次元位置測定装置で計測した直交座標系位置データの各成分の差分を用いてキャリブレーションを行っている。しかし、ロボット座標系と３次元位置測定装置の座標系の姿勢を精度よく一致させるのは困難であった。また、機構パラメータに誤差を含んだ状態で、ロボット制御装置が管理する同一位置に複数姿勢で位置決めすると、大きく位置がずれる可能性があるが、大きな位置ずれが発生すると、３次元測定装置の測定精度の非線形性やばらつきにより、測定結果に誤差が含まれ、精度よくキャリブレーションができないという問題があった。

この発明は上記のような問題点を解決するためになされたものであり、短時間で精度よくロボットの機構パラメータを算出しロボットの位置決め誤差を低減することができるロボットのキャリブレーション装置及び方法を得ることを目的としている。

この発明に係るロボットのキャリブレーション装置及び方法は、多関節ロボットのアーム先端に取り付けたキャリブレーション用治具の位置を計測し、その計測結果と計測時の前記ロボットへの指令値から前記ロボットの機構パラメータを算出し、前記ロボットのキャリブレーションを行うロボットのキャリブレーション装置及び方法であって、前記キャリブレーション用治具を前記測定装置の測定可能範囲に複数の位置姿勢で位置決めし、位置決めした位置と前記測定装置の測定値とから前記ロボットの機構パラメータ、前記キャリブレーション治具のツールパラメータ及び前記測定装置の設置位置姿勢を求める第１のキャリブレーション手段（ステップ）と、前記第１のキャリブレーションステップで求めた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、前記測定装置の計測範囲の一点からキャリブレーション対象のロボットで実現したい位置精度の距離以内に複数の姿勢で位置決めし、再度、機構パラメータ、ツールパラメータ及び設置位置姿勢を求める第２のキャリブレーション手段（ステップ）とを備えるものである。

この発明によれば、第１及び第２のキャリブレーション手段（ステップ）を備えることにより、簡便で、短時間かつ高精度にロボットのキャリブレーションが可能となる、といった従来にない顕著な効果を奏するものである。

実施の形態１．
図１は、この発明の実施の形態１によるロボットのキャリブレーション装置を示す構成図である。多関節ロボット１の各関節には、エンコーダなどの位置検出器１１が取り付けられている。ロボット１は、ロボット制御装置２と接続されており、ロボット制御装置２は、位置検出器１１の値から、ロボット１の各関節の角度を管理し、ロボット１の手先の位置を直交座標形の所望の位置（ｘ，ｙ，ｚ）に移動させる。

キャリブレーション治具としてのプローブ３は、ロボット１の手先、つまりアーム先端に取り付けられており、プローブ３の先端には球形になっている。測定装置としての３次元形状計測器４は、計測対象物にレーザ光を照射することにより、その３次元形状を測定するものであり、この発明のキャリブレーション装置では、ロボット１の手先に取り付けられたプローブ３先端の球の形状を測定する。３次元形状計測器４により計測した球表面の形状データより球の中心位置、つまりキャリブレーション治具としてのプローブ３の位置を求め、ロボット１のキャリブレーションに用いる。３次元形状計測器４は、センサ制御装置５によって制御され、計測した形状データはキャリブレーション制御装置６に送信される。

キャリブレーション制御装置６では、ロボット制御装置２やセンサ制御装置５と通信しながら、測定装置としての３次元形状計測器４からの計測結果と計測時のロボット１へのロボット制御装置２による指令値からロボット１の機構パラメータを算出し、ロボット１のキャリブレーションを行う。キャリブレーション制御装置６として、例えば、パーソナルコンピュータなどが用いられる。以下にキャリブレーション制御装置６で行われるキャリブレーション手順について説明する。

図２は、キャリブレーション制御装置６で行われるキャリブレーション手順を表したフローチャートである。ステップＳ２１（第１のキャリブレーション手段）では、プローブ３を３次元形状計測器４の計測可能範囲内に複数の異なる位置姿勢で位置決めし、位置決めした位置と３次元形状計測器４の測定値とからロボット１の機構パラメータ、前記キャリブレーション治具３のツールパラメータ及び３次元形状計測器４の設置位置姿勢を求める。

ステップＳ２２（第２のキャリブレーション手段）では、ステップＳ２１で求めた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、３次元形状計測器４の計測範囲の一点からキャリブレーション対象のロボット１で実現したい位置精度の距離以内に複数の姿勢で位置決めするものであって、３次元形状計測器４で計測可能な範囲の同一位置に対してプローブ３を複数の位置姿勢で位置決めし、再度、機構パラメータ、ツールパラメータ及び設置位置姿勢を求める。

図３は、図２のステップＳ２１（第１のキャリブレーション手段）の内容を詳細に説明したフローチャートである。ステップＳ２１１では、３次元形状計測器４で計測するロボット１の位置姿勢ｎ１点を生成する。計測位置姿勢点数ｎ１の値としては、２０〜４０点程度が望ましい。生成する計測位置は、例えば、３次元形状計測器４の計測可能範囲内にまんべんなく分布するようにランダムに決定する。また、姿勢についてもランダムに決定する。

ステップＳ２１２では、計測点数ｉ１を１にセットする。ステップＳ２１３では、ステップＳ２１１で生成した第ｉ１番目の計測位置姿勢にロボット１を位置決めする。

ステップＳ２１４では、３次元形状計測器４を用いてプローブ３の先端の球の表面形状を計測し、球の中心位置を求める。球の表面形状データから球の中心位置は次のようにして求める。

３次元形状計測器４によって計測された球の表面形状を（ｘｓｉ、ｙｓｉ、ｚｓｉ）（ｉ＝１，２，・・・，ｎｓ、ｎｓは球の表面データの計測点数）、球の半径をｒｓ、球の中心位置を（ｘｓｃ、ｙｓｃ、ｚｓｃ）とすると、次式が成り立つ。
（ｘｓｉ−ｘｓｃ）^２＋（ｙｓｉ−ｙｓｃ）^２
＋（ｚｓｉ−ｚｓｃ）^２＝ｒｓ^２ （１）

球の中心位置（ｘｓｃ、ｙｓｃ、ｚｓｃ）及び球の半径ｒｓが未知であるとすると、式（１）に（ｘｓｉ、ｙｓｉ、ｚｓｉ）（ｉ＝１，２，・・・，ｎｓ）を代入することにより、ｎｓ個の非線形連立方程式がたてられる。中心位置（ｘｓｃ、ｙｓｃ、ｚｓｃ）、半径ｒｓを未知数として、次のようにしてそれらの最小二乗解を求める。

今、ＸＳ＝（ｘｓｃ、ｙｓｃ、ｚｓｃ、ｒｓ）とし、
ｆｓｉ（ＸＳ）＝（ｘｓｉ−ｘｓｃ）^２＋（ｙｓｉ−ｙｓｃ）^２
＋（ｚｓｉ−ｚｓｃ）^２−ｒｓ^２ （２）
Ｍｓｉ（ＸＳ）＝［∂ｆｓｉ／ｘｓｃ ∂ｆｓｉ／∂ｙｓｃ
∂ｆｓｉ／∂ｚｓｃ・∂ｆｓｉ／∂ｒｓ］ （３）
とする。ここで、ｉ＝１，２，・・・，ｎｓである。

ｒｓ０をプローブ３の先端の球の半径の設計値とし、ＸＳの初期値をＸＳ_１＝（０、０、０、ｒｓ０）とすると、ＸＳの修正量ΔＸＳは次式で求められる。
ΔＸＳ＝（ＭＳ（ＸＳ）^Ｔ・ＭＳ（ＸＳ））^−１ＭＳ（ＸＳ）^Ｔ・ｄｆｓ （６）
ここで、Ｍ^ＴはＭの転置行列を表す。求めたΔＸＳよりＸＳの値を次式により更新する。
ＸＳ_ｋ＋１＝ＸＳ_ｋ＋ΔＸＳ （７）

ここで、ｋはＸＳの更新回数である。ΔＸＳの各成分が十分小さくなる、例えば、１０^−６になれば、上記ＸＳ_ｋ＋１の値を球の中心位置、半径とする。ΔＸＳの各成分が十分小さくなければ、ＸＳ_ｋ＋１をＸＳとして式（４）〜（６）に代入して再度ΔＸＳを求め、ΔＸＳの各成分が十分小さくなるまで繰り返す。

ステップＳ２１５では、計測数ｉ１に１を加え、ステップＳ２１６では、計測数ｉ１が計測位置姿勢点数ｎ１より大きい場合は、ステップＳ２１７に移行し、以下の場合はステップＳ２１４に戻り、計測を続ける。

ステップＳ２１７では、次のようにしてロボット１の機構パラメータ、ツールデータ、３次元形状計測器４の設置位置を求める。ステップＳ２１１からＳ２１６で計測したセンサ座標系Ｏｓにおける球の中心の計測位置をＰＳｉ＝［ｘｓｃｉ，ｙｓｃｉ，ｚｓｃｉ］^Ｔ（ｉ＝１，２，・・・，ｎ１）、ロボット座標系Ｏｒにおける球の中心位置をＰＲＳｉ、各計測時にロボット制御装置２で管理しているロボット１先端の球の位置（以下、指令値と呼ぶ）をＰＲｉ＝［ｘｒｉ，ｙｒｉ，ｚｒｉ］^Ｔとする。ロボット座標系Ｏｒから見たセンサ座標系Ｏｓの位置姿勢をＴＲＳとすると、
ＰＲＳｉ＝ＴＲＳ・ＰＳｉ （８）
となる。

ロボットの手先位置をＰ、各関節の関節角をΘ＝（θ１，θ２，・・・，θｍ）（ｍはロボット１の関節数）、ツールデータを含めたロボットの機構パラメータをＱ＝（ｑ１、ｑ２，・・・，ｑｌ）（ｌは機構パラメータ数）とすると、ロボット１の手先位置Ｐは、次式で表される。
Ｐ＝Ｔ（Θ、Ｑ） （９）

これらΘ、Ｑは、設計値Θ０、Ｑ０に対し偏差ΔΘ、ΔＱを持つことによりロボット１の位置決め誤差が生じる。Θ、Θ０、ΔΘ、Ｑ、Ｑ０、ΔＱの間には以下の関係がある。
Θ＝Θ０＋ΔΘ （１０）
Ｑ＝Ｑ０＋ΔＱ （１１）

式（８）〜（１１）より、次式が得られる。
ＰＲＳｉ＝Ｔ（Θ０＋ΔΘ、Ｑ０＋ΔＱ） （１２）
ＰＳｉ＝ＴＲＳ^−１ Ｔ（Θ０＋ΔΘ、Ｑ０＋ΔＱ） （１３）
ロボット座標系Ｏｒからセンサ座標系Ｏｓへの変換ＴＲＳ、関節角及び機構パラメータの偏差ΔΘ、ΔＱがキャリブレーションにより求める値である。

ここで、ＴＲＳ^−１＝（ｘｓｒ，ｙｓｒ，ｚｓｒ，ａｓｒ，ｂｓｒ，ｃｓｒ）とおく。（ａｓｒ，ｂｓｒ，ｃｓｒ）は姿勢を表し、それぞれＸ軸回りの回転、Ｙ軸回りの回転、Ｚ軸回りの回転を表す。また、Ｔ’（Θ、Ｑ）＝ＴＲＳ^−１ Ｔ（Θ、Ｑ）とおく。なお、以下では、座標変換や位置姿勢データを並進成分（ｘ，ｙ，ｚ）と回転成分（ａ，ｂ，ｃ）を組み合わせて便宜上（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）という形式で表記するが、演算時には同次変換行列の形式に変換して計算を行う。

ロボット座標系Ｏｒにおけるロボットの位置決め誤差ｄＰＲｉは、
ｄＰＲｉ＝ＰＲＳｉ−ＰＲｉ （１４）
となる。
ｉ番目の計測点で計測したときのＴＲＳ^−１、各関節角、機構パラメータが
Ｐｒｍｉ＝［ｘｓｒ ｙｓｒ ｚｓｒ ａｓｒ ｂｓｒ ｃｓｒ
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （１５）
であったとする。ここで、θ１ｉ ・・・ θｍｉは、ｉ番目の計測点に位置決めしたときの関節指令値である。

Ｐｒｍｉの時の行列Ｍｒｉを、

とおく。ここで、Ｔｘ’、Ｔｙ’、Ｔｚ’はそれぞれＴ’のｘ成分、ｙ成分、ｚ成分である。

Ｐｒｍｉの誤差を、
ｄＰｒｍ＝［δｘｓｒ δｙｓｒ δｚｓｒ δａｓｒ δｂｓｒ δｃｓｒ
δθ１ ・・・ δθｍ δｑ１ ・・・ δｑｌ ］^Ｔ （１７）
とすると、
ｄＰＲｉ＝Ｍｒｉ・ｄＰｒｍ （１８）
が成り立つ。

すべての計測位置姿勢について式（１８）が成り立つので、

とおくと、
ｄＰＲａ＝Ｍｒａ・ｄＰｒｍ （２１）
が成り立つ。これは、ｄＰｒｍを未知数とする線形連立方程式であるので、
ｄＰｒｍ＝（Ｍｒａ^Ｔ Ｍｒａ）^−１ Ｍｒａ^Ｔ・ｄＰＲａ （２２）
により、ｄＰｒｍの最小二乗解が求められる。

Ｐｒｍｉの初期値としてＴＲＳはロボット座標系Ｏｒから見たセンサ座標系Ｏｓの概略位置姿勢、θ１ｉ ・・・ θｍｉには計測時の関節角、ｑ１ ・・・ ｑｌには設計値を用い、式（１４）〜（２２）よりｄＰｒｍを求め、次式によりＰｒｍｉを更新する。
Ｐｒｍｉ_ｋ＋１＝Ｐｒｍｉ_ｋ＋ｄＰｒｍ （２３）

ここで、ｋはＰｒｍｉの更新回数である。ｄＰｒｍの各成分が十分小さくなったら、更新を終了し、そのときのＰｒｍｉの値を同定したパラメータとし、図３のステップＳ２１７を終了するとともに、図２のステップＳ２１を終了する。
ｄＰｒｍが十分小さくない場合は、Ｐｒｍｉ_ｋ＋１に含まれるΘ_i、Ｑ_iを用いてＰＲｉ＝Ｔ（Θ_i、Ｑ_i）により更新し、式（１４）〜（２２）からｄＰｒｍを求め、ｄＰｒｍが十分小さくなるまでＰｒｍｉを繰り返し更新する。

図２のステップＳ２１を終了すると、次にステップＳ２２に進み、ステップＳ２１で求めたパラメータを用いて同一位置に複数の姿勢で位置決めを行い、キャリブレーションを実施する。

図４は、図２のステップＳ２２（第２のキャリブレーション手段）の内容を詳細に説明したフローチャートである。ステップＳ２２１では、ステップＳ２１で求めたパラメータを用いて、計測時のロボット１の手先位置が同一の位置になるようにロボット１の位置姿勢をｎ２点生成する。計測位置姿勢点数ｎ２の値としては、２０〜４０点程度が望ましい。生成する計測位置は同一の位置とし、３次元形状計測器４の計測可能範囲の中心になるように決定する。また、姿勢についてはランダムに決定するが、計測時各関節角が様々な値をとるように決定する。

ステップＳ２２２では、計測点数ｉ２を１にセットする。ステップＳ２２３では、ステップＳ２２１で生成した第ｉ２番目の計測位置姿勢にロボット１を位置決めする。

ステップＳ２２４では、３次元形状計測器４を用いてプローブ３の先端の球の表面形状を計測し、球の中心位置を求める。球の表面形状データから球の中心位置の求め方は、ステップＳ２１４と同様である。

ステップＳ２２５では計測数ｉ２に１を加え、ステップＳ２２６では計測数ｉ２が計測位置姿勢点数ｎ２より大きい場合は、ステップＳ２２７、以下の場合はステップＳ２２４に戻り、計測を続ける。

ステップＳ２２７では、次のようにしてロボットの機構パラメータ、ツールデータ、３次元形状計測器の設置位置を求める。ステップＳ２２１からＳ２２６で計測したセンサ座標系Ｏｓにおける球の中心の計測位置をＰＳｉ＝[ｘｓｃｉ、ｙｓｃｉ、ｚｓｃｉ]^Ｔ（ｉ＝１，２，・・・，ｎ２）、ロボット座標系Ｏｒにおける球の中心位置をＰＲＳｉ、各計測時のロボットへの指令値をＰＲｉ＝[ｘｒｉ、ｙｒｉ、ｚｒｉ]^Ｔとする。ステップＳ２１で求めたロボット座標系Ｏｒからセンサ座標系Ｏｓの位置姿勢ＴＲＳを用いてＰＲＳｉは、
ＰＲＳｉ＝ＴＲＳ・ＰＳｉ （２４）
と求められる。

とすると、計測時の指令値ＰＲｉを、
ＰＲｉ’＝ＰＲｉ＋ＰＲＳＭ−ＰＲＳｉ （２６）
に修正することにより、ＰＲＳＭに高精度に位置決め可能となる。
このとき、ロボット座標系Ｏｒにおけるロボットの位置決め誤差ｄＰＲｉは、
ｄＰＲｉ＝ＰＲＳＭ−ＰＲｉ’ （２７）
となる。

ｉ番目の計測点で計測したときのＴＲＳ^−１、各関節角、機構パラメータが
Ｐｒｍｉ＝［ｘｓｒ ｙｓｒ ｚｓｒ
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （２８）
であったとする。このとき、ＴＲＳ^−１の成分ａｓｒ、ｂｓｒ、ｃｓｒはステップＳ２１で求めた値を用いる。

Ｐｒｍｉの時の行列Ｍｒｉを、

とおく。

Ｐｒｍｉの誤差を、
ｄＰｒｍ＝［δｘｓｒ δｙｓｒ δｚｓｒ
δθ１ ・・・ δθｍ δｑ１ ・・・ δｑｌ］^Ｔ （３０）
とすると、
ｄＰＲｉ＝Ｍｒｉ ｄＰｒｍ （３１）
が成り立つ。

すべての計測姿勢について式（３１）が成り立つので、

とおくと、
ｄＰＲａ＝Ｍｒａ・ｄＰｒｍ （３４）
が成り立つ。これはｄＰｒｍを未知数とする線形連立方程式であるので、
ｄＰｒｍ＝（Ｍｒａ^Ｔ Ｍｒａ）^−１ Ｍｒａ^Ｔ・ｄＰＲａ （３５）
により、ｄＰｒｍの最小二乗解が求められる。

Ｐｒｍｉの初期値としてＴＲＳはロボット座標系Ｏｒから見たセンサ座標系Ｏｓの概略位置姿勢、θ１ｉ ・・・ θｍｉには計測時の関節角、ｑ１ ・・・ ｑｌには設計値を用い、式（２７）〜（３５）よりｄＰｒｍを求め、次式によりＰｒｍｉを更新する。
Ｐｒｍｉ_ｋ＋１＝Ｐｒｍｉ_ｋ＋ｄＰｒｍ （３６）

ここで、ｋはＰｒｍｉの更新回数である。ｄＰｒｍの各成分が十分小さくなったら、更新を終了し、そのときのＰｒｍｉの値を同定したパラメータとして出力し、図４のステップＳ２２７を終了するとともに、図２のステップＳ２２を終了する。
ｄＰｒｍが十分小さくない場合は、Ｐｒｍｉ_ｋ＋１に含まれるΘ_i、Ｑ_iを用いてＰＲｉ＝Ｔ（Θ_i、Ｑ_i）により更新し、式（２７）〜（３５）からｄＰｒｍを求め、ｄＰｒｍが十分小さくなるまでＰｒｍｉを繰り返し更新する。

なお、ステップＳ２１１で計測位置を３次元形状計測器４の計測可能範囲内にまんべんなく分布するようにランダムに決定するとしたが、一定間隔で生成しても良い。例えば、３次元計測器４の計測可能範囲の中心位置に、Ｘ、Ｙ、Ｚ方向それぞれについて５ｍｍ間隔の位置を決定する。

また、ステップＳ２２では、同一位置に位置決めを行うとしたが、基準位置から実現したい位置精度の距離以内に位置決めしても良い。

また、ステップＳ２２の結果得られた機構パラメータ、ツールデータ、測定装置位置を用いて、ステップＳ２２を測定結果が所望の範囲に収まるまで繰り返すことにより高精度なキャリブレーションが可能となる。

また、プローブ３の先端に球を取り付け、その３次元形状を計測し、中心位置を求めることにより、ロボットの手先の位置としたが、球以外の形状、例えば直方体、３角錐などの形状など、３次元形状測定器で形状が測定でき、その形状からロボットの手先の位置が求められる形状であれば良い。

また、３次元形状測定器でロボットの手先に取り付けられた対象物の形状からロボットの手先位置を求めたが、ロボットの手先にＬＥＤなどのマーカーを取り付け、それを複数のカメラで撮像し、３次元位置を計測しても同様の効果が得られる。

このように、第１のキャリブレーション手段において複数の位置及び姿勢によりロボットを位置決めした時の誤差に基づきキャリブレーションを行って得たパラメータを使って第２のキャリブレーション手段で１点に複数の姿勢で位置決めし、キャリブレーションを行うことにより、繰り返してキャリブレーションすることが不要となり、簡便、短時間にロボットのキャリブレーションが可能となる。

また、第２のキャリブレーション手段において、計測器の最も精度の良い点でキャリブレーションを行うことにより、測定装置の測定位置に依存する計測精度のばらつきの影響を受けることなく、高精度にロボットのキャリブレーションが可能となる。

実施の形態２．
図５は、この発明の実施の形態２によるロボットのキャリブレーション装置を示す構成図である。図５において、図１と同一部分は同一符号を付してその説明は省略する。図１では、１箇所で３次元計測器４を用いてキャリブレーションを行ったが、図５に示すように複数箇所で計測を行い、キャリブレーションを行っても良い。図５に示す構成においては、３次元計測器４ａおよび４ｂの位置で計測し、キャリブレーションを行う。

キャリブレーション制御装置６で行われるキャリブレーション手順は、図２〜図４に示したフローチャートと同様であり、以下に処理内容の違う部分について説明する。図３において、ステップＳ２１１では、３次元形状計測器４で計測するロボット１の位置姿勢ｎ１点を生成するが、図５の２箇所の計測位置の計測点数がほぼ同じで、計測位置姿勢点数ｎ１の値としては、合計で２０〜４０点程度が望ましい。生成する計測位置は、例えば、３次元形状計測器４ａ、４ｂの計測可能範囲内にまんべんなく分布するようにランダムに決定する。また、姿勢についてもランダムに決定する。

ステップＳ２１７では、次のようにしてロボットの機構パラメータ、ツールデータ、３次元形状計測器の設置位置を求める。ステップＳ２１１からＳ２１６で計測したセンサ座標系Ｏｓにおける球の中心の計測位置をＰＳｉ＝［ｘｓｃｉ、ｙｓｃｉ、ｚｓｃｉ]^Ｔ（ｉ＝１，２，・・・，ｎ１）、ロボット座標系Ｏｒにおける球の中心位置をＰＲＳｉ、各計測時にロボット制御装置２で管理しているロボット先端の球の位置（以下、指令値と呼ぶ）をＰＲｉ＝［ｘｒｉ、ｙｒｉ、ｚｒｉ]^Ｔとする。

ロボット座標系Ｏｒから２つのセンサ座標系Ｏｓの位置姿勢をＴＲＳ１、ＴＲＳ２とすると、
ＰＲＳｉ＝ＴＲＳ１・ＰＳｉ （３７）
ＰＲＳｉ＝ＴＲＳ２・ＰＳｉ （３８）
となる。

ＴＲＳ１^−１＝（ｘｓｒ１、ｙｓｒ１、ｚｓｒ１、ａｓｒ１、ｂｓｒ１、ｃｓｒ１）、ＴＲＳ２^−１＝（ｘｓｒ２、ｙｓｒ２、ｚｓｒ２、ａｓｒ２、ｂｓｒ２、ｃｓｒ２）とおく。また、Ｔ’（Θ、Ｑ）＝ＴＲＳ^−１ Ｔ（Θ、Ｑ）とおく。

ロボット座標系Ｏｒにおけるロボットの位置決め誤差ｄＰＲｉは、
ｄＰＲｉ＝ＰＲＳｉ−ＰＲｉ （３９）
となる。

ｉ番目の計測点で計測したときのＴＲＳ１^−１、ＴＲＳ２^−１、各関節角、機構パラメータが
Ｐｒｍｉ＝［ｘｓｒ１ ｙｓｒ１ ｚｓｒ１ ａｓｒ１ ｂｓｒ１ ｃｓｒ１
ｘｓｒ２ ｙｓｒ２ ｚｓｒ２ ａｓｒ２ ｂｓｒ２ ｃｓｒ２
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （４０）
であったとする。ここで、θ１ｉ ・・・ θｍｉは、ｉ番目の計測点に位置決めしたときの関節指令値である。

Ｐｒｍｉの時の行列Ｍｒｉを、３次元形状計測器４ａで計測した場合、

とし、３次元形状計測器４ｂで計測した場合、

とする。

Ｐｒｍｉの誤差を、
ｄＰｒｍ＝［δｘｓｒ１ δｙｓｒ１ δｚｓｒ１ δａｓｒ１ δｂｓｒ１ δｃｓ ｒ１
δｘｓｒ２ δｙｓｒ２ δｚｓｒ２ δａｓｒ２ δｂｓｒ２ δｃｓ ｒ２
δθ１ ・・・ δθｍ δｑ１ ・・・ δｑｌ ］^Ｔ （４３）
とすると、
ｄＰＲｉ＝Ｍｒｉ・ｄＰｒｍ （４４）
が成り立つ。

すべての計測位置姿勢について式（４４）が成り立つので、

とおくと、
ｄＰＲａ＝Ｍｒａ・ｄＰｒｍ （４７）
が成り立つ。

これは、ｄＰｒｍを未知数とする線形連立方程式であるので、
ｄＰｒｍ＝（Ｍｒａ^Ｔ Ｍｒａ）^−１ Ｍｒａ^Ｔ・ｄＰＲａ （４８）
により、ｄＰｒｍの最小二乗解が求められる。

Ｐｒｍｉの初期値としてＴＲＳはロボット座標系Ｏｒから見たセンサ座標系Ｏｓの概略位置姿勢、θ１ｉ ・・・ θｍｉには計測時の関節角、ｑ１ ・・・ ｑｌには設計値を用い、式（３９）〜（４８）よりｄＰｒｍを求め、次式によりＰｒｍｉを更新する。
Ｐｒｍｉ_ｋ＋１＝Ｐｒｍｉ_ｋ＋ｄＰｒｍ （４９）

ここで、ｋはＰｒｍｉの更新回数である。ｄＰｒｍの各成分が十分小さくなったら、更新を終了し、そのときのＰｒｍｉの値を同定したパラメータとし、図３のステップＳ２１７を終了するとともに図２のステップＳ２１を終了する。

ｄＰｒｍが十分小さくない場合は、Ｐｒｍｉ_ｋ＋１に含まれるΘ_i、Ｑ_iを用いてＰＲｉ＝Ｔ（Θ_i、Ｑ_i）により更新し、式（３９）〜（４８）からｄＰｒｍを求め、ｄＰｒｍが十分小さくなるまでＰｒｍｉを繰り返し更新する。

図２のステップＳ２１を終了すると次にステップＳ２２に進み、ステップＳ２１で求めたパラメータを用いて同一位置に複数の姿勢で位置決めを行い、キャリブレーションを実施する。

図４のステップＳ２２１では、ステップＳ２１で求めたパラメータを用いて、計測時のロボット１の手先位置が各計測位置で同一の位置になるようにロボット１の位置姿勢を合計ｎ２点生成する。このとき、２箇所の計測位置の計測点数がほぼ同数になるようにする。計測位置姿勢点数ｎ２の値としては、２０〜４０点程度が望ましい。生成する計測位置は同一の位置とし、３次元形状計測器４ａ、４ｂの計測可能範囲の中心になるように決定する。また、姿勢についてはランダムに決定するが、計測時各関節角が様々な値をとるように決定する。

ステップＳ２２７では、次のようにしてロボットの機構パラメータ、ツールデータ、３次元形状計測器の設置位置を求める。ステップＳ２２１からＳ２２６で計測したセンサ座標系Ｏｓにおける球の中心の計測位置をＰＳｉ＝［ｘｓｃｉ、ｙｓｃｉ、ｚｓｃｉ]^Ｔ（ｉ＝１，２，・・・，ｎ２）、ロボット座標系Ｏｒにおける球の中心位置をＰＲＳｉ、各計測時のロボットへの指令値をＰＲｉ＝［ｘｒｉ、ｙｒｉ、ｚｒｉ]^Ｔとする。

ステップＳ２１で求めたロボット座標系Ｏｒからセンサ座標系Ｏｓの位置姿勢ＴＲＳ１、ＴＲＳ２を用いてＰＲＳｉは、
ＰＲＳ１ｉ＝ＴＲＳ１・ＰＳｉ （５０）
ＰＲＳ２ｉ＝ＴＲＳ２・ＰＳｉ （５１）
と求められる。

計測位置ごとに式（２５）によりＰＲＳＭをもとめ、計測時の指令値ＰＲｉを、
ＰＲｉ’＝ＰＲｉ＋ＰＲＳＭ−ＰＲＳｉ （５２）
に修正することにより、ＰＲＳＭに高精度に位置決め可能となる。

このとき、ロボット座標系Ｏｒにおけるロボットの位置決め誤差ｄＰＲｉは、
ｄＰＲｉ＝ＰＲＳＭ−ＰＲｉ’ （５３）
となる。

ｉ番目の計測点で計測したときのＴＲＳ１^−１、ＴＲＳ２^−１、各関節角、機構パラメータが、
Ｐｒｍｉ＝［ｘｓｒ１ ｙｓｒ１ ｚｓｒ１ ｘｓｒ２ ｙｓｒ２ ｚｓｒ２
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （５４）
であったとする。このとき、ＴＲＳ^−１の成分ａｓｒ、ｂｓｒ、ｃｓｒはステップＳ２１で求めた値を用いる。

３次元形状計測器４ａで計測したときの行列Ｍｒｉを、

とし、３次元形状計測器４ｂで計測したときの行列Ｍｒｉを、

とおく。

Ｐｒｍｉの誤差を、
ｄＰｒｍ＝［δｘｓｒ１ δｙｓｒ１ δｚｓｒ１
δｘｓｒ２ δｙｓｒ２ δｚｓｒ２
δθ１ ・・・ δθｍ δｑ１ ・・・ δｑｌ ］^Ｔ （５７）
とすると、
ｄＰＲｉ＝Ｍｒｉ・ｄＰｒｍ （５８）
が成り立つ。すべての計測姿勢について式（３１）が成り立つので、

とおくと、
ｄＰＲａ＝Ｍｒａ・ｄＰｒｍ （６１）
が成り立つ。これは、ｄＰｒｍを未知数とする線形連立方程式であるので、
ｄＰｒｍ＝（Ｍｒａ^Ｔ Ｍｒａ）^−１ Ｍｒａ^Ｔ・ｄＰＲａ （６２）
により、ｄＰｒｍの最小二乗解が求められる。

Ｐｒｍｉの初期値としてＴＲＳはロボット座標系Ｏｒから見たセンサ座標系Ｏｓの概略位置姿勢、θ１ｉ ・・・ θｍｉには計測時の関節角、ｑ１ ・・・ ｑｌには設計値を用い、式（５３）〜（６２）よりｄＰｒｍを求め、次式によりＰｒｍｉを更新する。
Ｐｒｍｉ_ｋ＋１＝Ｐｒｍｉ_ｋ＋ｄＰｒｍ （６３）

ここで、ｋはＰｒｍｉの更新回数である。ｄＰｒｍの各成分が十分小さくなったら、更新を終了し、そのときのＰｒｍｉの値を同定したパラメータとして出力し、図４のステップＳ２２７を終了するとともに図２のステップＳ２２を終了する。

ｄＰｒｍが十分小さくない場合は、Ｐｒｍｉ_ｋ＋１に含まれるΘ_i、Ｑ_iを用いてＰＲｉ＝Ｔ（Θ_i、Ｑ_i）により更新し、式（５３）〜（６２）からｄＰｒｍを求め、ｄＰｒｍが十分小さくなるまでＰｒｍｉを繰り返し更新する。

なお、ステップＳ２１１で計測位置を３次元形状計測器４の計測可能範囲内にまんべんなく分布するようにランダムに決定するとしたが、一定間隔で生成しても良い。例えば、３次元計測器４の計測可能範囲の中心位置に、Ｘ、Ｙ、Ｚ方向それぞれについて５ｍｍ間隔の位置を決定する。

図２において３次元形状計測器を２台設置した場合を記載したが、１台を移動して計測しても良い。また、２箇所で計測をした場合について記載したが、３箇所以上でも良い。

また、プローブ３の先端に球を取り付け、その３次元形状を計測し、中心位置を求めることにより、ロボットの手先の位置としたが、球以外の形状、例えば直方体、３角錐などの形状など、３次元形状測定器４で形状が測定でき、その形状からロボット１の手先の位置が求められる形状であれば良い。

また、３次元形状測定器４でロボット１の手先に取り付けられた対象物の形状からロボットの手先位置を求めたが、ロボット１の手先にＬＥＤなどのマーカーを取り付け、それを複数のカメラで撮像し、３次元位置を計測しても同様の効果が得られる。

このように、複数の位置でキャリブレーションを行うことにより高精度なキャリブレーションが可能となる。

実施の形態３．
図６は、この発明の実施の形態３によるロボットのキャリブレーション装置を示す構成図である。図６において、図１及び図５と同一部分は同一符号を付してその説明は省略する。図１６において、３次元形状測定器４は、搬送装置７上に設置され、搬送装置７によって移動し、複数位置でキャリブレーションを行う。

キャリブレーション制御装置６で行われるキャリブレーション手順は、図２〜４に示したフローチャートと同様であり、以下に実施の形態１及び２と処理内容の異なる部分について説明する。

３次元形状計測器４が搬送装置７によって位置決めされるため、複数の計測位置の相対位置が既知となる。今、２箇所で計測する場合を考え、ロボット座標系Ｏｒから２箇所のセンサ座標系Ｏｓの位置姿勢をＴＲＳ１、ＴＲＳ２とし、ＴＲＳ１を基準としたＴＲＳ２の相対位置がΔＴＲＳであるとすると、
ＴＲＳ２＝ＴＲＳ１・ΔＴＲＳ （６４）
となる。

これを式（３７）、（３８）に代入すると、
ＰＲＳｉ＝ＴＲＳ１・ＰＳｉ （６５）
ＰＲＳｉ＝ＴＲＳ１・ΔＴＲＳ・ＰＳｉ （６６）
となる。ここで、ΔＴＲＳは既知であるので、ΔＴＲＳ ＰＳｉをＰＳｉ’とすれば、実施の形態１の式（８）以降と同様に処理可能となる。

実施の形態４．
また、図６に示す実施の形態３の構成において、搬送装置７の移動量しか既知でなく、ΔＴＲＳのすべての成分が既知とならない場合は、以下のようにしてキャリブレーションを行う。

搬送装置座標系ＯｃをＸ軸が搬送装置の移動方向に一致し、原点が第１の計測位置のセンサ座標系Ｏｓと一致するようにとり、ロボット座標系Ｏｒから搬送装置座標系Ｏｃへの変換をＴＲＣ、搬送装置座標系から第１の計測位置のセンサ座標系Ｏｓへの変換をＴＣＳ１＝（０、０、０、ａｃｓ、ｂｃｓ、ｃｃｓ）とすると、ロボット座標系Ｏｒから見た第１の計測位置のセンサ座標系Ｏｓの位置姿勢ＴＲＳ１は、
ＴＲＳ１＝ＴＲＣ・ＴＣＳ１ （６７）
となる。

第２の計測位置が、搬送装置座標系において原点から既知の距離Ｌにあるとすると、ΔＴＣＳ２＝（Ｌ，０，０、０、０、０）を用いて、ロボット座標系Ｏｒから見た第２の計測位置のセンサ座標系Ｏｓの位置姿勢ＴＲＳ２は、
ＴＲＳ２＝ＴＲＣ・ΔＴＣＳ２ ＴＣＳ１ （６８）
となる。

座標変換ＴＲＣ、ＴＣＳ１の回転行列をＲＲＣ、ＲＣＳ１とすると、ロボット座標系Ｏｒから見たセンサ座標系Ｏｓの回転行列ＲＲＳは、
ＲＲＳ＝ＲＲＣ・ＲＣＳ１ （６９）
となる。

回転行列ＲＲＳの各座標軸成分表記を（ａｒｓ ｂｒｓ ｃｒｓ）とすると、第１のキャリブレーション手段における未知のパラメータＰｒｍｉは、
Ｐｒｍｉ＝［ｘｒｃ ｙｒｃ ｚｒｃ ａｒｓ ｂｒｓ ｃｒｓ
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （７０）
となる。

Ｐｒｍｉに対応して式（１６）〜（２３）と同様に行列Ｍｒｉ、ｄＰｒｍを作成し、上記パラメータＰｒｍｉを求めることが可能である。

また、ステップＳ２２では、未知のパラメータＰｒｍｉを
Ｐｒｍｉ＝［ｘｒｃ ｙｒｃ ｚｒｃ
θ１ｉ ・・・ θｍｉ ｑ１ ・・・ ｑｌ ］^Ｔ （７１）
とし、式（２９）〜（３６）と同様に処理することにより、未知の機構パラメータを算出する。

このように、複数計測位置の相対位置が既知であることにより、より高精度なキャリブレーションが可能となる。

この発明の実施の形態１によるキャリブレーション装置を示す構成図である。 図１のキャリブレーション制御装置６で行われるキャリブレーション手順を表したフローチャートである。 図２のステップＳ２１（第１のキャリブレーション手段）の内容を詳細に説明したフローチャートである。 図２のステップＳ２２（第２のキャリブレーション手段）の内容を詳細に説明したフローチャートである。 この発明の実施の形態２によるキャリブレーション装置を示す構成図である。 この発明の実施の形態３及び４によるキャリブレーション装置を示す構成図である。

符号の説明

１ ロボット（多関節ロボット）、２ ロボット制御装置、４，４ａ，４ｂ 測定装置、６ キャリブレーション制御装置、７ 搬送装置。

Claims (7)

1. 多関節ロボットのアーム先端に取り付けたキャリブレーション用治具と、
   前記キャリブレーション用治具の位置を計測する測定装置と、
   前記測定装置の計測結果と計測時の前記ロボットへの指令値から前記ロボットの機構パラメータを算出し、前記ロボットのキャリブレーションを行うキャリブレーション制御装置と
   を備え、
   前記キャリブレーション制御装置は、
   前記キャリブレーション用治具を前記測定装置の測定可能範囲に複数の位置姿勢で位置決めし、位置決めした位置と前記測定装置の測定値とから前記ロボットの機構パラメータ、前記キャリブレーション治具のツールパラメータ及び前記測定装置の設置位置姿勢を求める第１のキャリブレーション手段と、
   前記第１のキャリブレーション手段で求めた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、前記測定装置の計測範囲の一点からキャリブレーション対象の前記ロボットで実現したい位置精度の距離以内に複数の姿勢で位置決めし、再度、機構パラメータ、ツールパラメータ及び設置位置姿勢を求める第２のキャリブレーション手段と
   を有することを特徴とするロボットのキャリブレーション装置。
2. 請求項１に記載のキャリブレーション装置において、
   前記第２のキャリブレーション手段は、得られた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、測定結果が所望の範囲に収まるまで繰り返す
   ことを特徴とするロボットのキャリブレーション装置。
3. 請求項１または２に記載のキャリブレーション装置において、
   前記第２のキャリブレーション手段は、前記測定装置の計測範囲の同一位置に複数の姿勢で位置決めする
   ことを特徴とするロボットのキャリブレーション装置。
4. 請求項１から３までのいずれか１項に記載のキャリブレーション装置において、
   複数の計測位置でキャリブレーションを行う
   ことを特徴とするロボットのキャリブレーション装置。
5. 請求項４に記載のキャリブレーション装置において、
   前記複数の計測位置の相対位置が既知である
   ことを特徴とするロボットのキャリブレーション装置。
6. 請求項５に記載のキャリブレーション装置において、
   前記測定装置を移動させる搬送装置をさらに備え、
   前記測定装置は、前記搬送装置によって位置決めされる
   ことを特徴とするロボットのキャリブレーション装置。
7. 多関節ロボットのアーム先端に取り付けたキャリブレーション用治具の位置を計測し、その計測結果と計測時の前記ロボットへの指令値から前記ロボットの機構パラメータを算出し、前記ロボットのキャリブレーションを行うロボットのキャリブレーション方法であって、
   前記キャリブレーション用治具を前記測定装置の測定可能範囲に複数の位置姿勢で位置決めし、位置決めした位置と前記測定装置の測定値とから前記ロボットの機構パラメータ、前記キャリブレーション治具のツールパラメータ及び前記測定装置の設置位置姿勢を求める第１のキャリブレーションステップと、
   前記第１のキャリブレーションステップで求めた機構パラメータ、ツールパラメータ及び設置位置姿勢を用いて、前記測定装置の計測範囲の一点からキャリブレーション対象のロボットで実現したい位置精度の距離以内に複数の姿勢で位置決めし、再度、機構パラメータ、ツールパラメータ及び設置位置姿勢を求める第２のキャリブレーションステップと
   を有することを特徴とするロボットのキャリブレーション方法。

Images