JP4291092B2 - ２足歩行移動体の関節モーメント推定方法 - Google Patents

Description

本発明は、人間や２足歩行ロボット等の２足歩行移動体の各脚体の関節に作用するモーメント（関節モーメント）を推定する方法に関する。

例えば人間の歩行動作を補助する歩行アシスト装置の動作制御を行なう場合、人間の脚体の関節に実際に作用する関節モーメントを把握することが必要となる。この関節モーメントを把握することで、歩行アシスト装置の目標補助力を適正に決定することが可能となる。また、２足歩行ロボットにおいても、その動作制御を行なうために、脚体の各関節に実際に作用する関節モーメントを適宜把握する必要が生じる場合がある。

そこで、本願出願人は先に、例えば特開２００３−８９０８３号公報（特許文献１）等にて人間等の２足歩行移動体の脚体の関節モーメントを推定する手法を提案した。この手法では、２足歩行移動体の脚体の各関節の変位量（回転角）や、所定部位の加速度、角速度が所要のセンサを用いて計測され、それらの計測データや２足歩行移動体の剛体リンクモデル等を用いて各脚体に作用する床反力ベクトルとその作用点の位置とが推定される。ここで、剛体リンクモデルは、２足歩行移動体の構造を、複数の剛体要素を複数の関節要素で連結してなる連結体として表現するモデルであり、２足歩行移動体の全体重心の位置や、各剛体要素および各関節要素にそれぞれ対応する２足歩行移動体の剛体相当部（大腿部、下腿部、腰部等）および関節（膝関節、股関節等）の位置・姿勢を推定するために用いられる他、２足歩行移動体の動力学的な挙動を記述するモデルの基礎として用いられる。なお、剛体リンクモデルの各剛体要素には、その重量や長さ、重心の位置（各剛体要素上での位置）が付随的にあらかじめ設定される。

そして、上記特許文献１のものでは、推定した床反力ベクトルとその作用点の位置と剛体リンクモデルとを用いて、逆動力学モデルに基づく演算処理によって各脚体の膝関節や股関節の関節モーメントが推定される。逆動力学モデルは、それを一般的に言えば、物体に作用する外力と位置情報とを既知として（該外力および位置情報を入力パラメータとして）、該物体の内力である反力やモーメントを推定するための動力学モデルであり、物体の運動（位置の時系列パターン）と該物体に作用する力やモーメントとの関係を表すものである。特許文献１の手法では、逆動力学モデルは、前記剛体リンクモデルの各剛体要素の運動（並進運動および回転運動）に関する運動方程式を基に構築され、各脚体の関節モーメントが、床反力ベクトルの作用点により近い関節側のものから順番に推定される。

ところで、特許文献１のもののように関節モーメントを推定する場合、床反力ベクトルやその作用点、並びに逆動力学モデルは、ある１つの座標系で記述する必要がある。そして、特許文献１のものでは、その座標系として床に固定した絶対座標系が用いられていた。

このように絶対座標系を用いた場合には、２足歩行移動体のある所定部位（詳しくは剛体リンクモデルのある剛体要素に対応する剛体相当部）の絶対座標系での傾斜角、例えば鉛直方向に対する傾斜角を把握しつつ、２足歩行移動体のある基準部位に対する相対的な位置・姿勢として把握される２足歩行移動体の各部位（剛体リンクモデルの各剛体要素に対応する剛体相当部）の位置・姿勢を絶対座標系での値に変換しなければならない。また、床反力ベクトルや関節モーメントを求める場合、剛体リンクモデルの各胴体要素の重心の絶対座標系での位置も推定する必要があるので、それらの重心の位置を絶対座標系で表すための演算処理（座標変換処理）も必要となる。従って、上記傾斜角を用いた座標変換等の演算処理が多数、必要となる。

この場合、２足歩行移動体のある所定部位の鉛直方向に対する傾斜角の計測手法としては、角速度を検出するジャイロセンサの検出値を積分する手法、該ジャイロセンサと加速度センサとを併用して所謂カルマンフィルタの手法で傾斜角を推定する手法、あるいは、振子式の傾斜計で傾斜角を直接検出する手法等が一般的に知られている。しかるにこれらのいずれの手法でも、ジャイロセンサの検出値の積分に伴う誤差の蓄積や２足歩行移動体の運動時の慣性加速度などの影響で、一般には鉛直方向に対する傾斜角を十分に満足できる精度で計測することはできない場合が多い。従って、２足歩行移動体のある所定の部位の絶対座標系における傾斜角を高精度で把握することは一般には困難である。このため、上記のようにその傾斜角を用いた座標変換等の演算処理を多数、必要となる特許文献１の手法では、演算誤差が蓄積し易く、それが関節モーメントの推定精度を向上させる妨げとなっていた。
特開２００３−８９０８３号公報

本発明はかかる背景に鑑みてなされたものであり、２足歩行移動体の重力方向に対する傾斜情報を使用する演算処理を可能な限り少なくして、脚体の関節モーメントの推定精度を高めることができる２足歩行移動体の関節モーメント推定方法を提供することを目的とする。

本発明の２足歩行移動体の関節モーメント推定方法は、２足歩行移動体を、複数の剛体要素が少なくとも該２足歩行移動体の各脚体の股関節及び膝関節に対応する関節要素を含む複数の関節要素で連結された連結体として表現する剛体リンクモデルを用い、該２足歩行移動体の各脚体の少なくとも１つの関節に作用する関節モーメントを推定する方法であって、前記剛体リンクモデルの各関節要素に対応する前記２足歩行移動体の各関節の変位量を逐次把握する第１ステップと、前記剛体リンクモデルの所定の１つの剛体要素に固定された座標系としてあらかじめ設定された身体座標系の原点の加速度ベクトルの該身体座標系での値を少なくとも前記２足歩行移動体に装着した加速度センサの出力を用いて逐次把握する第２ステップと、前記２足歩行移動体の各脚体に作用する床反力ベクトルの前記身体座標系での値を逐次把握する第３ステップと、前記床反力ベクトルの作用点の位置ベクトルの前記身体座標系での値を逐次把握する第４ステップと、前記第１乃至第４ステップで把握した２足歩行移動体の各関節の変位量と前記身体座標系の原点の加速度ベクトルの値と前記床反力ベクトルの値とその作用点の位置ベクトルの値とを用いて、前記剛体リンクモデルの各剛体要素の運動と該剛体要素に作用する並進力およびモーメントとの関係を前記身体座標系を用いて表す逆動力学モデルに基づいて前記２足歩行移動体の各脚体の少なくとも一つの関節に作用する関節モーメントを逐次推定する第５ステップを備えると共に、
前記２足歩行移動体の全体重心の位置ベクトルの前記身体座標系での値を前記第１ステップで把握した２足歩行移動体の各関節の変位量と前記剛体リンクモデルとを用いて逐次把握する第６ステップと、その全体重心の位置ベクトルの値の時系列データおよび前記第２ステップで把握した身体座標系の原点の加速度ベクトルの値から該全体重心の加速度ベクトルの身体座標系での値を逐次把握する第７ステップと、前記２足歩行移動体の運動状態が一対の脚体のうちの一方の脚体のみが接地している単脚支持状態であるか、両脚体が接地している両脚支持状態であるかを逐次判断する第８ステップと、前記身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部の鉛直方向に対する傾斜角を逐次把握する第９ステップと、２足歩行移動体の各脚体毎に該脚体が接地しているか否かを逐次判断する第１０ステップと、接地している各脚体の足首関節の位置ベクトルの前記身体座標系での値と該脚体の足平部の中足趾節関節の位置ベクトルの前記身体座標系での値とを前記第１ステップで把握した２足歩行移動体の各関節の変位量と前記剛体リンクモデルとを用いて逐次把握する第１１ステップと、前記第６ステップで把握した前記全体重心の位置ベクトルの値と前記第１１ステップで把握した、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節のそれぞれの位置ベクトルの値と前記第９ステップで把握した傾斜角とに基づいて少なくとも該全体重心、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節の位置関係と該脚体の足首関節の鉛直方向位置を逐次把握する第１２ステップと、その把握した全体重心、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節の位置関係に基づき該脚体に作用する床反力ベクトルの作用点の水平面内位置を推定すると共に該脚体の足首関節の鉛直方向位置に基づき該脚体に作用する床反力ベクトルの作用点の鉛直方向位置を逐次推定する第１３ステップとを備え、
前記第３ステップは、２足歩行移動体の運動状態が前記単脚支持状態であるときには、前記第７ステップで把握した前記全体重心の加速度ベクトルの値と２足歩行移動体の全体重量と接地している脚体に作用する床反力ベクトルとにより表される該２足歩行移動体の全体重心の運動方程式に基づいて該床反力ベクトルの身体座標系での値を把握し、２足歩行移動体の運動状態が前記両脚支持状態であるときには、前記第７ステップで把握した前記全体重心の加速度ベクトルの値と２足歩行移動体の全体重量と両脚体のそれぞれに作用する床反力ベクトルとにより表される該２足歩行移動体の全体重心の運動方程式と、各脚体に作用する床反力ベクトルが該脚体の下端部近傍にあらかじめ定めた特定部から２足歩行移動体の全体重心に向かって作用するベクトルであると仮定して定まる、２足歩行移動体の全体重心に対する該脚体の特定部の相対位置と該脚体に作用する床反力ベクトルとの間の関係式とに基づいて両脚体のそれぞれに作用する床反力ベクトルの身体座標系での値を把握し、
前記第４ステップは、該第１３ステップで推定された床反力ベクトルの作用点の水平面内位置および鉛直方向位置と前記第９ステップで把握された傾斜角とに基づき前記身体座標系での作用点の位置ベクトルの値を把握することを特徴とするものである。

かかる本発明によれば、前記剛体リンクモデルの各関節要素に対応する２足歩行移動体の各関節の変位量（関節の回転角等）を逐次把握すると共に、剛体リンクモデルの所定の剛体要素に固定された身体座標系の原点の加速度ベクトルの身体座標系での値と、床反力ベクトルおよびその作用点の位置ベクトルの身体座標系での値とを逐次把握することで、主に身体座標系で記述されるアルゴリズムによって関節モーメントを推定することが可能となる。すなわち、前記第１ステップで把握した各関節の変位量（これは剛体リンクモデルの各関節要素の変位量に相当する）によって、前記剛体リンクモデルの各剛体要素に対応する２足歩行移動体の各剛体相当部の相対的な位置関係および姿勢関係を逐次把握することができるので、身体座標系の原点から見た２足歩行移動体の各剛体相当部の位置および姿勢（向き）（剛体リンクモデルの各剛体要素の位置及び姿勢）を逐次把握できる。従って、各剛体要素あるいはそれに対応する２足歩行移動体の各剛体相当部の、身体座標系の原点から見た運動状態（位置、速度、加速度などの状態）を逐次把握できる。さらに、前記第２ステップで加速度センサの出力を用いて把握した身体座標系の原点の加速度ベクトルの身体座標系での値（詳しくは加速度ベクトルの、身体座標系での座標成分値の組）を用いることによって、身体座標系を固定した前記所定の剛体要素に対応する２足歩行移動体の剛体相当部の運動（地面に対する運動）とその剛体相当部に対する２足歩行移動体の各部の相対的な運動とを合わせた全体的な運動に伴う剛体リンクモデルの各剛体要素の加速度等の運動状態を身体座標系での値によって逐次把握できることとなる。なお、前記身体座標系の原点の加速度ベクトルには、重力に伴う慣性加速度成分が含まれる。そして、このように剛体リンクモデルの各剛体要素の加速度等の運動状態を身体座標系での値によって逐次把握できるので、前記第３ステップおよび第４ステップで床反力ベクトルおよびその作用点の位置ベクトルの身体座標系での値（詳しくは、床反力ベクトルのおよびその作用点の位置ベクトルのそれぞれの、身体座標系での座標成分値の組）とを把握することで、前記逆動力学モデルを身体座標系で表現することが可能となり、身体座標系での床反力ベクトルの値、その作用点の位置ベクトルの値、各剛体要素の位置、加速度等の運動状態の値を用いた逆動力学モデルの演算処理によって脚体の関節に作用する関節モーメントを推定することが可能となる。
ここで、より詳しくは、前記床反力ベクトルの身体座標系での値を把握する前記第３ステップにおいては、単脚支持状態と両脚支持状態とで２足歩行移動体の全体重心の運動方程式（全体重心の並進運動に関する運動方程式）を基本として、接地している脚体に作用する床反力ベクトルを推定するので、２足歩行移動体の歩行の妨げあるいは負担となるような荷重センサを使用することなく、床反力ベクトルを推定できる。なお、両脚支持状態では、全体重心の運動方程式だけでは各脚体のそれぞれに作用する床反力ベクトルを特定できないが、各脚体に作用する床反力ベクトルが該脚体の下端部近傍にあらかじめ定めた特定部（例えば各脚体の足首関節、床反力作用点等）から２足歩行移動体の全体重心に向かって作用するベクトルであると仮定して定まる、２足歩行移動体の全体重心に対する該脚体の特定部の相対位置と該脚体に作用する床反力ベクトルとの間の関係式をさらに用いることによって、各脚体毎の床反力ベクトルを推定できる。この場合、前記運動方程式で必要となる２足歩行移動体の全体重心の加速度ベクトルは、その身体座標系での値が前記第７ステップで逐次求められるので、その全体重心の運動方程式を身体座標系の座標成分値だけで記述することができる。また、前記２足歩行移動体の全体重心に対する脚体の特定部の相対位置と該脚体に作用する床反力ベクトルとの間の関係式も身体座標系の座標成分値だけで記述できる。従って、身体座標系での床反力ベクトルの値を、２足歩行移動体の傾斜情報（鉛直方向あるいは水平方向に対する傾斜状態）を把握することなく求めることができる。
また、２足歩行移動体の接地している脚体に作用する床反力ベクトルの作用点の水平面内位置は、その脚体の足首関節と中足趾節関節と２足歩行移動体の全体重心との相対的位置関係に密接に関係している。また、床反力ベクトルの作用点の鉛直方向位置は、脚体の足首関節の鉛直方向位置とほぼ一定の相関性を有する。従って、前記床反力ベクトルの作用点の位置ベクトルの身体座標系での値を把握する第４ステップにおいて、２足歩行移動体の全体重心、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節の位置関係に基づき該脚体に作用する床反力ベクトルの作用点の水平面内位置を把握することができると共に該脚体の足首関節の鉛直方向位置に基づき該脚体に作用する床反力ベクトルの作用点の鉛直方向位置を把握することができる。そして、その把握した水平面内位置および鉛直方向位置と身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部の鉛直方向に対する傾斜角とを用いることで、身体座標系での床反力ベクトルの作用点の位置ベクトルを求めることができる。
このように床反力作用点の位置ベクトルの身体座標系での値を把握する場合には、身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部の鉛直方向に対する傾斜角を傾斜角検出用のセンサ（角速度センサや傾斜計）を用いて把握する必要がある。しかし、本発明では、その傾斜角は、床反力ベクトルの作用点を推定するためにだけ使用すればよいので、その傾斜角を用いる演算は必要最低限に抑えられる。従って、該傾斜角が誤差を有するような場合であっても、演算誤差の累積を最小限に抑えることができ、ひいては、関節モーメントの推定精度を十分な精度に確保できる。また、瞬間的に比較的大きな荷重がかかりやすい足平部の底部に圧力分布センサを設けずに済むため、関節モーメントの推定のための装置構成の耐久性上の利点もある。

