JP5288418B2 - 力学パラメータの同定法 - Google Patents

Description

本発明は、力学パラメータの同定法に係り、詳しくは、人間やヒューマノイドロボットなど脚を持つ移動システム等の全身の質量・重心・慣性テンソルといった力学パラメータを同定する方法に関するものである。
本明細書において、力学パラメータ（inertial parameters or dynamic parameters）には、全力学パラメータ、すなわち、一般に、剛体１個あたり、質量１、重心位置と質量の積３、慣性テンソル６の１０個のパラメータからなる通常の力学パラメータφ、および、最小力学パラメータ（base inertial parameters, or minimal set of inertial parameters）φ_Ｂが含まれる。
最小力学パラメータφ_Ｂを数値的・解析的に計算する手段は確立されており、例えば、非特許文献１乃至３に記載されている。
運動方程式は、最小力学パラメータφ_Ｂのみで表すことができる。

ヒューマノイドロボットは、労働力や介護など人間社会への進出が今後期待されている。ヒューマノイドは荷物を持つなど、運動中に自分自身の質量・重心などのパラメータが変化するため、高度な運動制御を安全に実現するためには同定技術は重要である。

力学同定はロボティクスにおける大きな研究分野であり、多分野への応用も見られるが、ヒューマノイドへの応用例は少ない。これには以下のような理由が考えられる。力学同定を行う場合、各関節の運動方程式の構造を利用することが多く、一般化座標、床反力、関節トルクのセンサ情報を必要とする。６軸力センサやフォースプレート等を利用して床反力を計測することは比較的容易であるが、多くのヒューマノイドは各関節にトルクセンサを搭載していないため正確な関節トルク計測は困難である。

力学パラメータはＣＡＤから概算可能であるが、電装系やアクチュエータも含めたモデル化は困難である。またＣＡＤなどに基づく理論的なモデル化に留まらず、力学パラメータを同定によって実測することが望ましい。

また、人体における力学パラメータは、リハビリなど医療の定期診断やスポーツなどの身体解析において利用価値が高く、力学パラメータの同定を被験者に負担を与えずに行うことが望まれる。
Gautier M. Numerical calculation of the base inertial parameters of robots. Proc. of the IEEE Int. Conf. on Robotics and Automation, Vol. 2, pp. 1020.1025, May 1990. H. Kawasaki, Y. Beniya, and K. Kanzaki. Minimum dynamics parameters of tree structure robot models. In Int. Conf. of Industrial Electronics, Control and Instrumentation, Vol. 2, pp. 1100. 1105, 1991. W. Khalil and F. Bennis. Symbolic calculation of the base inertial parameters of closed-loop robots. The Int. J. of Robotics Research, Vol. 14(2), pp. 112.128, April 1995.

本発明の目的は、関節トルク計測が不要な力学パラメータの同定法を提供することにある。

本発明は、環境に固定されていない多関節リンク機構で表現される被験体の運動を、任意に選択されたベースリンクの空間運動とリンク系の関節運動で表し、
ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、ならびに環境との接触点に働く外力、の各計測値と、
数１に示す多関節リンク機構のベースリンクに関する運動方程式と、
を用いて、多関節リンク構造の最小力学パラメータφ_Ｂを同定する方法、である。
ここで、
Ｙ_Ｂ１は、ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、から求められる観測行列、
Ｆ_ｋは、接触点ｋに働く外力、
Ｋ_ｋ１は、接触点ｋにおける外力を一般化力へ変換する行列、
Ｎ_ｃは、多関節リンク構造と環境との接触点の総数、
である。

ベースリンクは、多関節リンク構造を表現する木構造のルート（根）に相当するリンクである。典型的な態様例では、ルートリンクは体幹リンク、体幹が複数のリンクからなる場合はその中の１つ、あるいは、腰関節あるいは下腹部に対応する部位に設定される。
１つの態様では、前記観測行列は、ベースリンクの一般化座標（６自由度の一般化座標ベクトル）およびその微分（導関数）、及び、各関節角度及び各関節角度の速度、加速度の関数であるリグレッサ行列である。
１つの態様では、接触点ｋにおける外力を一般化力へ変換する行列は、点ｋへのヤコビ行列の転置行列である。

１つの態様では、前記被験体は、ヒューマノイドである。
１つの態様では、前記被験体は、人体である。人体は複数の関節を備えた剛体リンク構造としてモデル化することができる。
また、本発明は、ヒューマノイドや人体に限らず、脚を持つ移動システムに応用可能である。ヒューマノイドロボットや４足歩行ロボットなどのロボットの全身の各部位の力学パラメータ（質量、重心位置、慣性テンソル等）などの情報を運動情報（例えば、床反力計の上を移動したときの情報）から推定することができる。
本発明において、被験体が環境と接触した状態での運動であれば、力学パラメータ同定に用いられる被験体の運動の種類は限定されず、歩行、サイドステップ、体幹の折り曲げ、スクワット等が例示される。典型的には、全重心を大きく変化させる運動の組み合わせが用いられる。

１つの態様では、前記外力は床反力である。床反力は、６軸の力を検出することができる床反力計測手段によって取得することができ、このような手段は当業者によく知られている。より具体的には、床反力は、床面に敷設した力センサ（例えば、フォースプレート）、あるいは、被験体の脚部に装着した力センサ（例えば靴底力センサ）、によって計測することが可能である。

１つの態様では、前記各関節角度は、各関節に搭載されているエンコーダによって計測される。例えば、ヒューマノイドの場合、各関節にエンコーダが搭載されている。
１つの態様では、前記各関節角度は、被験体の各リンクにマーカを配置し、モーションキャプチャによって取得されたマーカ位置の時系列データから逆運動学計算によって取得される。モーションキャプチャによって取得した被験体の運動データから各関節角度の時系列データを取得することは当業者によく知られている。

１つの態様では、ベースリンクの一般化座標は、ベースリンクに搭載されたジャイロ加速度センサによって計測される。
１つの態様では、ベースリンクの一般化座標は、被験体のベースリンクにマーカを配置し、モーションキャプチャによって取得されたマーカ位置の時系列データからベースリンクの位置・姿勢を計算することで取得される。

１つの態様では、床面に敷設した床反力計測用のフォースプレート上で運動する被験体を、床反力の計測と同期するモーションキャプチャシステムのカメラで撮影し、
前記床反力は、前記フォースプレートにより計測され、
前記ベースリンクの一般化座標、前記各関節角度、は、モーションキャプチャによって取得されたマーカ位置の時系列データから計算される。
こうすることで、被験体に負担をかけることなく、力学パラメータ同定に必要な情報を取得することができる。
本発明において、必要な情報を取得するためのモーションキャプチャシステムは一つの好適な例ではマーカを被験体に配置するタイプの光学式モーションキャプチャシステムであるが、モーションキャプチャ方式はこのタイプの光学式のみには限定されない。
１つの態様では、本発明を実行するためのハードウェア構成としては、マーカが付された被験者を撮影する複数の撮像手段（カメラ）と、床反力計測手段（フォースプレート）と、一つ又は複数のコンピュータ装置とを含み、コンピュータ装置は、各種計算を行う演算処理部、入力部、出力部、表示部、各種データを格納する記憶部を備えている。

１つの態様では、ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、ならびに環境との接触点に働く外力、の計測時に、前記最小力学パラメータを実時間で計算する。
１つの態様では、前記被験体は人体であり、被験者の運動時に前記最小力学パラメータを実時間で計算する。
最小力学パラメータの実時間計算は、例えば、逐次的に最小二乗法を解くことで可能であることが当業者に理解される。後述する実施形態では、いわゆる忘却機能付き逐次的一般化最小二乗法を用いて実時間計算を行っている。

