JP2004276125A - 負荷パラメータ同定方法 - Google Patents

Abstract

【課題】高精度にパラメータを同定する負荷パラメータ同定方法を得る。
【解決手段】有負荷時にロボットを低速動作させた時（ＳＴ３）の動作データから摩擦力と重力に関するパラメータとを同定し（ＳＴ４）、有負荷時にロボットを高速動作させた時（ＳＴ５）の動作データから摩擦力と低速動作時には同定しなかった残りのパラメータとを同定する（ＳＴ６）ものである。低速動作させた時の動作データから重力に関するパラメータを同定し、高速動作させた時の動作データから低速動作時には同定しなかった残りのパラメータを同定することにより、全てのパラメータを高精度に同定することができる。また、低速動作時および高速動作時において摩擦力を同時に同定するため、摩擦力をキャンセルする必要がなく、同定に使用する動作パターンの制約を少なくすることができる。さらに、パラメータの同定に使用される動作も少なくすることができる。
【選択図】 図１

Description

【０００１】
【発明の属する技術分野】
この発明は、ロボット等のメカニカルシステムの負荷のパラメータを同定する負荷パラメータ同定方法に関するものである。
【０００２】
【従来の技術】
従来の負荷パラメータ同定方法としては、基準姿勢に基づいた動作パターンプログラムを作成しておき、作成した動作パターンプログラムで多関節ロボットを低速かつ一定速度で動作させ、動作中の駆動電流の平均値と軸角度の平均値とを求める。次に駆動電流の平均値から求めたトルクの平均値がアンバランストルク、すなわち重力トルクと等価であるとして、多関節ロボットの負荷重量および負荷重心位置を算出するものがある（下記特許文献１）。
【０００３】
【特許文献１】
特開平１０−１３８１８７号公報（特に、［０００８］から［００１１］）
【０００４】
【発明が解決しようとする課題】
従来の負荷パラメータ同定方法は以上のように構成されているので、重力トルクからでは算出できない負荷自身の慣性モーメントを算出することはできない課題があった。
また、軸角度の平均値に軸がある時の重力と動作中の重力の平均値が一致する動作パターンでなければ、負荷重量および負荷重心位置を正確に算出することができない。そのため、同定用の動作パターンが限定される課題があった。
さらに、動作パターンが限定されるため、実際の動作毎に負荷パラメータを算出することもできない課題があった。
さらに、一般にロボット自体の質量、重心位置、慣性モーメントのパラメータには何らかの誤差があり、ロボット自体の補正を行わないと負荷のパラメータを正確に算出できない課題があった。
さらに、ロボットを駆動するために使用するモータのトルク定数には若干のばらつきがあることがある。したがって個体毎に予めモータのトルク定数を正確に求めておかないと、駆動電流から求めたトルクが真のトルクからずれるため、負荷のパラメータを正確に算出できない課題があった。
【０００５】
この発明は上記のような課題を解決するためになされたもので、高精度にパラメータを同定する負荷パラメータ同定方法を得ることを目的とする。
【０００６】
【課題を解決するための手段】
この発明に係る負荷パラメータ同定方法は、有負荷時にメカニカルシステムを低速動作させた時の動作データから摩擦力と重力に関するパラメータとを同定し、有負荷時にメカニカルシステムを高速動作させた時の動作データから摩擦力と低速動作時には同定しなかった残りのパラメータとを同定するものである。
【０００７】
【発明の実施の形態】
以下、この発明の実施の一形態を説明する。
実施の形態１．
図１はこの発明の実施の形態１による負荷パラメータ同定方法を示すフローチャートであり、同図において、まず、メカニカルシステム、例えばロボットに負荷が取り付けられていない状態で、複数の動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ１）。次にステップＳＴ１で記憶した各軸の位置、速度、電流からロボット本体補正モデルのパラメータと、ステップＳＴ１での動作速度における摩擦力を同定する（ステップＳＴ２）。次に同定したい負荷をロボットに取り付け、ある動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ３）。次にステップＳＴ３で記憶した位置、速度、電流から、負荷の質量、重心位置とステップＳＴ３での動作速度における摩擦力とを同定する（ステップＳＴ４）。次にステップＳＴ３とは異なる動作パターンでロボットを高速動作させ、その時の各軸の位置、速度、加速度、電流を記憶しておく（ステップＳＴ５）。次にステップＳＴ５で記憶した各軸の位置、速度、加速度、電流から負荷の慣性モーメントと摩擦係数とを同定する（ステップＳＴ６）。
【０００８】
次に動作について説明する。
まず、ロボットモデルを、ロボット本体モデル、ロボット本体補正モデル、追加負荷モデルの３モデルの和で表現する。すなわち、各軸の駆動トルクをτ、ロボット本体モデルの駆動トルクをτ_ｒ、ロボット本体補正モデルの駆動トルクをτ_ｈ、追加負荷モデルの駆動トルクをτ_ｌとし、
τ＝τ_ｒ＋τ_ｈ＋τ_ｌ （１）
で表現する。ここで、ロボット本体モデルとは追加負荷がない時のロボットモデルで、追加負荷モデルとは、リンク長はロボット本体と同一で、ロボット本体は質量および慣性モーメントが０であるとし、追加負荷のみ質量および慣性モーメントが存在する仮想的なモデルである。ロボット本体補正モデルは、各軸の重力モーメント補正係数から構成されるモデルである。なお、摩擦トルクはロボット本体モデルに含まれるものとする。すなわち、ロボット本体モデルから摩擦を除いたモデルをτ_ｒｍ、摩擦トルクをτ_ｆとすると、
τ_ｒ＝τ_ｒｍ＋τ_ｆ （２）
なる関係がある。
【０００９】
図２は６軸垂直多関節型ロボットの一例を示す説明図であり、同図において、第１軸１から第６軸６と、負荷７と、第１リンク８および第２リンク９とから構成されている。
図２に示したように、６軸垂直多関節型ロボットの第６軸の先端部に設けられている手先に負荷７が取り付けられている場合、第３軸、第５軸、第６軸の重力から、手先に取り付けた負荷の質量および重心位置を求めることができる。
【００１０】
一方、第ｉ軸の重力モーメント補正係数をｍｇ_ｉとすると、ロボット本体補正モデルの第ｉ軸要素τ_ｈｉは、
τ_ｈｉ＝ｍｇ_ｉ・ｆ_ｉ（θ） （３）
と書ける。ここで、θは各軸の角度から構成されるベクトルで、重力モーメント補正係数ｍｇ_ｉが未知パラメータになる。
【００１１】
手先の負荷の同定が目的の場合、第３軸、第５軸、第６軸の重力モーメント補正係数が分かれば良いので、ステップＳＴ２で第３軸、第５軸、第６軸の重力モーメント補正係数の同定を行う。
まず、第３軸が動作する複数の動作パターンでロボットを低速かつ同一の一定速度で動作させ、一定速度で動作中の各軸の位置、速度、電流を記録する。第３軸での駆動トルクは電流にトルク定数を乗じることにより算出できる。また、一定速度で動作中は加速度は０である。したがって、記録した各軸の位置、速度、電流のデータから第ｋサンプルの第３軸駆動トルクτ_３［ｋ］、ロボット本体から摩擦を除いたモデルの第３軸トルクτ_ｒｍ３［ｋ］、式（３）におけるｆ_ｉ（θ）の第３軸の値ｆ_３［ｋ］が算出できる。
【００１２】
したがって、上記一定速度における第３軸の摩擦力をｆ_ｒ３、パラメータベクトルＰ_０３を、
Ｐ_０３＝［ｍｇ_３ ｆ_ｒ３］^Ｔ （４）
とすれば、

