JP2011125956A - ロボットのばね定数同定方法およびロボットのばね定数同定装置 - Google Patents

Abstract

【課題】多関節のロボットアームの各回転関節軸についてｚｘｙ軸まわりのばね定数を高精度に同定する。
【解決手段】ロボットアームを複数の位置に動作させて、動作位置毎に２つの重りを１つずつロボットアームの先端に付け、各動作位置において各回転関節軸の回転角を取得すると共に、重り毎の測定点の位置・姿勢を計測手段により計測する。また、各動作位置において質量の異なる重りが付けられたときの測定点の位置・姿勢について、各回転関節軸の回転角および各回転関節軸の各ばね定数の予測値を用いて計算によって求める。そして、位置・姿勢について、大なる質量の重りのときの計測値と計算値の誤差、および小なる質量の重りのときの計測値と計算値の誤差を求め、これら両誤差の差を各動作位置について合計した値が所定の閾値以下となるようにばね定数の予測値を修正してゆく。
【選択図】図４

Description

本発明は、ロボットアームの回転関節軸のばね定数を同定する方法およびその装置に関する。

例えば、垂直多関節型ロボットにおいて、そのロボットアームは複数の回転関節軸により複数のアームを連結して構成され、各アームは回転関節軸の回転によって水平方向或は垂直方向に旋回したり、捻り回転したりして所用の動作を行う。
ロボットアームの各回転関節軸はサーボモータにより歯車減速装置を介して回転駆動されるが、回転トルクを受けて捻れ方向の撓みを生じたり、アームやワークの質量を受けて曲げ方向の撓みを生じたりする。このため、各回転関節軸のばね定数を正確に把握して、各回転関節軸がどの程度捩れたり曲がったりするのかを精度良く演算できないと、ロボットアームの先端を教示された位置・姿勢に正しく制御することが困難となる。

回転関節軸のばね定数を求めるために採用されていた従来方法の一つに、ロボットアームの先端に、質量の異なる２個の重りを交互に取り付け、重り毎に、各回転関節軸に連結されたアームの垂直方向の撓み量をレーザ変位計などの外部計測装置により計測し、２つの重りの質量の差と、各アームの撓み量の差とから、各回転関節軸のばね定数を割り出す、というものがある。
特許文献１は、ロボットではないが、構造物をばね・質点系にモデル化し、ばね・質点系の一端を加振した場合の加振力と、任意の点での加振力に対する応答を測定し、この加振力と応答とからばね定数と質点の質量を同定する方法を開示している。

特開平５−２０９８０５

ロボットアームの回転関節軸は、両端部が軸受で支持された両端支持型の他、一端側のみが軸受で支持された片持ち型のものも多く存在する。回転関節軸の回転中心軸線と同一の軸線をｚ軸、このｚ軸に直交する２軸線であって互いに直交する２軸線をｘ軸およびｙ軸とした場合、両端支持型の回転関節軸であっても、片持ち型の回転関節軸であっても、ｚ軸を中心とする回転方向（捩り方向）の撓みを生ずることは勿論、ｘｙの各軸を中心とする回転方向（曲げ方向）にも撓みを生ずるので、ｚｘｙの各軸を中心とする回転方向の撓みについてのばね定数（以下、単にｚ軸、ｘ軸、ｙ軸まわりのばね定数という。）を考慮する必要がある。

しかしながら、上記従来の方法、つまり、ロボットアームの先端に２つの重りを取り付けたときの各アームの垂直方向の撓み量を計測し、２つの重りの質量差と各アームの上下方向の撓み量差とからばね定数を同定する方法では、計測した撓み量に対し、ｚ軸まわりのばね定数、ｘ軸まわりのばね定数およびｙ軸まわりのばね定数がそれぞれどの程度関与しているか分からない。このため、ｚｘｙの各軸まわりのばね定数の関与の程度については、作業者が経験と勘によって推定するしかなく、結局、ｚｘｙの各軸まわりのばね定数を真のばね定数に近い値として精度良く同定することが困難となる。
そして、各回転関節軸のｚｘｙの各軸まわりのばね定数を高精度に同定し得ないことは、サーボモータにより各回転関節軸を正しく回転させても、予測できない回転関節軸の撓みにより、予め教示された位置・姿勢に高精度に制御できなくなることを意味する。

本発明は上記の事情に鑑みてなされたもので、その目的は、多関節のロボットアームの各回転関節軸について、ｚ軸まわりのばね定数だけでなく、ｘ軸まわりのばね定数、ｙ軸まわりのばね定数をも高精度に同定することができるロボットのばね定数同定方法およびロボットのばね定数同定装置を提供することにある。

本発明では、ロボットアームを複数の位置に動作させて、各動作位置において質量の異なる少なくとも２つの重りを１つずつロボットアームの先端に付け、各動作位置において各回転関節軸の回転角を取得すると共に、重りが換わる毎に、ロボットアームの先端または先端近くに定められた測定点の位置および姿勢のうちの少なくとも位置を、計測手段により計測し、且つ、各回転関節軸の回転角および各回転関節軸の各ばね定数の予測値を用いて計算によって求め、更に、大なる質量の重りのときの計測による少なくとも位置と計算による少なくとも位置との誤差、および小なる質量の重りのときの計測による少なくとも位置と計算による少なくとも位置との誤差を求め、これら両誤差の差を各動作位置について合計する。この大なる質量の重りのときの計測による少なくとも位置と計算による少なくとも位置との誤差、および小なる質量の重りのときの計測による少なくとも位置と計算による少なくとも位置との誤差の差は、当初定めた各ばね定数の予測値と真のばね定数との差によって生じたものであるから、両者の差が所定の閾値以下となるようにばね定数の予測値を数学的手法により修正してゆくことにより、各回転関節軸の回転中心軸線（ｚ軸）まわりのばね定数ばかりでなく、回転中心と直交する２軸（ｘｙ軸）まわりのばね定数をも求めることができ、しかも、最終的に求められた各ばね定数の予測値（同定されたばね定数）はある程度の正確さをもって真のばね定数に近い値となる。その上、重りが大小換えられたときの少なくとも位置の差を利用してばね定数を求めるので、トルクが作用したときの回転関節軸の撓みを計算して測定点の位置を求める場合、アームの重量などによるトルクを考慮せずとも、重りの重量によるトルクだけを考慮すれば良く、計算が容易となる。

本発明の第１の実施形態を示すもので、回転関節軸のばね定数を同定するためのシステム構成をモデル的に示す図 垂直多関節型ロボットの斜視図 ロボットアームの先端近くの測定点を示す斜視図 フローチャート 捻り回転を行うアームの回転関節軸を示すもので、（ａ）は回転関節軸の支持構成とサーボモータのトルク伝達構成をモデル的に示す断面図、（ｂ）は回転関節軸の３方向の撓みの中心軸を示す斜視図 旋回を行うアームの回転関節軸を示すもので、（ａ）は回転関節軸の支持構成とサーボモータのトルク伝達構成をモデル的に示す断面図、（ｂ）は回転関節軸の３方向の撓みの中心軸を示す斜視図 回転関節軸の撓みを示すもので、（ａ）はｘ軸を中心とする回転方向の撓みを示す断面図、（ｂ）はｙ軸を中心とする回転方向の撓みを示す断面図、（ｃ）はｚ軸を中心とする回転方向の撓み（捩れ）を示す斜視図