１つの態様では、前記被験体のリンク機構モデルを表示部に表示し、各リンクの最小力学パラメータの同定の程度を、表示されたリンク機構モデルにおいてリンク毎に同定の程度に応じて視覚的に表示する。
１つの態様では、前記最小力学パラメータの同定の程度に応じてリンクの色を変化させて表示する。
同定の程度を色の変化で認識できるものであれば、色の変化の組み合わせは任意である。また、色の透明度や輝度などを変化させてもよい。
また、前記最小力学パラメータの同定の程度を濃淡やテクスチャの変化で視覚的に表示してよい。
同定の程度を理解できるのであれば他の形態でもよい。例えば、表示用のヒューマンフィギュアの各リンクの大きさを変化させるなど、形状変化によって同定の程度を表示してもよい。
１つの態様では、前記最小力学パラメータの同定が不十分なリンクを被験者に視覚的に認識させることで、当該リンクに対応する身体部位の運動を促す。
１つの態様では、各リンク毎に、各最小力学パラメータの同定の程度を求め、求めた複数の最小力学パラメータの同定の程度を用いて、各リンクとしての最小力学パラメータの同定の程度を算出する。
１つの態様では、各リンクにおいて、各最小力学パラメータを、求めた同定の程度に応じて３つのグループに分類して各グループに３原色（典型的にはＲＧＢ）の各色を割り当て、各グループに属する最小力学パラメータの数の割合に基づいて表示部に表示された当該リンクの画素における各色の濃度値を決定する。
個々の最小力学パラメータの同定の程度を用いて、リンク全体としての同定の程度をどのように決定するかについては、他にも手法があり得ることが当業者に理解される。
１つの態様では、各最小力学パラメータの同定の程度の指標は、各最小力学パラメータの推定値の時系列データの各推定値と当該最小力学パラメータの目標値との誤差の分散である。
すなわち、最小力学パラメータの推定時に、推定された各最小力学パラメータが目標値に対してどれくらいの誤差を持っているか、すなわち、ある時刻ｔまでに推定された誤差の分散を逐次計算して、分散が小さくなったならばパラメータが良く同定できている判定する。
逐次推定されていく最小力学パラメータの推定値と目標値（真値）との誤差の分散を計算する時に、被験体がロボットであればCAD等から最小力学パラメータの真値が分かるが、被験体が人体の場合には、最小力学パラメータの真値は一般に分からない。そこで、最小力学パラメータの推定値から外力の推定値を計算して、この外力の推定値と実際の外力の計測値との誤差を求める。求めた外力の推定値と外力の計測値との誤差と、運動データを行列表現したリグレッサ行列Ｙから、間接的に推定された最小力学パラメータの誤差の分散を計算することができる。すなわち、外力の誤差から、各々の最小力学パラメータの誤差の分散を直接計算する。
１つの態様では、前記指標としての誤差の分散は、相対標準偏差である。
また、データベースや文献から得られた値を最小力学パラメータの真値と仮定して、この仮想真値を目標値として用いても良い。
同定の程度を確認する手段は、上述のような「パラメータの推定値の分散を確認する」ことに限定されない。
パラメータの真値が分かっている場合は、推定値と真値を直接比較することで同定の程度を確認してもよい。この場合、現時点での値を比較する。また、上記のデータベースや文献から得られた仮想真値を用いてもよい。
床反力の推定値と床反力の計測値とが一致しているか否かを確認してもよい。また、床反力の推定地値と計測値はそれぞれベクトルとして矢印で表示部に表示され、両ベクトルがよく一致していれば、同定の程度は良いと判断できる。この場合、現時点での外力の計測値と推定値を比較すればよいが、ある一定の時系列フレームの全てにおいて外力が正しく推定できているかどうかを確認することが望ましい。

１つの態様では、
前記被験体のリンク機構モデルの各リンクの全力学パラメータの事前情報を用意し、
前記同定された最小力学パラメータを満たすように前記事前情報を補正することで、前記事前情報と前記同定された最小力学パラメータとから全力学パラメータを推定する。
１つの態様では、全力学パラメータの事前情報は、特開２００８−７７５５１号に開示された手法を用いることができる。この手法では、身体の各部位の寸法を表す変数から説明変数を選択し、他の変数を目的変数として、説明変数の実測値に基づいて目的変数の値を推定し、剛体リンク機構でモデル化した身体の各リンクの体積を前記説明変数及び前記目的変数の少なくとも一部を用いて算出し、身体の総体積と体重とから比重を算出し、算出された比重から各リンクの質量及び慣性モーメントを求める。身体各部位の寸法を含むデータベースと、被験者の身体の数箇所を計測したデータを用いて、各部位の全力学パラメータを推定することができる。光学式モーションキャプチャを用いる場合には、被験者に装着する光学式マーカの位置を用いて所望の身体部位の寸法を計測することができる。
全力学パラメータの事前情報は、人間の質量データベースや文献の値をそのまま用いるものでもよい。

本発明では、各関節のトルクを計測せずに、多関節リンク構造のベースリンクの運動方程式のみから、全外力を計測することで、多関節リンク構造の全身の最小力学パラメータが同定可能である。
本発明の力学パラメータの同定には、各関節トルクの計測が不要であるため、関節にトルクセンサが搭載されていないシステムに対しても、本発明の同定法を利用することが可能であるため、既存のシステムに対して広範囲に本技術を利用できる。また、本発明は、完成機（電気系の配線や追加部品が装着されたロボット）を用いた力学パラメータ同定を行うことが可能である。

本発明は、外力計測に基づくため摩擦などの内力に影響されない。床反力計で外部から計測する場合は、接地状態に依存されずに同定できる。また、ベースリンクの運動方程式しか同定に利用しないので、全運動方程式を用いる通常の手法よりも、同定にかかる計算量が少ない。

本発明は、人体の計測にも利用可能である。本手法は、人間が床反力計の上を移動する情報から全身の各部位の力学パラメータを推定することができるので、被験者に対しての負荷が軽く、簡単な運動計測で力学パラメータを同定することが可能である。リハビリなど医療の定期診断やスポーツなどの身体解析において特に利用価値が高い。

被験者の運動計測中に実時間で同定を行い、各身体部位の同定結果を視覚化することで、被験者に同定をする上で適切な運動を直感的に理解させることができ、被験者は同定が不十分な部位を特定して、その部位を十分に動かすことで、全身を同定するのに十分な運動を生成できる。

全力学パラメータの事前情報と、推定された最小力学パラメータと、からより精度の高い全力学パラメータ（体の部位毎と剛体としてモデル化した場合に、１個の剛体につき、質量１、重心位置と質量の積３、慣性テンソル６の１０個のパラメータ）を推定することができる。

本発明の力学パラメータ同定を説明する概略図である。 ベースリンクから末端リンクへの最小力学パラメータの編成を示す。 小型ヒューマノイドロボットＵＴ−μ２を示す。 小型ヒューマノイドロボットＵＴ−μ２を用いた実験の様子を示す。 シミュレーションデータによる同定結果を示す。 実験結果（４歩歩行）を示す。 実験結果（折り曲げ動作）を示す。 ３４自由度を備えた人体構造モデルを示す。 モーションキャプチャに用いた３５個のマーカセットの位置を示す人体骨格モデルである。 力学パラメータの実時間同定と視覚化アプリケーションを説明する図である。図中の較正データ(calibration data)は、モーションキャプチャのマーカの計測値から計算された被験体の各部位の長さに関する情報を意味する。 人体の各リンクの幾何形状をモデル化したものを示す図である。 被験者の運動計測時の力学パラメータの実時間計算において、被験体のリンク機構モデルにおいて、各リンクの力学パラメータの同定の程度を、リンク毎に同定の程度に応じて視覚的に表示した図を示す。 図９の最初の２フレームを拡大して示す図である。 図９の真ん中の２フレームを拡大して示す図である。 図９の最後の２フレームを拡大して示す図である。

［Ａ］関節トルクの計測を必要としない運動学パラメータの同定
二足歩行型システム等の脚を備えた移動システムの運動方程式は、式（１）に示すモデルで表現できる。このような移動システムとしては、典型的には、ヒューマノイドや人体が含まれる。式（１）のモデルにおいて、上段は、ベースリンクの自由運動を表し、下段は、全身を構成するＮ体（Ｎ：肢、体幹、頭部等であって、選択されたモデルによって異なる）の連鎖運動を表している。

