(57)【要約】【課題】ロボットの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出すること。【解決手段】予め定められた力学パラメータＰを用いて逆動力学演算を行って、トルクを示すベクトルＴｐを算出する。ベクトルＴｐと予め定められた一のリンクの力学パラメータＰのいずれかの成分に対し、所定の増分を加えたベクトルを算出する。算出した所定の増分を加えたベクトルを用いて逆動力学演算を行ってトルクを示すベクトルＴを算出し、ベクトルＴｐとベクトルＴとを用いて縦ベクトルを算出する。ベクトルＴの算出を、ベクトルＴｐと予め定められた一のリンクの力学パラメータＰの残りの成分と、複数のリンク残りのリンクとについて行い、ベクトルＴを複数算出し、縦ベクトルｋ１〜ｋ１０×Ｎを算出する。縦ベクトルｋ１〜ｋ１０×Ｎを合成して、リグレッサ行列を算出する。【選択図】図２
2015089585.tif 000002
【特許請求の範囲】
【請求項１】
多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系において、
予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、
前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、
前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、
前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、
複数の前記第６のベクトルを合成して前記リグレッサ行列を算出する、
リグレッサ行列の算出方法。
【請求項２】
トルクをτ、前記リグレッサ行列をＫ、前記第１のベクトルをＰとして、以下の式が成立し、
【数３１】
2015089585.tif 000034
ｍｉ（ｉは、１≦ｉ≦Ｎを満たす整数、Ｎは前記リンクの総数を示す正の整数）はｉ番目の前記リンクの質量とし、Ｓｘｉ、Ｓｙｉ及びＳｚｉは、それぞれｉ番目の前記リンクの重心位置のｘ、ｙ及びｚ座標とし、Ｍｘｘｉ、Ｍｘｙｉ、Ｍｘｚｉ、Ｍｙｙｉ、Ｍｙｚｉ、Ｍｚｚｉは、それぞれｉ番目の前記リンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分であるとして、前記第１のベクトルＰは以下の式で表され、
【数３２】
2015089585.tif 000035
ｉ番目の前記リンクについて、ｍｉ、Ｓｘｉ、Ｓｙｉ、Ｓｚｉ、Ｍｘｘｉ、Ｍｘｙｉ、Ｍｘｚｉ、Ｍｙｙｉ、Ｍｙｚｉ及びＭｚｚｉに、順に所定の増分を加えた逆動力学演算を行い、
前記順に所定の増分を加えた逆動力学演算を、前記複数のリンクのそれぞれについて行う、
請求項１に記載のリグレッサ行列の算出方法。
【請求項３】
前記順に所定の増分を加えた逆動力学演算をＮ個の前記リンクのそれぞれについて行うことで得られる縦ベクトルを横方向に並べて、前記リグレッサ行列を算出する、
請求項２に記載のリグレッサ行列の算出方法。
【請求項４】
前記多関節ロボットのそれぞれの関節のトルクを計測し、
前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度を算出し、
前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度と、前記第１のベクトルと、を用いて逆動力学演算を行って、前記第２のベクトルを算出し、
前記多関節ロボットのそれぞれの関節のトルクを計測し、
請求項１乃至３のいずれか一項に記載の前記リグレッサ行列の算出方法で前記リグレッサ行列を算出し、
算出した前記リグレッサ行列の疑似逆行列を算出し、
計測した前記トルクを示す第７のベクトルと前記第２のベクトルとの和に、前記リグレッサ行列の疑似逆行列を乗じて、力学パラメータの補正分を示す第８のベクトルを算出し、
前記第１のベクトルに前記第８のベクトルを加えて、前記多関節ロボットの力学パラメータを同定する、
力学パラメータの同定方法。
【請求項５】
多関節ロボットと、
前記多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系の前記リグレッサ行列を算出するリグレッサ行列算出部と、
算出した前記リグレッサ行列を用いて、前記多関節ロボットの動作を制御する制御装置と、を備え、
前記リグレッサ行列算出部は、
予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、
前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、
前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、
前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、
複数の前記第６のベクトルを合成して前記リグレッサ行列を算出する、
ロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はリグレッサ行列の算出方法、力学パラメータの同定方法及びロボットに関する。
【背景技術】
【０００２】
近年、手先で物体を把持して移動させるロボットの開発が行われている。ロボットを精度よく制御するため、様々な手法が提案されている。一例として、運動方程式をリンクの質量、重心位置と質量の積及び慣性テンソルを含む力学パラメータに対して、いわゆるリグレッサ行列を用いて線形な関係式で表す手法が知られている。
【０００３】
特許文献１には、リグレッサ行列を含む運動方程式を用いてロボットの姿勢制御を行う際の力学パラメータの同定方法が開示されている。この方法では、ロボットに様々な姿勢を取らせ、ロボットの位置及び姿勢、各関節の角度、床からの反力から、最小二乗法によりすべてのパラメータを同時に推定する。これのより、各関節に生じるトルクを計測することなく、力学パラメータを同定できる。
【０００４】
特許文献２には、関節に生じるトルク、外力及びベースリンクに関する運動方程式に基づいて、運動方程式の最小力学パラメータとトルクセンサのトルクのオフセット値を同時に同定できるトルクセンサ校正装置が開示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】国際公開２００９／１４７８７５
【特許文献２】特開２０１２−１７６４６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、発明者は、上述の手法には以下に示す問題点が有ることを見出した。リグレッサ行列は、多関節ロボットの関節位置や回転軸方向によって変化する。また、リグレッサ行列を求めようとすると、角速度ベクトルと外積を求めるなどの複雑な演算が必要である。そのため、特許文献１及び２の手法では、いずれもリグレッサ行列を導出する過程が煩雑である。その結果、力学パラメータを同定するためのシステムを開発するのに、多大のコストや開発時間を要してしまう。
【課題を解決するための手段】
【０００７】
本発明の一態様であるリグレッサ行列の算出方法は、多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系において、予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、複数の前記第６のベクトルを合成して前記リグレッサ行列を算出するものである。
【０００８】
上記のリグレッサ行列の算出方法においては、トルクをτ、前記リグレッサ行列をＫ、前記第１のベクトルをＰとして、以下の式が成立し、
【数１】
2015089585.tif 000003
ｍｉ（ｉは、１≦ｉ≦Ｎを満たす整数、Ｎは前記リンクの総数を示す正の整数）はｉ番目の前記リンクの質量とし、Ｓｘｉ、Ｓｙｉ及びＳｚｉは、それぞれｉ番目の前記リンクの重心位置のｘ、ｙ及びｚ座標とし、Ｍｘｘｉ、Ｍｘｙｉ、Ｍｘｚｉ、Ｍｙｙｉ、Ｍｙｚｉ、Ｍｚｚｉは、それぞれｉ番目の前記リンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分であるとして、前記第１のベクトルＰは以下の式で表され、
【数２】
2015089585.tif 000004
ｉ番目の前記リンクについて、ｍｉ、Ｓｘｉ、Ｓｙｉ、Ｓｚｉ、Ｍｘｘｉ、Ｍｘｙｉ、Ｍｘｚｉ、Ｍｙｙｉ、Ｍｙｚｉ及びＭｚｚｉに、順に所定の増分を加えた逆動力学演算を行い、
前記順に所定の増分を加えた逆動力学演算を、前記複数のリンクのそれぞれについて行うことが望ましい。
【０００９】
上記のリグレッサ行列の算出方法においては、前記順に所定の増分を加えた逆動力学演算をＮ個の前記リンクのそれぞれについて行うことで得られる縦ベクトルを横方向に並べて、前記リグレッサ行列を算出することが望ましい。また、一部のパラメータのみ同定する場合はそれに対応した縦ベクトルを横方向に並べ、あるいは最小力学パラメータを同定する場合はそれに対応するように変形してリグレッサ行列を算出することが望ましい。
【００１０】
本発明の一態様である力学パラメータの同定方法は、前記多関節ロボットのそれぞれの関節のトルクを計測し、前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度を算出し、前記多関節ロボットのそれぞれの関節の角度、角速度及び角加速度と、前記第１のベクトルと、を用いて逆動力学演算を行って、前記第２のベクトルを算出し、前記多関節ロボットのそれぞれの関節のトルクを計測し、請求項１乃至３のいずれか一項に記載の前記リグレッサ行列の算出方法で前記リグレッサ行列を算出し、算出した前記リグレッサ行列の疑似逆行列を算出し、計測した前記トルクを示す第７のベクトルと前記第２のベクトルとの和に、前記リグレッサ行列の疑似逆行列を乗じて、力学パラメータの補正分を示す第８のベクトルを算出し、前記第１のベクトルに前記第８のベクトルを加えて、前記多関節ロボットの力学パラメータを同定するものである。
【００１１】
本発明の一態様であるロボットシステムは、多関節ロボットと、前記多関節ロボットの力学パラメータからなるベクトルと、前記多関節ロボットのトルクからなるベクトルとが、リグレッサ行列を用いた線形形式で表現される系の前記リグレッサ行列を算出するリグレッサ行列算出部と、算出した前記リグレッサ行列を用いて、前記多関節ロボットの動作を制御する制御装置と、を備え、前記リグレッサ行列算出部は、予め定められた力学パラメータを示す第１のベクトルを用いて逆動力学演算を行って算出されたトルクを示す第２のベクトルを算出し、前記多関節ロボットの複数のリンクのうち、一のリンクの予め定められた力学パラメータを示す第３のベクトルのいずれかの成分に対し、所定の増分を加えて第４のベクトルを算出し、前記第４のベクトル用いて逆動力学演算を行って算出されたトルクを示す第５のベクトルを算出し、前記第２のベクトルと前記第５のベクトルと、を用いてリグレッサ行列の成分である第６のベクトルを算出し、前記第５のベクトルの算出を、前記第３のベクトルの残りの成分と、前記複数のリンク残りのリンクとについて行い、前記第６のベクトルを複数算出し、複数の前記第６のベクトルを合成して前記リグレッサ行列を算出するものである。
【発明の効果】
【００１２】
本発明によれば、ロボットアームの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出することができる。
【図面の簡単な説明】
【００１３】
【図１】実施の形態１にかかるロボットシステム１００の構成を模式的に示す図である。
【図２】実施の形態１にかかるリグレッサ行列Ｋの算出手順を示すフローチャートである。
【図３】逆動力学演算で得られた第１関節のトルクを示すグラフである。
【図４】逆動力学演算で得られた第４関節のトルクを示すグラフである。
【図５】実施の形態２にかかるロボット２００の構成を模式的に示す図である。
【図６】実施の形態２にかかる力学パラメータの同定の手順を示すフローチャートである。
【発明を実施するための形態】
【００１４】
実施の形態１
実施の形態１にかかるリグレッサ行列算出方法について説明する。図１は、実施の形態１にかかるロボットシステム１００の構成を模式的に示す図である。ロボットシステム１００は、ロボット１、トルクセンサ２、リグレッサ行列算出部３及び制御装置４を有する。
【００１５】
ロボット１は、多関節のロボットである。図１では、一例として、７関節のロボットを模式的に図示している。関節Ｊ１、Ｊ３、Ｊ５及びＪ７は、図１紙面に平行な方向を軸として回転する関節である。関節Ｊ２、Ｊ４及びＪ６は、図１紙面に垂直な方向を軸として回転する関節である。ＨＤは、物体の把持部である。
【００１６】
トルクセンサ２は、ロボット１が動作している際の各関節のトルクを測定し、ベクトルで示されるトルクτを出力する。以下の説明では、ベクトル及び行列については、太字で表す。
【００１７】
リグレッサ行列算出部３は、トルクτ、物理パラメータＰ及び関節角度ｑから、リグレッサ行列Ｋを算出する。
【００１８】
制御装置４は、制御信号ＣＯＮを与えることで、ロボット１の動作制御を行う。制御装置４には、ロボット１の動作制御に必要な情報が格納される。制御装置４は、リグレッサ行列算出部３に物理パラメータＰ及び関節角度ｑを出力し、リグレッサ行列算出部３が算出したリグレッサ行列Ｋを取得する。そして、制御装置４は、リグレッサ行列Ｋを含む運動方程式を用いて、ロボット１の運動を制御する。
【００１９】
次いで、リグレッサ行列算出部３が行うリグレッサ行列Ｋの算出の方法について説明する。多関節ロボットの運動方程式は、一般的に以下の式（１）で表される。
【数３】
2015089585.tif 000005
式（１）において、τは関節のトルクを示すベクトル、ｑは関節角度を示すベクトルである。
右辺第１項は、慣性力であり、慣性行列Ｍと関節の角加速度ベクトルｄ２ｑ／ｄｔ２（ｑの２階時間微分、式ではｑの上に微分階数を示す黒点を２つ付して表示する）積として表される。
右辺第２項の行列Ｈは遠心力及びコリオリ力から構成される非線形項であり、関節角度ｑ及び関節の角速度ｄｑ／ｄｔ（ｑの１階時間微分、式ではｑの上に微分階数を示す黒点を１つ付して表示する）を変数とする行列で表される。
右辺第３項の行列Ｇは重力を表し、関節角度ｑを変数とする行列である。
【００２０】
式（１）において、考慮すべき力学パラメータは、各関節の質量ｍ、重心位置ｓ及び慣性テンソルＭである。これらの力学パラメータを同定するため、Ｎ（Ｎは、１以上の整数）個のリンクを有する多関節ロボットの力学パラメータＰを、以下の式（２）のように縦ベクトルとして定義する。
【数４】
2015089585.tif 000006
式（２）において、ｍｉ（ｉは、１≦ｉ≦Ｎを満たす整数）はｉ番目のリンクの質量を示す。Ｓｘｉ、Ｓｙｉ及びＳｚｉは、それぞれｉ番目のリンクの重心位置のｘ、ｙ及びｚ座標を示す。Ｍｘｘｉ、Ｍｘｙｉ、Ｍｘｚｉ、Ｍｙｙｉ、Ｍｙｚｉ、Ｍｚｚｉは、それぞれｉ番目のリンクの慣性テンソルのｘｘ成分、ｘｙ成分、ｘｚ成分、ｙｙ成分、ｙｚ成分、ｚｚ成分を示す。つまり、各リンクについて１０個の力学パラメータが存在し、力学パラメータＰは合計で（１０×Ｎ）個の力学パラメータを含む。
【００２１】
力学パラメータＰを用いて、式（１）を変形すると、以下の式（３）が得られる。
【数５】
2015089585.tif 000007
式（３）のＫは、いわゆるリグレッサ行列と称される行列である。リグレッサ行列Ｋは、（１０×Ｎ）個の列ベクトルｋ１、ｋ２、・・・、ｋ１０Ｎを用いて、以下の式（４）で表される。なお、列ベクトルｋ１、ｋ２、・・・、ｋ１０Ｎは、それぞれ縦ベクトルである。
【数６】
2015089585.tif 000008
【００２２】
リグレッサ行列Ｋが力学パラメータＰに対して線形な形式で表すことができれば、力学パラメータＰは以下の式（５）で表される。
【数７】
2015089585.tif 000009
式（５）のＫ＋は、リグレッサ行列Ｋの疑似逆行列である。
【００２３】
本実施の形態では、複雑な演算を要することなくリグレッサ行列Ｋを求めるため、逆動力学演算を用いた簡素な演算でリグレッサ行列Ｋを求める方法について説明する。図２は、実施の形態１にかかるリグレッサ行列Ｋの算出手順を示すフローチャートである。
【００２４】
ステップＳ１１
ＣＡＤ情報などから、多関節ロボットの力学パラメータＰのおおよその値が判明している場合がある。まず、判明している力学パラメータＰ（第１のベクトル）を用いて、逆動力学演算を行う。この場合に、逆動力学演算を行って得られる結果Ｔｐ（第２のベクトル）を、以下の式（６）で表す。
【数８】
2015089585.tif 000010
【００２５】
ステップＳ１２
次いで、各リンクについて、力学パラメータの成分を順に微小増加させて逆動力学演算を行った結果を算出する。まず、１番目のリンクについて力学パラメータの成分を順に微小増加させるため、リンクの番号を示すパラメータｉを初期値である「１」に設定する。以下、説明の簡略化のため、ステップＳ１３〜Ｓ２２に関しては、ｉ＝１の場合について説明する。
【００２６】
ステップＳ１３
１番目のリンクの１個目の力学パラメータＰ（第３のベクトル）の値である質量ｍ１をΔｍ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ１＋Δｍ１）（第５のベクトル）を算出する。Ｔ（１，ｍ１＋Δｍ１）は、以下の式（７）で表される。
【数９】
2015089585.tif 000011
この場合、リグレッサ行列Ｋの第１列ｋ１（第６のベクトル）は、以下の式（８）で表される。
【数１０】
2015089585.tif 000012
【００２７】
ステップＳ１４
１番目のリンクの２個目の力学パラメータＰの値ｍ１ｓｘ１をｍ１Δｓｘ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ１（ｓｘ１＋Δｓｘ１））を算出する。Ｔ（１，ｍ１（ｓｘ１＋Δｓｘ１））は、以下の式（９）で表される。
【数１１】
2015089585.tif 000013
この場合、リグレッサ行列Ｋの第２列ｋ２（第６のベクトル）は、以下の式（１０）で表される。
【数１２】
2015089585.tif 000014
【００２８】
ステップＳ１５
１番目のリンクの３個目の力学パラメータＰの値ｍ１ｓｙ１をｍ１Δｓｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ１（ｓｙ１＋Δｓｙ１））を算出する。Ｔ（１，ｍ１（ｓｙ１＋Δｓｙ１））は、以下の式（１１）で表される。
【数１３】
2015089585.tif 000015
この場合、リグレッサ行列Ｋの第３列ｋ３（第６のベクトル）は、以下の式（１２）で表される。
【数１４】
2015089585.tif 000016
【００２９】
ステップＳ１６
１番目のリンクの４個目の力学パラメータＰの値ｍ１ｓｚ１をｍ１Δｓｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ｍ１（ｓｚ１＋Δｓｚ１））を算出する。Ｔ（１，ｍ１（ｓｚ１＋Δｓｚ１））は、以下の式（１３）で表される。
【数１５】
2015089585.tif 000017
この場合、リグレッサ行列Ｋの第４列ｋ４（第６のベクトル）は、以下の式（１４）で表される。
【数１６】
2015089585.tif 000018
【００３０】
ステップＳ１７
１番目のリンクの５個目の力学パラメータＰの値Ｍｘｘ１をΔＭｘｘ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｘｘ１）を算出する。Ｔ（１，ΔＭｘｘ１）は、以下の式（１５）で表される。
【数１７】
2015089585.tif 000019
この場合、リグレッサ行列Ｋの第５列ｋ５（第６のベクトル）は、以下の式（１６）で表される。
【数１８】
2015089585.tif 000020
【００３１】
ステップＳ１８
１番目のリンクの６個目の力学パラメータＰの値Ｍｘｙ１をΔＭｘｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｘｙ１）を算出する。Ｔ（１，ΔＭｘｙ１）は、以下の式（１７）で表される。
【数１９】
2015089585.tif 000021
この場合、リグレッサ行列Ｋの第６列ｋ６（第６のベクトル）は、以下の式（１８）で表される。
【数２０】
2015089585.tif 000022
【００３２】
ステップＳ１９
１番目のリンクの７個目の力学パラメータＰの値Ｍｘｚ１をΔＭｘｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｘｚ１）を算出する。Ｔ（１，ΔＭｘｚ１）は、以下の式（１９）で表される。
【数２１】
2015089585.tif 000023
この場合、リグレッサ行列Ｋの第７列ｋ７（第６のベクトル）は、以下の式（２０）で表される。
【数２２】
2015089585.tif 000024
【００３３】
ステップＳ２０
１番目のリンクの８個目の力学パラメータＰの値Ｍｙｙ１をΔＭｙｙ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｙｙ１）を算出する。Ｔ（１，ΔＭｙｙ１）は、以下の式（２１）で表される。
【数２３】
2015089585.tif 000025
この場合、リグレッサ行列Ｋの第８列ｋ８（第６のベクトル）は、以下の式（２２）で表される。
【数２４】
2015089585.tif 000026
【００３４】
ステップＳ２１
１番目のリンクの９個目の力学パラメータＰの値Ｍｙｚ１をΔＭｙｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｙｚ１）を算出する。Ｔ（１，ΔＭｙｚ１）は、以下の式（２３）で表される。
【数２５】
2015089585.tif 000027
この場合、リグレッサ行列Ｋの第９列ｋ９（第６のベクトル）は、以下の式（２４）で表される。
【数２６】
2015089585.tif 000028
【００３５】
ステップＳ２２
１番目のリンクの１０個目の力学パラメータＰの値Ｍｚｚ１をΔＭｚｚ１だけ変化させたベクトル（第４のベクトル）を用いて逆動力学演算を行った場合の結果Ｔ（１，ΔＭｚｚ１）を算出する。Ｔ（１，ΔＭｚｚ１）は、以下の式（２５）で表される。
【数２７】
2015089585.tif 000029
この場合、リグレッサ行列Ｋの第１０列ｋ１０（第６のベクトル）は、以下の式（２６）で表される。
【数２８】
2015089585.tif 000030
【００３６】
ステップＳ２３
次いで、リンクの番号を示すパラメータｉが「Ｎ」であるか（ｉ＝Ｎ）を判定する。
【００３７】
ステップＳ２４
リンクの番号を示すパラメータｉが「Ｎ」でない場合（ｉ≠Ｎ）、ｉに１を加算（ｉ＝ｉ＋１）し、ステップＳ１２に戻る。
【００３８】
ステップＳ２５
リンクの番号を示すパラメータｉが「Ｎ」である場合（ｉ＝Ｎ）、ステップＳ１２〜Ｓ２３がＮ回繰り返されることで、（１０×Ｎ）個の列ベクトルｋ１〜ｋ１０Ｎが算出されたこととなる。つまり、リグレッサ行列を構成する列ベクトルが全て算出されているので、列ベクトルｋ１〜ｋ１０Ｎを組み合わせて、リグレッサ行列Ｋを算出することができる。
【００３９】
以上、上述の方法によれば、差分計算を行う簡素な演算により、リグレッサ行列を算出することができる。これにより、ロボットの運動方程式に含まれるリグレッサ行列を、簡素な演算で導出することができる。
【００４０】
次いで、本実施の形態で算出したリグレッサ行列を用いた場合の力学パラメータの同定結果について説明する。ここでは、ロボットは7関節の冗長ロボットを想定し、仮想的にリンクの質量を変化させたシミュレーションにより、力学パラメータを同定した。7関節の冗長ロボットの全体の質量は約９．５（ｋｇ）であり、肩関節から手先位置までの長さは約６５（ｃｍ）である。
【００４１】
同定に用いるロボットの姿勢は、同定に必要な情報を含むようになるべくランダムな姿勢が望ましい。そのため、各関節の目標軌道は振幅１（ｒａｄ）の三角関数で生成した。また、その振幅は肩関節から手先関節に向けて０．２５Ｈｚ、０．２６Ｈｚ、０．２７Ｈｚ、０．２８Ｈｚ、０．２９Ｈｚ、０．３０Ｈｚ、０．３１Ｈｚとした。この目標軌道の中で、１秒から６．９秒までの６０種類の姿勢（０．１秒間隔）を同定に利用した。
【００４２】
以上の条件下で、第４リンク（肘）と第７リンク（手）にそれぞれ１．０（ｋｇ）の質量を追加して行った逆動力学演算の結果をセンサ値とした。以下、シミュレーションであそれにより得られた同定結果を表に示す。
【表１】
2015089585.tif 000031
【００４３】
以上の同定結果を適用して、再度逆動力学演算を行った。図３は、逆動力学演算で得られた第１関節のトルクを示すグラフである。図４は、逆動力学演算で得られた第４関節のトルクを示すグラフである。
【００４４】
図３及び４に示すように、同定に用いた姿勢を取得した６．９秒以降ではトルクのセンサ値（measured torque）は、トルク計算値（recalculated torque）に一致している。よって、本実施の形態で得られるリグレッサ行列を用いて、力学パラメータを正確に同定できていることが理解できる。
【００４５】
以上、本実施の形態についてまとめると、上述した従来技術では、リグレッサ行列を算出しようとした場合、角速度ベクトルと外積を求めるソフトなど、リグレッサ行列を演算するための専用ソフトが多く必要となる。そのため、力学パラメータを同定するためのシステムを開発するために多大なコストや開発時間を要してしまう。
【００４６】
これに対し、本実施の形態では、逆動力学演算を用いてリギレッサ行列を算出することができる。そのため、リグレッサ行列を算出するために必要な専用ソフト等を減らすことができる。従って、力学パラメータを同定するためのシステムを開発するために要するコストや時間を低減することができる。また、算出したリグレッサ行列算出方法を用いて力学パラメータを同定することができる。
【００４７】
実施の形態２
実施の形態２にかかるリグレッサ行列算出方法について説明する。図５は、実施の形態２にかかるロボット２００の構成を模式的に示す図である。ロボット２００は、実施の形態１にかかるロボットシステム１００に、パラメータ同定部５を追加した構成を有する。パラメータ同定部５は、ロボット１の力学パラメータを同定することができる。
【００４８】
以下、実施の形態２にかかる力学パラメータの同定方法について説明する。図６は、実施の形態２にかかる力学パラメータの同定の手順を示すフローチャートである。
【００４９】
ステップＳ３１
制御装置４はロボットに所定の動作をさせ、そのときの関節のトルクτ（第７のベクトル）をトルクセンサ２により計測する。
【００５０】
ステップＳ３２
制御装置４がロボットに所定の動作をさせた状態で、パラメータ同定部５は、そのときの関節の角度ｑ、角速度ｄｑ／ｄｔ、角加速度ｄ２ｑ／ｄｔ２を計測する。
【００５１】
ステップＳ３３
パラメータ同定部５は、計測した関節の角度ｑ、角速度ｄｑ／ｄｔ、角加速度ｄ２ｑ／ｄｔ２及び初期の力学パラメータを用いて逆動力学演算を行い、トルクτｃａｌｃを算出する。なお、初期の力学パラメータは、例えばＣＡＤデータから見積もることが可能である。
【００５２】
ステップＳ３４
リグレッサ行列算出部３は、計測したトルクτから、リグレッサ行列Ｋを算出する。リグレッサ行列Ｋの算出方法は、実施の形態１（図１）で説明した通りであるので、説明を省略する。
【００５３】
ステップＳ３５
パラメータ同定部５は、リグレッサの疑似逆行列Ｋ＋を求める。
【００５４】
ステップＳ３６
パラメータ同定部５は、例えば最小二乗法により、力学パラメータの差分ΔＰ（第８のベクトル）を求める。力学パラメータの差分ΔＰは、以下の式（２７）で表される。
【数２９】
2015089585.tif 000032
ステップＳ３７
パラメータ同定部５は、初期の力学パラメータＰから力学パラメータの差分ΔＰを加えて、力学パラメータＰＴを算出する。力学パラメータＰＴは、以下の式（２８）で表される。
【数３０】
2015089585.tif 000033
【００５５】
以上、本実施の形態によれば、初期の力学パラメータと力学パラメータの差分計算を利用して、ロボットのパラメータを簡便に求めることが可能である。
【００５６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述の実施の形態にかかるリグレッサ行列の算出方法及び力学パラメータの同定方法は、７軸以外の数の多関節ロボットに適用できることは言うまでもない。
【００５７】
リグレッサ行列算出部３は、コンピュータなどのハードウェア資源で実現することも可能である。この場合、実施の形態１にかかるリグレッサ行列の算出の手順を規定するプログラムを、リグレッサ行列算出部３に実行させることで、同様にリグレッサ行列を算出することができる。
【００５８】
パラメータ同定部５は、コンピュータなどのハードウェア資源で実現することも可能である。この場合、実施の形態２にかかる力学パラメータの同定の手順を規定するプログラムを、パラメータ同定部５に実行させることで、同様に力学パラメータの同定を同定することができる。
【符号の説明】
【００５９】
１  ロボット
２  トルクセンサ
３  リグレッサ行列算出部
４  制御装置
５  パラメータ同定部
１００、２００  ロボットシステム
【図１】
2015089585.tif 000036
【図２】
2015089585.tif 000037
【図３】
2015089585.tif 000038
【図４】
2015089585.tif 000039
【図５】
2015089585.tif 000040
【図６】
2015089585.tif 000041