JP2010142901A

JP2010142901A - ロボットのキャリブレーション方法及びロボットの制御装置

Info

Publication number: JP2010142901A
Application number: JP2008322342A
Authority: JP
Inventors: Naoya Kagawa; 尚哉香川; Tetsuya Sato; 哲也佐藤
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2008-12-18
Filing date: 2008-12-18
Publication date: 2010-07-01
Anticipated expiration: 2028-12-18
Also published as: US8369993B2; US20100168915A1; DE102009059221A1; JP5083194B2

Abstract

【課題】ロボット物理パラメータを演算する過程に誤差が含まれる要素を極力排除して、推定精度を向上させることができるロボットのキャリブレーション方法を提供する。
【解決手段】ロボットアームの手先を複数（ｎ）の位置に移動させた場合、位置計測器４により計測された手先の位置姿勢と制御上で指令した位置との誤差ΔφをそれぞれΔφ１，Δφ２，…，Δφｎとして求めると、任意の１つΔφｍを基準としてその他の誤差Δφｘとの差Δεｙを式により求め、各Δεｙの絶対値の和が所定値ε０以下となるまで各Δεｙを算出する元になるパラメータを収束計算する。
【選択図】図１

Description

本発明は、多関節型ロボットアームの手先位置を、制御指令に一致させるようキャリブレーションする方法，及びそのキャリブレーション機能を備えたロボットの制御装置に関する。

多関節型のアーム（マニピュレータ）を備えるロボットについて手先の位置を制御する場合、機械的な誤差等が存在するため、与えた制御指令と実際の到達位置とが一致するようにキャリブレーション（校正）を行う必要がある。例えば、特許文献１に開示されている方法では、座標上で計算される手先の位置姿勢と、外部計測機器により計測される手先の位置姿勢との誤差Δｒを求め、その誤差Δｒをヤコビ行列により各関節の角度に対するキャリブレーション量（オフセット量）Δθに変換する。そして、実際の各関節角度の検出値θｓにオフセット量Δθを加えて再度座標上の位置姿勢を計算して再び誤差Δｒを求め、誤差Δｒが許容値以下に収束するまで上記の過程を繰り返すようにしている。

以下、その詳細について図４を参照して説明する。図４は、例えば６軸垂直多関節型ロボットについてキャリブレーションを行う場合のシステム構成をモデル的に示すものである。ロボット１のアーム先端はフランジ２であり、フランジ２にツール３が取り付けられる。尚、図４においては、煩雑になるのを避けるためツール自体は図示しておらず、ツール３の先端位置に符号「３」を付して示している。そして、ツール３の先端がロボット１の「手先」となる。

位置計測器４は、例えばカメラ（或いは赤外線センサやレーザトラッカなど）であり、ロボット１の「手先」を撮像するなどしてその位置や姿勢を特定するための計測結果（視覚情報）を視覚センサコントローラ５に出力する。視覚センサコントローラ５は、位置計測器４の計測結果に基づいてロボット１の手先の位置姿勢情報を生成し、その情報を制御計算機６に出力する。
また、ロボット１の各軸に対応する間接機構部には、関節角度検出器７が配置されており、各関節角度の検出結果はロボットコントローラ（アーム制御手段）８に出力される。ロボットコントローラ８は、制御プログラムと上記各関節角度の検出結果とに基づいて、ロボット１の動作を制御すると共に、上記各関節角度の情報を制御計算機６に出力する。そして、制御計算機６は、与えられる各情報に基づいてキャリブレーションのための演算を行う。

図４に示すように、座標系としては、位置計測器４の位置を原点とする座標系（Ｘ_０，Ｙ_０，Ｚ_０）と、ロボット１の基部であるベースを原点とする座標系（Ｘ_ｒ，Ｙ_ｒ，Ｚ_ｒ）と、フランジ２を原点とする座標系（Ｘ_ｆ，Ｙ_ｆ，Ｚ_ｆ）とがある。そして、位置計測器４がツール３の先端を計測点として、その位置姿勢を計測する場合、座標系（Ｘ_０，Ｙ_０，Ｚ_０）の原点よりツール３の先端までのベクトルを表す同次変換行列をＴ_ｍｅａとする。