図面は本発明の第１の実施形態を示す。図２には、６軸の垂直多関節型ロボット１が示されている。このロボット１のロボットアーム２は、ベース３と、このベース３に第１軸Ｌｃ−１を中心に水平方向に旋回可能に支持されたショルダ部４と、このショルダ部４に第２軸Ｌｃ−２を中心に上下方向に旋回可能に支持された下アーム５と、この下アーム５に第３軸Ｌｃ−３を中心に上下方向に旋回可能に支持された第１の上アーム６と、この第１の上アーム６の先端部に第４軸Ｌｃ−４を中心に捻り回転可能に支持された第２の上アーム７と、この第２の上アーム７に第５軸Ｌｃ−５を中心に上下方向に旋回可能に支持された手首８と、この手首８に第６軸Ｌｃ−６を中心に捻り回転可能に支持されたフランジ９とから構成されている。

ベース３、ショルダ部４、下アーム５、第１の上アーム６、第２の上アーム７、手首８、フランジ９は、ロボットアーム２におけるアームとして機能し、ベース３を除く各アーム４〜９は、図５および図６に示すように、前段のアームに軸受１０を介して回転可能に支持された回転関節軸１１に固定連結されている。なお、各回転関節軸１１は、上記第１軸Ｌｃ−１〜第６軸Ｌｃ−６に相当するものである。

また、各回転関節軸１１は、それぞれ駆動源であるサーボモータ１２により減速装置、例えば歯車減速装置１３を介して回転駆動され、各アーム４〜９は、回転関節軸１１の回転に伴って旋回動作或は捻り動作を行うようになっている。図５および図６では、回転関節軸を示すに同じ符号１１を用いているが、各アーム４〜９の回転関節軸１１はそれぞれ径や長さの異なるものが用いられ、また、サーボモータ１２も、駆動するアームに応じた容量のものが各回転関節軸１１に一対一の関係で設けられているものである。

ロボット１は、動作プログラムに従ってサーボモータ１２を制御することによってロボットアーム２を制御する制御部（以下、ロボット制御部という。）１４を備えている。このロボット制御部１４には、図１に示すように、回転関節軸１１の回転角度を検出する関節角度検出器（軸角度検出手段）１５と記憶部１６が接続されている。関節角度検出器１５は、サーボモータ１２の回転軸（図示せず）に連結された回転検出手段としてのロータリエンコーダ（図示せず）をセンサとし、このロータリエンコーダの回転角検出情報に基づいて回転関節軸１１の回転角０度の基準位置からの回転角度を検出する。つまり、関節角度検出器１５は、回転関節軸１１の実際の回転角を検出するものではなく、ロボット制御部１４の回転指令によってサーボモータ１２の回転軸が回転したとき、そのサーボモータ１２の回転軸の回転角を検出し、検出した角度を歯車減速装置１３の減速比で除することにより、回転関節軸１１の回転角度に換算してロボット制御部１４に出力するものである。
上記記憶部１６には、ロボット制御用の各種ソフトウエアなどが予め記憶されており、また、実際のロボット作業に際しては、ティーチングされた動作プログラムなどが記憶される。更に、この記憶部１６には、後述する各種のパラメータが記憶されている。

ベース３および各アーム４〜９には、３次元の座標が規定されている。このうち、床面に据え付けられるベース３の座標系は、不動の座標系としてロボット座標（基準座標）とされるものである。各アーム４〜９の座標系は、各アーム４〜９の基端部（回転関節軸１１の回転中心軸線上の所定位置）に規定されており、各アーム４〜９の回転によりロボット座標上での姿勢（向き）、或は位置と姿勢が変化する。図１には、ベース３および各アーム４〜９の座標のうち、ロボット座標がＺｒ，Ｘｒ，Ｙｒの座標軸で示され、フランジ９の座標（以下、フランジ座標という。）が、Ｚｆ，Ｘｆ，Ｙｆの座標軸で示されている。

記憶部１６には、ロボット座標上におけるショルダ部４の座標位置、ショルダ部４の座標上における下アーム５の座標位置、下アーム５の座標上における第１の上アーム６の座標位置、第１の上アーム６の座標上における第２の上アーム７の座標位置、第２の上アーム７の座標上における手首８の座標位置、手首８の座標上におけるフランジ９の座標位置、および各アーム６〜９の長さを表す数値がパラメータとして記憶されている。

そして、ロボット制御部１４は、各アーム４〜９の座標の位置と姿勢とを、各アーム６〜９の回転位置と各アーム４〜９の座標の相互位置関係から、座標変換の計算機能によりロボット座標上での位置と姿勢とに変換して認識することができるようになっている。なお、各アーム４〜９の姿勢は、各アーム４〜９の座標をロボット座標まで並行移動したとき、各アーム４〜９の座標軸上の単位ベクトルの先端がロボット座標上でどの位置にあるかによって表される。

さて、図１にはロボットアーム２の各回転関節軸１１のばね定数を同定するためのシステム構成がモデル化して示されている。同定対象となるばね定数は、次のようなものである。つまり、図５（ｂ）、図６（ｂ）および図７に示すように、撓みのない状態での各回転関節軸１１の回転中心軸線Ｌｃと同一の軸線をｚ軸、このｚ軸に直交する２軸線であって互いに直交する２軸線をｘ軸およびｙ軸とする３次元座標を想定し、ｚ軸まわりのばね定数をＫｚ、ｘ軸まわりのばね定数をＫｘおよびｙ軸まわりのばね定数をＫｙとしている。これら各回転関節軸１１の各ばね定数Ｋｚ，Ｋｘ，Ｋｙのうち、同定すべきばね定数としては、全てを対象とすることもでき、所望のものを選択することもできる。なお、ばね定数の３次元座標の各軸ｚｘｙは、この実施形態にあっては各アーム４〜９の固有座標の各座標軸と同一であるが、異なっていても良い。

ここで、ｚ軸まわりのばね定数Ｋｚとは、図７（ｃ）に示す回転関節軸１１単独の撓み（捩れ）だけでなく、サーボモータ１２の回転軸から歯車減速装置１３を経て回転関節軸１１までのアーム４〜９へのトルク伝達経路を含んだ撓み（捩れ）についてのものとされる。また、ｘ軸およびｙ軸を中心とする回転方向の撓みは、回転関節軸１１自身が曲げの他、図７（ａ）および（ｂ）に示すように、軸受１０も変形するので、ｘｙの各軸まわりのばね定数には、軸受１０のばね定数も含む。なお、回転関節軸１１は軸受１０と共にｚ軸を中心に対象形であるので、通常は、ｘｙの各軸まわりのばね定数は同値となる。

図１において、床面に据え付けられたロボットアーム２はスケルトンにて示されている。また、床面上には、ロボットアーム２に相対するようにして、３次元の座標測定装置（計測手段）１７が設置されている。そして、ロボット制御部１４および座標測定装置１７の制御部（以下、測定制御部という。）１８の双方がばね定数同定装置１９に接続されている。このばね定数同定装置１９は、パソコンからなり、ＣＰＵを主体とする制御部、ばね定数同定のためのソフトウエアなどを記憶した記憶部、キーボードやマウスなどの入力部、液晶ディスプレイなどの出力部を備えている。