と書ける。ここで、ｖ_３［ｋ］は第ｋサンプルにおける第３軸速度、ｓｇｎ（）は正の時１、負の時−１となる関数であり、Ｔは転置を意味する。記録した複数動作の中で、一定速度で動作中の各サンプル毎のτ_３［ｋ］−τ_ｒｍ３［ｋ］を縦に並べたベクトルをＴ_０３、各サンプル毎の［ｆ_３［ｋ］ ｓｇｎ（ｖ_３［ｋ］）］を縦に並べた行列をＹ_０３とすれば、
Ｔ_０３＝Ｙ_０３・Ｐ_０３ （６）
が得られる。Ｙ_０３の擬似逆行列をＹ_０３ ^＋とすれば、Ｐ_０３の推定値Ｐ_０３ｈは、
Ｐ_０３ｈ＝Ｙ_０３ ^＋・Ｔ_０３ （７）
で求められる。
第５軸、第６軸の重力モーメント補正係数も全く同様にして求められる。
【００１３】
次に、ステップＳＴ３で負荷をロボットに取り付け、第３軸、第５軸、第６軸が同時に動く動作パターンで、ロボットを低速かつ一定速度で動作させる。追加負荷モデルの慣性力をτ_ｌａ、遠心・コリオリ力をτ_ｌｂ、重力をτ_ｌｃとすると、一定速度の時は加速度が０となるため、τ_ｌａ＝０となり、低速の場合はτ_ｌｂが無視できるため、式（１）、式（２）から、
τ＝τ_ｒｍ＋τ_ｆ＋τ_ｈ＋τ_ｌｃ （８）
と書ける。
【００１４】
ロボットの運動方程式は、質量、重心位置等のパラメータからは直接には線形化できないが、（質量）×（重心位置）等を改めてパラメータと見なすと、パラメータに関して線形化できることが知られている。したがって、追加負荷モデルの重力τ_ｌｃも負荷質量、重心位置等の組み合わせから構成されるパラメータＰ_ｌｇを用いて、
τ_ｌｃ＝Ｙ_ｌｇ・Ｐ_ｌｇ （９）
と線形化できる。手先負荷の質量をｍ_ｌ、重心位置をｌ_ｇｘ，ｌ_ｇｙ，ｌ_ｇｚとすると一例は、
Ｐ_ｌｇ＝［ｍ_ｌ ｍ_ｌ・ｌ_ｇｘ ｍ_ｌ・ｌ_ｇｙ ｍ_ｌ・ｌ_ｇｚ］^Ｔ （１０）
である。
【００１５】
Ｙ_ｌｇ，τ，τ_ｒｍ，τ_ｈの第ｉ行をそれぞれＹ_ｌｇｉ，τ_ｉ，τ_ｒｍｉ，τ_ｈｉとし、第ｉ軸の一定速度値における摩擦トルクをｆ_ｒｉとすると、

と書ける。
【００１６】
式（１１）は第３軸、第５軸、第６軸の全てで成り立つため、

