JP2021010983A - トルク補償装置及びトルク補償方法 - Google Patents

Abstract

【課題】機械的な機構に依らず、トルク外乱による誤差を補償可能とする。【解決手段】ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得するデータ取得部１と、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及びデータ取得部１により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部４２と、データ取得部１により取得されたトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部４３と、残差算出部４３により算出された補償後トルクを示すデータを出力する出力部４４とを備えた。【選択図】図１

Description

この発明は、トルクセンサが取付けられたロボットアームに対してトルク外乱による誤差の補償を行うトルク補償装置及びトルク補償方法に関する。

各関節にトルクセンサが取付けられ、このトルクセンサにより検出されたトルク、又は当該トルクから推定した力を制御するロボットアームが知られている（例えば特許文献１参照）。

特開２００６−０００９５５号公報

しかしながら、ロボットアームが減速機を用いている場合、トルクセンサにより検出されたトルクは、減速機の機械的変化が外乱となって誤差を含むことがある。これは、減速機が歯車のような回転機構を持つことに起因しており、例えばロボットアームで使われることが多い波動歯車を持つ減速機でも起こり得る誤差である。トルク外乱は、ロボットアームの関節角が僅かに変わっても大きく変わることがあり、トルク制御及び力制御の性能を劣化させる。このトルク外乱を抑制する機械的な機構を設計することは可能であるが、機構が複雑になり、また、コストも上昇する。

この発明は、上記のような課題を解決するためになされたもので、機械的な機構に依らず、トルク外乱による誤差を補償可能なトルク補償装置を提供することを目的としている。

この発明に係るトルク補償装置は、ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得部と、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及びデータ取得部により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部と、データ取得部により取得されたトルクと、補償値算出部により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部と、残差算出部により算出された補償後トルクを示すデータを出力する出力部とを備えたことを特徴とする。

この発明によれば、上記のように構成したので、機械的な機構に依らず、トルク外乱による誤差を補償可能となる。

実施の形態１に係るトルク補償装置の構成例を示す図である。 実施の形態１に係るトルク補償装置による推定動作例を示すフローチャートである。 実施の形態１に係るトルク補償装置の動作例を示す図であり、トルク集合と関節角集合の一例を示す図である。 実施の形態１における関数推定部による動作例を示す図である。 実施の形態１に係るトルク補償装置による補償動作例を示すフローチャートである。 実施の形態２に係るトルク補償装置の構成例を示す図である。 実施の形態２に係るトルク補償装置による推定動作例を示すフローチャートである。 実施の形態１に係るトルク補償装置による初期値算出動作例を示すフローチャートである。

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１は実施の形態１に係るトルク補償装置の構成例を示す図である。
トルク補償装置は、各関節にトルクセンサが取付けられたロボットアームに対してトルク外乱による誤差の補償を行う。ロボットアームは歯車のような回転機構を持つ減速機を用いている。また、トルク外乱は、回転機構を持つ減速機に起因するトルク外乱である。トルク補償装置は、図１に示すように、データ取得部１、前処理部２、推定部３及び補償部４を備えている。なお、トルク補償装置は、システムＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等により実現される。図１に示すトルク補償装置では、トルク外乱による誤差の補償を行う前に、事前に、その補償に用いる補償関数の推定を行う。

データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する。

前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う。
図１では、トルク補償装置に前処理部２が設けられた場合を示している。しかしながら、これに限らず、トルク補償装置に前処理部２が設けられていなくてもよい。この場合、推定部３及び補償部４は、データ取得部１により取得されたデータを用いて各種処理を行う。

推定部３は、前処理部２による前処理後のデータに基づいて、補償関数を推定する。補償関数は、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である。推定部３は、図１に示すように、関数推定部３１及び出力部３２を有している。

関数推定部３１は、前処理部２による前処理後のデータに基づいて、補償関数を推定する。
出力部３２は、関数推定部３１により推定された補償関数を示すデータを外部に出力する。

補償部４は、前処理部２による前処理後のデータ及び推定部３により推定された補償関数に基づいて、トルク外乱による誤差を補償したトルク（補償後トルク）を示すデータを出力する。補償部４は、図１に示すように、関数読込部４１、補償値算出部４２、残差算出部４３及び出力部４４を有している。

関数読込部４１は、出力部３２により出力されたデータから、関数推定部３１により推定された補償関数を読込む。