上記座標測定装置１７は、ロボットアーム２の先端または先端近くに定められた測定点の位置および姿勢を測定するためのものである。この実施形態では、フランジ９の手首８側を後、その反対側を前としたとき、図３に示すように、ロボットアーム２の先端を構成するフランジ９の外周にホルダ２０が固定され、このホルダ２０の前面（フランジ９の前面と平行）の所定箇所に測定点ｔが定められている。従って、測定点ｔは、この実施形態においては、ロボットアーム２の先端であるフランジ９の近くに定められていることとなる。なお、測定点ｔは、フランジ９の前面に定めても良く、要はフランジ９と一体に移動する箇所であれば良い。

上記測定点ｔには、当該測定点ｔを原点とする測定点座標が規定されている。この測定点座標は、フランジ９の座標軸Ｚｆ、ＸｆおよびＹｆにそれぞれ平行の座標軸Ｚｔ、ＸｔおよびＹｔを有する３次元座標によって構成されている。そして、Ｚｔ，Ｘｔ，Ｙｔの各座標軸上の単位ベクトルがそれぞれオリエントベクトルＯ、アプローチベクトルＡおよびノーマルベクトルＮとして定義されている。

ホルダ２０の先端面には、指標としての３個の発光ダイオード２１〜２３が接着などによって固定されている。この３個の発光ダイオード２１〜２３は発光色が互いに異なり、その固定位置は、例えば測定点ｔを重心とする正三角形を描いたとき、その正三角形の頂点となる位置に定められている。なお、発光ダイオード２１〜２３の固定位置は、上記位置に限られるものではない。

前記座標測定装置１７は、例えば３台のＣＣＤカメラを備えた３次元計測器（例えば、株式会社東洋テクニカ；ロボットキャリブレーションＲＯＣＡＬシステム）からなるもので、３台のカメラによって３個の発光ダイオード２１〜２３を撮影することにより、測定制御部１８が撮影画像を解析して測定点ｔの位置と姿勢を計測する。この座標測定装置１７による測定点ｔの位置は、図１において座標軸がＸｍ，Ｙｍ，Ｚｍで示された座標測定装置１７の３次元座標（以下、計測座標という。）上の位置で示され、姿勢は、計測座標における測定点座標のオリエントベクトルＯ、アプローチベクトルＡおよびノーマルベクトルＮの先端位置にて示される。なお、座標測定装置１７はレーザトラッカなどであっても良い。上記３個の発光ダイオード２１〜２３の発光色を異ならせる理由はそれぞれを座標測定装置１７に個別に認識させるためであるが、そのためには、３個の発光ダイオード２１〜２３の色を同一にして発光のタイミングを互いに異ならせるようにしても良い。

さて、本発明のばね定数の同定方法の基本的な考え方は次のようなものである。つまり、各回転関節軸１１の各ばね定数Ｋｘ，Ｋｙ，Ｋｚを同定するために、本実施形態では、質量の異なる２つの重り２４，２５がフランジ９の先端面の中央に交換可能に接着などによって固定される。なお、重り２４，２５はねじ止めによって固定するようにしても良い。この２つの重り２４，２５の質量は、重り２４の方が重り２５よりも軽量となっている。そして、この２つの重り２４，２５は、予め重心位置が求められており、フランジ９の先端面の中央に固定されたとき、それぞれの重心位置がフランジ座標上の位置として特定できるように構成されている。

ばね定数を同定するとき、ロボットアーム２の先端は複数の異なる位置に動作される。この複数の動作位置に移動する際、全ての回転関節軸１１は回転されて、各回転関節軸１１は複数の動作位置で互いに同じ回転角をとらないようになされる。そして、設定された動作位置でロボットアーム２が停止すると、そのときの各回転関節軸１１の回転角が検出されてばね定数同定装置１９に送られる。また、ロボットアーム２の各動作位置で、２つの重り２４，２５が交互にフランジ９に取り付けられ、そして、重り２４，２５が付けられる都度、座標測定装置１７によって測定点ｔの位置と姿勢が計測されてばね定数同定装置１９に送られる。

ばね定数同定装置１９は、ロボットアーム２の各動作位置において、２つの重り２４，２５が取り付けられたときの座標測定装置１７からの測定点ｔの位置と姿勢についての計測情報から、測定点ｔの同時変換行列Ｔ_mea（請求項１，２でいうＴｍ）を求める。また、ばね定数同定装置１９は、ロボット制御部１４から与えられた各回転関節軸１１の回転角と、重り２４，２５の質量と、各アーム４〜９の長さと、記憶部１６にパラメータとして記憶された各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_x0，Ｋ_y0，Ｋ_z0を用いて、ロボットアーム２の各動作位置において、２つの重り２４，２５が取り付けられたときの測定点ｔの位置および姿勢を示す同次変換行列Ｔ_rob（請求項１，２でいうＴｃ）を計算する。なお、予測値Ｋ_x0，Ｋ_y0，Ｋ_z0は、例えばロボット１の記憶部（記憶手段）１６に予め記憶されている。

測定点ｔの位置および姿勢を示す同次変換行列を計算するとき、各回転関節軸１１の撓みを考慮しなければならない。各回転関節軸１１には、重り２４，２５によって生ずるトルクの外、各アーム４〜９の質量によって生ずるトルクが作用し、この両トルクを受けて各回転関節軸１１は撓む。しかし、本実施形態では、測定点ｔの位置・姿勢を示す同次変換行列を計算するとき、重り２４，２５の質量によって生ずるトルクのみを考慮し、各アーム４〜９の質量により生ずるトルクを考慮しない。

その代わりに、本実施形態では、重り２４，２５毎に計測により求めた同次変換行列Ｔ_meaと計算によって求めた同次変換行列Ｔ_robとの差に着目し、質量の大きい重り２５についての計測による同次変換行列Ｔ_meaと計算による同次変換行列Ｔ_robとの差を示す誤差行列ΔＴ_highload、および質量の小さい重り２４についての計測による同次変換行列Ｔ_meaと計算による同次変換行列Ｔ_robとの差を示す誤差行列ΔＴ_lowloadとを求める。

この計測による同次変換行列Ｔ_meaと計算による同次変換行列Ｔ_robにおいて、各重り２４，２５によって生ずるトルクは同じであるから、Ｔ_meaとＴ_robの差を求めることにより、アーム４〜９の質量によって生ずるトルクが作用したとき、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_x0，Ｋ_y0，Ｋ_z0と真の値との差によって生じた各回転関節軸１１の撓みの差が求まる。しかし、アーム４〜９の質量により各回転関節軸１１に作用するトルクは不明で、そのトルクが作用したとき、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_x0，Ｋ_y0，Ｋ_z0と真の値との差によって各回転関節軸１１の撓みの差がどの程度生ずるものなのかは分からない。

そこで、本実施形態では、更に、各動作位置において、質量の大なる重り２５についての誤差行列ΔＴ_highloadと質量の小なる重り２４についての誤差行列ΔＴ_lowloadとの差を算出する。この両誤差行列の差（ΔＴ_highioad−ΔＴ_lowload）を取ることにより、各アーム４〜９の質量によって生ずるトルクが各回転関節軸１１に作用したとき、ばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_x0，Ｋ_y0，Ｋ_z0と真の値との差によって各回転関節軸１１に生ずる撓みの差が相殺される。