ベースリンクは基底であり、どのリンクにも設定可能であるが、一般的に体幹リンク（腰関節の近傍）とする。多リンク系の運動方程式は、質量・重心位置と質量の積・慣性テンソルなどの力学パラメータに対して、線形な関係式で表わすことができる。このとき、式（１）は式（２）のように変形できる。
力学パラメータφは、
と表すことができる、
φ_ｊは、各リンクＢ_ｊ（ｊ＝０からＮ）の力学パラメータのベクトルであり、
と表すことができる。
ここで、
ｍ_ｊは質量、
Ｉ_ｊ，ｘｘ，Ｉ_ｊ，ｙｙ，Ｉ_ｊ，ｚｚ，Ｉ_ｊ，ｙｚ，Ｉ_ｊ，ｚｘ，Ｉ_ｊ，ｘｙは、慣性行列Ｉ_ｊの６独立要素、
ｍｓ_ｊ，ｘ，ｍｓ_ｊ，ｙ，ｍｓ_ｊ，ｚは、ベクトルｍｓ_ｊの各要素であり、重心位置と質量の積、
である。

一般的に力学パラメータφは冗長であり、一意に同定できず、動力学モデルの計算に必要最小限なパラメータに定式化する必要がある。この最小力学パラメータを数値的・解析的に計算する手段は確立されており、例えば、非特許文献１乃至３に記載されている。

最小力学パラメータφ_Bを用いると、式（５）が得られる。
式（５）から、例えば、最小二乗法を用いて、φ_Bの推定値φ（ハット）_Bを同定することができる。

ここで、式（５）を用いて最小力学パラメータを同定するためには、ベースリンクの位置・姿勢、各関節の座標、接触力、各関節の関節トルクの情報が必要となる。すなわち、通常の力学同定には、すべての関節トルクの計測が必要となるが、一般的には関節トルクの正確な計測は困難である。例えば、多くのヒューマノイドは各関節にトルクセンサを搭載していない。またアクチュエータの入力から出力トルクの推定も可能だが、ギアなどの動力伝達部は複雑であり、正確なトルク計測は困難である。その一方で、足首に６軸力センサを搭載するなど、床反力を計測できるケースは多い。そこで、トルク計測の代わりに、床反力計測を用いた同定を考える。

式（５）の上段のみ、すなわちベースリンクの運動方程式のみに注目した。
式（６）の大きな特徴は、一般化力がゼロという力学的拘束条件である。式（６）中には、関節トルクτは含まれず、また一般化力は常にゼロなので、どちらも計測不要である。よって式（６）を利用した同定に必要な情報は、各接触点ｋとそこに働く外力Ｆ_ｋ、および関節角度θとベースリンクの一般化座標ｑ_０となる（図１参照）。

これらの情報は、エンコーダ、６軸力センサ、加速度センサ、ジャイロセンサ、モーションキャプチャシステム等で計測可能であり、多くのヒューマノイドや人体に対してこの同定法が適用できる。
また、式（６）の右辺は、ベースリンク原点にかかる６軸の全外力Ｆとなり、Ｆを外部から直接計測できる場合は、片脚支持、両脚支持というような接地条件によらずに同定できる。さらにベースリンクの同定は、式（６）中に個々の関節の摩擦力などを含まないため、これらの影響を受けないという大きな利点もある。

一方、式（６）だけを用いて式（５）の最小力学パラメータφ_Bを同定するためには、φ_Bが式（６）においても最小力学パラメータである必要がある。
ベースリンクの運動方程式における可同定性は、ベースリンクから末端リンクへ向かって最小力学パラメータを編成していく議論である。
図２の左側の鎖線で囲まれたサブリンク系Ｃ_j−1は、すでに最小力学パラメータの編成が終了しているとする。次に、図中の右側のように新しくリンクjを追加して、サブリンク系Ｃ_jについて最小力学パラメータを編成する。これをリンクｎ−1まで繰り返すことで、全リンクの最小力学パラメータを導出する。
本願の発明者らは、シミュレーション、実験により、式（６）だけを用いた場合でもφ_Bが同定可能であることを検証した。
また、回転関節又は直動関節で構成される多関節リンク構造において、ベースリンクの運動方程式における最小力学パラメータと、全リンクの運動方程式における最小力学パラメータが一致することを数式で立証した。
詳細については、以下の文献を参照することができる。
鮎澤光，ベンチャー・ジェンチャン，中村仁彦,"ベースリンクの運動方程式を利用したヒューマノイドロボットの力学パラメータの可同定性," 日本機械学会ロボティクス・メカトロニクス講演会'08, pp. 2P1.F09, 2008.

［Ｂ］本発明に係る力学パラメータの同定法のヒューマノイドへの適用
ヒューマノイドのベースリンクの運動方程式のみを利用し、ロボットの胴体の位置と姿勢、関節角度、床反力を計測することで、ヒューマノイドの全身の力学パラメータを同定する手法について述べる。
図３Ａ、表１に示す小型ヒューマノイドロボットＵＴ−μ２（T. Sugihara, K. Yamamoto, and Y. Nakamura. Architectural design of miniature anthropomorphic robots towards high-mobility. In Proc. of the 2005 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 1083.1088, Edmonton, August 2005.）を用いて、力学パラメータの同定を行った。
ＵＴ−μ２の多関節リンク構造は、全２１リンク、全２６自由度である。ベースリンクＢ_０（体幹）、リンクＢ_１からＢ_６（右脚）、リンクＢ_７からＢ_１２（左脚）、リンクＢ_１３からＢ_１６（右腕）、リンクＢ_１７からＢ_２０（左腕）、からなる。
下半身の同定に焦点を絞り、運動中は上半身を動作させない(ブレーキがかかった)状態とする。このときＵＴ−μ２は、各脚の６個の回転関節を持つことから、全１３リンク、全１８自由度となる。

各リンクの最小力学パラメータφ_Bは、解析的な手法(非特許文献２)を用いて計算した。φ_B、及び、それに対応するリグレッサ行列Y_B1の計算の詳細は後述する。

まず、動力学シミュレーションを行い、そのときの運動データを用いて同定を行った。シミュレーションの目的は、ベースリンクの運動方程式（６）によって、全リンクの運動方程式（５）における最小力学パラメータφ_Bを同定可能かの検証を行い、またφ_Bの精度の良い同定を行える運動の種類を求めることである。
シミュレーションは、動力学計算ソフトウェアライブラリ（K. Yamane and Y. Nakamura. Dynamics computation of structure-varying kinematic chains and its application to human figures. IEEE Trans. on Robotics and Automation, Vol. 16, No. 2, pp. 124. 134, 2000.）を利用した。地面との接触力はバネ・ダンパモデルを利用して計算した。
またシミュレーションで利用した力学パラメータの真値φ_Bには、CADからあらかじめ計算した値φ_Baprioriを用いた。

１０歩の前進歩行を行ったシミュレーションデータを用いて、同定したパラメータφ（ハット）_Bと、誤差e＝φ（ハット）_B−φ_Baprioriを図４に示す。同定した９４個の各パラメータの誤差は小さく、本発明に係る力学パラメータの同定法が有効であり、歩行データから下半身の同定が成功したことが分かる。

次に、実験について説明する。各リンクの力学パラメータは、ＣＡＤから概算できる。しかし、電気系の配線やハードウェアの追加部品、アクチュエータの慣性などを考慮に入れると、実際の力学パラメータとは誤差が生じる。例えば、ＣＡＤ上でのロボットの全質量は6.7[kg]であるが、実計測した全質量は8.0[kg]となった。両足先に追加パーツを加えていることもあるが、誤差が16.25[%]もあり、ロボット全体の力学パラメータに大きな影響を与え、力学パラメータの同定の重要性が理解できる。