補償値算出部４２は、関数読込部４１により読込まれた補償関数及び前処理部２による前処理後のデータが示す関節角に基づいて、当該関節角におけるトルク補償値を算出する。トルク補償値は、トルク外乱による誤差を補償するための補償値である。

残差算出部４３は、前処理部２による前処理後のデータが示すトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する。

出力部４４は、残差算出部４３により算出された補償後トルクを示すデータを外部に出力する。出力部４４により出力されたデータは、トルク制御又は力制御等に利用される。

なお図１では、推定部３に出力部３２が設けられ、補償部４に関数読込部４１が設けられている。しかしながら、これに限らず、推定部３による推定結果を示すデータを内部で保持してそのまま補償部４で用いる場合には出力部３２及び関数読込部４１は必須ではなく、出力部３２及び関数読込部４１はトルク補償装置に設けられていなくてもよい。

また図１では、トルク補償装置に推定部３及び補償部４の両方が設けられている。しかしながら、これに限らず、推定部３についてはトルク補償装置に設けず、トルク補償装置は外部から入力されたデータが示す補償関数を用いてトルク外乱による誤差の補償を行ってもよい。

次に、図１に示す実施の形態１に係るトルク補償装置の動作例について説明する。
ここで、トルク外乱は、回転機構を持つ減速機に起因することから、その回転周期に応じた周期性を持つと考えられる。そこで、実施の形態１に係るトルク補償装置では、トルク外乱が周期性を持つことを利用し、周期関数によってトルク外乱を推定してトルク外乱による誤差の補償を行う。

また上述の通り、ロボットアームの関節角の変化に伴ってトルク外乱は変化する。このことから、トルク外乱は、関節角の関数であると考えられる。更に、トルク外乱が周期性を持つことを考えると、補償関数は周期関数で表せられることがわかる。すなわち、関節角を入力としトルク補償値を出力とする周期関数を推定することで、トルク外乱による誤差を補償可能となる。これにより、実施の形態１に係るトルク補償装置では、トルク外乱を抑制する機械的な機構を追加せずに、ロボットアームにおけるトルク制御及び力制御の性能を確保可能となる。

まず、図１に示す実施の形態１に係るトルク補償装置による推定処理例について、図２を参照しながら説明する。
図１に示す実施の形態１に係るトルク補償装置による推定処理例では、図２に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ２０１）。

なお、トルク補償装置では、回転機構を持つ減速機に起因するトルク外乱による誤差を補償することを目的としている。よって、トルク補償装置による推定処理において用いられるトルクは、減速機に起因するトルク外乱以外の外乱が含まれていないことが望ましい。以下では、トルク補償装置は、上記外乱が含まれていないトルクを用いるものとする。例えば、トルクセンサにより検出されたトルクが重力によるトルクを含む場合は、事前にその成分を除去する必要がある。この重力トルク成分の算出は例えば特許文献２のように公知の技術なので、それらの技術を利用して除去が可能である。また、ロボットアームにかかる慣性及びコリオリ力の影響を十分に抑制するため、トルク値を取得する際はロボットアームの速度と加速度を十分小さく抑えるとよい。以上のようにすることで、トルク補償装置は上記外乱が事実上含まれていないトルクを得ることができる。
特開平０１−０６６７１５号公報

次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ２０２）。前処理部２による前処理は、外れ値除去又は移動平均等、その状況に適した処理であればよい。

次いで、関数推定部３１は、前処理部２による前処理後のデータに基づいて、補償関数を推定する（ステップＳＴ２０３）。実施の形態１では、関数推定部３１は、トルク外乱の持つ周期を推定した後、１周期分のトルク外乱による誤差を補償するための関数（補償関数）を推定する。

補償関数は、周期数分繰返した周期関数とトルク外乱による誤差とを最小化するような関数である。この補償関数としては様々な形態が考えられる。例えば、補償関数として、階段関数、区分線形関数、多項式又は三角関数等を用いることができる。そして、関数推定部３１は、それらを最尤推定、最小二乗法、アンサンブル回帰、ガウス過程回帰又はサポートベクター回帰等で推定する。

以下では、一例として、補償関数として３次元多項式を用いた場合を示す。補償関数をｆ（ｘ）とし、そのパラメータをａ，ｂ，ｃ，ｄとすると、３次多項式の補償関数は下式（１）で表される。
ｆ（ｘ）＝ａｘ^３＋ｂｘ^２＋ｃｘ＋ｄ （１）