また、制御計算機６において計算される、上記同次変換行列Ｔ_ｍｅａに対応する同次変換行列をＴ_ｒｏｂとする。更に、同様に計算される各同次変換行列を以下のように定義する。
^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}：位置計測器からロボットベースまでのベクトル
^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ}：ロボットベースからロボットフランジまでのベクトル
^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ：ロボットフランジからツール先端までのベクトル
行列Ｔ_ｍｅａを除き、計算上の各同次変換行列の関係は（０）式のようになる。
Ｔ_rob＝^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ}
・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ …（０）
そして、計測に基づく行列Ｔ_ｍｅａと、計算に基づく行列Ｔ_robとの差が、誤差Δφ_Ｔとなる。

上記のようなロボット１の各軸角度と、手先の位置姿勢との関係は、一般に（１）式のような４行４列の同次変換行列φで表わされる。

関数ｆの変数θは各軸の角度であり、ｑはキャリブレーションの対象となる変数（パラメータ）であり、ロボット１の物理パラメータや、フランジ２とツール３との関係（位置姿勢），位置計測器４とロボット１との関係などを示すものである。そして、行列Ｒは、ロボット１の座標系（Ｘ_ｒ，Ｙ_ｒ，Ｚ_ｒ）からみた手先のノーマルベクトル，オリエントベクトル，アプローチベクトルの各成分を示すもので（回転行列）、行列Ｐは上記手先の位置ベクトルを示す。

また、手先位置姿勢の誤差Δφと、キャリブレーション対象変数との関係は、ヤコビ行列Ｊ_ｑ（θ，ｑ）と、キャリブレーション対象変数の誤差ベクトルΔｑとによって（２）式のように表わされる。

そして、図４に示す誤差Δφ_Ｔは、（１）式を用いて（３）式のように表わされる。

その結果、誤差Δφは（４）式のように表現することができる。

すなわち（３）式において、ノーマル，オリエン，アプローチの各ベクトルについては、それらの内２つベクトルが決まると、それらの外積が残りの１つベクトルになるので、（４）式ではノーマルベクトルの誤差を省いた９行１列の行列で表現している。したがって、省くのはノーマルベクトルに限ることなく、オリエント，アプローチの何れかのベクトルでも良い。

また、（２）式を変形すると（５）式となる。

（５）式の右辺でΔφ以外の部分は、ヤコビ行列Ｊ_ｑ（θ，ｑ）の擬似逆行列となっている。

また、（２）式をｎ点について拡張すると（６）式となるから、

それに応じて（５）式を拡張すれば（７）式となる。

そして、キャリブレーションは、（７）式により誤差ベクトルΔｑを求め、その誤差ベクトルΔｑを設定したパラメータｑに加えて更新し、
ｑ＝ｑ＋Δｑ …（８）
（９）式で表わされる誤差Δφの和Δφ_allが、所定の閾値ε以下になるまで、同様の計算を繰り返すことで行われる。

尚、特許文献１では、キャリブレーション対象のパラメータが各軸角度のオフセット（各軸原点誤差）のみであるためヤコビ行列が正則になり、（５）式のように擬似逆行列を用いることなく、逆行列Ｊ^−１を用いて計算している。
特開平７−６８４８０号公報

キャリブレーションを行うのに求める必要があるのは、ツールや計測器とロボットとの関係などを示すロボット物理パラメータｑである。しかしながら、特許文献１の方法では、ロボット物理パラメータ以外に、計測器とロボットベースとの関係を示す位置姿勢，及びフランジ（アーム先端）とツール先端との関係を示す位置姿勢も求める必要がある。すなわち、後者の２つは、ロボットが出荷された工場先等の環境で異なるため未知の値であり、ロボット物理パラメータを計算するのに必要だからである。そのため、ロボット物理パラメータを演算する過程に、後者の２つが含んでいる誤差も織り込まれることになり、推定精度が劣化するという問題があった。

また、特許文献１では、絶対位置精度の向上を目指しているが、絶対位置精度は通常計測が非常に困難であり、ある点を基準とした相対位置姿勢関係で精度評価を代用する場合が多い。実際にロボットを適用する工場，ラインなどの現場では、ある点を教示し、その点を基準とする相対位置姿勢関係で他の教示点を教示するような使い方を行う。従来の手法では、相対位置姿勢精度には着目しておらず、現場での使い方ではそれほど精度が改善されない場合がある。

本発明は上記事情に鑑みてなされたものであり、その目的は、ロボット物理パラメータを演算する過程に誤差が含まれる要素を極力排除して、推定精度を向上させることができるロボットのキャリブレーション方法，及びそのキャリブレーション機能を備えたロボットの制御装置を提供することにある。

