JP4440759B2

JP4440759B2 - ２足歩行移動体の床反力推定方法

Info

Publication number: JP4440759B2
Application number: JP2004366907A
Authority: JP
Inventors: 康池内
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2004-12-17
Filing date: 2004-12-17
Publication date: 2010-03-24
Anticipated expiration: 2024-12-17
Also published as: US20060149420A1; JP2006167890A; US7756607B2

Description

本発明は人などの２足歩行移動体に作用する床反力の推定方法に関する。

例えば人などの２足歩行移動体の歩行動作を補助するために、該２足歩行移動体の各脚体の関節に補助トルクを付与する場合、各脚体の関節に発生すべきモーメント（２足歩行移動体の運動に必要な関節モーメント）を推定する必要があり、その関節モーメントを推定するためには、接地している各脚体に作用する床反力（ベクトル）を把握する必要がある。なお、「床反力」は、正確には、並進力とモーメントとから成るベクトル量であるが、本明細書では、「床反力」は、その並進力のベクトルを意味するものとする。

そして、その床反力を推定する手法としては、例えば特許文献１に本願出願人が提案したものが知られている。この手法では、２足歩行移動体の腰部に装着した加速度センサの出力や各脚体の関節（股関節、膝関節、足首関節）の変位（回転角）を検出する関節変位センサの出力を用いて２足歩行移動体の重心の加速度（ベクトル）が算出され、この重心の加速度と２足歩行移動体の重量とから、２足歩行移動体に作用する全ての床反力の総和である全床反力（ベクトル）が算出される。そして、２足歩行移動体の両脚支持状態（両脚体が接地している時期）では、各脚体に作用する床反力の方向が、各脚体の足首関節から２足歩行移動体の重心に向かう方向であると仮定して、それらの床反力の総和（ベクトル和）が上記全床反力に等しくなるように各脚体に作用する床反力が推定される。また、２足歩行移動体の単脚支持状態（片方の脚体のみが接地している時期）では、上記全床反力がそのまま、接地している脚体に作用する床反力として推定される。なお、接地していない脚体に作用する床反力の推定値は、当然のことながら「０」である。

このような床反力の推定手法は、力センサを２足歩行移動体に装着したり、床にフォースプレート（荷重を検知するプレート）を設置したりすることなく、床反力を推定できるという点で利点がある。
特開２００３−８９０８３号公報

ところで、前記特許文献１に見られる技術では、２足歩行移動体の両脚支持状態では、各脚体に作用する床反力の向きは、２足歩行移動体の重心と足首関節との相対的な位置関係によって決定される一方、単脚支持状態では、加速度センサの出力を用いて算出される２足歩行移動体の重心の加速度の向きがそのまま、接地している脚体に作用する床反力の向きとして決定される。従って、両脚支持状態と単脚支持状態とでは、接地している各脚体に作用する床反力の向きの決定の仕方が異なっている。このため、両脚支持状態と単脚支持状態との切り替り時に、接地している各脚体に作用する床反力の推定値の向きが不連続に変化することがあった。特に、２足歩行移動体の前後方向における加速度センサの出力（前後方向の加速度検出値）は外乱などの影響で、その向きが変動しやすいことから、接地している各脚体に作用する床反力のうち、２足歩行移動体の前後方向の成分の推定値が不連続に急変することがあった。

また、単脚支持状態においては、推定される床反力の大きさと向きとの両者に加速度センサの出力が直接的に反映される。このため、特に単脚支持状態では、接地している脚体に作用する床反力の前後方向成分の推定値の向きが振動的に反転することがあった。

従って、前記特許文献１に見られる技術では、滑らかに連続的に変化するような床反力の推定値を得ることができない場合ある。ひいては、その床反力の推定値を用いて関節モーメントを推定したとき、その関節モーメントの推定値も不連続に急変したり、振動的に頻繁に変動することがあった。

本発明はかかる背景に鑑みてなされたものであり、滑らかに連続的に変化するような床反力の推定値を求めることができる床反力推定方法を提供することを目的とする。

かかる目的を達成するために、本発明の２足歩行移動体の床反力推定方法の第１発明は、２足歩行移動体の各脚体に作用する床反力を推定する方法であって、前記２足歩行移動体の各脚体の股関節および膝関節を少なくとも含む所定の複数の関節の変位量を逐次計測する第１ステップと、前記２足歩行移動体を複数の剛体要素および複数の関節要素の連結体として表現してなる剛体リンクモデルと前記第１ステップで計測した関節の変位量とを少なくとも用いて２足歩行移動体の重心の位置を逐次算出する第２ステップとを備える。すなわち、第１ステップで計測した所定の複数の関節の変位量を用いることで、前記剛体リンクモデルの関節要素の変位量を定め、ひいては、該剛体リンクモデルの剛体要素の相互の位置関係および姿勢関係を定めることができるので、その剛体リンクモデルの重心の位置を２足歩行移動体の重心の位置として算出することができる。なお、この場合、剛体リンクモデルの各剛体要素には、その重量や長さがあらかじめ設定される。また、２足歩行移動体の重心の位置は、例えば、２足歩行移動体の、任意に選択した所定の部位（ある剛体要素に対応する部位）に固定した座標系での位置（これは、該座標系の原点に対する位置を意味する）、あるいは当該所定の部位に対して固定された所定の基準点に対する位置として求めればよい。

そして、第１発明は、前記第２ステップで算出した重心の位置と２足歩行移動体に装着した加速度センサの出力とを少なくとも用いて該２足歩行移動体の重心の加速度を逐次算出する第３ステップと、前記第３ステップで算出した重心の加速度と２足歩行移動体の重量とから２足歩行移動体に作用する全ての床反力の総和である全床反力の、少なくとも一つのあらかじめ定めた所定軸に垂直な面上での成分ベクトルを逐次算出する第４ステップとを備える。すなわち、加速度センサの出力によって、該加速度センサを装着した部位の加速度が判り、また、２足歩行移動体の重心の位置の時系列によって、その位置の基準とする２足歩行移動体の所定の基準点（前記所定の部位に固定された座標系の原点など）に対する重心の相対加速度が判るので、少なくとも加速度センサの出力と２足歩行移動体の重心の位置とから該重心の加速度を算出することができる。

さらに、第１発明は、前記剛体リンクモデルと前記第１ステップで計測した関節の変位量と前記第２ステップで算出した重心の位置と前記第３ステップで算出した重心の加速度と２足歩行移動体に装着した角速度センサの出力とを少なくとも用いて該２足歩行移動体の全体の角運動量の時間的変化率である全角運動量変化率の前記所定軸廻りの成分を逐次算出する第５ステップを備える。すなわち、前記全角運動量変化率は、剛体リンクモデルの各剛体要素のそれぞれの重心廻りの角運動量変化率の総和と、各剛体要素の重心の並進加速度運動による２足歩行移動体の重心廻りの角運動量変化率の総和とを加え合わせたものに等しいものとなる。そして、これらのモーメントは、少なくとも剛体リンクモデルと、計測した関節の変位量（関節要素の変位量）と、２足方向移動体の重心の位置と、該重心の加速度と、前記角速度センサの出力とを用いて求めることができ、それらの角運動量変化率の総和（少なくとも該角運動量変化率の前記所定軸廻りの成分の総和）を求めることで、前記全角運動量変化率の前記所定軸廻りの成分を算出できる。なお、この場合、剛体リンクモデルの各剛体要素の重心の位置（該剛体要素上での位置）や、各剛体要素の慣性モーメントはあらかじめ設定される。

また、第１発明は、前記２足歩行移動体の接地している各脚体に作用する床反力の作用点である床反力作用点を逐次把握する第６ステップを備える。該床反力作用点は、各脚体に作用する床反力の中心点に相当し、該脚体の接地面に分布する床反力がその点に集中していると見なすことができる点である。この場合、床反力作用点を把握する（床反力作用点の位置を把握する）手法としては、種々様々な手法があり、そのいずれの手法を用いてもよい。その手法の例としては、前記特許文献１に見られる手法や、ＰＣＴ国際出願PCT/JP2004/004457に本願出願人が先に提案した手法などが挙げられる。

そして、第１発明は、前記第１〜第６ステップに加えて、前記第２ステップで算出した重心の位置と前記第４ステップで算出した全床反力の成分ベクトルと前記第５ステップで算出した全角運動量変化率の成分と前記第６ステップで把握した床反力作用点とを用いて、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和によって２足歩行移動体の重心の廻りに発生するモーメントの向きが前記全角運動量変化率の前記所定軸廻りの成分の向きと同一になるように、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを決定する第７ステップと、該第７ステップで決定した床反力の成分ベクトルの方向と前記第４ステップで算出した全床反力の成分ベクトルとを用い、前記２足歩行移動体が両脚支持状態であるときには、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを前記第７ステップで決定した向きに規定して、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和のベクトルが該全床反力の当該面上での成分ベクトルに等しくなるように、各脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの推定値を求め、前記２足歩行移動体が単脚支持状態であるときには、接地している脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを前記第７ステップで決定した向きに規定して、該脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの大きさが該全床反力の当該面上での成分ベクトルの大きさに等しくなるように、該脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの推定値を求める第８ステップとを備えたことを特徴とするものである。

すなわち、２足歩行移動体に作用する外力が床反力のみである場合、前記全角運動量変化率は、全床反力（各脚体に作用する床反力の総和）によって前記２足歩行移動体の重心廻りに発生するモーメント（あるいは各脚体にそれぞれ作用する床反力によって２足歩行移動体の重心廻りに発生するモーメントの総和）に等しくなる。なお、全角運動量変化率およびモーメントはベクトルであるから、両者が等しくなるということはそれぞれの大きさ及び向きの両者が等しくなるということである。この場合、各脚体に作用する床反力によって２足歩行移動体の重心廻りに発生するモーメントは、該床反力の向き（該床反力の作用線の向き）に依存する。例えば、各脚体に作用する床反力の作用線が、該脚体の床反力作用点から２足歩行移動体の重心を通る向きの直線である場合には、その床反力が２足歩行移動体の重心廻りに発生するモーメントは０であるが、該作用線が２足歩行移動体の重心を通らない方向の直線である場合には、その床反力は２足歩行移動体の重心廻りにある大きさのモーメントを発生する。従って、前記全角運動量変化率が、全床反力によって前記２足歩行移動体の重心廻りに発生するモーメントの総和に等しくなるという動力学的な条件は、各脚体に作用する床反力の向きを制約し得る条件となる。なお、この条件は、全角運動量変化率の所定軸廻りの成分に着目すれば、全床反力の該所定軸に垂直な成分ベクトルによって前記２足歩行移動体の重心廻りに発生するモーメントの該所定軸廻りの成分（あるいは各脚体のそれぞれに作用する床反力の該所定軸に垂直な成分ベクトルによって２足歩行移動体の重心廻りに発生するモーメントの総和）が前記全角運動量変化率の該所定軸廻りの成分に等しくなるという条件（以下、本欄では条件１という）と同等である。

そこで、第１発明では、前記第７ステップにおいて、上記条件１を考慮し、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和のベクトルによって２足歩行移動体の重心の廻りに発生するモーメントの向きが前記全角運動量変化率の前記所定軸廻りの成分の向きと同一になるように、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを決定することとした。その成分ベクトルの向きは、２足歩行移動体の重心の位置と前記全床反力の所定軸に垂直な面上での成分ベクトルと前記全角運動量変化率の所定軸廻りの成分と前記床反力作用点とを用いて決定することが可能である。

そして、このように各脚体に作用する床反力の成分ベクトルの向きを決定したとき、２足歩行移動体の両脚支持状態では、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和が前記全床反力の当該面上での成分ベクトルに等しくなるという条件（以下、本欄では条件２という）によって、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトル（大きさと向きとを有する２次元ベクトル）を特定できることとなる。なお、条件２で「等しくなる」というのは、大きさおよび向きの両者が等しくなるということである。

また、２足歩行移動体の単脚支持状態では、接地している脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの大きさが該全床反力の当該面上での成分ベクトルの大きさに等しくなるという条件（以下、本欄では条件３という）によって、該床反力の前記所定軸に垂直な面上での成分ベクトル（大きさと向きとを有する２次元ベクトル）を特定できることとなる。

そこで、第１発明では、前記第８ステップにおいて、両脚支持状態であるときには、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを前記第７ステップで決定した向きに規定しつつ、前記条件２を満たすように各脚体に作用する床反力の所定軸に垂直な面上での成分ベクトルの推定値を求め、単脚支持状態であるときには、接地している脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを前記第７ステップで決定した方向に規定しつつ、前記条件３を満たすように該脚体に作用する床反力の所定軸に垂直な面上での成分ベクトルの推定値を求める。この場合、該推定値は、両脚支持状態および単脚支持状態のいずれであっても、第７ステップで決定した床反力の成分ベクトルの向きと前記第４ステップで算出した全床反力の成分ベクトルとを用いることで、求めることができる。

かかる第１発明によれば、前記全角運動量変化率は、一般に、その向きや大きさが急変することがないため、この全角運動量変化率を考慮して、床反力の向きを決定することにより、前記第８ステップで求められる床反力の推定値の向きが急変したり、振動するような事態を防止できる。つまり、床反力の推定値の向きを滑らかに変化させることができる。その結果、第１発明によれば、滑らかに連続的に変化するような床反力の推定値を求めることが可能となる。

かかる第１発明では、好ましくは、前記第７ステップは、前記第４ステップで算出した全床反力の成分ベクトルと前記第５ステップで算出した全角運動量変化率の成分とを用いて、該全床反力の前記所定軸に垂直な面上での成分ベクトルと該成分ベクトルによって２足歩行移動体の重心廻りに発生するモーメントとの関係を規定するモーメントアーム長を求めるステップと、その求めたモーメントアーム長と前記第２ステップで算出した重心の位置と前記第６ステップで把握した床反力作用点とを用いて、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での作用線が、２足歩行移動体の重心を中心として前記モーメントアーム長に等しい半径を有する円に接し、且つ、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和によって２足歩行移動体の重心廻りに発生するモーメントの向きが、前記全角運動量変化率の該所定軸廻りの成分の向きに一致するように、接地している各脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの向きを決定するステップとから成る（第２発明）。

すなわち、全床反力の前記所定軸に垂直な面上での成分ベクトルによって２足歩行移動体の重心廻りに発生するモーメントの大きさは、全床反力の所定軸に垂直な面上での成分ベクトルの大きさに、ある値のモーメントアーム長を乗算したものとなる。該モーメントアーム長は、全床反力の所定軸に垂直な面上での成分ベクトルの作用線と、２足歩行移動体の重心との距離としての意味を持つものである。そして、接地している各脚体に作用する床反力の所定軸に垂直な面上での成分ベクトルの作用線は、２足歩行移動体の重心を中心として、上記モーメントアーム長に等しい半径を有する円に接すると考えることができる。これにより該作用線の向きを規定できる。

