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

Description

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

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

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

特開平５−２０９８０５

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

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

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

本発明では、ロボットアームの先端または先端近くに定められた測定点の位置および姿勢のうちの少なくとも位置を、計測手段により計測すると共に、各回転関節軸のｚｘｙの各軸まわりのばね定数の予測値を用いて計算により求め、計測値と計算値の差は、各回転関節軸のｚｘｙの各軸まわりのばね定数の真の値と、予測値として人為的に定めた値との差によるものとして、両者の差が所定の閾値以下となるようにばね定数の予測値を数学的手法により修正してゆくので、各回転関節軸の回転中心軸線（ｚ軸）まわりのばね定数ばかりでなく、回転中心と直交する２軸（ｘｙ軸）まわりのばね定数をも求めることができ、しかも、最終的に求められた各ばね定数は、ある程度の正確さをもって真のばね定数に近い値となる。

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

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

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

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

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

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

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

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

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

ここで、ｚ軸まわりのばね定数Ｋｚとは、図７（ｃ）に示す回転関節軸１１単独の撓み（捩れ）だけでなく、サーボモータ１２の回転軸から歯車減速装置１３を経て回転関節軸１１までのアーム４〜９へのトルク伝達経路を含んだ撓み（捩れ）についてのものとされる。また、ｘ軸およびｙ軸を中心とする回転方向の撓みは、図７（ａ）および（ｂ）に示すように、軸受１０において生ずるものがほとんどで、回転関節軸１１の曲げはほとんど生じないことから、ｘ軸まわりのばね定数Ｋｘおよびｙ軸まわりのばね定数Ｋｙについては、回転関節軸１１を剛体として扱うこととする。

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

上記座標測定装置１７は、ロボットアーム２の先端または先端近くに定められた測定点の位置および姿勢を測定するためのものである。この実施形態では、フランジ９の手首８側を基端、手首８とは反対側を先端としたとき、図３に示すように、フランジ９の先端面（ロボットアーム２の先端）に、ばね定数の同定のために使用する重り２０が接着などによって固定されている。そして、この重り２０の先端面の重心位置が測定点ｔとして定められている。従って、測定点ｔは、この実施形態においては、ロボットアーム２の先端近くに定められていることとなる。なお、重り２０は、ねじによりフランジ９に取り付けるようにしても良い。

上記測定点ｔには、当該測定点ｔを原点とする測定点座標が規定されている。この測定点座標は、重り２０の先端面に直交する軸線をＺｔ座標軸、このＺｔ座標軸に直交する２軸線であって重り２０の先端面上に存在する２軸線をＸｔ座標軸およびＹｔ座標軸とした３次元座標によって構成されている。そして、Ｘｔ，Ｙｔ，Ｚｔの各座標軸上の単位ベクトルがそれぞれアプローチベクトルＡ、ノーマルベクトルＮおよびオリエントベクトルＯとして定義されている。

図３には、Ｚｆ，Ｘｆ，Ｙｆで表されたフランジ９座標も示されている。この場合、重り２０の先端面をフランジ９の先端面と平行、先端面の測定点ｔをフランジ９の先端面に垂直のＺｆ座標軸と一致させることにより、フランジ座標についての測定点座標の位置・姿勢は既知となっている。つまり、測定点ｔはフランジ座標の原点から重り２０の厚さだけ離れ、測定点座標のＺｔ座標軸はフランジ座標のＺｆ座標軸に重なり、測定点座標のＸｔ，Ｙｔの各座標軸はフランジ座標のＸｆ，Ｙｆの各座標軸と平行になっている。

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

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

さて、本発明のばね定数の同定方法の基本的な考え方は次のようなものである。つまり、ロボット制御部１４からの回転指令より各サーボモータ１２が回転してロボットアーム２を動作させたとき、ロボット制御部１４から各回転関節軸１１の回転角がばね定数同定装置１９に与えると共に、測定制御部１８から測定点ｔの位置・姿勢についての計測情報がばね定数同定装置１９に与えられる。ばね定数同定装置１９は、ロボット制御部１４から与えられた各回転関節軸１１の回転角と、各回転関節軸１１のｚｘｙの各軸まわりに作用するトルクと、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_z0，Ｋ_x0，Ｋ_y0を用いて測定点ｔの位置・姿勢を計算する。