このように本発明によれば、身体座標系での各剛体要素あるいはこれに対応する２足歩行移動体の各剛体相当部の運動状態の値を用いて脚体の関節モーメントを推定できるので、２足歩行移動体の傾斜情報（２足歩行移動体のある部位が鉛直方向あるいは水平方向に対してどれだけ傾いているか等の情報）を使用する演算処理を少なくできる。その結果、脚体の関節モーメントの推定精度を高めることができる。

かかる本発明では、前記加速度センサは、基本的には２足歩行移動体のどの剛体相当部に装着されていてもよいが、前記身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部に装着されていることが好ましい（第２発明）。すなわち、身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部以外の部位に加速度センサが装着されている場合には、その装着部位の加速度ベクトルから、２足歩行移動体の関節の変位量等を用いて身体座標系が固定された剛体要素に対応する剛体相当部の加速度ベクトルを算出する必要がある。これに対して、第２発明の如く、身体座標系が固定された剛体要素に対応する剛体相当部に加速度センサが装着されている場合には、該身体座標系の原点と加速度センサとの位置関係が固定されているので、２足歩行移動体の関節の変位量を使用することなく身体座標系の原点の加速度ベクトルの身体座標系での値を加速度センサの出力から把握することができる。その結果、把握する身体座標系の原点の加速度ベクトルの身体座標系での値の精度を高めることができる。

さらに、上記第２発明では、前記身体座標系が固定された剛体要素は、前記２足歩行移動体の一対の股関節に対応する一対の関節要素を連結する剛体要素であることが好ましい（第３発明）。すなわち、２足歩行移動体の一対の股関節に対応する一対の関節要素を連結する剛体要素は２足歩行移動体の腰部に対応するものであり、該腰部は、一般に２足歩行移動体の移動時の動きが比較的小さい。このため、加速度センサの出力の急変を少なくして、該加速度センサの出力を比較的安定させることができ、ひいては、把握する身体座標系の原点の加速度ベクトルの身体座標系での値の精度を高めることができる。

前記第１〜第３発明では、前記第３ステップおよび第４ステップでそれぞれ把握する床反力ベクトルの値およびその作用点の位置ベクトルの値は３次元の値であるときには、床反力ベクトルの作用点の水平面内位置（２足歩行移動体の前後方向および左右方向での位置）は、次のように把握することができる。すなわち、前記第１２ステップは、前記全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で後側に存在する場合には、該脚体の足首関節の水平面内位置（前後方向および左右方向での位置）を該脚体に作用する床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定し、前記全体重心が接地している脚体の足平部の中足趾節関節に対して２足歩行移動体の前後方向で前側に存在する場合には、該脚体の足平部の中足趾節関節の水平面内位置（前後方向および左右方向での位置）を該脚体に作用する床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定し、前記全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で前側に存在し、且つ該脚体の足平部の中足趾節関節に対して後側に存在する場合には、該脚体の足首関節と中足趾節関節とを結ぶ線分上で前記全体重心と前後方向の位置が同一となる点の水平面内位置（前後方向および左右方向での位置）を該脚体に作用するの床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定する（第４発明）。

すなわち、前記全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で後側に存在する場合には、該脚体は、通常、その足平部の踵で接地しており、その接地箇所は、該脚体の足首関節のほぼ直下に存在する。従って、この場合には、該脚体の足首関節の水平面内位置（前後方向および左右方向での位置）を該脚体に作用する床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定できる。また、前記全体重心が接地している脚体の足平部の中足趾節関節に対して２足歩行移動体の前後方向で前側に存在する場合には、該脚体は、通常、その足平部のつま先で接地しており、その接地箇所は、該脚体の足平部の中足趾節関節のほぼ直下に存在する。従って、この場合には、該脚体の足平部の中足趾節関節の水平面内位置（前後方向および左右方向での位置）を該脚体に作用する床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定できる。また、全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で前側に存在し、且つ該脚体の足平部の中足趾節関節に対して後側に存在する場合には、床反力ベクトルの作用点の前後方向での位置は全体重心の前後方向での位置とほぼ同じである。また、足平部は、概ね足首関節から中足趾節関節まで延在する剛体とみなせるので、床反力ベクトルの作用点は、足首関節と中足趾節関節とを結ぶ線分を床面に投影した線分上に存在すると考えることができる。従って、この場合には、脚体の足首関節と中足趾節関節とを結ぶ線分上で前記全体重心と前後方向の位置が同一となる点の水平面内位置（前後方向および左右方向での位置）を該脚体に作用する床反力ベクトルの作用点の水平面内位置（前後方向および左右方向での位置）として推定することができる。

また、上記第１〜第４発明では、床反力作用点ベクトルの鉛直方向位置の推定に関しては、例えば、前記第１３ステップは、接地している脚体に作用する床反力ベクトルの鉛直方向位置を、前記第１２ステップで把握された脚体の足首関節の鉛直方向位置からあらかじめ定めた所定値だけ鉛直方向下方に離れた位置として推定する（第５発明）。すなわち、２足歩行移動体の歩行時等に接地している脚体の足首関節は、一般に床面から概ね一定の高さの位置に存在する。従って、脚体の足首関節の鉛直方向位置から、あらかじめ定めた所定値（上記一定の高さに相当する所定値）だけ鉛直方向下方に離れた位置を床反力ベクトルの作用点の鉛直方向位置として推定できる。

接地している脚体の足首関節の床面からの高さは上記の如く概ね一定の高さであるが、２足歩行移動体の移動時に脚体の足平部の底面のつま先側の部分だけが接地している状態では、該脚体の足首関節の床面からの高さは、該足平部の底面のほぼ全体または踵側の部分が接地している場合に較べて多少高くなる。従って、床反力ベクトルの作用点の鉛直方向位置の推定精度をより高める上では、第５発明で次のようにすることが好ましい。

すなわち、前記第１０ステップでは、接地していると判断した脚体について、さらに該脚体の足平部のつま先側部分および踵側部分のそれぞれ接地の有無を判断し、前記第１２ステップでは、前記接地している脚体の足首関節の鉛直方向位置に加えて該脚体の足平部の中足趾節関節の鉛直方向位置を把握するようにする。そして、前記第１３ステップでは、前記第１０ステップで足平部のつま先側部分および踵側部分のうちのつま先側部分のみが接地していると判断されたときには、前記所定値の代りに、前記第１２ステップで把握した前記足首関節の鉛直方向位置と中足趾節関節の鉛直方向位置とから求められる該足首関節と中足趾節関節との鉛直方向の距離を用いて前記床反力ベクトルの作用点の鉛直方向位置を推定する（第６発明）。

これによれば、接地している脚体の足平部のつま先側部分だけが接地していると判断される状態では、脚体の足首関節の鉛直方向位置から、足首関節と中足趾節関節との鉛直方向距離だけ鉛直下方に離れた位置が床反力ベクトルの作用点の鉛直方向位置として推定される。その結果、床反力ベクトルの作用点の鉛直方向位置の推定精度を高めることができる。

以上説明した各発明では、前記第３ステップおよび第４ステップでそれぞれ把握する床反力ベクトルの値およびその作用点の位置ベクトルの値は、３次元の値であることが好ましい（第７発明）。これによれば、２足歩行移動体の空間的な挙動に則して床反力ベクトルの値およびその作用点の位置ベクトルの値を把握するので、それらを基に把握される関節モーメントの推定精度を高めることができる。

本発明の実施形態を図１〜図１０を参照して説明する。図１は本発明を２足歩行移動体としての人間に適用した実施形態での全体的装置構成を模式的に示す図である。同図に示すように、人間１は、その構成を大別すると、左右一対の脚体２，２と、胴体３と、左右一対の腕体４，４と、頭部５とを有する。胴体３は、腰部６、腹部７、および胸部８から構成され、その腰部６が脚体２，２のそれぞれに左右一対の股関節９，９を介して連結されて両脚体２，２上に支持されている。また、胴体３の胸部８の左右の両側部からそれぞれ腕体４，４が延設されると共に、胸部８の上端に頭部５が支持されている。各脚体２は、股関節９から延在する大腿部１０と、該大腿部１０の先端から膝関節１１を介して延在する下腿部１２と、該下腿部１２の先端から足首関節１３を介して延在する足平部１４とを備えている。

本実施形態では、このような構成を有する人間１の各脚体２の各関節９，１１，１３に作用する関節モーメントの推定を行うために、次のような装置を人間１に装備している。すなわち、腰部６の背面にはセンサボックス１５が図示しないベルト等の部材を介して装着されている。このセンサボックス１５の内部には、図２のブロック図で示すように３軸方向の加速度（並進加速度）を検出する加速度センサ１６と、３軸方向（３軸回り）の角速度を検出するジャイロセンサ１７と、マイクロコンピュータを用いて構成された演算処理装置１８と、後述する光ファイバ２６，２７に導入する光を発光したり、戻り光を受光する発光／受光器１９と、演算処理装置１８等の各電装品の電源としてのバッテリ２０とが収容されている。なお、加速度センサ１６及びジャイロセンサ１７は、センサボックス１５を介して腰部６に固定され、腰部６と一体的に動くようになっている。

各脚体２の股関節９、膝関節１１、足首関節１３の部位には、それぞれの関節の変位量を検出する関節変位センサ２１，２２，２３が図示しないベルト等の部材を介して装着されている。これらの各関節変位センサ２１，２２，２３が検出する変位量は、各関節９，１１，１３の３軸回りの回転角であり、その検出は、ポテンショメータ等を用いて行われる。

また、各脚体２の足平部１４の底面（詳しくは足平部１４に装着した靴の底面）には、２つの接地センサ２４，２５が設けられている。接地センサ２４，２５のうち、接地センサ２４は足首関節１３の直下の箇所（踵）に設けられ、接地センサ２５は足平部１４の中足趾節関節１４ａ（足平部１４の親指の付け根の関節）の直下の箇所（つま先）に設けられている。これらの接地センサ２４，２５は、それを設けた箇所が接地しているか否かを示すＯＮ／ＯＦＦ信号を出力するセンサである。なお、各関節変位センサ２１，２２，２３、および接地センサ２４，２５の検出出力は信号線（図示省略）を介してセンサボックス１５の演算処理装置１８に入力される。

さらに、図１に示す如く、センサボックス１５から２本の光ファイバ２６，２７が胴体３の背面沿いに上方に向かって延設され、その先端部がそれぞれ腹部７の背面、胸部８の背面に図示しないベルト等の部材を介して固定されている。光ファイバ２６，２７は、それぞれ腰部６に対する腹部７、胸部８の傾き角（矢状面での傾き角）を検出する検出手段の構成要素である。これらの光ファイバ２６，２７を用いた腹部７、胸部８の傾き角の計測は次のような手法により行われる。光ファイバ２６を用いた腹部７の傾き角の計測手法を代表的に説明すると、該光ファイバ２６には、センサボックス１５内に設けられた発光／受光器１９（図２に示す）から所定の強度の光が導入されると共に、その導入された光が該光ファイバ２６の先端で反射されてセンサボックス１５側に戻ってくるようになっている。そして、その光の戻り量（戻った光の強度）が前記発光／受光器１９により検出されるようになっている。また、光ファイバ２６には、微小な光漏れを許容する複数の刻み部（図示しない）が長手方向に間隔を存して設けられており、光ファイバ２６に導入された光のうち、腰部６に対する腹部７の傾き角に応じた量の光がそれらの刻み部を介して光ファイバ２６から漏出する。このため、センサボックス１５側への光の戻り量は、腹部７の傾き角に応じたものとなり、その戻り量を検出することで、腰部６に対する腹部７の傾き角が計測される。すなわち、光ファイバ２５の光の戻り量に応じた発光／受光器１９の検出出力が、腰部６に対する腹部７の傾き角に応じたものとなり、それが該傾き角を示す信号として演算処理装置１８に入力される。光ファイバ２７を用いた胸部８の傾き角の計測手法も同様である。

なお、前記関節変位センサ２１，２２，２３がそれぞれ検出する股関節９、膝関節１１、足首関節１３の回転角は、両足平部１４，１４を平行に前方に向けて人間１が直立姿勢で起立した状態（以下、人間１の基準姿勢状態という）を基準（ゼロ点）とする回転角である。光ファイバ２６，２７を用いて検出する腹部７、胸部８の傾き角についても同様である。

ここで、本実施形態で用いる人間１の剛体リンクモデルと座標系について説明しておく。図３はその剛体リンクモデルＳ１と座標系とを示している。なお、この剛体リンクモデルＳ１は、前記図１にも仮想線で併記されている。本実施形態では、剛体リンクモデルＳ１は、人間１を、９個の剛体要素と８個の関節要素とで構成される連結体として表現している。さらに詳説すると、剛体リンクモデルＳ１は、大別すると、人間１の各脚体２にそれぞれ対応する一対の脚体部Ｓ２，Ｓ２と、人間１の上体（腰部６から上側の部分）に対応する上体部ＳＵとから構成されている。上体部ＳＵは、人間１の腰部６に対応する剛体要素Ｓ６と腹部７に対応する剛体要素Ｓ７とを関節要素ＪＵ１で連結し、さらに、剛体要素Ｓ７と胸部８に対応する剛体要素Ｓ８とを関節要素ＪＵ２で連結してなる連結体として構成されている。以下、剛体要素Ｓ６，Ｓ７，Ｓ８をそれぞれ腰部要素Ｓ６、腹部要素Ｓ７、胸部要素Ｓ８と称し、関節要素ＪＵ１、ＪＵ２をそれぞれ上体下部関節ＪＵ１、上体上部関節ＪＵ２と称することがある。