従って、（ΔＴ_highioad−ΔＴ_lowload）は、質量の大なる重り２５と質量の小なる重り２４とで各回転関節軸１１に作用するトルクの差分のトルクが各回転関節軸１１に作用したとき、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚが予測値Ｋ_x0，Ｋ_y0，Ｋ_z0のときの各回転関節軸１１の撓みと、真の値のときの各回転関節軸１１の撓みの差となる。このように（ΔＴ_highioad−ΔＴ_lowload）は、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_x0，Ｋ_y0，Ｋ_z0と、真の値との差によって生じたものであるから、（ΔＴ_highioad−ΔＴ_lowload）を基にして真の値を同定することが可能である。本発明は以上のような考え方を基にしてばね定数を同定する。

以下、ばね定数の同定方法について、図４のフローチャートをも参照して説明する。まず、ばね定数を同定する基となる情報を取得するためのロボットアーム２の動作位置（姿勢を含む）を複数決定する（図２のステップＳ１）。この場合、ロボットアーム２の動作領域の全体を網羅するように各位置を定めると共に、前述のように各位置を取らせる場合に全ての回転関節軸１１が回転するようにして、各回転関節軸１１が複数の動作位置間で同じ回転角とならないようにする。

次に決定した複数位置にロボットアーム２を順次動作させる。そして、各動作位置において、２つの重り２４，２５をフランジ９の前端面に交互に取り付け、各重り２４，２５が取り付けられる都度、座標測定装置１７により測定点ｔの位置と姿勢を計測し、計測結果である位置・姿勢計測情報をばね定数同定装置１９に送る。また、ロボットアーム２を各動作位置に動作させる都度、各回転関節軸１１を駆動するサーボモータ１２の回転数から各回転関節軸１１の回転角を検知し、検知した回転角情報をロボット制御部１４からばね定数同定装置１９に送る。ばね定数同定装置１９に送られた各回転関節軸１１の回転角情報と測定点ｔの位置・姿勢計測情報は、当該ばね定数同定装置１９のＲＡＭなどの記憶部（図示せず）に記憶される（以上、ステップＳ２〜５の繰り返し：情報取得手段）。

ロボットアーム２を決定された複数の位置の全てに動作させ終えると（ステップＳ５で「ＹＥＳ」）、ばね定数同定装置１９は、位置・姿勢計測情報から、動作位置毎に各重り２４，２５を取り付けたときの計測座標（所定の固有座標）についての測定点ｔの位置・姿勢を表すベクトルの同次変換行列Ｔ_mea（請求項１，２でいうＴｍ）を算出する。また、動作位置毎に各重り２４，２５を取り付けたときの計測座標についての測定点ｔの位置・姿勢を計算し（位置計算手段）、この位置・姿勢示すベクトルの同次変換行列Ｔ_rob（請求項１，２でいうＴｃ）を求める（ステップＳ６：同次変換行列計算手段）。

同次変換行列Ｔ_robを計算によって求める方法は次の通りである。まず、図１において、^meabaseＴ_robbaseは計測座標についてのロボット座標の位置・姿勢を示す同次変換行列、^robbaseＴ_flangeはロボット座標についてのフランジ座標の位置・姿勢を示す同次変換行列、^flangeＴ_toolはフランジ座標についての測定点ｔの位置・姿勢を示す同次変換行列である。

従って、計測座標についての測定点ｔの位置・座標を示す同次変換行列Ｔ_robは、
^meabaseＴ_robbaseと^robbaseＴ_flangeと^flangeＴ_toolの積によって求めることができる。このうち、フランジ９の座標についての測定点の位置・姿勢^flangeＴ_toolは既知、計測座標とロボット座標との関係は測定可能であるから
^meabaseＴ_robbaseも既知である。そして、^robbaseＴ_flangeは、各回転関節軸１１の回転角情報や各回転関節軸１１に作用するトルクを用いて計算することができる。

さて、ロボット座標についてのフランジ９の位置姿勢を示す同次変換行列
^robbaseＴ_flangeは、次の（１）式に示されているように、回転関節軸１１（第１軸〜第６軸）間の同次変換行列の積で表現される。

なお、（１）式において、０はベース（ロボット座標）、１〜６は第１軸〜第６軸を示す。

（１）式における軸間の同次変換行列は、次の（２）式に示すように、パラメータθ，ａ，ｂ，ｄ，α，βで表される行列の積で表現される。なお、ｉは第１軸〜第６軸（Ｌｃ−１〜Ｌｃ−６）を示す１〜６までの整数であり、パラメータθ，ａ，ｂ，ｄ，α，βの各軸Ｌｃ−１〜Ｌｃ−６（各回転関節軸１１）の値は［数２］（ｂ）のパラメータ表に示されている。

上記パラメータの定義は、ａｉはｉ軸（ｉアーム）の座標とｉ＋１軸（ｉ＋１アーム）の座標のＸ軸方向のオフセット、ｂｉはＹ軸方向のオフセット、ｄｉはＺ座標軸方向のオ
フセットである。また、θｉはＺ座標軸まわりの回転角、αｉはＸ座標軸まわりの回転角、βｉはＹ座標軸まわりの回転角である。

ところで、ＺＸＹの各座標軸まわりの実際の回転角は、サーボモータ１２による回転各に、回転関節軸１１の撓みによる回転角を加えたものとなる。各回転関節軸１１のＺＸＹの各座標軸まわりの撓みによる回転角（撓み角；ｒａｄ）ｓ_zi，ｓ_xi，ｓ_yiは、各回転関節軸１１のＺ座標軸まわり、Ｘ座標軸まわり，Ｙ座標軸まわりに作用するトルク（Ｎｍ）をｔ_zi，ｔ_xi，ｔ_yi、記憶部１６にパラメータとして記憶された各回転関節軸１１のｚｘｙの各座標軸まわりのばね定数（Ｎｍ／ｒａｄ）の予測値をＫ_z0i，Ｋ_x0i，Ｋ_y0iとすると、
ｓ_zi＝ｔ_zi／Ｋ_z0i … （７）
ｓ_xi＝ｔ_xi／Ｋ_x0i … （８）
ｓ_yi＝ｔ_yi／Ｋ_y0i … （９）
で表される。

ここで、ばね定数の予測値の逆数をｋ_z0i，ｋ_x0i，ｋ_y0iとおくと、各回転関節軸１１のＺＸＹの各座標軸まわりの回転角は、ばね定数の予測値の逆数ｋ_z0i，ｋ_x0i，ｋ_y0iを用いて次のように表される。
ｓ_zi＝ｔ_zi・ｋ_z0i … （１０）
ｓ_xi＝ｔ_xi・ｋ_x0i … （１１）
ｓ_yi＝ｔ_yi・ｋ_y0i … （１２）
従って、実際のｚｘｙの各座標軸まわりの回転角は、
θｉ＝θｉ＋ｔ_zi・ｋ_z0i … （１３）
αｉ＝αｉ＋ｔ_xi・ｋ_x0i … （１４）
βｉ＝βｉ＋ｔ_yi・ｋ_y0i … （１５）
と表すことができるので、この撓み角を考慮した回転角を前記（３），（５），（６）式に代入し、前記（２）式によって軸間の同次変換行列を求めることができる。そして、求めた各軸間の同次変換行列の積によって（１）式のロボット座標についてのフランジ９の位置・姿勢を示す同次変換行列^robbaseＴ_flangeを求めることができる。

