JP2015089585A

JP2015089585A - リグレッサ行列の算出方法、力学パラメータの同定方法

Info

Publication number: JP2015089585A
Application number: JP2013229373A
Authority: JP
Inventors: 高橋　太郎; Taro Takahashi; 太郎高橋
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2013-11-05
Filing date: 2013-11-05
Publication date: 2015-05-11

Abstract

【課題】ロボットの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出すること。
【解決手段】予め定められた力学パラメータＰを用いて逆動力学演算を行って、トルクを示すベクトルＴ_ｐを算出する。ベクトルＴ_ｐと予め定められた一のリンクの力学パラメータＰのいずれかの成分に対し、所定の増分を加えたベクトルを算出する。算出した所定の増分を加えたベクトルを用いて逆動力学演算を行ってトルクを示すベクトルＴを算出し、ベクトルＴ_ｐとベクトルＴとを用いて縦ベクトルを算出する。ベクトルＴの算出を、ベクトルＴ_ｐと予め定められた一のリンクの力学パラメータＰの残りの成分と、複数のリンク残りのリンクとについて行い、ベクトルＴを複数算出し、縦ベクトルｋ_１〜ｋ_１０×Ｎを算出する。縦ベクトルｋ_１〜ｋ_１０×Ｎを合成して、リグレッサ行列を算出する。
【選択図】図２

Description

本発明はリグレッサ行列の算出方法、力学パラメータの同定方法及びロボットに関する。

近年、手先で物体を把持して移動させるロボットの開発が行われている。ロボットを精度よく制御するため、様々な手法が提案されている。一例として、運動方程式をリンクの質量、重心位置と質量の積及び慣性テンソルを含む力学パラメータに対して、いわゆるリグレッサ行列を用いて線形な関係式で表す手法が知られている。

特許文献１には、リグレッサ行列を含む運動方程式を用いてロボットの姿勢制御を行う際の力学パラメータの同定方法が開示されている。この方法では、ロボットに様々な姿勢を取らせ、ロボットの位置及び姿勢、各関節の角度、床からの反力から、最小二乗法によりすべてのパラメータを同時に推定する。これのより、各関節に生じるトルクを計測することなく、力学パラメータを同定できる。

特許文献２には、関節に生じるトルク、外力及びベースリンクに関する運動方程式に基づいて、運動方程式の最小力学パラメータとトルクセンサのトルクのオフセット値を同時に同定できるトルクセンサ校正装置が開示されている。

国際公開２００９／１４７８７５特開２０１２−１７６４６５号公報

しかし、発明者は、上述の手法には以下に示す問題点が有ることを見出した。リグレッサ行列は、多関節ロボットの関節位置や回転軸方向によって変化する。また、リグレッサ行列を求めようとすると、角速度ベクトルと外積を求めるなどの複雑な演算が必要である。そのため、特許文献１及び２の手法では、いずれもリグレッサ行列を導出する過程が煩雑である。その結果、力学パラメータを同定するためのシステムを開発するのに、多大のコストや開発時間を要してしまう。

本発明の一態様であるリグレッサ行列の算出方法は、多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系において、予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、複数の前記第６のベクトルを合成して前記リグレッサ行列を算出するものである。

上記のリグレッサ行列の算出方法においては、トルクをτ、前記リグレッサ行列をＫ、前記第１のベクトルをＰとして、以下の式が成立し、

ｍ_ｉ（ｉは、１≦ｉ≦Ｎを満たす整数、Ｎは前記リンクの総数を示す正の整数）はｉ番目の前記リンクの質量とし、Ｓ_ｘｉ、Ｓ_ｙｉ及びＳ_ｚｉは、それぞれｉ番目の前記リンクの重心位置のｘ、ｙ及びｚ座標とし、Ｍ_ｘｘｉ、Ｍ_ｘｙｉ、Ｍ_ｘｚｉ、Ｍ_ｙｙｉ、Ｍ_ｙｚｉ、Ｍ_ｚｚｉは、それぞれｉ番目の前記リンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分であるとして、前記第１のベクトルＰは以下の式で表され、

ｉ番目の前記リンクについて、ｍ_ｉ、Ｓ_ｘｉ、Ｓ_ｙｉ、Ｓ_ｚｉ、Ｍ_ｘｘｉ、Ｍ_ｘｙｉ、Ｍ_ｘｚｉ、Ｍ_ｙｙｉ、Ｍ_ｙｚｉ及びＭ_ｚｚｉに、順に所定の増分を加えた逆動力学演算を行い、
前記順に所定の増分を加えた逆動力学演算を、前記複数のリンクのそれぞれについて行うことが望ましい。