請求項１記載のロボットのキャリブレーション方法によれば、多関節型ロボットアームの手先を複数（ｎ）の位置に移動させた場合、外部の計測手段により計測された手先の位置と、制御上で指令した位置との誤差Δφを、それぞれΔφ１，Δφ２，…，Δφｎとして求める。そして、任意の１つΔφｍを基準としてその他の誤差Δφｘとの差（誤差間差と称す）Δεｙを求め、各Δεｙの絶対値の和が所定値ε０以下となるまで各Δεｙを算出する元になるパラメータを収束計算する。

より具体的には、パラメータ行列ｑ，ロボットアームの各軸角度の行列θについてヤコビ行列Ｊを求め、誤差間差Δεｙとヤコビ行列Ｊの擬似逆行列との積からパラメータ行列ｑの誤差行列Δｑを求める。この場合、誤差行列Δｑは、上述したように誤差間差Δεｙに、ヤコビ行列Ｊの擬似逆行列（特定の条件下では逆行列）を乗じることで得られる。そして、パラメータ行列ｑに誤差行列Δｑを加算してパラメータ行列ｑを更新すると、更新したパラメータ行列ｑに基づいてロボットアームの手先位置を再計算し、パラメータ行列ｑを収束させる。

すなわち、基準誤差Δφｍとその他の誤差Δφｘとの差を求める「相対位置姿勢誤差」という考え方を導入したことにより、誤差間差Δεｙを示す行列式では一部の演算項目が消去されるようになる。その結果、誤差間差Δεｙをより高い精度で計算できるようになり、相対的に得られる位置関係の精度を向上させることができる。

請求項２記載のロボットのキャリブレーション方法によれば、基準誤差Δφｍを、複数の位置につき求めて誤差Δφｍを得ると、それら複数の基準誤差Δφｍについて誤差間差Δεｙ，ヤコビ行列Ｊ，誤差行列Δｑを求める。そして、異なる基準誤差Δφｍについて求めた誤差間差Δεｙの絶対値の和が、所定値ε１以下となるまで各Δεｙを算出する元になるパラメータを収束計算する。この場合、所定値ε１は、請求項１における所定値ε０に等しく設定しても良いし、異なる値に設定しても良い。斯様な方法によれば、ロボットアームの動作領域のより広い範囲についてキャリブレーションが行われるので、ロボットアームの制御精度を向上させることができる。

請求項３記載のロボットのキャリブレーション方法によれば、ロボットアームの手先につき、複数の位置と共に姿勢についても外部の計測手段で計測し、且つ、制御上で指令した姿勢との誤差も求めて誤差Δφに含める場合、ｎを、推定したい未知のパラメータ数を６で除した商よりも大に設定する。すなわち、手先の位置姿勢をキャリブレーションする場合、計測によって得られるパラメータの個数は６となるので、ｎ＞（未知パラメータ数／６）とすれば、未知のパラメータのキャリブレーションを確実に行うことができる。

請求項４記載のロボットのキャリブレーション方法によれば、誤差間差Δεｙ（＝ｘ）を、各同次変換行列及び位置ベクトルにより、次式

で計算する。尚、請求項４に記載したノーマル，オリエント，アプローチの各ベクトルと、ロボットのベース，フランジやツールについては、背景技術で説明したものと同じものを意味する。この場合、背景技術で示した（６）式に比較すると、計測手段からロボットベースまでの位置ベクトルに相当する成分が消去される結果、計算を行う対象が３変数分少なくなっている。したがって、その分だけ誤差間差Δεｙの推定精度を向上させることができる。

請求項５乃至８記載のロボットの制御装置によれば、アーム制御手段，誤差計算手段，キャリブレーション手段を備え、そのキャリブレーション手段を、ヤコビ行列計算手段と、誤差行列計算手段と、パラメータ行列更新手段とにより構成することで、請求項１乃至４のキャリブレーション方法をそれぞれ実行することができる。

（第１実施例）
以下、本発明の第１実施例について図１及び図２を参照して説明する。図１は図４相当図であり、システム構成は全く同様であるが、制御計算機（制御装置）６によって行われるキャリブレーションの手法が異なることを示している。すなわち、図４のケースでは、ツール３の先端であるロボット１の「手先」を、１つの位置姿勢について位置計測器（計測手段）４による計測結果と、制御計算機６による制御結果との誤差を求めることでキャリブレーションを行っている。これに対して、本実施例では、ロボット１の「手先」を位置姿勢（１）と、位置姿勢（ｍ）との２つに制御した場合に、それぞれについて位置計測器４による計測結果と、制御計算機６による制御結果との誤差を求めてキャリブレーションする。尚、位置姿勢（ｍ）を基準とする。