上記トルクｔ_zi，ｔ_xi，ｔ_yiは、各アーム４〜９の質量によるものと、重り２４，２５の質量によるものとがある。各アーム４〜９の質量とその重心とを計測することは難しいので、各アーム４〜９の質量によるトルクは未知の値である。しかし、重り２４，２５によるトルクは、各回転関節軸１１から各重り２４，２５までの距離、関節角度検出器１５によって検出された各回転関節軸１１の回転角により計算することができる。なお、各回転関節軸１１から各重り２４，２５までの距離は、前記［数２］（ｄ）のパラメータを用いて計算できる各回転関節軸１１か
らフランジ座標までの位置ベクトルと、フランジ座標から重り２４，２５の重心までの位置ベクトルとによって計算することができる。

従って、本実施形態では、前記（１０）式〜（１２）式によって求められる各回転関節軸１１の撓みｓ_zi，ｓ_xiおよびｓ_yiは、重り２４，２５によるトルクのみを考慮して求める。そして、このｓ_zi，ｓ_xiおよびｓ_yiから撓み角を考慮した各回転関節軸１１の回転角θｉ，αｉおよびβｉを前記（１３）式〜（１５）式により求め、θｉ，αｉおよびβｉを前記（３），（５），（６）式に代入し、前記（２）式によって軸間の同時変換行列を求めるものである。そして、求めた各軸間の同次変換行列の積によって前記（１）式のロボット座標についてのフランジ９の位置・姿勢を求める。
計算により同次変換行列^robbaseＴ_flangeを求めた後、前述したように次の（１６）式によって計測座標についての測定点ｔの位置・姿勢を示す同次変換行列Ｔ_robを求めるものである。
Ｔ_rob＝^meabaseＴ_robbase・^robbaseＴ_flange・^flangeＴ_tool…（１６）

以上のようにして計算により求められたＴ_robは、次の（１７）式〜（１９）式に示すように、全てのばね定数の予測値の逆数ｋ_allと、重り２４，２５によるトルクｔ_allの関数であるといえる。

なお、（１８）式および（１９）式において、右辺の括弧の右肩に小さくｔで示す符号は、この行列が横書きされているが、実際には転置行列であることを示している。

同一の動作位置において、異なる重り２４，２５を付けたとき各回転関節軸１１の撓み角の差Δｓ_zi，Δｓ_xiおよびΔｓ_yiは、次の（２０）式〜（２２）式のようになる。

上記（２０）式〜（２２）式において、質量の大なる重り２５のときのトルクｔ_highioadと質量の小なる重り２４のときのトルクｔ_lowloadは、前述のように既知である。従って、前記（１７）式で示される質量の大なる重り２５のときの計算による同次変換行列Ｔ_highioadと、質量の小なる重り２４のときの計算による同次変換行列次Ｔ_lowloadとの差は、次の（２３）式のように、全てのばね定数の逆数ｋ_allの関数であるといえる。

ここで、動作位置における測定点ｔの測定による位置・姿勢と計算による位置・姿勢の誤差ΔＴは、次の（２４）式のように、ばね定数の逆数の微小変動Δｋの線形結合和で近似される。

上記（２４）式を質量の大なる重り２５における誤差ΔＴ_highloadと、質量の小なる重り２４における誤差ΔＴ_lowloadの差に着目した式にすると、次の（２５）式のようになる。

１つの動作位置において、重り２４，２５毎に、計測によって求められた測定点ｔの同時変換行列Ｔ_meaと計算によって求めた測定点ｔの同次変換行列Ｔ_robとの差を示す誤差行列ΔＴ_highioadおよびΔＴ_lowloadは、次の（２６）式によって求めることができる。なお、（２６）式では、ΔＴ_highioadおよびΔＴ_lowloadを、ΔＴ_matrixで示している。

以上のように、動作位置における測定点ｔの測定による位置・姿勢と計算値による位置・姿勢の誤差ΔＴは、前記（２４）式のように、ばね定数の逆数の微小変動Δｋの線形結合和で近似されるから、ΔＴ_highioadとΔＴ_lowloadとの差が小さければ、ばね定数の当初の予測値は真の値に近いといえる。

そこで、ばね定数同定装置１９は、ΔＴ_highioadとΔＴ_lowloadとの差を全動作位置に拡張してその総和（誤差和）ΔＴ_allを次の（２９）式により求める（ステップＳ７：誤差和行列計算手段）。

続いて、ばね定数同定装置１９は、この誤差和ΔＴ_allが所定の閾値ε以下であるか否かを判断し（ステップＳ８：判断手段）、閾値ε以下のときには、各回転関節軸１１のばね定数の予測値Ｋ_z0i，Ｋ_x0i，Ｋ_y0iをばね定数として同定し（同定手段）、この同定したばね定数が予め予測された適切な値以内かどうかを判断する（ステップＳ９）。そして、適切な値以内であれば、同定したばね定数を記憶部１６に保存し（ステップＳ９で「ＹＥＳ」、ステップＳ１０）、ばね定数の同定を終了する。なお、同定したばね定数が適切な値以内でなければ（ステップＳ９で「ＮＯ」）、前述のステップＳ１から同定作業をやり直すこととなる。

一方、誤差和が閾値εを超えている場合（ステップＳ８で「ＮＯ」）、ばね定数同定装置１９は、推定回数カウンタｘをインクリメントし（ステップＳ１１）、次にｘが予め定められた所定の上限回数Ｘmax未満であるかどうかを判断し（ステップＳ１２）、未満であれば（ステップＳ１２で「ＹＥＳ」）、作業者に同定したいばね定数を選択するように促す。これに応じて、作業者は、ばね定数同定装置１９の入力部（選択手段）を操作して全てのばね定数Ｋ_allから同定したいｍ個のばね定数Ｋ₁〜Ｋ_mを選択する（ステップＳ１３）。例えば、両端支持型の回転関節軸で、径寸法の大なる回転関節軸は、ｘｙの両軸まわりの撓みは０と扱っても良い場合があるので、その場合のばね定数は同定したいばね定数から除外することが可能である。勿論、全てのばね定数を同定するばね定数として選択しても良い。

ここで、Ｔ_meaとＴ_robとの前記誤差ΔＴを、測定点座標のノーマルベクトル、オリエントベクトルおよびアプローチベクトルの差Δｎ，ΔｏおよびΔａと、計測座標からの位置ベクトルの差Δｐで示すと、次の（３０）式のようになる。また、前記ヤコビ行列Ｊは、オリエントベクトル、アプローチベクトルおよび位置ベクトルを用いて表すと、次の（３１）式で示されるように、計算により求めた測定点のノーマルベクトル、オリエントベクトル、アプローチベクトルのうちの例えばオリエントベクトル、アプローチベクトルの計測座標の各座標軸Ｚｍ，Ｘｍ，Ｙｍ上での位置ｏ_x，ｏ_y，ｏ_z，ａ_x，ａ_y，ａ_z，および測定点の位置ベクトルの計測座標の各座標軸Ｚｍ，Ｘｍ，Ｙｍ上での位置ｐ_x，ｐ_y，ｐ_zを同定したいばね定数の逆数ｋ₁〜ｋ_mで除した値の行列で表される。