上記のリグレッサ行列の算出方法においては、前記順に所定の増分を加えた逆動力学演算をＮ個の前記リンクのそれぞれについて行うことで得られる縦ベクトルを横方向に並べて、前記リグレッサ行列を算出することが望ましい。また、一部のパラメータのみ同定する場合はそれに対応した縦ベクトルを横方向に並べ、あるいは最小力学パラメータを同定する場合はそれに対応するように変形してリグレッサ行列を算出することが望ましい。

本発明の一態様である力学パラメータの同定方法は、前記多関節ロボットのそれぞれの関節のトルクを計測し、前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度を算出し、前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度と、前記第１のベクトルと、を用いて逆動力学演算を行って、前記第２のベクトルを算出し、前記多関節ロボットのそれぞれの関節のトルクを計測し、請求項１乃至３のいずれか一項に記載の前記リグレッサ行列の算出方法で前記リグレッサ行列を算出し、算出した前記リグレッサ行列の疑似逆行列を算出し、計測した前記トルクを示す第７のベクトルと前記第２のベクトルとの和に、前記リグレッサ行列の疑似逆行列を乗じて、力学パラメータの補正分を示す第８のベクトルを算出し、前記第１のベクトルに前記第８のベクトルを加えて、前記多関節ロボットの力学パラメータを同定するものである。

本発明の一態様であるロボットシステムは、多関節ロボットと、前記多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系の前記リグレッサ行列を算出するリグレッサ行列算出部と、算出した前記リグレッサ行列を用いて、前記多関節ロボットの動作を制御する制御装置と、を備え、前記リグレッサ行列算出部は、予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、複数の前記第６のベクトルを合成して前記リグレッサ行列を算出するものである。

本発明によれば、ロボットアームの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出することができる。

実施の形態１にかかるロボットシステム１００の構成を模式的に示す図である。実施の形態１にかかるリグレッサ行列Ｋの算出手順を示すフローチャートである。逆動力学演算で得られた第１関節のトルクを示すグラフである。逆動力学演算で得られた第４関節のトルクを示すグラフである。実施の形態２にかかるロボット２００の構成を模式的に示す図である。実施の形態２にかかる力学パラメータの同定の手順を示すフローチャートである。

実施の形態１
実施の形態１にかかるリグレッサ行列算出方法について説明する。図１は、実施の形態１にかかるロボットシステム１００の構成を模式的に示す図である。ロボットシステム１００は、ロボット１、トルクセンサ２、リグレッサ行列算出部３及び制御装置４を有する。

ロボット１は、多関節のロボットである。図１では、一例として、７関節のロボットを模式的に図示している。関節Ｊ１、Ｊ３、Ｊ５及びＪ７は、図１紙面に平行な方向を軸として回転する関節である。関節Ｊ２、Ｊ４及びＪ６は、図１紙面に垂直な方向を軸として回転する関節である。ＨＤは、物体の把持部である。

トルクセンサ２は、ロボット１が動作している際の各関節のトルクを測定し、ベクトルで示されるトルクτを出力する。以下の説明では、ベクトル及び行列については、太字で表す。

リグレッサ行列算出部３は、トルクτ、物理パラメータＰ及び関節角度ｑから、リグレッサ行列Ｋを算出する。

制御装置４は、制御信号ＣＯＮを与えることで、ロボット１の動作制御を行う。制御装置４には、ロボット１の動作制御に必要な情報が格納される。制御装置４は、リグレッサ行列算出部３に物理パラメータＰ及び関節角度ｑを出力し、リグレッサ行列算出部３が算出したリグレッサ行列Ｋを取得する。そして、制御装置４は、リグレッサ行列Ｋを含む運動方程式を用いて、ロボット１の運動を制御する。

次いで、リグレッサ行列算出部３が行うリグレッサ行列Ｋの算出の方法について説明する。多関節ロボットの運動方程式は、一般的に以下の式（１）で表される。