位置計測器４の座標系（Ｘ_０，Ｙ_０，Ｚ_０）の原点よりツール３先端の位置姿勢（１）までのベクトルを表す同次変換行列をＴ_{ｍｅａ，１}とし、制御計算機６において計算される、上記同次変換行列Ｔ_{ｍｅａ，１}に対応する同次変換行列をＴ_{ｒｏｂ１と}する。また、位置姿勢（ｍ）までのベクトルを表す同次変換行列を、それぞれＴ_{ｍｅａ，ｍ}，
Ｔ_{ｒｏｂ，ｍ}とする。

そして、位置姿勢（１），（ｍ）について、相対位置姿勢の差、すなわち、計測による相対位置姿勢間の差（Ｔ_{ｍｅａ，１}−Ｔ_{ｍｅａ，ｍ}）と、計算上の相対位置姿勢間の差
（Ｔ_{ｒｏｂ，１}−Ｔ_{ｒｏｂ，ｍ}）とについて差を求めると、（１０）式となる。
（Ｔ_{ｍｅａ，１}−Ｔ_{ｍｅａ，ｍ}）−（Ｔ_rob，１−Ｔ_{ｒｏｂ，ｍ}）
＝（Ｔ_{ｍｅａ，１}−Ｔ_{ｒｏｂ，１}）−（Ｔ_{ｍｅａ，ｍ}−Ｔ_{ｒｏｂ，ｍ}）
＝Δφ１−Δφｍ …（１０）
この（１０）式と、（６）式より、

となり、対応するΔｑは（１２）式のように表わされる。

ここで、位置姿勢（１）を一般化するよう拡張した位置姿勢（ｎ）について得られる誤差Δφ_Ｔ，ｎは
Δφ_Ｔ，ｎ＝Ｔ_{ｍｅａ，ｎ}−Ｔ_ｒｏｂ …（１３）
である。但しここでは、位置姿勢は（１）〜（ｎ）まで最大でｎ個あるとして、パラメータｎは、（１〜ｎ）の何れかを示す変数（特許請求の範囲ではｘに相当）として扱っている。またｎは、手先の位置姿勢についてキャリブレーションする場合、位置計測器４により得られるパラメータ個数が６となるから、その他の未知パラメータ数を６で除した商よりも大きくなるように設定する。（０）式より
Δφ_Ｔ，ｎ＝Ｔ_{ｍｅａ，ｎ}−^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}
・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｎ}
・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ …（１４）
となる。また、同次変換行列Ｔ_{ｍｅａ，ｎ}，^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｎ}を、それぞれ（１５），（１６）式で定義する。

これらを用いて、位置姿勢（ｎ）に関する誤差Δφ_Ｔ，ｎと、位置姿勢（ｍ）に関する誤差Δφ_Ｔ，ｍとの差（誤差間差Δεｙ（ｙ＝ｎ））を求める。
Δφ_Ｔ，ｎ−Δφ_Ｔ，ｍ
＝Ｔ_{ｍｅａ，ｎ}−^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}
・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｎ}・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ
−（Ｔ_{ｍｅａ，ｍ}−^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}
・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｍ}・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ）
＝（Ｔ_{ｍｅａ，ｎ}−Ｔ_{ｍｅａ，ｍ}）
−（^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｎ}
・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ）
＋（^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}・^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｍ}
・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ）
＝（Ｔ_{ｍｅａ，ｎ}−Ｔ_{ｍｅａ，ｍ}）
−^{ｍｅａｂａｓｅ}Ｔ_{ｒｏｂｂａｓｅ}・（^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｎ}
−^{ｒｏｂｂａｓｅ}Ｔ_{ｆｌａｎｇｅ，ｍ}）・^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ …（１７）
（１７）式は、更に以下のように展開される。

尚、（１９）式における^{ｍｅａｂａｓｅ}Ｒ_{ｒｏｂｂａｓｅ}，
^{ｍｅａｂａｓｅ}Ｒ_{ｒｏｂｂａｓｅ，ｎ}，^{ｍｅａｂａｓｅ}Ｒ_{ｒｏｂｂａｓｅ，ｍ}，
^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ，ｎ}，^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ，ｍ}，
^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌは、それぞれ特許請求の範囲（請求項４，８）における
^ｍｂＲ_ｒｂ，^ｍｂＲ_ｒｂ，ｘ，^ｍｂＲ_ｒｂ，ｍ，^ｍｂＰ_ｒｂ，ｘ，^ｍｂＰ_ｒｂ，ｍ，
^ｆｌＴ_ｔｏｏｌに対応している。