関数推定部３１は、まず、前処理部２による前処理が行われたデータに基づいて、トルクに含まれる成分の周期の推定を行う。関数推定部３１による周期の推定方法としては幾つか考えられ、例えば減速機の仕様又はトルクを離散フーリエ変換によって周波数領域で表した結果等から推定可能である。関数推定部３１は、周期を推定した後、その１周期分の区間におけるトルク及び関節角を取出し、補償関数の推定に用いる。

図３に示すように、トルクのうちの周期を持つ成分の周期数をｎとし、トルク（トルク集合）を周期毎に区切った区間トルクをｔ_１，ｔ_２，・・・，ｔ_ｎとし、関節角（関節角集合）を当該周期毎に区切った区間関節角をｘ_１，ｘ_２，・・・，ｘ_ｎとし、一つの区間関節角の幅をδとする。なお、任意の区間（ｊ∈｛１，２，・・・，ｎ｝）における区間トルク（ｔ_ｊ）は複数のデータ点（ｔ_ｊ１，ｔ_ｊ２，・・・）を含み、任意の区間（ｊ∈｛１，２，・・・，ｎ｝）における関節角区間（ｘ_ｊ）は複数のデータ点（ｘ_ｊ１，ｘ_ｊ２，・・・）を含む。
ここでは、補償関数の推定例として、任意の区間における区間トルクとの誤差を最小化するような補償関数を推定し、その結果を他の区間でも適用してトルク補償を行うこととする。以下では、ｊ＝１として説明する。また、説明を平易にするため、ｊにおける任意のデータ点（ｋ）の関節角（ｘ_ｊｋ）は、（ｊ−１）×δ≦ｘ_ｊｋ＜ｊ×δを満たすものとする。なお、実際に取得した関節角の値がこの条件を満たしていなかったとしても、条件に合うように平行移動させればよい。

まず、前提として、下式（２），（３）に示す誤差関数であるｌ（ｘ）を最小化するような補償関数を推定する。なお、｜ｖ｜はベクトルであるｖの大きさ（絶対値）を表す。
ｌ（ｘ_１）＝｜ｔ_１−ｆ（ｘ_１）｜ （２）
ｆ（ｘ_１）＝｛ｆ（ｘ_１１），ｆ（ｘ_１２），・・・｝ （３）

トルク集合は、トルクの周期性を利用してその１周期分毎に区切られた区間トルクの集合であることを踏まえると、式（２）を最小化するような補償関数であれば、他の区間においても補償に利用可能であると考えられる。すなわち、誤差関数を最小化するような補償関数のパラメータを最小二乗法等によって求めればよい。

以上の例では、補償関数を１区間分だけ推定したが、この補償関数を他の区間にもそのまま周期的に適用するので、周期関数となるような補償関数を推定したことと同じになる。
図４に、関数推定部３１により推定される補償関数の一例を示す。図４において、符号４０１に示す波形は前処理部２による前処理後のデータが示すトルクの波形を示し、符号４０２に示す波形は関数推定部３１により推定された補償関数の波形を示している。

次いで、出力部３２は、関数推定部３１により推定された補償関数を示すデータを外部に出力する（ステップＳＴ２０４）。

次に、図１に示す実施の形態１に係るトルク補償装置による補償処理例について、図５を参照しながら説明する。
図１に示す実施の形態１に係るトルク補償装置による補償処理例では、図５に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ５０１）。

次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ５０２）。

また、関数読込部４１は、出力部３２により出力されたデータから、関数推定部３１により推定された補償関数を読込む（ステップＳＴ５０３）。なお、補償関数は、同一のロボットアームに対しては一度読込んだものを繰返し用いることが可能であり、毎回読込む必要はないものとしてもよい。

次いで、補償値算出部４２は、関数読込部４１により読込まれた補償関数及び前処理部２により前処理が行われたデータが示す関節角に基づいて、当該関節角におけるトルク補償値を算出する（ステップＳＴ５０４）。

ここで、任意の関節角（ｘ（ハット））が得られた際のトルク補償値をｙ（ハット）とする。まず、補償値算出部４２は、関節角（ｘ（ハット））が、どの区間関節角（ｘ_１）に相当するかを判定する。すなわち、補償値算出部４２は、一つの区間関節角の幅（δ）で関節角（ｘ（ハット））を割り、その余り（λ）を求める。そして、下式（４）のように、補償値算出部４２は、その余りを補償関数に代入することでトルク補償値を算出する。
ｙ（ハット）＝ｆ（λ）＝ａλ^３＋ｂλ^２＋ｃλ＋ｄ （４）