そこで、第２発明では、まず、第４ステップで算出した全床反力の成分ベクトルと前記第５ステップで算出した全角運動量変化率の成分とを用いて前記モーメントアーム長を求める。この場合、このモーメントアーム長を求めるためには、全床反力の所定軸に垂直な面上での成分ベクトルの大きさを用いればよく、その大きさは、加速度センサの出力を基に前記第２及び第３ステップにより求められる２足歩行移動体の重心の加速度の変動の影響を受け難い。これは、全床反力の、２足歩行移動体に作用する重力に釣り合う成分（これは鉛直方向の成分である）が、２足歩行移動体の運動によって発生する慣性力に釣り合う成分に比して大きいため、２足歩行移動体の重心の加速度が該２足歩行移動体の運動によって変動して、前記第４ステップで算出される該全床反力の向きが変動しても、該全床反力の大きさの変動は小さいからである。そして、第２発明では、上記の如く求めたモーメントアーム長と前記第２ステップで算出した重心の位置と前記第６ステップで把握した床反力作用点とを用いて、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを決定する。この場合、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での作用線が、２足歩行移動体の重心を中心として前記モーメントアーム長に等しい半径を有する円に接し、且つ、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和によって２足歩行移動体の重心廻りに発生するモーメントの向きが、前記全角運動量変化率の該所定軸廻りの成分の向きに一致するように、接地している各脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの向きが決定される。これにより、急激な変動を生じないような、床反力の所定軸に垂直な面上での成分ベクトルの向きを好適に決定することができる。

かかる第１発明および第２発明では、前記所定軸は、少なくとも２足歩行移動体のほぼ左右方向に延びる軸を含むことが好ましい（第３発明）。すなわち、例えば脚体の関節に作用する関節モーメントは、２足歩行移動体のほぼ左右方向に延びる軸の廻りのモーメント成分が大きいので、そのモーメントを推定する上では、２足歩行移動体のほぼ左右方向に延びる軸に垂直な面上での床反力の成分ベクトルを推定することが特に望ましい。従って、前記所定軸に、少なくとも２足歩行移動体のほぼ左右方向に延びる軸を含めることにより、その左右方向の軸廻りの関節モーメントを推定する上で重要な床反力の成分ベクトルを推定することができる。

ところで、前記第３発明において、２足歩行移動体が両脚支持状態であるときに、両足平部の前後方向の距離が比較的小さい場合には、前記第７ステップで決定される床反力の、前記左右方向に延びる軸に垂直な面上での成分ベクトルの向きが両脚体でほぼ同じ向きとなる。そして、このような場合には、前記第８ステップで、床反力の、前記左右方向に延びる軸に垂直な面上での成分ベクトルの推定値を適切に求めることが困難となる。これは、両脚体のそれぞれの床反力の、前記左右方向に延びる軸に垂直な面上での成分ベクトルの１次独立性が低下するため、前記条件２を満たすような各脚体の床反力の当該面上での成分ベクトルを精度よく一義的に定めることが困難となるからである。

そこで、前記第３発明では、前記２足歩行移動体のほぼ左右方向に延びる軸をＹ軸、このＹ軸に垂直な平面をＸＺ平面としたとき、前記２足歩行移動体の両脚支持状態において、両脚体にそれぞれ作用する床反力の前記ＸＺ平面上での成分ベクトルが互いに同一であり、且つ、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの総和のベクトルの大きさが前記第４ステップで算出した全床反力のＸＺ平面上での成分ベクトルの大きさに一致し、且つ、その総和のベクトルの向きが、ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点を通って前記円に接する直線の向きと同一になるように各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値を求める第９ステップを備え、少なくとも両脚支持状態における２足歩行移動体の前後方向での両脚体の足平部の間の距離である足平間前後距離があらかじめ定めた第１閾値よりも小さいときには、前記第８ステップで求められる推定値の代わりに、前記第９ステップで求められた推定値を各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値として得るようにすることが好ましい（第４発明）。

かかる第４発明によれば、前記足平間前後距離が前記第１閾値よりも小さいとき、すなわち、前記第７ステップで決定される、前記ＸＺ平面上での床反力の成分ベクトルの向きが両脚体でほぼ同じになるようなときには、両脚体にそれぞれ作用する床反力の前記ＸＺ平面上での成分ベクトルが互いに同一であり、且つ、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの総和のベクトルの大きさが前記第４ステップで算出した全床反力の前記ＸＺ平面上での成分ベクトルの大きさに一致し、且つ、その総和のベクトルの向きが、ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点を通って前記円に接する直線の向きと同一になるように、各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値が求められる。これにより、前記足平間前後距離が小さい場合でも、ＸＺ平面上での床反力の成分ベクトルの推定値を支障なく求めることができる。なお、この場合、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの大きさは、全床反力の前記ＸＺ平面上での成分ベクトルの大きさの１／２倍の値となる。

また、上記のように、足平間全後距離が小さい場合を考慮したとき、前記第３発明では、前記２足歩行移動体のほぼ左右方向に延びる軸をＹ軸、このＹ軸に垂直な平面をＸＺ平面としたとき、前記２足歩行移動体の両脚支持状態において、両脚体にそれぞれ作用する床反力の前記ＸＺ平面上での成分ベクトルが互いに同一であり、且つ、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの総和のベクトルの大きさが前記第４ステップで算出した全床反力の前記ＸＺ平面上での成分ベクトルの大きさに一致し、且つ、その総和のベクトルの向きが、ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点を通って前記円に接する直線の向きと同一になるように各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値を求める第９ステップと、前記第８ステップで求めた床反力のＸＺ平面上での成分ベクトルの推定値と前記第９ステップで求めた床反力のＸＺ平面上での成分ベクトルの推定値とにそれぞれ所定の重み係数Ｗ1，Ｗ2を乗算したものを加え合わせてなる重み付き平均値を各脚体毎に求め、その求めた各脚体に係る重み付き平均値を該脚体に作用する床反力の前記ＸＺ平面上での成分の推定値の真値として得る第１０ステップとを備え、前記重み係数Ｗ1，Ｗ2は、両脚支持状態における２足歩行移動体の前後方向での両脚体の足平部の間の距離である足平間前後距離があらかじめ定めた第１閾値よりも小さいときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が前記第９ステップで求めた推定値と一致し、前記足平間前後距離が前記第１閾値よりも大きい値にあらかじめ定めた第２閾値よりも大きいときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が前記第８ステップで求めた推定値と一致し、前記足平間前後距離が第１閾値以上で第２閾値以下であるときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が、前記足平間前後距離が大きくなるに伴い前記第９ステップで求めた推定値から前記第８ステップで求めた推定値に近づいていくように前記足平間前後距離に応じて設定されていることがより好適である（第５発明）。

この第５発明によれば、両脚支持状態において、足平間前後距離が第１閾値よりも小さいときには、前記第１０ステップで得られる、各脚体の床反力のＸＺ平面上での成分ベクトルの推定値の真値は、前記第９ステップで求められる推定値と同じになり、足平間前後距離が第２閾値よりも大きいときには、前記第８ステップで求められる推定値（前記第７ステップで決定された向きを基に求められる推定値）と同じになる。そして、足平間前後距離が第１閾値以上で第２閾値以下であるときには、前記第１０ステップで得られる、各脚体の床反力のＸＺ平面上での成分ベクトルの推定値の真値は、足平間前後距離が大きくなるに伴い前記第９ステップで求めた推定値から前記第８ステップで求めた推定値に近づいていく。このため、例えば２足歩行移動体がその前後方向で歩幅を徐々に変化させるようにして移動するような場合でも、両脚支持状態における各脚体の床反力のＸＺ平面上での成分の推定値の真値が１歩毎に急変するような事態を防止できる。

なお、第５発明では、前記重み係数Ｗ1，Ｗ2は、より具体的には、足平間前後距離が第１閾値よりも小さいときには、Ｗ1＝０、Ｗ2＝１とし、足平間前後距離が第２閾値よりも大きいときには、Ｗ1＝１、Ｗ2＝０とし、足平間前後距離が第１閾値以上で第２閾値以下であるときには、Ｗ1＋Ｗ2＝１という関係を満たしつつ、Ｗ1を０から１まで足平間前後距離の増加に伴い連続的に増加させるように設定すればよい。

また、前記第４および第５発明において、前記ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点は、例えば、ＸＺ平面上で両脚体の前記床反力中心点を結ぶ線分上の中間点（例えば中点）に決定すればよい。

また、前記第４および第５発明において、前記足平間前後距離が第１まはた第２閾値よりも大きいか否かろ判断するときには、必ずしも足平間前後距離の値そのものを使用する必要はなく、該足平間前後距離と一定の相関関係を有するパラメータ（足平間前後距離の変化に対して単調に値が変化するパラメータ）を使用してもよい。

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

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

各脚体２の股関節９、膝関節１１、足首関節１３の部位には、それぞれの関節の変位量を検出する関節変位センサ２１，２２，２３が図示しないベルト等の部材を介して装着されている。これらの各関節変位センサ２１，２２，２３のうち、股関節９の関節変位センサ２１が検出する変位量は、該股関節９の３軸廻りの回転角（これらの３軸廻りの回転角の組から成る３次元量）であり、膝関節１１の関節変位センサ２２が検出する変位量は、該膝関節１１の１軸廻りの回転角、足首関節１３の関節変位センサ２２が検出する変位量は、該足首関節１３の１軸廻りの回転角である。この場合、関節変位センサ２１が検出する回転角のうちの１つと、関節変位センサ２２，２３が各々検出する回転角との回転軸は、それぞれ図３に示す如く、それらのセンサ２１〜２３を装着した各脚体２の股関節９、膝関節１１、足首関節１３のそれぞれのほぼ中心点を通る平面としての脚平面ＰＬにほぼ垂直な軸ａ９，ａ１１，ａ１３である。なお、図３は関節変位センサ２１〜２３を各脚体２に装備した人間１の下半身を正面側から見た図である。この図３では脚平面ＰＬは同図の紙面に垂直な平面である。

ここで、脚平面ＰＬは、それに対応する脚体２を膝関節１１で屈曲させて該脚体２の屈伸を行ったときに、該脚体２の股関節９、膝関節１１、足首関節１３のほぼ中心点が存在するような平面である。換言すれば、脚体２の屈伸は、その各関節９，１１，１３の中心点をほぼ脚平面ＰＬ上に位置させた状態で行われる。そして、例えば、図３の左脚体２のように、股関節９の運動によって左脚体２を外転させると、該左脚体２に対応する脚平面ＰＬは傾く。関節変位センサ２１〜２３は、それぞれ各関節９，１１，１３の上記回転軸ａ９，ａ１１，ａ１３廻りの回転角を例えば、ポテンショメータやロータリエンコーダを用いて検出する。

また、股関節９の関節変位センサ２１が検出する他の２軸廻りの回転角について図４を参照して説明する。図４はセンサボックス１５と関節変位センサ２１を装備した人間１の腰部６付近の構成を示す側面図である。本実施形態では、同図示の如く、関節変位センサ２１は前記センサボックス１５にゴム等から成る平板状の弾性部材５０を介して連結されている。なお、弾性部材５０のセンサボックス１５側の端部は、センサボックス１５から延設された硬質の剛体部材５０ａを介してセンサボックス１５に連結されている。そして、関節変位センサ２１は、例えば弾性部材５０の延在方向の回転軸ｂ９廻りの股関節９の回転角（弾性部材５０を捻る方向の回転角）と、該回転軸ｂ９に直交する回転軸ｃ９廻りの股関節９の回転角（弾性部材５０を撓ませる方向の回転角）とを、弾性部材５０の変形量を検出する図示しないひずみセンサや、後述するような光ファイバを用いたセンサを用いて検出する。なお、上記回転軸ｂ９，ｃ９は脚平面ＰＬに平行である。

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

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

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

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

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

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

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

以上の如く構成された剛体リンクモデルＳ１の各剛体要素及び各関節要素は、各関節要素の回転運動によって、その相互の位置関係および姿勢関係（向きの関係）が各剛体要素および各関節要素に対応する人間１の各部の相互の位置関係および姿勢関係とほぼ同一になるように運動可能とされている。従って、以降の説明では、剛体リンクモデルＳ１の各剛体要素及び各関節要素を、しばしばこれらの要素に対応する人間１の部位と同一視する。

この場合、上体下部関節ＪＵ１及び上体上部関節ＪＵ２は、それぞれ３軸廻りの回転が可能とされており、その中の１軸を計測軸として、その計測軸廻りの回転（図５に各関節要素ＪＵ１，ＪＵ２に対応して記載した矢印（回転方向を表す矢印））を前記光ファイバ２６，２７および発光／受光器１９により構成された検出手段により計測するようにしている。その計測軸は、本実施形態では、前記一対の股関節Ｊ９，Ｊ９の中心を結ぶ線分（腰部要素Ｓ６の部分Ｓ６ａの延在方向）と平行な軸である。また、各脚体部Ｓ２の股関節Ｊ９は、左脚体部Ｓ２の股関節Ｊ９に関して代表的に図５中に記載した矢印（回転方向を表す矢印）で示す如く３軸廻りの回転が可能とされている。さらに、各脚体部Ｓ２の膝関節Ｊ１１および足首関節Ｊ１３はそれぞれ、左脚体部Ｓ２の各関節要素Ｊ１１，Ｊ１３に関して代表的に図５中に記載した矢印（回転方向を表す矢印）で示す如く１軸廻りの回転が可能とされている。膝関節Ｊ１１および足首関節Ｊ１３のそれぞれの回転軸は、股関節Ｊ９、膝関節Ｊ１１および足首関節Ｊ１３のそれぞれの中心点を通る脚平面ＰＬ（図５では左脚体部Ｓ２に係る脚平面は図示を省略している）に垂直な軸である。右脚体部Ｓ２の股関節Ｊ９、膝関節Ｊ１１、および足首関節Ｊ１３の回転動作についても左脚体部Ｓ２と同様である。この場合、右脚体部Ｓ２の膝関節Ｊ１１および足首関節Ｊ１３のそれぞれの回転軸（１軸）は、該右脚体部Ｓ２に対応して図示した脚平面ＰＬに垂直な軸である。なお、各股関節Ｊ９は、いずれの脚体部Ｓ２についても３軸廻りの回転が可能であるから、それぞれの脚体部Ｓ２に対応する脚平面に垂直な軸廻りの回転も可能である。

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

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

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

なお、本実施形態では、重量および重心の位置は、人間１に本実施形態の装置（センサボックス１５、関節変位センサ２１〜２３など）を装備した状態での重量および重心の位置である。また、図５では、便宜上、各重心Ｇ８，Ｇ７，Ｇ６，Ｇ１０，Ｇ１２，Ｇ１４は、それぞれに対応する剛体要素の軸心上に位置するように記載しているが、必ずしもその軸心上に位置するとは限らず、その軸心からずれた位置に存在してもよい。

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

