(57)【要約】【課題】高精度にパラメータを同定する負荷パラメータ同定方法を得る。【解決手段】有負荷時にロボットを低速動作させた時（ＳＴ３）の動作データから摩擦力と重力に関するパラメータとを同定し（ＳＴ４）、有負荷時にロボットを高速動作させた時（ＳＴ５）の動作データから摩擦力と低速動作時には同定しなかった残りのパラメータとを同定する（ＳＴ６）ものである。低速動作させた時の動作データから重力に関するパラメータを同定し、高速動作させた時の動作データから低速動作時には同定しなかった残りのパラメータを同定することにより、全てのパラメータを高精度に同定することができる。また、低速動作時および高速動作時において摩擦力を同時に同定するため、摩擦力をキャンセルする必要がなく、同定に使用する動作パターンの制約を少なくすることができる。さらに、パラメータの同定に使用される動作も少なくすることができる。【選択図】        図１
2004276125.tif 000002
【特許請求の範囲】
【請求項１】
有負荷時にメカニカルシステムを低速動作させた時の動作データから摩擦力と重力に関するパラメータとを同定し、有負荷時に上記メカニカルシステムを高速動作させた時の動作データから摩擦力と上記低速動作時には同定しなかった残りのパラメータとを同定することを特徴とする負荷パラメータ同定方法。
【請求項２】
メカニカルシステムモデルを、メカニカルシステム本体モデル、メカニカルシステム本体補正モデル、追加負荷モデルの３つのモデルの和で表現し、そのメカニカルシステム本体補正モデルのパラメータを無負荷時に上記メカニカルシステムを低速動作させた時の動作データから予め求めておくことを特徴とする負荷パラメータ同定方法。
【請求項３】
実際に行う動作パターンに基づいてメカニカルシステムを動作させた時の動作データから逐次的にパラメータの修正を行うことにより、パラメータの同定を行うことを特徴とする負荷パラメータ同定方法。
【請求項４】
実際に行う動作パターンに基づいてメカニカルシステムを動作させた時の動作データから特定のパラメータのみ同定を行うことを特徴とする負荷パラメータ同定方法。
【請求項５】
メカニカルシステムの動作の加速区間と減速区間とで独立にパラメータの同定を行うことを特徴とする負荷パラメータ同定方法。
【請求項６】
パラメータの同定に使用されるメカニカルシステムの軸のトルク定数を予め同定しておくことを特徴とする負荷パラメータ同定方法。
【請求項７】
複数の軸から構成されるメカニカルシステムの隣接する２つの軸の間に取り付けられた負荷のパラメータの同定を行うことを特徴とする負荷パラメータ同定方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、ロボット等のメカニカルシステムの負荷のパラメータを同定する負荷パラメータ同定方法に関するものである。
【０００２】
【従来の技術】
従来の負荷パラメータ同定方法としては、基準姿勢に基づいた動作パターンプログラムを作成しておき、作成した動作パターンプログラムで多関節ロボットを低速かつ一定速度で動作させ、動作中の駆動電流の平均値と軸角度の平均値とを求める。次に駆動電流の平均値から求めたトルクの平均値がアンバランストルク、すなわち重力トルクと等価であるとして、多関節ロボットの負荷重量および負荷重心位置を算出するものがある（下記特許文献１）。
【０００３】
【特許文献１】
特開平１０−１３８１８７号公報（特に、［０００８］から［００１１］）
【０００４】
【発明が解決しようとする課題】
従来の負荷パラメータ同定方法は以上のように構成されているので、重力トルクからでは算出できない負荷自身の慣性モーメントを算出することはできない課題があった。
また、軸角度の平均値に軸がある時の重力と動作中の重力の平均値が一致する動作パターンでなければ、負荷重量および負荷重心位置を正確に算出することができない。そのため、同定用の動作パターンが限定される課題があった。
さらに、動作パターンが限定されるため、実際の動作毎に負荷パラメータを算出することもできない課題があった。
さらに、一般にロボット自体の質量、重心位置、慣性モーメントのパラメータには何らかの誤差があり、ロボット自体の補正を行わないと負荷のパラメータを正確に算出できない課題があった。
さらに、ロボットを駆動するために使用するモータのトルク定数には若干のばらつきがあることがある。したがって個体毎に予めモータのトルク定数を正確に求めておかないと、駆動電流から求めたトルクが真のトルクからずれるため、負荷のパラメータを正確に算出できない課題があった。
【０００５】
この発明は上記のような課題を解決するためになされたもので、高精度にパラメータを同定する負荷パラメータ同定方法を得ることを目的とする。
【０００６】
【課題を解決するための手段】
この発明に係る負荷パラメータ同定方法は、有負荷時にメカニカルシステムを低速動作させた時の動作データから摩擦力と重力に関するパラメータとを同定し、有負荷時にメカニカルシステムを高速動作させた時の動作データから摩擦力と低速動作時には同定しなかった残りのパラメータとを同定するものである。
【０００７】
【発明の実施の形態】
以下、この発明の実施の一形態を説明する。
実施の形態１．
図１はこの発明の実施の形態１による負荷パラメータ同定方法を示すフローチャートであり、同図において、まず、メカニカルシステム、例えばロボットに負荷が取り付けられていない状態で、複数の動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ１）。次にステップＳＴ１で記憶した各軸の位置、速度、電流からロボット本体補正モデルのパラメータと、ステップＳＴ１での動作速度における摩擦力を同定する（ステップＳＴ２）。次に同定したい負荷をロボットに取り付け、ある動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ３）。次にステップＳＴ３で記憶した位置、速度、電流から、負荷の質量、重心位置とステップＳＴ３での動作速度における摩擦力とを同定する（ステップＳＴ４）。次にステップＳＴ３とは異なる動作パターンでロボットを高速動作させ、その時の各軸の位置、速度、加速度、電流を記憶しておく（ステップＳＴ５）。次にステップＳＴ５で記憶した各軸の位置、速度、加速度、電流から負荷の慣性モーメントと摩擦係数とを同定する（ステップＳＴ６）。
【０００８】
次に動作について説明する。
まず、ロボットモデルを、ロボット本体モデル、ロボット本体補正モデル、追加負荷モデルの３モデルの和で表現する。すなわち、各軸の駆動トルクをτ、ロボット本体モデルの駆動トルクをτｒ、ロボット本体補正モデルの駆動トルクをτｈ、追加負荷モデルの駆動トルクをτｌとし、
τ＝τｒ＋τｈ＋τｌ                                （１）
で表現する。ここで、ロボット本体モデルとは追加負荷がない時のロボットモデルで、追加負荷モデルとは、リンク長はロボット本体と同一で、ロボット本体は質量および慣性モーメントが０であるとし、追加負荷のみ質量および慣性モーメントが存在する仮想的なモデルである。ロボット本体補正モデルは、各軸の重力モーメント補正係数から構成されるモデルである。なお、摩擦トルクはロボット本体モデルに含まれるものとする。すなわち、ロボット本体モデルから摩擦を除いたモデルをτｒｍ、摩擦トルクをτｆとすると、
τｒ＝τｒｍ＋τｆ                                  （２）
なる関係がある。
【０００９】
図２は６軸垂直多関節型ロボットの一例を示す説明図であり、同図において、第１軸１から第６軸６と、負荷７と、第１リンク８および第２リンク９とから構成されている。
図２に示したように、６軸垂直多関節型ロボットの第６軸の先端部に設けられている手先に負荷７が取り付けられている場合、第３軸、第５軸、第６軸の重力から、手先に取り付けた負荷の質量および重心位置を求めることができる。
【００１０】
一方、第ｉ軸の重力モーメント補正係数をｍｇｉとすると、ロボット本体補正モデルの第ｉ軸要素τｈｉは、
τｈｉ＝ｍｇｉ・ｆｉ（θ）                          （３）
と書ける。ここで、θは各軸の角度から構成されるベクトルで、重力モーメント補正係数ｍｇｉが未知パラメータになる。
【００１１】
手先の負荷の同定が目的の場合、第３軸、第５軸、第６軸の重力モーメント補正係数が分かれば良いので、ステップＳＴ２で第３軸、第５軸、第６軸の重力モーメント補正係数の同定を行う。
まず、第３軸が動作する複数の動作パターンでロボットを低速かつ同一の一定速度で動作させ、一定速度で動作中の各軸の位置、速度、電流を記録する。第３軸での駆動トルクは電流にトルク定数を乗じることにより算出できる。また、一定速度で動作中は加速度は０である。したがって、記録した各軸の位置、速度、電流のデータから第ｋサンプルの第３軸駆動トルクτ３［ｋ］、ロボット本体から摩擦を除いたモデルの第３軸トルクτｒｍ３［ｋ］、式（３）におけるｆｉ（θ）の第３軸の値ｆ３［ｋ］が算出できる。
【００１２】
したがって、上記一定速度における第３軸の摩擦力をｆｒ３、パラメータベクトルＰ０３を、
Ｐ０３＝［ｍｇ３  ｆｒ３］Ｔ                        （４）
とすれば、
2004276125.tif 000003
と書ける。ここで、ｖ３［ｋ］は第ｋサンプルにおける第３軸速度、ｓｇｎ（）は正の時１、負の時−１となる関数であり、Ｔは転置を意味する。記録した複数動作の中で、一定速度で動作中の各サンプル毎のτ３［ｋ］−τｒｍ３［ｋ］を縦に並べたベクトルをＴ０３、各サンプル毎の［ｆ３［ｋ］  ｓｇｎ（ｖ３［ｋ］）］を縦に並べた行列をＹ０３とすれば、
Ｔ０３＝Ｙ０３・Ｐ０３                            （６）
が得られる。Ｙ０３の擬似逆行列をＹ０３＋とすれば、Ｐ０３の推定値Ｐ０３ｈは、
Ｐ０３ｈ＝Ｙ０３＋・Ｔ０３                          （７）
で求められる。
第５軸、第６軸の重力モーメント補正係数も全く同様にして求められる。
【００１３】
次に、ステップＳＴ３で負荷をロボットに取り付け、第３軸、第５軸、第６軸が同時に動く動作パターンで、ロボットを低速かつ一定速度で動作させる。追加負荷モデルの慣性力をτｌａ、遠心・コリオリ力をτｌｂ、重力をτｌｃとすると、一定速度の時は加速度が０となるため、τｌａ＝０となり、低速の場合はτｌｂが無視できるため、式（１）、式（２）から、
τ＝τｒｍ＋τｆ＋τｈ＋τｌｃ                      （８）
と書ける。
【００１４】
ロボットの運動方程式は、質量、重心位置等のパラメータからは直接には線形化できないが、（質量）×（重心位置）等を改めてパラメータと見なすと、パラメータに関して線形化できることが知られている。したがって、追加負荷モデルの重力τｌｃも負荷質量、重心位置等の組み合わせから構成されるパラメータＰｌｇを用いて、
τｌｃ＝Ｙｌｇ・Ｐｌｇ                              （９）
と線形化できる。手先負荷の質量をｍｌ、重心位置をｌｇｘ，ｌｇｙ，ｌｇｚとすると一例は、
Ｐｌｇ＝［ｍｌ  ｍｌ・ｌｇｘ  ｍｌ・ｌｇｙ  ｍｌ・ｌｇｚ］Ｔ        （１０）
である。
【００１５】
Ｙｌｇ，τ，τｒｍ，τｈの第ｉ行をそれぞれＹｌｇｉ，τｉ，τｒｍｉ，τｈｉとし、第ｉ軸の一定速度値における摩擦トルクをｆｒｉとすると、
2004276125.tif 000004
と書ける。
【００１６】
式（１１）は第３軸、第５軸、第６軸の全てで成り立つため、
2004276125.tif 000005
とすれば、Ｔｌ＝Ｙｌ・Ｐｌ                                  （１４）
とパラメータＰｌに対して線形化できる。
【００１７】
Ｔｌ，Ｙｌを各サンプル毎に縦に並べた行列をそれぞれＴ２，Ｙ２とすれば、
Ｔ２＝Ｙ２・Ｐｌ                                  （１５）
となる。したがって、Ｙ２の擬似逆行列Ｙ２＋を用いてＰｌの推定値Ｐｌｈは、
Ｐｌｈ＝Ｙ２＋・Ｔ２                              （１６）
で求められる。Ｐｌｈが求められれば、式（１２）より手先負荷の質量ｍｌ、重心位置ｌｇｘ，ｌｇｙ，ｌｇｚも算出できる。
【００１８】
次に、ステップＳＴ５において高速動作のデータから負荷の慣性モーメントＩｘ，Ｉｙ，Ｉｚを求める。まず、第５軸、第６軸が動作する複数の動作パターンで高速にロボットを動作させ、動作中の各軸の位置、速度、加速度、電流を記録する。手先の負荷の質量ｍｌ、重心位置ｌｇｘ，ｌｇｙ，ｌｇｚは算出済みなので、追加負荷モデルの駆動トルクτｌは、
τｌ＝τｌｍ＋Ｙ３・Ｐ３Ｉ                        （１７）
Ｐ３Ｉ＝［Ｉｘ  Ｉｙ  Ｉｚ］Ｔ                    （１８）
と表現できる。ここで、τｌｍは負荷の慣性モーメントＩｘ，Ｉｙ，Ｉｚを含まないτｌの成分である。したがって、τｄを、
τｄ＝τ−τｈ−τｌｍ−τｒｍ                    （１９）
とし、τｄの第ｉ軸要素をτｄｉ、Ｙ３の第ｉ行ベクトルをＹ３ｉとすれば、
τｄ５＝Ｙ３５・Ｐ３Ｉ＋ｆｃ５・ｓｇｎ（ｖ５）＋ｆｖ５・ｖ５      （２０）
τｄ６＝Ｙ３６・Ｐ３Ｉ＋ｆｃ６・ｓｇｎ（ｖ６）＋ｆｖ６・ｖ６      （２１）
と書ける。ここで、ｆｃ５，ｆｖ５，ｆｃ６，ｆｖ６はそれぞれ第５軸のクーロン摩擦係数、第５軸の粘性摩擦係数、第６軸のクーロン摩擦係数、第６軸の粘性摩擦係数である。したがって、
Ｐ３＝［Ｉｘ  Ｉｙ  Ｉｚ  ｆｃ５  ｆｖ５  ｆｃ６  ｆｖ６］Ｔ    （２２）
で定義すれば、式（２０）、式（２１）の右辺はＰ３に関して線形化できる。
【００１９】
以上より、第５軸、第６軸が高速動作した時の動作データから、
Ｔ４＝Ｙ４・Ｐ３                                （２３）
が導出できる。ここで、Ｔ４は各サンプルのτｄ５，τｄ６から構成されるベクトルで、Ｙ４は各サンプルのＹ３５，Ｙ３６，ｓｇｎ（ｖ５），ｓｇｎ（ｖ６），ｖ５，ｖ６から構成される行列である。Ｙ４の擬似逆行列をＹ４＋とすれば、Ｐ３の推定値Ｐ３ｈは、
Ｐ３ｈ＝Ｙ４＋・Ｔ４                            （２４）
で算出できる。
【００２０】
以上のように、この実施の形態１によれば、重力から同定可能なパラメータは重力に基づいて同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。また、摩擦を同時に同定するため、摩擦をキャンセルする必要がなく、同定に使用する動作パターンの制約が少ない。さらに、ロボットモデルとしてロボット本体補正モデルを設け、そのロボット本体補正モデルのパラメータを無負荷時の動作データから予め算出しておくことにより、ロボット本体の誤差の影響を受けずに高精度に負荷パラメータの同定を行うことができる。
【００２１】
実施の形態２．
この実施の形態２では、ロボット本体補正モデルを求めるために行う無負荷時のロボット動作パターンを、ステップＳＴ３で行う重力に基づく負荷パラメータ同定用の動作パターンと全く同じ動作パターンとするものである。すなわち、低速動作で負荷の質量、重心位置を同定するのに使用するのと同一の動作パターンのみに基づいてロボット本体補正モデルのパラメータを同定する。ステップＳＴ３以降の処理は上記実施の形態１と同一なので説明は省略する。
【００２２】
以上のように、この実施の形態２によれば、同定に使用する動作に特化したロボット本体モデルの補正が行えるため、ロボット本体のモデル化誤差の影響をさらに低減することができる。
【００２３】
実施の形態３．
この実施の形態３では、負荷の慣性モーメントを求めるためにステップＳＴ５で行う高速動作の動作パターンを、ステップＳＴ３の低速動作の動作パターンと全く同一のものとする。
2004276125.tif 000006
で定義すれば、第３軸、第５軸、第６軸の動作データから、
Ｔ５＝Ｙ５・Ｐ５                                （２６）
が上記実施の形態１と同様にして導出できる。Ｐ５の推定値Ｐ５ｈはＹ５の擬似逆行列Ｙ５＋を用いて、
Ｐ５ｈ＝Ｙ５＋・Ｔ５                            （２７）
で算出できる。なお、ステップＳＴ４までの処理は上記実施の形態１と全く同一である。
【００２４】
以上のように、この実施の形態３によれば、負荷の同定に使用する動作パターンが１種類で良いため、動作パターンの準備に要する時間を短縮することができる。
【００２５】
実施の形態４．
この実施の形態４では、上記実施の形態１のステップＳＴ２、ステップＳＴ４、ステップＳＴ６でのパラメータの算出方法において若干異なるものである。上記実施の形態１では各サンプル毎の各軸の電流、位置、速度、加速度をそのまま用いていたが、この実施の形態４では平滑化フィルタに通してから使用する。
【００２６】
まず、ステップＳＴ２でロボット本体補正モデルのパラメータを算出する際、式（５）のτ３［ｋ］−τｒｍ３［ｋ］，ｆ３［ｋ］，ｓｇｎ（ｖ３［ｋ］）の各要素と、ｖ３［ｋ］とを平滑化フィルタに通したものをそれぞれｆａ１［ｋ］，ｆａ２［ｋ］，ｆａ３［ｋ］，ｆａ４［ｋ］とし、ｆａ４［ｋ］が等速になっている区間のｆａ１［ｋ］，ｆａ２［ｋ］，ｆａ３［ｋ］を用いて同定する。すなわち、ｆａ４［ｋ］が等速になっている区間のｆａ１を縦に並べたベクトルをＴ０ｆ３、［ｆａ２［ｋ］  ｆａ３［ｋ］］を縦に並べた行列をＹ０ｆ３、Ｙ０ｆ３の擬似逆行列をＹ０ｆ３＋とし、
Ｐ０３ｈ＝Ｙ０ｆ３＋・Ｔ０ｆ３                  （２８）
でＰ０３の推定値Ｐ０３ｈを求める。平滑化フィルタとしては移動平均フィルタを用いる。第５軸、第６軸のロボット本体補正モデルのパラメータも同様に求める。
【００２７】
ステップＳＴ４で負荷の質量、重心位置を算出する際も同様である。まず、式（１４）のＴｌ，Ｙｌの各要素と、第３軸の速度ｖ３を平滑化フィルタに通す。ｖ３の平滑化フィルタに通した時の出力が等速になっている区間のＴｌ，Ｙｌの各要素のフィルタに通した時の出力を使用して同定する。すなわち、Ｔｌ，Ｙｌの各要素のフィルタに通した時の出力を縦に並べた行列をそれぞれＴ２ｆ，Ｙ２ｆとし、Ｙ２ｆの擬似逆行列Ｙ２ｆ＋を用いて、
Ｐｌｈ＝Ｙ２ｆ＋・Ｔ２ｆ                        （２９）
より、Ｐｌの推定値Ｐｌｈを算出する。
【００２８】
ステップＳＴ６で残りのパラメータを求める時もほぼ同様である。式（２０）、式（２１）のτｄ５，τｄ６，Ｙ３５，Ｙ３６，ｓｇｎ（ｖ５），ｓｇｎ（ｖ６），ｖ５，ｖ６をそれぞれ平滑化フィルタに入力し、Ｔ４ｆを各サンプルのτｄ５，τｄ６のフィルタ出力から構成されるベクトル、Ｙ４ｆを各サンプルのＹ３５，Ｙ３６，ｓｇｎ（ｖ５），ｓｇｎ（ｖ６），ｖ５，ｖ６のフィルタ出力から構成される行列とし、Ｐ３の推定値Ｐ３ｈを、
Ｐ３ｈ＝Ｙ４ｆ＋・Ｔ４ｆ                        （３０）
で算出する。ここで、Ｙ４ｆ＋はＹ４ｆの擬似逆行列である。
【００２９】
以上のように、この実施の形態４によれば、動作データを平滑化フィルタに通してから使用したことにより、さらに、負荷パラメータを高精度に同定することができる。
【００３０】
実施の形態５．
この実施の形態５では、上記実施の形態１のステップＳＴ２、ステップＳＴ４、ステップＳＴ６でのパラメータの算出方法において異なるものである。上記実施の形態１では同定パラメータに関する線形化式を導出した後、擬似逆行列を用いてパラメータを算出しているが、この実施の形態５では擬似逆行列を用いず、記憶したデータに対して適応同定理論を繰り返し適用することによりパラメータを算出するものである。
【００３１】
まず、ステップＳＴ２でロボット本体補正モデルのパラメータを算出する際、式（５）の（τ３［ｋ］−τｒｍ３［ｋ］），［ｆ３［ｋ］  ｓｇｎ（ｖ３［ｋ］）］をそれぞれＴａ，Ｙａとする。この時、第ｋサンプル目の未知パラメータＰａの推定値Ｐａ［ｋ］を、
2004276125.tif 000007
で算出する。ここで、Δｔ，σは定数、Ｇは定数から構成される行列で、行列Ｒおよびベクトルｑの初期値は各要素が０であるとする。また、同定パラメータＰａの初期値は予め定めている値とする。
【００３２】
ステップＳＴ２の第３軸のロボット本体補正モデルでは未知パラメータＰａは式（４）のＰ０３となる。式（３１）〜式（３３）の演算をまず、記憶したデータに対して、サンプル数ｋが記憶したデータ個数ｍｌに達するまで行う。次に第ｍｌ番目のＰａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍｌに達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎｌ回繰り返す。最後のｎｌ回目の繰り返しにおけるＰａ［ｋ］の平均値を求めるパラメータ値とする。第５軸、第６軸のロボット本体補正モデルのパラメータも全く同様に算出する。
【００３３】
ステップＳＴ４で負荷および重心位置の算出を行う際には、式（１３）のＴｌをＴａ、式（１４）のＹｌをＹａとし、ステップＳＴ３で記憶したデータを用いて式（３１）〜式（３３）の計算を、サンプル数ｋが記憶したデータ個数ｍ２に達するまで行う。次に第ｍ２番目のＰａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ２に達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎ２回繰り返す。最後のｎ２回目の繰り返しにおけるＰａ［ｋ］の平均値を求めるパラメータ値とする。
【００３４】
ステップＳＴ６で残りのパラメータの同定を行う際には、式（２０）〜式（２２）から、
Ｔ３＝［τｄ５  τｄ６］                        （３４）
Ｔ３＝Ｙ３ｂ・Ｐ３                              （３５）
を導出する。Ｔａ＝Ｔ３、Ｙａ＝Ｙ３ｂとし、ステップＳＴ５で記憶したデータを用いて式（３１）〜式（３３）の計算を、サンプル数ｋが記憶したデータ個数ｍ３に達するまで行う。次に第ｍ３番目のＰａ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍ３に達するまで式（３１）〜式（３３）の演算を行う。この繰り返しを予め決められた回数ｎ３回繰り返す。最後のｎ３回目の繰り返しにおけるＰａ［ｋ］の平均値を求めるパラメータ値とする。
【００３５】
以上のように、この実施の形態５によれば、擬似逆行列を用いないので、同定に使用する動作パターンが、擬似逆行列をとる行列がフルランクになる動作に限る必要をなくすことができる。
【００３６】
実施の形態６．
この実施の形態６では、上記実施の形態１のステップＳＴ４、ステップＳＴ６でのパラメータの算出方法とステップＳＴ３、ステップＳＴ５で行う動作のパターン数において異なるものである。まず、ステップＳＴ３では複数の第３軸、第５軸、第６軸が動く動作パターンでロボットを動作させる。ステップＳＴ４ではステップＳＴ３で行ったそれぞれの動作に対して式（８）〜式（１６）の演算を行い、それぞれの動作パターンに対するパラメータ同定値Ｐｌｈｌ，・・・，Ｐｌｈｎを算出する。
【００３７】
次に、Ｐｌｈｌから手先負荷の質量初期値ｍｌ０、重心位置初期値ｌｇｘ０，ｌｇｙ０，ｌｇｚ０をまず算出する。
2004276125.tif 000008
【数１】
2004276125.tif 000009
Ｂ＝［ＢｌＴ  Ｂ２Ｔ  ・・・  ＢｎＴ］Ｔ        （３９）
ΔＰｌｈ＝［ΔＰｌｈｌＴ  ΔＰｌｈ２Ｔ  ・・・  ΔＰｌｈｎＴ］Ｔ    （４０）
ΔＰｇ＝Ｂ＋・ΔＰｌｈ                          （４１）
を計算し、式（４１）で算出される第１要素〜第４要素をそれぞれ負荷の質量推定値ｍｌ０、重心位置推定値ｌｇｘ０，ｌｇｙ０，ｌｇｚ０に加算したものを改めて手先負荷の質量推定値をｍｌ０、重心位置推定値ｌｇｘ０，ｌｇｙ０，ｌｇｚ０とする。式（３６）〜式（４１）でΔＰｇを求め、負荷および重心位置の推定値を修正することを規定回数繰り返す。規定回数終了後の推定値を求めるパラメータとする。
【００３８】
ステップＳＴ５では第５軸、第６軸が動く動作を、同定に必要なパターンのｎ２倍の動作パターンで動作させる。ｎ２個の動作パターンの組み合わせ毎に式（２４）でパラメータを算出し、それぞれの平均値を求めるパラメータとする。
【００３９】
以上のように、この実施の形態６によれば、複数の同定結果から平均を出すことによって、あらゆる動作パターンに基づいて負荷パラメータを高精度に同定することができる。
【００４０】
実施の形態７．
図３はこの発明の実施の形態７による負荷パラメータ同定方法を示すフローチャートであり、同図において、実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する（ステップＳＴ１１）。次にステップＳＴ１１で記憶した位置、速度、加速度、電流を用いて、その動作に特化した負荷パラメータ値の同定を行う（ステップＳＴ１２）。この実施の形態７では、実作業で動作させる各々の動作に対して、ロボットに取り付けた負荷が変わるかどうかに関わらず、ステップＳＴ１１、ステップＳＴ１２の同定作業を実施するものである。
【００４１】
次に動作について説明する。
ステップＳＴ１２での同定方法について説明する。式（１）の追加負荷モデルの駆動トルクτｌは、追加負荷の質量、重心位置、慣性モーメントの演算結果から構成されるパラメータベクトルＰ７に関して、
τｌ＝Ｙ７・Ｐ７                                （４２）
と線形化できる。また、式（１）、式（２）より、
τｌ＋τｆ＝τ−τｒｍ−τｈ                    （４３）
が得られる。したがって、パラメータベクトルＰ７を各軸のクーロン摩擦係数、粘性摩擦係数を含む新しいパラメータベクトルＰ７ｂに拡張し、式（４３）の左辺をτ７ｂとおけば、
τ７ｂ＝Ｙ７ｂ・Ｐ７ｂ                          （４４）
と線形化できる。
【００４２】
このパラメータベクトルの各要素を上記実施の形態５と同様に、適応同定理論を繰り返し適用することによりパラメータを算出する。具体的には、第ｋサンプル目の未知パラメータＰ７ｂの推定値Ｐ７ｂｈ［ｋ］を、
2004276125.tif 000010
で算出する。ここで、Δｔ，σは定数、Ｇは定数から構成される行列で、行列Ｒおよびベクトルｑの初期値は各要素が０であるとする。また、同定パラメータＰ７ｂｈの初期値は予め定めている値とする。式（４５）〜式（４７）の演算をまず、記憶したデータに対して、サンプル数ｋが記憶したデータ個数ｍｌに達するまで行う。次に第ｍｌ番目のＰ７ｂｈ，Ｒ，ｑの値を初期値として、再度サンプル数ｋが記憶したデータ個数ｍｌに達するまで式（４５）〜式（４７）の演算を行う。この演算を予め決められた回数ｎｌ回繰り返す。最後のｎｌ回目の繰り返しにおけるＰ７ｂｈ［ｋ］の平均値を求めるパラメータ値とする。
【００４３】
以上のように、この実施の形態７によれば、擬似逆行列を用いないので、同定に使用する動作パターンが、擬似逆行列をとる行列がフルランクになる動作に限る必要をなくすことができる。また、動作毎に特化したパラメータを同定するため、その動作のトルク推定精度がさらに向上し、最適な加減速時間の算出を行うことができる。
【００４４】
実施の形態８．
この実施の形態８による負荷パラメータ同定方法も図３のフローチャートで示されるものである。ステップＳＴ１１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ１２で、ステップＳＴ１１で記憶した位置、速度、加速度、電流を用いて、その動作に特化した負荷パラメータ値の同定を行う。この実施の形態８では、上記実施の形態７とは異なり、負荷７の質量のみを未知パラメータとして同定する。質量以外のパラメータと摩擦係数は既知とし、初期値、もしくは、予め同定した値を使用する。式（１）の右辺は、手先負荷質量を含む成分をτｌａ、負荷を含まない成分をτｌｂとすると、
τ＝τｌａ＋τｌｂ                              （４８）
と変形できる。τ−τｌｂをτ８とすると、手先負荷質量を含む成分をτｌａは手先負荷質量Ｐ８に関して線形化できるため、
τ８＝Ｙ８・Ｐ８                                （４９）
と書ける。したがって、ステップＳＴ１で記憶した各サンプル毎のτ８，Ｙ８を縦に並べた行列をそれぞれＴ８，Ｙ８ｂとすれば、Ｙ８ｂの擬似逆行列Ｙ８ｂ＋を用いて、
Ｐ８＝Ｙ８ｂ＋・Ｔ８                            （５０）
でその動作の手先負荷質量Ｐ８が推定できる。
【００４５】
以上のように、この実施の形態８によれば、擬似逆行列をとる行列がほぼ全ての場合でフルランクになるため、同定に使用する動作パターンがほとんど制限されることはない。また、動作毎に特化したパラメータを同定するため、その動作のトルク推定精度がさらに向上し、最適な加減速時間の算出を行うことができる。
【００４６】
実施の形態９．
図４はこの発明の実施の形態９による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ２３〜ＳＴ２８はそれぞれ図１のステップＳＴ１〜ＳＴ６に対応するので、説明は省略する。ステップＳＴ２１では、ロボットの手先に何も取り付けず（無負荷状態）にある決まった動作をさせ、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。次に質量、重心位置、慣性モーメントが既知の負荷を手先に取り付け、ステップＳＴ２１で無負荷状態にて行った動作と同じ動作を行い、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。
ステップＳＴ２２では、各軸のトルク定数が公称値であるとして、各軸の電流から各軸のトルクを算出し、無負荷時の動作データおよび既知の負荷を取り付けた時の動作データから、既知負荷のパラメータを同定する。同定した既知負荷のパラメータと、既知負荷パラメータの真値の比に応じて、トルク定数を修正する。ステップＳＴ２３以降では、修正したトルク定数を用いて、未知負荷のパラメータを同定する。
【００４７】
以上のように、この実施の形態９によれば、トルク定数が個体毎にばらつく場合でも、高精度に追加負荷のパラメータ同定を行うことができる。
【００４８】
実施の形態１０．
この実施の形態１０による負荷パラメータ同定方法も図４のフローチャートで示されるものである。図４のステップＳＴ２３〜ステップＳＴ２８はそれぞれ図１のステップＳＴ１〜ステップＳＴ６に対応するので、説明は省略する。
まず、予め機種毎に、ある動作を行った時のロボット本体補正モデルのパラメータ値を算出しておく。次に、ステップＳＴ２１で、ロボット本体補正モデルのパラメータの同定に使用されたのと全く同じ動作を行い、その時の位置、速度、加速度、電流を決まったサンプリング周期毎に記憶しておく。
ステップＳＴ２２では、各軸のトルク定数が公称値であるとして、各軸の電流から各軸のトルクを算出し、ロボット本体補正モデルのパラメータを同定する。同定したロボット本体補正モデルのパラメータ値と予め記憶してある機種毎のロボット本体補正モデルのパラメータ値の比に応じて、トルク定数を修正する。ステップＳＴ２３以降では、修正したトルク定数を用いて、未知負荷のパラメータを同定する。
【００４９】
以上のように、この実施の形態１０によれば、トルク定数が個体毎にばらつく場合でも、高精度に追加負荷のパラメータ同定を行うことができる。
【００５０】
実施の形態１１．
図５はこの発明の実施の形態１１による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ３１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ３２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。ステップＳＴ３３では加速区間、減速区間のそれぞれに対して式（４４）の線形化を行い、式（４５）〜式（４７）の計算を加速区間、減速区間のそれぞれに対して繰り返すことにより、加速区間のパラメータと減速区間のパラメータを別個に算出する。
【００５１】
以上のように、この実施の形態１１によれば、動作毎の加速区間と、減速区間とのそれぞれに特化したパラメータを同定するため、その動作の加速区間および減速区間におけるトルク推定精度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００５２】
実施の形態１２．
図６はこの発明の実施の形態１２による負荷パラメータ同定方法を示すフローチャートであり、同図において、ステップＳＴ４１で実際の作業と同じ速度で、実際に行う動作パターンの１つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ４２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。
【００５３】
ステップＳＴ４３では、まず、加速区間の電流から加速区間のトルクを求め、加速区間の各軸のトルク最大値を算出する。各軸の加速区間トルク最大値をその軸のトルク許容値で除した値の絶対値が最も大きい軸ｉｋを求める。同様にして、各軸の減速区間トルク最大値をその軸のトルク許容値で除した値の絶対値が最も大きい軸ｉｇを求める。
【００５４】
ステップＳＴ４４では、加速区間は軸ｉｋのデータに基づいてパラメータの同定を行い、減速区間は軸ｉｇのデータに基づいてパラメータの同定を実施する。
まず、加速区間の式（４４）から軸ｉｋに関する部分のみ取り出せば、
τ７ｂｉｋ＝Ｙ７ｂｉｋ・Ｐ７ｂ                  （５１）
が得られる。そこで、Ｐ７ｂを上記実施の形態１１と同様に、
2004276125.tif 000011
を繰り返し計算することにより算出する。
【００５５】
減速区間も同様に、減速区間の式（４４）から軸ｉｇに関する部分のみ取り出せば、
τ７ｂｉｇ＝Ｙ７ｂｉｇ・Ｐ７ｂ                  （５５）
が得られる。そこで、Ｐ７ｂを上記実施の形態１１と同様に、
2004276125.tif 000012
を繰り返し計算することにより算出する。
【００５６】
以上のように、この実施の形態１２によれば、動作毎の加速区間と、減速区間とのそれぞれにおいて、加減速時間を制約するであろう軸に特化したパラメータを同定するため、その動作の加速区間および減速区間における加減速時間を制約するであろう軸のトルク推定精度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００５７】
実施の形態１３．
この実施の形態１３による負荷パラメータ同定方法も図１のフローチャートで示されるものである。この実施の形態１３では、図２に示した６軸垂直多関節ロボットの第２リンク９に取り付けた負荷のパラメータを同定する。
図７は垂直２自由度ロボットの一例を示す説明図であり、図において、第１軸１０および第２軸１１と、第１リンク１２および第２リンク１３と、負荷１４ととから構成されている。上記実施の形態１と異なるのは、追加負荷モデルとして図７に示す垂直２自由度ロボットモデルを用いことであり、図７の垂直２自由度ロボットは第１リンク１２の質量は０で、第２リンク１３は負荷１４の質量、重心位置、慣性モーメントで構成される。第１リンク１２の長さは本体ロボットの第１リンクの長さと同一である。
同定した追加負荷パラメータは、図１の第２リンク９の要素の一部と見なしてロボット本体モデルの第２リンク１３のパラメータを修正してから、加減速時間の算出、トルクフィードフォワードの計算等に使用する。
【００５８】
以上のように、この実施の形態１３によれば、ロボットの手先以外に取り付けられた負荷１４のパラメータも簡単な計算により同定することができる。
【００５９】
実施の形態１４．
図８はこの発明の実施の形態１４による負荷パラメータ同定方法を示すフローチャートであり、図９はエレベータドアの一例を示す説明図である。図９において、エレベータドア駆動用モータ１５の回転は、Ｖベルト１７を通じてプーリ１６を回転させ、さらに、そのプーリ１６を回転は、タイミングベルト１８を通じてカゴドア１９を動作させる。乗り場ドア（図示せず）は、その乗り場ドアに設けられた装置（図示せず）でカゴドア１９と結合され、連動して動作する。なお、戸締め力発生リンク２０は、カゴドア１９に連動して、その重力に応じた戸締め力をカゴドア１９に与えるものである。
【００６０】
次に動作について説明する。
図８および図９において、まず、乗り場ドアがドアカゴ１９と結合された状態で、エレベータドア駆動用モータ１５を低速かつ一定速度で動作させ、その時のエレベータドア駆動用モータ１５の回転角、速度、電流を記憶しておく（ステップＳＴ５１）。次にステップＳＴ５１で記憶したエレベータドア駆動用モータ１５の回転角、速度、電流から戸締め力発生リンク２０の質量、重心位置と、ステップＳＴ５１での動作速度における摩擦力とを同定する（ステップＳＴ５２）。次に乗り場ドアがドアカゴ１９と結合された状態で、エレベータドア駆動用モータ１５を高速動作させ、その時のエレベータドア駆動用モータ１５の回転角、速度、加速度、電流を記憶しておく（ステップＳＴ５３）。次にステップＳＴ５３で記憶したエレベータドア駆動用モータ１５の回転角、速度、加速度、電流からドアカゴ１９と乗り場ドアを合わせた質量と、摩擦係数とを同定する（ステップＳＴ５４）。
【００６１】
以上のように、この実施の形態１４によれば、重力から同定可能な戸締め力発生リンク２０のパラメータは重力から同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。
【００６２】
実施の形態１５．
この実施の形態１５による負荷パラメータ同定方法も図５のフローチャートで示されるものである。まず、実装機（図示しない）の先端に実際の作業の一つで実装される電子部品を取り付ける。次にステップＳＴ３１で実際の作業と同じ速度で、実際に行う動作パターンの一つを動作させ、その時の一定サンプル時間毎の位置、速度、加速度、電流を記憶する。次にステップＳＴ３２で各サンプル毎の速度ノルムを計算し、速度ノルムの差分をとることにより加速度ノルムを算出する。加速度ノルムが正のサンプルにおける位置、速度、加速度、電流のデータは加速区間のデータとして記憶し、加速度ノルムが負のサンプルにおける位置、速度、加速度、電流のデータは減速区間のデータとして記憶する。ステップＳＴ３３では、加速区間、減速区間のそれぞれに対して、実装機の先端に取り付けた電子部品の質量、重心位置、慣性モーメントの演算結果と摩擦係数から構成されるベクトルＰに関して、式（４４）と同様な線形化を実施し、式（４５）〜式（４７）と同様な計算を加速区間、減速区間のそれぞれに対して繰り返すことにより、加速区間のパラメータと減速区間のパラメータとを別個に計算する。
【００６３】
以上のように、この実施の形態１５によれば、動作毎の加速区間と減速区間とのそれぞれに特化したパラメータを同定するため、その動作の加速区間および減速区間におけるトルク推定速度がさらに向上し、最適な加速時間および減速時間の算出を行うことができる。
【００６４】
実施の形態１６．
図１０はこの発明の実施の形態１６による負荷パラメータ同定方法を示すフローチャートであり、同図において、まず、工作機械（図示しない）の先端に負荷が取り付けられていない状態で、複数の動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ６１）。次にステップＳＴ６１で記憶した各軸の位置、速度、電流から工作機械本体補正モデルのパラメータと、ステップＳＴ６１での動作速度における摩擦力を同定する（ステップＳＴ６２）。次に同定したい切削工具等の負荷を工作機械に取り付け、ある動作を低速かつ一定速度で動作させ、その時の各軸の位置、速度、電流を記憶しておく（ステップＳＴ６３）。次にステップＳＴ６３で記憶した各軸の位置、速度、電流から、負荷の質量、重心位置と、ステップＳＴ６３での動作速度における摩擦力とを同定する（ステップＳＴ６４）。次にステップＳＴ６３とは異なる動作パターンで工作機械を高速動作させ、その時の各軸の位置、速度、加速度、電流を記憶しておく（ステップＳＴ６５）。次にステップＳＴ６５で記憶した各軸の位置、速度、加速度、電流から負荷の慣性モーメントと摩擦係数とを同定する（ステップＳＴ６６）。
【００６５】
以上のように、この実施の形態１６によれば、重力から同定可能なパラメータは重力から同定し、重力からでは同定できないパラメータのみ慣性力、遠心・コリオリ力に基づいて同定するため、全てのパラメータを高精度に同定することができる。
【００６６】
【発明の効果】
以上のように、この発明によれば、低速動作させた時の動作データから重力に関するパラメータを同定し、高速動作させた時の動作データから低速動作時には同定しなかった残りのパラメータを同定することにより、全てのパラメータを高精度に同定することができる。また、低速動作時および高速動作時において摩擦力を同時に同定するため、摩擦力をキャンセルする必要がなく、同定に使用する動作パターンの制約を少なくすることができる。さらに、パラメータの同定に使用される動作も少なくすることができる効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１による負荷パラメータ同定方法を示すフローチャートである。
【図２】６軸垂直多関節型ロボットの一例を示す説明図である。
【図３】この発明の実施の形態７による負荷パラメータ同定方法を示すフローチャートである。
【図４】この発明の実施の形態９による負荷パラメータ同定方法を示すフローチャートである。
【図５】この発明の実施の形態１１による負荷パラメータ同定方法を示すフローチャートである。
【図６】この発明の実施の形態１２による負荷パラメータ同定方法を示すフローチャートである。
【図７】垂直２自由度ロボットの一例を示す説明図である。
【図８】この発明の実施の形態１４による負荷パラメータ同定方法を示すフローチャートである。
【図９】エレベータドアの一例を示す説明図である。
【図１０】この発明の実施の形態１６による負荷パラメータ同定方法を示すフローチャートである。
【符号の説明】
１，１０  第１軸、２，１１  第２軸、３  第３軸、４  第４軸、５  第５軸、６  第６軸、７，１４  負荷、８，１２  第１リンク、９，１３  第２リンク、１５  エレベータドア駆動用モータ、１６  プーリ、１７  Ｖベルト、１８  タイミングベルト、１９  カゴドア、２０  戸締め力発生リンク。
【図１】
2004276125.tif 000013
【図２】
2004276125.tif 000014
【図３】
2004276125.tif 000015
【図４】
2004276125.tif 000016
【図５】
2004276125.tif 000017
【図６】
2004276125.tif 000018
【図７】
2004276125.tif 000019
【図８】
2004276125.tif 000020
【図９】
2004276125.tif 000021
【図１０】
2004276125.tif 000022