ノーマルベクトル、オリエントベクトル、アプローチベクトルには、次の（３２）式の関係がある。

つまり、上記の３つのベクトルのうち２つのベクトルが既知であれば、残る１つのベクトルは算出できる。このことから、上記（３０）式、（３１）式は、次の（３３）式と（３４）式、（３５）式と（３６）式の組み合わせとすることもできる。

なお、（２６）式のΔＴの添え字の_matrixは、ΔＴがノーマルベクトル、オリエントベクトル、アプローチベクトルの全てを使用して表されたことを意味し、（２５），（２８），（３０）の各式のΔＴに_matrixの添え字がないことは、ノーマルベクトル、オリエントベクトル、アプローチベクトルのいずれか２つのベクトルを使用して表したことを意味している。

さて、前記（２５）式、つまり各動作位置におけるΔＴ_highioadとΔＴ_lowloadとの誤差（ΔＴ_highioad−ΔＴ_lowload）を全ての動作位置に拡張すると、次の（３７）式のようになり、これを簡易に表すと（３８）式となる。なお、ｈは動作位置の数を示す。

上記（３８）式を次の（４１）式、（４２）式のように変形してゆくと、（４３）式のように、同定したいばね定数の逆数の微小変動行列Δｋはヤコビ和行列Ａの疑似逆行列で表される。

そこで、ばね定数同定装置１９は、（４３）式を計算してΔｋを求める（ステップＳ１４：ヤコビ和行列計算手段、微小変動行列計算手段）。このとき、ヤコビ和行列Ａ^TＡの行列式Ｓが０となる（但し、Ｓ＝｜Ａ^TＡ｜）場合には、ヤコビ和行列Ａの擬似逆行列が得られなくなるので、この場合（ステップＳ１５で「ＮＯ」）には、推定回数カウンタｘを０にし（ステップＳ１８）、そして、ステップＳ１に戻って再度動作位置を決定することから同定作業をやり直す。

（４３）式により求めたΔｋは、次の（４４）式のように、同定したいｍ個のばね定数Ｋ１〜Ｋｍの真の値の逆数と予測値の逆数ｋ₁〜ｋ_mの差（微小変動）Δｋ₁，〜，Δｋ_mを表す行列であるから、ばね定数の予測値の逆数ｋ_vとΔｋとの和ｋ_v+1は、ばね定数の真の値に近付く。

そこで、ばね定数同定装置１９は、このΔｋを算出（ステップＳ１６）した後、次の（４５）式のｋ_vに、同定したいばね定数Ｋ１〜Ｋｍの予測値の逆数ｋ₁〜ｋ_mの行列を代入し、同定したいばね定数の予測値の逆数の行列を更新する（ステップＳ１７:予測値逆数行列更新手段）。

その後、ばね定数同定装置１９は、前記（１０）〜（１２）式のばね定数の予測値の逆数として、同定したいばね定数Ｋ１〜Ｋｍについては更新した予測値の逆数、つまり（４５）式におけるｋ_v+1を用い、同定を希望しないばね定数については当初の予測値の逆数ｋ_z0i，ｋ_x0i，ｋ_y0iをそのまま用い、そして、前記（１３）式〜（１５）式によりθｉ，αｉ，βｉを求め、このθｉ，αｉ，βｉを前記（３）式、（５）式および（６）式に代入して前記（２）式により軸間の同次変換行列を求め、これら軸間の同次変換行列をもとに前記行列（１）式により各動作位置における各重り２４，２５についての^robbaseＴ_flangeを再度求める（位置再計算手段）。

次いで、ばね定数同定装置１９は、前記（１６）式からＴ_robを再度求め（ステップＳ６：同次変換行列再計算手段）、且つ、先に求めたＴ_meaとの誤差ΔＴを再度求め、続いて、（２９）式により誤差ΔＴの総和ΔＴ_allを再度求め（ステップＳ７：誤差和行列再計算手段）、その誤差和ΔＴ_allが所定の閾値ε以下であるかを判断する（ステップＳ８：再判断手段）。

誤差和ΔＴ_allが所定の閾値ε以下であれば、ばね定数同定装置１９は、更新した同定したいばね定数の予測値の逆数ｋ_v+1から求めたばね定数、同定を希望しないばね定数については当初の予測値を各ばね定数として同定し、同定した値が適切であれば（ステップＳ９で「ＹＥＳ」）、これを記憶部１６に記憶する（ステップＳ１０）。

閾値ε以下でなければ、ばね定数同定装置１９は、推定回数カウンタｘをインクリメントし（ステップＳ１１）、再度、（４３）式により、ばね定数の逆数の微小変動行列Δｋを求め（微小変動行列再計算手段）、このΔｋを（４５）式に代入し、当該（４５）式のｋ_vに先に更新したばね定数の逆数ｋ_v+1を代入して（４５）式により再度同定したいばね定数の予測値の逆数ｋ_v+1を更新する（予測値逆数行列再更新手段）。なお、推定回数カウンタｘが２以上のときは、同定したいばね定数の選択は必ずしも行わなくとも良い。勿論、ｘ＝１のときと同じばね定数をｘが２以上のときにも選択操作するものであっても良く、更には、ばね定数の推定に影響のない範囲で同定したいばね定数を追加すべく、ｘが２以上のときに選択操作するようにしても良い。

そして、再度更新した同定したいばね定数の予測値の逆数ｋ_v+1を用いて行列（２）により各位置・姿勢についての同次変換行列Ｔ_robを求め、且つ、先に求めたＴ_meaとの差ΔＴを求め、次いで、その総和（誤差和）ΔＴ_all（ΔＲ）が所定の閾値ε以下であるか判断し、閾値ε以下でなければ、再度、（４３）式によりΔｋを求め、（４５）式のｋ_vに、先に更新した同定したいばね定数の予測値の逆数ｋ_v+1を代入して更に同定したいばね定数の予測値の逆数ｋ_vを更新するという動作を、誤差ΔＴの総和ΔＲが所定の閾値ε以下となるまで繰り返し実行する（繰り返し実行手段）。

そして、誤差和ΔＴ_allが所定の閾値以下となったところで、更新したｋ_v+1を同定したいばね定数の逆数とし、このｋ_v+1からばね定数を求めてこれを記憶部１６に記憶する。
実際にロボットアーム２を動作させる場合、以上のようにして同定したばね定数を用いて回転関節軸１１のｚｘｙの各軸方向の撓みを求め、この撓みを用いて各サーボモータ１２の回転数を補正することにより、ロボットアーム２の先端を教示された位置・姿勢に精度良く動作させることができるようになる。

このように本実施形態によれば、各回転関節軸１１のばね定数を、ｚｘｙの各軸まわりのばね定数に分けて同定することができる。しかも、各回転関節軸１１に作用するｚｘｙの各軸を中心とするトルクを解析してｚｘｙの各軸まわりのばね定数を同定するので、同定したばね定数は真のばね定数に精度良く一致させることができる。