そして、各アーム３〜９は構造上ほとんど撓みを生じないことから剛体として扱うことができるので、計測した測定点ｔの位置・姿勢と、計算により求めた測定点ｔの位置・姿勢との間に差が生じた場合、その差は、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの予測値Ｋ_z0，Ｋ_x0，Ｋ_y0と、各回転関節軸１１のばね定数Ｋｘ，Ｋｙ，Ｋｚの真の値との差によって生じたものと考えられる。従って、計測した測定点ｔの位置・姿勢と、計算により求めた測定点ｔの位置・姿勢との間に差がなくなるように、予測値Ｋ_z0，Ｋ_x0，Ｋ_y0を変化させてゆくことによってばね定数Ｋｘ，Ｋｙ，Ｋｚの真の値、或はそれに近い値に同定することができるものである。なお、当初の予測値Ｋ_z0，Ｋ_x0，Ｋ_y0は、例えばロボット１の記憶部１６に予め記憶されている。

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

次に、ロボットアーム２を決定した複数の動作位置に順次移動させる（ステップＳ２）。このとき、各動作位置に移動させる毎に、座標測定装置１７によりロボットアーム２の測定点ｔの位置と姿勢を計測する。そして、計測結果である位置・姿勢計測情報を測定制御部１８からばね定数同定装置１９に送る共に、各回転関節軸１１を駆動するサーボモータ１２の回転数から検出された各回転関節軸１１の回転角情報をロボット制御部１４からばね定数同定装置１９に送る。このばね定数同定装置１９に送られた位置・姿勢計測情報と各回転関節軸１１の回転角情報は、当該ばね定数同定装置１９のＲＡＭなどの記憶部（図示せず）に記憶される（以上、ステップＳ２〜５で「ＮＯ」の繰り返し；情報取得手段）。

ロボットアーム２を複数の動作位置の全てに移動させ終えると（ステップＳ５で「ＹＥＳ」）、ばね定数同定装置１９は、位置・姿勢計測情報から測定点ｔの位置・姿勢を表すベクトルの同次変換行列Ｔ_mea（請求項でいうＴｍ）を求める。勿論、この計測情報から求めた測定点ｔの位置・姿勢の同次変換行列Ｔ_meaは、計測座標（所定の固定座標）についてのものである。

また、ばね定数同定装置１９は、各回転関節軸１１の回転角情報を用いて測定点ｔの位置・姿勢を計算によって求める（測定点計算手段）。ここで、図１において、^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 … （９）
で表される。

上記トルクｔ_zi，ｔ_xi，ｔ_yiは、関節角度検出器１５によって検出された各回転関節軸１１の回転角情報から得られる回転角、各アーム４〜９の質量、各アーム４〜９の重心位置、各アーム４〜９のイナーシャ、重り２０の質量および重心位置により、ニュートンオイラー法によって計算する。なお、ニュートンオイラー法は、角速度、角加速度に関する情報を必要とするが、ロボットアーム２は停止しているので、角速度、角加速度に関する情報はなくて良い。

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

計算により同次変換行列^robbaseＴ_flangeを求めた後、前述したように次の（１６）式によって計測座標についての測定点ｔの位置・姿勢を示す同次変換行列Ｔ_robを求める（ステップＳ６：同次変換行列計算手段）。
Ｔ_rob＝^meabaseＴ_robbase・^robbaseＴ_flange・^flangeＴ_tool…（１６）

以上のようにしてＴ_robを計算によって求めると、次に、ばね定数同定装置１９は、ロボットアーム２の動作位置毎に、計測によって求められた測定点ｔの同時変換行列Ｔ_meaと計算によって求めた測定点ｔの同次変換行列Ｔ_robとの差を示す誤差行列ΔＴを求め（ステップＳ７：誤差行列計算手段）、次いで、各動作位置における上記誤差行列ΔＴの総和（誤差和）ΔＴ_all（請求項でいう誤差和行列ΔＲに相当）を次の（１７）式によって求める（誤差和行列計算手段）。

なお、（１７）式のｈは、動作位置の数を示す。

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

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

ところで、以上の説明から理解されるように、計算により求めた測定点ｔの位置・姿勢を示す同次変換行列Ｔ_robは、各回転関節軸１１の未知の撓み角ｓの関数であり、撓み角ｓはｚｘｙの各軸のばね定数Ｋｘ，Ｋｙ，Ｋｚの逆数ｋの関数であるから、次の（１８）式で表すことができ、また、全てのばね定数の逆数ｋは（１９）式の行列で表すことができる。なお、（１９）式の括弧の右肩に小さくｔで示す符号は、この行列が横書きされているが、実際には転置行列であることを示している。また、ｎは回転関節軸１１の数、本実施形態では６である。