ＵＴ−μ２は、現状では加速度・ジャイロセンサによる胴体の一般化座標ｑ₀の正確な計測ができないため、実験では光学式モーションキャプチャを利用してｑ₀を計測する。ロボットの胴体に光学マーカをつけて、１０個のカメラを用いて、ロボットの運動を計測する。外力計測には、ロボットの足首に搭載した力センサの代わりに、６軸の接触力を計測可能なフォースプレートを利用する。フォースプレートはモーションキャプチャと同期して、約70[g]の分解能で６軸の全接触力Ｆを計測できる。また、ロボットの関節角度θは、各関節に搭載されているエンコーダで計測する。実験の様子を図３Ａに示す。

実験では、次の二種類の運動データを計測した。
●４歩だけ前進歩行する(１歩は0.5[s]で踏み出す)
●体幹をｘ，ｙ，ｚ軸周りにゆっくり折り曲げる。
まず上記の歩行データのみを利用して、力学パラメータφ（ハット）_Bwalkを同定した。次に、φ（ハット）_Bwalkを用いて、上記の両方の運動時の外力を再構成し、実際に計測した外力と比較した。

同定結果を図５Ａ、図５Ｂに示す。データのサンプリングタイムは共に3[ms]であり、図中の太実線が、計測された外力Ｋ_１Ｆと、同定した力学パラメータφ（ハット）_Bwalkから計算した外力Ｙ_B1φ（ハット）_Bwalkと、が一致して重なっている状態を示している。細点線が、ＣＡＤから得られた力学パラメータφ_Baprioriから計算した外力Ｙ_B1φ_Baprioriとなる。

９４次元の力学パラメータを同定し、同定したパラメータから求めた外力はよく推定できていることが分かる。また、歩行データから同定したパラメータは、体幹を曲げる動作においても外力パターンを再現できている。一方、同定したパラメータと予めＣＡＤから求めたパラメータでは、FZ,NX,NYにおいて明らかに違いがあり、同定したパラメータの方がより正確に外力パターンを再現できている。

一方、同定されたロボットの全質量は7.93[kg]となり、実際の全質量の計測値に対して0.07[kg]の誤差が生じている。また、NY，NZにおいて、0.2[Nm]程度の誤差が生じている。これらは、フォースプレートの分解能0.7[N]の範囲内である。

Ｙ_Ｂ１およびφ_Ｂの計算について詳細に説明する。
ここでは、ｎ個の回転関節のみをもつヒューマノイドの力学パラメータφ_B、及びそれに対応するベースリンクのリグレッサ行列Ｙ_Ｂ１を導出する。
ここで用いる変数を以下に示す。

通常の力学パラメータφは次のようになる。

まず、φに対応するリグレッサ行列Ｙのうち、ベースリンクにかかる上部分の行列Ｙ_１を求める。
ただし、[ｗ×]、[ｗ●]は次のような行列とする。

次に、最小力学パラメータφ_Bを求める。
まず、M_i，MS_i，J_iを計算する。M_i，MS_i，J_iは、リンクiの末端側に属する全リンクの力学パラメータの組み合わせで表わされ、末端リンクから順に計算する。ここでは全関節が回転関節であり、またベースリンクが６自由度を持つため、次のように計算できる。
ただし，ⁱ⁻¹ri，Ｕは次のようにおく。

M_i，MS_i，J_iから、全リンクの最小力学パラメータφ_Bは次のようになる。

上記の最小力学パラメータφ_Bに対応する行列Ｙ_B1は次のようになる。

次に、ＵＴ-μ２の最小力学パラメータについて説明する。ＵＴ-μ２は２１リンク、２６自由度を持つが、本稿では上半身を固定して考え、両脚各６関節、全１３リンク、全１８自由度とする。ここで、胴体リンクをベースリンクB₀、右脚６リンクを胴体側からB₁−B₆、左脚６リンクを胴体側からB₇−B₁₂と表わす。

B_j(0≦j≦12)リンクの力学パラメータをφ_jとし、全身の力学パラメータφを次のように構成する。

ＣＡＤから求めたφを用いて、上記の手法を用いて94次元の最小力学パラメータφ_Baprioriを計算した。

本発明に係る力学パラメータの同定法のヒューマノイドへの適用について小括する。
各関節のトルクを計測せず、ベースリンクの運動方程式のみから、全外力を計測することで全身の最小力学パラメータを同定する手法について述べた。
上半身を動作させないようにブレーキをかけた状態のＵＴ−μ2を用いて、シミュレーションを行い、１０歩の歩行データから、９４次元の最小力学パラメータを誤差なく同定できた。
実験を行い、４歩の歩行データのみから力学パラメータを同定した。同定したパラメータを用いて、歩行データと体幹を曲げる動作の二種類に対して、外力パターンを再構成した。同定したパラメータを用いて再構成した外力パターンは、ＣＡＤから概算したパラメータを用いた場合よりも、実験データをよく再現できており、同定の重要性を示している。

［Ｃ］本発明に係る力学パラメータの同定法の人体への適用
本発明に係る力学パラメータの同定法は、人体にも適用することができる。人体は、剛体リンク構造としてモデル化することができる。リンクの数や自由度は、当業者において適宜設定できる。ここでは、１５の部分からなり、３４自由度のモデルを考える（図６Ａ、表２）。

ヒューマノイドの場合と同様に、人体モデルの力学パラメータφから最小力学パラメータφ_Ｂを計算することができる。
回転関節の場合は、
となる。
球面関節の場合は、
となる。
ここで、
Ｍ_ｉは、連鎖における下位のリンクの質量の合計を表すリンクｉの力学パラメータ；
ｍ_ｉは、リンクｉ（１〜１４）の質量；
ＭＳ_ｉは、第１慣性モーメントの合計（ｉ：１〜１４）を表す、リンクｉの力学パラメータ；
Ｊ_ｉは、リンクｉの慣性を表す力学パラメータ；
である。

そして、最小力学パラメータを取得する。

最小力学パラメータを同定するためには、ベースリンクの位置・姿勢、各関節の関節角度、環境との接触力が必要となる。これらの情報は、１つの態様では、フォースプレートと同期したモーションキャプチャシステムにより取得される。運動データと床反力を同時計測するシステムは、当業者に知られており、例えば、瀬里、山根、中村：「ビヘイビアキャプチャシステムによる意識行動の同時実時間計測」、日本機会学会ロボティックス・メカトロニクス講演会、'０１講演論文集、１Ｐ１−Ｈ７，２００１に記載されている。
フォースプレートは、６軸の接触力を取得する。モーションキャプチャデータによってベースリンクの動きを取得する。適切な数のマーカを被験者に装着することで、人体モデルの逆運動力学計算によって、モーションキャプチャデータから各関節角度を計算することができる。
実験では、３５個のマーカ（図６Ｂ参照）を所定位置に装着した被験者がフォースプレート上を運動することを撮影することで、モーションデータおよび接触力を取得した。実験での被験者の動きは、歩行、サイドステップ、体幹の折り曲げ、上半身のランダムな動き、である。フォースプレートから取得したＫ_１Ｆと、同定したφ_Ｂから再構成したＹ_Ｂ１φ_Ｂを比較したところ、ヒューマノイドの実験の場合と同様に、略一致する結果となった。

［Ｄ］人間の全力学パラメータの同定及び実時間同定に基づく視覚化アプリケーション
上記実施形態において、人体の力学パラメータの同定法について提案した。提案手法は、モーションキャプチャと床反力計のみを利用して、全身の運動方程式を構成するのに必要十分な力学パラメータ(最小力学パラメータφ_Ｂ)を同定できる。しかし、ロボットの制御等ではこの最小表現でも実用上は問題ないが、医療などの他分野への応用を考慮すると最小表現のみでは不十分である。

また、上述の実施形態では、体操のような全身を動かす運動計測を複数行い、それらを組み合わせることで力学パラメータの同定を行なっているが、力学同定において重要となる同定に適した運動をどのように生成するかという問題についての議論は不十分であった。