また、従来のばね定数同定方法の場合には、各回転関節軸１１の撓みをベース３に近いものから順に計測する必要があり、間違えて回転関節軸を飛ばしてしまうと、正しく計測できないという不具合があり、計測作業自体が難しいという問題があったが、本実施形態の同定方法によれば、計測としては、ロボットアーム２を動作させる都度、２つの重り２４，２５についての測定点ｔの位置・姿勢を座標測定装置１７によって計測するだけで済むので、計測作業は簡単なものとなり、間違える恐れがない。

その上、重り２４，２５の質量の差から生ずるトルク差を利用してばね定数を求めるので、各回転関節軸１１の撓みを求める場合、各アーム４〜９の質量、重心位置、イナーシャなどが既知でないと演算できない各アーム４〜９の質量により発生するトルクについては考慮する必要がなく、計算が容易となる。

［第２の実施形態］
上述の第１の実施形態では、各動作位置について重り２４，２５毎の測定点ｔの位置と姿勢の双方を用いてばね定数を同定した。本実施形態では、姿勢を計測できない場合などに対応できるように、位置だけでばね定数を同定することができるようにしたものである。

つまり、前記（３７）式において、位置のみに着目すると、次の（４６）式のようになる。

上記（４６）式を次の（４９）式のように表し、この（４９）式を変形してゆくと、前記（４３）式で表される同定したいｍ個のばね定数の逆数の微小変動行列Δｋは（５２）式のようになる。

従って、同定したいばね定数の予測値の逆数を与えると、次の（５３）式によって同定したいばね定数の予測値の逆数を更新することができるので、第１の実施形態と同様に、誤差和ΔＲ_pの値が閾値ε以下となるように繰り返し計算することによってばね定数を同定することができる。

［第３の実施形態］
この実施形態は、^meabaseＴ_robbase或は^flangeＴ_toolが未知である場合、これらをばね定数の同定前に同定（推定）しようとするものである。

一般に、ロボットアーム２が１つの位置・姿勢を取ったとき、座標測定装置１７による計測値と、計算値との誤差ΔＴ（後述の（６２）式で示されるように９行１列）は次の（５４）式のように同定したいパラメータａの微小変動Δａの線形結合和で近似される。

上記（５６）式のＴ_meaは、第１の実施形態で説明したと同様の座標測定装置１７による計測値による同次変換行列、（５７）式のＴ_robは、計算値による同次変換行列である。ただし、本実施形態では、Ｔ_robを求めるに際し、各回転関節軸１１の撓みを考慮せず、各回転関節軸１１の回転角だけを基にする。

^meabaseＴ_robbaseおよび^flangeＴ_toolを表す同次変換行列は、次の（５８）式および（５９）式で表される。

また、同定したいパラメータａは、次の（６０）式で示され、当該パラメータａの微小変動Δａは、（６１）式で示される。

ここで、ノーマルベクトル、アプローチベクトル、オリエントベクトルは、第１の実施形態で示した関係があるので、（５４）式の誤差ΔＴおよびヤコビ行列Ｊをオリエントベクトルとアプローチベクトルを用いて表すと、それぞれ（６２）式および（６３）式のようになる。

誤差ΔＴを複数の動作位置の全ての動作位置に拡張すると、（６４）式のようになる。

上記（６４）式を次の（６５）式のように表し、この（６５）式を前記第１の実施形態における（４２）式、（４３）式のように変形してゆくと、同定したいパラメータａの微小変動行列Δａは、（６８）式のようになる。

従って、次の（６９）式のａ_vに同定したいパラメータの予測値ａ₀を与えると、パラメータａの予測値を更新することができる。

そして、以上の計算を、ΔＲが所定の閾値以下となるまで繰り返すことによって同定したいパラメータａを求めることができる。

［その他の実施形態］
第３の実施形態における^meabaseＴ_robbaseおよび^flangeＴ_toolの同定は、第１の実施形態におけるばね定数の同定と同時に行うようにしても良い。
フランジ９に取り付ける重りは２つに限られず、３つ以上であっても良い。この場合において、（ΔＴ_highioad−ΔＴ_lowload）は２つずつの重りの関係において求めれば良い。
ばね定数Ｋｚ，Ｋｘ，Ｋｙの予測値Ｋ_z0，Ｋ_x0，Ｋ_y0は、ばね定数同定装置１９の記憶部（記憶手段）に記憶させるようにしても良い。
対象とする多関節型のロボットは６軸のものに限らない。

図面中、２はロボットアーム、９はフランジ（ロボットアームの先端）、１１は回転関節軸、１４はロボット制御部、１５は関節角度検出器、１６は記憶部（記憶手段）、１７は座標測定装置（計測手段）、１８はばね定数同定装置（情報取得手段、測定点計算手段、同次変換行列計算手段、誤差和行列計算手段、同定手段、選択手段、ヤコビ和行列計算手段、微小変動行列計算手段、予測値逆数行列更新手段、同次変換行列再計算手段）、１９は測定制御部、２０は重り、ｔは測定点を示す。

Claims (2)

1. 複数の回転関節軸により複数のアームを連結して構成された多関節のロボットアームにおいて、前記各回転関節軸の回転中心軸線と同一の軸線をｚ軸、このｚ軸に直交する２軸線であって互いに直交する２軸線をｘ軸およびｙ軸とし、前記各回転関節軸へのトルク伝達経路を含む前記ｚ軸を中心とする回転方向の撓みのばね定数をＫｚ、前記ｘ軸を中心とする回転方向の撓みのばね定数をＫｘおよび前記ｙ軸を中心とする回転方向の撓みのばね定数をＫｙとしたとき、
   前記ロボットアームの先端または当該ロボットアームの先端近くに測定点を定め、前記ロボットアームを複数の互いに異なる位置に動作させて動作位置毎に質量の異なる少なくとも２つの重りを１つずつ前記ロボットアームの先端に取り付けたとき、各動作位置において、前記各回転関節軸の回転角を取得すると共に、前記重りが替わる毎に、前記測定点の位置および姿勢のうちの少なくとも位置を計測手段により計測し、
   前記ロボットアームの前記各動作位置において前記ロボットアームの先端に取り付けた重り毎に、当該重りの質量、前記各アームの長さ、前記各回転関節軸の各ばね定数の予測値および前記回転関節軸の回転角を用いて前記測定点の位置を計算し、
   前記各動作位置において前記重り毎に、前記計測手段により計測した前記測定点の少なくとも位置についての所定の固定座標に基づく同次変換行列Ｔｍおよび前記計算により求めた前記測定点の少なくとも位置についての前記所定の固定座標に基づく同次変換行列Ｔｃを求めて当該両同次変換行列Ｔｍ，Ｔｃの差の誤差行列ΔＴを求め、
   前記各動作位置において前記重り毎に求めた前記誤差行列ΔＴのうち、質量の大なる重りについての前記誤差行列ΔＴ_highloadと質量の小なる重りについての前記誤差行列ΔＴ_lowloadとの差を前記複数の動作位置について合計した誤差和行列ΔＲを求め、
   前記誤差和行列ΔＲの値が所定の閾値以下であるか否かを判断し、
   前記誤差和行列ΔＲの値が前記所定の閾値以下であれば、前記各回転関節軸の前記各ばね定数Ｋｘ，Ｋｙ，Ｋｚの前記予測値を前記各ばね定数として同定し、
   前記誤差和行列ΔＲの値が前記所定の閾値を超えていた場合には、その後、
   前記各回転関節軸の前記各ばね定数Ｋｘ，Ｋｙ，Ｋｚのうちから同定すべきばね定数を選択し、
   前記各動作位置において前記重り毎に計算により求めた前記測定点の少なくとも位置についての前記同次変換行列Ｔｃと前記同定すべきばね定数の予測値の逆数の行列ｋ_vについてのヤコビ行列Ｊを求め、且つ、質量の大なる重りのヤコビ行列Ｊ_highloadと質量の小なる重りのヤコビ行列Ｊ_lowloadとの差を前記各動作位置について合計したヤコビ和行列Ａを求め、
   前記ヤコビ和行列Ａの疑似逆行列と前記誤差和行列ΔＲとの積から前記同定すべきばね定数の予測値についての逆数の微小変動行列Δｋを求め、
   前記同定すべきばね定数の予測値の逆数の行列ｋ_vに前記微小変動行列Δｋを加算して前記同定すべきばね定数の予測値の逆数の行列ｋ_vを更新し、
   前記同定すべきばね定数については前記更新した予測値の逆数の行列ｋ_v+1から求めたばね定数を用い、前記同定すべきばね定数以外のばね定数については当初の前記予測値を用いて前記複数の各動作位置について前記重り毎の前記測定点の少なくとも位置を再計算して当該再計算した前記測定点の少なくとも位置についての前記同次変換行列Ｔｃを再度求め、
   前記複数の動作位置について、前記重り毎に前記計測手段により求めた前記測定点の前記同次変換行列Ｔｍと前記再度計算により求めた前記測定点の前記同次変換行列Ｔｃとから前記誤差行列ΔＴを再度求め、且つ、再度前求めた記重り毎の前記誤差行列ΔＴのうち、質量の大なる重りについての誤差行列ΔＴ_highloadと質量の小なる重りについての誤差行列ΔＴ_lowloadの差を前記複数の動作位置について合計した前記誤差和行列ΔＲを再度求め、
   前記再度求めた誤差和行列ΔＲの値が前記所定の閾値以下であるか否かを、前記再度求めた誤差和行列ΔＲが所定の閾値以下となるまで繰り返して前記同定すべき各ばね定数を同定するようにしてなるロボットのばね定数同定方法。