（１９）式では、（１８）式中の^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ}が消去されている。これは位置計測器４からロボットベースまでの位置ベクトルであり、３つの変数からなる。キャリブレーションの対象となるのはロボット１の物理パラメータであるが、従来の手法では、そのロボット１の物理パラメータを得るために、
計測器４とロボットベースとの関係を示す位置，姿勢
：^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ}，^{ｍｅａｂａｓｅ}Ｒ_{ｒｏｂｂａｓｅ}
ロボットフランジと手先との関係を示す位置，姿勢：^{ｆｌａｎｇｅ}Ｔ_ｔｏｏｌ
を計算する必要があった。これに対して本実施例によれば、（１９）式を計算する場合、位置ベクトル^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ}の３変数を求める必要がなくなり、その分だけ誤差間差Δεｙ（ｙ＝ｎ）の計算精度が向上する。そして、誤差間差Δεｙが求まれば（１２）式でΔｑが計算できるので、以降は従来と同様にキャリブレーションを行うことができる。したがって、結果としてキャリブレーションの精度が向上する。

図２は、上記の手法で誤差間差Δεｙの計算を行うプロセスを含む、キャリブレーション全体の処理内容を示すフローチャートである。先ず、ロボット１のアームの可動領域内で基準位置姿勢（ｍ）を決定し（ステップＳ１）、上記可動領域の全体を網羅するようにキャリブレーション位置姿勢（１，２，…，ｎ）を決定する（ステップＳ２）。それから、制御計算機６が、選択した位置姿勢の１つに「手先」であるツール３の先端を移動させ（ステップＳ３）、位置計測器４がその位置姿勢について計測を行った結果を取得して保存する（ステップＳ４）。また、制御計算機６は、ロボット１の各関節の角度θを関節角度検出器７より取得して記憶する（ステップＳ５）。

そして、全ての位置姿勢（ｎ）について計測を行うまで（ステップＳ６：ＮＯ）、ステップＳ３に戻って処理を繰り返す。全ての計測が完了すると（ステップＳ６：ＹＥＳ）、全ての位置姿勢について計算上の位置姿勢を求め（ステップＳ７）、各位置姿勢について誤差Δφを求めると（誤差計算手段）、（１９）式により誤差間差Δεｙ（ｙ＝１〜ｎ：但しｍを除く）を全ての位置姿勢について計算する（ステップＳ８，誤差間差計算手段）。

続いて、誤差間差Δεｙの総和Δε_ａｌｌを求め、そのΔε_ａｌｌが所定値ε０以下であるか否かを判断する（ステップＳ９）。

Δε_ａｌｌ＞ε０であれば（ＮＯ）、キャリブレーション対象とするパラメータｑを決定し（ステップＳ１１）、そのパラメータｑ及び各軸角度θに対応するヤコビ行列Ｊを計算する（ステップＳ１２，ヤコビ行列計算手段）。それから、（１２）式で誤差行列Δｑを計算すると（ステップＳ１３，誤差行列計算手段）、パラメータ行列ｑを更新し（ｑ＝ｑ＋Δｑ，ステップＳ１４，パラメータ行列更新手段）ステップＳ７に戻り処理を繰り返す。尚、ステップＳ９〜Ｓ１４が、キャリブレーション手段に対応する。

以上の処理を繰り返すことで、ステップＳ９においてΔε_ａｌｌ≦ε０となると（ＹＥＳ）、その時点で得られているキャリブレーションデータ，すなわちロボット１のパラメータｑを保存して（ステップＳ１０）キャリブレーションを終了する。

以上のように本実施例によれば、ロボットアームの手先を複数（ｎ）の位置に移動させた場合、位置計測器４により計測された手先の位置姿勢と制御上で指令した位置との誤差ΔφをそれぞれΔφ１，Δφ２，…，Δφｎとして求めると、任意の１つΔφｍを基準としてその他の誤差Δφｘとの差Δεｙを（１９）式により求め、各Δεｙの絶対値の和が所定値ε０以下となるまで各Δεｙを算出する元になるパラメータを収束計算する。
具体的には、パラメータ行列ｑ，ロボットアームの各軸角度の行列θについてヤコビ行列Ｊを求め、誤差間差Δεｙとヤコビ行列Ｊの擬似逆行列との積からパラメータ行列ｑの誤差行列Δｑを求め、パラメータ行列ｑに誤差行列Δｑを加算してパラメータ行列ｑを更新すると、更新したパラメータ行列ｑに基づいてロボットアームの手先位置を再計算し、パラメータ行列ｑを収束させるようにした。