本実施形態では、人体寸法の計測値とデータベースから力学パラメータを推定する手法（特開２００８−７７５５１号等）を利用して事前に力学パラメータを推定し、事前に推定した値と最小力学パラメータφ_Ｂの同定結果を組み合わせて、全力学パラメータを推定する手法を提案する。推定値は力学同定の結果を厳密に満たし、事前情報との誤差を最小化する形式で得ることができる。

本実施形態では、さらに、運動計測中に実時間で同定を行い、各身体部位の同定結果を視覚的に表示することで、同定性能を改善させる手法を提案する。被験者は計測中に同定が不十分な部位を特定して、その部位を十分動かすことで、全身のパラメータを同定するのに十分な運動を生成することができる。

［Ｄ−１］床反力計測に基づく全力学パラメータの同定法
（１）ヒューマノイドの同定モデル
人体を剛体多リンク系でモデル化すると、ｎリンク、N_j自由度のヒューマノイドの運動方程式は、式(1)のようになる。なお式(1)の上段は、ベースリンクの運動方程式となる。ベースリンクは基底であり、どのリンクにも設定可能であるが、一般的に体幹リンクとする。

多リンク系の運動方程式は、質量・重心・慣性テンソルなどの力学パラメータに対して、線形な関係式で表わすことができる。このとき、式(1)は式(2)のように変形できる。

力学パラメータφは動力学モデルを記述する上では冗長であり、モデルの表現に必要最小限なパラメータだけ同定が可能であることが知られている。同定可能な最小力学パラメータφ_B∈R^NBは動力学モデルの構造に依存し、運動方程式において冗長なパラメータを削減・再構成して得られる。最小力学パラメータを数値的・解析的に計算する手段は確立されており（非特許文献１、２、３）、式(2)は式(3)のように変形可能である。なお、Y_B∈R^NJ×NBは最小力学パラメータのリグレッサ行列に対応する。

（２）床反力計測を利用した同定法
一般的な同定法は式(3)を利用し、以下の情報の計測を必要とする。
＊ベースリンクの一般化座標q₀
＊関節の一般化座標q_cと関節トルクτ
＊N_c個の接触点における外力F^ext _k
しかし、同定対象がロボットと人間のどちらの場合でも、関節トルクの正確な計測は困難である。トルクセンサを全関節に搭載することは、どちらの場合でも運動中には大きな負担となる。またロボットはギア・アクチュエータ、人間は筋腱複合体といった動力伝達部で生じる摩擦等の非線形項のモデル化が問題となる。そこで最小力学パラメータφ_Bを同定するために、式(3)の上段の運動方程式のみ、すなわちベースリンクの運動方程式のみに注目する。
式(4)の特徴は、一般化力が常にゼロという力学的拘束条件であり、式(4)の中には、関節トルクτは含まれないため、τの計測は不要である。
同定に必要な情報は、各接触点kとそこに働く外力F^ext _k、および関節角度q_cとベースリンクの一般化座標q₀となる。
これらの情報は、ロボットの場合はエンコーダ、6軸力センサ、加速度センサ、ジャイロセンサ等で計測可能であり、人間の場合はモーションキャプチャと床反力計によって計測できる。
さらに全外力F^extを直接計測可能な場合は、片脚支持や両脚支持など接触状況に依存しないメリットがある。また同定式はベースリンクのみで構成されるので計算量が軽く、後述する実時間同定を行う上で大きな利点となる。同定法の概観については図１を参照することができる。

この同定法を利用するためには、ベースリンクの6個の運動方程式だけに縮小した力学モデルにおいて、原理的に同定可能な最小力学パラメータの数が変化していないことが必須となる。本発明者等は、この等価性の厳密な証明を行い、全身の最小力学パラメータが可同定であることが示されている。
詳細については、以下の文献を参照することができる。
鮎澤光，ベンチャー・ジェンチャン，中村仁彦,"ベースリンクの運動方程式を利用したヒューマノイドロボットの力学パラメータの可同定性," 日本機械学会ロボティクス・メカトロニクス講演会'08, pp. 2P1.F09, 2008.

［Ｄ−２］全力学パラメータの導出法
一般的に、力学同定とは力学パラメータφを同定するのではなく、最小力学パラメータφ_Bを同定することを意味する。最小力学パラメータφ_Bは、多リンク系の運動方程式を構成する上で必要最小限な情報となり、ロボットの制御に利用する上では、この最小表現であっても実用上の問題はない。しかし、この最小表現ではリンク機構に従って通常の力学パラメータを複雑に再構成するため、直感的には分かりにくい値となる。特に人間のパラメータを計測する場合、日々の計測やリハビリなど他分野への応用を考慮すると、通常の力学パラメータφの形式で求まることが望ましい。

ここで、文献・データベースなどを利用して、通常の力学パラメータが事前に獲得できる場合を考える。全次元の力学パラメータの情報を得られるが正確ではない。本実施態様では、力学同定によって得られる最小力学パラメータを厳密に満たした上で、上記の事前情報との誤差を最小化する全力学パラメータの導出法について述べる。

最小力学パラメータφ_B∈R^NBは、通常の力学パラメータφ∈R¹⁰ⁿを用いて次のように表現できる。
ただし、Ｚ∈R^NB×10nはリンク系の機構によって求まる最小力学パラメータの編成行列とする。

編成行列Ｚは、通常の力学パラメータから最小力学パラメータを編成するために用いられる行列である。すなわち、式(5)に示すように、通常の力学パラメータφと編成行列Ｚとから、最小力学パラメータφ_Bを求めることができる。
本実施形態では、編成行列Ｚが全力学パラメータから、最小力学パラメータを射影する線形行列であることを利用し、その逆射影を行うことで全力学パラメータを推定しようとするものである。そして、編成行列Ｚでは射影できない(最小力学パラメータではない)質量情報については、文献やデータベースの値を用いることで、逆射影を可能とするものである。
編成行列の詳細については、以下の文献を参照することができる。
鮎澤光，ベンチャー・ジェンチャン，中村仁彦,"ベースリンクの運動方程式を利用したヒューマノイドロボットの力学パラメータの可同定性," 日本機械学会ロボティクス・メカトロニクス講演会'08, pp. 2P1.F09, 2008.

全力学パラメータの推定法は以下のとおりである。
まず、線形方程式(5)の最小二乗解は以下のようになる。
ただし、z∈R¹⁰ⁿは任意のベクトルとし、Eは単位行列とする。力学同定によって得られる最小力学パラメータの推定値φ（ハット）_Bを用いる場合、編成行列Zの零空間によって射影されるzをどのように決めるかが課題となる。ここで、文献・データベース等を利用した通常の力学パラメータの事前情報φ^refを用いて、z＝φ^refと選ぶ。

このとき、力学パラメータの推定値φ（ハット）は次のように求まる。
ただし、
とした。
式(7)は式(5)を満たすので、推定値φ（ハット）の最小表現は同定したパラメータφ（ハット）_Bと一致することが分かる。また式(7)から、φ（ハット）はφ−φ^refのノルムを最小化する解、すなわち事前情報φ^refとの誤差ノルムを最小化する解であることが分かる。

［Ｄ−３］実時間同定と視覚化アプリケーション
（１）アプリケーションの概要
この節では、前節で述べた人間の全力学パラメータの同定手法を用いて、実時間でのオンライン同定と同定結果を視覚化するアプリケーションの概要について説明する。本実施態様では、一般化座標と外力計測には、光学式のマーカ位置計測を利用したモーションキャプチャと床反力計を利用する。キャプチャシステムは、10台のカメラを用いて光学式マーカを5[ms]おきに計測する。また床反力を1[ms]おきに計測可能であり、また両者は同期して計測可能である。

同定の流れは下記のようになる。概観を図７に載せる。
１．計測されるマーカ位置から幾何モデル（リンクの長さ、向き、太さ（周回りの寸法）等）を決定し、幾何モデルと人体データベースから全力学パラメータを事前に推定する。
２．マーカ位置と床反力から、最小力学パラメータの実時間同定を行い、最小力学パラメータの同定結果と全力学パラメータの事前情報から全力学パラメータを推定する。
３．各リンクごとの最小力学パラメータの同定結果を色表示して、動作が不十分な部位を特定して運動させることで、同定性能を向上させる。
それぞれの項目については、各小節ごとに説明する。