式（１）において、τは関節のトルクを示すベクトル、ｑは関節角度を示すベクトルである。
右辺第１項は、慣性力であり、慣性行列Ｍと関節の角加速度ベクトルｄ^２ｑ／ｄｔ^２（ｑの２階時間微分、式ではｑの上に微分階数を示す黒点を２つ付して表示する）積として表される。
右辺第２項の行列Ｈは遠心力及びコリオリ力から構成される非線形項であり、関節角度ｑ及び関節の角速度ｄｑ／ｄｔ（ｑの１階時間微分、式ではｑの上に微分階数を示す黒点を１つ付して表示する）を変数とする行列で表される。
右辺第３項の行列Ｇは重力を表し、関節角度ｑを変数とする行列である。

式（１）において、考慮すべき力学パラメータは、各関節の質量ｍ、重心位置ｓ及び慣性テンソルＭである。これらの力学パラメータを同定するため、Ｎ（Ｎは、１以上の整数）個のリンクを有する多関節ロボットの力学パラメータＰを、以下の式（２）のように縦ベクトルとして定義する。

式（２）において、ｍ_ｉ（ｉは、１≦ｉ≦Ｎを満たす整数）はｉ番目のリンクの質量を示す。Ｓ_ｘｉ、Ｓ_ｙｉ及びＳ_ｚｉは、それぞれｉ番目のリンクの重心位置のｘ、ｙ及びｚ座標を示す。Ｍ_ｘｘｉ、Ｍ_ｘｙｉ、Ｍ_ｘｚｉ、Ｍ_ｙｙｉ、Ｍ_ｙｚｉ、Ｍ_ｚｚｉは、それぞれｉ番目のリンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分を示す。つまり、各リンクについて１０個の力学パラメータが存在し、力学パラメータＰは合計で（１０×Ｎ）個の力学パラメータを含む。

力学パラメータＰを用いて、式（１）を変形すると、以下の式（３）が得られる。

式（３）のＫは、いわゆるリグレッサ行列と称される行列である。リグレッサ行列Ｋは、（１０×Ｎ）個の列ベクトルｋ_１、ｋ_２、・・・、ｋ_１０Ｎを用いて、以下の式（４）で表される。なお、列ベクトルｋ_１、ｋ_２、・・・、ｋ_１０Ｎは、それぞれ縦ベクトルである。

リグレッサ行列Ｋが力学パラメータＰに対して線形な形式で表すことができれば、力学パラメータＰは以下の式（５）で表される。

式（５）のＫ^＋は、リグレッサ行列Ｋの疑似逆行列である。

本実施の形態では、複雑な演算を要することなくリグレッサ行列Ｋを求めるため、逆動力学演算を用いた簡素な演算でリグレッサ行列Ｋを求める方法について説明する。図２は、実施の形態１にかかるリグレッサ行列Ｋの算出手順を示すフローチャートである。

ステップＳ１１
ＣＡＤ情報などから、多関節ロボットの力学パラメータＰのおおよその値が判明している場合がある。まず、判明している力学パラメータＰ（第１のベクトル）を用いて、逆動力学演算を行う。この場合に、逆動力学演算を行って得られる結果Ｔ_ｐ（第２のベクトル）を、以下の式（６）で表す。

ステップＳ１２
次いで、各リンクについて、力学パラメータの成分を順に微小増加させて逆動力学演算を行った結果を算出する。まず、１番目のリンクについて力学パラメータの成分を順に微小増加させるため、リンクの番号を示すパラメータｉを初期値である「１」に設定する。以下、説明の簡略化のため、ステップＳ１３〜Ｓ２２に関しては、ｉ＝１の場合について説明する。

ステップＳ１３
１番目のリンクの１個目の力学パラメータＰ（第３のベクトル）の値である質量ｍ_１をΔｍ_１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ_１＋Δｍ_１）（第５のベクトル）を算出する。Ｔ（１，ｍ_１＋Δｍ_１）は、以下の式（７）で表される。

この場合、リグレッサ行列Ｋの第１列ｋ_１（第６のベクトル）は、以下の式（８）で表される。

ステップＳ１４
１番目のリンクの２個目の力学パラメータＰの値ｍ_１ｓ_ｘ１をｍ_１Δｓ_ｘ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ_１（ｓ_ｘ１＋Δｓ_ｘ１））を算出する。Ｔ（１，ｍ_１（ｓ_ｘ１＋Δｓ_ｘ１））は、以下の式（９）で表される。

この場合、リグレッサ行列Ｋの第２列ｋ_２（第６のベクトル）は、以下の式（１０）で表される。