次いで、残差算出部４３は、前処理部２により前処理が行われたデータが示すトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する（ステップＳＴ５０５）。

次いで、出力部４４は、残差算出部４３により算出された補償後トルクを示すデータを外部に出力する（ステップＳＴ５０６）。

このように、実施の形態１に係るトルク補償装置では、トルク外乱が周期性を持つことを利用し、１周期単位のある１つの区間において推定した周期関数を繰返し用いることで、他の区間も含めたトルク集合全体の補償を実施可能となる。

また上記では、推定部３が、一区間を用いて補償関数を推定する場合を示した。しかしながら、これに限らず、推定部３は、複数区間を用いて補償関数を推定してもよい。この場合、推定部３は、対象とする区間における誤差関数を求め、その総和を最小化するような補償関数を推定する。この際、推定部３は、各区間の誤差関数に任意の重みをかけることで、制御に重要な区間をより重点的に補償するような推定も可能である。これにより、制御性能の向上が見込める。

なお、補償関数として、ロボットアームの稼動条件に応じて複数の関数を使い分けて適用することも可能である。これにより、実施の形態１に係るトルク補償装置は、稼動条件によるトルク外乱の変動への対応が可能となる。

以上のように、この実施の形態１によれば、トルク補償装置は、ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得するデータ取得部１と、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及びデータ取得部１により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部４２と、データ取得部１により取得されたトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部４３と、残差算出部４３により算出された補償後トルクを示すデータを出力する出力部４４とを備えた。これにより、実施の形態１に係るトルク補償装置は、機械的な機構に依らず、トルク外乱による誤差を補償可能となる。

実施の形態２．
実施の形態１では、補償関数として適用可能な関数を限定していない。これに対し、実施の形態２では、補償関数として用いる関数を正弦関数に限定している。正弦関数はそれ自体が周期関数であり、その重ね合わせで任意の周期関数を表現できるため、補償関数として好適である。また、正弦関数は周期関数であるため、実施の形態１のように周期毎に区切らなくても関数推定が可能であり、実施の形態２では複数区間を対象として一括で関数推定を行う。また、推定部３は、関数推定の前に初期値の算出を行うことが好ましい。これは、正弦関数を表すパラメータの推定を効率化するためである。
図６は実施の形態２に係るトルク補償装置の構成例を示す図である。図６に示す実施の形態２に係るトルク補償装置は、図１に示す実施の形態１に係るトルク補償装置に対し、初期値算出部３３を追加している。その他の構成は同様であり、同一の符号を付して異なる部分についてのみ説明を行う。

初期値算出部３３は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータの初期値を算出する。初期値算出部３３は、図６に示すように、初期振幅算出部３３１、初期周波数算出部３３２及び初期位相算出部３３３を有している。

初期振幅算出部３３１は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである振幅の初期値を算出する。
初期周波数算出部３３２は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである周波数の初期値を算出する。

なお図６では、初期振幅算出部３３１の後段に初期周波数算出部３３２が設けられた場合を示している。しかしながら、これに限らず、初期振幅算出部３３１の前段に初期周波数算出部３３２が設けられていてもよい。

初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値及び初期周波数算出部３３２により算出された周波数の初期値に基づいて、正弦関数を表すパラメータである位相の初期値を算出する。

なお、関数推定部３１は、前処理部２による前処理後のデータ及び初期値算出部３３により算出された初期値に基づいて、補償関数を推定する。補償関数は、正弦関数の重ね合わせにより表される。

なお、初期値算出部３３は、関数推定部３１による関数推定を効率化するための手段であり、トルク補償装置に必須の構成ではない。また、トルク補償装置に初期値算出部３３が設けられていない場合には、正弦関数を表すパラメータの初期値は乱数等の任意の値とする。

次に、図６に示す実施の形態２に係るトルク補償装置による推定処理例について、図７を参照しながら説明する。
図６に示す実施の形態２に係るトルク補償装置による推定処理例では、図７に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ７０１）。

次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ７０２）。
ステップＳＴ７０１，７０２における処理は、実施の形態１におけるステップＳＴ２０１，２０２における処理と同様である。