（２）幾何モデルの決定と全力学パラメータの事前推定
力学同定を行うためにはシステムのモデリングが必須となる。人間の力学特性の解析をする上で、人間を剛体多リンク系で表現する場合、そのリンク数や関節の選び方は、モデルの解析目的や計測環境によっては異なる。キャプチャされるマーカ数や後述する人体寸法データベースの次元や、歩行などの運動解析に十分な自由度を考慮して、本実施態様では15リンクから構成される計34自由度の人間モデルを利用する。モデル概要は表２のようになる。

力学同定にはモデルの幾何パラメータ（リンクの長さ、向きに加えて、太さ（周回りの寸法）等も含む）も同様に必要とする。ロボットの場合はCADなどから正確な値が算出可能ではあるが、人間の場合はこれらのパラメータを実際に計測する必要性がある。モーションキャプチャには、図６Ｂのような全身35個の光学式マーカを利用した。マーカは人体の関節近傍の特徴点に配置されているため、計測されるマーカ位置から関節間の距離を計算することで、各関節の幾何パラメータを容易に計測することができる。

最後に得られた幾何モデルから全力学パラメータを概算する。本実施形態では、人体寸法データベースから、人間の力学パラメータを推定する手法を用いる。利用するデータベースは日本人308人分の特徴的な49箇所の寸法と体重の計50個のパラメータが記録されている。
人体寸法データベースとしては、例えば、産業技術総合研究所デジタルヒューマン研究センター,"人体寸法データベース,"http://unit.aist.go.jp/collab-pro/indusstan/jis/theme/final/finalreports/measure/anthrop.htm.を利用することができる。
人間の力学パラメータを推定する手法としては、例えば、山口能迪, 山根克, and 中村仁彦,"人体筋骨格モデルの筋・腱および質量パラメータの同定," 日本機械学会ロボティクス・メカトロニクス講演会'06 講演論文集, 2A1-D07, 2006.、特開２００８−７７５５１号に開示された手法を用いることができる。

推定手法は次のようになる。
まず50個の中から幾つかパラメータを計測する。残りの未計測なパラメータは最も相関の高い計測パラメータとの回帰直線から推定を行う。次に図８のように人体の各リンクの幾何形状を楕円球や楕円錐台等でモデル化し、人体寸法49箇所から各リンクの体積を計算する。さらに密度を一定と近似することで、全体重から各リンクの質量・重心・慣性テンソルを概算できる。ここでは、マーカから計測可能な幾何パラメータと床反力計から求まる全体重を推定器の入力として、幾何形状と力学パラメータを推定する。なお、全力学パラメータの事前情報は、人間の質量データベースや文献の値を代用しても良く、上記の手法は一例にすぎない。

（３）力学パラメータの実時間同定
この小節では、運動計測中に力学パラメータを実時間で同定する手法について述べる。
計測されるマーカ位置から逆運動学計算を行い、各リンクの一般化座標を計算し、数値微分から速度・加速度を計算する。また計測される床反力から、ベースリンク原点に射影される全外力を計算する。同定に最終的に必要な情報は全外力であるため、全外力が直接計測できれば各接触点における外力情報は特に必要としない。

これらの計測値から式(4)中のリグレッサ行列と全外力が計算できる。式(4)を用いて最小二乗法からφ^Bを同定することができる。実時間で同定を行うためには、逐次的に最小二乗法を解く必要がある。一方、式(4)の右辺の外力項は力とモーメントで物理次元が異なり、各成分の計測精度が異なる。このような場合、推定値と計測値の誤差の分散を重み付けした最小二乗法が利用される場合が多い。また力学パラメータが時間変化する場合は、計測した過去のデータを忘却していく必要がある。人体計測の場合、荷物を持ったり、道具を使用したり、また日々の体型変化などで力学パラメータは変化しうる。

以上の要求から、本実施形態では以下のような忘却機能付き逐次的一般化最小二乗法を利用する。
忘却機能付き逐次的一般化最小二乗法については、例えば、下記の文献を参照することができる。
J.-J.E.Slotione, W.Li, "On the Adaptive Control of Robot Manipulatiors," Int. J. of Robotics Research, vol. 5, no. 2, pp. 49.59, 1987.
相良節夫，秋月影雄，中溝高好，片山徹, システム同定, 計測自動制御学会, 1994.

ｔ＝[1・・・n]の時系列データに対して、時刻ｔ＝ｎ−1の推定値φ（ハット）_B，n−1から時刻ｔ＝ｎの推定値φ（ハット）_B，nは以下のように求まる。
ただし、
時刻nにおける式(4)のリグレッサ行列をY_OB，nと外力ベクトルをF_nとする。
K_n∈R^NB×NBは、下記のようなゲイン行列となる。
P_n∈R^NB×NBは式(10)で定義される逆行列となり、その逆行列解は式(11)のように逐次的に計算できる。
Σ_ｎ∈R^6×6は重み行列とする。
λ_n(0≦λ_n≦1)は忘却係数で、現時刻以前のデータに毎回掛かる重み係数となり、指数関数的に忘却させる。
V_n∈R^6×6は以下のように定義される行列。

重み行列Σ_nには、出力信号Fの共分散行列を選ぶ。各出力信号は独立であると仮定して、Σ_nは対角行列とする。各対角要素σ² _ii,n(1≦i≦6)は、時t＝nまでにおける外力計測値Fと推定値Y_OBφ_Bの各6軸成分ごとの誤差の分散とする。
ここで、時刻t＝nにおける各6軸力成分ごとの外力の要素、およびリグレッサ行列の部分行ベクトルをそれぞれf_i,n∈R、ｙ_i,n∈R^1×NB(1≦i≦6)とすると、以下のようなA_i,n∈R^NB×NB、b_i,n∈R^NB、c_i,n∈R、d_n∈Rを用いてσ_ii,nを逐次的に計算する。

以上から、式(8)、式(11)、式(13)〜(17)を用いて、毎時刻ごとにW_n、P_nおよびφ（ハット）_B,nを更新すればよい。
また通常の力学パラメータφ（ハット）_nは式(7)を用いることで計算できる。
なお初期条件P₀、φ（ハット）₀については、既に別の計測データが存在しているならば、それらを初期条件として利用することができる。しかし一般的には、φ（ハット）₀＝0、P₀＝γEと定める場合が多い。A_i,0、b_i,0、c_i,0、d_i,0も同様に、既に計測データがあれば初期条件とし、そうでなければゼロとする。γ(>0)の値が大きいほど推定値の収束は早くなるが、可同定な運動データをすぐに得られない場合にはP_nは急速に発散するという相反性を持つ。また忘却係数λ_nについては0.995から1の定数値がとられる場合が多い。ここでは人体の同定を対象としているため、荷物を持つ、道具を使うなどのパラメータが運動中に変化する場合は、忘却係数を1未満の値で固定して用いることが望ましい。通常の同定時は基本的にはλ＝１とすればよい。日常的なパラメータの変化を追う用途であれば、忘却係数の初回値λ１のみを１未満に設定し、λ_ｔ（ｔ＞１）＝１として、初期値Ｐ₀、φ_B,0は、前日の最終計測値を用いれば、日毎に計測データを忘却することができる。

（４）Persistent Excitation Trajectoryの視覚化
前小節までの手法を用いて、人体の力学パラメータを同定できるが、全ての最小力学パラメータを同定するためには、同定を行うのに適した運動データを計測する必要がある。例えば全く運動せずに静止し続けている場合は、人体モデルは単一剛体と等しくなるため、全パラメータの同定は不可能である。よって全一般化座標が十分に変化する運動データが必要となる。このようなPersistent Excitation Trajectory(PE性をもつ運動)の導出は、力学同定においては特に重要な問題となる。
PE性をもつ運動の導出については、下記文献に言及がある。
M. Gautier and W. Khalil, "Exciting trajectories for inertial parameters identification," Int. J. of Robotics Research, vol. 11(4), pp. 362.375, 1992.