とすれば、Ｔ_ｌ＝Ｙ_ｌ・Ｐ_ｌ （１４）
とパラメータＰ_ｌに対して線形化できる。
【００１７】
Ｔ_ｌ，Ｙ_ｌを各サンプル毎に縦に並べた行列をそれぞれＴ_２，Ｙ_２とすれば、
Ｔ_２＝Ｙ_２・Ｐ_ｌ （１５）
となる。したがって、Ｙ_２の擬似逆行列Ｙ_２ ^＋を用いてＰ_ｌの推定値Ｐ_ｌｈは、
Ｐ_ｌｈ＝Ｙ_２ ^＋・Ｔ_２ （１６）
で求められる。Ｐ_ｌｈが求められれば、式（１２）より手先負荷の質量ｍ_ｌ、重心位置ｌ_ｇｘ，ｌ_ｇｙ，ｌ_ｇｚも算出できる。
【００１８】
次に、ステップＳＴ５において高速動作のデータから負荷の慣性モーメントＩ_ｘ，Ｉ_ｙ，Ｉ_ｚを求める。まず、第５軸、第６軸が動作する複数の動作パターンで高速にロボットを動作させ、動作中の各軸の位置、速度、加速度、電流を記録する。手先の負荷の質量ｍ_ｌ、重心位置ｌ_ｇｘ，ｌ_ｇｙ，ｌ_ｇｚは算出済みなので、追加負荷モデルの駆動トルクτ_ｌは、
τ_ｌ＝τ_ｌｍ＋Ｙ_３・Ｐ_３Ｉ （１７）
Ｐ_３Ｉ＝［Ｉ_ｘ Ｉ_ｙ Ｉ_ｚ］^Ｔ （１８）
と表現できる。ここで、τ_ｌｍは負荷の慣性モーメントＩ_ｘ，Ｉ_ｙ，Ｉ_ｚを含まないτ_ｌの成分である。したがって、τ_ｄを、
τ_ｄ＝τ−τ_ｈ−τ_ｌｍ−τ_ｒｍ （１９）
とし、τ_ｄの第ｉ軸要素をτ_ｄｉ、Ｙ_３の第ｉ行ベクトルをＹ_３ｉとすれば、
τ_ｄ５＝Ｙ_３５・Ｐ_３Ｉ＋ｆ_ｃ５・ｓｇｎ（ｖ_５）＋ｆ_ｖ５・ｖ_５ （２０）
τ_ｄ６＝Ｙ_３６・Ｐ_３Ｉ＋ｆ_ｃ６・ｓｇｎ（ｖ_６）＋ｆ_ｖ６・ｖ_６ （２１）
と書ける。ここで、ｆ_ｃ５，ｆ_ｖ５，ｆ_ｃ６，ｆ_ｖ６はそれぞれ第５軸のクーロン摩擦係数、第５軸の粘性摩擦係数、第６軸のクーロン摩擦係数、第６軸の粘性摩擦係数である。したがって、
Ｐ_３＝［Ｉ_ｘ Ｉ_ｙ Ｉ_ｚ ｆ_ｃ５ ｆ_ｖ５ ｆ_ｃ６ ｆ_ｖ６］^Ｔ （２２）
で定義すれば、式（２０）、式（２１）の右辺はＰ_３に関して線形化できる。
【００１９】
以上より、第５軸、第６軸が高速動作した時の動作データから、
Ｔ_４＝Ｙ_４・Ｐ_３ （２３）
が導出できる。ここで、Ｔ_４は各サンプルのτ_ｄ５，τ_ｄ６から構成されるベクトルで、Ｙ_４は各サンプルのＹ_３５，Ｙ_３６，ｓｇｎ（ｖ_５），ｓｇｎ（ｖ_６），ｖ_５，ｖ_６から構成される行列である。Ｙ_４の擬似逆行列をＹ_４ ^＋とすれば、Ｐ_３の推定値Ｐ_３ｈは、
Ｐ_３ｈ＝Ｙ_４ ^＋・Ｔ_４ （２４）
で算出できる。
【００２０】
以上のように、この実施の形態１によれば、重力から同定可能なパラメータは重力に基づいて同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。また、摩擦を同時に同定するため、摩擦をキャンセルする必要がなく、同定に使用する動作パターンの制約が少ない。さらに、ロボットモデルとしてロボット本体補正モデルを設け、そのロボット本体補正モデルのパラメータを無負荷時の動作データから予め算出しておくことにより、ロボット本体の誤差の影響を受けずに高精度に負荷パラメータの同定を行うことができる。
【００２１】
実施の形態２．
この実施の形態２では、ロボット本体補正モデルを求めるために行う無負荷時のロボット動作パターンを、ステップＳＴ３で行う重力に基づく負荷パラメータ同定用の動作パターンと全く同じ動作パターンとするものである。すなわち、低速動作で負荷の質量、重心位置を同定するのに使用するのと同一の動作パターンのみに基づいてロボット本体補正モデルのパラメータを同定する。ステップＳＴ３以降の処理は上記実施の形態１と同一なので説明は省略する。
【００２２】
以上のように、この実施の形態２によれば、同定に使用する動作に特化したロボット本体モデルの補正が行えるため、ロボット本体のモデル化誤差の影響をさらに低減することができる。
【００２３】
実施の形態３．
この実施の形態３では、負荷の慣性モーメントを求めるためにステップＳＴ５で行う高速動作の動作パターンを、ステップＳＴ３の低速動作の動作パターンと全く同一のものとする。