ステップＳ１５
１番目のリンクの３個目の力学パラメータＰの値ｍ_１ｓ_ｙ１をｍ_１Δｓ_ｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ_１（ｓ_ｙ１＋Δｓ_ｙ１））を算出する。Ｔ（１，ｍ_１（ｓ_ｙ１＋Δｓ_ｙ１））は、以下の式（１１）で表される。

この場合、リグレッサ行列Ｋの第３列ｋ_３（第６のベクトル）は、以下の式（１２）で表される。

ステップＳ１６
１番目のリンクの４個目の力学パラメータＰの値ｍ_１ｓ_ｚ１をｍ_１Δｓ_ｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ_１（ｓ_ｚ１＋Δｓ_ｚ１））を算出する。Ｔ（１，ｍ_１（ｓ_ｚ１＋Δｓ_ｚ１））は、以下の式（１３）で表される。

この場合、リグレッサ行列Ｋの第４列ｋ_４（第６のベクトル）は、以下の式（１４）で表される。

ステップＳ１７
１番目のリンクの５個目の力学パラメータＰの値Ｍ_ｘｘ１をΔＭ_ｘｘ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｘｘ１）を算出する。Ｔ（１，ΔＭ_ｘｘ１）は、以下の式（１５）で表される。

この場合、リグレッサ行列Ｋの第５列ｋ_５（第６のベクトル）は、以下の式（１６）で表される。

ステップＳ１８
１番目のリンクの６個目の力学パラメータＰの値Ｍ_ｘｙ１をΔＭ_ｘｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｘｙ１）を算出する。Ｔ（１，ΔＭ_ｘｙ１）は、以下の式（１７）で表される。

この場合、リグレッサ行列Ｋの第６列ｋ_６（第６のベクトル）は、以下の式（１８）で表される。

ステップＳ１９
１番目のリンクの７個目の力学パラメータＰの値Ｍ_ｘｚ１をΔＭ_ｘｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｘｚ１）を算出する。Ｔ（１，ΔＭ_ｘｚ１）は、以下の式（１９）で表される。

この場合、リグレッサ行列Ｋの第７列ｋ_７（第６のベクトル）は、以下の式（２０）で表される。

ステップＳ２０
１番目のリンクの８個目の力学パラメータＰの値Ｍ_ｙｙ１をΔＭ_ｙｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｙｙ１）を算出する。Ｔ（１，ΔＭ_ｙｙ１）は、以下の式（２１）で表される。

この場合、リグレッサ行列Ｋの第８列ｋ_８（第６のベクトル）は、以下の式（２２）で表される。

ステップＳ２１
１番目のリンクの９個目の力学パラメータＰの値Ｍ_ｙｚ１をΔＭ_ｙｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｙｚ１）を算出する。Ｔ（１，ΔＭ_ｙｚ１）は、以下の式（２３）で表される。

この場合、リグレッサ行列Ｋの第９列ｋ_９（第６のベクトル）は、以下の式（２４）で表される。

ステップＳ２２
１番目のリンクの１０個目の力学パラメータＰの値Ｍ_ｚｚ１をΔＭ_ｚｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭ_ｚｚ１）を算出する。Ｔ（１，ΔＭ_ｚｚ１）は、以下の式（２５）で表される。

この場合、リグレッサ行列Ｋの第１０列ｋ_１０（第６のベクトル）は、以下の式（２６）で表される。

ステップＳ２３
次いで、リンクの番号を示すパラメータｉが「Ｎ」であるか（ｉ＝Ｎ）を判定する。

ステップＳ２４
リンクの番号を示すパラメータｉが「Ｎ」でない場合（ｉ≠Ｎ）、ｉに１を加算（ｉ＝ｉ＋１）し、ステップＳ１２に戻る。

ステップＳ２５
リンクの番号を示すパラメータｉが「Ｎ」である場合（ｉ＝Ｎ）、ステップＳ１２〜Ｓ２３がＮ回繰り返されることで、（１０×Ｎ）個の列ベクトルｋ_１〜ｋ_１０Ｎが算出されたこととなる。つまり、リグレッサ行列を構成する列ベクトルが全て算出されているので、列ベクトルｋ_１〜ｋ_１０Ｎを組み合わせて、リグレッサ行列Ｋを算出することができる。

以上、上述の方法によれば、差分計算を行う簡素な演算により、リグレッサ行列を算出することができる。これにより、ロボットの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出することができる。