時系列に並べたリグレッサ行列の条件数が1に近い運動を求めれば良く、運動制御の安定性を確保した上で条件数を小さくする運動を模索する必要がある。人体計測の場合は、ロボットのような運動制御の安定性による制約はほとんど存在しないが、大自由度なシステムであり設置状況の時変性から、PE性を十分に持つ最適な運動計画は容易ではない。さらに、計画した最適な運動をロボットのように正確に再現するのも困難である。

本実施形態では実時間同定の特長を活かして、運動計測中に各リンクの同定状況を視覚的に表示させることで、被験者が運動が不十分な部位を特定して動作へフィードバックさせて、同定性能を高める仕組みを提案する。必ずしも最適な運動計画ではないが、同定に適した運動を直感的に理解して即時に反映させてPE性を向上できることが期待できる。

各リンクの同定状況は、各リンクにおける同定した各最小パラメータの推定値の相対標準偏差を指標とする。
相対標準偏差を用いてパラメータの同定の評価を行なうことについては、下記文献を参照することができる。
M. Gautier and W. Khalil, "Exciting trajectories for inertial parameters identification," Int. J. of Robotics Research, vol. 11(4), pp. 362.375, 1992.
G. Venture, P.J. Ripert, W. Khalil, M. Gautier, and P. Bodson, "Modeling and identification of passenger car dynamics using robotics formalism," IEEE Trans. on Intelligent Transportation Systems, vol. 7, no. 3, pp. 349.359, September 2006.
最小力学パラメータの推定値と真値との誤差の分散を計算する時に、最小力学パラメータの推定値から外力の推定値を計算して、この外力の推定値と実際の外力の計測値との誤差を求める。求めた外力の推定値と外力の計測値との誤差と、運動データを行列表現したリグレッサ行列Ｙから、間接的に推定された最小力学パラメータの誤差の分散を計算することができる。
下記の数３５において、右辺の第２項は、リグレッサ行列Y_OBに推定値φ（ハット）_Bを掛けて計算される外力推定値Ｆ（ハット）であり、ρは外力の誤差である。このときに、逐次一般化最小二乗法(数29から数34)を解く。
時刻ｎの時の外力の誤差ρ_nは、数29の右辺のF_n−Y_OB,nφ（ハット）_B,n-1の項となる。このとき数29から数34までの過程で、行列P_n(数31)が求まる。この行列P_nが、時刻ｎのときの最小力学パラメータの誤差の共分散行列C_nと一致する。
C_ｎの行列の各対角成分から最小力学パラメータの誤差の分散(相対標準偏差)を求めることができる。
以下に繰り返し説明する。
式(4)で表される線形モデルにおいて、Y_OBが確定的で、推定誤差ρ
が平均ゼロの正規分布と仮定した場合、逐次一般化最小二乗法によって得られる推定値φ（ハット）_B,nを用いると、共分散行列C_n∈R^NB×NBは下記のように計算できる。

逐次一般化最小二乗法による更新式となる式(8)、式(9)、式(11)は、システム雑音が加わっていない場合のカルマンフィルタに相当し、P_nは時刻t＝_nまでの観測に基づく推定値の誤差の共分散行列C_nと一致する。

共分散行列C_nの各対角成分をc_n,(j,j)とすると、相対標準偏差σ_φj%は下記のように求められる。

以上の相対標準偏差σ_φj%を用いることで、現在計測中の運動データが各々の最小力学パラメータを同定し易いか否かを判断することができる。σ_φj%が、同定対象によっては異なるがある閾値を下回れば、その最小力学パラメータは良く同定できていると判断する場合が多い。ただし、微小パラメータについては相対値が大きくなるため判断が困難となるおそれもある。

視覚的表示方法として、ここでは実時間でキャプチャされた被験者のヒューマンフィギュア（人体のリンク機構モデル）を画面上に提示し、ヒューマンフィギュアの各リンクを、次のような規則で色を変更して被験者に提示する。
リンクjの全最小力学パラメータ数n_Bjのうち、σ_φj%が15[%]を下回ったパラメータ数n_Bj,G、σ_φj%が15[%]を上回るが推定値が微小(<0.02)なパラメータ数n_Bj,B、どちらでもないパラメータ数をn_Bj,R＝n_Bj−n_Bj,G−n_Bj,Bとして、それぞれの比率を用いてRGB色を決定した。
１つの態様では、各画素値P_R,P_G,P_Bは以下のように決める。
P_R＝n_Bj,R／n_Bj×100；
P_G＝n_Bj,G／n_Bj×100：
P_B＝n_Bj,B／n_Bj×100；
ただし、ここでの画素値は最大値を1、最小値を0として正規化した値である。
したがって、ここでの態様では、同定が進むにしたがって、リンクの色が赤から緑へと変化していく。

［Ｄ−４］実験結果
視覚化アプリケーションを用いて、計測を3回行って同定を行った。ただし、λ_n＝1.0、γ＝0.001、他の初期条件はゼロとした。ここでは得られた計測データをオフライン処理して同定した結果について述べる。視覚化アプリケーションによる実時間同定を行う主な目的は、計測中に同定に適した運動生成を行うことである。実時間同定によって同定結果をすぐに確認することも有意義ではあるが、より精度の高い同定結果を求める場合には、オフライン処理による同定が望ましい。
オンラインで同定した際に視覚化アプリケーションが提示する画像のスナップショットを図９に載せる。同定が進むにつれて、前節のルールに基づいて身体のパーツを表すリンクが赤色（図では濃色）から緑色（図では淡色）へ変化していく。図には、床反力計から計測した外力、同定したパラメータから求めた外力を、それぞれベクトルとして矢印で示す。図９Ａの最初のフレームでは２本の矢印が表示されているが、その他のフレームでは２本の矢印は略一致している。また、濃色の球は全重心を表す。図９は、実際にオンライン同定を行った結果を示しており、計測が進むにつれて、リンクの色が変化していき、パラメータが同定されていくことが分かる。

同定には50[ms]おきのリグレッサ行列と外力のデータを利用し、得られた3つの計測データとそれら全てを組み合わせた場合の条件数cond(Y_OB)と利用したサンプル数を表３に載せる。また全128個のパラメータのうち、σ_φj,n%が15%未満で、微小でないパラメータ(>0.02)の総数も載せる。どの計測値の条件数も30前後となっていることが分かる。既述の実施形態での計測方法では、1回の計測データの条件数は500前後となる場合が多く、体操のような全身運動の計測データを複数組み合わせることで、50前後の条件数を実現していた。このことから、視覚化アプリケーションを用いることで、PE性を大きく向上できていることが理解できる。

次に式(7)を用いて計算した、通常の力学パラメータの推定結果φ（ハット）について述べる。質量M[kg]、重心C_i[kg-m²]、関節原点回りの慣性テンソルJ_ij[kg-m²]の順に、下腹部(L1)、胴体部(L2)、右足先(L3)、右手先(L4)、頭部(L5)、右腿部(L6)の推定結果φ（ハット）を表４に載せる。ここでは全計測データを組み合わせた同定結果を利用した。また参考として、データベースからの事前推定値φ^refを表５に載せる。

質量分布については事前推定値と近い値となっている。胴体部の各値、右足先、足先の慣性テンソルも事前推定値と比較的似た傾向を示している。しかし下腹部の重心や、頭部の重心、慣性テンソル、右腿部の慣性テンソルなどは、重心が各リンク形状の凸包の外に位置したり、負の慣性主成分を持ち、推定に失敗していることが分かる。
推定の失敗の原因としては、幾つか考えられる。先ず、十分なExcitationが得られていない場合が考えられる。今回の計測では単純に３つの計測データを組み合わせただけなので、多くの運動を計測して条件数の低い部分を切り出して組み合わせることで、PE性は改善することはできる。
次に、事前推定値の精度が低い場合が考えられる。通常の力学同定では分からない、すなわち最小力学パラメータには含まれない残りのパラメータは、データベースから求めた通常の力学パラメータを基にして推定している。よって、この残りのパラメータはデータベースから求めた事前値が持つ精度に依存する。また最小力学パラメータは重心と質量を掛けた値であるため、重心の推定値は質量の推定精度にも依存している。改善策としては、事前情報導出の精度向上、重心や慣性テンソルの力学的拘束条件を考慮するなどの手段が取り得ることが当業者に理解される。