次いで、初期値算出部３３は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである初期値を算出する（ステップＳＴ７０３）。以下、初期値算出部３３による初期値算出動作例について、図８を参照しながら説明する。

初期値算出部３３による初期値算出動作例では、図８に示すように、まず、初期振幅算出部３３１は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである振幅の初期値を算出する（ステップＳＴ８０１）。

この際、初期振幅算出部３３１は、まず、前処理部２による前処理後のデータが示すトルクに対し、離散フーリエ変換を行う。ここでは、離散フーリエ変換として、離散フーリエ変換を高速に行う高速フーリエ変換（ＦＦＴ：Ｆａｓｔ Ｆｏｕｒｉｅｒ Ｔｒａｎｓｆｏｒｍ）を用いるものとする。このＦＦＴにより、トルクの波形を、関節角領域から周波数領域で表すことができる。周期関数は単純な正弦関数又は余弦関数の重ね合わせで表現できることは公知であり、トルク外乱に周期性が有ることを踏まえると、トルクを周波数領域で表した結果からその関数を推定可能であるとわかる。ここでは、一例として、三角関数のうちの正弦関数を用いるものとする。

そして、初期振幅算出部３３１は、周波数領域で表したトルクに基づいて、ピークの絶対値を取ることで振幅の初期値（ａ_ｉｎｉｔ）を求める。周波数領域におけるピークの絶対値の大小は、その関数における影響の大小であると言えるため、絶対値の大きい周波数から補償に適用することが効率的である。すなわち、初期振幅算出部３３１が、振幅の初期値として選ぶのは、最も大きいピークの絶対値となる。

また、初期周波数算出部３３２は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである周波数の初期値を算出する（ステップＳＴ８０２）。
この際、初期周波数算出部３３２は、まず、前処理部２による前処理後のデータが示すトルクに対し、離散フーリエ変換を行う。ここでは、離散フーリエ変換として高速フーリエ変換（ＦＦＴ）を用いるものとする。そして、初期周波数算出部３３２は、周波数領域で表したトルクに基づいて、ピークの絶対値を持つ周波数を、周波数の初期値（ｆ_ｉｎｉｔ）とする。

次いで、初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値及び初期周波数算出部３３２により算出された周波数の初期値に基づいて、正弦関数を表すパラメータである位相の初期値を算出する（ステップＳＴ８０３）。

この際、初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値と、初期周波数算出部３３２により算出された周波数の初期値とを用いて、初期周期関数（ｌ（ｘ））を生成し、その初期周期関数を用いて位相の初期値（ｐ_ｉｎｉｔ）を求める。初期周期関数は、補償に用いる周期関数として正弦関数を用いることを想定しているため、下式（５）で表される。
ｌ（ｘ）＝ａ_ｉｎｉｔ×ｓｉｎ（２π×ｆ_ｉｎｉｔ×ｘ） （５）

初期位相算出部３３３は、トルクが持つ周波数成分のうちの影響が大きいとされる周波数とその振幅を用いて、初期周期関数を生成する。そのため、初期位相算出部３３３は、初期周期関数とトルクの位相差を求め、その位相差を位相の初期値とすればよい。初期位相算出部３３３は、初期周期関数とトルクとの相互相関関数によって位相差を求めることができる。すなわち、上記相互相関関数が最大となるときの位相差が、初期周期関数とトルクの波形が最もよく重なるときの位相差であり、初期位相算出部３３３はこれを位相の初期値とする。

再び図７に示すトルク補償装置による推定処理例の説明に戻り、次いで、関数推定部３１は、前処理部２による前処理後のデータ及び初期値算出部３３により算出された初期値に基づいて、補償関数を推定する（ステップＳＴ７０４）。

この際、関数推定部３１は、まず、初期値算出部３３により算出された初期値（振幅の初期値、周波数の初期値及び位相の初期値）を用いて、回転機構を持つ減速機に起因するトルク外乱による誤差を補償するための正弦関数として適切なパラメータ（振幅（ａ）、周波数（ｆ）及び位相（ｐ））を推定する。

前提として、上記トルク外乱による誤差を補償するための正弦関数として適切なパラメータとは、下式（６）を示す関数（Ｌ（ｘ））を最小化するものである。ここで、関節角（ｘ）及びトルク（ｔ）はＮ個のデータ点を含むものであり、ｔ_ｊは関節角（ｘ_ｊ）に対するトルクの値を示している。