すなわち、「相対位置姿勢誤差」という考え方を導入したことにより、背景技術で示した（４）式に比較して、位置計測器４からロボットベースまでの位置ベクトル^{ｍｅａｂａｓｅ}Ｐ_{ｒｏｂｂａｓｅ}を消去でき、計算を行う対象を３変数分少なくして誤差間差Δεｙの推定精度を向上させることができ、相対的に得られる位置関係の精度を向上させることができる。また、測定対象とする位置姿勢の数ｎを、推定したい未知のパラメータ数を６で除した商よりも大に設定するので、未知のパラメータのキャリブレーションを確実に行うことができる。

（第２実施例）
次に、本発明の第２実施例について説明する。第２実施例のシステム構成は基本的に第１実施例と同様である。第１実施例では、基準位置姿勢（ｍ）を１点のみ選択した場合についてキャリブレーションを行ったが、第２実施例では、基準位置姿勢（ｍ）を複数選択してキャリブレーションを行う。基準位置姿勢（ｍ）をｌ点選択すると、誤差行列Δｑは（１２）式に替えて（２１）式で表わされる。

また、図２のフローチャートでは、ステップＳ１において、基準位置姿勢（ｍ）をｌ点選択することになり、ステップＳ６において計測完了を判断する場合には、ステップＳ３の実行前に基準位置姿勢（ｍ）を１〜ｌまで順次変更した結果、ｌに達していれば完了と判断することになる。

また、誤差間差Δεｙについても、基準位置姿勢（ｍ）を１〜ｌまで順次変更することで、第１実施例のステップＳ８に対してｌ倍の誤差間差Δεｙを積算することになる。この場合、誤差収束の判定は、誤差間差Δεｙの総和を例えば所定値ε１と比較することで行うようにするが、所定値ε１は、第１実施例の所定値ε０に等しく設定しても良いし、異なる値に設定しても良い。斯様な方法によれば、ロボットアームの動作領域のより広い範囲についてキャリブレーションが行われるので、ロボットアームの制御精度を向上させることができる。

（第３実施例）
図３は本発明の第３実施例を示すものである。第１実施例では、図２に示すフローチャートのステップＳ２で決定するキャリブレーション用の位置姿勢が、動作範囲の全領域について概ね偏りを生じることなく適切に選択された結果、ヤコビ行列Ｊの擬似逆行列が確実に得られる場合を前提としている。
しかしながら、極めてまれなケースであるが、上記キャリブレーション用の位置姿勢の選択が全領域中の特定の範囲に集中していたり、誤って同じ位置姿勢を複数選択する等した場合には、ヤコビ行列Ｊの擬似逆行列が得られなくなる場合がある。そこで、第３実施例では、そのようなケースも考慮して対応した場合の処理を示す。

ヤコビ行列Ｊの擬似逆行列が得られなくなる場合、具体的には、以下に示す３つの事象の発生が想定される。
（１）Δｑを計算する（１２）式において、右辺第１項及び第２項の絶対値が０になる場合。すなわち、（２２）式の行列式Ｓが０になる場合である。

（２）最終的に推定したパラメータ行列ｑと、推定前の行列ｑとの差の絶対値が、ユーザが指定する閾値Δｑ＿０を超えた場合。
（３）誤差の総和Δε_ａｌｌが、ユーザが指定するループ上限回数Ｘ＿max回以内の間に、閾値ε０以下に収束しない場合。
これらの事象が発生した場合には、ステップＳ２におけるキャリブレーション用の位置姿勢を選択し直して再度計算を行うようにする。

図３は、第１実施例における図２のフローチャートに、上記事象の発生に対応した処理を追加したものである。すなわち、ステップＳ１,Ｓ２の間に、ループ回数をカウントするカウンタｘをリセットするステップＳ２０を追加し、ステップＳ９,Ｓ１０の間に、上記（２）に対応して、推定したパラメータ行列ｑと推定前の行列ｑとの差の絶対値が、閾値Δｑ＿０以下になっているか否かを判断するステップＳ２１を挿入する。ここで「ＹＥＳ」と判断すればステップＳ１０に移行し、「ＮＯ」と判断すればステップＳ２０に移行する。
また、ステップＳ９で「ＮＯ」と判断した場合は、上記のカウンタｘをインクリメントし（ステップＳ２２）、上記（３）に対応して、ループ回数ｘが上限回数Ｘ＿max未満か否かを判断する（ステップＳ２３）。ここで「ＹＥＳ」と判断すればステップＳ１１に移行し、「ＮＯ」と判断すればステップＳ２０に移行する。