この場合、腰部要素Ｓ６は、逆Ｔ字形となっており、その上端に前記上体下部関節ＪＵ１が設けられると共に、左右の両端に人間１の一対の股関節９，９に対応する一対の関節要素Ｊ９，Ｊ９（以下、単に股関節Ｊ９ということがある）が設けられている。つまり、腰部要素Ｓ６は、股関節Ｊ９，Ｊ９の間でそれらの中心を結ぶ線分方向（左右方向）に延在する部分Ｓ６ａとこの部分Ｓ６ａの中央から上体下部関節ＪＵ１に向かってほぼ上方に伸びる部分Ｓ６ｂとから構成されている。ここで、上体下部関節ＪＵ１は、人間１の腰部６と腹部７との境界付近で人間１の背骨上に想定した関節に対応するものであり、上体上部関節ＪＵ２は、腹部７と胸部８との境界付近で人間１の背骨上に想定した関節に対応するものである。人間１の胴体３の曲げ動作をつかさどる実際の背骨は多数の関節で構成されるが、剛体リンクモデルＳ１では、上体部ＳＵの曲げ動作は、上体下部関節ＪＵ１および上体上部関節ＪＵ２の２つの関節要素で行われる。そして、腹部要素Ｓ７は、上体下部関節ＪＵ１と上体上部関節ＪＵ２との間でそれらの中心を結ぶ線分方向に延在している。なお、胸部要素Ｓ８は、図１に示す如く、上体上部関節ＪＵ２から人間１の首の付け根（より詳しくは胴体３と首との境界付近での背骨上の部位）まで延在するものとされている。

剛体リンクモデルＳ１の各脚体部Ｓ２は、大腿部１０に対応する剛体要素Ｓ１０を前記股関節Ｊ９を介して腰部要素Ｓ６に連結し、下腿部１２に対応する剛体要素Ｓ１２を膝関節１１に対応する関節要素Ｊ１１を介して連結し、足平部１４に対応する剛体要素Ｓ１４を足首関節１３に対応する関節要素Ｊ１３を介して連結してなる連結体として構成されている。以下、剛体要素Ｓ１０，Ｓ１２，Ｓ１４をそれぞれ大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４と称し、関節要素Ｊ１１，Ｊ１３をそれぞれ単に膝関節Ｊ１１、足首関節Ｊ１３と称することがある。

この場合、大腿部要素Ｓ１０および下腿部要素Ｓ１２は、それぞれの両端の関節要素の間でそれらの中心を結ぶ線分方向に延在している。また、足平要素Ｓ１４の先端は、人間１の足平部１４の中足趾節関節１４ａ（以下、ＭＰ関節１４ａという）に対応しており、図１に示す如く、足首関節１３（Ｊ１３）から足平部１４の中足趾節関節１４ａ（以下、ＭＰ関節１４ａという）まで延在している。剛体リンクモデルＳ１では、足平要素Ｓ１４の先端は関節としての機能を持つものではないが、以下、便宜上、その先端をＭＰ関節Ｊ１４ａと称することがある。

以上の如く構成された剛体リンクモデルＳ１の各剛体要素及び各関節要素は、各関節要素の回転運動によって、その相互の位置関係および姿勢関係（向きの関係）が各剛体要素および各関節要素に対応する人間１の各部の相互の位置関係および姿勢関係とほぼ同一になるように運動可能とされている。この場合、上体下部関節ＪＵ１及び上体上部関節ＪＵ２は、それぞれ３軸回りの回転が可能とされており、その中の１軸を計測軸として、その計測軸回りの回転（図３中に各関節要素ＪＵ１，ＪＵ２に対応して記載した矢印（回転方向を表す矢印））を計測するようにしている。その計測軸は、本実施形態では、前記一対の股関節Ｊ９，Ｊ９の中心を結ぶ線分（腰部要素Ｓ６の部分Ｓ６ａの延在方向）と平行な軸である。また、各脚体部Ｓ２の股関節Ｊ９、膝関節Ｊ１１、および足首関節Ｊ１３はそれぞれ、左側の脚体部Ｓ２の各関節要素Ｊ９，Ｊ１１，Ｊ１３に関して代表的に図３中に記載した矢印（回転方向を表す矢印）で示す如く３軸回りの回転が可能とされている。

また、剛体リンクモデルＳ１では、その各剛体要素の重量および長さ（軸心方向の長さ）、各剛体要素の重心の位置（各剛体要素での位置）とがあらかじめ定められて、演算処理装置１８の図示しないメモリに記憶保持されている。図３の黒点Ｇ８，Ｇ７，Ｇ６，Ｇ１０，Ｇ１２，Ｇ１４はそれぞれ胸部要素Ｓ８、腹部要素Ｓ７、腰部要素Ｓ６、大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４の重心を例示的に示している。なお、腰部要素Ｓ６は前記したように逆Ｔ字形であるので、その長さについては、前記部分Ｓ６ａの長さと部分Ｓ６ｂの長さとがある。

各剛体要素の重量、長さ、重心の位置は、基本的にはそれぞれの剛体要素に対応する人間１の剛体相当部の重量、長さ、重心の位置とほぼ同一に設定されている。例えば、大腿部要素Ｓ１０の重量、長さ、重心の位置は、それぞれ人間１の大腿部１０の実際の重量、長さ、重心の位置とほぼ同一である。但し、重量および重心の位置は、人間１に本実施形態の装置を装備した状態での重量および重心の位置である。また、胸部要素Ｓ８の重量および重心の位置は、人間１の胸部８と両腕体４，４と頭部５とを合わせたものの重量および重心の位置である。補足すると、人間１の移動時の両腕体４，４の運動（腕を前後に振る動作）に伴う胸部要素Ｓ８の重心の位置変化は比較的小さく、該胸部要素Ｓ８のほぼ一定の位置に維持される。また、各剛体要素の重心の位置は、各剛体要素にあらかじめ固定して設定された後述の要素座標系での位置ベクトルとして、該要素座標系の各座標成分値で設定されている。

各剛体要素の重量、長さ、重心の位置は、基本的には、人間１の各部の寸法や重量の実測値に基づいて定めればよいが、人間１の身長や体重から、人間の平均的な統計データに基づいて推定するようにしてもよい。一般に、各剛体要素に対応する人間１の剛体相当部の重心の位置や重量、長さは、人間の身長や体重（全体重量）と相関性があり、その相関関係に基づいて人間１の身長および体重の実測データから各剛体要素に対応する人間１の剛体相当部の重心の位置や重量、長さを比較的精度よく推定することが可能である。

なお、図３では、便宜上、各重心Ｇ８，Ｇ７，Ｇ６，Ｇ１０，Ｇ１２，Ｇ１４は、それぞれに対応する剛体要素の軸心上に位置するように記載しているが、必ずしもその軸心上に位置するとは限らず、その軸心からずれた位置に存在してもよい。

本実施形態では、剛体リンクモデルＳ１に対して、次のような座標系があらかじめ設定されている。すなわち、図３に示す如く身体座標系ＢＣが腰部要素Ｓ６に固定して設定されている。この身体座標系ＢＣは、一対の股関節Ｊ１１，Ｊ１１の中心を結ぶ線分の中点（腰部要素Ｓ６の部分Ｓ６ａの中央点）を原点とし、その線分の方向をＹ軸、原点から上体下部関節ＪＵ１の中心に向かう方向をＺ軸、これらのＹ軸およびＺ軸に直交する方向をＸ軸とする３次元座標系（ＸＹＺ座標系）として設定されている。人間１の前記基準姿勢状態では、身体座標系ＢＣのＸ軸、Ｙ軸、Ｚ軸はそれぞれ人間１の前後方向、左右方向、上下方向（鉛直方向）に向き、ＸＹ平面は水平面である。

また、各剛体要素には、例えば参照符号Ｃ８，Ｃ７，Ｃ６，Ｃ１０，Ｃ１２，Ｃ１４で示すように要素座標系が固定的に設定されている。本実施形態では、腰部要素Ｓ６の要素座標系Ｃ６は身体座標系ＢＣと同一とされている。また、胸部要素Ｓ８、腹部要素Ｓ７、各大腿部要素Ｓ１０、各下腿部要素Ｓ１２、および各足平部要素Ｓ１４のそれぞれの要素座標系Ｃ８，Ｃ７，Ｃ１０，Ｃ１２，Ｃ１４はそれぞれ、上体上部関節ＪＵ２、上体下部関節ＪＵ１、膝関節Ｊ１１、足首関節Ｊ１３、ＭＰ関節Ｊ１４ａの中心点を原点とする３次元座標系（ＸＹＺ座標系）とされている。別の言い方をすれば、各脚体部Ｓ２については、その各剛体要素Ｓ１０，Ｓ１２，Ｓ１４の要素座標系Ｃ１０，Ｃ１２，Ｃ１４は、各剛体要素Ｓ１０，Ｓ１２，Ｓ１４の両端の関節要素のうち、腰部要素Ｓ６からより遠い側の関節要素の中心点を原点としている。また、上体部ＳＵの腹部要素Ｓ７および胸部要素Ｓ８のそれぞれの要素座標系Ｃ７，Ｃ８は、腹部要素Ｓ７および胸部要素Ｓ８のそれぞれの両端の関節要素のうち、腰部要素Ｓ６により近い側の関節要素の中心点を原点としている。なお、図３では、要素座標系Ｃ１０，Ｃ１２，Ｃ１４は図示の便宜上、右側脚体部Ｓ２についてのみ図示しているが、左側脚体部Ｓ２についても右側脚体部Ｍ２と同様に要素座標系が設定されている。

また、要素座標系Ｃ８，Ｃ７は、それぞれ胸部要素Ｓ８、腹部要素Ｓ７の延在方向（軸心方向）にＺ軸が設定されると共に、Ｙ軸が身体座標系ＢＣのＹ軸と同一方向に設定されている。また、要素座標系Ｃ１０，Ｃ１２，Ｃ１４はそれぞれ大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４の延在方向（軸心方向）にＺ軸が設定されると共に、Ｙ軸が股関節Ｊ９、膝関節Ｊ１１及び足首関節Ｊ１３のそれぞれの中心点を含む平面（以下、脚平面ということがある）の法線方向に設定されている。上記のいずれの要素座標系Ｃ８，Ｃ７，Ｃ１０，Ｃ１２，Ｃ１４でも、Ｘ軸はＹ軸及びＺ軸に直交する方向に設定されている。以下の説明では、各要素座標系Ｃ８，Ｃ７，Ｃ６，Ｃ１０，Ｃ１２，Ｃ１４をそれぞれ胸部座標系Ｃ８、腹部座標系Ｃ７、腰部座標系Ｃ６、大腿部座標系Ｃ１０、下腿部座標系Ｃ１２、足平部座標系Ｃ１４と称することがある。

なお、要素座標系Ｃ８，Ｃ７，Ｃ１０，Ｃ１２，Ｃ１４は、必ずしも上記の如く設定する必要はなく、基本的にはその原点や各軸の向きの設定は任意でよい。

図４は前記演算処理装置１８の演算処理機能を示すブロック図である。同図に示すように、演算処理装置１８は、前記各関節変位センサ２１，２２，２３および発光／受光器１９の検出出力を基に後述する座標変換のための変換テンソルを作成する変換テンソル作成手段２８と、その変換テンソルを用いて前記剛体リンクモデルＳ１の各関節要素および各剛体要素の重心の身体座標系ＢＣでの位置ベクトルの値（座標成分値）を求める関節・要素重心位置算出手段２９と、前記加速度センサ１６及びジャイロセンサ１７の検出出力を基に身体座標系ＢＣの原点の加速度ベクトル（並進加速度）の値（身体座標系ＢＣでの座標成分値）を求める身体座標系加速度算出手段３０と、前記加速度センサ１６及びジャイロセンサ１７の検出出力を基に身体座標系ＢＣの鉛直方向に対する傾斜角を算出する身体座標系傾斜角算出手段３１とを備えている。

さらに演算処理装置１８は、関節・要素重心位置算出手段２９が求めた各剛体要素の重心の位置ベクトルの値を用いて身体座標系ＢＣでの剛体リンクモデルＳ１の全体重心（人間１の全体重心）の位置ベクトルの値を求めると共に、その位置ベクトルの値の時系列データと身体座標系加速度算出手段３０が求めた身体座標系ＢＣの原点の加速度ベクトルの値とを用いて該全体重心の加速度ベクトル（並進加速度）の値（身体座標系ＢＣでの座標成分値）を求める全体／要素重心運動算出手段３２を備えている。なお、全体／要素重心運動演算算出手段３２では、関節・要素重心位置算出手段２９が求めた各大腿部要素Ｓ１０、下腿部要素Ｓ１２および足平部要素Ｓ１２のそれぞれの重心の位置ベクトルの値の時系列データと身体座標系加速度算出手段３０が求めた身体座標系ＢＣの原点の加速度ベクトルの値とを用いて各大腿部要素Ｓ１０、下腿部要素Ｓ１２および足平部要素Ｓ１４の重心の加速度ベクトル（並進加速度）の値（身体座標系ＢＣでの座標成分値）も求められる。

また、演算処理装置１８は、関節・要素重心位置算出手段２９が求めた各足首関節Ｊ１３の位置ベクトルの値と全体／要素重心運動算出手段３２が求めた全体重心の位置ベクトルの値およびその加速度ベクトルの値と前記接地センサ２４，２５の検出出力とを用いて人間１の各脚体２，２に作用する床反力ベクトル（並進床反力）の身体座標系ＢＣでの値（座標成分値）を推定する床反力推定手段３３と、関節・要素重心位置算出手段２９が求めた各足首関節Ｊ１３および各ＭＰ関節Ｊ１４ａの位置ベクトルの値と身体座標系傾斜角算出手段３１が求めた身体座標系ＢＣの傾斜角と全体／要素重心運動算出手段３２が求めた全体重心の位置ベクトルの値と接地センサ２４，２５の検出出力とを用いて各脚体２に作用する床反力ベクトルの作用点（以下、単に床反力作用点という）の位置ベクトルの身体座標系ＢＣでの値を求める床反力作用点推定手段３４とを備える。

そして、演算処理装置１８は、変換テンソル作成手段２８が作成した変換テンソルと全体／要素重心運動算出手段３０が求めた各大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４のそれぞれの重心の加速度ベクトルと床反力推定手段３３が求めた床反力ベクトルの値と床反力作用点推定手段３４が求めた床反力作用点の位置ベクトルの値とを用いて各脚体２の足首関節１３、膝関節１１および股関節９に作用する関節モーメントを推定する関節モーメント推定手段３５を備えている。

詳細は後述するが、演算処理装置１８は、上記各手段２８〜３５の演算処理を所定の演算処理周期で逐次実行し、各演算処理周期において最終的に関節モーメント推定手段３５により関節モーメントの推定値を逐次算出する。