で定義すれば、第３軸、第５軸、第６軸の動作データから、
Ｔ_５＝Ｙ_５・Ｐ_５ （２６）
が上記実施の形態１と同様にして導出できる。Ｐ_５の推定値Ｐ_５ｈはＹ_５の擬似逆行列Ｙ_５ ^＋を用いて、
Ｐ_５ｈ＝Ｙ_５ ^＋・Ｔ_５ （２７）
で算出できる。なお、ステップＳＴ４までの処理は上記実施の形態１と全く同一である。
【００２４】
以上のように、この実施の形態３によれば、負荷の同定に使用する動作パターンが１種類で良いため、動作パターンの準備に要する時間を短縮することができる。
【００２５】
実施の形態４．
この実施の形態４では、上記実施の形態１のステップＳＴ２、ステップＳＴ４、ステップＳＴ６でのパラメータの算出方法において若干異なるものである。上記実施の形態１では各サンプル毎の各軸の電流、位置、速度、加速度をそのまま用いていたが、この実施の形態４では平滑化フィルタに通してから使用する。
【００２６】
まず、ステップＳＴ２でロボット本体補正モデルのパラメータを算出する際、式（５）のτ_３［ｋ］−τ_ｒｍ３［ｋ］，ｆ_３［ｋ］，ｓｇｎ（ｖ_３［ｋ］）の各要素と、ｖ_３［ｋ］とを平滑化フィルタに通したものをそれぞれｆａ１［ｋ］，ｆａ２［ｋ］，ｆａ３［ｋ］，ｆａ４［ｋ］とし、ｆａ４［ｋ］が等速になっている区間のｆａ１［ｋ］，ｆａ２［ｋ］，ｆａ３［ｋ］を用いて同定する。すなわち、ｆａ４［ｋ］が等速になっている区間のｆａ１を縦に並べたベクトルをＴ_０ｆ３、［ｆａ２［ｋ］ ｆａ３［ｋ］］を縦に並べた行列をＹ_０ｆ３、Ｙ_０ｆ３の擬似逆行列をＹ_０ｆ３ ^＋とし、
Ｐ_０３ｈ＝Ｙ_０ｆ３ ^＋・Ｔ_０ｆ３ （２８）
でＰ_０３の推定値Ｐ_０３ｈを求める。平滑化フィルタとしては移動平均フィルタを用いる。第５軸、第６軸のロボット本体補正モデルのパラメータも同様に求める。
【００２７】
ステップＳＴ４で負荷の質量、重心位置を算出する際も同様である。まず、式（１４）のＴ_ｌ，Ｙ_ｌの各要素と、第３軸の速度ｖ_３を平滑化フィルタに通す。ｖ_３の平滑化フィルタに通した時の出力が等速になっている区間のＴ_ｌ，Ｙ_ｌの各要素のフィルタに通した時の出力を使用して同定する。すなわち、Ｔ_ｌ，Ｙ_ｌの各要素のフィルタに通した時の出力を縦に並べた行列をそれぞれＴ_２ｆ，Ｙ_２ｆとし、Ｙ_２ｆの擬似逆行列Ｙ_２ｆ＋を用いて、
Ｐ_ｌｈ＝Ｙ_２ｆ ^＋・Ｔ_２ｆ （２９）
より、Ｐ_ｌの推定値Ｐ_ｌｈを算出する。
【００２８】
ステップＳＴ６で残りのパラメータを求める時もほぼ同様である。式（２０）、式（２１）のτ_ｄ５，τ_ｄ６，Ｙ_３５，Ｙ_３６，ｓｇｎ（ｖ_５），ｓｇｎ（ｖ_６），ｖ_５，ｖ_６をそれぞれ平滑化フィルタに入力し、Ｔ_４ｆを各サンプルのτ_ｄ５，τ_ｄ６のフィルタ出力から構成されるベクトル、Ｙ_４ｆを各サンプルのＹ_３５，Ｙ_３６，ｓｇｎ（ｖ_５），ｓｇｎ（ｖ_６），ｖ_５，ｖ_６のフィルタ出力から構成される行列とし、Ｐ_３の推定値Ｐ_３ｈを、
Ｐ_３ｈ＝Ｙ_４ｆ ^＋・Ｔ_４ｆ （３０）
で算出する。ここで、Ｙ_４ｆ ^＋はＹ_４ｆの擬似逆行列である。
【００２９】
以上のように、この実施の形態４によれば、動作データを平滑化フィルタに通してから使用したことにより、さらに、負荷パラメータを高精度に同定することができる。
【００３０】
実施の形態５．
この実施の形態５では、上記実施の形態１のステップＳＴ２、ステップＳＴ４、ステップＳＴ６でのパラメータの算出方法において異なるものである。上記実施の形態１では同定パラメータに関する線形化式を導出した後、擬似逆行列を用いてパラメータを算出しているが、この実施の形態５では擬似逆行列を用いず、記憶したデータに対して適応同定理論を繰り返し適用することによりパラメータを算出するものである。
【００３１】
まず、ステップＳＴ２でロボット本体補正モデルのパラメータを算出する際、式（５）の（τ_３［ｋ］−τ_ｒｍ３［ｋ］），［ｆ_３［ｋ］ ｓｇｎ（ｖ_３［ｋ］）］をそれぞれＴ_ａ，Ｙ_ａとする。この時、第ｋサンプル目の未知パラメータＰ_ａの推定値Ｐ_ａ［ｋ］を、