次いで、本実施の形態で算出したリグレッサ行列を用いた場合の力学パラメータの同定結果について説明する。ここでは、ロボットは7関節の冗長ロボットを想定し、仮想的にリンクの質量を変化させたシミュレーションにより、力学パラメータを同定した。7関節の冗長ロボットの全体の質量は約９．５（ｋｇ）であり、肩関節から手先位置までの長さは約６５（ｃｍ）である。

同定に用いるロボットの姿勢は、同定に必要な情報を含むようになるべくランダムな姿勢が望ましい。そのため、各関節の目標軌道は振幅１（ｒａｄ）の三角関数で生成した。また、その振幅は肩関節から手先関節に向けて０．２５Ｈｚ、０．２６Ｈｚ、０．２７Ｈｚ、０．２８Ｈｚ、０．２９Ｈｚ、０．３０Ｈｚ、０．３１Ｈｚとした。この目標軌道の中で、１秒から６．９秒までの６０種類の姿勢（０．１秒間隔）を同定に利用した。

以上の条件下で、第４リンク（肘）と第７リンク（手）にそれぞれ１．０（ｋｇ）の質量を追加して行った逆動力学演算の結果をセンサ値とした。以下、シミュレーションであそれにより得られた同定結果を表に示す。

以上の同定結果を適用して、再度逆動力学演算を行った。図３は、逆動力学演算で得られた第１関節のトルクを示すグラフである。図４は、逆動力学演算で得られた第４関節のトルクを示すグラフである。

図３及び４に示すように、同定に用いた姿勢を取得した６．９秒以降ではトルクのセンサ値（measured torque）は、トルク計算値（recalculated torque）に一致している。よって、本実施の形態で得られるリグレッサ行列を用いて、力学パラメータを正確に同定できていることが理解できる。

以上、本実施の形態についてまとめると、上述した従来技術では、リグレッサ行列を算出しようとした場合、角速度ベクトルと外積を求めるソフトなど、リグレッサ行列を演算するための専用ソフトが多く必要となる。そのため、力学パラメータを同定するためのシステムを開発するために多大なコストや開発時間を要してしまう。

これに対し、本実施の形態では、逆動力学演算を用いてリギレッサ行列を算出することができる。そのため、リグレッサ行列を算出するために必要な専用ソフト等を減らすことができる。従って、力学パラメータを同定するためのシステムを開発するために要するコストや時間を低減することができる。また、算出したリグレッサ行列算出方法を用いて力学パラメータを同定することができる。

実施の形態２
実施の形態２にかかるリグレッサ行列算出方法について説明する。図５は、実施の形態２にかかるロボット２００の構成を模式的に示す図である。ロボット２００は、実施の形態１にかかるロボットシステム１００に、パラメータ同定部５を追加した構成を有する。パラメータ同定部５は、ロボット１の力学パラメータを同定することができる。

以下、実施の形態２にかかる力学パラメータの同定方法について説明する。図６は、実施の形態２にかかる力学パラメータの同定の手順を示すフローチャートである。

ステップＳ３１
制御装置４はロボットに所定の動作をさせ、そのときの関節のトルクτ（第７のベクトル）をトルクセンサ２により計測する。

ステップＳ３２
制御装置４がロボットに所定の動作をさせた状態で、パラメータ同定部５は、そのときの関節の角度ｑ、角速度ｄｑ／ｄｔ、角加速度ｄ^２ｑ／ｄｔ^２を計測する。

ステップＳ３３
パラメータ同定部５は、計測した関節の角度ｑ、角速度ｄｑ／ｄｔ、角加速度ｄ^２ｑ／ｄｔ^２及び初期の力学パラメータを用いて逆動力学演算を行い、トルクτ_ｃａｌｃを算出する。なお、初期の力学パラメータは、例えばＣＡＤデータから見積もることが可能である。

ステップＳ３４
リグレッサ行列算出部３は、計測したトルクτから、リグレッサ行列Ｋを算出する。リグレッサ行列Ｋの算出方法は、実施の形態１（図１）で説明した通りであるので、説明を省略する。

ステップＳ３５
パラメータ同定部５は、リグレッサの疑似逆行列Ｋ^＋を求める。

ステップＳ３６
パラメータ同定部５は、例えば最小二乗法により、力学パラメータの差分ΔＰ（第８のベクトル）を求める。力学パラメータの差分ΔＰは、以下の式（２７）で表される。