また、ステップＳ１２でヤコビ行列Ｊを算出すると、上記（１）に対応して、（２２）式の行列式Ｓを求め、その行列式Ｓが０でないか否かを判断する（ステップＳ２４）。ここで「ＹＥＳ」と判断すればステップＳ１３に移行し、「ＮＯ」と判断すればステップＳ２０に移行する。尚、第２実施例に適用するには、ステップＳ２１，Ｓ２３，Ｓ２４で「ＮＯ」と判断した場合に、ステップＳ１の基準位置姿勢についても、選択をやり直すようにすれば良い。

本発明は上記し又は図面に記載した実施例のみ限定されるものではなく、以下のような変形又は拡張が可能である。
６軸垂直多関節型ロボットに限ることはない。
手先の姿勢を省いて、特許文献１と同様に位置のみについてキャリブレーションを行っても良い。その場合ｎは、ｎ＞（未知パラメータ数／３）の商よりも大となるように設定すれば良い。

本発明の第１実施例であり、ロボットについてキャリブレーションを行う場合のシステム構成をモデル的に示す図フローチャート本発明の第３実施例を示す図２相当図従来技術を説明する図１相当図

符号の説明

図面中、１はロボット、４は位置計測器（計測手段）、６は制御計算機（制御装置），８はロボットコントローラ（制御装置）を示す。

Claims

多関節型ロボットアームの手先を複数（ｎ）の位置に移動させ、
前記複数の位置について、外部の計測手段により計測された前記手先の位置と、制御上で指令した位置との誤差Δφを、それぞれΔφ１，Δφ２，…，Δφｎ（ｎは、推定したい未知のパラメータ数を３で除した商より大の自然数とする）として求め、
前記ｎ個の誤差Δφの内、任意の１つΔφｍを基準として、その他の誤差Δφｘ（１≦ｘ≦ｎ，但しｍを除く）との差Δεｙ（１≦ｙ≦ｎ−１）を求め、
前記各Δεｙの絶対値の和が所定値ε０（ε０＞０）以下となるまで、各Δεｙを算出する元になるパラメータを収束計算するロボットのキャリブレーション方法であって、
前記パラメータの行列をｑとし、前記ロボットアームの各軸角度の行列をθとすると、パラメータ行列ｑ，θについてのヤコビ行列Ｊを求め、
前記誤差Δφｍ，Δφｘとの差Δεｙと、前記ヤコビ行列Ｊの擬似逆行列との積から前記パラメータ行列ｑの誤差行列Δｑを求め、
前記パラメータ行列ｑに前記誤差行列Δｑを加算してパラメータ行列ｑを更新し、
更新したパラメータ行列ｑに基づいて前記ロボットアームの手先位置を再計算することで、前記パラメータ行列ｑを収束させることを特徴とするロボットのキャリブレーション方法。
基準とする誤差Δφｍを、複数の位置について設定し、
前記複数の基準誤差Δφｍについて、前記差Δεｙ，前記ヤコビ行列Ｊ，前記誤差行列Δｑを求め、
異なる基準誤差Δφｍについて求めた前記差Δεｙの絶対値の和が、所定値ε１（ε１＞０）以下となるまで、各Δεｙを算出する元になるパラメータを収束計算することを特徴とする請求項１記載のロボットのキャリブレーション方法。
前記ロボットアームの手先について、前記複数の位置と共に姿勢についても外部の計測手段により計測し、且つ、制御上で指令した姿勢との誤差も求めて前記Δφに含めるようにし、
前記ｎを、推定したい未知のパラメータ数を６で除した商よりも大とすることを特徴とする請求項１又は２記載のロボットのキャリブレーション方法。
前記計測手段により計測された、当該計測手段の位置から前記ロボットアームの手先であるツール先端位置までのベクトルを、前記ｘ，ｍに対応して表す同次変換行列をＴ_{ｍｅａ,ｘ}，Ｔ_{ｍｅａ,ｍ}，
前記計測手段により計測された、前記ロボットアームのフランジから前記ツール先端位置までのベクトルを表す同次変換行列を^ｆｌＴ_ｔｏｏｌ，
計算上の前記計測手段の位置から前記ロボットアームのベースまでのノーマル，オリエント，アプローチの各ベクトルを表す行列を^ｍｂＲ_ｒｂ，
前記行列^ｍｂＲ_ｒｂを、前記ｘ，ｍに対応して表す行列を^ｍｂＲ_ｒｂ,ｘ，^ｍｂＲ_ｒｂ,ｍ，
計算上の前記計測手段の位置から前記ロボットアームのベースまでの位置ベクトルを、前記ｘ，ｍに対応して表す行列を^ｍｂＰ_ｒｂ,ｘ，^ｍｂＰ_ｒｂ,ｍとすると、
前記誤差Δφｍ，Δφｘとの差Δεｙを、次式