次に演算処理装置１８の各手段の詳細な演算処理と併せて本実施形態の装置の作動を説明する。なお、以下の説明において、一般的に、ベクトル量をある座標系Ｃaから別の座標系Ｃbに座標変換する変換テンソル、すなわち座標系Ｃaの成分値で表されるベクトル量を座標系Ｃbの成分値で表されるベクトル量に変換するテンソルを「Ｒ(Ｃa→Ｃb)」というように表記する。また、ある座標系Ｃaで見たある点Ｐもしくは部位Ｐの位置ベクトルをＵ(P/Ca)というように表記する。また、ある座標系Ｃaの座標成分値で表される、物体Ｑもしくは部位Ｑの作用力、加速度等の物理量のベクトルＡをＡ(Q/Ca)というように表記する。この場合、位置ベクトルＵ(P/Ca)や、物理量ベクトルＡ(Q/Ca)の座標系Ｃａでの座標成分値を表すときは、各座標軸の名称であるｘ、ｙ、ｚをさらに付加して表記する。例えば、位置ベクトルＵ(P/Ca)のＸ座標成分は、Ｕ(P/Ca)ｘというように表記する。

また、前記各要素座標系Ｃ８，Ｃ７，Ｃ６，Ｃ１０，Ｃ１２，Ｃ１４をそれぞれ対応する部位の名称を用いてＣ＿胸部、Ｃ＿腹部、Ｃ＿腰部、Ｃ＿大腿部、Ｃ＿下腿部、Ｃ＿足平部と称することがある。このことは、各剛体要素Ｓ８，Ｓ７，Ｓ６，Ｓ１０，Ｓ１２，Ｓ１４、各剛体要素Ｓの重心Ｇ８，Ｇ７，Ｇ６，Ｇ１０，Ｇ１２，Ｇ１４についても同様とする。例えば腰部剛体要素Ｓ８およびその重心Ｇ８をそれぞれＳ＿腰部、Ｇ＿腰部と表記することがある。なお、大腿部１０、下腿部１２、足平部１４に関するものについては、その左右を区別する必要があるときは、「右」、「左」をさらに付加して記述する。例えば右側大腿部要素Ｓ１０をＳ＿右大腿部と称することがある。また、股関節Ｊ９、膝関節Ｊ１１、足首関節Ｊ１３、およびＭＰ関節Ｊ１４ａをそれぞれＪ＿股、Ｊ＿膝、Ｊ＿足首、Ｊ＿ＭＰと称することがある。この場合も左右を区別する必要があるときは、上記と同様、「右」、「左」をさらに付加して表記する。

演算処理装置１８は、所定の演算処理周期で前記各関節変位センサ２１，２２，２３、発光／受光器１９、加速度センサ１６、ジャイロセンサ１７の検出出力を図示しないＡ／Ｄ変換器を介して取り込むと共に、各接地センサ２４，２５の検出出力（ＯＮ／ＯＦＦ信号）を取り込む。そして、まず、前記変換テンソル作成手段２８および関節・要素重心位置算出手段２９の演算処理を順次実行する。