ステップＳ３７
パラメータ同定部５は、初期の力学パラメータＰから力学パラメータの差分ΔＰを加えて、力学パラメータＰ_Ｔを算出する。力学パラメータＰ_Ｔは、以下の式（２８）で表される。

以上、本実施の形態によれば、初期の力学パラメータと力学パラメータの差分計算を利用して、ロボットのパラメータを簡便に求めることが可能である。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述の実施の形態にかかるリグレッサ行列の算出方法及び力学パラメータの同定方法は、７軸以外の数の多関節ロボットに適用できることは言うまでもない。

リグレッサ行列算出部３は、コンピュータなどのハードウェア資源で実現することも可能である。この場合、実施の形態１にかかるリグレッサ行列の算出の手順を規定するプログラムを、リグレッサ行列算出部３に実行させることで、同様にリグレッサ行列を算出することができる。

パラメータ同定部５は、コンピュータなどのハードウェア資源で実現することも可能である。この場合、実施の形態２にかかる力学パラメータの同定の手順を規定するプログラムを、パラメータ同定部５に実行させることで、同様に力学パラメータの同定を同定することができる。

１ロボット
２トルクセンサ
３リグレッサ行列算出部
４制御装置
５パラメータ同定部
１００、２００ロボットシステム

Claims

多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系において、
予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、
前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、
前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、
前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、
複数の前記第６のベクトルを合成して前記リグレッサ行列を算出する、
リグレッサ行列の算出方法。
トルクをτ、前記リグレッサ行列をＫ、前記第１のベクトルをＰとして、以下の式が成立し、

ｍ_ｉ（ｉは、１≦ｉ≦Ｎを満たす整数、Ｎは前記リンクの総数を示す正の整数）はｉ番目の前記リンクの質量とし、Ｓ_ｘｉ、Ｓ_ｙｉ及びＳ_ｚｉは、それぞれｉ番目の前記リンクの重心位置のｘ、ｙ及びｚ座標とし、Ｍ_ｘｘｉ、Ｍ_ｘｙｉ、Ｍ_ｘｚｉ、Ｍ_ｙｙｉ、Ｍ_ｙｚｉ、Ｍ_ｚｚｉは、それぞれｉ番目の前記リンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分であるとして、前記第１のベクトルＰは以下の式で表され、

ｉ番目の前記リンクについて、ｍ_ｉ、Ｓ_ｘｉ、Ｓ_ｙｉ、Ｓ_ｚｉ、Ｍ_ｘｘｉ、Ｍ_ｘｙｉ、Ｍ_ｘｚｉ、Ｍ_ｙｙｉ、Ｍ_ｙｚｉ及びＭ_ｚｚｉに、順に所定の増分を加えた逆動力学演算を行い、
前記順に所定の増分を加えた逆動力学演算を、前記複数のリンクのそれぞれについて行う、
請求項１に記載のリグレッサ行列の算出方法。
前記順に所定の増分を加えた逆動力学演算をＮ個の前記リンクのそれぞれについて行うことで得られる縦ベクトルを横方向に並べて、前記リグレッサ行列を算出する、
請求項２に記載のリグレッサ行列の算出方法。
前記多関節ロボットのそれぞれの関節のトルクを計測し、
前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度を算出し、
前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度と、前記第１のベクトルと、を用いて逆動力学演算を行って、前記第２のベクトルを算出し、
前記多関節ロボットのそれぞれの関節のトルクを計測し、
請求項１乃至３のいずれか一項に記載の前記リグレッサ行列の算出方法で前記リグレッサ行列を算出し、
算出した前記リグレッサ行列の疑似逆行列を算出し、
計測した前記トルクを示す第７のベクトルと前記第２のベクトルとの和に、前記リグレッサ行列の疑似逆行列を乗じて、力学パラメータの補正分を示す第８のベクトルを算出し、
前記第１のベクトルに前記第８のベクトルを加えて、前記多関節ロボットの力学パラメータを同定する、
力学パラメータの同定方法。
多関節ロボットと、
前記多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系の前記リグレッサ行列を算出するリグレッサ行列算出部と、
算出した前記リグレッサ行列を用いて、前記多関節ロボットの動作を制御する制御装置と、を備え、
前記リグレッサ行列算出部は、
予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、
前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、
前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、
前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、
複数の前記第６のベクトルを合成して前記リグレッサ行列を算出する、
ロボットシステム。