で算出する。ここで、Δｔ，σは定数、Ｇは定数から構成される行列で、行列Ｒおよびベクトルｑの初期値は各要素が０であるとする。また、同定パラメータＰ_ａの初期値は予め定めている値とする。
【００３２】
ステップＳＴ２の第３軸のロボット本体補正モデルでは未知パラメータＰ_ａは式（４）のＰ_０３となる。式（３１）〜式（３３）の演算をまず、記憶したデータに対して、サンプル数ｋが記憶したデータ個数ｍ_ｌに達するまで行う。次に第ｍ_ｌ番目のＰ_ａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ_ｌに達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎ_ｌ回繰り返す。最後のｎ_ｌ回目の繰り返しにおけるＰ_ａ［ｋ］の平均値を求めるパラメータ値とする。第５軸、第６軸のロボット本体補正モデルのパラメータも全く同様に算出する。
【００３３】
ステップＳＴ４で負荷および重心位置の算出を行う際には、式（１３）のＴ_ｌをＴ_ａ、式（１４）のＹ_ｌをＹ_ａとし、ステップＳＴ３で記憶したデータを用いて式（３１）〜式（３３）の計算を、サンプル数ｋが記憶したデータ個数ｍ_２に達するまで行う。次に第ｍ_２番目のＰ_ａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ_２に達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎ_２回繰り返す。最後のｎ_２回目の繰り返しにおけるＰ_ａ［ｋ］の平均値を求めるパラメータ値とする。
【００３４】
ステップＳＴ６で残りのパラメータの同定を行う際には、式（２０）〜式（２２）から、
Ｔ_３＝［τ_ｄ５ τ_ｄ６］ （３４）
Ｔ_３＝Ｙ_３ｂ・Ｐ_３ （３５）
を導出する。Ｔ_ａ＝Ｔ_３、Ｙ_ａ＝Ｙ_３ｂとし、ステップＳＴ５で記憶したデータを用いて式（３１）〜式（３３）の計算を、サンプル数ｋが記憶したデータ個数ｍ_３に達するまで行う。次に第ｍ_３番目のＰ_ａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ_３に達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎ_３回繰り返す。最後のｎ_３回目の繰り返しにおけるＰ_ａ［ｋ］の平均値を求めるパラメータ値とする。
【００３５】
以上のように、この実施の形態５によれば、擬似逆行列を用いないので、同定に使用する動作パターンが、擬似逆行列をとる行列がフルランクになる動作に限る必要をなくすことができる。
【００３６】
実施の形態６．
この実施の形態６では、上記実施の形態１のステップＳＴ４、ステップＳＴ６でのパラメータの算出方法とステップＳＴ３、ステップＳＴ５で行う動作のパターン数において異なるものである。まず、ステップＳＴ３では複数の第３軸、第５軸、第６軸が動く動作パターンでロボットを動作させる。ステップＳＴ４ではステップＳＴ３で行ったそれぞれの動作に対して式（８）〜式（１６）の演算を行い、それぞれの動作パターンに対するパラメータ同定値Ｐ_ｌｈｌ，・・・，Ｐ_ｌｈｎを算出する。
【００３７】
次に、Ｐ_ｌｈｌから手先負荷の質量初期値ｍ_ｌ０、重心位置初期値ｌ_ｇｘ０，ｌ_ｇｙ０，ｌ_ｇｚ０をまず算出する。

【数１】

Ｂ＝［Ｂ_ｌ ^Ｔ Ｂ_２ ^Ｔ ・・・ Ｂ_ｎ ^Ｔ］^Ｔ （３９）
ΔＰ_ｌｈ＝［ΔＰ_ｌｈｌ ^Ｔ ΔＰ_ｌｈ２ ^Ｔ ・・・ ΔＰ_ｌｈｎ ^Ｔ］^Ｔ （４０）
ΔＰ_ｇ＝Ｂ^＋・ΔＰ_ｌｈ （４１）
を計算し、式（４１）で算出される第１要素〜第４要素をそれぞれ負荷の質量推定値ｍ_ｌ０、重心位置推定値ｌ_ｇｘ０，ｌ_ｇｙ０，ｌ_ｇｚ０に加算したものを改めて手先負荷の質量推定値をｍ_ｌ０、重心位置推定値ｌ_ｇｘ０，ｌ_ｇｙ０，ｌ_ｇｚ０とする。式（３６）〜式（４１）でΔＰ_ｇを求め、負荷および重心位置の推定値を修正することを規定回数繰り返す。規定回数終了後の推定値を求めるパラメータとする。
【００３８】
ステップＳＴ５では第５軸、第６軸が動く動作を、同定に必要なパターンのｎ_２倍の動作パターンで動作させる。ｎ_２個の動作パターンの組み合わせ毎に式（２４）でパラメータを算出し、それぞれの平均値を求めるパラメータとする。
【００３９】
以上のように、この実施の形態６によれば、複数の同定結果から平均を出すことによって、あらゆる動作パターンに基づいて負荷パラメータを高精度に同定することができる。
【００４０】
実施の形態７．
図３はこの発明の実施の形態７による負荷パラメータ同定方法を示すフローチャートであり、同図において、実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する（ステップＳＴ１１）。次にステップＳＴ１１で記憶した位置、速度、加速度、電流を用いて、その動作に特化した負荷パラメータ値の同定を行う（ステップＳＴ１２）。この実施の形態７では、実作業で動作させる各々の動作に対して、ロボットに取り付けた負荷が変わるかどうかに関わらず、ステップＳＴ１１、ステップＳＴ１２の同定作業を実施するものである。
【００４１】
次に動作について説明する。
ステップＳＴ１２での同定方法について説明する。式（１）の追加負荷モデルの駆動トルクτ_ｌは、追加負荷の質量、重心位置、慣性モーメントの演算結果から構成されるパラメータベクトルＰ_７に関して、
τ_ｌ＝Ｙ_７・Ｐ_７ （４２）
と線形化できる。また、式（１）、式（２）より、
τ_ｌ＋τ_ｆ＝τ−τ_ｒｍ−τ_ｈ （４３）
が得られる。したがって、パラメータベクトルＰ_７を各軸のクーロン摩擦係数、粘性摩擦係数を含む新しいパラメータベクトルＰ_７ｂに拡張し、式（４３）の左辺をτ_７ｂとおけば、
τ_７ｂ＝Ｙ_７ｂ・Ｐ_７ｂ （４４）
と線形化できる。
【００４２】
このパラメータベクトルの各要素を上記実施の形態５と同様に、適応同定理論を繰り返し適用することによりパラメータを算出する。具体的には、第ｋサンプル目の未知パラメータＰ_７ｂの推定値Ｐ_７ｂｈ［ｋ］を、