変換テンソル作成手段２８の演算処理では、各要素座標系と身体座標系ＢＣとの間のベクトル量の座標変換を行うための変換テンソルが作成される。この変換テンソルは、次の手順で作成される。まず、各関節変位センサ２１，２２，２３の検出出力を基に、それぞれ各脚体２の股関節９、膝関節１１および足首関節１３のそれぞれの回転角（３軸回りの回転角）が剛体リンクモデルＳ１の各股関節Ｊ９、膝関節Ｊ１１および足首関節Ｊ１３の回転角として把握されると共に、発光／受光器１９の検出出力を基に、剛体リンクモデルＳ１の腰部要素Ｍ６に対する腹部要素Ｓ７と胸部要素Ｓ８の傾斜角（詳しくは腰部要素座標系Ｃ６のＺ軸方向に対する矢状面（ＸＺ平面）上での傾斜角）が把握される。そして、これらの把握された回転角および傾斜角を用いて、剛体要素の要素座標系の間でベクトル量の座標変換を行うための変換テンソルが作成される。この要素座標系間変換テンソルは、本実施形態では、足平部座標系Ｃ１４から下腿部座標系Ｃ１２への変換テンソルＲ(Ｃ＿足平部→Ｃ＿下腿部）と、下腿部座標系Ｃ１２から大腿部座標系Ｃ１０への変換テンソルＲ(Ｃ＿下腿部→Ｃ＿大腿部)と、大腿部座標系Ｃ１０から腰部座標系Ｃ６への変換テンソルＲ(Ｃ＿大腿部→Ｃ＿腰部)と、腹部座標系Ｃ７から腰部座標系Ｃ６への変換テンソルＲ(Ｃ＿腹部→Ｃ＿腰部）と、胸部座標系Ｃ８から腰部座標系Ｃ６への変換テンソルＲ(Ｃ＿胸部→Ｃ＿腰部）とから構成される。なお、Ｒ(Ｃ＿足平部→Ｃ＿下腿部），Ｒ(Ｃ＿下腿部→Ｃ＿大腿部)、Ｒ(Ｃ＿大腿部→Ｃ＿腰部)は、左右の脚体部Ｓ２毎に各別に作成される。

ここで作成される要素座標系間変換テンソルの一例を示すと、例えば図５に示すように膝関節１１の関節変位センサ２２の検出出力から把握される膝関節１１の回転角（膝関節Ｊ１１の回転角）が、大腿部座標系Ｃ１０のＹ軸（図５の紙面に垂直な軸）回りにθyであるとする。但し、この例では要素座標系Ｃ１０のＸ軸回り、Ｚ軸回りの膝関節１１の回転角は０であるとする。また、膝関節１１のＹ軸回り回転角は、脚体２が伸びる方向を正方向としており、図示の例ではθy＜０である。このとき、変換テンソルＲ(Ｃ＿下腿部→Ｃ＿大腿部)は、次式（１）の如く３次の行列で表される。

なお、図５の例では、膝関節１１の１軸回り（Ｙ軸回り）の回転のみを想定したため、変換テンソルＲ(Ｃ＿下腿部→Ｃ＿大腿部)は式（１）のような簡単な行列で表されるが、一般にはより複雑な形式になる。

また、例えば変換テンソルＲ(Ｃ＿腹部→Ｃ＿腰部）は、腰部６に対する腹部７の傾斜角をθy（但し傾斜角θyは人間１の前方側への傾斜方向を正方向とする）とおくと、前記式（１）の右辺の行例と同じ形の行列で表される。変換テンソルＲ(Ｃ＿胸部→Ｃ＿腰部）も同様である。補足すると、本実施形態では、剛体リンクモデルＳ１の上体下部関節ＪＵ１および上体上部関節ＪＵ２を１軸回り（Ｃ＿腹部およびＣ＿胸部のＹ軸回り）の回転による腹部要素Ｓ７および胸部要素Ｓ８の腰部要素Ｓ６に対する傾斜角を計測するようにしたため、変換テンソルＲ(Ｃ＿腹部→Ｃ＿腰部）およびＲ(Ｃ＿胸部→Ｃ＿腰部）は常に前記式（１）の右辺の行例と同じ形の行列で表される。但し、上体下部関節ＪＵ１および上体上部関節ＪＵ２をそれぞれ例えば２軸回り（例えばＣ＿腹部およびＣ＿胸部のＹ軸とＸ軸との２軸回り）の回転が可能なものとして、腹部要素Ｍ７および胸部要素Ｍ８の２軸回りの傾斜角を計測するようにしてもよい。

次いで、上記の如く求めた要素座標系間変換テンソルから、各要素座標系で記述された（各要素座標系の座標成分値で記述された）ベクトル量を身体座標系ＢＣに座標変換する（ベクトル量を身体座標系ＢＣの座標成分値での記述に変換する）ための変換テンソルが求められる。

この場合、本実施形態では腰部座標系Ｃ６（Ｃ＿腰部）は身体座標系ＢＣに等しいものとしたので、Ｃ＿腰部から身体座標系ＢＣへの変換テンソルＲ(Ｃ＿腰部→ＢＣ)は３次の単位行列で表される。また、腹部座標系Ｃ７（Ｃ＿腹部）および胸部座標系Ｃ８（Ｃ＿胸部）のそれぞれから身体座標系ＢＣへの変換テンソルＲ(Ｃ＿腹部→ＢＣ），Ｒ(Ｃ＿胸部→ＢＣ）は、それぞれ先に求めた要素座標系間変換テンソルＲ(Ｃ＿腹部→Ｃ＿腰部），Ｒ(Ｃ＿胸部→Ｃ＿腰部）と同一である。そして、他の要素座標系Ｃ１０（Ｃ＿大腿部），Ｃ１２（Ｃ＿下腿部），Ｃ１４（Ｃ＿足平部）から身体座標系ＢＣへの変換テンソルは、次式（２ａ）〜（２ｃ）の如く、腰部要素Ｓ６寄りのものから順番に決定される。

Ｒ(Ｃ＿大腿部→BC)＝Ｒ(Ｃ＿腰部→BC)×Ｒ(Ｃ＿大腿部→Ｃ＿腰部) ……（２ａ）
Ｒ(Ｃ＿下腿部→BC)＝Ｒ(Ｃ＿大腿部→BC)×Ｒ(Ｃ＿下腿部→Ｃ＿大腿部)
……（２ｂ）
Ｒ(Ｃ＿足平部→BC)＝Ｒ(Ｃ＿下腿部→BC)×Ｒ(Ｃ＿足平部→Ｃ＿下腿部)
……（２ｃ）

なお、Ｒ(Ｃ＿大腿部→BC)，Ｒ(Ｃ＿下腿部→BC)，Ｒ(Ｃ＿足平部→BC)は、各脚体部Ｓ２毎に各別に求められる。補足すると、各要素座標系から身体座標系ＢＣへの変換テンソルを転置したものが逆変換（身体座標系ＢＣから該要素座標系への座標変換）を行うための変換テンソルとなる。例えばＲ(BC→Ｃ＿大腿部)＝Ｒ(Ｃ＿大腿部→BC)^T（Ｔは転置を意味する）である。

上記変換テンソル作成手段２８の演算処理に続く関節・重心位置算出手段２９の演算処理では、上記の如く求めた変換テンソルと、あらかじめメモリに記憶された各剛体要素の長さおよび重心位置（各要素座標系での位置）とから各関節要素および各剛体要素の重心の身体座標系ＢＣでの位置ベクトルが求められる。なお、ここで位置ベクトルを求める関節要素には前記ＭＰ関節Ｊ１４ａが含まれる。

各関節要素の位置ベクトルの算出は次のように行われる。例えば左側脚体部Ｓ２の各関節要素Ｊ９，Ｊ１１，Ｊ１３の位置ベクトルの算出について説明する。腰部要素Ｓ６の、両股関節Ｊ９，Ｊ９間の部分Ｓ６ａ長さをＬ６ａとおくと、Ｃ＿腰部における左側股関節Ｊ６の位置ベクトルＵ(Ｊ＿左股/Ｃ＿腰部)は、（０，Ｌ６ａ／２，０）^Tである。従って、身体座標系ＢＣにおける左側股関節Ｊ９の位置ベクトルＵ(Ｊ＿左股/ＢＣ)は、次式（３）により求められる。

Ｕ(Ｊ＿左股/BC)＝Ｒ(Ｃ＿腰部→BC）×Ｕ(Ｊ＿左股/Ｃ＿腰部）
＝Ｒ(Ｃ＿腰部→BC）×（０，Ｌ６ａ／２，０）^T ……（３）

さらに、左側大腿部要素Ｓ１０の長さをＬ１０とおくと、左側大腿部座標系Ｃ１０（Ｃ＿左大腿部）における左側股関節Ｊ９の位置ベクトルＵ(Ｊ＿左股/Ｃ＿左大腿部）は、（０，０，Ｌ１０）^Tであるから、身体座標系ＢＣにおける左側膝関節Ｊ１１の位置ベクトルＵ(Ｊ＿左膝/ＢＣ)は、次式（４）により求められる。

Ｕ(Ｊ＿左膝/BC)＝Ｕ(Ｊ＿左股/BC)
＋Ｒ(Ｃ＿左大腿部→BC）×（−Ｕ(Ｊ＿左股/Ｃ＿左大腿部)）
＝Ｕ(Ｊ＿左股/BC)
＋Ｒ(Ｃ＿左大腿部→BC）×（０，０，−Ｌ１０）^T
……（４）

以下同様にして、左側足首関節Ｊ１３、左側ＭＰ関節Ｊ１４ａの身体座標系ＢＣでの位置ベクトルＵ(Ｊ＿左足首/BC)、Ｕ(Ｊ＿左ＭＰ/BC)がそれぞれ次式（５），（６）により順番に算出される。

Ｕ(Ｊ＿左足首/BC)＝Ｕ(Ｊ＿左膝/BC)
＋Ｒ(Ｃ＿下腿→BC)×（−Ｕ(Ｊ＿左膝/Ｃ＿左下腿部))
＝Ｕ(Ｊ＿左膝/BC)
＋Ｒ(Ｃ＿下腿→BC)×（０，０，−Ｌ１２）^T ……（５）
Ｕ(Ｊ＿左ＭＰ/BC)＝Ｕ(Ｊ＿左足首/BC)
＋Ｒ(Ｃ＿左足平部→BC)×（−Ｕ(Ｊ＿左足首/Ｃ＿左足平部)）
＝Ｕ(Ｊ＿左足首/BC)
＋Ｒ(Ｃ＿左足平部→BC)×（０，０，−Ｌ１４）^T ……（６）

なお、式（５）のＬ１２、式（６）のＬ１４はそれぞれ左側下腿部要素Ｓ１２、左側足平部要素Ｓ１４の長さである。右側脚体部Ｓ２の各関節要素の位置ベクトルも上記と同様に求められる。

さらに、上体部ＳＵの各関節要素の位置ベクトルも上記と同様に求められる。すなわち、上体下部関節ＪＵ１および上体上部関節ＪＵ２の身体座標系ＢＣでの位置ベクトルは、それぞれ次式（７），（８）により順番に求められる。

Ｕ(JU1/BC)＝Ｒ(Ｃ＿腰部→BC)・Ｕ(JU1/Ｃ＿腰部)
＝Ｒ(Ｃ＿腰部→BC)・（０，０，Ｌ６ｂ）^T ……（７）
Ｕ(JU2/BC)＝Ｕ(JU1/BC)＋Ｒ(Ｃ＿腹部→BC)・Ｕ(JU2/Ｃ＿腹部)
＝Ｕ(JU1/BC)＋Ｒ(Ｃ＿腹部→BC)・（０，０，Ｌ７）^T ……（８）

なお、式（７）のＬ６ｂは、腰部要素Ｓ６の部分Ｓ６ｂの長さである。また、式（８）のＬ７は腹部要素Ｓ７の長さである。

また、各剛体要素の重心の、身体座標系ＢＣでの位置ベクトルの算出は次のように行われる。すなわち、腰部要素Ｓ６、大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４のそれぞれの重心の身体座標系ＢＣでの位置ベクトルＵ(Ｇ＿腰部/BC)，Ｕ(Ｇ＿大腿部/BC)，Ｕ(Ｇ＿下腿部/BC)，Ｕ(Ｇ＿足平部/BC)は、それぞれ前記式（３）〜（６）の右辺のＵ(Ｊ＿左股/Ｃ＿腰部)、Ｕ(Ｊ＿左股/Ｃ＿左大腿部)、Ｕ(Ｊ＿左膝/Ｃ＿左下腿部)、Ｕ(Ｊ＿左足首/Ｃ＿左足平部)を、それぞれ各要素座標系であらかじめ設定されている各剛体要素の重心の位置ベクトルＵ(Ｇ＿腰部/Ｃ＿腰部)、Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)、Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)、Ｕ(Ｇ＿足平部/Ｃ＿足平部）で置き換えた式を演算することで求められる。なお、Ｇ＿大腿部，Ｇ＿下腿部，Ｇ＿足平部の位置ベクトルの算出は、各脚体部Ｓ２毎に各別に行われる。

また、腹部要素Ｓ７、胸部要素Ｓ８のそれぞれの重心Ｇ７，Ｇ８の身体座標系ＢＣでの位置ベクトルＵ(Ｇ＿腹部/BC)，Ｕ(Ｇ＿胸部/BC)は、それぞれ次式（９），（１０）により求められる。

Ｕ(Ｇ＿腹部/BC)＝Ｕ(JU1/BC)＋Ｒ(Ｃ＿腹部→BC)・Ｕ(Ｇ＿腹部/Ｃ＿腹部)
……（９）
Ｕ(Ｇ＿胸部/BC)＝Ｕ(JU2/BC)＋Ｒ(Ｃ＿胸部→BC)・Ｕ(Ｇ＿胸部/Ｃ＿胸部)
……（１０）

以上が変換テンソル作成手段２８および関節・要素重心位置算出手段２９の演算処理である。なお、以上のように関節・要素重心位置算出手段２９で算出される各関節要素と各剛体要素の重心との位置ベクトルは、それぞれに対応する人間１の実際の部位の、身体座標系ＢＣで見た位置ベクトルとしての意味を持つ。

演算処理装置１８は、上記した変換テンソル作成手段２８および関節・要素重心位置算出手段２９の演算処理と並行して、身体座標系加速度算出手段３０および身体座標系傾斜角算出手段３１の演算処理を実行する。

身体座標系加速度算出手段３０の演算処理では、加速度センサ１６の検出出力から把握される３軸方向の加速度（並進加速度）とジャイロセンサ１７の検出出力から把握される３軸回りの角速度とから次のように身体座標系ＢＣの原点の加速度ベクトルの身体座標系ＢＣでの値（座標成分値）が求められる。まず、各センサ１６，１７がそれぞれ検出する加速度、角速度はそれらのセンサ１６，１７に対して固定された３軸の座標系（以下、センサ座標系ＳＣ又はＣ＿センサという）であらわされるベクトル量であるので、それを身体座標系ＢＣでの値に変換する。その変換は、腰部６に対する加速度センサ１６およびジャイロセンサ（角速度センサ）１７の相対的な取り付け位置関係（腰部座標系Ｃ６（＝身体座標系ＢＣ）に対するセンサ座標系ＳＣの相対的姿勢関係）に応じてあらかじめ設定された変換テンソルをセンサ座標系ＳＣでそれぞれ検出された加速度ベクトル、角速度ベクトルに乗算することで行われる。すなわち、センサ座標系ＳＣでの加速度ベクトルの検出値をACC(センサ/ＳＣ)、それを身体座標系ＢＣに変換した加速度ベクトルをACC(センサ/ＢＣ)、センサ座標系ＳＣでの角速度ベクトルの検出値をω(センサ/ＳＣ)、それを身体座標系ＢＣに変換した角速度ベクトルをω(センサ/ＢＣ)とおくと、加速度ベクトルACC(センサ/ＢＣ)、角速度ベクトルω(センサ/ＢＣ)は、それぞれ次式（１１），（１２）により求められる。ここで、ACC(センサ/BC)、ω(センサ/BC)は、より詳しくは、それぞれ加速度センサ１６、ジャイロセンサ１７の箇所の加速度ベクトル、角速度ベクトルであり、その意味で、ACC、ωの表記に「センサ」を付加している。なお、この例では加速度センサ１６、ジャイロセンサ１７の箇所はほぼ同一箇所とし、センサ座標系ＳＣは両センサ１６，１７について同じ座標系としている。

ACC(センサ/BC)＝Ｒ(SC→BC)・ACC(センサ/SC) ……（１１）
ω(センサ/BC)＝Ｒ(SC→BC)・ω(センサ/SC) ……（１２）

ここで、変換テンソルＲ(SC→BC)はセンサ座標系ＳＣと身体座標系ＢＣとの相対的な姿勢関係（詳しくは、センサ座標系ＳＣの各軸の身体座標系ＢＣの各軸に対する傾き角）から前記した要素座標系間変換テンソル（Ｒ(C＿大腿部→Ｃ＿腰部)等）と同様に求められる。例えば、センサ座標系ＳＣの３軸（ＸＹＺ軸）が図６に示す如く身体座標系ＢＣのＹ軸（図６の紙面に垂直な軸）回りに角度θyだけ傾いている場合には、変換テンソルＲ(SC→BC)は前記式（１）の右辺と同じ形の行列で表される。この場合、加速度センサ１６およびジャイロセンサ１７は身体座標系ＢＣを設けた腰部８に固定されているので、センサ座標系ＳＣの各軸の身体座標系ＢＣの各軸に対する傾き角は、加速度センサ１６およびジャイロセンサ１７の腰部８への取り付け時に実測されて判明しており、その傾き角を基にあらかじめ変換テンソルＲ(SC→BC)が求められて、演算処理装置１８の図示しないメモリに記憶保持されている。補足すると、加速度センサ１６やジャイロセンサ１７を腰部６以外の部位（剛体リンクモデルＳ１のいずれかの剛体要素に対応する剛体相当部）に装着してもよい。この場合には、加速度ベクトルACC(センサ/BC)および角速度ベクトルω(センサ/BC)は、センサ座標系ＳＣでの検出値を加速度センサ１６やジャイロセンサ１７を装着した剛体要素の要素座標系での値に変換した後、さらに前記変換テンソル作成手段２８で求めた変換テンソルによって身体座標系ＢＣでの値に変換すればよい。

身体座標系加速度算出手段３０の演算処理では、上記の如く加速度ベクトルACC(センサ/BC)および角速度ベクトルω(センサ/BC)を求めた後、次式（１３）によって、身体座標系ＢＣの原点の加速度ベクトルACC(BCO/BC)を求める。「BCO」は身体座標系ＢＣの原点を表す符号である。

この式（１３）中の、Ｕ(センサ/ＢＣ)は、身体座標系ＢＣでの加速度センサ１６およびジャイロセンサ１７の位置ベクトルであり、Ｕ(センサ/ＢＣ)ｘ、Ｕ(センサ/BC)ｙ、Ｕ(センサ/BC)ｚはそれぞれ、前述した本明細書でのベクトルの座標成分値の表記手法の定義にしたがって、Ｕ(センサ/ＢＣ)の身体座標系ＢＣでの各座標成分値である。Ｕ(センサ/ＢＣ)は、加速度センサ１６およびジャイロセンサ１７の腰部８への取り付け時に実測されて演算処理装置１８のメモリに記憶保持されている。また、ω(センサ/ＢＣ)ｘ、ω(センサ/BC)ｙ、ω(センサ/BC)ｚはそれぞれ先に求めた角速度ベクトルω(センサ/ＢＣ)の各座標成分値である。また、ω(センサ/BC)’は、ω(センサ/BC)の１階微分値を示しており、その値は、演算処理装置１８の演算処理周期毎に前記式（１２）により求めるω(センサ/BC)の時系列データから算出される。補足すると、腰部要素Ｓ６内のどの部分でも角速度は同一であり、例えば腰部要素Ｓ６に固定されている身体座標系ＢＣの原点ＢＣＯの角速度ω(BCO/BC)は、ω(センサ/BC)に等しい。

なお、加速度センサ１６は重力に伴う加速度も検出するので、上記のように求められた加速度ベクトルACC(BCO/BC)には、重力による慣性加速度成分が含まれる。また、本実施系形態では、腰部要素Ｓ６の角速度を考慮して身体座標系ＢＣの原点ＢＣＯの加速度ベクトルACC(BCO/BC)を求めるようにしたが、腰部要素Ｓ６の角速度やその変化率は比較的小さいので、前記式（１１）で求めたACC(センサ/BC)をそのまま身体座標系ＢＣの原点ＢＣＯの加速度ベクトルACC(BCO/BC)としてもよい。

また、前記身体座標系傾斜角算出手段３１の演算処理では、加速度センサ１６およびジャイロセンサ１７の検出出力から所謂カルマンフィルタによって鉛直方向（重力方向）に対する腰部要素Ｓ６の傾斜角（身体座標系ＢＣのＺ軸の傾斜角）が算出される。この算出手法は公知であるのでここでの説明は省略する。なお、ここで算出される傾斜角は、前後方向の水平軸と左右方向の水平軸との２軸回りの傾斜角である。

次に、演算処理装置１８は、全体／要素重心運動算出手段３０の演算処理を実行する。この全体／要素重心運動算出手段３０の演算処理では、まず、前記関節・要素重心位置算出手段２９によって求められた各剛体要素の重心位置（身体座標系ＢＣでの位置ベクトル）と、あらかじめ前述したように設定された各剛体要素の重量とから、次式（１４）によって、剛体リンクモデルＳ１の全体重心（人間１の全体重心。以下、Ｇ＿全体と表記することがある）の身体座標系ＢＣでの位置ベクトルＵ(Ｇ＿全体/BC)が求められる。

Ｕ(Ｇ＿全体/BC)＝{Ｕ(Ｇ＿胸部/BC)×ｍ＿胸部＋Ｕ(Ｇ＿腹部/BC)×ｍ＿腹部
＋Ｕ(Ｇ＿腰部/BC)×ｍ＿腰部＋Ｕ(Ｇ＿右大腿部/BC)×ｍ＿右大腿部
＋Ｕ(Ｇ＿左大腿部/BC)×ｍ＿左大腿部＋Ｕ(Ｇ＿右下腿部/BC)×ｍ＿右下腿部
＋Ｕ(Ｇ＿左下腿部/BC)×ｍ＿左下腿部＋Ｕ(Ｇ＿右足平部/BC)×ｍ＿右足平部
＋Ｕ(Ｇ＿左足平部/BC)×ｍ＿左足平部}／全体重量 ……（１４）

なお、ｍ＿胸部など、「ｍ＿○○」は○○の名称に対応する剛体要素の重量である。この式（１４）の如く、全体重心の位置ベクトルＵ(Ｇ＿全体/BC)は、剛体リンクモデルＳ１の各剛体要素の重心の身体座標系ＢＣでの位置ベクトルとその剛体要素の重量との積の総和を、人間１の全体重量（＝全ての剛体要素の重量の総和）で除算することで求められる。

全体／要素重心運動算出手段３０の演算処理では、さらに、上記のように求めた全体重心の位置ベクトルＵ(Ｇ＿全体/BC)の時系列データ（演算処理装置１８の演算処理周期毎のＵ(Ｇ＿全体/BC)の時系列値）から、Ｕ(Ｇ＿全体/BC)の２階微分値Ｕ(Ｇ＿全体/BC)''が算出される。この２階微分値Ｕ(Ｇ＿全体/BC)''は、身体座標系ＢＣの原点ＢＣＯに対する全体重心Ｇ＿全体の相対的な加速度を意味している。そして、この２階微分値Ｕ(Ｇ＿全体/BC)''、すなわち、身体座標系ＢＣの原点ＢＣＯに対するＧ＿全体の相対加速度を、前記身体座標系加速度算出手段３０で先に求めた身体座標系ＢＣの原点ＢＣＯの加速度ベクトルACC(BCO/BC)に加算することにより、Ｇ＿全体の実際の加速度ベクトルACC(Ｇ＿全体/BC)が算出される。すなわち、次式（１５）により、ACC(Ｇ＿全体/BC)が算出される。

ACC(Ｇ＿全体/BC)＝ACC(BCO/BC)＋Ｕ(Ｇ＿全体/BC)'' ……（１５）

さらに、全体／要素重心運動算出手段３０の演算処理では、関節・要素重心位置算出手段２９で先に求めた各脚体部Ｓ２の大腿部要素Ｓ１０、下腿部要素Ｓ１２、および足平部要素Ｓ１４のそれぞれの重心の位置ベクトル（身体座標系ＢＣでの位置ベクトル）の時系列データ（演算処理装置１８の演算処理周期毎の時系列値）から、それぞれの重心の位置ベクトルの２階微分値、すなわち、身体座標系ＢＣの原点ＢＣＯに対する各剛体要素の重心の相対加速度が算出される。そして、この各剛体要素の重心の相対加速度を、前記身体座標系加速度算出手段３０で先に求めた身体座標系ＢＣの原点ＢＣＯの加速度ベクトルACC(BCO/BC)に加算することにより、各剛体要素の重心の実際の加速度ベクトルが算出される。例えば、各大腿部要素Ｓ１０の重心Ｇ＿大腿部（Ｇ１０）の加速度ベクトルACC(Ｇ＿大腿部/BC)は、前記式（１５）と同様、Ｇ＿大腿部の位置ベクトルＵ(Ｇ＿大腿部/BC)の２階微分値Ｕ(Ｇ＿大腿部/BC)''にACC(BCO/BC)を加算することで算出される。下腿部要素Ｓ１２、足平部要素Ｓ１４についても同様である。

次に、演算処理装置１８は、前記床反力推定手段３３および床反力作用点推定手段３４の算出処理を実行する。床反力推定手段３３の演算処理では、まず、接地センサ２４，２５の検出出力に基づき、人間１の運動状態が両脚体２，２が接地する両脚支持状態であるか、一方の脚体２のみが接地する単脚支持状態であるかが判断される。すなわち、一方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力し、且つ、他方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力している場合には両脚接地状態であると判断される。また、両脚体２，２のうちの一方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力しており、且つ、他方の脚体２の接地センサ２４，２５の両者が接地有りを示すＯＮ信号を出力していない場合には、単脚支持状態であると判断される。そして、床反力推定手段３１の処理では、両脚支持状態であるか単脚支持状態であるかに応じて、各別の演算処理により各脚体２に作用する床反力ベクトルを推定する。

この床反力ベクトルの推定処理の基本的な考え方は、本願出願人が先に提案した特開２００３−８９０８３号公報等のものと同じであるが、本実施形態では、主に、その推定処理に用いる座標系等が同公報等に記載した手法と相違している。以下に図７および図８（ａ），（ｂ）を参照して説明する。図７は矢状面で見た人間１の単脚支持状態を例示しており、図８（ａ），（ｂ）はそれぞれ矢状面、前額面で見た人間１の両脚支持状態を例示している。なお、これらの図７及び図８では人間１は剛体リンクモデル状に模式化して示している。図７に示すように、人間１の運動状態が単脚支持状態であるときには、接地している脚体２（ここでは例えば右側脚体２であるとする）に作用する床反力ベクトルＦｒｆ(右脚体/BC)、すなわち、接地している右側脚体２に作用する床反力ベクトルを身体座標系ＢＣの座標成分値で表したものが、全体重心Ｇ＿全体の、身体座標系ＢＣでの並進運動に関する運動方程式を表す次式（１６）により算出される。

Ｆｒｆ(右脚体/BC)＝全体重量×ACC(Ｇ＿全体/BC) ……（１６）

すなわち、前記全体／要素重心運動算出手段３０で算出されたＧ＿全体の加速度ベクトルACC(Ｇ＿全体/BC)と人間１の全体重量（剛体リンクモデルＳ１の全体重量）とから式（１６）により床反力ベクトルＦｒｆ(右脚体/BC)が算出される。左側脚体２が接地している場合でも、単脚支持状態では同様に、式（１６）の右辺の演算によって、床反力ベクトルＦｒｆ(左脚体/BC)が算出される。この場合、前述したようにACC(Ｇ＿全体/BC)には、重力による慣性加速度成分が含まれ、また、床反力ベクトルＦｒｆを身体座標系ＢＣで表すので、重力加速度やその方向を考慮する必要はない。なお、接地していない側の脚体２に作用する床反力ベクトルＦｒｆは０である。また、図７では、図示の便宜上、身体座標系ＢＣのＺ軸を鉛直方向に記載しているが、式（１６）は身体座標系ＢＣの傾きにはよらない。

一方、図８（ａ），（ｂ）に示すように、両脚支持状態であるときには、右側脚体２に作用する床反力ベクトルＦｒｆ(右脚体/BC)と左側脚体２に作用する床反力ベクトルＦｒｆ(左脚体/BC)とが次の５つの関係式（１７）〜（２１）を基に算出される。

Ｆｒｆ(右脚体/BC)＋Ｆｒｆ(左脚体/BC)＝全体重量×ACC(Ｇ＿全体/BC)
……（１７）
Ｆｒｆ(右脚体/BC)ｘ：Ｆｒｆ(右脚体/BC)ｚ
＝Ｕ(Ｇ＿全体/BC)ｘ−Ｕ(Ｊ＿右足首/BC)ｘ
：Ｕ(Ｇ＿全体/BC)ｚ−Ｕ(Ｊ＿右足首/BC)ｚ……（１８）
Ｆｒｆ(左脚体/BC)ｘ：Ｆｒｆ(左脚体/BC)ｚ
＝Ｕ(Ｇ＿全体/BC)ｘ−Ｕ(Ｊ＿左足首/BC)ｘ
：Ｕ(Ｇ＿全体/BC)ｚ−Ｕ(Ｊ＿左足首/BC)ｚ……（１９）
Ｆｒｆ(右脚体/BC)ｙ：Ｆｒｆ(右脚体/BC)ｚ
＝Ｕ(Ｇ＿全体/BC)ｙ−Ｕ(Ｊ＿右足首/BC)ｙ
：Ｕ(Ｇ＿全体/BC)ｚ−Ｕ(Ｊ＿右足首/BC)ｚ……（２０）
Ｆｒｆ(左脚体/BC)ｙ：Ｆｒｆ(左脚体/BC)ｚ
＝ACC(Ｇ＿全体/BC)ｙ−Ｕ(Ｊ＿左足首/BC)ｙ
：Ｕ(Ｇ＿全体/BC)ｚ−Ｕ(Ｊ＿左足首/BC)ｚ……（２１）

ここで、これらの式（１７）〜（２１）の意味を説明すると、式（１７）は、全体重心Ｇ＿全体の、身体座標系ＢＣでの並進運動に関する運動方程式を表している。また、式（１８）〜（２１）は、図８（ａ），（ｂ）に示すように、床反力ベクトルＦｒｆ(右脚体/BC)および床反力ベクトルＦｒｆ(左脚体/BC)がそれぞれ右側脚体２の足首関節１３、左側脚体２の足首関節１３から全体重心Ｇ＿全体に向かうベクトルであると仮定して、換言すれば、床反力ベクトルＦｒｆと、左側足首関節１３から見たＧ＿全体の位置ベクトルとの向きが同じであると仮定して得られる幾何学的関係式である。この場合、式（１８），（１９）は矢状面（身体座標系ＢＣのＸＺ平面）で見た関係式であり、式（２０），（２１）は前額面（身体座標系ＢＣのＹＺ）平面で見た関係式である。なお、図８では、図示の便宜上、身体座標系ＢＣのＺ軸を鉛直方向に記載しているが、式（１７）〜（２１）は身体座標系ＢＣの傾きにはよらない。また、本実施形態では、各脚体２の足首関節１３は、該脚体２の下端部近傍の特定部としての意味を持つ。

両脚支持状態での床反力ベクトルＦｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)を求める場合には、それらのベクトルの座標成分値を未知数として、前記式（１７）〜（２１）により構成される連立方程式を解くことで、Ｆｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)が算出される。すなわち、Ｆｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)は、人間１の全体重量と全体／要素重心運動算出手段３０で求めたACC(Ｇ＿全体/BC)およびＵ(Ｇ＿全体/BC)と関節・要素重心位置算出手段２９で求めたＵ(Ｊ＿右足首/BC)およびＵ(Ｊ＿左足首/BC)とから算出される。このように、本実施形態では、両脚支持状態での床反力ベクトルＦｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)は、身体座標系ＢＣで記述される前記関係式（１７）〜（２１）に基づいて算出される。

なお、Ｆｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)のＺ軸成分は、矢状面に関する式（１８），（１９）、あるいは前額面に関する式（２０），（２１）のいずれを用いても求めることが可能である。

床反力作用点推定手段３４の演算処理では、まず、前記身体座標系傾斜角算出手段３１で算出された腰部要素Ｓ６の鉛直方向に対する傾斜角を基に、身体座標系ＢＣから絶対座標系ＩＣへの変換テンソルＲ(BC→IC)が作成される。ここで、絶対座標系ＩＣは、鉛直方向をＺ軸とする直交座標系で、前記基準姿勢状態において身体座標系ＢＣと各座標軸の向きが同一となる座標系である。なお、絶対座標系ＩＣから身体座標系ＢＣへの変換テンソルＲ(IC→BC)は変換テンソルＲ(BC→IC)の転置Ｒ(BC→IC)^Tである。

次いで、上記変換テンソルＲ(BC→IC)を用いて、前記全体／要素重心運動算出手段３２で先に求めた全体重心Ｇ＿全体の位置ベクトルＵ(Ｇ＿全体/BC)と、関節・要素重心位置算出手段２９で先に求めた各脚体部Ｓ２の足首関節Ｊ１３およびＭＰ関節Ｊ１４ａのそれぞれの位置ベクトルＵ(Ｊ＿足首/BC)，Ｕ(Ｊ＿ＭＰ/BC)とにそれぞれ上記変換テンソルＲ(BC→IC)を乗算することにより、全体重心Ｇ＿全体、各足首関節Ｊ１３およびＭＰ関節Ｊ１４ａの絶対座標系ＩＣで見た位置ベクトルＵ(Ｇ＿全体/IC)，Ｕ(Ｊ＿足首/IC)，Ｕ(Ｊ＿ＭＰ/IC)が算出される。なお、これらの位置ベクトルＵ(Ｇ＿全体/IC)，Ｕ(Ｊ＿足首/IC)，Ｕ(Ｊ＿ＭＰ/IC)は、身体座標系ＢＣと同じ原点を有する絶対座標系ＩＣでの位置ベクトルである。また、このとき、接地センサ２４，２５の検出出力により接地無しと判断される脚体２に関しては、位置ベクトルＵ(Ｊ＿足首/IC)，Ｕ(Ｊ＿ＭＰ/IC)を算出する必要はない。

次いで、接地センサ２４，２５の検出出力により接地有りと判断される各脚体２毎に、位置ベクトルＵ(Ｇ＿全体/IC)，Ｕ(Ｊ＿足首/IC)，Ｕ(Ｊ＿ＭＰ/IC)のＸ軸方向成分Ｕ(Ｇ＿全体/IC)ｘ，Ｕ(Ｊ＿足首/IC)ｘ，Ｕ(Ｊ＿ＭＰ/IC)ｘの大小関係に応じて、換言すれば、全体重心Ｇ＿全体、足首関節１３およびＭＰ関節１４ａの前後方向での相対的な水平位置関係に応じて、床反力作用点の位置ベクトル（絶対座標系ＩＣでの位置ベクトル）Ｕ(COP/IC)のＸ軸成分およびＹ軸成分が決定される。この決定手法を図９（ａ）〜（ｃ）および図１０を参照してさらに詳説する。なお、以下の説明では、左側脚体２が接地しているとする。図９（ａ）〜（ｃ）は矢状面で見た人間１の左脚体２が接地している状態（これらの図では単脚支持状態）を例示しており、図１０は図９（ｂ）の状態での接地側の足平部１４を平面視で見た図を示している。なお、図９及び図１０では人間１は剛体リンクモデル状に模式化して示している。

図９（ａ）に示すように、全体重心Ｇ＿全体が接地している左側脚体２のＭＰ関節１４ａよりも前方に存在する場合、すなわち、Ｕ(Ｇ＿全体/IC)ｘ＞Ｕ(Ｊ＿左ＭＰ/IC)ｘである場合には、該左側脚体２の足平部１４は、主にそのつま先側部分で踏ん張って接地している。この場合には、床反力作用点COPは、その足平部１４のＭＰ関節１４ａのほぼ直下の位置に存在する。そこで、この場合には、床反力作用点COPの位置ベクトルＵ(左COP/IC)のＸ，Ｙ軸成分はそれぞれＭＰ関節１４ａの位置ベクトルＵ(Ｊ＿左ＭＰ/IC)のＸ，Ｙ軸成分に等しいとする。すなわち、Ｕ(左COP/IC)ｘ＝Ｕ(Ｊ＿左ＭＰ/IC)ｘ、Ｕ(左COP/IC)ｙ＝Ｕ(Ｊ＿左ＭＰ/IC)ｙとする。

また、図９（ｃ）に示す如く、全体重心Ｇ＿全体が接地している左側脚体２の足首関節１３よりも後方に存在する場合、すなわち、Ｕ(Ｇ＿全体/IC)ｘ＜Ｕ(Ｊ＿左足首/IC)ｘである場合には、該左側脚体２の足平部１４は、主にその踵側部分で踏ん張って接地している。この場合には、床反力作用点COPは、その左側脚体２の足首関節１３のほぼ直下の位置に存在する。そこで、この場合には、床反力作用点COPの位置ベクトルＵ(左COP/IC)のＸ，Ｙ軸成分はそれぞれ足首関節１３の位置ベクトルＵ(Ｊ＿右足首/IC)のＸ，Ｙ軸成分に等しいとする。すなわち、Ｕ(左COP/IC)ｘ＝Ｕ(Ｊ＿左足首/IC)ｘ、Ｕ(左COP/IC)ｙ＝Ｕ(Ｊ＿左足首/IC)ｙとする。

また、図９（ｂ）に示すように、全体重心Ｇ＿全体が前後方向で左側脚体２の足首関節１３とＭＰ関節１４ａとの間に存在する場合、すなわち、Ｕ(Ｊ＿左ＭＰ/IC)ｘ≦Ｕ(Ｇ＿全体/IC)ｘ≦Ｕ(Ｊ＿左足首/IC)ｘである場合には、床反力作用点COPは、図示の矢状面上では、全体重心Ｇ＿全体のほぼ真下に存在する。そこで、この場合には、床反力作用点COPの位置ベクトルＵ(左COP/IC)のＸ軸成分は、全体重心Ｇ＿全体のＸ軸成分に等しいとする。すなわち、Ｕ(左右COP/IC)ｘ＝Ｕ(Ｇ＿全体/IC)ｘとする。そして、床反力作用点COPは、接地している左側脚体２の足平部１４と床面との接触面（この場合、足平部１４の底面のほぼ全面）に存在しており、その位置は、概ね、足首関節１３の中心点とＭＰ関節１４ａの中心点とを結ぶ線分を床面に投影した線分上に存在すると考えられる。そこで、床反力作用点COPの位置ベクトルＵ(右COP/IC)のＹ軸成分は、図１０に示す如く、左側脚体２に関する足平部要素Ｓ１４の軸心上（足首関節１３の中心点とＭＰ関節１４ａの中心点とを結ぶ線分上）で、全体重心Ｇ＿全体とＸ軸成分（絶対座標系ＩＣでのＸ軸成分）の値が同じになるような点ＰのＹ軸成分と等しいとする。このような位置ベクトルＵ(右COP/IC)のＹ軸成分の値は、次式（２２）に比例関係式に基づいて求められる。

Ｕ(左COP/IC)ｘ−Ｕ(Ｊ＿左足首/IC)ｘ：Ｕ(Ｊ＿左ＭＰ/IC)ｘ−Ｕ(Ｊ＿左足首/IC)ｘ
＝Ｕ(左COP/IC)ｙ−Ｕ(Ｊ＿左足首/IC)ｙ
：Ｕ(Ｊ＿左ＭＰ/IC)ｙ−Ｕ(Ｊ＿左足首/IC)ｙ ……（２２）

また、床反力作用点の位置ベクトルＵ(左COP/IC)のＺ軸成分は、左側脚体２の足首関節１３（足首要素Ｊ１３）からあらかじめ定めた所定値Ｈ０（＞０）だけ鉛直方向下方に離れた点のＺ軸成分に等しいとする。すなわち、Ｕ(右COP/IC)ｚ＝Ｕ(Ｊ＿左足首/IC)ｚ−Ｈ０とする。ここで、所定値Ｈ０は、前記基準姿勢状態（より正確には足平部１４の底面のほぼ全体を水平な床面上に接触させた状態）における床面から足首関節１３の中心までの鉛直方向距離であり、あらかじめ実測されて演算処理装置１８のメモリに記憶保持されている。所定値Ｈ０は左右の各脚体２毎に各別に実測してもよいが、いずれかの一方の脚体２について実測した値を左右の両脚体２で共通に使用してもよい。

本実施形態では、以上の如く、左側脚体２が接地している場合に該左側脚体に作用する床反力ベクトルＦｒｆの床反力作用点の位置ベクトルＵ(左COP/IC)が求められる。右側脚体２が接地している場合についても同様である。この場合、両脚接地状態では、各脚体２のそれぞれについて上記の如く床反力作用点の位置ベクトルが求められる。

なお、本実施形態では、床反力作用点の位置ベクトルＵ(COP/IC)のＺ軸成分を求めるために用いる前記所定値Ｈ０を一定値としたが、接地センサ２４，２５により、足平部１４のつま先側のみが接地していること、すなわち、接地センサ２５のみが接地有りを示すＯＮ信号を出力している場合には、上記所定値Ｈ０の代わりに、その接地している脚体２のついて、足首関節１３およびＭＰ関節１４ａのそれぞれの位置ベクトルＵ(Ｊ＿足首/IC)，Ｕ(Ｊ＿ＭＰ/IC)のＺ軸成分の差（Ｕ(Ｊ＿足首/IC)ｚ−Ｕ(Ｊ＿ＭＰ/IC)ｚ）、すなわち、足首関節１３とＭＰ関節１４ａとの鉛直方向距離を使用するようにしてもよい。このようにすると、Ｕ(COP/IC)の精度を高めることができる。

床反力作用点推定手段３４の演算処理では、最後に、上記の如く接地している各脚体２について求めた床反力作用点の位置ベクトルＵ(COP/IC)に、先に求めた変換テンソルＲ(BC→IC)の転置である逆変換テンソルＲ(IC→BC)を乗算することにより、床反力作用点の位置ベクトルの身体座標系ＢＣでの値Ｕ(COP/BC)が求められる。

次に、演算処理装置１８は、関節モーメント推定手段３５による演算処理を実行する。この関節モーメント推定手段３５の演算処理の概略を説明すると、各脚体部Ｓ２の足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの並進運動および回転運動に関する運動方程式に基づく逆動力学モデルの演算によって、足平部要素Ｓ１４、下腿部要素Ｓ１２、および大腿部要素Ｓ１０のそれぞれの腰部要素Ｓ６側の端点の関節要素Ｊ＿足首、Ｊ＿膝、Ｊ＿股の関節モーメントが順番に算出される。

以下、具体的に説明すると、足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの並進運動の運動方程式は次の式（２３）〜（２５）により与えられる。なお、以下の説明において、一般的に、足平部要素Ｓ１４、下腿部要素Ｓ１２、および大腿部要素Ｓ１０のそれぞれの剛体要素の両端のうち、腰部要素Ｓ６に近い側の一端部を「Ｐ＿○○」、遠い側の他端部「Ｄ＿○○」（○○は剛体要素を表す名称）というように表記することがある。例えば下腿部要素Ｓ１２の膝関節Ｊ＿膝（Ｊ１１）側の端部を「Ｐ＿下腿部」、足首関節Ｊ＿足首（Ｊ１３）側の端部を「Ｄ＿下腿部」というように表記する。

Ｆ(Ｐ＿足平部/BC)＝ｍ＿足平部×ACC(Ｇ＿足平部/BC)−Ｆ(Ｄ＿足平部/BC)
……（２３）
Ｆ(Ｐ＿下腿部/BC)＝ｍ＿下腿部×ACC(Ｇ＿下腿部/BC)−Ｆ(Ｄ＿下腿部/BC)
……（２４）
Ｆ(Ｐ＿大腿部/BC)＝ｍ＿大腿部×ACC(Ｇ＿大腿部/BC)−Ｆ(Ｄ＿大腿部/BC)
……（２５）

ここで、上記各式（２３）〜（２５）中に現れる２つのＦ（Ｐ＿○○/BC)、Ｆ（Ｄ＿○○/BC)は、その○○で表される名称の剛体要素の両端が、それぞれに接触する物体から受ける反力（身体座標系ＢＣで表した並進力ベクトル）を意味している。この場合、Ｆ(Ｄ＿足平部/BC)は、前記床反力推定手段３１で求めた床反力ベクトルＦｒｆ(脚体/BC)に等しい。また、Ｆ(Ｄ＿下腿部/BC)＝−Ｆ(Ｐ＿足平部/BC)、Ｆ(Ｄ＿大腿部/BC)＝−Ｆ(Ｐ＿下腿部/BC)である。

従って、床反力推定手段３１で求めた床反力ベクトルＦｒｆ(脚体/BC)と、前記全体／要素重運動算出手段３０で求めた足平部要素Ｓ１４の重心の加速度ベクトルACC(Ｇ＿足平部/BC)と、足平部要素Ｓ１４の重量ｍ＿足平部とから式（２３）の右辺の演算により、Ｆ(Ｐ＿足平部/BC)、すなわち、足首関節Ｊ＿足首に作用する並進力が求められる。また、その求めたＦ(Ｐ＿足平部/BC)（＝−Ｆ(Ｄ＿下腿部/BC)）と全体／要素重運動算出手段３０で求めた下腿部要素Ｓ１２の重心の加速度ベクトルACC(Ｇ＿下腿部/BC)と下腿部要素Ｓ１２の重量ｍ＿下腿部とから式（２４）の右辺の演算によりＦ(Ｐ＿下腿部/BC)、すなわち、膝関節Ｊ＿膝に作用する並進力が求められる。同様に、その求めたＦ(Ｐ＿下腿部/BC)（＝Ｆ(Ｄ＿大腿部/BC)）を用いて、式（２５）の右辺の演算によりＦ(Ｐ＿大腿部/BC)、すなわち、股関節Ｊ＿股に作用する並進力が求められる。このように、関節要素Ｊ＿足首、Ｊ＿膝、Ｊ＿股に作用する反力ベクトル（並進力ベクトル）が上記（２３）〜（２５）の運動方程式に基づいて順番に算出される。

次に、足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの回転運動（それぞれの重心回りの回転運動）の運動方程式は次の式（２６）〜（２８）により与えられる。

Ｍ(Ｐ＿足平部/Ｃ＿足平部)＝Ｉ＿足平部×ω(足平部/Ｃ＿足平部)’
＋ω(足平部/Ｃ＿足平部)×（Ｉ＿足平部×ω(足平部/Ｃ＿足平部)）
−（Ｕ(COP/Ｃ＿足平部)−Ｕ(Ｇ＿足平部/Ｃ＿足平部)）
×（Ｒ(BC→Ｃ＿足平部)×Ｆ(Ｄ＿足平部/BC)）
−（Ｕ(Ｐ＿足平部/Ｃ＿足平部)−Ｕ(Ｇ＿足平部/Ｃ＿足平部)）
×（Ｒ(BC→Ｃ＿足平部)×Ｆ(Ｐ＿足平部/BC)）
−Ｒ(BC→Ｃ＿足平部)×Ｍ(Ｄ＿足平部/BC)
……（２６）
Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)＝Ｉ＿下腿部×ω(下腿部/Ｃ＿下腿部)’
＋ω(下腿部/Ｃ＿下腿部)×（Ｉ＿下腿部×ω(下腿部/Ｃ＿下腿部)）
−（−Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)）
×（Ｒ(BC→Ｃ＿下腿部)×Ｆ(Ｄ＿下腿部/BC)）
−（Ｕ(Ｐ＿下腿部/Ｃ＿下腿部)−Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)）
×（Ｒ(BC→Ｃ＿下腿部)×Ｆ(Ｐ＿下腿部/BC)）
−Ｒ(BC→Ｃ＿下腿部)×Ｍ(Ｄ＿下腿部/BC)
……（２７）
Ｍ(Ｐ＿大腿部/Ｃ＿大腿部)＝Ｉ＿大腿部×ω(大腿部/Ｃ＿大腿部)’
＋ω(大腿部/Ｃ＿大腿部)×（Ｉ＿大腿部×ω(大腿部/Ｃ＿大腿部)）
−（−Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)）
×（Ｒ(BC→Ｃ＿大腿部)×Ｆ(Ｄ＿大腿部/BC)）
−（Ｕ(Ｐ＿大腿部/Ｃ＿大腿部)−Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)）
×（Ｒ(BC→Ｃ＿大腿部)×Ｆ(Ｐ＿大腿部/BC)）
−Ｒ(BC→Ｃ＿大腿部)×Ｍ(Ｄ＿大腿部/BC)
……（２８）

ここで、上記各式（２６）〜（２８）中にそれぞれ現れる２つのＦ（Ｐ＿○○/BC)、Ｆ（Ｄ＿○○/BC)は、その○○で表される名称の剛体要素の両端が、それぞれに接触する物体から受ける反力モーメント（身体座標系ＢＣｄで表したモーメントベクトル）を意味している。この場合、式（２６）のＭ(Ｄ＿足平部/BC)は０である。Ｍ(Ｄ＿下腿部/BC)＝−Ｒ(Ｃ＿足平部→BC)×Ｍ(Ｐ＿足平部/Ｃ＿足平部)、Ｍ(Ｄ＿大腿部/BC)＝−Ｒ(Ｃ＿下腿部→BC)×Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)である。また、Ｉ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部は、それぞれ足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの重心回りの慣性モーメントであり、ω(足平部/Ｃ＿足平部)、ω(下腿部/Ｃ＿下腿部)、ω(大腿部/Ｃ＿大腿部)は、それぞれ、足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの重心回りの角速度を意味している。この場合、一般に、慣性モーメントＩ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部は、十分に小さい値（０に近い値）であるので、本実施形態では、Ｉ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部はいずれも０に近似する。

従って、前記式（２６）〜（２８）は、近似的に次の式（２９）〜（３１）に書き換えられる。なお、式（２９）〜（３１）では、Ｍ(Ｄ＿足平部/BC)＝０、Ｍ(Ｄ＿下腿部/BC)＝−Ｒ(Ｃ＿足平部→BC)×Ｍ(Ｐ＿足平部/Ｃ＿足平部)、Ｍ(Ｄ＿大腿部/BC)＝−Ｒ(Ｃ＿下腿部→BC)×Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)であることを考慮している。