また、各脚体２に対応する脚平面ＰＬには、脚座標系ＬＣが固定・設定されている。なお、図５では便宜上、右脚体部Ｓ２の脚平面ＰＬに対応する脚座標系ＬＣのみを代表的に記載している。この脚座標系ＬＣは、脚平面ＰＬ上の股関節Ｊ９の中心点を原点とする３次元座標系（ＸＹＺ座標系）であり、脚平面ＰＬに垂直な方向をＹ軸、身体座標系ＢＣのＺ軸を脚平面ＰＬに投影した軸と平行な方向をＺ軸、これらのＹ軸およびＺ軸に直交する方向をＸ軸としている。なお、脚座標系ＬＣのＸＺ平面は、脚平面ＰＬと一致する。

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

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

なお、要素座標系Ｃ８，Ｃ７，Ｃ１０，Ｃ１２，Ｃ１４は、それぞれに対応する剛体要素に対して固定されている限り、必ずしも上記のように設定する必要はなく、その原点や各軸の向きの設定は任意でよい。また、脚座標系ＬＣの原点を股関節Ｊ９の中心からずらしたり、身体座標系ＢＣを腰部要素Ｓ６と異なる部位に対して固定・設定するようにしてもよい。座標系の設定の仕方は基本的には任意でよく、種々様々な設定形態を採ることが可能である。従って、本実施形態での座標系の設定の仕方は一例に過ぎない。

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

さらに演算処理装置１８は、３次元関節・要素重心位置算出手段３０が求めた各剛体要素の重心の位置ベクトルの値を用いて身体座標系ＢＣでの剛体リンクモデルＳ１の全体重心（人間１の全体重心）の位置ベクトルの値を求める全体重心位置算出手段３３を備えている。

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

そして、演算処理装置１８は、床反力推定手段３５が求めた床反力（ベクトル）の値と床反力作用点推定手段３４が求めた床反力作用点の位置ベクトルの値と身体座標系加速度・角速度算出手段３１が求めた加速度ベクトルおよび角速度ベクトルの値を、変換テンソル作成手段２８が作成した変換テンソルを用いて各脚体２に対応する脚平面ＰＬに投影する脚平面投影手段３６と、この投影により得られた値（２次元量）と２次元脚姿勢・要素重心位置算出手段２９が求めた位置・姿勢とを用いて各脚体２の足首関節１３、膝関節１１および股関節９に作用する関節モーメントを推定する関節モーメント推定手段３７とを備えている。

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

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

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

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

変換テンソル作成手段２８の演算処理では、各脚平面ＰＬに対応する脚座標系ＬＣと身体座標系ＢＣとの間のベクトル量の座標変換を行うための変換テンソルＲ(LC→BC)と、腹部要素Ｓ７の要素座標系Ｃ７および胸部要素Ｓ８の要素座標系Ｃ８のそれぞれと身体座標系ＢＣの間のベクトル量の座標変換を行うための変換テンソルＲ(C＿腹部→BC)、Ｒ(C＿胸部→BC)とが作成される。

変換テンソルＲ(LC→BC)は、身体座標系ＢＣで見た脚平面ＰＬの法線ベクトルの向きを表すテンソルである。この変換テンソルＲ(LC→BC)は、本実施形態では、股関節９の関節変位センサ２１で検出される前記回転軸ｂ９，ｃ９（図４参照）のそれぞれの軸廻りの回転角α，βを用いて次式（１）により算出される。

ここで、式（１）におけるＣ＿ｂは、図４に示す如く前記弾性部材５０のセンサボックス１５側の一端（センサボックス１５から延設された剛体部材５０ａに固定されている一端）に原点を有し、前記回転軸ｂ９，ｃ９の方向をそれぞれＺ軸方向、Ｘ軸方向とする３次元座標系である。なお、この座標系Ｃ＿ｂのＹ軸は、図４の紙面に垂直な方向であり、人間１の前記基準姿勢状態では脚平面ＰＬに垂直な方向である。そして、式（１）のＲ(LC→C＿ｂ)は、座標系Ｃ＿ｂで見た脚平面ＰＬの法線ベクトルの向きを表すテンソルである。また、式（１）中のＲ(C＿ｂ→BC)は上記座標系Ｃ＿ｂで見たベクトル量を身体座標系ＢＣで見たベクトル量に座標変換するためのテンソルである。この場合、座標系Ｃ＿ｂは、剛体部材５０ａおよびセンサボックス１５を介して腰部６に対して固定されているので、身体座標系ＢＣに対して固定的な位置・姿勢関係を有している。このため、テンソルＲ(C＿ｂ→BC)は一定値（固定値）であり、本実施形態の装置を人間１に装着した時に、あらかじめ設定されて、演算処理装置１８のメモリに記憶保持されている。従って、股関節９の関節変位センサ２１で検出される前記回転軸ｂ９，ｃ９のそれぞれの軸廻りの回転角α，βを用いて上記式（１）により変換テンソルＲ(LC→BC)が算出される。なお、変換テンソルＲ(LC→BC)は、左右の脚体２毎に各別に求められる。

また、変換テンソルＲ(C＿腹部→BC)、Ｒ(C＿胸部→BC)は次のように作成される。まず、発光／受光器１９の検出出力を基に、剛体リンクモデルＳ１の腰部要素Ｓ６に対する腹部要素Ｓ７と胸部要素Ｓ８の傾斜角（詳しくは身体座標系ＢＣのＺ軸方向に対する矢状面（ＸＺ平面）上での傾斜角）が把握される。そして、例えば変換テンソルＲ(Ｃ＿腹部→BC)は、腰部６に対する腹部７の傾斜角をθy（但し傾斜角θyは人間１の前方側への傾斜方向を正方向とする）とおくと、次式（２）の如く３次の行列で表される。