関数推定部３１は、上記関数を最小化するようなパラメータを最小二乗法によって推定する。関数推定部３１は、上記推定の際の初期値として、初期値算出部３３により算出された初期値（振幅の初期値、周波数の初期値及び位相の初期値）を用いる。これにより、実施の形態２に係るトルク補償装置では、乱数等で定めた初期値に対して解に到達するまでの推定時間を短縮可能である。

その後、関数推定部３１は、上記処理を繰返し、トルクに含まれる各周波数成分についてそれぞれ補償する正弦関数のパラメータを推定し、複数の正弦関数を求める。なお、関数推定部３１は、推定の際に複数の正弦関数を用いてそれらのパラメータを同時に推定してもよく、その推定を更に繰返して正弦関数の数を増やしてもよい。

その後、関数推定部３１は、下式（７）から、任意の関節角（ｘ（ハット））におけるトルク外乱による誤差を補償するためのトルク補償値（ｙ（ハット））を求める。式（７）において、ｍは関数推定部３１が推定に用いた正弦関数の数を示し、ａ_ｉはｉ番目の正弦関数を表すパラメータである振幅を示し、ｆ_ｉはｉ番目の正弦関数を表すパラメータである周波数を示し、ｐ_ｉはｉ番目の正弦関数を表すパラメータである位相を示している。

次いで、出力部３２は、関数推定部３１により推定された補償関数を示すデータを出力する（ステップＳＴ７０５）。

このように、実施の形態２に係るトルク補償装置では、トルク外乱が関節角の周期関数であることを前提として正弦関数による近似を行うことで、その周波数成分の補償を行うことができる。これを繰返し、トルクに含まれる各周波数成分を補償する複数の正弦関数の重ね合わせを補償関数として求めることで、トルク全体の補償を行うことができる。補償に用いる正弦関数は、トルク外乱との誤差が最小化されるようなパラメータによって生成されることが望ましく、トルク補償装置は、予め初期値を算出しておくことで効率的に補償関数を推定可能である。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組合わせ、或いは各実施の形態の任意の構成要素の変形、若しくは各実施の形態において任意の構成要素の省略が可能である。
また上記では、ロボットアームが、回転関節を持ったロボットアームである場合を例に示した。しかしながら、これに限らず、ロボットアームは、トルクセンサが取付けられた回転駆動機構を持ったロボットアームであればよく、例えば、スカラ型ロボットアームのような複数軸構成を持ったロボットアーム、パラレルリンク型ロボットアーム、又は一軸のみを持つロボットアームでもよい。

１ データ取得部
２ 前処理部
３ 推定部
４ 補償部
３１ 関数推定部
３２ 出力部
３３ 初期値算出部
４１ 関数読込部
４２ 補償値算出部
４３ 残差算出部
４４ 出力部
３３１ 初期振幅算出部
３３２ 初期周波数算出部
３３３ 初期位相算出部

Claims (5)

1. ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得部と、
   回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及び前記データ取得部により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部と、
   前記データ取得部により取得されたトルクと、前記補償値算出部により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部と、
   前記残差算出部により算出された補償後トルクを示すデータを出力する出力部と
   を備えたトルク補償装置。
2. 前記データ取得部による取得結果に基づいて、前記減速機に起因するトルク外乱の関節角に対する周期関数である補償関数を推定する関数推定部を備え、
   前記補償値算出部は、前記関数推定部により推定された補償関数を用いてトルク補償値を算出する
   ことを特徴とする請求項１記載のトルク補償装置。
3. 前記関数推定部は、正弦関数の重ね合わせにより表された補償関数を推定する
   ことを特徴とする請求項２記載のトルク補償装置。
4. 前記データ取得部による取得結果に基づいて、正弦関数を表すパラメータの初期値を算出する初期値算出部を備え、
   前記関数推定部は、前記データ取得部による取得結果及び前記初期値算出部により算出された初期値に基づいて、補償関数を推定する
   ことを特徴とする請求項３記載のトルク補償装置。
5. ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得ステップと、
   回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及び前記データ取得ステップにおいて取得した関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出ステップと、
   前記データ取得ステップにおいて取得したトルクと、前記補償値算出ステップにおいて算出したトルク補償値との残差を補償後トルクとして算出する残差算出ステップと、
   前記残差算出ステップにおいて算出した補償後トルクを示すデータを出力する出力ステップと
   を有するトルク補償方法。

Images