2. 複数の回転関節軸により複数のアームを連結して構成された多関節のロボットアームの制御部、および前記ロボットアームの先端または当該ロボットアームの先端近くに定められた測定点の位置および姿勢のうちの少なくとも位置を計測する計測手段の制御部に接続され、
   前記各回転関節軸の回転中心軸線と同一の軸線をｚ軸、このｚ軸に直交する２軸線であって互いに直交する２軸線をｘ軸およびｙ軸とし、前記各回転関節軸へのトルク伝達経路を含む前記ｚ軸を中心とする回転方向の撓みのばね定数をＫｚ、前記ｘ軸を中心とする回転方向の撓みのばね定数をＫｘおよび前記ｙ軸を中心とする回転方向の撓みのばね定数をＫｙとしたとき、それらばね定数の予測値を記憶した記憶手段と、
   前記ロボットアームを複数の互いに異なる位置に動作させて動作位置毎に質量の異なる少なくとも２つの重りを１つずつ前記ロボットアームの先端に取り付けたとき、各動作位置において、前記ロボットアームの前記制御部から前記各回転関節軸の回転角を取得すると共に、前記重りが換わる毎に、前記計測手段から前記測定点の位置および姿勢のうちの少なくとも位置を取得する情報取得手段と、
   前記ロボットアームの前記各動作位置において前記ロボットアームの先端に取り付けた重り毎に、当該重りの質量、前記各アームの長さ、前記各回転関節軸の各ばね定数の予測値および前記回転関節軸の回転角を用いて前記測定点の位置を計算する位置計算手段と、
   前記各動作位置において前記重り毎に、前記計測手段により計測した前記測定点の少なくとも位置についての所定の固定座標に基づく同次変換行列Ｔｍおよび前記計算により求めた前記測定点の少なくとも位置についての前記所定の固定座標に基づく同次変換行列Ｔｃを求める同次変換行列計算手段と、
   前記両同次変換行列Ｔｍ，Ｔｃの差の誤差行列ΔＴを求め、前記各動作位置において前記重り毎に求めた前記誤差行列ΔＴのうち、質量の大なる重りについての前記誤差行列ΔＴ_highloadと質量の小なる重りについての前記誤差行列ΔＴ_lowloadとの差を前記複数の動作位置について合計した誤差和行列ΔＲを求める誤差和行列計算手段と、
   前記誤差和行列ΔＲの値が所定の閾値以下であるか否かを判断する判断手段と、
   前記誤差和行列ΔＲの値が前記所定の閾値以下であれば、前記各回転関節軸の前記各ばね定数Ｋｘ，Ｋｙ，Ｋｚの前記予測値を前記各ばね定数として同定する同定手段と、
   前記誤差和行列ΔＲの値が前記所定の閾値を超えていた場合、前記各動作位置の前記各ばね定数のうちから同定すべきばね定数を選択する選択手段と、
   前記各動作位置において前記重り毎に計算により求めた前記測定点の少なくとも位置についての前記同次変換行列Ｔｃと前記同定すべきばね定数の予測値の逆数の行列ｋ_vについてのヤコビ行列Ｊを求め、且つ、質量の大なる重りのヤコビ行列Ｊ_highloadと質量の小なる重りのヤコビ行列Ｊ_lowloadとの差を前記各動作位置について合計したヤコビ和行列Ａを求めるヤコビ和行列計算手段と、
   前記ヤコビ和行列Ａの疑似逆行列と前記誤差和行列ΔＲとの積から前記同定すべきばね定数の予測値についての逆数の微小変動行列Δｋを求める微小変動行列計算手段と、
   前記同定すべきばね定数の予測値の逆数の行列ｋ_vに前記微小変動行列Δｋを加算して前記同定すべきばね定数の予測値の逆数の行列ｋ_vを更新する予測値逆数行列更新手段と、
   前記同定すべきばね定数については前記更新した予測値の逆数の行列ｋ_v+1から求めたばね定数を用い、前記同定すべきばね定数以外のばね定数については当初の前記予測値を用いて前記複数の各動作位置について前記重り毎の前記測定点の少なくとも位置を再計算して当該再計算した前記測定点の少なくとも位置についての前記同次変換行列Ｔｃを再度求める同時変換行列再計算手段と、
   前記複数の動作位置について、前記重り毎に前記計測手段により求めた前記測定点の前記同次変換行列Ｔｍと前記再度計算により求めた前記測定点の前記同次変換行列Ｔｃとから前記誤差行列ΔＴを再度求め、且つ、再度前求めた記重り毎の前記誤差行列ΔＴのうち、質量の大なる重りについての誤差行列ΔＴ_highloadと質量の小なる重りについての誤差行列ΔＴ_lowloadの差を前記複数の動作位置について合計した前記誤差和行列ΔＲを再度求める誤差和行列再計算手段と、
   前記再度求めた誤差和行列ΔＲの値が前記所定の閾値以下であるか否かを、前記再度求めた誤差和行列ΔＲが所定の閾値以下となるまで繰り返して前記同定すべき各ばね定数を同定する繰り返し実行手段と
   を具備してなるロボットのばね定数同定装置。

Images