Ｍ(Ｐ＿足平部/Ｃ＿足平部)＝−（Ｕ(COP/Ｃ＿足平部)−Ｕ(Ｇ＿足平部/Ｃ＿足平部)）
×（Ｒ(BC→Ｃ＿足平部)×Ｆ(Ｄ＿足平部/BC)）
−（Ｕ(Ｐ＿足平部/Ｃ＿足平部)−Ｕ(Ｇ＿足平部/Ｃ＿足平部)）
×（Ｒ(BC→Ｃ＿足平部)×Ｆ(Ｐ＿足平部/BC)）
……（２９）
Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)＝−（−Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)）
×（Ｒ(BC→Ｃ＿下腿部)×Ｆ(Ｄ＿下腿部/BC)）
−（Ｕ(Ｐ＿下腿部/Ｃ＿下腿部)−Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)）
×（Ｒ(BC→Ｃ＿下腿部)×Ｆ(Ｐ＿下腿部/BC)）
−Ｒ(BC→Ｃ＿下腿部)
×（−Ｒ(Ｃ＿足平部→BC)×Ｍ(Ｐ＿足平部/Ｃ＿足平部)）
……（３０）
Ｍ(Ｐ＿大腿部/Ｃ＿大腿部)＝−（−Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)）
×（Ｒ(BC→Ｃ＿大腿部)×Ｆ(Ｄ＿大腿部/BC)）
−（Ｕ(Ｐ＿大腿部/Ｃ＿大腿部)−Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)）
×（Ｒ(BC→Ｃ＿大腿部)×Ｆ(Ｐ＿大腿部/BC)）
−Ｒ(BC→Ｃ＿大腿部)
×（−Ｒ(Ｃ＿下腿部→BC)×Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)）
……（３１）