で算出する。ここで、Δｔ，σは定数、Ｇは定数から構成される行列で、行列Ｒおよびベクトルｑの初期値は各要素が０であるとする。また、同定パラメータＰ_７ｂｈの初期値は予め定めている値とする。式（４５）〜式（４７）の演算をまず、記憶したデータに対して、サンプル数ｋが記憶したデータ個数ｍ_ｌに達するまで行う。次に第ｍ_ｌ番目のＰ_７ｂｈ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ_ｌに達するまで式（４５）〜式（４７）の演算を行う。この演算を予め決められた回数ｎ_ｌ回繰り返す。最後のｎ_ｌ回目の繰り返しにおけるＰ_７ｂｈ［ｋ］の平均値を求めるパラメータ値とする。
【００４３】
以上のように、この実施の形態７によれば、擬似逆行列を用いないので、同定に使用する動作パターンが、擬似逆行列をとる行列がフルランクになる動作に限る必要をなくすことができる。また、動作毎に特化したパラメータを同定するため、その動作のトルク推定精度がさらに向上し、最適な加減速時間の算出を行うことができる。
【００４４】
実施の形態８．
この実施の形態８による負荷パラメータ同定方法も図３のフローチャートで示されるものである。ステップＳＴ１１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ１２で、ステップＳＴ１１で記憶した位置、速度、加速度、電流を用いて、その動作に特化した負荷パラメータ値の同定を行う。この実施の形態８では、上記実施の形態７とは異なり、負荷７の質量のみを未知パラメータとして同定する。質量以外のパラメータと摩擦係数は既知とし、初期値、もしくは、予め同定した値を使用する。式（１）の右辺は、手先負荷質量を含む成分をτ_ｌａ、負荷を含まない成分をτ_ｌｂとすると、
τ＝τ_ｌａ＋τ_ｌｂ （４８）
と変形できる。τ−τ_ｌｂをτ_８とすると、手先負荷質量を含む成分をτ_ｌａは手先負荷質量Ｐ_８に関して線形化できるため、
τ_８＝Ｙ_８・Ｐ_８ （４９）
と書ける。したがって、ステップＳＴ１で記憶した各サンプル毎のτ_８，Ｙ_８を縦に並べた行列をそれぞれＴ_８，Ｙ_８ｂとすれば、Ｙ_８ｂの擬似逆行列Ｙ_８ｂ ^＋を用いて、
Ｐ_８＝Ｙ_８ｂ ^＋・Ｔ_８ （５０）
でその動作の手先負荷質量Ｐ_８が推定できる。
【００４５】
以上のように、この実施の形態８によれば、擬似逆行列をとる行列がほぼ全ての場合でフルランクになるため、同定に使用する動作パターンがほとんど制限されることはない。また、動作毎に特化したパラメータを同定するため、その動作のトルク推定精度がさらに向上し、最適な加減速時間の算出を行うことができる。
【００４６】
実施の形態９．
図４はこの発明の実施の形態９による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ２３〜ＳＴ２８はそれぞれ図１のステップＳＴ１〜ＳＴ６に対応するので、説明は省略する。ステップＳＴ２１では、ロボットの手先に何も取り付けず（無負荷状態）にある決まった動作をさせ、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。次に質量、重心位置、慣性モーメントが既知の負荷を手先に取り付け、ステップＳＴ２１で無負荷状態にて行った動作と同じ動作を行い、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。
ステップＳＴ２２では、各軸のトルク定数が公称値であるとして、各軸の電流から各軸のトルクを算出し、無負荷時の動作データおよび既知の負荷を取り付けた時の動作データから、既知負荷のパラメータを同定する。同定した既知負荷のパラメータと、既知負荷パラメータの真値の比に応じて、トルク定数を修正する。ステップＳＴ２３以降では、修正したトルク定数を用いて、未知負荷のパラメータを同定する。
【００４７】
以上のように、この実施の形態９によれば、トルク定数が個体毎にばらつく場合でも、高精度に追加負荷のパラメータ同定を行うことができる。
【００４８】
実施の形態１０．
この実施の形態１０による負荷パラメータ同定方法も図４のフローチャートで示されるものである。図４のステップＳＴ２３〜ステップＳＴ２８はそれぞれ図１のステップＳＴ１〜ステップＳＴ６に対応するので、説明は省略する。
まず、予め機種毎に、ある動作を行った時のロボット本体補正モデルのパラメータ値を算出しておく。次に、ステップＳＴ２１で、ロボット本体補正モデルのパラメータの同定に使用されたのと全く同じ動作を行い、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。
ステップＳＴ２２では、各軸のトルク定数が公称値であるとして、各軸の電流から各軸のトルクを算出し、ロボット本体補正モデルのパラメータを同定する。同定したロボット本体補正モデルのパラメータ値と予め記憶してある機種毎のロボット本体補正モデルのパラメータ値の比に応じて、トルク定数を修正する。ステップＳＴ２３以降では、修正したトルク定数を用いて、未知負荷のパラメータを同定する。
【００４９】
以上のように、この実施の形態１０によれば、トルク定数が個体毎にばらつく場合でも、高精度に追加負荷のパラメータ同定を行うことができる。
【００５０】
実施の形態１１．
図５はこの発明の実施の形態１１による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ３１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ３２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。ステップＳＴ３３では加速区間、減速区間のそれぞれに対して式（４４）の線形化を行い、式（４５）〜式（４７）の計算を加速区間、減速区間のそれぞれに対して繰り返すことにより、加速区間のパラメータと減速区間のパラメータを別個に算出する。
【００５１】
以上のように、この実施の形態１１によれば、動作毎の加速区間と、減速区間とのそれぞれに特化したパラメータを同定するため、その動作の加速区間および減速区間におけるトルク推定精度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００５２】
実施の形態１２．
図６はこの発明の実施の形態１２による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ４１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ４２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。
【００５３】
ステップＳＴ４３では、まず、加速区間の電流から加速区間のトルクを求め、加速区間の各軸のトルク最大値を算出する。各軸の加速区間トルク最大値をその軸のトルク許容値で除した値の絶対値が最も大きい軸ｉｋを求める。同様にして、各軸の減速区間トルク最大値をその軸のトルク許容値で除した値の絶対値が最も大きい軸ｉｇを求める。
【００５４】
ステップＳＴ４４では、加速区間は軸ｉｋのデータに基づいてパラメータの同定を行い、減速区間は軸ｉｇのデータに基づいてパラメータの同定を実施する。
まず、加速区間の式（４４）から軸ｉｋに関する部分のみ取り出せば、
τ_７ｂｉｋ＝Ｙ_７ｂｉｋ・Ｐ_７ｂ （５１）
が得られる。そこで、Ｐ_７ｂを上記実施の形態１１と同様に、

