JP3844695B2 - Method of estimating floor reaction force action point for bipedal mobile body - Google Patents

Description

【００２２】
従って、二足歩行移動体１の重心Ｇ0の加速度ａ＝^T(ａx，ａz)を把握するとと共に、二足歩行移動体１の重心Ｇ0に対する各脚体２，２のそれぞれの特定部位３f，３rの位置（これは式（５）ではΔＸf，ΔＺf，ΔＸr，ΔＺrにより表される）を把握すれば、その加速度ａ及び特定部位３f，３rの位置と、二足歩行移動体１の重量Ｍの値と、重力加速度ｇの値とを用いて、前記式（５）により、各脚体２毎の床反力Ｆf＝^T(Ｆfx，Ｆfz)、Ｆr＝^T(Ｆrx，Ｆrz)の推定値を得ることができることとなる。
【００２３】
この場合、床反力Ｆf，Ｆrの推定値を得るために必要な重量Ｍは、あらかじめ計測等により把握することができる。また、重心Ｇ0の加速度ａや重心Ｇ0の位置、該重心Ｇ0に対する前記特定部位３f，３rの位置については、詳細は後述するが、二足歩行移動体１の各関節の屈曲角度（回転角度）を検出するセンサや、加速度センサ、ジャイロセンサ等のセンサの出力を用いて、公知の手法等により逐次把握することが可能である。
【００２４】
以下、二足歩行移動体としての人間に本発明の床反力作用点推定方法を適用した実施形態について説明する。
【００２５】
図２に模式化して示すように、人間１は、その構成を大別すると、左右一対の脚体２，２と、腰部３及び胸部４からなる胴体５と、頭部６と、左右一対の腕体７，７とを有する。胴体５は、その腰部３が脚体２，２のそれぞれに左右一対の股関節８，８を介して連結され、両脚体２，２上に支持されている。また、胴体５の胸部４は、腰部３の上側に該腰部３に対して人間１の前方側に傾斜可能に存している。そして、この胸部４の上部の左右両側部から腕体７，７が延設され、該胸部４の上端部に頭部６が支持されている。
【００２６】
各脚体２，２は、股関節８から延在する大腿部９と、該大腿部９の先端から膝関節１０を介して延在する下腿部１１とを有し、下腿部１１の先端部に、足首部（足首関節）１２を介して足平部１３が連結されている。
【００２７】
本実施形態では、このような構成を有する人間１の各脚体２に作用する床反力の推定、さらには膝関節１０及び股関節８に作用するモーメントの推定を行うために、次のような装置を人間１に装備している。
【００２８】
すなわち、胴体５の胸部４には、胸部４の傾斜に伴う角速度に応じた出力を発生するジャイロセンサ１４（以下、胸部ジャイロセンサ１４という）と、胸部４の前後方向の加速度に応じた出力を発生する加速度センサ１５（以下、胸部前後加速度センサ１５という）と、CPU、RAM、ROM等から構成される演算処理装置１６と、該演算処理装置１６等の電源となるバッテリ１７とが装着されている。この場合、これらの胸部ジャイロセンサ１４、胸部前後加速度センサ１５、演算処理装置１６及びバッテリ１７は、例えば胸部４に図示しないベルト等を介して固定されるショルダーバッグ状の収容部材１８に収容され、該収容部材１８を介して胸部４に一体的に固定されている。
【００２９】
尚、胸部加速度センサ１５の出力が表す加速度は、より詳しくは、胸部４の水平断面方向（胸部４の軸心と直交する方向）での前後方向の加速度であり、人間１が平地に直立姿勢で起立した状態では、前後水平方向（図２の絶対座標系のＸ軸方向）での加速度であるが、腰部３あるいは胸部４が鉛直方向（図２の絶対座標系のＺ軸方向）から傾斜した状態では、胸部４の鉛直方向に対する傾斜角度分だけ水平方向に対して傾斜した方向での加速度となる。
【００３０】
また、胴体５の腰部３には、腰部３の傾斜に伴う角速度に応じた出力を発生するジャイロセンサ１９（以下、腰部ジャイロセンサ１９という）と、腰部３の前後方向の加速度に応じた出力を発生する加速度センサ２０（以下、腰部前後加速度センサ２０という）と、腰部３の上下方向の加速度に応じた出力を発生する加速度センサ２１（以下、腰部上下加速度センサ２１という）とが、図示しないベルト等の固定手段を介して一体的に装着・固定されている。
【００３１】
ここで、腰部前後加速度センサ２０は、より詳しくは胸部前後加速度センサ１５と同様、腰部３の水平断面方向（腰部３の軸心と直交する方向）での前後方向の加速度を検出するセンサである。また、腰部上下加速度センサ２１は、より詳しくは、腰部３の軸心方向での上下方向の加速度（これは腰部前後加速度センサ２０が検出する加速度と直交する）を検出するセンサである。尚、腰部前後加速度センサ２０及び腰部上下加速度センサ２１は、二軸型の加速度センサにより一体的に構成されたものであってもよい。
【００３２】
さらに各脚体２の股関節８と膝関節１０とには、それぞれの屈曲角度Δθc，Δθdに応じた出力を発生する股関節角度センサ２２及び膝関節角度センサ２３が装着されている。尚、股関節角度センサ２２については、図２では手前側（人間１の前方に向かって右側）の脚体２の股関節８に係わる股関節角度センサ２２のみが図示されているが、他方側（人間１の前方に向かって左側）の脚体２の股関節８には、手前側の股関節角度センサ２２と同心に、股関節角度センサ２２が装着されている。
【００３３】
これらの角度センサ２２，２３は、例えばポテンショメータにより構成されたものであり、各脚体２に図示しないバンド部材等の手段を介して装着されている。ここで、各股関節角度センサ２２が検出する屈曲角度Δθcは、より詳しくは、腰部３と各脚体２の大腿部９との姿勢関係が所定の姿勢関係（例えば人間１の直立姿勢状態のように腰部３の軸心と大腿部９の軸心とがほぼ平行となる姿勢関係）にあるときを基準とした、腰部３に対する各脚体２の大腿部９の股関節８回り（人間１の左右方向における股関節８の軸心回り）の回転角度である。同様に、各膝関節角度センサ２３が検出する屈曲角度Δθdは、各脚体２の大腿部９と下腿部１１との姿勢関係が所定の姿勢関係（例えば大腿部９の軸心と下腿部１１の軸心とがほぼ平行となる姿勢関係）にあるときを基準とした、大腿部９に対する下腿部１１の膝関節１０回り（人間１の左右方向における膝関節１０の軸心回り）の回転角度である。
【００３４】
尚、前記各センサ１４，１５，１９〜２３は、それらの出力を演算処理装置１６に入力すべく、図示を省略する信号線を介して演算処理装置１６に接続されている。
【００３５】
前記演算処理装置１６は、図３に示すような機能的手段を備えている。すなわち、演算処理装置１６は、腰部上下加速度センサ２１の検出データと、後述する床反力推定手段３５により求められた各脚体２の床反力の推定値のデータとを用いて、人間１の脚体２，２の運動状態が単脚支持状態（図１（ａ）の状態）であるか、両脚支持状態（図１（ｂ）の状態）であるかを判断する脚体運動判断手段２４を備えている。また、演算処理装置１６は、胸部前後加速度センサ１５及び胸部ジャイロセンサ１４の検出データを用いて、胸部４の絶対座標系Ｃfにおける傾斜角度θa（具体的には例えば鉛直方向に対する傾斜角度θa。図２参照）を計測する胸部傾斜角度計測手段２５と、腰部前後加速度センサ２０及び腰部ジャイロセンサ１９の検出データを用いて、腰部３の絶対座標系Ｃfにおける傾斜角度θb（具体的には例えば鉛直方向に対する傾斜角度θb。図２参照）を計測する腰部傾斜角度計測手段２６とを備えている。
【００３６】
さらに、演算処理装置１６は、腰部前後加速度センサ２０及び腰部上下加速度センサ２１の検出データと前記腰部傾斜角度計測手段２６により計測された腰部３の傾斜角度θbのデータとを用いて、本実施形態における人間１の基準点として図２に示すように腰部３に設定される身体座標系Ｃp（図２のｘｚ座標）の原点Ｏの絶対座標系Ｃfにおける加速度（並進加速度）ａ₀＝^T(ａ₀x，ａ₀z)を求める基準加速度計測手段２７を備えている。ここで、身体座標系Ｃｐは、より詳しくは、例えば人間１の左右の股関節８，８のそれぞれの中心を結ぶ線の中点を原点Ｏとし、鉛直方向をｚ軸方向、人間１の前方に向かう水平方向をｘ軸方向とした座標系（３軸の方向が前記絶対座標系Ｃfと同一の座標系）である。
【００３７】
また、演算処理装置１６は、各脚体２の股関節角度センサ２２及び膝関節角度センサ２３の検出データと、前記腰部傾斜角度計測手段２６による腰部３の傾斜角度θbのデータとを用いて、絶対座標系Ｃfにおける各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θd（具体的には例えば鉛直方向に対する傾斜角度θc，θd。図２参照）求める脚体姿勢算出手段２８を備えている。
【００３８】
また、演算処理装置１６は、前記胸部傾斜角度計測手段２５、腰部傾斜角度計測手段２６及び脚体姿勢算出手段２８により得られる胸部４の傾斜角度θa、腰部３の傾斜角度θb、並びに各脚体２の大腿部９の傾斜角度θc及び下腿部１１の傾斜角度θdのデータを用いて、後述の剛体リンクモデルに対応する人間１の各剛体相当部の重心の位置（詳しくは前記身体座標系Ｃpにおける各剛体相当部の重心の位置）を求める各部重心位置算出手段２９と、その各剛体相当部の重心の位置のデータを用いて、上記身体座標系Ｃpにおける人間１の全体の重心の位置を求める身体重心位置算出手段３０と、その人間１の全体の重心G0（図１参照。以下、身体重心G0という）の位置のデータと前記脚体姿勢算出手段２８による各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdのデータとを用いて本実施形態における各脚体２の特定部位としての各脚体２の足首部１２の身体重心G0に対する位置（詳しくは、前記式（５）におけるΔＸf，ΔＺf，ΔＸr，ΔＺr）を求める足首位置算出手段３１と、前記身体重心位置算出手段３０による身体重心の位置のデータと前記基準加速度計測手段２７による身体座標系Ｃpの原点Ｏの加速度ａ₀のデータとを用いて絶対座標系Ｃfにおける身体重心G0の加速度ａ＝^T(ａx，ａz)(図１参照）を求める身体重心加速度算出手段３２とを備えている。
【００３９】
さらに、演算処理装置１６は、前記各部重心位置算出手段２９による人間１の各剛体相当部の重心の位置（詳しくは脚体２に係わる剛体相当部の重心の位置）のデータと前記基準加速度計測手段２７による身体座標系Cpの原点Ｏの加速度ａ₀のデータとを用いて絶対座標系Ｃfにおける各脚体２の大腿部９及び下腿部１１のそれぞれの重心の加速度（並進加速度）を求める脚体各部加速度算出手段３３と、前記脚体姿勢算出手段２８による各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdのデータを用いて絶対座標系Ｃfにおける各脚体２，２の大腿部９及び下腿部１１の角加速度を求める脚体各部角加速度算出手段３４と、前記脚体姿勢算出手段２８により求められる大腿部９の傾斜角度θcや、前記膝関節角度センサ２３で計測される膝関節１０の屈曲角度Δθdのデータを用いて接地している各脚体２の床反力作用点の位置を推定する床反力作用点推定手段３５とを備えている。
【００４０】
また、演算処理装置１６は、前記身体重心加速度算出手段３２による身体重心の加速度ａのデータと前記足首位置算出手段３１による各脚体２の足首部１２の身体重心に対する位置のデータと前記脚体運動判断手段２４による脚体２の運動状態の判断結果のデータとを用いて各脚体２に作用する床反力の推定値を求める床反力推定手段３６と、この床反力の推定値のデータと脚体各部加速度算出手段３３による各脚体２の大腿部９及び下腿部１１の重心の加速度のデータと脚体各部角加速度算出手段３４による各脚体２の大腿部９及び下腿部１１の角加速度のデータと床反力作用点推定手段３５による床反力作用点の推定位置のデータと前記脚体姿勢算出手段２８による各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdのデータとを用いて各脚体２の膝関節１０及び股関節８にそれぞれ作用するモーメントを推定する関節モーメント推定手段３７とを備えている。
【００４１】
次に、上述の演算処理装置１６の各手段のより詳細な処理内容と併せて、本実施形態の作動を説明する。
【００４２】
本実施形態では、例えば人間１が歩行等の脚体２の運動を行うに際して、両脚体２，２を着床させた状態で演算処理装置１６の図示しない電源スイッチを投入すると、該演算処理装置１６による処理が所定のサイクルタイム毎に以下に説明するように逐次実行され、各脚体２に作用する床反力の推定値等が逐次求められる。
【００４３】
すなわち、まず、演算処理装置１６は、前記脚体運動判断手段２４の処理を実行する。この脚体運動判断手段２４の処理では、前記サイクルタイム毎に、前記腰部上下加速度センサ２１による腰部３の上向き方向の加速度の検出データがあらかじめ定めた所定の閾値と比較される。そして、該加速度の検出値がその閾値を超えたときに、前記図１（ｂ）に示したような両脚支持状態が開始し、且つ、前記図１（ａ）に示したような単脚支持状態が終了すると判断される。すなわち、人間１の歩行時に、単脚支持状態から両脚支持状態への移行の際には、遊脚側の脚体２が着床することで、股関節８の近傍の腰部３には、ほぼ上向きに比較的大きな加速度（通常的な単脚支持状態では発生し得ない加速度）が発生する。このため、前記脚体運動判断手段２４は、上記のように腰部上下加速度センサ２１による腰部３の上向き方向の加速度の検出データを所定の閾値と比較することで、両脚支持状態の開始及び単脚支持状態の終了を判断する。
【００４４】
また、脚体運動判断手段２４の処理では、両脚支持状態で床反力推定手段３５により後述するように求められた両脚体２，２のそれぞれに作用する床反力Ｆf，Ｆr（図１（ｂ）参照）の推定値のうち、人間の進行方向に対して後側の脚体２に係る床反力Ｆr＝^T(Ｆrx，Ｆrz)の推定値（詳しくは演算処理装置１６の前回のサイクルタイムで求められた床反力Ｆrの絶対値＝√(Ｆrx²+Ｆrz²)）があらかじめ定めた所定の閾値（略「０」の正の値）と比較される。そして、該床反力Ｆrの推定値の絶対値がその閾値以下に低下したときに、両脚支持状態が終了し、且つ単脚支持状態が開始すると判断される。尚、本実施形態では、脚体２の運動状態の初期状態は、両脚支持状態であり、いずれか一方の脚体２に係る床反力の推定値が上記閾値以下に低下するまでは、脚体運動判断手段２４は、脚体２の運動状態が両脚支持状態であると判断する。
【００４５】
上述のような脚体運動判断手段２４の処理と並行して、演算処理装置１６は、前記胸部傾斜角度計測手段２５及び腰部傾斜角度計測手段２６による処理を実行する。この場合、胸部傾斜角度計測手段２５の処理では、胸部前後加速度センサ１５及び胸部ジャイロセンサ１４からそれぞれ入力される胸部４の前後方向の加速度、胸部４の角速度の検出データから、所謂カルマンフィルタの処理を用いた公知の手法により、絶対座標系Ｃfにおける胸部４の傾斜角度θaが前記サイクルタイム毎に逐次求められる。同様に、腰部傾斜角度計測手段２５の処理では、腰部前後加速度センサ２０及び腰部ジャイロセンサ１９からそれぞれ入力される腰部３の前後方向の加速度、腰部３の角速度の検出データから、カルマンフィルタの処理を用いて絶対座標系Ｃfにおける腰部３の傾斜角度θbが逐次求められる。ここで、絶対座標系Ｃfにおける胸部４及び腰部３のそれぞれの傾斜角度θa，θbは、本実施形態では例えば鉛直方向（重力方向）に対する傾斜角度である。
【００４６】
尚、例えばジャイロセンサ１４，１９による角速度の検出データを積分することで、胸部４や腰部３の傾斜角度を求めることも可能であるが、本実施形態のようにカルマンフィルタの処理を用いることで、胸部４や腰部３の傾斜角度θa，θbを精度よく計測することができる。
【００４７】
次に、演算処理装置１６は、前記脚体姿勢算出手段２８の処理と前記基準加速度計測手段２７の処理とを実行する。
【００４８】
前記脚体姿勢算出手段２８による処理では、絶対座標系Ｃfにおける各脚体２の大腿部９及び下腿部１１の傾斜角度θc，θd（鉛直方向に対する傾斜角度。図２参照）が前記サイクルタイム毎に次のように求められる。すなわち、各脚体２の大腿部９の傾斜角度θcは、その脚体２に装着されている前記股関節角度センサ２２による股関節８の屈曲角度Δθcの検出データの今回値と、前記腰部傾斜角度計測手段２５により求められた腰部３の傾斜角度θbの今回値とから次式（６）により算出される。
【００４９】
θc＝θb＋Δθc ……（６）
【００５０】
ここで、腰部３の傾斜角度θbは、該腰部３の上端部が下端部よりも人間１の前方側に突き出るように該腰部３が鉛直方向に対して傾斜している場合に負の値となるものであり、股関節８の屈曲角度Δθcは、大腿部９の下端部が人間１の前方側に突き出るように大腿部９が腰部３の軸心に対して傾斜している場合に正の値となるものである。
【００５１】
さらに、各脚体２の下腿部１１の傾斜角度θdは、上記のように求められた大腿部９の傾斜角度θcの今回値と、該脚体２に装着されている前記膝関節角度センサ２３による膝関節１０の屈曲角度Δθdの検出データの今回値とから次式（７）により算出される。
【００５２】
θd＝θc−Δθd ……（７）
【００５３】
ここで、膝関節１０の屈曲角度Δθdは、下腿部１１が大腿部９の軸心に対して該大腿部９の背面側に傾斜している場合に正の値となるものである。
【００５４】
また、前記基準加速度計測手段２７の処理では、前記身体座標系Ｃpの原点Ｏの絶対座標系Ｃfにおける加速度ａ₀＝^T(a₀x，a₀z）が次のように求められる。すなわち、前記腰部前後加速度センサ２０による腰部３の前後方向の加速度の検出データの今回値をap、前記腰部上下加速度センサ２１による腰部３の上下方向の加速度の検出データの今回値をaqとすると、それらの検出データap，aqと、前記腰部傾斜角度計測手段２５により求められた腰部３の傾斜角度θbの今回値とから、次式（８）により絶対座標系Ｃfにおける加速度ａ₀＝^T(a₀x，a₀z）が求められる。
【００５５】

【００５６】
次に、演算処理装置１６は、前記各部重心位置算出手段２９の処理を実行し、以下に説明する剛体リンクモデルを用いて、前記身体座標系Ｃpにおける人間１の各剛体相当部の重心の位置（身体座標系Ｃpの原点に対する位置）を求める。
【００５７】
図４に示すように、本実施形態で用いる剛体リンクモデルＲは、人間１を、各脚体２の大腿部９に相当する剛体R1，R1と、下腿部１１に相当する剛体R2，R2と、腰部３に相当する剛体R3と、前記胸部４、腕体７，７及び頭部６を合わせた部分３８（以下、上体部３８という）に相当する剛体R4とを連結してなるものとして表現するモデルである。この場合、各剛体R1と剛体R3との連結部、並びに、各剛体R1と剛体R2との連結部がそれぞれ股関節８、膝関節１０に相当する。また、剛体R3と剛体R4との連結部は腰部３に対する胸部４の傾動支点部３９である。
【００５８】
そして、本実施形態では、このような剛体リンクモデルＲの各剛体R1〜R4に対応する人間１の剛体相当部（各脚体２の大腿部９及び下腿部１１、腰部３、上体部３８）のそれぞれの重心G1、G2、G3、G4の各剛体相当部における位置があらかじめ求められ、演算処理装置１６の図示しないメモリに記憶されている。
【００５９】
ここで、演算処理装置１６に記憶保持している各剛体相当部の重心G1、G2、G3、G4の位置は、各剛体相当部に対して固定した座標系での位置である。この場合、各剛体相当部の重心G1、G2、G3、G4の位置を表すデータとして、例えば、各剛体相当部の一端部の関節の中心点から該剛体相当部の軸心方向の距離が用いられる。具体的には、例えば図４に示すように、各大腿部９の重心G1の位置は、該大腿部９の股関節８の中心から大腿部９の軸心方向に距離t1の位置、各下腿部１１の重心G2の位置は、該下腿部１１の膝関節１０の中心から下腿部１１の軸心方向に距離t2の位置として表され、それらの距離t1，t2の値があらかじめ求められて演算処理装置１６に記憶保持されている。他の剛体相当部の重心、G3、G4の位置についても同様である。
【００６０】
尚、上体部３８の重心G4の位置は、厳密には、該上体部３８に含まれる腕体７，７の動きの影響を受けるが、歩行時における各腕体７，７は、一般に胸部４の軸心に対して対称的な位置関係になるので、上体部３８の重心G4の位置はさほど変動せず、例えば直立姿勢状態における上体部３８の重心G4の位置とほぼ同一となる。
【００６１】
また、本実施形態では、各剛体相当部（各脚体２の大腿部９及び下腿部１１、腰部３、上体部３８）の重心G1、G2、G3、G4の位置を表すデータの他、各剛体相当部の重量のデータや、各剛体相当部のサイズのデータ（例えば各剛体相当部の長さのデータ）があらかじめ求められて、演算処理装置１６に記憶保持されている。
【００６２】
尚、下腿部１１の重量は、足平部１３を含めた重量である。また、上述のように演算処理装置１６にあらかじめ記憶保持したデータは、実測等により求めておいてもよいが、人間１の身長や体重から、人間の平均的な統計データに基づいて推測するようにしてもよい。一般に、上記各剛体相当部の重心G1、G2、G3、G4の位置や、重量、サイズは、人間の身長や体重と相関性があり、その相関データに基づいて、人間の身長及び体重のデータから、上記各剛体相当部の重心G1、G2、G3、G4の位置や、重量、サイズを比較的精度よく推測することが可能である。
【００６３】
前記各部重心位置算出手段２９は、上述のように演算処理装置１６にあらかじめ記憶保持したデータと、前記胸部傾斜角度計測手段２５及び腰部傾斜角度計測手段２６によりそれぞれ求められた胸部４の傾斜角度θa（＝上体部３８の傾斜角度）及び腰部３の傾斜角度θbの今回値と、前記脚体姿勢算出手段２８により求められた各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdの今回値とから、腰部３に固定された原点Ｏを有する身体座標系Ｃp（図４のｘｚ座標）での各剛体相当部の重心G1、G2、G3、G4の位置を求める。
【００６４】
この場合、各剛体相当部（各脚体２の大腿部９及び下腿部１１、腰部３、上体部３８）の傾斜角度θa〜θdが上述のように求められているので、その傾斜角度θa〜θｄのデータと、各剛体相当部のサイズのデータとから身体座標系Ｃpにおける各剛体相当部の位置及び姿勢が判る。従って、身体座標系Cpにおける各剛体相当部の重心G1、G2、G3、G4の位置が求められることとなる。
【００６５】
具体的には、例えば図４を参照して、同図４の左側に位置する脚体２に関し、大腿部９の身体座標系Ｃpにおける傾斜角度（ｚ軸方向に対する傾斜角度）はθc（この場合、図４ではθc＜０である）であるので、身体座標系Ｃpにおける大腿部９の重心G1の位置の座標は、（t1・sinθc，−t1・cosθc）となる。また、下腿部１１の身体座標系Ｃpにおける傾斜角度はθd（図４ではθd＜０）であるので、身体座標系Ｃpにおける下腿部１１の重心G2の位置の座標は、大腿部９の長さをＬcとすると、（Ｌc・sinθc＋t2・sinθd，−Lc・cosθc−t2・cosθd）となる。他の脚体２の大腿部９及び下腿部１１並びに、腰部３及び上体部３８の重心についても上記と同様に求められる。
【００６６】
このようにして、各部重心位置算出手段２９により、身体座標系Ｃpにおける各剛体相当部の重心G1、G2、G3、G4の位置を求めた後、演算処理装置１６は、前記身体重心位置算出手段３０の処理実行し、各剛体相当部の重心G1、G2、G3、G4の位置のデータと、各剛体相当部の重量のデータとを用いて身体座標系Ｃpにおける人間１の身体重心G0の位置（xg，zg）を求める。
【００６７】
ここで、身体座標系Ｃpにおける腰部３の重心G3の位置及び重量をそれぞれ（x3，z3）、m3、上体部３８の重心G4の位置及び重量をそれぞれ（x4，z4）、m4、人間１の前方に向かって左側の脚体２の大腿部９の重心G1の位置及び重量をそれぞれ（x1L，z1L）、m1L、同脚体２の下腿部１１の重心G2の位置及び重量をそれぞれ（x2L，z2L）、m2L、右側の脚体２の大腿部９の重心G1の位置及び重量をそれぞれ（x1R，z1R）、m1R、同脚体２の下腿部１１の重心G2の位置及び重量をそれぞれ（x2R，z2R）、m2R、人間１の体重をＭ（＝m1L＋m2L＋m1R＋m2R＋m3＋m4）とすると、身体座標系Ｃpにおける人間１の身体重心G0の位置（xg，zg）は次式（９）により求められる。
【００６８】

【００６９】
このようにして身体重心位置算出手段３０の処理を実行した後、さらに、演算処理装置１６は、前記身体重心加速度算出手段３２の処理と、前記足首位置算出手段３１の処理とを実行する。
【００７０】
この場合、身体重心加速度算出手段３２の処理では、まず、前記サイクルタイム毎に身体重心位置算出手段３０により求められる身体座標系Ｃpにおける身体重心G0の位置（xg，zg）の時系列データを用いて、身体座標系Ｃpにおける身体重心G0の位置（xg，zg）の２階微分値、すなわち、身体座標系Ｃpの原点Ｏに対する身体重心G0の加速度^T(d²xg／dt²，d²zg／dt²)が求められる。そして、この加速度^T(d²xg／dt²，d²zg／dt²)と、前記基準加速度計測手段２７により求められた身体座標系Ｃpの原点Ｏの絶対座標系Ｃfにおける加速度a₀＝^T(a₀x，a₀z）とのベクトル和を求めることにより、絶対座標系Ｃfにおける身体重心G0の加速度ａ＝^T(ax，az)が求められる。
【００７１】
また、前記足首位置算出手段３１の処理では、まず、前記脚体姿勢算出手段２８により求められた各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdのデータの今回値と、前記腰部傾斜角度計測手段２６により求められた腰部３の傾斜角度θbのデータの今回値と、該大腿部９及び下腿部１１のサイズ（長さ）のデータとから、前記各部重心位置算出手段２９の処理と同様の処理によって、前記身体座標系Ｃpにおける各脚体２の足首部１２の位置が求められる。具体的には、図４を参照して、同図４の左側に位置する脚体２に関し、下腿部１１の長さ（膝関節１０の中心から足首部１２までの長さ）をＬdとすると、身体座標系Ｃpにおける足首部１２の位置の座標（x12，z12）は、（Lc・sinθc＋Ld・sinθd，−Lc・cosθc−Ld・cosθd）となる（但し、図４ではθc＜０、θd＜０）。他方の脚体２についても同様である。
【００７２】
そして、この足首部１２の身体座標系Ｃpにおける位置（x12，z12）と前記身体重心位置算出手段３０により求められた身体座標系Ｃpにおける身体重心G0の位置（xg，zg）のデータの今回値とから、身体重心Ｇ0に対する各脚体２の足首部１２の位置ベクトル^T(x12−xg，z12−zg）、すなわち、前記式（５）におけるΔＸf，ΔＺf，ΔＸr，ΔＺrが求められる。
【００７３】
次に、演算処理手段１６は、前記床反力推定手段３６の処理を次のように実行する。すなわち、この処理では、前記脚体運動判断手段２４により今回のサイクルタイムで判断された脚体２の運動状態が単脚支持状態である場合には、人間１の体重Ｍ及び重力加速度ｇの値（これらはあらかじめ演算処理装置１６に記憶されている）と、前記身体重心加速度算出手段３２により求められた絶対座標系Ｃfにおける身体重心G0の加速度ａ＝^T(ａx，ａz)の今回値とから、前記式（２）により、接地している脚体２に作用する床反力Ｆ＝^T(Ｆx，Ｆz)の推定値が求められる。尚、この場合、非接地側の脚体２（遊脚側の脚体２）に作用する床反力は、^T(０，０)である。
【００７４】
また、脚体運動判断手段２４により今回のサイクルタイムで判断された脚体２の運動状態が両脚支持状態である場合には、人間１の体重Ｍ及び重力加速度ｇと、前記身体重心加速度算出手段３２により求められた絶対座標系Ｃfにおける身体重心G0の加速度ａ＝^T(ａx，ａz)の今回値と、前記足首位置算出手段３１により求められた各脚体２の足首部１２の身体重心G0に対する位置の今回値のデータ（式（５）のΔＸf，ΔＺf，ΔＸr，ΔＺrのデータの今回値）とから、前記式（５）により、各脚体２毎の床反力Ｆf＝^T(Ｆfx，Ｆfz)、Ｆr＝^T(Ｆrx，Ｆrz)の推定値が求められる。
【００７５】
一方、演算処理装置１６は、上述のような身体重心位置算出手段３０、身体重心加速度加速度算出手段３２、足首位置算出手段３１、及び床反力推定手段３６の処理と並行して、前記脚体各部加速度算出手段３３、脚体各部角加速度算出手段３４、床反力作用点推定手段３５の処理を実行する。
【００７６】
この場合、前記脚体各部加速度算出手段３３の処理では、前記身体重心加速度算出手段３２の処理と同様、まず、前記サイクルタイム毎に前記各部重心位置算出手段２９により求められる身体座標系Ｃpにおける各脚体２の剛体相当部である大腿部９及び下腿部１１の重心G1，G2の位置のそれぞれの時系列データを用いて、身体座標系Ｃpにおける大腿部９及び下腿部１１の重心G1，G2の位置のそれぞれの２階微分値、すなわち、身体座標系Ｃpにおける大腿部９及び下腿部１１の重心G1，G2のそれぞれの加速度（身体座標系Ｃpの原点Ｏに対する加速度）が求められる。そして、このそれぞれの加速度と、前記基準加速度計測手段２７による腰部３の絶対座標系Ｃfにおける加速度a₀＝^T(a₀x，a₀z）とのベクトル和を求めることにより、絶対座標系Ｃfにおける大腿部９及び下腿部１１のそれぞれの加速度（より詳しくは、該加速度の絶対座標系Ｃfにおける座標成分）が求められる。
【００７７】
また、前記脚体各部角加速度算出手段３４の処理では、前記サイクルタイム毎に前記脚体姿勢算出手段２８により求められる各脚体２の大腿部９及び下腿部１１のそれぞれの傾斜角度θc，θdの時系列データを用いて、該大腿部９及び下腿部１１のそれぞれ傾斜角度θc，θdの２階微分値、すなわち、大腿部９及び下腿部１１のそれぞれの角加速度が求められる。
【００７８】
また、床反力作用点推定手段３５の処理では、接地している脚体２について、例えば前記脚体姿勢算出手段２８により求められた大腿部９の傾斜角度θcの今回値から、図５及び図６に示すようにあらかじめ定められた相関関係に基づいて該脚体２の足首部１２から、該脚体２の足平部１３の床反力作用点（足平部１３の接地箇所に作用する全床反力が集中するとみなせる点）へのベクトル（足首部１２に対する床反力作用点の位置ベクトル。以下、床反力作用点ベクトルという）を該床反力作用点の位置を表すデータとして求める。
【００７９】
すなわち、本願発明者の知見によれば、接地している脚体２の大腿部９の傾斜角度θcや膝関節１０の屈曲角度Δθdは、床反力作用点と比較的顕著な相関性を有し、例えば大腿部９の傾斜角度θcに対して、前記床反力作用点ベクトル、詳しくは、人間１の進行方向（Ｘ軸方向）における該床反力作用点ベクトルの成分と、鉛直方向（Ｚ軸方向）における該床反力作用点ベクトルの成分とは、それぞれ図５、図６に示すように変化する。ここで、大腿部９の負の傾斜角度θcは、脚体２が人間１の後側に延びるように大腿部９が腰部３の軸心に対して傾斜しているとき（例えば図２の人間１の前方に向かって右側の脚体２）の角度であり、正の傾斜角度θcは、脚体２が人間１の前側に存するように大腿部９が腰部３の軸心に対して傾斜しているとき（例えば図２の人間１の前方に向かって左側の脚体２）の角度である。
【００８０】
そこで、本実施形態では、図５及び図６の相関関係を表す、大腿部９の傾斜角度θcをパラメータとする近似式を作成して、この近似式を演算処理装置１６にあらかじめ記憶保持させている。そして、前記床反力作用点推定手段３５の処理では、前記脚体姿勢算出手段２８により求められた大腿部９の傾斜角度θcの今回値を上記近似式に代入して、前記床反力作用点ベクトル（詳しくは該床反力作用点ベクトルのＸ軸方向、Ｚ軸方向の成分）を求めている。
【００８１】
ここで、図５及び図６のように、大腿部９の傾斜角度θcが極小値を持つ相関関係では、大腿部９の傾斜角度θcが同一であっても傾斜角度θcの減少過程と増加過程とで床反力作用点ベクトルの値が異なる。そこで、本実施形態では、上記近似式を作成する際、足平部１３の踵が着床してからつま先が離床するまでの前記相関関係の推移を、大腿部９の傾斜角度θcが正である第１の相（図５ではａ１の相、図６ではｂ１の相）と、大腿部９の傾斜角度θcが負で、且つ、大腿部９の傾斜角度θcの変化速度、即ち、大腿部９の傾斜角速度が負である第２の相（図５ではａ２の相、図６ではｂ２の相）と、大腿部９の傾斜角度θcが負で、且つ、大腿部９の傾斜角速度が正である第３の相（図５ではａ３の相、図６ではｂ３の相）とに区分し、床反力作用点ベクトルのＸ軸方向成分、Ｚ軸方向成分のそれぞれについて、各相を同一または異なる関数で近似するようにした。図５の相関関係における第１および第２の相ａ１，ａ２を合わせた相の近似式は、床反力作用点ベクトルのＸ軸方向成分をｐxとして、例えば、
ｐx＝x₁・θc⁶＋x₂・θc⁵＋x₃・θc⁴＋x₄・θc³＋x₅・θc²＋x₆・θc＋x₇
という形の６次の多項式関数（x₁〜x₇は定数値）により表される。また、図５の相関関係における第３の相ａ３の近似式は、例えば
ｐx＝x₈・θc⁴＋x₉・θc³＋x₁₀・θc²＋x₁₁・θc＋x₁₂
という形の４次の多項式関数（x₈〜x₁₂は定数値）により表される。
【００８２】
また、図6の相関関係における第１および第２の相ｂ１，ｂ２を合わせた相の近似式は、床反力作用点ベクトルのＺ軸方向成分をｐzとして、例えば
ｐz＝z₁・θc⁶＋z₂・θc⁵＋z₃・θc⁴＋z₄・θc³＋z₅・θc²＋z₆・θc＋z₇
という形の６次の多項式関数（z₁〜z₇は定数値）により表される。また、図６の相関関係における第３の相ｂ３の近似式は、例えば
ｐz＝z₈・θc³＋z₉・θc²＋z₁₀・θc＋z₁₁
という形の３次の多項式関数（z₈〜z₁₁は定数値）により表される。
【００８３】
そして、床反力作用点ベクトルを求める際は、大腿部９の傾斜角度θcの正負を識別すると共に、大腿部９の傾斜角度θcの時系列データの一階微分で算出される大腿部９の傾斜角速度の正負を識別する。さらに、これらの識別された傾斜角度θcの正負と傾斜角速度の正負とから現在どの相に存するかを判別して、判別された相の近似式に大腿部９の傾斜角度θcの今回値を代入することにより床反力作用点ベクトルを算出する。これにより、大腿部９の傾斜角度θcの減少過程での床反力作用点ベクトルの値と増加過程での床反力作用点ベクトルの値とを区別して算出することができる。
【００８４】
尚、床反力作用点の位置は、接地している脚体２の膝関節１０の屈曲角度との相関性もあり、大腿部９の傾斜角度θcの代わりに、膝関節角度センサ２３で計測される膝関節１０の屈曲角度Δθdから、床反力作用点の位置を推定するようにしてもよく、あるいは、大腿部９の傾斜角度θcと膝関節１０の屈曲角度Δθdとの両者を用いて、マップ等により床反力作用点の位置を推定するようにしてもよい。
【００８５】
また、人間１が椅子に座るときや椅子に座っている状態から立ち上がるときは、床反力作用点の位置と膝関節１０の屈曲角度Δθdとの間に図７（椅子座り時），図８（椅子立ち時）に示す相関関係が成立し、階段を上るときや下るときは、床反力作用点の位置と大腿部９の傾斜角度θcとの間に図９（階段上り時）、図１０（階段下り時）が成立する。従って、椅子に座ったり立ち上がるときは、膝関節１０の屈曲角度Δθdから図７，図８の相関関係に基づいて床反力作用点の位置を推定でき、また、階段を上り下りするときは、大腿部９の傾斜角度θcから図９，図１０の相関関係に基づいて床反力作用点の位置を推定できる。
【００８６】
上記の如く床反力作用点の位置を推定すると、次に、演算処理装置１６は、前記関節モーメント推定手段３７の処理を実行して、各脚体２の膝関節１０及び股関節８に作用するモーメントを求める。この処理は、前記床反力推定手段３６、脚体各部加速度算出手段３３、脚体各部角加速度算出手段３４、床反力作用点推定手段３５、及び脚体姿勢算出手段２８によりそれぞれ求められたデータの今回値を用いて、所謂逆動力学モデルに基づいて行われる。この逆動力学モデルは、人間１の各剛体相当部の並進運動に関する運動方程式と回転運動に関する運動方程式とを用いて、床反力作用点により近い関節から順番に該関節に作用するモーメントを求めるものであり、本実施形態では、各脚体２の膝関節１０、股関節８に作用するモーメントが順番に求められる。
【００８７】
さらに詳細には、図１１を参照して、まず、各脚体２の下腿部１１に関し、下腿部１１の先端部の足首部１２に作用する力（関節反力）、下腿部１１の膝関節１０の部分に作用する力（関節反力）、及び下腿部１１の重心G2の並進加速度を、それぞれ絶対座標系Ｃfにおける成分表記によって、^T(Ｆ₁x，Ｆ₁z)、^T(Ｆ₂x，Ｆ₂z)、^T(a₂x，a₂z)とし、該下腿部１１の重量をm₂とする。このとき、下腿部１１の重心G2の並進運動に関する運動方程式は、次式（１０）となる。
【００８８】

【００８９】
ここで、下腿部１１の重心G2の加速度^T(a₂x，a₂z)は、前記脚体各部加速度算出手段３３により求められるものである。また、下腿部１１の先端部の足首部１２に作用する関節反力^T(Ｆ₁x，Ｆ₁z)は、近似的には、該下腿部１１を有する脚体２について前記床反力推定手段３６により求められる床反力の推定値に等しい。より詳しくは、単脚支持状態において、該脚体２が接地しているときには、関節反力^T(Ｆ₁x，Ｆ₁z)は、前記式（２）により求められる床反力^T(Fx，Fz）であり、該脚体２が遊脚側の脚体であるときには、^T(Ｆ₁x，Ｆ₁z)＝^T(０，０)である。また、両脚支持状態において、該脚体２が人間１の進行方向前方に向かって後側の脚体であるときには、関節反力^T(Ｆ₁x，Ｆ₁z)は、前記式（５）の床反力^T(Frx，Frz）であり、該脚体２が前側の脚体であるときには、前記式（５）の床反力^T(Ffx，Ffz）である。
【００９０】
従って、各脚体２の膝関節１０に作用する関節反力^T(Ｆ₂x，Ｆ₂z)は、脚体各部加速度算出手段３３により求められた下腿部１１の重心G2の加速度^T(a₂x，a₂z)のデータと、床反力推定手段３６により求められる床反力（＝^T(Ｆ₁x，Ｆ₁z)）のデータと、下腿部１１のあらかじめ求められた重量m₂のデータと、重力加速度ｇの値とから、上記式（１０）により求められる。
【００９１】
また、図１１を参照して、下腿部１１の先端部の足首部１２に作用するモーメントをＭ₁、下腿部１１の膝関節１０の部分に作用するモーメントをＭ₂、下腿部１１の重心G2の回りの慣性モーメントをI_G2、下腿部１１の重心G2の回りの角加速度をα₂とする。また、前記図４に対応させて、下腿部１１の重心G2と膝関節１０の中心との間の距離をt2、下腿部１１の重心G2と足首部１２との間の距離をt2’（＝Ld−t2）とすると、下腿部１１の重心G2の回りの回転運動に関する運動方程式は、次式（１１）となる。
【００９２】

【００９３】
ここで、式（１１）中のM₁は、同式（１１）に係わる下腿部１１を有する脚体２について前記床反力作用点推定手段３５により求められる床反力作用点ベクトルと、該脚体２について前記床反力推定手段３６により求められる床反力ベクトルとの外積（ベクトル積）として得られるモーメントである。また、α₂は、前記脚体各部角加速度算出手段３４により求められる下腿部１１の角加速度である。また、θdは前記脚体姿勢算出手段２８により求められる下腿部１１の傾斜角度である。また、^T(Ｆ₁x，Ｆ₁z)は、前述の通り、床反力推定手段３６により求められる床反力の推定値である。さらに、^T(F₂x，F₂z)は、前記式（１０）により求められるものである。また、慣性モーメントI_G2は下腿部１１の重量m₂やサイズのデータ等と共に、あらかじめ求められて演算処理装置１６に記憶されるものである。
【００９４】
従って、膝関節１０に作用するモーメントM₂は、床反力推定手段３６による床反力の推定値のデータと、床反力作用点推定手段３５による床反力作用点ベクトルのデータと、脚体各部角加速度算出手段３４による下腿部１１の角加速度α₂のデータと、脚体姿勢算出手段２８による下腿部１１の傾斜角度θdのデータと、前記式（１０）により求められた関節反力^T(F₂x，F₂z)のデータと、あらかじめ求めた下腿部１１の慣性モーメントI_G2、サイズ（Ld）、重心G2の位置(t2)のデータとから前記式（１１）により求められる。
【００９５】
関節モーメント推定手段３７は、上記のようにして下腿部１１の膝関節１０の部分に作用するモーメントM₂を求めた後、その算出処理と同様の処理によって、大腿部９の股関節８の部分に作用するモーメントを求める。この処理の基本的な考え方は、膝関節１０のモーメントM₂を求める手法と同一であるので、詳細な図示及び説明は省略するが、その概要は次の通りである。
【００９６】
すなわち、まず、大腿部９の重心G1（図４参照）の並進運動に関する運動方程式に基づく次式（１２）（前記式（１０）と同じ形の式）により、大腿部９の股関節８の部分に作用する関節反力^T(F₃x，F₃z)が求められる。
【００９７】
^T(Ｆ₃x，Ｆ₃z)＝^T(Ｆ₂x−m₁・a₁x，Ｆ₂z−m₁・a₁z−m₁・ｇ)……（１２）
【００９８】
ここで、^T(Ｆ₂x，Ｆ₂z)は、先に前記式（１０）により求めた膝関節１０の関節反力である。また、^T(a₁x，a₁z)は、前記脚体各部加速度算出手段３３により求められる大腿部９の重心G1の絶対座標系Ｃfにおける加速度（並進加速度）である。また、m₁はあらかじめ求めた大腿部９の重量、ｇは重力加速度である。
【００９９】
次いで、大腿部９の重心G1の回りの回転運動に関する運動方程式に基づく次式（１３）（前記式（１１）と同じ形の式）により、大腿部９の股関節８の部分に作用するモーメントM₃が求められる。
【０１００】

【０１０１】
ここで、M2は、前記式（１１）により求められた膝関節１０のモーメント、^T(Ｆ₂x，Ｆ₂z)は、前記式（１０）により求められた膝関節１０の関節反力、^T(Ｆ₃x，Ｆ₃z)は、前記式（１２）により求められた股関節８の関節反力、I_G1は、あらかじめ求めた大腿部９の重心G1の回りの慣性モーメント、α₁は前記脚体各部角加速度算出手段３４により求められる大腿部９の角加速度、θcは前記脚体姿勢算出手段２８により求められる大腿部９の傾斜角度である。また、t1は、股関節８の中心から大腿部９の重心G1までの距離（図４参照）、t1’は、膝関節１０の中心から大腿部９の重心G1までの距離（図４ではLc−t1）であり、これらは、あらかじめ求めた重心G1の位置や大腿部９のサイズ（長さ）から定まるものである。
【０１０２】
以上説明した処理が、前記演算処理装置１６のサイクルタイム毎に逐次実行され、各脚体２に作用する床反力や、各脚体２の膝関節１０及び股関節８に作用するモーメントが逐次リアルタイムで推定される。
【０１０３】
尚、本明細書での詳細な説明は省略するが、求められた膝関節１０や股関節８のモーメントの推定値は、例えば人間１の歩行を補助する装置（膝関節１０や股関節８に補助トルクを付与可能な電動モータ等を含む装置）の制御に用いられる。
【０１０４】
前述した演算処理装置１６の処理により求められた床反力の推定値（詳しくは、該床反力の推定値の絶対値）の経時変化の様子を図１２〜図１４に実線で例示する。また、演算処理装置１６の処理により求められた膝関節１０及び股関節８のモーメントの推定値の経時変化の様子を図１５に実線で例示する。ここで、図１２及び図１５は、人間１が平地をほぼ一定速度で歩行した場合の例示、図１３は人間１が階段を上った場合の例示、図１４は人間１が椅子に座った状態から立ち上がった場合の例示である。この場合、図１２〜図１４では、フォースメータ等を用いて床反力を実測した比較例（床反力の真値に相当するもの）が仮想線で併記されている。また、図１５ではトルクメータ等を用いて膝関節１０及び股関節８のモーメントを実測した比較例（膝関節１０及び股関節８のモーメントの真値に相当するもの）が仮想線で併記されている。
【０１０５】
図１２〜図１４を参照して明らかなように、本実施形態によれば、脚体２の運動形態や運動環境によらずに精度のよい床反力の推定値が得られていることが判る。また、本実施形態ではこの床反力の推定値および床反力作用点の推定位置を用いることによって、図１５に示されるように、膝関節１０や股関節８のモーメントも比較的精度よく推定することができる。
【０１０６】
以上のように、本実施形態によれば、脚体２に人間１の歩行の邪魔となったり、脚体２の運動に負担がかかるようなセンサを装着したりすることなく、股関節８や股関節８に装着した角度センサ２２，２３や、胴体５に装備したジャイロセンサ１４，１９及び加速度センサ１５，２０，２１というような比較的小型で軽量なセンサを用いて各脚体２に作用する床反力および床反力作用点の位置を推定でき、この推定値から各脚体２の股関節８及び膝関節１０に作用するモーメントをリアルタイムで容易に推定することができる。しかも、その推定を、平地での歩行、階段での歩行等、脚体２の運動形態や運動環境によらずに比較的精度よく行うことができる。
【０１０７】
尚、以上説明した実施形態では、本発明を人間１に適用した場合を例にとって説明したが、二足歩行移動体としての二足歩行ロボットにも本発明を適用することができる。ここで、二足歩行ロボットでは、腰部と胸部とが一体的な構造となっている場合があるが、この場合には、ジャイロセンサや前後方向の加速度センサを腰部及び胸部のいずれか一方だけに取り付けて、床反力や脚体の関節のモーメントを本実施形態と同様に推定するようにすることも可能である。また、二足歩行ロボットでは、股関節や膝関節の屈曲角度は、それらの関節のアクチュエータに対する制御装置の制御量により把握するようにすることも可能である。
【０１０８】
また、前記実施形態では、脚体２の運動状態を判断するために、腰部上下加速度センサ２１の検出データをそのまま用いたが、該検出データの代わりに、例えば前記基準加速度計測手段２７により求められる絶対座標系Ｃfでの腰部３の加速度ａ₀の鉛直方向（Ｚ軸方向）の成分の値を用いるようにしてもよい。
【図面の簡単な説明】
【図１】床反力推定方法の基本的原理を説明するための図。
【図２】本発明の一実施形態における二足歩行移動体としての人間と該人間に装備する装置構成を模式化して示す図。
【図３】図２の装置に備える演算処理装置の機能を説明するためのブロック図。
【図４】図３の演算処理装置の処理に用いる剛体リンクモデルを示す図。
【図５】通常歩行時における床反力作用点ベクトルの進行方向成分と大腿部の傾斜角度との相関関係を示す線図。
【図６】通常歩行時における床反力作用点ベクトルの鉛直方向成分と大腿部の傾斜角度との相関関係を示す線図。
【図７】椅子座り時における床反力作用点ベクトルの進行方向成分と膝関節の屈曲角度との相関関係を示す線図。
【図８】椅子立ち上がり時における床反力作用点ベクトルの進行方向成分と膝関節の屈曲角度との相関関係を示す線図。
【図９】階段上り時における床反力作用点ベクトルの進行方向成分と大腿部の傾斜角度との相関関係を示す線図。
【図１０】階段下り時における床反力作用点ベクトルの進行方向成分と大腿部の傾斜角度との相関関係を示す線図。
【図１１】図３の演算処理装置の関節モーメント推定手段における処理を説明するための図。
【図１２】本発明の実施形態により求められた通常歩行時の床反力の推定値の経時変化の様子を例示するグラフ。
【図１３】本発明の実施形態により求められた階段上り時の床反力の推定値の経時変化の様子を例示するグラフ。
【図１４】本発明の実施形態により求められた椅子立ち上がり時の床反力の推定値の経時変化の様子を例示するグラフ。
【図１５】本発明の実施形態により求められた膝関節及び股関節のモーメントの推定値の経時変化の様子を例示するグラフ。
【符号の説明】
１…人間（二足歩行移動体）、２…脚体、８…股関節、１０…膝関節、１２…足首部（特定部位）、１３…足平部、２２…股関節角度センサ、２３…膝関節角度センサ。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a method for sequentially estimating the position of a floor reaction force action point for each leg of a biped walking moving body such as a human or a biped walking robot in real time during the movement of the biped walking moving body.
[0002]
[Prior art]
For example, when controlling the movement of a walking assist device that assists the human walking movement or the movement movement of a biped robot, the floor reaction force acting on the leg of the human or the biped robot (specifically, the leg body) It is necessary to successively grasp the force acting on the foot of the floor from the floor) and the position of the floor reaction force action point. By grasping the position of the floor reaction force and the floor reaction force action point, it becomes possible to grasp the moment acting on the joint of the leg of the biped walking mobile body, and based on the grasped moment etc. It becomes possible to determine the target assisting force of the walking assist device, the target driving torque of each joint of the biped walking robot, and the like.
[0003]
As a method for grasping the floor reaction force, for example, one disclosed in Japanese Patent Application Laid-Open No. 2000-249570 is known. In this technique, since the waveform of the floor reaction force of each leg changes with time during steady walking of a bipedal mobile body, the floor reaction force of each leg is changed to 1 / of the walking cycle. This is grasped as a composite value (primary combination) of a plurality of trigonometric functions having different periods of n (n = 1, 2,...). However, this cannot grasp the position of the floor reaction force action point, and is insufficient for grasping the moment acting on the joint of the leg of the bipedal walking moving body.
[0004]
There is also known a method of walking a bipedal walking body on a force plate installed on the floor and grasping the position of the floor reaction force and the floor reaction force action point based on the output of the force plate (Japanese Patent Laid-Open No. 2001-2001). No. 29329). However, this method has a problem that the floor reaction force and the position of the floor reaction force action point can be grasped only in an environment where the force plate is installed, and cannot be applied to walking in a normal environment.
[0005]
[Problems to be solved by the invention]
In view of the above points, the present invention can grasp the position of the floor reaction force action point with high accuracy in real time by a relatively simple method, and in particular, the floor reaction force action point related to a human being as a bipedal walking moving body. An object of the present invention is to provide a floor reaction force action point estimation method suitable for grasping the position of the floor.
[0006]
[Means for Solving the Problems]
In order to achieve the above object, the present invention is a method for sequentially estimating the position of the floor reaction force action point for each leg of a biped walking mobile body, and comprising the inclination angle of the thigh of each leg and the knee At least one of the flexion angles of the joint is taken as the measurement target angle, and this measurement target angle is sequentially measured during the movement of the bipedal mobile body, and the position vector of the floor reaction force acting point on the ankle of each leg and the measurement target The position vector is sequentially estimated from the measured value of the measurement target angle based on a predetermined correlation established with the angle.
[0007]
As a result of diligent efforts, the inventor of the present application, for example, has a relatively significant correlation between the inclination angle of the thigh of the leg that is in contact with the ground during normal walking and the bending angle of the knee joint with the floor reaction force action point. For example, it has been found that the correlation shown in FIGS. 5 and 6 is established between the traveling direction component and the vertical direction component of the position vector of the floor reaction force action point and the inclination angle of the thigh. Therefore, as in the present invention, the position vector of the floor reaction force acting point can be grasped in real time from the measured values of the inclination angle of the thigh as the measurement target angle and the bending angle of the knee joint.
[0008]
By the way, in order to obtain the position vector of the floor reaction force action point from the measurement value of the measurement target angle, the above correlation is stored as a data table, and the position of the floor reaction force action point corresponding to the measurement value of the measurement target angle is stored. A table may be searched for the vector. However, since this requires a large storage capacity, an approximate expression using the measurement target angle as a parameter representing the above correlation is created and stored, and the measured value of the measurement target angle is substituted into this approximate expression. It is desirable to calculate the position vector of the floor reaction force action point.
Here, it may be difficult to approximate the correlation between the position vector of the floor reaction force action point and the angle to be measured with one approximate expression. In this case, when creating the approximate expression, the transition of the correlation from when the heel of the foot of each leg is landed until the toe leaves is divided into several phases, and each phase is the same or Approximate with different functions. In particular, as shown in FIG. 5 and FIG. 6, the correlation has a minimum value with respect to the measurement target angle (thigh inclination angle), and even if the measurement target angle is the same, If the position vector value of the floor reaction force action point differs between the increase process and the transition of the correlation from the landing of the foot heel of each leg to the release of the toe in several phases In classifying, the phases are divided according to at least the change rate of the angle to be measured (the amount of change of the angle to be measured per unit time). Thereby, the position vector value in the process of decreasing the measurement target angle and the value of the position vector in the process of increasing the measurement target angle can be distinguished and calculated.
[0009]
DETAILED DESCRIPTION OF THE INVENTION
Before describing the embodiment of the present invention, the basic concept of the method for estimating the floor reaction force of a biped walking mobile will be described with reference to FIG. The motion state of the leg of the biped walking mobile body, for example, the motion state of the leg during walking motion is one of the two legs 2 and 2 of the biped walking mobile body 1 as illustrated in FIG. There are a single-leg support state in which only the leg body 2 (the front leg body in the traveling direction in the figure) is grounded, and a both-leg support state in which both legs 2 and 2 are grounded as shown in FIG.
[0010]
Here, first, in the single leg support state, the equation of motion of the center of gravity of the biped walking mobile body in the absolute coordinate system fixed to the floor on which the biped walking mobile body moves (specifically, the translational motion of the center of gravity) The equation of motion) is that the product of the acceleration of the center of gravity and the weight of the bipedal moving body is the gravity acting on the center of gravity (= weight of the bipedal moving body × gravity acceleration) and the grounded leg It becomes a relational expression that it is equal to the resultant force with the floor reaction force acting from the floor to the grounding part. Specifically, for example, as shown in FIG. 1 (a), in the absolute coordinate system Cf fixed with respect to the floor A, the acceleration a of the center of gravity G0 of the biped walking moving body 1 in the X-axis direction (biped walking movement) The horizontal component in the traveling direction of the body 1 and the components in the Z-axis direction (vertical direction) are ax and az, respectively, and X of the floor reaction force F related to the grounded leg 2 (support leg 2). When the components in the axial direction and the Z-axis direction are respectively Fx and Fz, the equation of motion of the center of gravity G0 is expressed by the following equation (1).
[0011]
^T(Fx, Fz-M · g) = M ·^T(ax, az) ...... (1)
(However, M is the weight of the bipedal moving object, g is the gravitational acceleration)
In addition, parentheses on both sides in the formula (1)^T(,) Means a two-component vector. In this specification^TA notation of the form (,) represents a vector.
[0012]
Therefore, the acceleration a = of the center of gravity G0 of the biped walking mobile 1^TIf (ax, az) is grasped, the floor reaction force F == by the following equation (2) using the acceleration a, the value of the weight M of the biped walking moving body 1 and the value of the gravitational acceleration g.^TAn estimated value of (Fx, Fz) can be obtained.
[0013]
^T(Fx, Fz) = M ・^T(ax, az-g) (2)
[0014]
In this case, the weight M necessary for obtaining the estimated value of the floor reaction force F can be grasped beforehand by measurement or the like. Although the details of the position of the center of gravity G0 and the acceleration a will be described later, the output of a sensor that detects the bending angle (rotation angle) of each joint of the bipedal walking moving body 1, an acceleration sensor, a gyro sensor, or the like. Can be sequentially grasped by a known method or the like.
[0015]
Next, the motion equation of the center of gravity of the biped walking mobile body in the state where both legs are in contact (specifically, the motion equation regarding the translational motion of the center of gravity) is the product of the acceleration of the center of gravity and the weight of the biped walking mobile body. Of the gravity acting on the body (= the weight of the bipedal moving body x gravity acceleration) and the floor reaction force (two floor reaction forces corresponding to both legs) acting on the grounding parts of both legs from the floor It becomes the relational expression that is equal to. Specifically, as shown in FIG. 1 (b), the XZ coordinate component of the floor reaction force Ff applied to the front leg 2 in the traveling direction of the biped walking mobile body 1 is Ffx, Ffz, and the rear leg. If the XZ coordinate components of the floor reaction force Fr related to the body 2 are Frx and Frz, the equation of motion of the center of gravity G0 is expressed by the following equation (3).
[0016]
^T(Ffx + Frx, Ffz + Frz−M · g) = M ·^T(ax, az) ...... (3)
The meanings of ax, az, M, and g in the formula (3) are as described above.
[0017]
On the other hand, according to the knowledge of the inventors of the present application, the floor reaction forces Ff and Fr related to the legs 2 and 2 in the both-leg-supported state are approximately as shown in FIG. , 2 can be regarded as acting toward the center of gravity G0 of the biped walking mobile body 1 from the specific parts 12f, 12r (for example, ankle part) in the vicinity of the lower end part. At this time, a fixed relational expression exists between the positions of the specific portions 12f and 12r of the legs 2 and 2 with respect to the center of gravity G0 and the floor reaction forces Ff and Fr acting on the legs 2 and 2. That is, the direction of the line segment connecting the center of gravity G0 and the specific parts 3f, 3r of the legs 2, 2 (the direction of the position vector of the specific parts 3f, 3r with respect to the center of gravity G0) is the legs 2, 2 A relational expression representing a relation that the floor reaction forces Ff and Fr are equal to each other is established.
[0018]
Specifically, referring to FIG. 1B, the coordinates of the position of the center of gravity G0 in the absolute coordinate system Cf are (Xg, Zg), and the coordinates of the position of the specific part 3f of the front leg 2 are (Xf, Zf), where the coordinates of the position of the specific part 3r of the rear leg 2 are (Xr, Zr), the above relational expression becomes the following expression (4).
[0019]
(Zf-Zg) / (Xf-Xg) = Ffz / Ffx
(Zr-Zg) / (Xr-Xg) = Frz / Frx (4)
[0020]
Then, the following equation (5) is obtained from the equation (4) and the equation (3).
[0021]

[0022]
Therefore, the acceleration a = of the center of gravity G0 of the biped walking mobile 1^TAs well as grasping (ax, az), the positions of the specific portions 3f and 3r of the legs 2 and 2 with respect to the center of gravity G0 of the biped walking mobile body 1 (this is ΔXf, ΔZf, ΔXr, (Expressed by ΔZr), using the acceleration a and the positions of the specific parts 3f and 3r, the value of the weight M of the biped walking mobile body 1, and the value of the gravitational acceleration g, 5), the floor reaction force Ff = for each leg 2^T(Ffx, Ffz), Fr =^TAn estimated value of (Frx, Frz) can be obtained.
[0023]
In this case, the weight M necessary for obtaining the estimated values of the floor reaction forces Ff and Fr can be grasped beforehand by measurement or the like. The acceleration a of the center of gravity G0, the position of the center of gravity G0, and the positions of the specific parts 3f and 3r with respect to the center of gravity G0 will be described in detail later, but the bending angle (rotation angle) of each joint of the biped walking mobile body 1 will be described later. It is possible to sequentially grasp by a known method or the like using the output of a sensor such as an acceleration sensor, an acceleration sensor, or a gyro sensor.
[0024]
Hereinafter, an embodiment in which the floor reaction force action point estimation method of the present invention is applied to a human being as a biped walking moving body will be described.
[0025]
As schematically shown in FIG. 2, the human 1 roughly divides the configuration into a pair of left and right legs 2, 2, a body 5 including a waist 3 and a chest 4, a head 6, and a pair of left and right Arms 7 and 7 are provided. The waist 5 of the body 5 is connected to the legs 2 and 2 via a pair of left and right hip joints 8 and 8, and is supported on both legs 2 and 2. Further, the chest part 4 of the torso 5 can be inclined to the front side of the human 1 with respect to the waist part 3 on the waist part 3. Arms 7 and 7 are extended from the left and right sides of the upper portion of the chest 4, and the head 6 is supported on the upper end of the chest 4.
[0026]
Each leg 2, 2 has a thigh 9 extending from the hip joint 8 and a crus 11 extending from the tip of the thigh 9 via the knee joint 10. A foot portion 13 is connected to the distal end portion of the foot portion via an ankle portion (ankle joint) 12.
[0027]
In the present embodiment, in order to estimate the ground reaction force acting on each leg 2 of the human 1 having such a configuration, and further to estimate the moment acting on the knee joint 10 and the hip joint 8, The device is equipped with a human 1.
[0028]
That is, the chest 4 of the torso 5 has a gyro sensor 14 (hereinafter referred to as a chest gyro sensor 14) that generates an output according to the angular velocity associated with the inclination of the chest 4 and an output according to the longitudinal acceleration of the chest 4. A generated acceleration sensor 15 (hereinafter referred to as a chest longitudinal acceleration sensor 15), an arithmetic processing device 16 including a CPU, a RAM, a ROM, and the like, and a battery 17 serving as a power source for the arithmetic processing device 16 are mounted. Yes. In this case, the chest gyro sensor 14, the chest longitudinal acceleration sensor 15, the arithmetic processing unit 16, and the battery 17 are housed in a shoulder bag-like housing member 18 that is fixed to the chest 4 via a belt (not shown), for example. It is integrally fixed to the chest 4 via the housing member 18.
[0029]
More specifically, the acceleration represented by the output of the chest acceleration sensor 15 is the acceleration in the front-rear direction in the horizontal sectional direction of the chest 4 (direction orthogonal to the axis of the chest 4), and the human 1 is standing upright on a flat ground. In the standing state, the acceleration is in the front-rear horizontal direction (X-axis direction of the absolute coordinate system in FIG. 2), but the waist 3 or the chest 4 is inclined from the vertical direction (Z-axis direction of the absolute coordinate system in FIG. 2). In this state, the acceleration is in the direction inclined with respect to the horizontal direction by the inclination angle of the chest 4 with respect to the vertical direction.
[0030]
Further, the waist 3 of the body 5 has a gyro sensor 19 (hereinafter referred to as a waist gyro sensor 19) that generates an output according to the angular velocity associated with the inclination of the waist 3 and an output according to the longitudinal acceleration of the waist 3. A belt (not shown) includes a generated acceleration sensor 20 (hereinafter referred to as a waist longitudinal acceleration sensor 20) and an acceleration sensor 21 (hereinafter referred to as a waist vertical acceleration sensor 21) that generates an output corresponding to the vertical acceleration of the waist 3. It is attached and fixed integrally through fixing means such as.
[0031]
Here, the waist longitudinal acceleration sensor 20 is a sensor that detects the acceleration in the longitudinal direction in the horizontal sectional direction of the waist 3 (direction perpendicular to the axis of the waist 3), more specifically, like the chest longitudinal acceleration sensor 15. . The waist vertical acceleration sensor 21 is more specifically a sensor that detects vertical acceleration in the axial direction of the waist 3 (which is orthogonal to the acceleration detected by the waist longitudinal acceleration sensor 20). Note that the waist longitudinal acceleration sensor 20 and the waist vertical acceleration sensor 21 may be integrally configured by a biaxial acceleration sensor.
[0032]
Further, a hip joint angle sensor 22 and a knee joint angle sensor 23 that generate outputs corresponding to the respective bending angles Δθc and Δθd are attached to the hip joint 8 and the knee joint 10 of each leg 2. As for the hip joint angle sensor 22, only the hip joint angle sensor 22 related to the hip joint 8 of the leg 2 on the front side (right side toward the front of the person 1) is shown in FIG. The hip joint angle sensor 22 is mounted concentrically with the hip joint angle sensor 22 on the front side of the hip joint 8 of the leg 2 on the left side of the leg 2.
[0033]
These angle sensors 22 and 23 are constituted by, for example, potentiometers, and are attached to the respective leg bodies 2 through means such as band members (not shown). Here, more specifically, the bending angle Δθc detected by each hip joint angle sensor 22 is a predetermined posture relationship between the waist 3 and the thigh 9 of each leg 2 (for example, in the upright posture state of the human 1). Thus, the hip joint 8 of the leg 2 with respect to the waist 3 around the hip joint 8 (human being) is based on the position where the axis of the waist 3 and the axis of the thigh 9 are substantially parallel to each other. 1 is a rotation angle around the axis of the hip joint 8 in the left-right direction. Similarly, the flexion angle Δθd detected by each knee joint angle sensor 23 is a predetermined posture relationship (for example, the axis of the thigh 9 and the axis of the thigh 9). The knee joint 10 around the knee joint 10 of the lower leg 11 with respect to the thigh 9 (the axis of the knee joint 10 in the left-right direction of the human 1) with reference to the case where the axis of the lower leg 11 is substantially parallel to the axis. Rotation angle.
[0034]
Each of the sensors 14, 15, 19 to 23 is connected to the arithmetic processing unit 16 through a signal line (not shown) in order to input the output to the arithmetic processing unit 16.
[0035]
The arithmetic processing unit 16 includes functional means as shown in FIG. That is, the arithmetic processing unit 16 uses the detection data of the waist vertical acceleration sensor 21 and the data of the estimated value of the floor reaction force of each leg 2 obtained by the floor reaction force estimation means 35 described later. Leg motion determining means for determining whether the motion state of the legs 2 and 2 is a single-leg support state (the state of FIG. 1A) or a both-leg support state (the state of FIG. 1B) 24. Further, the arithmetic processing unit 16 uses the detection data of the chest longitudinal acceleration sensor 15 and the chest gyro sensor 14, and uses the inclination angle θa in the absolute coordinate system Cf of the chest 4 (specifically, for example, the inclination angle θa with respect to the vertical direction). 2), and the tilt angle θb (specifically, for example, in the vertical direction) of the waist 3 in the absolute coordinate system Cf using the detection data of the chest front / rear acceleration sensor 20 and the waist gyro sensor 19. And a waist inclination angle measuring means 26 for measuring an inclination angle θb with respect to (see FIG. 2).
[0036]
Further, the arithmetic processing unit 16 uses the detection data of the waist longitudinal acceleration sensor 20 and the waist vertical acceleration sensor 21 and the data of the inclination angle θb of the waist 3 measured by the waist inclination angle measuring means 26 in the present embodiment. 2, the acceleration (translation acceleration) a in the absolute coordinate system Cf of the origin O of the body coordinate system Cp (xz coordinate in FIG. 2) set as the reference point of the human 1 at the waist 3 as shown in FIG.₀=^T(a₀x, a₀Reference acceleration measuring means 27 for obtaining z) is provided. Here, the body coordinate system Cp is more specifically, for example, the center point of the line connecting the centers of the left and right hip joints 8 and 8 of the human 1 is the origin O, the vertical direction is the z-axis direction, and the human 1 is in front. This is a coordinate system (the coordinate system in which the directions of the three axes are the same as the absolute coordinate system Cf) in which the horizontal direction is the x-axis direction.
[0037]
Further, the arithmetic processing unit 16 uses the detection data of the hip joint angle sensor 22 and the knee joint angle sensor 23 of each leg 2 and the data of the inclination angle θb of the waist 3 by the waist inclination angle measuring means 26 to make absolute calculation. Leg posture calculation for obtaining inclination angles θc and θd (specifically, for example, inclination angles θc and θd with respect to the vertical direction of each leg 2 in the coordinate system Cf). Means 28 are provided.
[0038]
The arithmetic processing unit 16 also includes an inclination angle θa of the chest 4 obtained by the chest inclination angle measuring means 25, a waist inclination angle measuring means 26 and a leg posture calculating means 28, an inclination angle θb of the waist 3 and each leg. 2 using the data of the inclination angle θc of the thigh 9 and the inclination angle θd of the crus 11, the position of the center of gravity of each rigid body corresponding to the rigid link model described later (specifically, the body coordinates The position of the center of gravity of each part corresponding to the rigid body in the system Cp) and the data of the position of the center of gravity of each part corresponding to the rigid body are used to calculate the center of gravity of the entire human 1 in the body coordinate system Cp. The body centroid position calculating means 30 for obtaining the position, the position data of the centroid G0 of the entire human 1 (see FIG. 1; hereinafter referred to as the body centroid G0) and the size of each leg 2 by the leg posture calculating means 28. Thigh 9 and lower thigh 11 Using the data of the respective inclination angles θc and θd, the position of each leg 2 as the specific part of each leg 2 in the present embodiment with respect to the body center of gravity G0 of the leg 2 (specifically, in the above equation (5)) Ankle position calculation means 31 for obtaining ΔXf, ΔZf, ΔXr, ΔZr), body gravity center position data by the body gravity center position calculation means 30, and acceleration a of the origin O of the body coordinate system Cp by the reference acceleration measurement means 27₀Acceleration of the body center of gravity G0 in the absolute coordinate system Cf using the data of^Tbody gravity center acceleration calculating means 32 for obtaining (ax, az) (see FIG. 1).
[0039]
Further, the arithmetic processing unit 16 uses the data of the center of gravity of the corresponding part of the human body 1 (specifically, the position of the center of gravity of the corresponding part of the rigid body related to the leg 2) and the reference acceleration measurement. Acceleration a of origin O of body coordinate system Cp by means 27₀And each leg body acceleration calculating means 33 for obtaining accelerations (translational accelerations) of the center of gravity of the thigh 9 and the lower leg 11 of each leg 2 in the absolute coordinate system Cf using the data of Using the data of the inclination angles θc and θd of the thigh 9 and the crus 11 of each leg 2 by the calculation means 28, the thigh 9 and the crus of each leg 2 and 2 in the absolute coordinate system Cf The leg angular acceleration calculation means 34 for obtaining the angular acceleration of the part 11 and the inclination angle θc of the thigh 9 obtained by the leg posture calculation means 28 and the knee joint 10 measured by the knee joint angle sensor 23. The floor reaction force action point estimation means 35 for estimating the position of the floor reaction force action point of each leg 2 that is grounded using the data of the bending angle Δθd.
[0040]
The arithmetic processing unit 16 also includes data on the acceleration a of the body center of gravity by the body center-of-gravity acceleration calculating unit 32, data on the position of the ankle portion 12 of each leg 2 with respect to the body center of gravity by the ankle position calculating unit 31, and the legs. The floor reaction force estimation means 36 for obtaining an estimated value of the floor reaction force acting on each leg 2 using the data of the determination result of the motion state of the leg 2 by the motion determination means 24, and the estimated value of the floor reaction force Data, acceleration data of the center of gravity of the thigh 9 and the lower leg 11 of each leg 2 by the leg part acceleration calculation means 33, and the thigh 9 of each leg 2 by the leg angular acceleration calculation means 34 And the data of the angular acceleration of the lower leg 11, the data of the estimated position of the floor reaction force action point by the floor reaction force action point estimation means 35, and the thigh 9 and the lower part of each leg 2 by the leg posture calculation means 28. Data on the inclination angles θc and θd of the thigh 11 And a joint moment estimating means 37 for estimating a moment acting respectively on the knee joint 10 and the hip joint 8 of each leg 2 with and.
[0041]
Next, the operation of the present embodiment will be described together with more detailed processing contents of each means of the arithmetic processing unit 16 described above.
[0042]
In the present embodiment, for example, when the human 1 performs the movement of the leg 2 such as walking, when the power switch (not shown) of the arithmetic processing unit 16 is turned on with both legs 2 and 2 being landed, the arithmetic processing unit 16 is turned on. The processing according to 16 is sequentially executed as described below every predetermined cycle time, and an estimated value of the floor reaction force acting on each leg 2 is sequentially obtained.
[0043]
That is, first, the arithmetic processing device 16 executes the processing of the leg motion determining means 24. In the processing of the leg movement determining means 24, the detection data of the acceleration in the upward direction of the waist 3 by the waist vertical acceleration sensor 21 is compared with a predetermined threshold value every cycle time. When the acceleration detection value exceeds the threshold value, the both-leg support state as shown in FIG. 1 (b) starts and the single-leg support as shown in FIG. 1 (a). It is determined that the state ends. That is, when the human 1 walks from the single-leg support state to the both-leg support state, the leg 2 on the free leg side is landed so that the waist 3 in the vicinity of the hip joint 8 is almost upward. A relatively large acceleration (acceleration that cannot be generated in a normal single-leg support state) occurs. For this reason, the leg movement determination means 24 compares the detection data of the acceleration in the upward direction of the waist 3 by the waist vertical acceleration sensor 21 with a predetermined threshold as described above, thereby starting the both-leg support state and the single leg. Determine the end of the support state.
[0044]
Further, in the processing of the leg motion determining means 24, the floor reaction forces Ff and Fr acting on the legs 2 and 2 respectively obtained as described later by the floor reaction force estimating means 35 in the state where both legs are supported (FIG. 1 ( Among the estimated values of b), the floor reaction force Fr on the rear leg 2 with respect to the traveling direction of the human being =^TEstimated value of (Frx, Frz) (specifically, the absolute value of the floor reaction force Fr obtained at the previous cycle time of the arithmetic processing unit 16 = √ (Frx²+ Frz²)) Is compared with a predetermined threshold value (a positive value of approximately “0”). Then, when the absolute value of the estimated value of the floor reaction force Fr falls below the threshold value, it is determined that the both-leg support state ends and the single-leg support state starts. In the present embodiment, the initial state of the motion state of the leg 2 is a both-leg support state, and the leg reaction force related to any one of the legs 2 is reduced until the estimated value of the floor reaction force falls below the threshold value. The body movement determination means 24 determines that the movement state of the leg body 2 is a both-leg support state.
[0045]
In parallel with the processing of the leg movement determination means 24 as described above, the arithmetic processing device 16 executes processing by the chest inclination angle measurement means 25 and the waist inclination angle measurement means 26. In this case, the chest inclination angle measuring means 25 performs so-called Kalman filter processing from the longitudinal acceleration of the chest 4 and the angular velocity detection data of the chest 4 respectively inputted from the chest longitudinal acceleration sensor 15 and the chest gyro sensor 14. By the known method used, the inclination angle θa of the chest 4 in the absolute coordinate system Cf is sequentially obtained every cycle time. Similarly, in the processing of the waist inclination angle measuring means 25, Kalman filter processing is used from the longitudinal acceleration of the waist 3 and the angular velocity detection data of the waist 3 input from the waist longitudinal acceleration sensor 20 and the waist gyro sensor 19, respectively. Thus, the inclination angle θb of the waist 3 in the absolute coordinate system Cf is sequentially obtained. Here, the inclination angles θa and θb of the chest 4 and the waist 3 in the absolute coordinate system Cf are, for example, inclination angles with respect to the vertical direction (gravity direction) in the present embodiment.
[0046]
For example, by integrating the angular velocity detection data by the gyro sensors 14 and 19, it is possible to obtain the inclination angle of the chest 4 and the waist 3, but by using the Kalman filter processing as in this embodiment, The inclination angles θa and θb of the chest 4 and the waist 3 can be accurately measured.
[0047]
Next, the arithmetic processing unit 16 executes the process of the leg posture calculating unit 28 and the process of the reference acceleration measuring unit 27.
[0048]
In the processing by the leg posture calculating means 28, the inclination angles θc and θd (inclination angles with respect to the vertical direction, see FIG. 2) of the thigh 9 and the crus 11 of each leg 2 in the absolute coordinate system Cf are the cycle. It is calculated as follows for each time. That is, the inclination angle θc of the thigh 9 of each leg 2 is the current value of the detection data of the bending angle Δθc of the hip joint 8 by the hip joint angle sensor 22 attached to the leg 2, and the waist inclination angle. It is calculated by the following equation (6) from the current value of the inclination angle θb of the waist 3 obtained by the measuring means 25.
[0049]
θc = θb + Δθc (6)
[0050]
Here, the inclination angle θb of the waist 3 is a negative value when the waist 3 is inclined with respect to the vertical direction so that the upper end of the waist 3 protrudes forward of the human 1 with respect to the lower end. The bend angle Δθc of the hip joint 8 is positive when the thigh 9 is inclined with respect to the axis of the waist 3 so that the lower end of the thigh 9 protrudes forward of the human 1. Is the value of.
[0051]
Further, the inclination angle θd of the lower leg 11 of each leg 2 is the current value of the inclination angle θc of the thigh 9 obtained as described above and the knee joint angle attached to the leg 2. It is calculated by the following equation (7) from the current value of the detection data of the bending angle Δθd of the knee joint 10 by the sensor 23.
[0052]
θd = θc−Δθd (7)
[0053]
Here, the bending angle Δθd of the knee joint 10 takes a positive value when the crus part 11 is inclined to the back side of the thigh part 9 with respect to the axial center of the thigh part 9. .
[0054]
Further, in the processing of the reference acceleration measuring means 27, the acceleration a in the absolute coordinate system Cf of the origin O of the body coordinate system Cp.₀=^T(a₀x, a₀z) is obtained as follows. That is, if the current value of the longitudinal acceleration detection data of the waist 3 by the waist longitudinal acceleration sensor 20 is ap, and the current value of the acceleration detection data of the waist 3 by the waist vertical acceleration sensor 21 is aq, From these detection data ap, aq and the current value of the inclination angle θb of the waist 3 obtained by the waist inclination angle measuring means 25, the acceleration a in the absolute coordinate system Cf is given by the following equation (8).₀=^T(a₀x, a₀z) is required.
[0055]

[0056]
Next, the arithmetic processing unit 16 executes the processing of each part gravity center position calculation means 29 and uses the rigid body link model described below to determine the position of the gravity center of each rigid body equivalent part of the human 1 in the body coordinate system Cp. (Position with respect to the origin of the body coordinate system Cp) is obtained.
[0057]
As shown in FIG. 4, the rigid body link model R used in the present embodiment includes a human body 1 having rigid bodies R1 and R1 corresponding to the thighs 9 of each leg 2 and rigid bodies R2 and R2 corresponding to the crus 11. R2 is formed by connecting a rigid body R3 corresponding to the waist 3 and a rigid body R4 corresponding to a portion 38 (hereinafter referred to as an upper body portion 38) combining the chest 4, arms 7, 7 and head 6 together. It is a model expressed as a thing. In this case, the connecting portions between the rigid bodies R1 and R3 and the connecting portions between the rigid bodies R1 and R2 correspond to the hip joint 8 and the knee joint 10, respectively. The connecting portion between the rigid body R3 and the rigid body R4 is a tilting fulcrum 39 of the chest 4 with respect to the waist 3.
[0058]
In the present embodiment, the rigid body equivalent parts of the human 1 corresponding to the rigid bodies R1 to R4 of the rigid link model R (the thigh 9 and the lower leg 11, the waist 3, the upper body of each leg 2). The positions of the respective gravity centers G1, G2, G3, and G4 of the portion 38) in the corresponding rigid body portions are obtained in advance and stored in a memory (not shown) of the arithmetic processing unit 16.
[0059]
Here, the positions of the centroids G1, G2, G3, and G4 of the corresponding rigid body parts stored in the arithmetic processing unit 16 are positions in a coordinate system fixed to the rigid body equivalent parts. In this case, as data representing the position of the center of gravity G1, G2, G3, G4 of each rigid body equivalent part, for example, the distance in the axial direction of the rigid body equivalent part from the center point of the joint at one end of each rigid body equivalent part is used. It is done. Specifically, for example, as shown in FIG. 4, the position of the center of gravity G1 of each thigh 9 is a position at a distance t1 from the center of the hip joint 8 of the thigh 9 in the axial direction of the thigh 9; The position of the center of gravity G2 of each crus part 11 is represented as a position at a distance t2 from the center of the knee joint 10 of the crus part 11 to the axial direction of the crus part 11, and the values of these distances t1, t2 are It is obtained in advance and stored in the arithmetic processing unit 16. The same applies to the center of gravity of other rigid body equivalent parts, and the positions of G3 and G4.
[0060]
Strictly speaking, the position of the center of gravity G4 of the upper body part 38 is influenced by the movement of the arm bodies 7 and 7 included in the upper body part 38. Since the position is symmetrical with respect to the axis of the chest 4, the position of the center of gravity G4 of the upper body part 38 does not change so much, for example, is substantially the same as the position of the center of gravity G4 of the upper body part 38 in the upright posture state. Become.
[0061]
In the present embodiment, data representing the positions of the center of gravity G1, G2, G3, and G4 of each rigid body equivalent portion (the thigh 9 and the lower thigh 11, the waist 3, and the upper body 38 of each leg 2). In addition, data on the weight of each rigid body equivalent part and data on the size of each rigid body equivalent part (for example, data on the length of each rigid body equivalent part) are obtained in advance and stored in the arithmetic processing unit 16.
[0062]
In addition, the weight of the lower leg part 11 is a weight including the foot part 13. Further, the data stored and held in advance in the arithmetic processing unit 16 as described above may be obtained by actual measurement or the like, but it is estimated from the height and weight of the person 1 based on average human statistical data. It may be. In general, the position, weight, and size of the center of gravity G1, G2, G3, G4 of each rigid body correlate with human height and weight, and based on the correlation data, human height and weight data Therefore, it is possible to estimate the position, weight, and size of the center of gravity G1, G2, G3, and G4 of each rigid body corresponding portion with relatively high accuracy.
[0063]
The respective center-of-gravity position calculating means 29 includes the data stored in advance in the arithmetic processing unit 16 as described above, and the inclination angle θa of the chest 4 obtained by the chest inclination angle measuring means 25 and the waist inclination angle measuring means 26, respectively. (= The inclination angle of the upper body part 38) and the current value of the inclination angle θb of the waist part 3, and the thigh 9 and the lower leg part 11 of each leg 2 obtained by the leg body posture calculating means 28, respectively. From the current values of the inclination angles θc and θd, the positions of the centroids G1, G2, G3, and G4 of the rigid body corresponding parts in the body coordinate system Cp (xz coordinate in FIG. 4) having the origin O fixed to the waist 3 are obtained. Ask.
[0064]
In this case, the inclination angles θa to θd of the respective rigid body equivalent parts (the thigh 9 and the lower leg 11, the waist 3, the upper body 38 of each leg 2) are obtained as described above. From the data of the angles θa to θd and the data of the size of each rigid body equivalent portion, the position and posture of each rigid body equivalent portion in the body coordinate system Cp can be determined. Accordingly, the positions of the centroids G1, G2, G3, and G4 of the respective rigid body equivalent parts in the body coordinate system Cp are obtained.
[0065]
Specifically, referring to FIG. 4, for example, regarding the leg 2 located on the left side of FIG. 4, the inclination angle (inclination angle with respect to the z-axis direction) of the thigh 9 in the body coordinate system Cp is θc (this In this case, since θc <0 in FIG. 4), the coordinates of the position of the center of gravity G1 of the thigh 9 in the body coordinate system Cp are (t1 · sinθc, −t1 · cosθc). Since the inclination angle of the lower leg 11 in the body coordinate system Cp is θd (θd <0 in FIG. 4), the coordinates of the position of the center of gravity G2 of the lower leg 11 in the body coordinate system Cp are the thigh 9 If Lc is Lc, then (Lc · sin θc + t 2 · sin θd, −Lc · cos θc−t 2 · cos θd). The center of gravity of the thigh 9 and the lower thigh 11 of the other leg 2 and the waist 3 and the upper body 38 can be obtained in the same manner as described above.
[0066]
In this way, after the positions of the center of gravity G1, G2, G3, G4 of each rigid body equivalent part in the body coordinate system Cp are obtained by the respective part gravity center position calculating means 29, the arithmetic processing unit 16 performs the body gravity center position calculating means. 30 processes are executed, and the position of the body center of gravity G0 of the human 1 in the body coordinate system Cp using the data of the positions of the center of gravity G1, G2, G3, G4 of each rigid body equivalent and the data of the weight of each rigid body equivalent Find (xg, zg).
[0067]
Here, the position and weight of the center of gravity G3 of the waist 3 in the body coordinate system Cp are (x3, z3), m3, and the position and weight of the center of gravity G4 of the upper body 38 are (x4, z4), m4, human 1, respectively. The position and weight of the center of gravity G1 of the thigh 9 of the left leg 2 toward the front (x1L, z1L), m1L, and the position and weight of the center of gravity G2 of the lower leg 11 of the leg 2 respectively (X2L, z2L), m2L, the position and weight of the center of gravity G1 of the thigh 9 of the right leg 2 (x1R, z1R), m1R, the position of the center of gravity G2 of the lower leg 11 of the leg 2 and If the weight is (x2R, z2R), m2R, and the weight of the human 1 is M (= m1L + m2L + m1R + m2R + m3 + m4), the position (xg, zg) of the body center of gravity G0 of the human 1 in the body coordinate system Cp is obtained by the following equation (9) It is done.
[0068]

[0069]
After executing the processing of the body gravity center position calculating means 30 in this way, the arithmetic processing device 16 further executes the processing of the body gravity center acceleration calculating means 32 and the processing of the ankle position calculating means 31.
[0070]
In this case, in the process of the body center of gravity acceleration calculating means 32, first, time series data of the position (xg, zg) of the body center of gravity G0 in the body coordinate system Cp obtained by the body center of gravity position calculating means 30 for each cycle time is used. The second-order differential value of the position (xg, zg) of the body centroid G0 in the body coordinate system Cp, that is, the acceleration of the body centroid G0 with respect to the origin O of the body coordinate system Cp.^T(d²xg / dt², D²zg / dt²) Is required. And this acceleration^T(d²xg / dt², D²zg / dt²) And acceleration a in the absolute coordinate system Cf of the origin O of the body coordinate system Cp obtained by the reference acceleration measuring means 27₀=^T(a₀x, a₀z) and the acceleration a = the body center of gravity G0 in the absolute coordinate system Cf.^T(ax, az) is required.
[0071]
Further, in the processing of the ankle position calculating means 31, first, the data of the inclination angles θc and θd of the thigh 9 and the crus 11 of each leg 2 obtained by the leg posture calculating means 28 is obtained. From the current value, the current value of the data of the inclination angle θb of the waist 3 obtained by the waist inclination angle measuring means 26, and the data of the size (length) of the thigh 9 and the lower leg 11 The position of the ankle portion 12 of each leg 2 in the body coordinate system Cp is obtained by the same processing as the processing of each part gravity center position calculating means 29. Specifically, with reference to FIG. 4, regarding the leg 2 located on the left side of FIG. 4, the length of the lower leg 11 (the length from the center of the knee joint 10 to the ankle 12) is represented by Ld. Then, the coordinates (x12, z12) of the position of the ankle portion 12 in the body coordinate system Cp are (Lc · sinθc + Ld · sinθd, −Lc · cosθc−Ld · cosθd) (however, in FIG. 4, θc <0, θd). <0). The same applies to the other leg 2.
[0072]
The current value of the position (x12, z12) in the body coordinate system Cp of the ankle 12 and the position (xg, zg) of the body centroid G0 in the body coordinate system Cp obtained by the body centroid position calculating means 30 is obtained. From the above, the position vector of the ankle portion 12 of each leg 2 with respect to the body center of gravity G0^T(x12−xg, z12−zg), that is, ΔXf, ΔZf, ΔXr, ΔZr in the equation (5) is obtained.
[0073]
Next, the arithmetic processing means 16 executes the processing of the floor reaction force estimating means 36 as follows. That is, in this process, when the motion state of the leg 2 determined by the leg motion determination means 24 at the current cycle time is a single leg support state, the values of the body weight M and the gravitational acceleration g of the human 1 are obtained. (These are stored in advance in the arithmetic processing unit 16) and the acceleration a = of the body center of gravity G0 in the absolute coordinate system Cf obtained by the body center of gravity acceleration calculating means 32^TFrom the present value of (ax, az), the floor reaction force F = acting on the leg 2 that is in contact with the ground according to the above equation (2).^TAn estimated value of (Fx, Fz) is obtained. In this case, the floor reaction force acting on the non-grounded leg 2 (free leg-side leg 2) is:^T(0,0).
[0074]
Further, when the motion state of the leg 2 determined by the leg motion determination means 24 at the current cycle time is a both-leg support state, the weight M and gravity acceleration g of the human 1 and the body center-of-gravity acceleration calculation means The acceleration a of the body center of gravity G0 in the absolute coordinate system Cf obtained by^TThe current value of (ax, az) and the current value data (ΔXf, ΔZf, ΔXr in equation (5)) of the position of the ankle portion 12 of each leg 2 with respect to the body center of gravity G0 obtained by the ankle position calculation means 31 , ΔZr data) and the floor reaction force Ff for each leg 2 according to the equation (5) =^T(Ffx, Ffz), Fr =^TAn estimated value of (Frx, Frz) is obtained.
[0075]
On the other hand, the arithmetic processing unit 16 performs the leg body in parallel with the processes of the body gravity center position calculating means 30, the body gravity center acceleration / acceleration calculating means 32, the ankle position calculating means 31, and the floor reaction force estimating means 36 as described above. Processing of each part acceleration calculating means 33, each leg angular acceleration calculating means 34, and floor reaction force action point estimating means 35 is executed.
[0076]
In this case, in the processing of the leg part acceleration calculation means 33, as in the processing of the body gravity center acceleration calculation means 32, first, each part in the body coordinate system Cp obtained by the parts gravity center position calculation means 29 for each cycle time. Using the time series data of the positions of the center of gravity G1 and G2 of the thigh 9 and the crus 11, which are rigid body equivalent parts of the leg 2, the thigh 9 and the crus 11 in the body coordinate system Cp are used. Second-order differential values of the positions of the centroids G1 and G2, that is, accelerations of the centroids G1 and G2 of the thigh 9 and crus 11 in the body coordinate system Cp (acceleration with respect to the origin O of the body coordinate system Cp) Is required. Each acceleration and acceleration a in the absolute coordinate system Cf of the waist 3 by the reference acceleration measuring means 27 are used.₀=^T(a₀x, a₀By obtaining the vector sum with z), the respective accelerations of the thigh 9 and the crus 11 in the absolute coordinate system Cf (more specifically, the coordinate components of the acceleration in the absolute coordinate system Cf) are obtained.
[0077]
Further, in the processing of each leg angular acceleration calculating means 34, the respective inclination angles θc of the thigh 9 and the crus 11 of each leg 2 obtained by the leg posture calculating means 28 every cycle time. , Θd, the second-order differential values of the inclination angles θc, θd of the thigh 9 and the crus 11, that is, the angular accelerations of the thigh 9 and the crus 11, respectively. Desired.
[0078]
Further, in the processing of the ground reaction force action point estimating means 35, for the grounded leg 2, for example, from the current value of the inclination angle θc of the thigh 9 obtained by the leg posture calculating means 28, FIG. And from the ankle portion 12 of the leg 2 based on a predetermined correlation as shown in FIG. 6, the floor reaction force action point of the foot 13 of the leg 2 (from the ground contact point of the foot 13). A vector (a position vector of a floor reaction force action point with respect to the ankle portion 12; hereinafter referred to as a floor reaction force action point vector) to a point at which all acting floor reaction forces can be regarded as concentrated represents the position of the floor reaction force action point. Find as data.
[0079]
That is, according to the knowledge of the present inventor, the inclination angle θc of the thigh 9 of the leg 2 that is in contact with the ground and the bending angle Δθd of the knee joint 10 have a relatively significant correlation with the floor reaction force action point. For example, with respect to the inclination angle θc of the thigh 9, the floor reaction force action point vector, more specifically, the component of the floor reaction force action point vector in the traveling direction (X-axis direction) of the human 1 and the vertical The component of the floor reaction force action point vector in the direction (Z-axis direction) changes as shown in FIGS. 5 and 6, respectively. Here, the negative inclination angle θc of the thigh 9 is determined when the thigh 9 is inclined with respect to the axis of the waist 3 so that the leg 2 extends to the rear side of the human 1 (for example, FIG. 2). Of the right leg 2) toward the front of the human 1, and the positive inclination angle θc is such that the thigh 9 is relative to the axis of the waist 3 so that the leg 2 is on the front side of the human 1. (For example, the left leg 2 toward the front of the person 1 in FIG. 2).
[0080]
Therefore, in the present embodiment, an approximate expression using the inclination angle θc of the thigh 9 as a parameter and representing the correlation shown in FIGS. 5 and 6 is created, and this approximate expression is stored and held in advance in the arithmetic processing unit 16. ing. Then, in the processing of the floor reaction force action point estimating means 35, the current value of the inclination angle θc of the thigh 9 obtained by the leg posture calculating means 28 is substituted into the approximate expression, and the floor reaction force is calculated. The action point vector (specifically, the X-axis direction and Z-axis direction components of the floor reaction force action point vector) is obtained.
[0081]
Here, as shown in FIG. 5 and FIG. 6, in the correlation in which the inclination angle θc of the thigh 9 has a minimum value, even if the inclination angle θc of the thigh 9 is the same, the decreasing process of the inclination angle θc The value of the floor reaction force action point vector differs depending on the increase process. Therefore, in the present embodiment, when creating the above approximate expression, the transition of the correlation from when the heel of the foot 13 is landed to when the toe leaves the floor, the inclination angle θc of the thigh 9 is positive. The first phase (phase a1 in FIG. 5 and phase b1 in FIG. 6) and the inclination angle θc of the thigh 9 are negative, and the rate of change of the inclination angle θc of the thigh 9 is: , The second phase in which the inclination angular velocity of the thigh 9 is negative (phase a2 in FIG. 5 and phase b2 in FIG. 6), the inclination angle θc of the thigh 9 is negative, and the thigh 9 is divided into the third phase (phase a3 in FIG. 5 and phase b3 in FIG. 6), and each of the X-axis direction component and the Z-axis direction component of the floor reaction force action point vector Each phase is approximated by the same or different function. The approximate expression of the phase obtained by combining the first and second phases a1 and a2 in the correlation of FIG. 5 is expressed by, for example, assuming that the X-axis direction component of the floor reaction force action point vector is px,
px = x₁・ Θc⁶+ X₂・ Θc^Five+ X_Three・ Θc^Four+ X_Four・ Θc^Three+ X_Five・ Θc²+ X₆・ Θc + x₇
A sixth-order polynomial function (x₁~ X₇Is a constant value). Further, the approximate expression of the third phase a3 in the correlation of FIG.
px = x₈・ Θc^Four+ X₉・ Θc^Three+ X_Ten・ Θc²+ X₁₁・ Θc + x₁₂
A fourth-order polynomial function (x₈~ X₁₂Is a constant value).
[0082]
Further, the approximate expression of the phase of the first and second phases b1 and b2 in the correlation of FIG. 6 is expressed as follows, where the Z-axis direction component of the floor reaction force action point vector is pz:
pz = z₁・ Θc⁶+ Z₂・ Θc^Five+ Z_Three・ Θc^Four+ Z_Four・ Θc^Three+ Z_Five・ Θc²+ Z₆・ Θc + z₇
6th order polynomial function (z₁~ Z₇Is a constant value). Further, the approximate expression of the third phase b3 in the correlation of FIG.
pz = z₈・ Θc^Three+ Z₉・ Θc²+ Z_Ten・ Θc + z₁₁
A cubic polynomial function (z₈~ Z₁₁Is a constant value).
[0083]
When obtaining the floor reaction force action point vector, the sign of the inclination angle θc of the thigh 9 is identified and the thigh calculated by the first derivative of the time series data of the inclination angle θc of the thigh 9 is used. Whether the inclination angular velocity of the unit 9 is positive or negative is identified. Further, it is determined which phase is present from the positive / negative of the identified inclination angle θc and the positive / negative of the inclination angular velocity, and the current value of the inclination angle θc of the thigh 9 is added to the approximate expression of the determined phase. By substituting, the floor reaction force action point vector is calculated. Thereby, the value of the floor reaction force action point vector in the process of decreasing the inclination angle θc of the thigh 9 can be distinguished from the value of the floor reaction force action point vector in the increase process.
[0084]
Note that the position of the floor reaction force action point has a correlation with the bending angle of the knee joint 10 of the leg 2 that is in contact with the ground, and instead of the inclination angle θc of the thigh 9, the knee joint angle sensor 23 is used. The position of the floor reaction force acting point may be estimated from the measured bending angle Δθd of the knee joint 10, or both the inclination angle θc of the thigh 9 and the bending angle Δθd of the knee joint 10 are obtained. The position of the floor reaction force action point may be estimated using a map or the like.
[0085]
When the person 1 sits on the chair or stands up from the sitting state, the position of the floor reaction force action point and the bending angle Δθd of the knee joint 10 are shown in FIGS. When the correlation shown in (when the chair is standing) is established and when going up or down the stairs, between the position of the floor reaction force action point and the inclination angle θc of the thigh 9 (when going up the stairs), FIG. FIG. 10 (when going down the stairs) is established. Therefore, when sitting or standing on a chair, the position of the floor reaction force action point can be estimated from the flexion angle Δθd of the knee joint 10 based on the correlation of FIGS. 7 and 8, and when going up and down the stairs, The position of the floor reaction force acting point can be estimated from the inclination angle θc of the thigh 9 based on the correlation shown in FIGS.
[0086]
When the position of the floor reaction force acting point is estimated as described above, the arithmetic processing unit 16 next executes the processing of the joint moment estimating means 37 to act on the knee joint 10 and the hip joint 8 of each leg 2. Find the moment. This processing was obtained by the floor reaction force estimation means 36, leg part acceleration calculation means 33, leg angular acceleration calculation means 34, floor reaction force action point estimation means 35, and leg posture calculation means 28, respectively. This is done based on a so-called inverse dynamic model using the current value of the data. This inverse dynamics model uses a motion equation relating to translational motion and a motion equation relating to rotational motion of each rigid body equivalent part of the human 1 to obtain moments acting on the joints in order from the joint closer to the floor reaction force action point. In this embodiment, moments acting on the knee joint 10 and the hip joint 8 of each leg 2 are obtained in order.
[0087]
More specifically, referring to FIG. 11, first, regarding the lower leg portion 11 of each leg 2, the force (joint reaction force) acting on the ankle portion 12 at the distal end of the lower leg portion 11, the lower leg portion 11. The force acting on the knee joint 10 (joint reaction force) and the translational acceleration of the center of gravity G2 of the crus 11 are respectively expressed by component notations in the absolute coordinate system Cf.^T(F₁x, F₁z),^T(F₂x, F₂z),^T(a₂x, a₂z) and the weight of the lower leg 11 is m₂And At this time, the equation of motion regarding the translational motion of the center of gravity G2 of the lower leg 11 is expressed by the following equation (10).
[0088]

[0089]
Here, the acceleration of the center of gravity G2 of the lower leg 11^T(a₂x, a₂z) is obtained by the leg part acceleration calculating means 33. Further, the joint reaction force acting on the ankle portion 12 at the tip of the crus 11^T(F₁x, F₁z) is approximately equal to the estimated value of the floor reaction force obtained by the floor reaction force estimating means 36 for the leg 2 having the crus 11. More specifically, when the leg 2 is in contact with a single leg in a supported state, the joint reaction force^T(F₁x, F₁z) is the floor reaction force determined by the above equation (2).^T(Fx, Fz), and when the leg 2 is a leg on the free leg side,^T(F₁x, F₁z) =^T(0,0). Further, when the leg 2 is a rear leg toward the front in the traveling direction of the human 1 in the state where both legs are supported, the joint reaction force^T(F₁x, F₁z) is the floor reaction force of the formula (5).^T(Frx, Frz), and when the leg 2 is a front leg, the floor reaction force of the above formula (5)^T(Ffx, Ffz).
[0090]
Therefore, the joint reaction force acting on the knee joint 10 of each leg 2^T(F₂x, F₂z) is the acceleration of the center of gravity G2 of the lower leg 11 obtained by the leg part acceleration calculating means 33.^T(a₂x, a₂z) and the floor reaction force (=^T(F₁x, F₁z)) data and previously determined weight m of the lower leg 11₂And the value of the gravitational acceleration g are obtained by the above equation (10).
[0091]
In addition, referring to FIG.₁, The moment acting on the knee joint 10 of the lower leg 11 is expressed as M₂, The moment of inertia around the center of gravity G2 of the lower leg 11 is I_G2, The angular acceleration around the center of gravity G2 of the lower leg 11 is α₂And In correspondence with FIG. 4, the distance between the center of gravity G2 of the crus 11 and the center of the knee joint 10 is t2, and the distance between the center of gravity G2 of the crus 11 and the ankle 12 is t2 ′. Assuming that (= Ld−t2), the equation of motion related to the rotational motion around the center of gravity G2 of the lower leg 11 is expressed by the following equation (11).
[0092]

[0093]
Here, M in the formula (11)₁Is the floor reaction force action point vector obtained by the floor reaction force action point estimation means 35 for the leg 2 having the lower leg 11 related to the equation (11), and the floor reaction force estimation means for the leg 2. 36 is a moment obtained as an outer product (vector product) with the floor reaction force vector obtained by 36. Α₂Is the angular acceleration of the lower leg 11 obtained by the angular acceleration calculating means 34 for each part of the leg. Θd is the inclination angle of the lower leg 11 obtained by the leg posture calculating means 28. Also,^T(F₁x, F₁z) is an estimated value of the floor reaction force obtained by the floor reaction force estimation means 36 as described above. further,^T(F₂x, F₂z) is obtained by the equation (10). Also, the moment of inertia I_G2Is the weight of the lower leg 11 m₂Along with the data and size data, it is obtained in advance and stored in the arithmetic processing unit 16.
[0094]
Therefore, the moment M acting on the knee joint 10₂Is the data of the estimated value of the floor reaction force by the floor reaction force estimation means 36, the data of the floor reaction force action point vector by the floor reaction force action point estimation means 35, and the leg part by the angular acceleration calculation means 34 of each leg. 11 angular acceleration α₂, Data of the inclination angle θd of the crus 11 by the leg posture calculation means 28, and the joint reaction force obtained by the equation (10)^T(F₂x, F₂z) data and the moment of inertia I of the lower leg 11 obtained in advance_G2, Size (Ld), and data of the position (t2) of the center of gravity G2 are obtained by the above equation (11).
[0095]
The joint moment estimating means 37 is a moment M acting on the knee joint 10 of the crus 11 as described above.₂Is obtained, and a moment acting on the hip joint 8 portion of the thigh 9 is obtained by a process similar to the calculation process. The basic idea of this process is that the moment M of the knee joint 10₂Therefore, although the detailed illustration and description are omitted, the outline is as follows.
[0096]
That is, first, the hip joint 8 of the thigh 9 according to the following equation (12) (the same formula as the equation (10)) based on the equation of motion related to the translational motion of the center of gravity G1 of the thigh 9 (see FIG. 4). Joint reaction force acting on the part of^T(F_Threex, F_Threez) is required.
[0097]
^T(F_Threex, F_Threez) =^T(F₂x−m₁・ A₁x, F₂z−m₁・ A₁z−m₁・ G) …… (12)
[0098]
here,^T(F₂x, F₂z) is the joint reaction force of the knee joint 10 previously obtained by the equation (10). Also,^T(a₁x, a₁z) is an acceleration (translational acceleration) in the absolute coordinate system Cf of the center of gravity G1 of the thigh 9 obtained by the leg part acceleration calculation means 33. M₁Is the weight of the thigh 9 obtained in advance, and g is the acceleration of gravity.
[0099]
Next, it acts on the portion of the hip joint 8 of the thigh 9 by the following equation (13) (an equation having the same form as the equation (11)) based on the equation of motion related to the rotational motion around the center of gravity G1 of the thigh 9. Moment M_ThreeIs required.
[0100]

[0101]
Here, M2 is the moment of the knee joint 10 obtained by the equation (11),^T(F₂x, F₂z) is the joint reaction force of the knee joint 10 obtained by the above equation (10),^T(F_Threex, F_Threez) is the joint reaction force of the hip joint 8 obtained by the equation (12), I_G1Is the moment of inertia around the center of gravity G1 of the thigh 9 determined in advance, α₁Is the angular acceleration of the thigh 9 obtained by the angular acceleration calculation means 34 for each leg, and θc is the inclination angle of the thigh 9 obtained by the leg posture calculation means 28. T1 is the distance from the center of the hip joint 8 to the center of gravity G1 of the thigh 9 (see FIG. 4), and t1 ′ is the distance from the center of the knee joint 10 to the center of gravity G1 of the thigh 9 (in FIG. 4). Lc−t1), which are determined from the position of the center of gravity G1 obtained in advance and the size (length) of the thigh 9.
[0102]
The processing described above is sequentially executed at every cycle time of the arithmetic processing unit 16, and the floor reaction force acting on each leg 2 and the moment acting on the knee joint 10 and the hip joint 8 of each leg 2 are sequentially real-time. Estimated by
[0103]
Although detailed description in this specification is omitted, the estimated moment values of the knee joint 10 and the hip joint 8 obtained are, for example, devices that assist the walking of the human 1 (the auxiliary torque applied to the knee joint 10 and the hip joint 8). Used in the control of an apparatus including an electric motor or the like that can be applied.
[0104]
The state of the temporal change of the estimated value of the floor reaction force (specifically, the absolute value of the estimated value of the floor reaction force) obtained by the processing of the arithmetic processing unit 16 described above is illustrated by solid lines in FIGS. Further, FIG. 15 shows a solid line in FIG. 15 as an example of the temporal change in the estimated moment values of the knee joint 10 and the hip joint 8 obtained by the processing of the arithmetic processing unit 16. Here, FIGS. 12 and 15 are examples when the human 1 walks on the flat ground at a substantially constant speed, FIG. 13 is an example when the human 1 goes up the stairs, and FIG. 14 shows the human 1 sitting on the chair. It is an example when standing up from a state. In this case, in FIGS. 12 to 14, a comparative example (corresponding to a true value of the floor reaction force) in which the floor reaction force is actually measured using a force meter or the like is written together with a virtual line. Further, in FIG. 15, a comparative example (corresponding to the true value of the moments of the knee joint 10 and the hip joint 8) obtained by actually measuring the moments of the knee joint 10 and the hip joint 8 using a torque meter or the like is shown together with a virtual line.
[0105]
As is apparent with reference to FIGS. 12 to 14, according to the present embodiment, it is possible to obtain an accurate estimated value of the floor reaction force regardless of the motion form and the motion environment of the leg 2. I understand. Further, in the present embodiment, by using the estimated value of the floor reaction force and the estimated position of the floor reaction force action point, the moments of the knee joint 10 and the hip joint 8 are estimated with relatively high accuracy as shown in FIG. be able to.
[0106]
As described above, according to the present embodiment, the hip joint 8 and the hip joint are not attached to the leg body 2 without interfering with the walking of the human 1 or wearing a sensor that imposes a burden on the motion of the leg body 2. The floor acting on each leg 2 using relatively small and light sensors such as the angle sensors 22 and 23 attached to the body 8 and the gyro sensors 14 and 19 and the acceleration sensors 15, 20 and 21 attached to the body 5. The positions of the reaction force and the floor reaction force acting point can be estimated, and the moment acting on the hip joint 8 and the knee joint 10 of each leg 2 can be easily estimated in real time from this estimated value. Moreover, the estimation can be performed with relatively high accuracy regardless of the motion form or the motion environment of the leg 2 such as walking on a flat ground or walking on stairs.
[0107]
In the embodiment described above, the case where the present invention is applied to the human 1 has been described as an example. However, the present invention can also be applied to a biped walking robot as a biped walking moving body. Here, in biped robots, the waist and chest may have an integral structure, but in this case, a gyro sensor or a longitudinal acceleration sensor is attached only to either the waist or the chest. It is also possible to estimate the floor reaction force and the leg joint moment in the same manner as in this embodiment. Further, in a biped robot, the flexion angles of the hip joint and the knee joint can be grasped by the control amount of the control device for the actuator of those joints.
[0108]
In the embodiment, the detection data of the waist vertical acceleration sensor 21 is used as it is in order to determine the motion state of the leg 2. However, instead of the detection data, for example, the reference acceleration measurement means 27 obtains the detection data. Acceleration of waist 3 in absolute coordinate system Cf₀The value of the component in the vertical direction (Z-axis direction) may be used.
[Brief description of the drawings]
FIG. 1 is a diagram for explaining a basic principle of a floor reaction force estimation method.
FIG. 2 is a diagram schematically showing a human being as a biped walking moving body and a device configuration equipped on the human being in one embodiment of the present invention.
3 is a block diagram for explaining functions of an arithmetic processing unit provided in the apparatus of FIG. 2;
4 is a diagram showing a rigid link model used for processing of the arithmetic processing unit of FIG. 3;
FIG. 5 is a diagram showing the correlation between the traveling direction component of the floor reaction force action point vector and the thigh inclination angle during normal walking.
FIG. 6 is a diagram showing a correlation between a vertical component of a floor reaction force action point vector and a tilt angle of a thigh during normal walking.
FIG. 7 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a knee joint flexion angle when sitting on a chair;
FIG. 8 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a knee joint flexion angle when the chair stands up.
FIG. 9 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and an inclination angle of a thigh when climbing stairs.
FIG. 10 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a tilt angle of a thigh when descending stairs.
11 is a diagram for explaining processing in a joint moment estimation unit of the arithmetic processing unit in FIG. 3;
FIG. 12 is a graph exemplifying a change over time of an estimated value of a floor reaction force during normal walking obtained according to an embodiment of the present invention.
FIG. 13 is a graph exemplifying a change over time of an estimated value of a floor reaction force when climbing a staircase obtained according to an embodiment of the present invention.
FIG. 14 is a graph exemplifying a change over time of an estimated value of a floor reaction force when a chair stands up obtained according to an embodiment of the present invention.
FIG. 15 is a graph exemplifying a change over time of estimated values of moments of a knee joint and a hip joint obtained according to an embodiment of the present invention.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 ... Human (biped walking mobile body), 2 ... Leg body, 8 ... Hip joint, 10 ... Knee joint, 12 ... Ankle part (specific part), 13 ... Foot part, 22 ... Hip joint angle sensor, 23 ... Knee joint Angle sensor.

Claims (3)

A method for sequentially estimating the position of the floor reaction force action point for each leg of a biped walking moving body,
Measuring at least one of the inclination angle of the thigh of each leg and the bending angle of the knee joint as a measurement target angle, and sequentially measuring this measurement target angle during the movement of the bipedal walking moving body,
The position vector is sequentially estimated from the measured value of the measurement target angle based on a predetermined correlation established between the position vector of the floor reaction force acting point with respect to the ankle portion of each leg and the measurement target angle. To estimate the floor reaction force action point of a biped walking moving body. An approximate expression using the measurement target angle as a parameter representing the correlation is created and stored, and the position vector is calculated by substituting the measurement value of the measurement target angle into the approximate expression,
In creating the approximate expression, the transition of the correlation from when the foot of the foot of each leg is landed to when the toe leaves is divided into several phases, and each phase is the same or different function The floor reaction force action point estimation method for a biped walking mobile body according to claim 1, characterized in that: In classifying the transition of the correlation from when the heel of the foot of each leg is landed to when the toe leaves the floor, according to at least the sign of the change rate of the angle of measurement. The method for estimating a floor reaction force action point of a biped walking mobile body according to claim 2, wherein phases are divided.

Images