そして、本実施形態では、式（２９）の右辺の演算により、Ｍ(Ｐ＿足平部/Ｃ＿足平部)、すなわち、足首関節１３に作用する関節モーメントＭ(Ｐ＿足平部/Ｃ＿足平部)（足平部座標系Ｃ＿足平部で表したモーメントベクトル）が求められる。この場合、式（２９）の右辺のＵ(COP/Ｃ＿足平部)は、前記床反力作用点推定手段３２で先に求めた床反力作用点の位置ベクトルＵ(COP/BC)に、前記変換テンソル作成手段２８で求めた変換テンソルＲ(Ｃ＿足平部→BC)の逆変換テンソルＲ(BC→Ｃ＿足平部)＝Ｒ(Ｃ＿足平部→BC)^Tを乗算することにより求められる。また、Ｕ(Ｇ＿足平部/Ｃ＿足平部)は、あらかじめ設定されたものであり、Ｕ(Ｐ＿足平部/Ｃ＿足平部)は、足平部要素Ｓ１４のあらかじめ定められた長さＬ１４により定まる、足平部座標系Ｃ＿足平部における足首関節Ｊ１３の位置ベクトル（０，０，Ｌ１４）^Tである。また、Ｆ(Ｄ＿足平部/BC)は床反力推定手段３１で先に求めた床反力ベクトルＦｒｆ(脚体/BC)の値である。さらに、Ｆ(Ｐ＿足平部/BC)は、前記式（２３）により前述の通り求められる。従って、変換テンソル作成手段２８で作成された変換テンソルＲ(Ｃ＿足平部→BC)と、床反力作用点推定手段３２で求めた床反力作用点の位置ベクトルＵ(COP/BC)と、床反力推定手段３１で求めた床反力ベクトルＦｒｆ(脚体/BC)と、前記式（２３）により求めた反力ベクトルＦ(Ｐ＿足平部/BC)とを用いて式（２９）の右辺の演算により、Ｍ(Ｐ＿足平部/Ｃ＿足平部)、すなわち、足首関節１３に作用する関節モーメント（足平部座標系Ｃ＿足平部で表したモーメントベクトル）が求められる。なお、接地していない脚体２については、その床反力作用点の位置ベクトルＵ(COP/Ｃ＿足平部)は不定であるが、Ｆ(Ｄ＿足平部/BC)＝０であるので、式（２９）の第１項の値はＵ(COP/Ｃ＿足平部)の値によらずに０になる。