変換テンソルＲ(Ｃ＿胸部→BC）も同様である。補足すると、本実施形態では、剛体リンクモデルＳ１の上体下部関節ＪＵ１および上体上部関節ＪＵ２の１軸廻り（Ｃ＿腹部およびＣ＿胸部のＹ軸廻り）の回転による腹部要素Ｓ７および胸部要素Ｓ８の腰部要素Ｓ６に対する傾斜角を計測するようにしたため、変換テンソルＲ(Ｃ＿腹部→BC）およびＲ(Ｃ＿胸部→BC）は上記式（２）の右辺の形の行列で表される。但し、例えば上体下部関節ＪＵ１および上体上部関節ＪＵ２のそれぞれの２軸廻り（例えばＣ＿腹部およびＣ＿胸部のＹ軸とＸ軸との２軸廻り）の回転による、腹部要素Ｍ７および胸部要素Ｍ８の２軸廻りの傾斜角を計測するようにしてもよい。このようにした場合には、変換テンソルＲ(Ｃ＿腹部→BC)，Ｒ(Ｃ＿胸部→BC）はより複雑な形式になる。

なお、上記の如く求めた変換テンソルＲ(LC→BC)，Ｒ(Ｃ＿腹部→BC)，Ｒ(Ｃ＿胸部→BC）を転置したものがそれぞれの逆変換を行うための変換テンソルとなる。従って、Ｒ(BC→LC)＝Ｒ(LC→BC)^T、Ｒ(BC→Ｃ＿腹部)＝Ｒ(Ｃ＿腹部→BC)^T、Ｒ(BC→Ｃ＿胸部)＝Ｒ(Ｃ＿胸部→BC)^T（Ｔは転置を意味する）である。

前記２次元脚姿勢・要素重心位置算出手段２９の演算処理では、まず、各脚体２の関節変位センサ２１〜２３の検出出力から把握される、各関節９，１１，１３の脚平面ＰＬに垂直な軸（図３の回転軸ａ９，ａ１１，ａ１３）廻りの回転角から、該脚体２の剛体相当部である大腿部１０、下腿部１２および足平部１４の傾斜角θ＿大腿部、θ＿下腿部、θ＿足平部が、それぞれに対応する剛体リンクモデルＳ１の大腿部要素Ｓ１０、下腿部要素Ｓ１２および足平部要素Ｓ１４の傾斜角として算出される。ここで、傾斜角θ＿大腿部、θ＿下腿部、θ＿足平部は、脚平面ＰＬに係わる脚座標系ＬＣのＺ軸方向に対する傾斜角である。

具体的には、図７に示す如く、股関節９、膝関節１１、足首関節１３のそれぞれの検出された回転角（前記基準姿勢状態からの、脚平面ＰＬ（＝脚座標系ＬＣのＸＺ平面）に垂直な軸廻りの回転角）をθ＿股、θ＿膝、θ＿足首とすると、θ＿大腿部、θ＿下腿部、θ＿足平部は、それぞれ次式（３ａ）〜（３ｃ）により順番に求められる。

θ＿大腿部＝−θ＿股 ……（３ａ）
θ＿下腿部＝θ＿大腿部＋θ＿膝 ……（３ｂ）
θ＿足平部＝θ＿下腿部−θ＿足首−９０° ……（３ｃ）
尚、図７の例では、θ＿股＞０、θ＿膝＞０、θ＿足首＞０であり、θ＿大腿部＜０、θ＿下腿部０＞０、θ＿足平部＜０である。また、θ＿大腿部、θ＿下腿部、θ＿足平部の算出は、各脚体部Ｓ２毎に各別に行われる。

次に、各脚体部Ｓ２の各関節要素の、脚座標系ＬＣのＸＺ平面、すなわち、脚平面ＰＬ上での位置が、上記の如く求めたθ＿大腿部、θ＿下腿部、θ＿足平部と、あらかじめ演算処理装置１８のメモリに記憶保持された各脚体部Ｓ２の各剛体要素の長さとを用いて求められる。具体的には、各脚体部Ｓ２の関節要素Ｊ＿股（Ｊ９），Ｊ＿膝（Ｊ１１），Ｊ＿足首（Ｊ１３）およびＪ＿ＭＰ（Ｊ１４ａ）のそれぞれの脚座標系ＬＣでの位置ベクトルＵ(Ｊ＿股/LC)，Ｕ(Ｊ＿膝/LC)，Ｕ(Ｊ＿足首/LC)，Ｕ(Ｊ＿ＭＰ/LC)がそれぞれ次式（４ａ）〜（４ｄ）により順番に算出される。なお、このとき、脚座標系ＬＣのＹ軸方向（脚平面ＰＬの法線方向）でのＪ＿股，Ｊ＿膝、Ｊ＿足首、Ｊ＿ＭＰの位置、すなわち、Ｊ＿股，Ｊ＿膝、Ｊ＿足首、Ｊ＿ＭＰの、脚座標系ＬＣでのＹ軸成分はいずれも０とされる。つまり、本実施形態では、Ｊ＿股，Ｊ＿膝、Ｊ＿足首、Ｊ＿ＭＰは、いずれも脚平面ＰＬ上でのみ運動可能とされる。

Ｕ(Ｊ＿股/LC)＝(０，０，０)^T …（４ａ）
Ｕ(Ｊ＿膝/LC)＝Ｕ(Ｊ＿股/LC)
＋(−L10×sin(θ＿大腿部)，０，−L10×cos(θ＿大腿部))^T
…（４ｂ）
Ｕ(Ｊ＿足首/LC)＝Ｕ(Ｊ＿膝/LC)
＋(−L12×sin(θ＿下腿部)，０，−L12×cos(θ＿大腿部))^T
…（４ｃ）
Ｕ(Ｊ＿ＭＰ/LC)＝Ｕ(Ｊ＿足首/LC)
＋(−L14×sin(θ＿足平部)，０，−L14×cos(θ＿足平部))^T
…（４ｄ）
ここで、式（４ｂ），（４ｃ），（４ｄ）中のＬ１０，Ｌ１２，Ｌ１４はそれぞれ大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４の長さであり、前記した如く演算処理装置１８のメモリにあらかじめ記憶保持されている。また、式（４ｂ）〜（４ｄ）のそれぞれの右辺第２項のベクトルは、股関節要素Ｊ９から見た膝関節要素Ｊ１１の位置ベクトル、膝関節要素Ｊ１１から見た足首関節要素Ｊ１３の位置ベクトル、足首関節要素Ｊ１３から見たＭＰ関節要素Ｊ１４ａの位置ベクトルを意味している。なお、式（４ａ）〜（４ｄ）により求められる位置ベクトルＵ(Ｊ＿股/LC)，Ｕ(Ｊ＿膝/LC)，Ｕ(Ｊ＿足首/LC)，Ｕ(Ｊ＿ＭＰ/LC)のそれぞれのＸ軸成分およびＺ軸成分の組が脚平面ＰＬ上での２次元的位置を表している。

さらに、各脚体部Ｓ２の各剛体要素の重心の、脚座標系ＬＣでの位置ベクトルが、式（４ｂ）〜（４ｄ）により上記の如く算出された関節要素の位置ベクトルを用いて算出される。具体的には、各脚体部Ｓ２の大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４のそれぞれの重心Ｇ＿大腿部（Ｇ１０），Ｇ＿下腿部（Ｇ１２），Ｇ＿足平部（Ｇ１４）のそれぞれの位置ベクトルＵ(Ｇ＿大腿部/LC)，Ｕ(Ｇ＿下腿部/LC)，Ｕ(Ｇ＿足平部/LC)がそれぞれ次式（５ａ）〜（５ｃ）により算出される。

Ｕ(Ｇ＿大腿部/LC)＝Ｕ(Ｊ＿膝/LC)＋
Ｒ(Ｃ＿大腿部→LC)×Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)…（５ａ）
Ｕ(Ｇ＿下腿部/LC)＝Ｕ(Ｊ＿足首/LC)＋
Ｒ(Ｃ＿下腿部→LC)×Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)…（５ｂ）
Ｕ(Ｇ＿足平部/LC)＝Ｕ(Ｊ＿ＭＰ/LC)＋
Ｒ(Ｃ＿足平部→LC)×Ｕ(Ｇ＿足平部/Ｃ＿足平部)…（５ｃ）
ここで、式（５ａ）〜（５ｃ）のＲ(Ｃ＿大腿部→LC)、Ｒ(Ｃ＿下腿部→LC)、Ｒ(Ｃ＿足平部→LC)は、それぞれ大腿部座標系Ｃ＿大腿部（Ｃ１０）から脚座標系ＬＣへの変換テンソル、下腿部座標系Ｃ＿下腿部（Ｃ１２）から脚座標系ＬＣへの変換テンソル、足平部座標系Ｃ＿足平部（Ｃ１４）から脚座標系ＬＣへの変換テンソルであり、それぞれ先に算出したθ＿大腿部、θ＿下腿部、θ＿足平部（式（３ａ）〜（３ｃ）を参照）を用いて決定される。また、Ｕ(Ｇ＿大腿部/Ｃ＿大腿部)、Ｕ(Ｇ＿下腿部/Ｃ＿下腿部)、Ｕ(Ｇ＿足平部/Ｃ＿足平部)は、各剛体要素の要素座標系で表された該剛体要素の重心の位置ベクトルであり、前記した如くあらかじめ演算処理装置１８のメモリに記憶保持されている。

上記式（５ａ）〜（５ｃ）により求められる位置ベクトルＵ(Ｇ＿大腿部/LC)，Ｕ(Ｇ＿下腿部/LC)，Ｕ(Ｇ＿足平部/LC)のＸ軸成分及びＺ軸系分の組が脚平面ＰＬ上での２次元的位置を表している。以上説明した演算処理が２次元脚姿勢・要素重心位置算出手段２９の演算処理である。

次に３次元関節・要素重心位置算出手段３０の演算処理では、変換テンソル作成手段２８で求めた変換テンソルと２次元脚姿勢・要素重心位置算出手段２９で求めた各脚体部Ｓ２の各関節要素および各剛体要素の重心の位置（脚座標系ＬＣでの位置）とを用いて、剛体リンクモデルＳ１の各関節要素および各剛体要素の重心の身体座標系ＢＣでの位置ベクトルが求められる。

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

Ｕ(Ｊ＿左股/BC)＝(０，Ｌ６ａ／２，０)^T …（６ａ）
さらに、身体座標系ＢＣにおける左膝関節Ｊ１１、左足首関節Ｊ１３、左ＭＰ関節Ｊ１４ａのそれぞれの位置ベクトルＵ(Ｊ＿左膝/ＢＣ)、Ｕ（Ｊ＿左足首/ＢＣ）、Ｕ（Ｊ＿左ＭＰ/ＢＣ）は、変換テンソルＲ(LC→BC)と、左脚体部Ｓ２に対応する脚座標系ＬＣ（左ＬＣ）での位置ベクトルＵ(Ｊ＿左膝/LC)、Ｕ（Ｊ＿左足首/LC）、Ｕ（Ｊ＿左ＭＰ/LC）とを用いて次式（６ｂ）〜（６ｄ）により順番に求められる。

Ｕ(Ｊ＿左膝/BC)＝Ｕ(Ｊ＿左股/BC)＋Ｒ(LC→BC）×Ｕ(Ｊ＿左膝/LC）…（６ｂ）
Ｕ(Ｊ＿左足首/BC)＝Ｕ（Ｊ＿左股/BC)＋Ｒ(LC→BC)×Ｕ(Ｊ＿左足首/LC)
…（６ｃ）
Ｕ(Ｊ＿左ＭＰ/BC)＝Ｕ（Ｊ＿左股/BC)＋Ｒ(LC→BC)×Ｕ(Ｊ＿左ＭＰ/LC)
…（６ｄ）
右脚体部Ｓ２の各関節要素の身体座標系ＢＣでの位置ベクトルも上記と同様に求められる。

さらに、上体部ＳＵの上体下部関節ＪＵ１および上体上部関節ＪＵ２の身体座標系ＢＣでの位置ベクトルＵ(JU1/BC)，Ｕ(JU2/BC)は、それぞれ次式（７ａ），（７ｂ）により順番に求められる。

Ｕ(JU1/BC)＝(０，０，Ｌ６ｂ)^T ……（７ａ）
Ｕ(JU2/BC)＝Ｕ(JU1/BC)＋Ｒ(Ｃ＿腹部→BC)×(０，０，Ｌ７)^T ……（７ｂ）
なお、式（７ａ）のＬ６ｂは、腰部要素Ｓ６の部分Ｓ６ｂの長さ、式（７ｂ）のＬ７は腹部要素Ｓ７の長さであり、これらの長さは前述の通りあらかじめ演算処理装置１８のメモリに記憶保持されている。

また、各剛体要素の重心の、身体座標系ＢＣでの位置ベクトルの算出は次のように行われる。すなわち、大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４のそれぞれの重心の身体座標系ＢＣでの位置ベクトルＵ(Ｇ＿大腿部/BC)，Ｕ(Ｇ＿下腿部/BC)，Ｕ(Ｇ＿足平部/BC)は、それぞれ前記式（６ｂ）〜（６ｄ）の右辺のＵ(Ｊ＿左膝/LC)、Ｕ(Ｊ＿左足首/LC)、Ｕ(Ｊ＿左ＭＰ/LC)を、それぞれ前記２次元脚姿勢・要素重心位置算出手段２９で算出した大腿部要素Ｓ１０、下腿部要素Ｓ１２、足平部要素Ｓ１４の重心の位置ベクトルＵ(Ｇ＿大腿部/LC)、Ｕ(Ｇ＿下腿部/LC)、Ｕ(Ｇ＿足平部/LC）で置き換えた式を演算することで求められる。なお、Ｇ＿大腿部，Ｇ＿下腿部，Ｇ＿足平部の身体座標系ＢＣでの位置ベクトルの算出は、各脚体部Ｓ２毎に各別に行われる。

また、腰部要素Ｓ６の重心Ｇ６の位置ベクトルＵ(Ｇ＿腰部/BC)は、あらかじめ記憶保持された腰部座標系Ｃ＿腰部（Ｃ６）での重心Ｇ＿腰部の位置ベクトルＵ(Ｇ＿腰部/Ｃ＿腰部)から、次式（８）により求められる。

Ｕ(Ｇ＿腰部/BC)＝Ｒ(Ｃ＿腰部→BC)×Ｕ(Ｇ＿腰部/Ｃ＿腰部) …（８）
ここで、Ｒ(Ｃ＿腰部→BC)は、腰部座標系Ｃ＿腰部から身体座標系ＢＣへの変換テンソルである。本実施形態では、Ｃ＿腰部は身体座標系ＢＣに等しいので、Ｒ(Ｃ＿腰部→BC)は３次の単位行列で表される。従って、Ｕ(Ｇ＿腰部/Ｃ＿腰部)がそのままＵ(Ｇ＿腰部/BC)として得られる。

さらに、腹部要素Ｓ７、胸部要素Ｓ８のそれぞれの重心Ｇ７，Ｇ８の身体座標系ＢＣでの位置ベクトルＵ(Ｇ＿腹部/BC)，Ｕ(Ｇ＿胸部/BC)は、変換テンソル作成手段２８で求めた変換テンソルＲ(Ｃ＿腹部→BC)，Ｒ(Ｃ＿胸部→BC)と、あらかじめ記憶保持された腹部座標系Ｃ＿腹部（Ｃ７）での腹部要素Ｓ７の重心の位置ベクトルＵ(Ｇ＿腹部/Ｃ＿腹部)、及び胸部座標系Ｃ＿胸部（Ｃ８）での胸部要素Ｓ８の重心の位置ベクトルＵ(Ｇ＿胸部/Ｃ＿胸部)とを用いてそれぞれ次式（９），（１０）により求められる。

Ｕ(Ｇ＿腹部/BC)＝Ｕ(JU1/BC)＋Ｒ(Ｃ＿腹部→BC)・Ｕ(Ｇ＿腹部/Ｃ＿腹部)
……（９）
Ｕ(Ｇ＿胸部/BC)＝Ｕ(JU2/BC)＋Ｒ(Ｃ＿胸部→BC)・Ｕ(Ｇ＿胸部/Ｃ＿胸部)
……（１０）
なお、Ｕ(JU1/BC)、Ｕ(JU2/BC)は、前記式（７ａ），（７ｂ）により求められたものである。

以上説明した演算処理が３次元関節・要素重心位置算出手段３０の演算処理である。なお、以上のように３次元関節・要素重心位置算出手段３０で算出される各関節要素と各剛体要素の重心との位置ベクトルは、それぞれに対応する人間１の実際の部位の、身体座標系ＢＣで見た位置ベクトルとしての意味を持つ。

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

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

ACC(センサ/BC)＝Ｒ(SC→BC)・ACC(センサ/SC) ……（１１）
ω(センサ/BC)＝Ｒ(SC→BC)・ω(センサ/SC) ……（１２）
ここで、変換テンソルＲ(SC→BC)はセンサ座標系ＳＣと身体座標系ＢＣとの相対的な姿勢関係（詳しくは、センサ座標系ＳＣの各軸の身体座標系ＢＣの各軸に対する傾き角）から求められる。例えば、センサ座標系ＳＣの３軸（ＸＹＺ軸）が図８に示す如く身体座標系ＢＣのＹ軸（図８の紙面に垂直な軸）廻りに角度θ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)に等しい。従って、ω(センサ/BC)がそのまま身体座標系ＢＣの原点ＢＣＯの角速度ω(BCO/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→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)と表記する代わりに、Ｆｒｆ(右脚体)と略記し、そのＸ軸成分をＦｒｆ(右脚体)xと略記する。

人間１の運動時には、一般に、該人間１の全体の角運動量が時間的に変化し、この角運動量の時間的変化率（時間微分値）である全角運動量変化率は、床反力以外の外力（重力を除く）が人間１に作用していない場合、人間１に作用する全ての床反力（ベクトル）の総和である全床反力（ベクトル）が人間１の全体重心Ｇ＿全体の廻りに発生するモーメント（あるいは各脚体２に作用する床反力がそれぞれＧ＿全体の廻りに発生するモーメントの総和）に等しい。なお、人間１の単脚支持状態では、接地していない脚体２に作用する床反力は０であるから、接地している一つの脚体２に作用する床反力が全床反力であり、人間１の両脚支持状態では、接地している左右の脚体２，２にそれぞれ作用する床反力の総和（ベクトル和）が全床反力である。補足すると、接地している脚体２に作用する実際の床反力は、該脚体２の接地面に分布するものとなるが、本明細書では該脚体２の床反力作用点COPに床反力が集中している（接地面に分布する床反力の重心が床反力作用点COPに在る）とみなす。

また、一般に、ある並進力が、任意の点PP廻りに発生するモーメントの、任意の一つの軸XX廻りの成分の大きさは、軸XXに垂直な面SS上で見た、該並進力の作用線（該並進力の作用点を通って該並進力と同じ方向の直線）と点PPとの距離であるモーメントアーム長を、該並進力の面SS上での成分ベクトル（該並進力の軸XXに垂直な成分ベクトル）の大きさに乗算したものに等しい。

従って、人間１の全角運動量変化率の、ある所定軸廻りの成分の大きさは、人間１に作用する全床反力の、該所定軸に垂直な面上での成分ベクトル（２次元ベクトル）の大きさ（絶対値）に、ある長さのモーメントアーム長を乗算したものに一致する。また、該全床反力の、所定軸に垂直な面上での成分ベクトルがＧ＿全体の廻りに発生するモーメントの向きは、全角運動量変化率の所定軸廻りの成分の向きに一致する。

このようなことを考慮し、本実施形態では、接地している各脚体２に作用する床反力のの所定軸に垂直な面での成分ベクトルの向きを、上記モーメントアーム長と全角運動量変化率の所定軸廻りの成分（特にその向き）とを用いて規制することとした。

具体的には、人間１の全角運動量変化率のＹ軸（人間１の左右方向の軸）廻りの成分に着目し、図１１に示す人間１の単脚支持状態では、接地している脚体２（図示の例では右脚体２）に作用している床反力Ｆｒｆ(右脚体）（＝全床反力）のＹ軸に垂直な面（ＸＺ平面）上での成分ベクトルＦｒｆ(右脚体)x,ｚの向きを、その作用線Ｋ（図示の例では接地している右脚体２の床反力作用点COPを通る直線Ｋ（右））がＧ＿全体を中心とするある半径ｒ（＝モーメントアーム長）の円Cｉに接し、且つ、その作用線の向きのＦｒｆ(右脚体)x,zによってＧ＿全体の廻りに発生するモーメントの向きが全角運動量変化率Ｌ'（Ｌ'は全角運動量Ｌの１階微分値を意味する）のＹ軸廻り成分Ｌ'yの向きと同じになるような向きに規制する。このことは、左脚体２のみが接地している単脚支持状態でも同様である。

補足すると、単脚支持状態では、接地している脚体２に作用する床反力は、全床反力に等しいので、上記のように接地している脚体２に作用する床反力のＸＺ平面上での成分ベクトルの向きを規制したとき、全床反力のＸＺ平面上での成分ベクトルがＧ＿全体の廻りに発生するモーメントの向きも、全角運動量変化率のＹ軸廻り成分Ｌ'yの向きと同じになる。そして、この場合は、接地している脚体２に作用している床反力のＹ軸に垂直な面上での成分ベクトルＦｒｆ(脚体)x,ｚの大きさ（絶対値）に、上記円Ｃiの半径であるモーメントアーム長ｒを乗算したものが全角運動量変化率のＹ軸廻り成分Ｌ'yの大きさ（絶対値）に一致する。

また、図１２に示す人間１の両脚支持状態では、右脚体２に作用している床反力Ｆｒｆ(右脚体)のＹ軸に垂直な面（ＸＺ平面）上での成分ベクトルＦｒｆ(右脚体)x,z、の方向を、その作用線Ｋ(右)がＧ＿全体を中心とするある半径ｒ（＝モーメントアーム長）の円Cｉに接し、且つ、その作用線Ｋ(右)の向きのＦｒｆ(右脚体)x,zによってＧ＿全体の廻りに発生するモーメントの向きが全角運動量変化率Ｌ'のＹ軸廻り成分Ｌ'yの向きと同じになるような向きに規制する。同様に、左脚体２に作用している床反力Ｆｒｆ(左脚体)のＹ軸に垂直な面（ＸＺ平面）上での成分ベクトルＦｒｆ(左脚体)x,z、の向きを、その作用線Ｋ(左)が上記半径ｒの円Cｉに接し、且つ、その作用線Ｋ(左)の向きのＦｒｆ(左脚体)x,zによってＧ＿全体の廻りに発生するモーメントの向きが全角運動量変化率Ｌ'のＹ軸廻り成分Ｌ'yの向きと同じになるような向きに規制する。

補足すると、両脚支持状態では、上記のように左右の各脚体２に作用する床反力のＸＺ平面上での成分ベクトルの向きを規制したとき、それらの成分ベクトルの総和（全床反力のＸＺ平面上での成分ベクトル）がＧ＿全体の廻りに発生するモーメントの向きも、全角運動量変化率のＹ軸廻り成分Ｌ'yの向きと同じになる。但し、この場合は、右脚体２に作用している床反力Ｆｒｆ(右脚体)のＸＺ平面上での成分ベクトルＦｒｆ(右脚体)x,zの大きさ（絶対値）と、左脚体２に作用している床反力Ｆｒｆ(左脚体)のＸＺ平面上での成分Ｆｒｆ(左脚体)x,zの大きさ（絶対値）とにそれぞれ、上記円Ｃiの半径であるモーメントアーム長ｒを乗算したものの総和は、一般には、全角運動量変化率Ｌ'のＹ軸廻り成分Ｌ'yの大きさに一致しない。

以上説明したことを考慮し、本実施形態における床反力推定手段３５の演算処理では、以下に説明するように、全角運動量変化率Ｌ'のＹ軸廻り成分Ｌ'yと、全床反力（以下、これをＦｒｆ(全体)と称する）のＹ軸に垂直な面上での成分ベクトル（身体座標系ＢＣの矢状面としてのＸＺ平面上での成分ベクトル）とからモーメントアーム長ｒを求める。そして、このモーメントアーム長ｒを用いて、接地している各脚体２に作用する床反力（詳しくは該床反力のＸＺ平面上での成分ベクトル）の向きを決定する。このように床反力の向きを決定すれば、該床反力の各座標成分値の比率が定まるので、それと、全床反力とから該床反力の各座標成分値を求めることができる。なお、床反力のＹ軸成分は、関節モーメント推定手段３７が最終的に求める関節モーメント（脚平面ＰＬに垂直な軸廻りの関節モーメント）にはさほど影響を及ぼさないことから、本実施形態では、床反力のＹ軸成分は無視する（Ｙ軸成分を０とする）。

以下に床反力推定手段３５の演算処理を図１３などを参照しつつ具体的に説明する。図１３は、床反力推定手段３５の演算処理機能を示すブロック図である。

図１３に示すように、床反力推定手段３５は、全角運動量変化率算出手段５１、全床反力算出手段５２、モーメントアーム長算出手段５３、床反力方向決定手段５４および床反力成分算出手段５５を備えている。全角運動量変化率算出手段５１には、前記身体座標系加速度・角速度算出手段３１で求められた身体座標系ＢＣの原点ＢＣＯの角速度ω(BCO/BC)と、前記３次元関節・要素重心位置算出手段３０で求められた各関節要素の位置ベクトルおよび各剛体要素の重心の位置ベクトルと、前記全体重心位置算出手段３３で求められたＧ＿全体の位置ベクトルとが入力される。

そして、全角運動量変化率算出手段５１は、これらの入力値を使用して、次式（４０）の演算により、全角運動量変化率Ｌ’のＹ軸廻り成分Ｌ'yを求める。

Ｌ'y＝Σ（Ｉ＿n×θ''(n)y）
＋Σ（ｍ＿n×ΔＵ(Ｇ＿ｎ)
×ΔACC(Ｇ＿ｎ))y
……（４０）
但し、ΔＵ(Ｇ＿ｎ)≡Ｕ(Ｇ＿ｎ)−Ｕ(Ｇ＿全体)
ΔACC(Ｇ＿ｎ)≡ACC(Ｇ＿ｎ)−ACC(Ｇ＿全体)
＝Ｕ(Ｇ＿ｎ)''−Ｕ(Ｇ＿全体)''
この式（４０）において、「ｎ」は各剛体要素に任意に割り当てた番数を意味し、Ｉ＿ｎは番数ｎの剛体要素の慣性モーメント、θ''(n)は、番数ｎの剛体要素のその重心廻りの角加速度である。従って、式（４０）の第１項のΣの演算は、各剛体要素の慣性モーメントＩ＿ｎと角加速度θ''(n)のＹ軸成分との積（これは各剛体要素がその回転運動によって発生するモーメントのＹ軸成分である）を、全ての剛体要素について加算する演算である。また、ｍ＿ｎは番数ｎの剛体要素の重量、Ｕ(Ｇ＿ｎ)は、番数ｎの剛体要素の重心の身体座標系ＢＣでの位置ベクトル、ACC(Ｇ＿n)は、番数ｎの剛体要素の重心の加速度、ΔＵ(Ｇ＿ｎ)は、番数ｎの剛体要素の重心の、Ｇ＿全体に対する位置ベクトル、ΔACC(Ｇ＿n)は、番数ｎの剛体要素の重心の、Ｇ＿全体に対する相対加速度である。従って、式（４０）の第２項のΣの演算は、各剛体要素の重心のＧ＿全体に対する位置ベクトルΔＵ(Ｇ＿ｎ)と、該剛体要素の重心の全体重心Ｇ＿全体に対する相対加速度ΔACC(Ｇ＿n)とのベクトル積に該剛体要素の重量を乗算したもの（これは各剛体要素がその重心の並進加速度運動によってＧ＿全体廻りに発生するモーメントである）のＹ軸成分を、全ての剛体要素について加算する演算である。補足すると、番数ｎの剛体要素の重心を通ってΔACC(Ｇ＿n)と同方向の直線とＧ＿全体とのＸＺ平面上での距離をｒ(Ｇ＿n)、番数ｎの剛体要素の重心の並進運動をＧ＿全体の廻りの回転運動とみなしたときの該剛体要素の重心のＹ軸廻りの角加速度をθ(Ｇ＿n)とおいたとき、ΔＵ(Ｇ＿ｎ)とΔACC(Ｇ＿n)とのベクトル積のＹ軸成分（＝（ΔＵ(Ｇ＿ｎ)×ΔACC(Ｇ＿ｎ))y）は、ｒ(Ｇ＿n)^２×θ(Ｇ＿n)に等しい。

この場合、式（４０）の第１項のΣの演算で必要なＩ＿nは、あらかじめ演算処理装置１８の図示しないメモリに記憶保持されている。また、θ''(n)yは、次のように求められる。すなわち、前記３次元関節・要素重心位置算出手段３０で求めた脚体部Ｓ２の各関節要素の位置ベクトル（Ｕ(Ｊ＿左股/BC)等）から、ＸＺ平面（身体座標系ＢＣの矢状面）上での各剛体要素の傾斜角度が逐次求められ、さらにその傾斜角度の２階微分値に、前記身体座標系加速度・角速度算出手段３１で求めた身体座標系ＢＣの原点ＢＣＯの角速度ω(BCO/BC)のＹ軸成分の１階微分値を加算することで、該剛体要素の角加速度のＹ軸成分θ''(n)yが求められる。

また、式（４０）の第２のΣの演算で必要なΔＵ(Ｇ＿ｎ)は、前記３次元関節・要素重心位置算出手段３０で求めた各剛体要素の重心の位置ベクトル（Ｕ(Ｇ＿大腿部/BC)等）と、前記全体重心位置算出手段３３で求めたＧ＿全体の位置ベクトルＵ(Ｇ＿全体/BC)とから、式（４０）の但し書きの定義に従って求められる。さらに、ΔACC(Ｇ＿n)は、３次元関節・要素重心位置算出手段３０で求めた各剛体要素の重心の位置ベクトルの2階微分値Ｕ(Ｇ＿ｎ)''と、全体重心位置算出手段３３で求めたＧ＿全体の位置ベクトルの２階微分値Ｕ(Ｇ＿全体)''とから、式（４０）の但し書きの定義に従って求められる。

前記全床反力算出手段５２には、前記身体座標系加速度・角速度算出手段３１で求められた身体座標系ＢＣの原点ＢＣＯの加速度ACC(BCO/BC)と、前記全体重心位置算出手段３３で求められたＧ＿全体の位置ベクトルＵ(Ｇ＿全体/BC)とが入力される。

そして、全床反力算出手段５２は、これらの入力値から、次式（４１）の演算により、全床反力Ｆｒｆ(全体）のＸＺ平面上での成分ベクトルＦｒｆ(全体)x,z（Ｆｒｆ(全体）のＸ軸成分およびＺ軸成分からなる２次元ベクトル）を求める。

Ｆｒｆ(全体)x,z＝全体重量×(ACC(BCO/BC)＋Ｕ(Ｇ＿全体/BC)'')x,z
……（４１）
すなわち、全床反力のＹ軸に垂直な成分ベクトルＦｒｆ(全体)x,zは、身体座標系ＢＣの原点ＢＣＯの加速度ACC(BCO/BC)と、Ｇ＿全体の位置ベクトルＵ(Ｇ＿全体/BC)の２階微分値Ｕ(Ｇ＿全体/BC)''とを加算してなるベクトル（これはＧ＿全体の加速度を意味する）のＸＺ平面上での成分に、人間１の全体重量を乗算することで求められる。

次に、上記の如く求められた全角運動量変化率Ｌ'のＹ軸廻り成分Ｌ'yと、全床反力のＸＺ平面（矢状面）上での成分ベクトルＦｒｆ(全体)x,zとは、前記モーメントアーム長算出手段５３に入力され、ここで、それらの入力値からモーメントアーム長が求められる。この場合、前記した如く、Ｌ'yの大きさは、全床反力のＸＺ平面上での成分ベクトルＦｒｆ(全体)x,zの大きさ（絶対値）にモーメントアーム長を乗算したものに一致する。そこで、モーメントアーム長算出手段５３は、次式（４２）によりモーメントアーム長ｒを求める。

ｒ＝Ｌ'y／｜Ｆｒｆ(全体)x,z｜ ……（４２）
なお、本実施形態では、上記式（４２）により求められるモーメントアーム長ｒは、全角運動量変化率のＹ軸廻り成分Ｌ'yの向き（符号）に応じて正負の値を採る。この場合、本実施形態では、Ｌ'yが前記図１１および図１２に示す矢印の向きであるとき、Ｌ'yの値が正の値であるとした。従って、モーメントアーム長ｒが正の値であるか負の値であるかによって、Ｆｒｆ(全体)x,zがＧ＿全体廻りに発生するモーメント（Ｙ軸廻りのモーメント）の向きが定まるようになっている。

このように求められたモーメントアーム長ｒは、次に前記床反力方向決定手段５４に入力される。該床反力方向決定手段５４には、さらに接地センサ２４，２５の出力と、前記床反力作用点推定手段３４で推定された床反力作用点COPの位置ベクトルＵ(COP/BC)とが入力される。そして、床反力方向決定手段５４は、これらの入力値を使用して、次のように接地している各脚体２に作用する床反力の方向を決定する。

まず、接地センサ２４，２５の検出出力を基に、人間１の両脚支持状態であるか、単脚支持状態であるかが判断される。すなわち、一方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力し、且つ、他方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力している場合には両脚接地状態であると判断される。また、両脚体２，２のうちの一方の脚体２の接地センサ２４，２５のいずれかが接地有りを示すＯＮ信号を出力しており、且つ、他方の脚体２の接地センサ２４，２５の両者が接地有りを示すＯＮ信号を出力していない場合には、単脚支持状態であると判断される。

そして、両脚支持状態であるときには、図１２に示す如く、右脚体２に係る床反力Ｆｒｆ（右脚体）の向き（ＸＺ平面上での向き）については、右脚体２の床反力作用点COPを通って、Ｇ＿全体を中心とする半径ｒ（より正確には｜ｒ｜）の円に接する直線Ｋ（右）の身体座標系ＢＣでの傾きα(右脚体）を該脚体２の床反力Ｆｒｆ(右脚体)の向き（ＸＺ平面上での向き）を表すものとして決定する。この場合、モーメントアーム長ｒが正であるときには、直線Ｋ（右）は、半径ｒの円Ｃｉの後方側の部分に接するものとする。つまり、直線Ｋ（右）を作用線として床反力Ｆｒｆ（右脚体）のＸＺ平面上での成分ベクトルＦｒｆ(右脚体)x,zが床反力作用点ＣＯＰに作用したとき、それによって、Ｇ＿全体廻りに発生するモーメントのＹ軸廻り成分の向きが全角運動量変化率のＹ軸廻り成分Ｌ'yの向きと同じなるように円Ｃｉに接する直線Ｋ（右）の傾きα(右脚体)が決定される。左脚体２についても右脚体２の場合と同様に、該左脚体２の床反力Ｆｒｆ(左脚体)の向き（ＸＺ平面上での向き）を表す直線Ｋ（左）の身体座標系ＢＣでの傾きα(左脚体)が決定される。なお、本実施形態では、傾きαは、左右いずれの脚体２についても、直線Ｋの、Ｘ軸方向の単位変化量に対するＺ軸方向の変化量の比の値である。

また、単脚支持状態であるときには、図１１に示す如く、接地している脚体２（図の例では右脚体２）の床反力作用点COPからＧ＿全体までのベクトル（COPに対するＧ＿全体の位置ベクトル）のＸＺ平面上での成分ベクトル（身体座標系ＢＣの矢状面で見た２次元ベクトル）をCGとおくと、このCGと、右脚体２の床反力作用点COPを通って、Ｇ＿全体を中心とする半径ｒ（より正確には｜ｒ｜）の円に接する直線Ｋ（右）とのなす角度θａが、床反力Ｆｒｆ（右脚体）の向き（ＸＺ平面上での向き）を表すものとして決定される。この場合、モーメントアーム長ｒが正であるときには、直線Ｋ（右）は、半径ｒの円Ｃｉの後方側の部分に接するものとする。つまり、直線Ｋ（右）を作用線として床反力Ｆｒｆ(右脚体)のＸＺ平面上での成分ベクトルＦｒｆ(右脚体)x,zが床反力作用点COPに作用したとき、それによって、Ｇ＿全体廻りに発生するモーメントのＹ軸廻り成分の向きが全角運動量変化率のＹ軸廻り成分の向きと同じなるようにベクトルCGに対する直線Ｋ（右）のＸＺ平面上での角度θａが決定される。

このような角度θａは、次式（４３）により算出される。

θａ＝−sin^−１（ｒ／｜CG｜) ……（４３）
この場合、ベクトルCGの絶対値は、前記床反力作用点推定手段３４で求められた床反力作用点COPの位置ベクトルＵ(右COP/BC）と、前記全体重心位置算出手段３３で求められたＧ＿全体の位置ベクトルＵ(Ｇ＿全体/BC)とから算出される。

左脚体２のみが接地する単脚支持状態においても、上記と同様に左脚体２の床反力作用点COPに対するＧ＿全体の位置ベクトルのＸＺ平面上での成分CGと、左脚体２の床反力作用点COPを通って、Ｇ＿全体を中心とする半径ｒ（より正確には｜ｒ｜）の円に接する直線Ｋ（左）とのなす角度θａが、床反力Ｆｒｆ(左脚体)の向き（ＸＺ平面上での向き）を表すものとして決定される。

以上説明した床反力方向決定手段５４の処理により、接地している各脚体２に作用する床反力ＦｒｆのＸＺ平面（身体座標系ＢＣの矢状面）での成分ベクトルＦｒｆ(脚体)x,zの作用線が前記式（４２）により決定したモーメントアーム長ｒを半径、Ｇ＿全体を中心とする円Ｃiに接し、且つ、その作用線の方向のＦｒｆ(脚体)x,zがＧ＿全体廻りに発生するモーメントが、角運動量変化率のＹ軸廻り成分Ｌ'yに一致するように、接地している各脚体２に作用する床反力Ｆｒｆの向き（身体座標系ＢＣの矢状面上での向き）が決定されることとなる。

次に、床反力方向決定手段５４により決定された床反力の方向（両脚支持状態では傾きα、単脚支持状態では角度θａ）は、前記床反力成分算出手段５５に入力される。なお、床反力成分算出手段５５には、前記全床反力算出手段５２で求められた全床反力のＸＺ平面上での成分ベクトルＦｒｆ(全体)x,zも入力される。また、単脚支持状態では、床反力方向決定手段５４から、その処理で求めたベクトルCGも入力される。さらに、各脚体２の床反力作用点COPの位置ベクトルおよびＧ＿全体の位置ベクトルも入力される。

そして、床反力成分算出手段５５は、それらの入力値から、接地している各脚体２に作用する床反力Ｆｒｆの、身体座標系ＢＣでの座標成分値を次のようにして求める。

まず、両脚支持状態である場合について説明する。両脚支持状態での各脚体２，２に作用する床反力（詳しくはそのＸ軸成分およびＺ軸成分）は、次式（４４）〜（４７）の関係式に基づいて決定される。

Ｆｒｆ(右脚体)x＋Ｆｒｆ(左脚体)x＝Ｆｒｆ(全体)x ……（４４）
Ｆｒｆ(右脚体)z＋Ｆｒｆ(左脚体)z＝Ｆｒｆ(全体)z ……（４５）
Ｆｒｆ(右脚体)z＝α(右脚体)×Ｆｒｆ(右脚体)x ……（４６）
Ｆｒｆ(左脚体)z＝α(左脚体)×Ｆｒｆ(左脚体)x ……（４７）
式（４４）、（４５）は、各脚体２に作用する床反力の総和が全床反力に一致するということを表す式である。また、式（４６）、（４７）は、各脚体２に作用する床反力の方向によって、その床反力の成分値の比率が定まるということを示す式である。

そして、これらの式（４４）〜（４７）から、Ｆｒｆ(右脚体)x、Ｆｒｆ(右脚体)z、Ｆｒｆ(左脚体)x、Ｆｒｆ(左脚体)zがそれぞれ次式（４８）〜（５１）により求められる。

Ｆｒｆ(右脚体)x＝(Ｆｒｆ(全体)z
−α(左脚体)×Ｆｒｆ(全体)x）／Δα ……（４８）
Ｆｒｆ(右脚体)z＝(α(右脚体)×Ｆｒｆ(全体)z
−α(右脚体)×α(左脚体)×Ｆｒｆ(全体)x）／Δα
……（４９）
Ｆｒｆ(左脚体)x＝(−Ｆｒｆ(全体)z
＋α(右脚体)×Ｆｒｆ(全体)x）／Δα ……（５０）
Ｆｒｆ(左脚体)z＝(−α(左脚体)×Ｆｒｆ(全体)z
＋α(右脚体)×α(左脚体)×Ｆｒｆ(全体)x）／Δα
……（５１）
但し、Δα＝α(右脚体)−α(左脚体)
従って、両脚支持状態であるときには、基本的には、上記式（４８）〜（５１）の右辺の演算によって、各脚体２に作用する床反力のＸ軸成分、Ｚ軸成分、すなわち、Ｆｒｆ(脚体)x,zを算出することができる。

但し、両脚支持状態での両脚体２，２の足平部１４，１４の前後方向（Ｘ軸方向）の距離が短いときには、α(右脚体)およびα(左脚体)の値が互いに近い値となり（Δαが０に近い値となる）、前記式（４８）〜（５１）で求められる床反力の精度が低下する。

ここで、両脚支持状態での両足平部１４，１４のＸ軸方向での距離（以下、足平間前後距離という。図１２を参照）が短いときには、両脚体２，２にそれぞれ作用する床反力がほぼ等しいと考えられる。また、それらの床反力の合力がＧ＿全体廻りに発生するモーメントのＹ軸廻り成分の向きは、全角運動量変化率のＹ軸廻り成分Ｌ'yの向きに一致すべきである。そこで、本実施形態では、足平間前後距離が所定の第１閾値以下であるときには、次の式（５２）により各脚体２に作用する床反力のＸ軸成分、Ｚ軸成分を算出することとした。

式（５２）の但し書き中のCavGは、左右の脚体２，２のそれぞれの床反力作用点COPを結ぶ線分の中点Cavから、Ｇ＿全体までのベクトル（当該中点Cavに対するＧ＿全体の位置ベクトル）のＸＺ平面上での成分ベクトルである。なお、Ｃavの身体座標系ＢＣでの位置ベクトルは、左右の各床反力中心点COＰの位置ベクトルの平均値（＝（Ｕ(右COP)＋Ｕ(左COP)）／２）として算出される。また、θbは、ベクトルCavGと、ＸＺ平面上で上記中点Cavを通って、Ｇ＿全体を中心とする半径ｒ（より正確には｜ｒ｜）の円に接する直線とのなす角度であり、前記式（４３）の右辺の「CG」を「CavG」で置き換えた式によって算出される。従って、式（５２）のFrf_s(全体)x,zは、両脚体２の床反力作用点COPを結ぶ線分の中点CavからＧ＿全体までのベクトルのＸＺ平面上での成分ベクトルと同じ向きを持ち、且つ、Ｆｒｆ(全体)x,zの大きさを持つベクトルを角度θｂだけ当該中点の廻りにＸＺ平面（矢状面）上で回転させたベクトル（２次元ベクトル）である。このため、ＸＺ平面上において、Frf_s(全体)x,zの作用線（ＸＺ平面上で上記中点Cavを通ってベクトルFrf_s(全体)x,zと同方向の直線）は、Ｇ＿全体を中心とする半径ｒの円Ｃｉに接する。

従って、両脚支持状態で足平間前後距離が第１閾値以下であるときに前記式（５２）により求められるＦｒｆ(右脚体)x,z，Ｆｒｆ(左脚体)x,zは、互いにその大きさ及び向きが等しく、且つ、その総和の大きさが全床反力Ｆｒｆ(全体)の大きさに等しいものとなる。また、足平間前後距離が第１閾値以下のときには、各脚体２の床反力作用点COPのＸＺ平面上での位置と、前記中点のＸＺ平面での位置とは、ほぼ等しいので、Ｆｒｆ(右脚体)x,z，Ｆｒｆ(左脚体)x,zのそれぞれの作用線（床反力中心点COPを通るＸＺ平面上での作用線）は、概ね、Ｇ＿全体を中心とする半径ｒの円Ｃｉに接するものとなる。

また、足平間前後距離が上記第１閾値よりも大きい所定の第２閾値以上であるときには、前記式（４８）〜（５１）により各脚体２に作用する床反力のＸ軸成分、Ｚ軸成分を算出することとした。さらに、足平間前後距離が第１閾値と第２閾値との間の距離であるときには、式（４８）〜（５１）により求められる床反力のＸ軸成分、Ｚ軸成分（以下、これらの成分の組から成るベクトルに参照符号Ｆｒｆ１x,zを付する）と、式（５２）により求められる床反力のＸ軸成分、Ｚ軸成分（以下、これらの成分の組から成るベクトルに参照符号Ｆｒｆ２x,zを付する）との重み付き平均値を、各脚体２に作用する床反力のＸ軸成分、Ｚ軸成分として求めることとすると共に、その重み係数の値を足平間前後距離に応じて連続的に変化させるようにした。

従って、両脚支持状態において床反力成分算出手段５５が最終的に求める、各脚体２の床反力のＸＺ平面上での成分ベクトルＦｒｆ(脚体)x,z（Ｆｒｆ(脚体)x,zの推定値の真値）は、次式（５４）により求められる。

Ｆｒｆ(脚体)x,z＝Ｗ×Ｆｒｆ１x,z＋（１−Ｗ）×Ｆｒｆ２x,z ……（５４）
ここで、Ｗは重み係数であり、その値は、図１４に示す如く定められたデータテーブルあるいは演算式により、足平間前後距離に応じて決定される。この場合、足平間前後距離としては、例えば両脚体２，２のそれぞれの床反力作用点COPの、身体座標系ＢＣでのＸ軸方向の距離、すなわち｜Ｕ(右COP/BC)x−Ｕ(左COP/BC)x｜が使用される。なお、足平間前後距離として、例えば両脚体２，２のそれぞれのＭＰ関節のＸ軸方向距離や、足首関節のＸ軸方向距離を使用するようにしてもよい。補足すると、重み係数Ｗ、（１−Ｗ）は、本発明における重み係数Ｗ1、Ｗ2に相当するものである。

なお、両脚支持状態において、直線Ｋ（右）又は直線Ｋ（左）がＺ軸にほぼ平行となる場合もあり、その場合には、傾きα(右脚体)またはα(左脚体)の全体値が無限大もしくは過大に大きなものとなる。そこで、本実施形態では、傾きαの絶対値があらかじめ定めた所定値を超えるような場合には、強制的にαの値をその所定値に制限するようにしている。

一方、単脚支持状態で接地している脚体２に作用する床反力（詳しくはそのＸ軸成分およびＺ軸成分からなる２次元ベクトル）は、次式（５５）により算出される。

すなわち、接地している脚体２に作用する床反力のＸＺ平面上での成分ベクトルＦｒｆ(脚体)x,zは、その脚体２の床反力作用点COPからＧ＿全体に向かい、且つ、Ｆｒｆ(全体)x,zの大きさを持つベクトルを角度θａだけ床反力作用点COPの廻りにＸＺ平面（矢状面）上で回転させたベクトルとして求められる。このようにして求められるＦｒｆ(脚体)x,zは、その作用線がＧ＿全体を中心とする半径ｒの円Ｃｉに接することとなる。

なお、本実施形態では、両脚支持状態および単脚支持状態のいずれの場合にあっても、先に述べたように床反力のＹ軸成分は０とする。また、単脚支持状態で、接地していない脚体２に作用する床反力は０とされる。

以上のように床反力成分算出手段５５の処理を行なうことで、両脚支持状態では、床反力の成分ベクトルＦｒｆ(右脚体)x,z、Ｆｒｆ(左脚体)x,zのそれぞれの作用線（ＸＺ平面上で各脚体２の床反力作用点COPを通る作用線）がＧ＿全体を中心とする半径ｒ（モーメントアーム長）の円Ｃiに接すると共に、Ｆｒｆ(右脚体)x,z、Ｆｒｆ(左脚体)x,zのそれぞれが（ひいては、それらの総和が）Ｇ＿全体の廻りに発生するモーメント（Ｙ軸廻りのモーメント）の向きが全角運動量変化率のＹ軸廻り成分Ｌ'yの向きに一致するように、Ｆｒｆ(右脚体)x,z、Ｆｒｆ(左脚体)x,zのそれぞれの向きが決定される。そして、その上で、Ｆｒｆ(全体)x,zに、Ｆｒｆ(右脚体)x,z、Ｆｒｆ(左脚体)x,zの総和（＝Ｆｒｆ(右脚体)x,z＋Ｆｒｆ(左脚体)x,z）が一致するようにＦｒｆ(右脚体)x,z、Ｆｒｆ(左脚体)x,zの推定値（Ｘ軸成分の値とＺ軸成分の値との組））が求められる。

また、単脚支持状態では、接地している脚体２の床反力の成分ベクトルＦｒｆ(脚体)x,z、の作用線（該脚体２の床反力作用点COPを通る作用線）がＸＺ平面上においてＧ＿全体を中心とする半径ｒ（モーメントアーム長）の円Ｃiに接すると共に、そのＦｒｆ(脚体)x,z、がＧ＿全体の廻りに発生するモーメント（Ｙ軸廻りのモーメント）の向きが全角運動量変化率のＹ軸廻り成分Ｌ'yの向きに一致するように、Ｆｒｆ(脚体)x,zの向きが決定される。そして、その上で、Ｆｒｆ(全体)x,zの大きさ（絶対値）に、Ｆｒｆ(脚体)x,zの大きさ（絶対値）が一致するように接地している脚体２に係るＦｒｆ(脚体)x,zが求められる。

その結果、両脚支持状態および単脚支持状態のいずれにおいても、身体座標系ＢＣの矢状面上でのＦｒｆの向き（Ｆｒｆ(脚体)x,zの向き）を安定させることができる。

なお、本実施形態では、床反力Ｆｒｆ(脚体)のＹ軸成分を０としたが、さらに全角運動量変化率のＸ軸廻り成分を考慮し、この全角運動量変化率のＸ軸廻り成分の向きと、接地して各脚体２の床反力のＹＺ平面上での成分ベクトルＦｒｆ(脚体)y,zの総和がＧ＿全体の廻りに発生するモーメント（Ｘ軸廻りのモーメント）の向きとが一致するように、ＹＺ平面上での床反力の成分ベクトルＦｒｆ(脚体)y,zの向きを決定するようにしてもよい。その向きの決定は、前記した場合と同様に、ＹＺ平面上でのモーメントアーム長と、そのモーメントアーム長の半径を有する円を使用して行なえばよい。そして、Ｆｒｆ(脚体)y,zの向きを決定した上で、両脚支持状態では、各脚体２に作用する床反力のＹＺ平面上での成分ベクトルＦｒｆ(脚体)y,zの総和が全床反力のＹＺ平面上での成分ベクトルＦｒｆ(全体)y,zに一致するように各Ｆｒｆ(脚体)y,zの各座標成分値を求め、また、単脚支持状態では、接地している脚体２に作用する床反力のＹＺ平面上での成分ベクトルＦｒｆ(脚体)y,zの大きさが全床床反力のＹＺ平面上での成分ベクトルＦｒｆ(全体)y,zの大きさに一致するようにＦｒｆ(脚体)y,zの各座標成分値を求めるようにしてもよい。

以上が床反力推定手段３５の演算処理である。なお、上記の如く求めた床反力の推定値からノイズ成分を除去するために、該推定値にフィルタリング処理を施したものを最終的に床反力の推定値として得るようにしてもよい。そのフィルタリング処理としては、例えば２次のバタワースフィルタ（ローパスフィルタ）を使用するフィルタリング処理が挙げられ、該バタワースフィルタの遮断周波数は、例えば３Ｈｚ程度が好適である。

上記のように床反力作用点推定手段３４および床反力推定手段３５の演算処理を行なった後、次に、演算処理装置１８は、前記脚平面投影手段３６の演算処理を実行する。この処理では、身体座標系加速度・角速度算出手段３１で算出された身体座標系ＢＣの原点ＢＣＯの加速度ベクトルACC(BCO/BC)および角速度ベクトルω(BCO/BC)と、床反力推定手段３５で算出された床反力Ｆｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)と、床反力作用点推定手段３４で算出された床反力作用点COPの位置ベクトルＵ(COP/BC)とが変換テンソル作成手段２８で作成された変換テンソルＲ(LC→BC)の転置である変換テンソルＲ(BC→LC)（＝Ｒ(LC→BC)^T）を用いて各脚体部Ｓ２毎に、それに対応する脚平面ＰＬに投影される。

具体的には、加速度ベクトルACC(BCO/BC)および角速度ベクトルω(BCO/BC)に次式（２３ａ）、（２３ｂ）の如く、それぞれ変換テンソルＲ(BC→LC)を乗算することにより、各脚座標系ＬＣから見た加速度ベクトルACC(BCO/LC)および角速度ベクトルω(BCO/LC)が求められる。

ACC(BCO/LC)＝Ｒ(BC→LC)×ACC(BCO/BC) ……（２３ａ）
ω(BCO/LC)＝Ｒ(BC→LC)×ω(BCO/BC) ……（２３ｂ）
なお、加速度ベクトルACC(BCO/LC)および角速度ベクトルω(BCO/LC)は、それぞれ左脚体部Ｓ２に係わる脚座標系ＬＣに対応するものと、右脚体部Ｓ２に係わる脚座標系ＬＣに対応するものとが各別に求められる。

同様に、床反力Ｆｒｆ(右脚体/BC)，Ｆｒｆ(左脚体/BC)に次式（２３ｃ）、（２３ｄ）の如く、それぞれ変換テンソルＲ(BC→右LC)、Ｒ(BC→左LC)を乗算することにより、各脚座標系ＬＣから見た床反力Ｆｒｆ(右脚体/右LC)，Ｆｒｆ(左脚体/右LC)が求められる。

Ｆｒｆ(右脚体/右LC)＝Ｒ(BC→右LC)×Ｆｒｆ(右脚体/BC) ……（２３ｃ）
Ｆｒｆ(左脚体/左LC)＝Ｒ(BC→左LC)×Ｆｒｆ(左脚体/BC) ……（２３ｄ）
さらに、接地している各脚体２に係わる床反力作用点COPの位置ベクトルＵ(COP/BC)に次式（２３ｅ）の如く、その接地している脚体２に対応する変換テンソルＲ(BC→LC)を乗算することにより、該脚体２に対応する脚座標系ＬＣから見た床反力作用点ＣＯＰの位置ベクトルＵ(COP/LC)が求められる。

Ｕ(COP/LC)＝Ｒ(BC→LC)×Ｕ(COP/BC) ……（２３ｅ）
なお、位置ベクトルＵ(COP/LC)は、接地している脚体２が１つであれば、その脚体２に対応するもののみが求められ、両脚体２が接地しているときには、左右のそれぞれの脚体２毎に求められる。

ここで、加速度ベクトルACC(BCO/LC)、床反力Ｆｒｆ(右脚体/右LC)，Ｆｒｆ(左脚体/左LC)および床反力作用点の位置ベクトルＵ(COP/LC)については、それぞれのＸ軸成分およびＺ軸成分の組が、それぞれに対応する身体座標系ＢＣでのベクトル（３次元量）を各脚平面ＰＬ（脚座標系ＬＣのＸＺ平面）に投影してなる２次元量のベクトルとして得られる。例えば、図１５を参照して、身体座標系ＢＣでの右側脚体２に係わる床反力Ｆｒｆ(右脚体/BC)が図示の実線で示すようなベクトルであるとすると、床反力Ｆｒｆ(右脚体/右LC)のＸ軸成分およびＺ軸成分の組は、同図に破線で示すような脚平面ＰＬ（右）上のベクトルとなる。

なお、脚平面ＰＬ上での脚体２の回転運動は、脚平面ＰＬの法線方向（脚座標系ＬＣのＹ軸方向）の軸廻りの回転運動であるから、角速度ベクトルω(BCO/BC)を脚平面ＰＬに投影したものは、前記式（２３ｂ）により求められる脚座標系ＬＣでの角速度ベクトルω(BCO/LC)のＹ軸成分である。

以下の説明では、加速度ベクトルACC(BCO/LC)、床反力Ｆｒｆ(右脚体/右LC)，Ｆｒｆ(左脚体/左LC)および床反力作用点の位置ベクトルＵ(COP/LC)は、そのＸ軸成分およびＺ軸成分の組から成る２次元ベクトルを意味するものとする。例えば加速度ベクトルACC(BCO/LC)は、（ACC(BCO/LC)x，ACC(BCO/LC)z)^Tを意味する。なお、角速度ωについては、脚平面ＰＬ上での値はω(BCO/LC)yで表す。

次に、演算処理装置１８は、関節モーメント推定手段３５による演算処理を実行する。この関節モーメント推定手段３５の演算処理の概略を説明すると、各脚体部Ｓ２の足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの並進運動および回転運動に関する運動方程式に基づく逆動力学モデルの演算によって、足平部要素Ｓ１４、下腿部要素Ｓ１２、および大腿部要素Ｓ１０のそれぞれの腰部要素Ｓ６側の端点の関節要素Ｊ＿足首、Ｊ＿膝、Ｊ＿股の関節モーメントが順番に算出される。この場合、逆動力学モデルは、各脚体部Ｓ２毎に、それに対応する脚平面ＰＬ（脚座標系ＬＣのＸＺ平面）上で取扱われる。なお、この算出処理の基本的な考え方は、逆動力学モデルを取扱う平面及び座標系を除いて本願出願人が先に提案した特開２００３−８９０８３号公報等のものと同じである。

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

Ｆ(Ｐ＿足平部/LC)＝ｍ＿足平部×(ACC(BCO/LC)＋Ｕ(Ｇ＿足平部/LC)'')
−Ｆｒｆ(脚体/LC) ……（２４）
Ｆ(Ｐ＿下腿部/LC)＝ｍ＿下腿部×(ACC(BCO/LC)＋Ｕ(Ｇ＿下腿部/LC)'')
−Ｆ(Ｄ＿下腿部/LC) ……（２５）
Ｆ(Ｐ＿大腿部/LC)＝ｍ＿大腿部×(ACC(BCO/LC)＋Ｕ(Ｇ＿大腿部/LC)'')
−Ｆ(Ｄ＿大腿部/LC) ……（２６）
ここで、上記各式（２４）〜（２６）中に現れる２つのＦ（Ｐ＿○○/BC)、Ｆ（Ｄ＿○○/BC)は、その○○で表される名称の剛体要素の端部が、それに接触する物体から受ける反力（脚平面ＰＬ上での２次元並進力ベクトル）を意味している。このため、作用・反作用の法則によって、Ｆ(Ｄ＿下腿部/BC)＝−Ｆ(Ｐ＿足平部/BC)、Ｆ(Ｄ＿大腿部/BC)＝−Ｆ(Ｐ＿下腿部/BC)である。なお、足平部要素Ｓ１４に係わる式（２４）においては、該足平部要素Ｓ１４の、腰部要素Ｓ６から遠い側の端部は、床反力作用点ＣＯＰとみなされ、その端部（床反力作用点ＣＯＰ）に床から作用する反力として、前記脚平面投影手段３６で求められた床反力Ｆｒｆ(脚体/LC)が用いられる。

また、Ｕ(Ｇ＿足平部/LC)''、Ｕ(Ｇ＿下腿部/LC)''、Ｕ(Ｇ＿大腿部/LC)''は、それぞれ、前記２次元脚姿勢・要素重心位置算出手段２９で先に算出された脚座標系ＬＣでの重心Ｇ＿足平部、Ｇ＿下腿部、Ｇ＿大腿部の位置ベクトル（より正確には、該位置ベクトルのＸ軸成分及びＺ軸成分の組）の２階微分値、すなわち、脚平面ＰＬ上で見た重心Ｇ＿足平部、Ｇ＿下腿部、Ｇ＿大腿部の、脚座標系ＬＣの原点に対する相対加速度（２次元ベクトル）を意味している。この場合、脚座標系ＬＣの原点（股関節Ｊ９の中心）の脚平面ＰＬ上での加速度ベクトルは、身体座標系ＢＣの原点の加速度ベクトルACC(BCO/LC)とほぼ同一であるので、この加速度ベクトルACC(BCO/LC)に、Ｕ(Ｇ＿足平部/LC)''、Ｕ(Ｇ＿下腿部/LC)''、Ｕ(Ｇ＿大腿部/LC)''を加算したものが、脚平面ＰＬ上での重心Ｇ＿足平部、Ｇ＿下腿部、Ｇ＿大腿部の実際の加速度ベクトルを示すものとなる。

なお、図１６には代表的に、下腿部要素Ｓ１２に関する式（２５）のパラメータの関係を例示している。

従って、脚平面投影手段３６で求めた床反力Ｆｒｆ(脚体/LC)および加速度ベクトルACC(BCO/LC)と、前記２次元脚姿勢・要素重心位置算出手段２９で求めた足平部要素Ｓ１４の重心の位置ベクトルＵ(Ｇ＿足平部/LC)の時系列データから得られる相対加速度ベクトルＵ(Ｇ＿足平部/LC)''と、足平部要素Ｓ１４の重量ｍ＿足平部とから式（２４）の右辺の演算により、Ｆ(Ｐ＿足平部/LC)、すなわち、足首関節Ｊ＿足首に作用する並進力（脚平面ＰＬ上での２次元ベクトル）が求められる。また、その求めたＦ(Ｐ＿足平部/LC)（＝−Ｆ(Ｄ＿下腿部/LC)）と、脚平面投影手段３６で求めた加速度ベクトルACC(BCO/LC)と、２次元脚姿勢・要素重心位置算出手段２９で求めた下腿部要素Ｓ１２の重心の位置ベクトルＵ(Ｇ＿下腿部/LC)の時系列データから得られる相対加速度ベクトルＵ(Ｇ＿下腿部/LC)''と、下腿部要素Ｓ１２の重量ｍ＿下腿部とから式（２５）の右辺の演算によりＦ(Ｐ＿下腿部/LC)、すなわち、膝関節Ｊ＿膝に作用する並進力（脚平面ＰＬ上での２次元ベクトル）が求められる。同様に、その求めたＦ(Ｐ＿下腿部/LC)（＝−Ｆ(Ｄ＿大腿部/LC)）等を用いて、式（２６）の右辺の演算によりＦ(Ｐ＿大腿部/LC)、すなわち、股関節Ｊ＿股に作用する並進力（脚平面ＰＬ上での２次元ベクトル）が求められる。このように、関節要素Ｊ＿足首、Ｊ＿膝、Ｊ＿股に作用する反力ベクトル（並進力ベクトル）が上記（２４）〜（２６）の運動方程式に基づいて順番に算出される。

次に、足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの回転運動（それぞれの重心を通って脚平面ＰＬに垂直な軸廻りの回転運動）の運動方程式は次の式（２７）〜（２９）により与えられる。

Ｍ(Ｐ＿足平部)＝Ｉ＿足平部×(ω(足平部)’＋ω(BCO/LC)y’)
−(Ｕ(COP/LC)−Ｕ(Ｇ＿足平部/LC))×Ｆｒｆ(脚体/LC)
−(Ｕ(Ｐ＿足平部/LC)−Ｕ(Ｇ＿足平部/LC)）
×Ｆ(Ｐ＿足平部/LC) ……（２７）
Ｍ(Ｐ＿下腿部)＝Ｉ＿下腿部×(ω(下腿部)’＋ω(BCO/LC)y’)
−(Ｕ(Ｄ＿下腿部/LC)−Ｕ(Ｇ＿下腿部/LC))×Ｆ(Ｄ＿下腿部/LC)
−(Ｕ(Ｐ＿下腿部/LC)−Ｕ(Ｇ＿下腿部/LC)）
×Ｆ(Ｐ＿下腿部/LC)
−Ｍ（Ｄ＿下腿部） ……（２８）
Ｍ(Ｐ＿大腿部)＝Ｉ＿大腿部×(ω(大腿部)’＋ω(BCO/LC)y’)
−(Ｕ(Ｄ＿大腿部/LC)−Ｕ(Ｇ＿大腿部/LC))×Ｆ(Ｄ＿大腿部/LC)
−{(Ｕ(Ｐ＿大腿部/LC)−Ｕ(Ｇ＿大腿部/LC)）
×Ｆ(Ｐ＿大腿部/LC)
−Ｍ（Ｄ＿大腿部） ……（２９）
ここで、上記各式（２６）〜（２８）中に現れるＭ（Ｐ＿○○)、Ｍ（Ｄ＿○○)は、その○○で表される名称の剛体要素の端部が、それぞれに接触する物体から受ける反力モーメント（脚平面ＰＬに垂直な軸廻り（脚座標系ＬＣのＹ軸に平行な軸廻り）のモーメント）を意味している（図１４参照）。このため、作用・反作用の法則によって、Ｍ(Ｄ＿下腿部)＝−Ｍ(Ｐ＿足平部)、Ｍ(Ｄ＿大腿部)＝−Ｍ(Ｐ＿下腿部)である。また、Ｉ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部は、それぞれ足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０のそれぞれの重心廻りの慣性モーメントであり、これは、各剛体要素の重量などと同様、あらかじめ実測データ等に基づいて決定されて演算処理装置１８のメモリに記憶保持されている。また、ω(足平部)’、ω(下腿部)’、ω(大腿部)’はそれぞれ、足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１６の、脚座標系ＬＣから見た相対角速度ω(足平部)、ω(下腿部)、ω(大腿部)（脚平面ＰＬに垂直な軸廻りの相対角速度）の１階微分値、すなわち、相対角加速度を意味し、これらはそれぞれ、次式（２９ａ）〜（２９ｃ）の如く、前記２次元脚姿勢・要素重心位置算出手段２９で求めた足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１０の傾斜角θ＿足平部、θ＿下腿部、θ＿大腿部の２階微分値として与えられる。

ω(足平部)’＝θ＿足平部'' ……（２９ａ）
ω(下腿部)’＝θ＿下腿部'' ……（２９ｂ）
ω(大腿部)’＝θ＿大腿部'' ……（２９ｃ）
そして、ω(BCO/LC)y’は、前記脚平面投影手段３６で求めた身体座標系ＢＣの原点ＢＣＯの実際の角速度ω(BCO/LC)yの１階微分値であり、この１階微分値ω(BCO/LC)y’にω(足平部)’、ω(下腿部)’、ω(大腿部)’をそれぞれ加算したものが、それぞれ足平部要素Ｓ１４、下腿部要素Ｓ１２、大腿部要素Ｓ１４の実際の角加速度（脚平面ＰＬに垂直な軸廻りの角加速度）を表すものとなる。

なお、図１６には代表的に、下腿部要素Ｓ１２に関する式（２８）のパラメータの関係を例示している。

関節モーメント推定手段３７では、最終的に上記式（２７）〜（２９）により、関節モーメントＭ(Ｐ＿足平部)、Ｍ(Ｐ＿下腿部)、Ｍ(Ｐ＿大腿部)が順次求められる。すなわち、前記脚平面投影手段３６で求めた床反力Ｆｒｆ(脚体/LC)およびＵ(COP/LC)と、該脚平面投影手段３６で求めた角速度ω(BCO/LC)yの時系列データから把握される角加速度ω(BCO/LC)y’と、前記２次元脚姿勢・要素重心位置算出手段２９で求めた傾斜角θ＿足平部の時系列データから把握される相対角加速度ω(足平部)’（＝θ＿足平部''）と、該２次元脚姿勢・要素重心位置算出手段２９で求めた位置ベクトルＵ(Ｇ＿足平部/LC)およびＵ(Ｐ＿足平部/LC)（＝Ｕ(Ｊ＿足首/LC)（より正確には、これらの位置ベクトルのＸ軸成分及びＺ軸成分の組）と、前記式（２４）により先に求めた反力Ｆ(Ｐ＿足平部/LC)と、あらかじめ設定された慣性モーメントＩ＿足平部とから、前記式（２７）の右辺の演算により、関節モーメントＭ(Ｐ＿足平部)、すなわち、足首関節１３に作用する、脚平面ＰＬに垂直な軸廻りのモーメントが求められる。

また、この求めた関節モーメントＭ(Ｐ＿足平部)（＝−Ｍ(Ｄ＿下腿部)）と、前記式（２４），（２５）により先に求めた反力Ｆ(Ｐ＿足平部/LC)（＝−Ｆ(Ｄ＿下腿部/LC)）およびＦ(Ｐ＿下腿部/LC)と、脚平面投影手段３６で求めた角速度ω(BCO/LC)yの時系列データから把握される角加速度ω(BCO/LC)y’と、前記２次元脚姿勢・要素重心位置算出手段２９で求めた傾斜角θ＿下腿部の時系列データから把握される相対角加速度ω(下腿部)’（＝θ＿下腿部''）と、該２次元脚姿勢・要素重心位置算出手段２９で求めた位置ベクトルＵ(Ｇ＿下腿部/LC)、Ｕ(Ｐ＿下腿部/LC)（＝Ｕ(Ｊ＿膝/LC)）、およびＵ(Ｄ＿下腿部/LC)（＝Ｕ(Ｊ＿足首/LC)）（より正確には、これらの位置ベクトルのＸ座標成分及びＺ座標成分の組）と、あらかじめ設定された慣性モーメントＩ＿下腿部とから、前記式（２８）の右辺の演算により、関節モーメントＭ(Ｐ＿下腿部)、すなわち、膝関節１１に作用する、脚平面ＰＬに垂直な軸廻りのモーメントが求められる。同様に、その求めたＭ(Ｐ＿下腿部)（＝−Ｍ(Ｄ＿大腿部)）等を用いて、式（２８）の右辺の演算によりＭ(Ｐ＿大腿部)、すなわち、股関節９に作用する、脚平面ＰＬに垂直な軸廻りのモーメントが求められる。

なお、本実施形態では、各脚体部Ｓ２の各剛体要素の慣性モーメントＩ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部を考慮したが、これらは一般的には、十分に０に近い値である。このため、式（２６）〜（２８）の演算では、慣性モーメントＩ＿足平部、Ｉ＿下腿部、Ｉ＿大腿部を含む項を省略してもよい。この場合には、足平部要素Ｓ１４、下腿部要素Ｓ１２および大腿部要素Ｓ１０の角速度や角加速度を把握する必要はない。

以上のように、関節モーメント推定手段３７の演算処理では、各脚体２の足首関節１３、膝関節１１、および股関節９の、脚平面ＰＬに垂直な軸廻りの関節モーメントＭ(Ｐ＿足平部)、Ｍ(Ｐ＿下腿部)、Ｍ(Ｐ＿大腿部)が足首関節１３側から順番に算出される。なお、このように求められた関節モーメントは、例えば人間１の歩行を補助する装置（足首関節１３や、膝関節１１、股関節９に補助トルクを付与可能な電動モータを含む装置）の制御に用いられる。この場合、関節モーメントＭ(Ｐ＿足平部)、Ｍ(Ｐ＿下腿部)、Ｍ(Ｐ＿大腿部)は、脚平面ＰＬに垂直な軸廻りのモーメント、すなわち、脚体２の屈伸方向でのモーメントであるため、特に各脚体２の屈伸運動を補助するトルクを脚体２に適切に付与することが可能となる。

次に図１７を参照して、本実施形態による床反力の推定値の実施例を説明する。図１７は、両脚支持状態から単脚支持状態を経て両脚支持状態に至る１歩の期間において、接地状態に維持される脚体２に作用する床反力の、身体座標系ＢＣでのＸ軸成分Ｆｒｆ(脚体)xとＺ軸成分Ｆｒｆ(脚体)zとの推定値の経時変化の実測例を示すグラフである。なお、これらのグラフは、前述の通り床反力推定手段３５により求められ床反力の推定値に例えば２次のバタワースフィルタを使用したフィルタリング処理を施したものである。

このグラフに見られるように、Ｆｒｆ(脚体)zは、滑らかで連続的な変化を呈すると共に、２つの極大値を持ち、人間１の歩行時に特有の、いわゆる二峰性が見られる。また、Ｆｒｆ(脚体)xは、Ｆｒｆ(脚体)zと同様に滑らかで連続的な変化を呈すると共に、人間１の歩行時の特有の現象として、その値が負の値となる減速期と正の値となる加速期とが順番に明確に現れている。このことから、本実施形態によれば、連続的で滑らかな変化を呈する床反力の推定値を良好に求めることができると共に、その求めた床反力の推定値の信頼性も高いものとすることができることがわかる。

なお、以上説明した実施形態では、脚平面ＰＬに垂直な軸廻りの関節モーメントを求めるようにしたが、例えば身体座標系ＢＣのＹ軸廻りの関節モーメントを求めるようにしてもよい。この場合には、床反力推定手段３５で求めた、身体座標系ＢＣのＸＺ平面上での床反力の成分ベクトルを用いて、逆動力学演算によって関節モーメントを推定するようにすればよい。

また、前記実施形態では、足首関節Ｊ１３を備える剛体リンクモデルＳ１を用いたが、足首関節Ｊ１３を省略した剛体リンクモデル（足首関節が固定されているような剛体リンクモデル）を用いて、床反力の推定などを行なうようにすることも可能である。この場合には、足首関節１３の関節変位センサ２３を省略してよい。

また、前記実施形態では、２足歩行移動体として、人間１を例に採った場合を説明したが、例えば人間１の歩行補助を行なうために該人間１の脚体２などに装着される装具（脚体２と同じような運動を行なうことが可能な装具）を２足歩行移動体とみなし、その装具の単独の床反力を前記実施形態と同様に推定することも可能である。さらに、その推定値を、移動体がスムーズに移動可能なように床反力の目標値として利用することも可能である。

本発明を２足歩行移動体としての人間に適用した実施形態での全体的装置構成を模式的に示す図。図１の装置に備えたセンサボックスの構成を示すブロック図。図１の装置に備えた関節変位センサにより検出する関節の回転角と脚平面との関係を示す図（人間の下半身の正面図）。図１の装置における股関節の関節変位センサによる股関節の回転角の検出に関して説明するための図（人間の腰部近辺の側面図）。実施形態で用いる剛体リンクモデルと脚平面と座標系とを示す斜視図。図２の演算処理装置の機能的手段を示すブロック図。図６の２次元脚姿勢・要素重心位置算出手段により脚体の姿勢を求める処理を説明するための図。実施形態におけるセンサ座標系と身体座標系との間の座標変換の例を説明するための図。（ａ）〜（ｃ）は床反力の作用点の推定手法を説明するための図。図９（ｂ）の状態での床反力の作用点のＹ軸方向成分の推定手法を説明するための図。２足歩行移動体の単脚支持状態での床反力の推定手法を説明するための図。２足歩行移動体の両脚支持状態での床反力の推定手法を説明すための図。図６の床反力推定手段の演算処理機能を示すブロック図。図１３の床反力成分算出手段の処理で使用する重み係数と足平間前後距離との関係を示すグラフ。図６の脚平面投影手段の処理を説明するための図。関節モーメントを求めるための逆動力学モデルによる演算処理を説明するための図。人間の歩行時の１歩の期間における床反力の推定値の推移を示すグラフ。

符号の説明

１…人間（２足歩行移動体）、２…脚体、９…股関節、１１…膝関節、１３…足首関節、２１〜２３…関節変位センサ、１６…加速度センサ、１７…ジャイロセンサ（角速度センサ）、３３…全体重心位置算出手段、３４…床反力作用点推定手段、３５…床反力推定手段、Ｓ１…剛体リンクモデル、ＪＵ１，ＪＵ２，Ｊ９，Ｊ１１，Ｊ１３…関節要素、Ｓ８，Ｓ７，Ｓ６，Ｓ１０，Ｓ１２，Ｓ１４…剛体要素、Ｇ＿全体…全体重心（２足歩行移動体の重心）、Ｆｒｆ…床反力、ＣＯＰ…床反力作用点。

Claims

２足歩行移動体の各脚体に作用する床反力を推定する方法であって、
前記２足歩行移動体の各脚体の股関節および膝関節を少なくとも含む所定の複数の関節の変位量を逐次計測する第１ステップと、
前記２足歩行移動体を複数の剛体要素および複数の関節要素の連結体として表現してなる剛体リンクモデルと前記第１ステップで計測した関節の変位量とを少なくとも用いて２足歩行移動体の重心の位置を逐次算出する第２ステップと、
前記第２ステップで算出した重心の位置と２足歩行移動体に装着した加速度センサの出力とを少なくとも用いて該２足歩行移動体の重心の加速度を逐次算出する第３ステップと、
前記第３ステップで算出した重心の加速度と２足歩行移動体の重量とから２足歩行移動体に作用する全ての床反力の総和である全床反力の、少なくとも一つのあらかじめ定めた所定軸に垂直な面上での成分ベクトルを逐次算出する第４ステップと、
前記剛体リンクモデルと前記第１ステップで計測した関節の変位量と前記第２ステップで算出した重心の位置と前記第３ステップで算出した重心の加速度と２足歩行移動体に装着した角速度センサの出力とを少なくとも用いて該２足歩行移動体の全体の角運動量の時間的変化率である全角運動量変化率の前記所定軸廻りの成分を逐次算出する第５ステップと、
前記２足歩行移動体の接地している各脚体に作用する床反力の作用点である床反力作用点を逐次把握する第６ステップと、
前記第２ステップで算出した重心の位置と前記第４ステップで算出した全床反力の成分ベクトルと前記第５ステップで算出した全角運動量変化率の成分と前記第６ステップで把握した床反力作用点とを用いて、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和によって２足歩行移動体の重心の廻りに発生するモーメントの向きが前記全角運動量変化率の前記所定軸廻りの成分の向きと同一になるように、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを決定する第７ステップと、
該第７ステップで決定した床反力の成分ベクトルの向きと前記第４ステップで算出した全床反力の成分ベクトルとを用い、前記２足歩行移動体が両脚支持状態であるときには、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの向きを前記第７ステップで決定した向きに規定して、両脚体にそれぞれ作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和のベクトルが該全床反力の当該面上での成分ベクトルに等しくなるように、各脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの推定値を求め、前記２足歩行移動体が単脚支持状態であるときには、接地している脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの方向を前記第７ステップで決定した向きに規定して、該脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの大きさが該全床反力の当該面上での成分ベクトルの大きさに等しくなるように、該脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの推定値を求める第８ステップとを備えたことを特徴とする２足歩行移動体の床反力推定方法。
前記第７ステップは、前記第４ステップで算出した全床反力の成分ベクトルと前記第５ステップで算出した全角運動量変化率の成分とを用いて、該全床反力の前記所定軸に垂直な面上での成分ベクトルと該成分ベクトルによって２足歩行移動体の重心廻りに発生するモーメントとの関係を規定するモーメントアーム長を求めるステップと、その求めたモーメントアーム長と前記第２ステップで算出した重心の位置と前記第６ステップで把握した床反力作用点とを用いて、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での作用線が、２足歩行移動体の重心を中心として前記モーメントアーム長に等しい半径を有する円に接し、且つ、接地している各脚体に作用する床反力の前記所定軸に垂直な面上での成分ベクトルの総和によって２足歩行移動体の重心廻りに発生するモーメントの向きが、前記全角運動量変化率の該所定軸廻りの成分の向きに一致するように、接地している各脚体に作用する床反力の該所定軸に垂直な面上での成分ベクトルの向きを決定するステップとから成ることを特徴とする請求項１記載の２足歩行移動体の床反力推定方法。
前記所定軸は、少なくとも２足歩行移動体のほぼ左右方向に延びる軸を含むことを特徴とする請求項１又は２記載の２足歩行移動体の床反力推定方法。
前記２足歩行移動体のほぼ左右方向に延びる軸をＹ軸、このＹ軸に垂直な平面をＸＺ平面としたとき、前記２足歩行移動体の両脚支持状態において、両脚体にそれぞれ作用する床反力の前記ＸＺ平面上での成分ベクトルが互いに同一であり、且つ、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの総和のベクトルの大きさが前記第４ステップで算出した全床反力のＸＺ平面上での成分ベクトルの大きさに一致し、且つ、その総和のベクトルの向きが、ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点を通って前記円に接する直線の向きと同一になるように各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値を求める第９ステップを備え、少なくとも両脚支持状態における２足歩行移動体の前後方向での両脚体の足平部の間の距離である足平間前後距離があらかじめ定めた第１閾値よりも小さいときには、前記第８ステップで求められる推定値の代わりに、前記第９ステップで求められた推定値を各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値として得るようにしたことを特徴とする請求項３記載の２足歩行移動体の床反力推定方法。
前記２足歩行移動体のほぼ左右方向に延びる軸をＹ軸、このＹ軸に垂直な平面をＸＺ平面としたとき、前記２足歩行移動体の両脚支持状態において、両脚体にそれぞれ作用する床反力の前記ＸＺ平面上での成分ベクトルが互いに同一であり、且つ、両脚体にそれぞれ作用する床反力の該ＸＺ平面上での成分ベクトルの総和のベクトルの大きさが前記第４ステップで算出した全床反力の前記ＸＺ平面上での成分ベクトルの大きさに一致し、且つ、その総和のベクトルの向きが、ＸＺ平面上で両脚体の前記床反力作用点に応じて決定された作用点を通って前記円に接する直線の向きと同一になるように各脚体に作用する床反力の前記ＸＺ平面上での成分ベクトルの推定値を求める第９ステップと、前記第８ステップで求めた床反力のＸＺ平面上での成分ベクトルの推定値と前記第９ステップで求めた床反力のＸＺ平面上での成分ベクトルの推定値とにそれぞれ所定の重み係数Ｗ1，Ｗ2を乗算したものを加え合わせてなる重み付き平均値を各脚体毎に求め、その求めた各脚体に係る重み付き平均値を該脚体に作用する床反力の前記ＸＺ平面上での成分の推定値の真値として得る第１０ステップとを備え、
前記重み係数Ｗ1，Ｗ2は、両脚支持状態における２足歩行移動体の前後方向での両脚体の足平部の間の距離である足平間前後距離があらかじめ定めた第１閾値よりも小さいときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が前記第９ステップで求めた推定値と一致し、前記足平間前後距離が前記第１閾値よりも大きい値にあらかじめ定めた第２閾値よりも大きいときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が前記第８ステップで求めた推定値と一致し、前記足平間前後距離が第１閾値以上で第２閾値以下であるときには、前記第１０ステップで得られる床反力の前記ＸＺ平面上での成分ベクトルの推定値の真値が、前記足平間前後距離が大きくなるに伴い前記第９ステップで求めた推定値から前記第８ステップで求めた推定値に近づいていくように前記足平間前後距離に応じて設定されていることを特徴とする請求項３記載の足歩行移動体の床反力推定方法。