選択された同定したいｍ個のばね定数Ｋ１〜Ｋｍの逆数ｋ１〜ｋｍは次の行列（２０）で示される。

ロボットアーム２に取らせた各動作位置についてＴ_meaとＴ_robとの誤差行列ΔＴは、次の（２１）式で示されるように、同定したいｍ個のばね定数Ｋ１〜Ｋｍの逆数ｋ₁〜ｋ_mの微小変動Δｋの線形結合和で近似され、従って、計算により求めた測定点ｔの同次変換行列Ｔ_robおよび同定したいｍ個のばね定数Ｋ１〜Ｋｍの逆数ｋ₁〜ｋ_mについてのヤコビ行列Ｊとなる。

ここで、Ｔ_meaとＴ_robとの差は、測定点座標のノーマルベクトル、オリエントベクトルおよびアプローチベクトルの差Δｎ，ΔｏおよびΔａと、計測座標からの位置ベクトルの差Δｐで示すと、次の（２２）式のようになる。

従って、Ｔ_meaとＴ_robとの差は、Δｏ、ΔａおよびΔｐを用いて表すと、次の（２５）式のようになる。

また、前記ヤコビ行列Ｊは、オリエントベクトル、アプローチベクトルおよび位置ベクトルを用いて表すと、次の（２６）式で示されるように、計算により求めた測定点のノーマルベクトル、オリエントベクトル、アプローチベクトルのうちの例えばオリエントベクトル、アプローチベクトルの計測座標のＺｍ，Ｘｍ，Ｙｍの各座標軸上での位置および測定点の位置ベクトルの計測座標のＺｍ，Ｘｍ，Ｙｍの各座標軸上での位置を同定したいばね定数の逆数ｋ₁〜ｋ_mで除した値の行列で表される。

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

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

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

さて、前記（２１）式、つまり各位置・姿勢におけるＴ_meaとＴ_robとの誤差（Ｔ_mea−Ｔ_rob）を全ての動作位置に拡張してその総和ΔＲ（誤差和行列）を求めると、次の（３２）式、（３３）式となる。

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

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

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

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

その後、ばね定数同定装置１９は、前記（１３）〜（１５）式のばね定数の予測値の逆数に、同定したいばね定数Ｋ１〜Ｋｍについては更新した予測値の逆数ｋ_v+1を用い、同定を希望しないばね定数については当初の予測値の逆数をそのまま用い、そして、前記行列（１）式および（２）により各動作位置についての^robbaseＴ_flangeを再度求める。次いで、ばね定数同定装置１９は、前記（１６）式からＴ_robを再度求め（ステップＳ６：同時変換行列再計算手段）、且つ、先に求めたＴ_meaとの誤差ΔＴを再度求め（ステップＳ７：誤差行列再計算手段）、続いて、（１７）式により誤差ΔＴの総和ΔＴ_all（ΔＲ）を再度求め（誤差和行列再計算手段）、その誤差和ΔＴ_all（ΔＲ）が所定の閾値ε以下であるかを判断する（ステップＳ８）。

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

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

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

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

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

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

［第２の実施形態］
上述の第１の実施形態では、動作位置毎の位置と姿勢の双方を用いてばね定数を同定した。本実施形態では、姿勢を計測できない場合などに対応できるように、位置だけでばね定数を同定することができるようにしたものである。
つまり、前記（３２）式において、位置のみに着目すると、次の（３９）式のようになる。

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

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

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

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

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

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

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

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

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

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

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

そして、以上の計算を、ΔＲが所定の閾値以下となるまで繰り返すことによって同定したいパラメータａを求めることができる。
［その他の実施形態］
^meabaseＴ_robbaseおよび^flangeＴ_toolは、ばね定数と同時に同定するようにしても良い。
ロボットアームの先端に重りを付けなくても良い。但し、重りを付けた方が回転関節軸１１の撓みが大きくなるので、ばね定数を同定し易くなる。
ばね定数Ｋｚ，Ｋｘ，Ｋｙの予測値Ｋ_z0，Ｋ_x0，Ｋ_y0は、ばね定数同定装置１９の記憶部に記憶させるようにしても良い。
（２２）式から理解されるように、１つの動作位置について９つの連立方程式ができるので、理論的には、同定したいばね定数が９個以下の場合には、ロボットアーム２の動作位置は１つで良く、１８個以下であれば、動作位置は２つあれば良い。

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

Claims (2)

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

Images