また、式（３０）の右辺の演算により、Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)、すなわち、膝関節１１に作用する関節モーメント（下腿部座標系Ｃ＿下腿部で表したモーメントベクトル）が求められる。この場合、式（３０）の右辺のＵ(Ｇ＿下腿部/Ｃ＿下腿部)）は、あらかじめ設定されたものであり、Ｕ(Ｐ＿下腿部/Ｃ＿下腿部)は、下腿部要素Ｓ１２のあらかじめ定められた長さＬ１２により定まる、下腿部座標系Ｃ＿下腿部（Ｃ１２）における膝関節Ｊ１１の位置ベクトル（０，０，Ｌ１２）^Tである。また、Ｒ(BC→Ｃ＿下腿部)は、変換テンソル作成手段２８で作成した変換テンソルＲ(Ｃ＿下腿部→BC)の逆変換テンソルＲ(Ｃ＿下腿部→BC)^Tである。また、Ｆ(Ｄ＿下腿部/BC)は、前記式（２３）により求められるＦ(Ｐ＿足平部/BC)の符号を反転させたものであり、Ｆ(Ｐ＿下腿部/BC)は、前記式（２４）により求められるものである。さらに、Ｍ(Ｐ＿足平部/Ｃ＿足平部)は、前記式（２９）により求められる。従って、変換テンソル作成手段２８で作成された変換テンソルＲ(Ｃ＿下腿部→BC)，Ｒ(Ｃ＿足平部→BC)と、前記式（２３），（２４）によりそれぞれ先に求めた反力ベクトルＦ(Ｐ＿足平部/BC)，Ｆ(Ｐ＿下腿部/BC)と、あらかじめ設定された下腿部要素Ｓ１２の重心の位置ベクトルＵ(Ｇ＿下腿部/Ｃ＿下腿部)と、下腿部要素Ｓ１２の長さＬ１２と、前記式（２９）により先に求めたモーメントＭ(Ｐ＿足平部/Ｃ＿足平部)とを用いて式（３０）の右辺の演算により、Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)、すなわち、膝関節１１に作用する関節モーメント（下腿部座標系Ｃ＿下腿部で表したモーメントベクトル）が求められる。

同様に、式（３１）の右辺の演算により、Ｍ(Ｐ＿大腿部/Ｃ＿大腿部)、すなわち、股関節９に作用する関節モーメント（大腿部座標系Ｃ＿大腿部で表したモーメントベクトル）が求められる。この場合、式（３１）の右辺のＵ(Ｇ＿大腿部/Ｃ＿大腿部)）は、あらかじめ設定されたものであり、Ｕ(Ｐ＿大腿部/Ｃ＿大腿部)は、大腿部要素Ｓ１０のあらかじめ定められた長さＬ１０により定まる、大腿部座標系Ｃ＿大腿部における股関節Ｊ９の位置ベクトル（０，０，Ｌ１０）^Tである。また、Ｒ(BC→Ｃ＿大腿部)は、変換テンソル作成手段２８で作成した変換テンソルＲ(Ｃ＿大腿部→BC)の逆変換テンソルＲ(Ｃ＿大腿部→BC)^Tである。また、Ｆ(Ｄ＿大腿部/BC)は、前記式（２４）により求められるＦ(Ｐ＿下腿部/BC)の符号を反転させたものであり、Ｆ(Ｐ＿大腿部/BC)は、前記式（２５）により求められるものである。さらに、Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)は、前記式（３０）により求められる。従って、変換テンソル作成手段２８で作成された変換テンソルＲ(Ｃ＿大腿部→BC)，Ｒ(Ｃ＿下腿部→BC)と、前記式（２４），（２５）によりそれぞれ先に求めた反力ベクトルＦ(Ｐ＿下腿部/BC)，Ｆ(Ｐ＿大腿部/BC)と、あらかじめ設定された大腿部要素Ｓ１０の重心の位置ベクトルＵ(Ｇ＿大腿部/Ｃ＿大腿部)と、大腿部要素Ｓ１０の長さＬ１０と、前記式（３０）により先に求めたモーメントＭ(Ｐ＿下腿部/Ｃ＿下腿部)とを用いて式（３１）の右辺の演算により、Ｍ(Ｐ＿大腿部/Ｃ＿大腿部)、すなわち、股関節９に作用する関節モーメント（大腿部座標系Ｃ＿大腿部で表したモーメントベクトル）が求められる。

以上のように、関節モーメント推定手段３５の演算処理では、各脚体２の足首関節１３、膝関節１１、および股関節９の関節モーメントＭ(Ｐ＿足平部/Ｃ＿足平部)、Ｍ(Ｐ＿下腿部/Ｃ＿下腿部)、Ｍ(Ｐ＿大腿部/Ｃ＿大腿部)が足首関節１３側から順番に算出される。なお、このように求められた関節モーメントは、例えば人間１の歩行を補助する装置（足首関節１３や、膝関節１１、股関節９に補助トルクを付与可能な電動モータを含む装置）の制御に用いられる。

以上説明した実施形態では、身体座標系ＢＣを基本的な座標系として用いて各種の演算処理が実行される。そして、身体座標系ＢＣあるいは腰部６の鉛直方向に対する傾斜角を考慮して演算処理を行うのは、床反力作用点推定手段３２の演算処理だけである。このため、鉛直方向に対する腰部６等の傾斜角を使用する演算処理を従来に比して大幅に少なくすることができる。その結果、傾斜角を高い精度で把握することが困難な場合であっても、誤差の蓄積を最小限に留め、関節モーメントの推定精度を高めることができる。さらに、傾斜角を用いない床反力作用点推定手段を用いれば、関節モーメント推定システムに３次元姿勢センサなどが不要となり、システムの小型化、簡略化が可能となる。

また、本実施形態では、演算処理装置１８の各手段の処理を３次元で行っているため推定する関節モーメントの精度を高めることができる。

本発明を２足歩行移動体としての人間に適用した実施形態での全体的装置構成を模式的に示す図。 図１の装置に備えたセンサボックスの構成を示すブロック図。 実施形態で使用する剛体リンクモデルの斜視図。 図２の演算処理装置の機能的手段を示すブロック図。 実施形態で用いる座標変換の変換テンソルの例を説明するための図。 実施形態で用いる座標変換の変換テンソルの例を説明するための図。 ２足歩行移動体の単脚支持状態での床反力ベクトルの推定手法を説明するための図。 ２足歩行移動体の両脚支持状態での床反力ベクトルの推定手法を説明するための図。 （ａ）〜（ｃ）は床反力ベクトルの作用点の推定手法を説明するための図。 図９（ｂ）の状態での床反力ベクトルの作用点のＹ軸方向成分の推定手法を説明するための図。

符号の説明

１…人間（２足歩行移動体）、２…脚体、６…腰部、９…股関節、１０…大腿部、１１…膝関節、１２…下腿部、１３…足首関節、１４ａ…中足趾節関節、２１〜２３…関節変位センサ、１６…加速度センサ、Ｓ１…剛体リンクモデル、ＪＵ１，ＪＵ２，Ｊ９，Ｊ１１，Ｊ１３…関節要素、Ｓ８，Ｓ７，Ｓ６，Ｓ１０，Ｓ１２，Ｓ１４…剛体要素、ＢＣ…身体座標系、Ｇ８，Ｇ７，Ｇ６，Ｇ１０，Ｇ１２，Ｇ１４…剛体要素の重心、Ｇ＿全体…全体重心、Ｆｒｆ…床反力ベクトル、ＣＯＰ…床反力作用点（床反力ベクトルの作用点）。

Claims (7)

1. ２足歩行移動体を、複数の剛体要素が少なくとも該２足歩行移動体の各脚体の股関節及び膝関節に対応する関節要素を含む複数の関節要素で連結された連結体として表現する剛体リンクモデルを用い、該２足歩行移動体の各脚体の少なくとも１つの関節に作用する関節モーメントを推定する方法であって、
   前記剛体リンクモデルの各関節要素に対応する前記２足歩行移動体の各関節の変位量を逐次把握する第１ステップと、前記剛体リンクモデルの所定の１つの剛体要素に固定された座標系としてあらかじめ設定された身体座標系の原点の加速度ベクトルの該身体座標系での値を少なくとも前記２足歩行移動体に装着した加速度センサの出力を用いて逐次把握する第２ステップと、前記２足歩行移動体の各脚体に作用する床反力ベクトルの前記身体座標系での値を逐次把握する第３ステップと、前記床反力ベクトルの作用点の位置ベクトルの前記身体座標系での値を逐次把握する第４ステップと、前記第１乃至第４ステップで把握した２足歩行移動体の各関節の変位量と前記身体座標系の原点の加速度ベクトルの値と前記床反力ベクトルの値とその作用点の位置ベクトルの値とを用いて、前記剛体リンクモデルの各剛体要素の運動と該剛体要素に作用する並進力およびモーメントとの関係を前記身体座標系を用いて表す逆動力学モデルに基づいて前記２足歩行移動体の各脚体の少なくとも一つの関節に作用する関節モーメントを逐次推定する第５ステップを備えると共に、
   前記２足歩行移動体の全体重心の位置ベクトルの前記身体座標系での値を前記第１ステップで把握した２足歩行移動体の各関節の変位量と前記剛体リンクモデルとを用いて逐次把握する第６ステップと、その全体重心の位置ベクトルの値の時系列データおよび前記第２ステップで把握した身体座標系の原点の加速度ベクトルの値から該全体重心の加速度ベクトルの身体座標系での値を逐次把握する第７ステップと、前記２足歩行移動体の運動状態が一対の脚体のうちの一方の脚体のみが接地している単脚支持状態であるか、両脚体が接地している両脚支持状態であるかを逐次判断する第８ステップと、前記身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部の鉛直方向に対する傾斜角を逐次把握する第９ステップと、２足歩行移動体の各脚体毎に該脚体が接地しているか否かを逐次判断する第１０ステップと、接地している各脚体の足首関節の位置ベクトルの前記身体座標系での値と該脚体の足平部の中足趾節関節の位置ベクトルの前記身体座標系での値とを前記第１ステップで把握した２足歩行移動体の各関節の変位量と前記剛体リンクモデルとを用いて逐次把握する第１１ステップと、前記第６ステップで把握した前記全体重心の位置ベクトルの値と前記第１１ステップで把握した、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節のそれぞれの位置ベクトルの値と前記第９ステップで把握した傾斜角とに基づいて少なくとも該全体重心、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節の位置関係と該脚体の足首関節の鉛直方向位置を逐次把握する第１２ステップと、その把握した全体重心、接地している各脚体の足首関節および該脚体の足平部の中足趾節関節の位置関係に基づき該脚体に作用する床反力ベクトルの作用点の水平面内位置を推定すると共に該脚体の足首関節の鉛直方向位置に基づき該脚体に作用する床反力ベクトルの作用点の鉛直方向位置を逐次推定する第１３ステップとを備え、
   前記第３ステップは、２足歩行移動体の運動状態が前記単脚支持状態であるときには、前記第７ステップで把握した前記全体重心の加速度ベクトルの値と２足歩行移動体の全体重量と接地している脚体に作用する床反力ベクトルとにより表される該２足歩行移動体の全体重心の運動方程式に基づいて該床反力ベクトルの身体座標系での値を把握し、２足歩行移動体の運動状態が前記両脚支持状態であるときには、前記第７ステップで把握した前記全体重心の加速度ベクトルの値と２足歩行移動体の全体重量と両脚体のそれぞれに作用する床反力ベクトルとにより表される該２足歩行移動体の全体重心の運動方程式と、各脚体に作用する床反力ベクトルが該脚体の下端部近傍にあらかじめ定めた特定部から２足歩行移動体の全体重心に向かって作用するベクトルであると仮定して定まる、２足歩行移動体の全体重心に対する該脚体の特定部の相対位置と該脚体に作用する床反力ベクトルとの間の関係式とに基づいて両脚体のそれぞれに作用する床反力ベクトルの身体座標系での値を把握し、
   前記第４ステップは、該第１３ステップで推定された床反力ベクトルの作用点の水平面内位置および鉛直方向位置と前記第９ステップで把握された傾斜角とに基づき前記身体座標系での作用点の位置ベクトルの値を把握することを特徴とする２足歩行移動体の関節モーメント推定方法。
2. 前記加速度センサは、前記身体座標系が固定された剛体要素に対応する２足歩行移動体の剛体相当部に装着されていることを特徴とする請求項１記載の２足歩行移動体の関節モーメント推定方法。
3. 前記身体座標系が固定された剛体要素は、前記２足歩行移動体の一対の股関節に対応する一対の関節要素を連結する剛体要素であることを特徴とする請求項２記載の２足歩行移動体の関節モーメント推定方法。
4. 前記第３ステップおよび第４ステップでそれぞれ把握する床反力ベクトルの値およびその作用点の位置ベクトルの値は３次元の値であり、前記第１３ステップは、前記全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で後側に存在する場合には、該脚体の足首関節の水平面内位置を該脚体に作用する床反力ベクトルの作用点の水平面内位置として推定し、前記全体重心が接地している脚体の足平部の中足趾節関節に対して２足歩行移動体の前後方向で前側に存在する場合には、該脚体の足平部の中足趾節関節の水平面内位置を該脚体に作用する床反力ベクトルの作用点の水平面内位置として推定し、前記全体重心が接地している脚体の足首関節に対して２足歩行移動体の前後方向で前側に存在し、且つ該脚体の足平部の中足趾節関節に対して後側に存在する場合には、該脚体の足首関節と中足趾節関節とを結ぶ線分上で前記全体重心と前後方向の位置が同一となる点の水平面内位置を該脚体に作用する床反力ベクトルの作用点の水平面内位置して推定することを特徴とする請求項１〜３のいずれか１項に記載の２足歩行移動体の関節モーメント推定方法。
5. 前記第１３ステップは、接地している脚体に作用する床反力ベクトルの鉛直方向位置を、該第１２ステップで把握された脚体の足首関節の鉛直方向位置からあらかじめ定めた所定値だけ鉛直方向下方に離れた位置として推定することを特徴とする請求項１〜４のいずれか１項に記載の２足歩行移動体の関節モーメント推定方法。
6. 前記第１０ステップでは、接地していると判断した脚体について、さらに該脚体の足平部のつま先側部分および踵側部分のそれぞれの接地の有無を判断し、前記第１２ステップでは、前記接地している脚体の足首関節の鉛直方向位置に加えて該脚体の足平部の中足趾節関節の鉛直方向位置を把握し、前記第１３ステップでは、前記第１０ステップで足平部のつま先側部分および踵側部分のうちのつま先側部分のみが接地していると判断されたときには、前記所定値の代りに、前記第１２ステップで把握した前記足首関節の鉛直方向位置と中足趾節関節の鉛直方向位置とから求められる該足首関節と中足趾節関節との鉛直方向の距離を用いて前記床反力ベクトルの作用点の鉛直方向位置を推定することを特徴とする請求項５記載の２足歩行移動体の関節モーメント推定方法。
7. 前記第３ステップおよび第４ステップでそれぞれ把握する床反力ベクトルの値およびその作用点の位置ベクトルの値は３次元の値であることを特徴とする請求項１〜６のいずれか１項に記載の２足歩行移動体の関節モーメント推定方法。

Images