を繰り返し計算することにより算出する。
【００５５】
減速区間も同様に、減速区間の式（４４）から軸ｉｇに関する部分のみ取り出せば、
τ_７ｂｉｇ＝Ｙ_７ｂｉｇ・Ｐ_７ｂ （５５）
が得られる。そこで、Ｐ_７ｂを上記実施の形態１１と同様に、

を繰り返し計算することにより算出する。
【００５６】
以上のように、この実施の形態１２によれば、動作毎の加速区間と、減速区間とのそれぞれにおいて、加減速時間を制約するであろう軸に特化したパラメータを同定するため、その動作の加速区間および減速区間における加減速時間を制約するであろう軸のトルク推定精度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００５７】
実施の形態１３．
この実施の形態１３による負荷パラメータ同定方法も図１のフローチャートで示されるものである。この実施の形態１３では、図２に示した６軸垂直多関節ロボットの第２リンク９に取り付けた負荷のパラメータを同定する。
図７は垂直２自由度ロボットの一例を示す説明図であり、図において、第１軸１０および第２軸１１と、第１リンク１２および第２リンク１３と、負荷１４ととから構成されている。上記実施の形態１と異なるのは、追加負荷モデルとして図７に示す垂直２自由度ロボットモデルを用いことであり、図７の垂直２自由度ロボットは第１リンク１２の質量は０で、第２リンク１３は負荷１４の質量、重心位置、慣性モーメントで構成される。第１リンク１２の長さは本体ロボットの第１リンクの長さと同一である。
同定した追加負荷パラメータは、図１の第２リンク９の要素の一部と見なしてロボット本体モデルの第２リンク１３のパラメータを修正してから、加減速時間の算出、トルクフィードフォワードの計算等に使用する。
【００５８】
以上のように、この実施の形態１３によれば、ロボットの手先以外に取り付けられた負荷１４のパラメータも簡単な計算により同定することができる。
【００５９】
実施の形態１４．
図８はこの発明の実施の形態１４による負荷パラメータ同定方法を示すフローチャートであり、図９はエレベータドアの一例を示す説明図である。図９において、エレベータドア駆動用モータ１５の回転は、Ｖベルト１７を通じてプーリ１６を回転させ、さらに、そのプーリ１６を回転は、タイミングベルト１８を通じてカゴドア１９を動作させる。乗り場ドア（図示せず）は、その乗り場ドアに設けられた装置（図示せず）でカゴドア１９と結合され、連動して動作する。なお、戸締め力発生リンク２０は、カゴドア１９に連動して、その重力に応じた戸締め力をカゴドア１９に与えるものである。
【００６０】
次に動作について説明する。
図８および図９において、まず、乗り場ドアがドアカゴ１９と結合された状態で、エレベータドア駆動用モータ１５を低速かつ一定速度で動作させ、その時のエレベータドア駆動用モータ１５の回転角、速度、電流を記憶しておく（ステップＳＴ５１）。次にステップＳＴ５１で記憶したエレベータドア駆動用モータ１５の回転角、速度、電流から戸締め力発生リンク２０の質量、重心位置と、ステップＳＴ５１での動作速度における摩擦力とを同定する（ステップＳＴ５２）。次に乗り場ドアがドアカゴ１９と結合された状態で、エレベータドア駆動用モータ１５を高速動作させ、その時のエレベータドア駆動用モータ１５の回転角、速度、加速度、電流を記憶しておく（ステップＳＴ５３）。次にステップＳＴ５３で記憶したエレベータドア駆動用モータ１５の回転角、速度、加速度、電流からドアカゴ１９と乗り場ドアを合わせた質量と、摩擦係数とを同定する（ステップＳＴ５４）。
【００６１】
以上のように、この実施の形態１４によれば、重力から同定可能な戸締め力発生リンク２０のパラメータは重力から同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。
【００６２】
実施の形態１５．
この実施の形態１５による負荷パラメータ同定方法も図５のフローチャートで示されるものである。まず、実装機（図示しない）の先端に実際の作業の一つで実装される電子部品を取り付ける。次にステップＳＴ３１で実際の作業と同じ速度で、実際に行う動作パターンの一つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ３２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。ステップＳＴ３３では、加速区間、減速区間のそれぞれに対して、実装機の先端に取り付けた電子部品の質量、重心位置、慣性モーメントの演算結果と摩擦係数から構成されるベクトルＰに関して、式（４４）と同様な線形化を実施し、式（４５）〜式（４７）と同様な計算を加速区間、減速区間のそれぞれに対して繰り返すことにより、加速区間のパラメータと減速区間のパラメータとを別個に計算する。
【００６３】
以上のように、この実施の形態１５によれば、動作毎の加速区間と減速区間とのそれぞれに特化したパラメータを同定するため、その動作の加速区間および減速区間におけるトルク推定速度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００６４】
実施の形態１６．
図１０はこの発明の実施の形態１６による負荷パラメータ同定方法を示すフローチャートであり、同図において、まず、工作機械（図示しない）の先端に負荷が取り付けられていない状態で、複数の動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ６１）。次にステップＳＴ６１で記憶した各軸の位置、速度、電流から工作機械本体補正モデルのパラメータと、ステップＳＴ６１での動作速度における摩擦力を同定する（ステップＳＴ６２）。次に同定したい切削工具等の負荷を工作機械に取り付け、ある動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ６３）。次にステップＳＴ６３で記憶した各軸の位置、速度、電流から、負荷の質量、重心位置と、ステップＳＴ６３での動作速度における摩擦力とを同定する（ステップＳＴ６４）。次にステップＳＴ６３とは異なる動作パターンで工作機械を高速動作させ、その時の各軸の位置、速度、加速度、電流を記憶しておく（ステップＳＴ６５）。次にステップＳＴ６５で記憶した各軸の位置、速度、加速度、電流から負荷の慣性モーメントと摩擦係数とを同定する（ステップＳＴ６６）。
【００６５】
以上のように、この実施の形態１６によれば、重力から同定可能なパラメータは重力から同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。
【００６６】
【発明の効果】
以上のように、この発明によれば、低速動作させた時の動作データから重力に関するパラメータを同定し、高速動作させた時の動作データから低速動作時には同定しなかった残りのパラメータを同定することにより、全てのパラメータを高精度に同定することができる。また、低速動作時および高速動作時において摩擦力を同時に同定するため、摩擦力をキャンセルする必要がなく、同定に使用する動作パターンの制約を少なくすることができる。さらに、パラメータの同定に使用される動作も少なくすることができる効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１による負荷パラメータ同定方法を示すフローチャートである。
【図２】６軸垂直多関節型ロボットの一例を示す説明図である。
【図３】この発明の実施の形態７による負荷パラメータ同定方法を示すフローチャートである。
【図４】この発明の実施の形態９による負荷パラメータ同定方法を示すフローチャートである。
【図５】この発明の実施の形態１１による負荷パラメータ同定方法を示すフローチャートである。
【図６】この発明の実施の形態１２による負荷パラメータ同定方法を示すフローチャートである。
【図７】垂直２自由度ロボットの一例を示す説明図である。
【図８】この発明の実施の形態１４による負荷パラメータ同定方法を示すフローチャートである。
【図９】エレベータドアの一例を示す説明図である。
【図１０】この発明の実施の形態１６による負荷パラメータ同定方法を示すフローチャートである。
【符号の説明】
１，１０ 第１軸、２，１１ 第２軸、３ 第３軸、４ 第４軸、５ 第５軸、６ 第６軸、７，１４ 負荷、８，１２ 第１リンク、９，１３ 第２リンク、１５ エレベータドア駆動用モータ、１６ プーリ、１７ Ｖベルト、１８ タイミングベルト、１９ カゴドア、２０ 戸締め力発生リンク。