人間の全力学パラメータの同定及び実時間同定に基づく視覚化アプリケーションについて小括する。
モーションキャプチャによる人体寸法の計測値と人体寸法データベースから力学パラメータを推定して、力学同定の結果を厳密に満たし、事前情報との誤差を最小化する形式で全力学パラメータを推定する手法を提案した。
運動計測中に実時間で同定を行い、各身体部位の同定結果を視覚的に表示することで、同定に適した運動生成を促し、同定性能を向上させる手法を提案した。
上記の手法を用いて実際に同定を行った結果、運動データが持つ同定性能(リグレッサ行列の条件数)を大きく向上させた。

本発明の同定法は、ロボットの関節にトルクセンサが搭載されていないシステムに対して利用することができるので、既存のシステムに対して広範囲に本発明を適用することが可能である。特に、ヒューマノイドは、労働力や介護など人間社会の進出が期待されている。ヒューマノイドは荷物を持つなど、運動中に自分自身の質量・重心などのパラメータが変化するため、高度な運動制御を安全に実現するためには同定技術は重要である。
本発明の同定法は、人体の計測にも利用可能である。本発明の同定法は、被験者に対しての負荷が軽く、簡単な運動計測で同定することが可能である。リハビリなど医療の定期診断やスポーツなどの身体解析において特に利用価値が高いと考えられる。

Claims (24)

1. 環境に固定されていない多関節リンク機構で表現される被験体の運動を、任意に選択されたベースリンクの空間運動とリンク系の関節運動で表し、
   ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、ならびに環境との接触点に働く外力、の各計測値と、
   数１に示す多関節リンク機構のベースリンクに関する運動方程式と、
   を用いて、多関節リンク構造の最小力学パラメータφ_Ｂを同定する方法。
   ここで、
   Ｙ_Ｂ１は、ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、から求められる観測行列、
   Ｆ_ｋは、接触点ｋに働く外力、
   Ｋ_ｋ１は、接触点ｋにおける外力を一般化力へ変換する行列、
   Ｎ_ｃは、多関節リンク構造と環境との接触点の総数、
   である。
2. 前記被験体は、ヒューマノイドである、請求項１に記載の最小力学パラメータの同定法。
3. 前記被験体は、人体である、請求項１に記載の最小力学パラメータの同定法。
4. 前記外力は床反力である、請求項１乃至３いずれかに記載の最小力学パラメータの同定法。
5. 前記床反力は、床面に敷設した力センサ、あるいは、被験体の脚部に装着した力センサ、によって計測される、請求項４に記載の最小力学パラメータの同定法。
6. 前記各関節角度は、各関節に搭載されているエンコーダによって計測される、請求項１乃至５いずれかに記載の最小力学パラメータの同定法。
7. 前記各関節角度は、被験体の各リンクの位置を、モーションキャプチャによって計測した時系列データから逆運動学計算することによって取得される、請求項１乃至５いずれかに記載の最小力学パラメータの同定法。
8. ベースリンクの一般化座標は、ベースリンクに搭載されたジャイロ加速度センサによって計測される、請求項１乃至７いずれかに記載の最小力学パラメータの同定法。
9. ベースリンクの一般化座標は、被験体のベースリンクにマーカを配置し、モーションキャプチャによって取得されたマーカ位置の時系列データからベースリンクの位置・姿勢を計算することで取得される、請求項１乃至７いずれかに記載の最小力学パラメータの同定法。
10. 床面に敷設した床反力計測用のフォースプレート上で運動する被験体を、床反力の計測と同期するモーションキャプチャシステムのカメラで撮影し、
    前記床反力は、前記フォースプレートにより計測され、
    前記ベースリンクの一般化座標、前記各関節角度、は、モーションキャプチャによって取得されたマーカ位置の時系列データから計算される、
    請求項４に記載の最小力学パラメータの同定法。
11. ベースリンクの一般化座標、各関節角度及びこれらの速度、加速度、ならびに環境との接触点に働く外力、の計測時に、前記最小力学パラメータを実時間で計算することを特徴とする、請求項１乃至１０いずれかに記載の最小力学パラメータの同定法。
12. 前記被験体は人体であり、被験者の運動時に前記最小力学パラメータを実時間で計算することを特徴とする、請求項１１（請求項２を引用しないものに限る）に記載の最小力学パラメータの同定法。
13. 前記被験体のリンク機構モデルを表示部に表示し、各リンクの最小力学パラメータの同定の程度を、表示されたリンク機構モデルにおいてリンク毎に同定の程度に応じて視覚的に表示する、
    請求項１２に記載の最小力学パラメータの同定法。
14. 前記最小力学パラメータの同定の程度に応じてリンクの色を変化させて表示する、請求項１３に記載の最小力学パラメータの同定法。
15. 前記最小力学パラメータの同定が不十分なリンクを被験者に視覚的に認識させることで、当該リンクに対応する身体部位の運動を促す、請求項１３、１４いずれかに記載の最小力学パラメータの同定法。
16. 各リンク毎に、各最小力学パラメータの同定の程度を求め、求めた複数の最小力学パラメータの同定の程度を用いて、各リンクとしての最小力学パラメータの同定の程度を算出する、
    請求項１３乃至１５いずれかに記載の最小力学パラメータの同定法。
17. 各リンクにおいて、各最小力学パラメータを、求めた同定の程度に応じて３つのグループに分類して各グループに３原色の各色を割り当て、各グループに属する最小力学パラメータの数の割合に基づいて表示部に表示された当該リンクの画素における各色の濃度値を決定する、
    請求項１６に記載の最小力学パラメータの同定法。
18. 各最小力学パラメータの同定の程度の指標は、各最小力学パラメータの推定値の時系列データの各推定値と当該最小力学パラメータの目標値との誤差の分散である、請求項１３乃至１６いずれかに記載の最小力学パラメータの同定法。
19. 前記誤差の分散を、前記最小力学パラメータの推定値を用いて外力の推定値の時系列データを算出し、外力の推定値の時系列データと外力の計測値の時系列データとの誤差から取得する、請求項１８に記載の最小力学パラメータの同定法。
20. 前記指標としての誤差の分散は、相対標準偏差である、請求項１８、１９いずれかに記載の最小力学パラメータの同定法。
21. 各最小力学パラメータの同定の程度の指標は、各最小力学パラメータの推定値と当該最小力学パラメータの目標値との差である、請求項１３乃至１６いずれかに記載の最小力学パラメータの同定法。
22. 各最小力学パラメータの同定の程度の指標は、各最小力学パラメータの推定値から計算された外力の推定値と外力の計測値との差である、請求項１３乃至１６いずれかに記載の最小力学パラメータの同定法。
23. 前記被験体のリンク機構モデルの各リンクの全力学パラメータの事前情報を用意し、
    請求項１乃至２０いずれかに記載の方法によって同定された最小力学パラメータφ_Ｂを満たすように前記事前情報を補正することで、前記事前情報と前記同定された最小力学パラメータφ_Ｂとから全力学パラメータφを推定する、
    全力学パラメータの同定法。
24. 全力学パラメータの推定値φ（ハット）は、数２を用いて推定する、請求項２３に記載の全力学パラメータの同定法。
    ここで、
    φ（ハット）_Ｂは、最小力学パラメータの推定値、
    Ｚは、リンク系の機構によって決定される最小力学パラメータの編成行列、
    Ｚ ^♯ は、Ｚの疑似逆行列、
    φ^refは、全力学パラメータの事前情報、
    、
    である。