で計算することを特徴とする請求項１乃至３の何れかに記載のロボットのキャリブレーション方法。
多関節型ロボットアームの手先を複数（ｎ）の位置に移動させるアーム制御手段と、
前記複数の位置について、外部の計測手段により計測された前記手先の位置と、制御上で指令した位置との誤差Δφを、それぞれΔφ１，Δφ２，…，Δφｎ（ｎは、推定したい未知のパラメータ数を３で除した商より大の自然数とする）として求める誤差計算手段と、
前記ｎ個の誤差Δφの内、任意の１つΔφｍを基準として、その他の誤差Δφｘ（１≦ｘ≦ｎ，但しｍを除く）との差Δεｙ（１≦ｙ≦ｎ−１）を求める誤差間差計算手段と、
前記各Δεｙの絶対値の和が所定値ε０（ε０＞０）以下となるまで、各Δεｙを算出する元になるパラメータを収束計算することでキャリブレーションを行うキャリブレーション手段とを備えたロボットの制御装置であって、
前記キャリブレーション手段は、
前記パラメータの行列をｑとし、前記ロボットアームの各軸角度の行列をθとすると、パラメータ行列ｑ，θについてのヤコビ行列Ｊを求めるヤコビ行列計算手段と、
前記誤差Δφｍ，Δφｘとの差Δεｙと、前記ヤコビ行列Ｊの擬似逆行列との積から前記パラメータ行列ｑの誤差行列Δｑを求める誤差行列計算手段と、
前記パラメータ行列ｑに前記誤差行列Δｑを加算してパラメータ行列ｑを更新するパラメータ行列更新手段とを備え、
更新されたパラメータ行列ｑに基づいて前記ロボットアームの手先位置を再計算することで、前記パラメータ行列ｑを収束させることを特徴とするロボットの制御装置。
前記誤差行列計算手段は、基準とする誤差Δφｍを、複数の位置について設定し、
前記複数の基準誤差Δφｍについて、前記差Δεｙ，前記ヤコビ行列Ｊ，前記誤差行列Δｑを求め、
異なる基準誤差Δφｍについて求めた前記差Δεｙの絶対値の和が、所定値ε１（ε１＞０）以下となるまで、各Δεｙを算出する元になるパラメータを収束計算することを特徴とする請求項５記載のロボットの制御装置。
前記誤差計算手段は、前記ロボットアームの手先について、前記複数の位置と共に姿勢についても外部の計測手段により計測し、且つ、制御上で指令した姿勢との誤差も求めて前記Δφに含め、前記ｎを、推定したい未知のパラメータ数を６で除した商よりも大に設定することを特徴とする請求項５又は６記載のロボットの制御装置。
前記計測手段により計測された、当該計測手段の位置から前記ロボットアームの手先であるツール先端位置までのベクトルを、前記ｘ，ｍに対応して表す同次変換行列をＴ_{ｍｅａ,ｘ}，Ｔ_{ｍｅａ,ｍ}，
前記計測手段により計測された、前記ロボットアームのフランジから前記ツール先端位置までのベクトルを表す同次変換行列を^ｆｌＴ_ｔｏｏｌ，
計算上の前記計測手段の位置から前記ロボットアームのベースまでのノーマル，オリエント，アプローチの各ベクトルを表す行列を^ｍｂＲ_ｒｂ，
前記行列^ｍｂＲ_ｒｂを、前記ｘ，ｍに対応して表す行列を^ｍｂＲ_ｒｂ,ｘ，^ｍｂＲ_ｒｂ,ｍ，
計算上の前記計測手段の位置から前記ロボットアームのベースまでの位置ベクトルを、前記ｘ，ｍに対応して表す行列を^ｍｂＰ_ｒｂ,ｘ，^ｍｂＰ_ｒｂ,ｍとすると、
前記誤差間差計算手段は、前記誤差Δφｍ，Δφｘとの差Δεｙを、次式

で計算することを特徴とする請求項５乃至７の何れかに記載のロボットの制御装置。