Claims (7)

1. 有負荷時にメカニカルシステムを低速動作させた時の動作データから摩擦力と重力に関するパラメータとを同定し、有負荷時に上記メカニカルシステムを高速動作させた時の動作データから摩擦力と上記低速動作時には同定しなかった残りのパラメータとを同定することを特徴とする負荷パラメータ同定方法。
2. メカニカルシステムモデルを、メカニカルシステム本体モデル、メカニカルシステム本体補正モデル、追加負荷モデルの３つのモデルの和で表現し、そのメカニカルシステム本体補正モデルのパラメータを無負荷時に上記メカニカルシステムを低速動作させた時の動作データから予め求めておくことを特徴とする負荷パラメータ同定方法。
3. 実際に行う動作パターンに基づいてメカニカルシステムを動作させた時の動作データから逐次的にパラメータの修正を行うことにより、パラメータの同定を行うことを特徴とする負荷パラメータ同定方法。
4. 実際に行う動作パターンに基づいてメカニカルシステムを動作させた時の動作データから特定のパラメータのみ同定を行うことを特徴とする負荷パラメータ同定方法。
5. メカニカルシステムの動作の加速区間と減速区間とで独立にパラメータの同定を行うことを特徴とする負荷パラメータ同定方法。
6. パラメータの同定に使用されるメカニカルシステムの軸のトルク定数を予め同定しておくことを特徴とする負荷パラメータ同定方法。
7. 複数の軸から構成されるメカニカルシステムの隣接する２つの軸の間に取り付けられた負荷のパラメータの同定を行うことを特徴とする負荷パラメータ同定方法。

Images