(57)【要約】【課題】６軸力センサに依存しないZMPの算出方法及び装置を提供する。【解決手段】床面に接触する各リンクに設けられた１つの１軸以上の力センサと、床面に接触する各リンクの関節トルクを取得する関節トルク取得手段と、ZMP算出手段と、を備え、床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、前記関節トルク取得手段によって取得した関節トルクをx軸周りのトルクTx、y軸周りのトルクTyとして用い、前記１軸以上の力センサによってz方向の力Fzを取得して床反力とし、前記ZMP算出手段は、x方向の力Fx、y方向の力Fyを無視して、前記関節トルクと、前記z方向の力Fzと、前記力センサの位置と、を用いて、ZMPの位置を算出する。【選択図】図９
2017144512.tif 000002
【特許請求の範囲】
【請求項１】
床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、
前記リンクの関節トルクを取得する関節トルク取得手段によって取得した関節トルクをx軸周りのトルクTx、y軸周りのトルクTyとして用い、
前記床反力取得手段によってz方向の力Fzを取得し、
前記関節トルクと前記z方向の力Fzを必須要素とし、x方向の力Fxとy方向の力Fyを任意要素として、ZMPの位置を算出することを特徴とするZMPの算出方法。
【請求項２】
前記関節トルク取得手段は、筋張力と、筋張力を関節トルクに変換する係数と、を用いて関節トルクを取得する、請求項１に記載のZMPの算出方法。
【請求項３】
前記床反力取得手段は、１軸以上の力センサである、請求項１、２いずれか１項に記載のZMPの算出方法。
【請求項４】
前記１軸以上の力センサは、１軸力センサないし３軸力センサである、請求項３に記載のZMPの算出方法。
【請求項５】
前記ZMPの位置は、x方向の力Fx、y方向の力Fyを無視して、以下の式によって算出される、請求項３、４いずれか１項に記載のZMPの算出方法。
2017144512.tif 000014
ここで、
pm,x：ZMPのx座標、
pm,y：ZMPのy座標、
pjx：各力センサのx座標、
pjy：各力センサのy座標、
fjz：各力センサでのz方向の力、
τm,jx：各関節での張力に基づくx軸周りのトルク、
τm,jy：各関節での張力に基づくy軸周りのトルク、
j(1,…,N):床面と接触するリンク番号、
である。
【請求項６】
前記関節トルク取得手段は、関節トルクセンサである、請求項１に記載のZMPの算出方法。
【請求項７】
床面と接触するリンクに作用する床反力を取得する床反力取得手段と、
床面と接触するリンクの関節トルクを取得する関節トルク取得手段と、
ZMP算出手段と、
を備え、
床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、
前記関節トルク取得手段によって取得した関節トルクをx軸周りのトルクTx、y軸周りのトルクTyとして用い、
前記床反力取得手段によってz方向の力Fzを取得し、
前記ZMP算出手段は、前記関節トルクと前記z方向の力Fzを必須要素とし、x方向の力Fxとy方向の力Fyを任意要素として、ZMPの位置を算出することを特徴とするZMPの算出装置。
【請求項８】
前記関節トルク取得手段は、筋張力と、筋張力を関節トルクに変換する係数と、を用いて関節トルクを取得する、請求項７に記載のZMPの算出装置。
【請求項９】
前記床反力取得手段は、１軸以上の力センサである、請求項７、８いずれか１項に記載のZMPの算出装置。
【請求項１０】
前記１軸以上の力センサは、１軸力センサないし３軸力センサである、請求項９に記載のZMPの算出装置。
【請求項１１】
前記ZMP算出手段は、x方向の力Fx、y方向の力Fyを無視して、以下の式によってZMPの位置を算出する、請求項９、１０いずれか１項に記載のZMPの算出装置。
2017144512.tif 000015
ここで、
pm,x：ZMPのx座標、
pm,y：ZMPのy座標、
pjx：各力センサのx座標、
pjy：各力センサのy座標、
fjz：各力センサでのz方向の力、
τm,jx：各関節での張力に基づくx軸周りのトルク、
τm,jy：各関節での張力に基づくy軸周りのトルク、
j(1,…,N):床面と接触するリンク番号、
である。
【請求項１２】
前記関節トルク取得手段は、関節トルクセンサである、請求項７に記載のZMPの算出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ZMP（ゼロモーメントポイント）の算出方法及び装置に関するものである。
【背景技術】
【０００２】
ZMP（ゼロモーメントポイント）は、床反力によるモーメントの水平成分が0になる点として定義され、ヒューマノイド等のバランス制御に広く用いられている（非特許文献１）。
【０００３】
一般的なヒューマノイドにおいて、ZMPは、足リンクに搭載された６軸力センサの出力情報（xyzの力、xyz周りのトルク）を用いて計算される場合が多い。具体的には、床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、Tx、Ty、Fx、Fy、Fzの全てを６軸力センサによって取得する。６軸力センサを用いたZMPの算出は、ヒューマノイドの片足毎にセンサを１つ設ければよいので構成がシンプルであるという利点がある。しかしながら、６軸力センサに依存した計測方法には、以下のような欠点がある。
【０００４】
●ヒューマノイドに搭載するのに適した小型の６軸力センサは、ヒューマノイドの負荷に対して定格が低い（図７参照）。一方、大きな定格のモーメントを計測するためには、大きなセンサを用いる必要があり構成が大型化しやすい。
●一般に６軸力センサはヒューマノイドの足部に作用するような大きな衝撃を想定して設計されておらず、６軸力センサの主な取付部である足部は、衝撃負荷を受けるため、６軸力センサの故障が起こり得る。
●バランス制御のためのZMP計算を６軸力センサ（冗長でない集中型センサとして用いられる）に依存すると、センサの故障は制御において致命的である。
●６軸力センサは比較的高価であり、また、大きな定格のモーメントを計測し、また、衝撃負荷に耐え得るような仕様とすると、さらなる高コスト化を招く。
【０００５】
６軸力センサに依存しないZMPの算出方法としては、複数個の１軸力センサないし３軸力センサ用いる手法（例えば、４つの３軸力センサを用いる）も知られているが、複数個のセンサを搭載することは構造が複雑になってしまうという不具合がある。
【非特許文献１】ヒューマノイドロボット  梶田秀司  編著  株式会社オーム社
【発明の開示】
【発明が解決しようとする課題】
【０００６】
本発明は、簡単な構成でありながら、６軸力センサに依存しないZMPの算出方法及び装置を提供することを目的とするものである。
【課題を解決するための手段】
【０００７】
本発明が採用した技術手段は、
床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、
前記リンクの関節トルクを取得する関節トルク取得手段によって取得した関節トルクをx軸周りのトルクTx、y軸周りのトルクTyとして用い、
前記床反力取得手段によってz方向の力Fzを取得し、
前記関節トルクと前記z方向の力Fzを必須要素とし、x方向の力Fxとy方向の力Fyを任意要素として、ZMPの位置を算出することを特徴とするZMPの算出方法、
および、
床面と接触するリンクに作用する床反力を取得する床反力取得手段と、
床面と接触するリンクの関節トルクを取得する関節トルク取得手段と、
ZMP算出手段と、
を備え、
床面と接触するリンクに作用するx軸周りのトルクTx、y軸周りのトルクTy、x方向の力Fx、y方向の力Fy、z方向の力Fzにおいて、
前記関節トルク取得手段によって取得した関節トルクをx軸周りのトルクTx、y軸周りのトルクTyとして用い、
前記床反力取得手段によってz方向の力Fzを取得し、
前記ZMP算出手段は、前記関節トルクと前記z方向の力Fzを必須要素とし、x方向の力Fxとy方向の力Fyを任意要素として、ZMPの位置を算出することを特徴とするZMPの算出装置、である。
【０００８】
１つの態様では、前記床面と接触するリンクは、ヒューマノイドのリンク（足リンク）であるが、前記床面と接触するリンクは、二足以外の多足ロボットのリンク（足リンク）であってもよい。
後述する実施形態では、ワイヤ駆動型の筋骨格ヒューマノイドを例示するが、本発明が適用され得るロボットのタイプは限定されない。
また、本発明の内容を限定するものではないが、本明細書においては、ロボットの前後方向をx軸（正面が＋）、左右方向をy軸（左が＋）、上下方向をz軸(上方向が＋)の右手系座標系とする。
ここで、ZMPという概念がカバーする技術範囲は、ヒューマノイドロボット等のロボットの安定化制御に限定されるものではなく、広く、物体（人体やパーソナルモビリティを含む）の転倒防止に利用可能な概念である。
１つの態様では、ZMPの位置を算出する対象は、身体装着型デバイスを装着した人体であってもよく、算出されたZMPを用いて、当該身体装着型デバイスによって、人間のバランスを維持したり、転倒を防止したりする制御を行ってもよい。例えば、身体装着型デバイスは、人体のバランス制御や転倒防止を行うアシストスーツである。
【０００９】
１つの態様では、前記関節トルク取得手段は、筋張力と、筋張力を関節トルクに変換する係数と、を用いて関節トルクを取得する。
１つの典型的な態様では、前記係数は、ヤコビアン（ヤコビ転置行列）である。トルクは、a(モーメントアーム) * F(筋張力)によって算出できることは当業者によく知られており、aが姿勢（関節角度）に応じて変化し、かつ、筋本数が多い場合には、ヤコビアンを用いてトルクを計算する。なお、係数はヤコビアンに限定されるものではなく、姿勢に応じてaが変化しないような場合（径一定のプーリを使うなど）や、筋本数が１本の時は、単にモーメントアームを用いて、トルクを求めてもよい。
【００１０】
筋張力を用いて関節トルクを算出する態様においては、対象となるロボットが、筋を代用した要素（典型的にはワイヤ、）を備えていることが前提となる。このようなロボットとしては、筋骨格型ロボット、ワイヤ駆動型ロボット、空気圧駆動型ロボットを例示することができる。筋骨格型ロボットにおいては、筋アクチュエータ（モータ＋ワイヤ、または、空気圧、または、直動シリンダ)が筋に対応する要素である。ワイヤ駆動型ロボットにおいては、ワイヤが筋に対応する要素である。空気圧駆動型ロボットにおいては、空気圧アクチュエータ(もしくは，空気圧アクチュエータに取り付けたワイヤ)が筋に対応する要素である。
また、上記の身体装着型デバイスを装着した人体の態様では、例えば、身体装着型デバイスを構成するワイヤ駆動機構には、関節トルク（例えば、足首関節トルク）を算出できるように張設されたワイヤが含まれており、当該ワイヤの張力を計測することで関節トルクを算出する。
【００１１】
１つの態様では、前記床反力取得手段は、１軸以上の力センサである。
各リンクについて、１個の１軸以上の力センサを設ければよい。
１つの態様では、前記床反力取得手段は、１軸力センサないし３軸力センサである。
床反力取得手段として、３軸力以上の力センサを用いる場合には、当該力センサによって、x方向の力Fx、y方向の力Fyが取得可能であり、この場合、ZMPの算出において、x方向の力Fx、y方向の力Fyを用いてもよく（この場合、後述する式(2)、(3)を用いることができる）、あるいは、次に述べるように、x方向の力Fx、y方向の力Fy（取得可能であるものの）を用いなくてもよい。
また、上記の身体装着型デバイスを装着した人体の態様では、例えば、人体の足部（靴）に埋め込んだ一軸力センサから垂直反力を計測してもよい。
【００１２】
１つの態様では、前記ZMPの位置は、x方向の力Fx、y方向の力Fyを無視して、以下の式によって算出される。
2017144512.tif 000003
ここで、
pm,x：ZMPのx座標、
pm,y：ZMPのy座標、
pjx：各力センサのx座標、
pjy：各力センサのy座標、
fjz：各力センサでのz方向の力、
τm,jx：各関節での張力に基づくx軸周りのトルク、
τm,jy：各関節での張力に基づくy軸周りのトルク、
j(1,…,N):床面と接触するリンク番号、
である。
【００１３】
すなわち、関節トルク取得手段で取得した各リンクの各関節トルクτm,jx、τm,jy、各１軸以上の力センサで取得したfjz、各１軸以上の力センサの位置pjx、pjy、を用いてZMPの位置pm,x、pm,y、を算出する。
１軸以上の力センサの位置pjx、pjyとしては、例えば、ヒューマノイド(N=2)において、直立姿勢における両足の中心位置を原点として、そこからの距離を測定して用いてもよい。例えば、その値を初期値として記憶部に記憶しておき、直立状態で、足の移動が無い場合には、初期値をそのまま用い、踏み出すなど、足が移動した場合には、移動後の位置を計算して（各関節角度やリンク長を用いて位置計算が可能であることが当業者に理解される）初期値を更新して用いてもよい。
１軸以上の力センサの高さ位置pjzは、床面になるべく近接した位置が望ましい。１軸以上の力センサが床面に近いほど、x方向の力Fx、y方向の力Fyを無視することの影響が少ないと考えられる。
【００１４】
１つの態様では、力センサの位置情報を任意情報として用い、上記式において、pjx=0,pjy=0として、計算した値pm,x、pm,yのいずれか一方あるいは両方を用いて制御を行ってもよい。
例えば、後述する実施形態では、前後方向のpm,xについては、τm,jyが支配的であり、比較的有効な値が得られると考えられる。用いる用途をアシストスーツや簡単な前後方向の安定化制御などで考えると、前後方向のpm,xのみを用いた制御の可能性もある。
【００１５】
１つの態様では、前記関節トルク取得手段は、関節トルクセンサである。
関節トルクセンサとしては、例えば、(ひずみゲージ型)トルクセンサ、静電容量型トルクセンサ、磁歪式トルクセンサが知られており、適宜、市販されているセンサ、あるいは、これらの原理に基づいて独自に開発したセンサを用いることができる。
ワイヤ駆動方式ではなく、関節を直接モータで駆動させる方式のロボットにおいては、関節トルクセンサを用いて関節トルクを取得する。
【００１６】
床反力取得手段は、１軸以上の力センサに限定されるものではなく、例えば、
床面に接触する第１節（足部）、第１関節を介して第１節に接続される第２節（脛部）、第２関節を介して第２節に接続する第３節（大腿部）とした時に、第１関節（足首関節）、第２関節（膝関節）、第３関節（股関節）の関節トルクと関節角度を用いて、床反力を取得してもよい。あるいは、フォースプレートが備わった環境において、フォースプレートから床反力を取得してもよい。
【発明の効果】
【００１７】
本発明によれば、これまで６軸力センサのTx、Ty、Fx、Fy、Fzを用いて求めていたZMP算出に必要な値を、別の手段（関節トルク取得手段、床反力取得手段）により求めることができる。
Tx、Tyは床面に接触するリンクに作用するモーメント負荷であり、当該リンクの関節トルクを求めることで得ることができる。関節トルク取得手段には、筋張力（ワイヤ張力）を用いる手法や関節トルクセンサがある。
Fzは垂直反力であり、１軸以上の力センサの、Fz成分を用いることができる。
【００１８】
したがって、本発明によって、６軸力センサに依存しなZMPの算出が可能となり、
耐衝撃性能、大定格が求められる力センサとして、比較的安価かつコンパクトな１軸力センサ（６軸力センサよりも定格が大きい）を用いることができる。
１軸力センサを用いた場合には、x方向の力Fx、y方向の力Fyを無視して、ZMPの位置の算出を行うが、このZMPの位置を用いて安定化制御が可能である。
モーメント負荷計測に筋張力（ワイヤ張力）を用いる態様では、ワイヤ張力の計測は比較的容易であるため、コンパクトな構成で大モーメント負荷の計測が可能となる。
また、センサの冗長化が可能となり、故障信頼性が向上する。
【図面の簡単な説明】
【００１９】
【図１】本実施形態に係る筋骨格ヒューマノイドの上半身を示す図である。
【図２】本実施形態に係る筋骨格ヒューマノイドの背骨構造を示す図である。
【図３】本実施形態に係る筋骨格ヒューマノイドの脚部の筋配置を示す図である。
【図４】本実施形態に係る筋骨格ヒューマノイドの脚部の関節配置を示す図である。
【図５】本実施形態に係る筋骨格ヒューマノイドの足首の筋配置を示す図である。
【図６】ZMP計算の幾何学構成を示す図である。
【図７】本実施形態に係る筋張力から取得したトルクと６軸力センサから取得したトルクとを対比して示す図である。
【図８】本実施形態に係るZMPを導出するための足首の幾何学条件を示す。
【図９】本実施形態に係るZMP算出のブロック図である。
【図１０】本実施形態にしたがって算出したZMPと６軸力センサを用いて算出したZMPの比較を示す図である。
【図１１】背骨スタビライザを用いた制御の考え方を示す図である。
【図１２】背骨スタビライザのシーケンスを示すフローチャートである。
【図１３】本実施形態に係る筋骨格ヒューマノイドの前屈み動作を示す図である。
【図１４】前屈み動作制御の実験データを示す図である。
【図１５】本実施形態に係る筋骨格ヒューマノイドの後反り動作を示す図である。
【図１６】後反り動作制御の実験データを示す図である。
【図１７】本実施形態に係る筋骨格ヒューマノイドに対する重り負荷実験を示す図である。
【図１８】重り負荷時の制御の実験のデータを示す図である。
【発明を実施するための形態】
【００２０】
［Ａ］本実施形態に係る筋骨格ヒューマノイド
本実施形態に係る筋骨格ヒューマノイド（「腱志郎」と称する）の筋（筋ワイヤ）配置は、人体の基本動作に重要な筋を選択し、人体解剖学に基づいて人体を模倣した配置となっている。本実施形態に係るヒューマノイドの上半身の筋配置、筋名（番号、関連する関節と共に）を、それぞれ図１、表１に示す。
【表１】
2017144512.tif 000004
本実施形態に係るヒューマノイドの上半身における背骨構造を図２に示す。背骨構造は、５個の脊椎骨から構成されている。
【００２１】
本実施形態に係るヒューマノイドの脚部の筋配置、筋名（番号、関連する関節と共に）を、それぞれ図３、表２に示す。
【表２】
2017144512.tif 000005
本実施形態に係るヒューマノイドの脚部の関節配置を図４に示す。図５は本実施形態に係るヒューマノイドの足首の筋配置を示し、足首は、６つの筋（番号：7,8,9,23,24,25）と６軸力センサを備えている。
【００２２】
本実施形態に係るヒューマノイドは、６４の自由度（顔と手を除く）を有している。各関節は、筋アクチュエータ（ワイヤとモータ）によって駆動される。アクチュエータの数は、全体で１０５個（上半身に５５個、脚部に５０個）である。関節の可動範囲を、表３に示す。
【表３】
2017144512.tif 000006
なお、本実施形態に係る筋骨格ヒューマノイドは、ワイヤ駆動型の筋骨格ヒューマノイドの例示に過ぎないものであり、本発明が適用され得る対象を限定するものではない。
【００２３】
［Ｂ］ZMPの算出
ZMPの導出は、例えば、非特許文献１に記載されており、ヒューマノイドの足が床面に押し付けられた時の足に取り付けられた力センサの出力情報を用いてZMPの位置を計算することができる。図６に示す幾何条件にしたがい、点p＝[py py pz]Tに作用するモーメントは、以下のように記述することができる。
2017144512.tif 000007
ここで、pはZMPの位置である。pj(j=1,…,N)は力センサの位置であり、pjにおいて、力fj、モーメントτjが計測されているとする。ZMPの位置は、右辺のx,y成分を0とおいて、上記式をpx,pyについて解くことによって計算される。
【００２４】
すなわち、ZMPの位置px,pyは、以下のように記述できる。
2017144512.tif 000008
ここで、
px：ZMPのx座標、
py：ZMPのy座標、
pz：ZMPのz座標、
pjx：各力センサのx座標、
pjy：各力センサのy座標、
pjz：各力センサのz座標、
fjx：各力センサでのx方向の力、
fjy：各力センサでのy方向の力、
fjz：各力センサでのz方向の力、
τjx：各力センサでのx軸周りのトルク、
τjy：各力センサでのy軸周りのトルク、
j(1,…,N)：床面と接触するリンク番号、
である。
【００２５】
一般的なヒューマノイドにおいて、ZMPは、足リンクに搭載された６軸力センサの出力情報を用いて計算される。しかしながら、６軸力センサに依存した計測方法に欠点があることは、従来技術の欄で言及した通りである。以下に、６軸力センサに依存しないZMP算出、具体的には、筋張力及び床反力を用いて算出するZMP（「筋ZMP」という）について説明する。
【００２６】
［Ｃ］筋骨格ヒューマノイドにおける筋ZMP
［Ｃ−１］筋骨格ヒューマノイドにおける関節トルク
筋骨格ヒューマノイドにおける筋張力（ワイヤ張力）に基づいた関節トルクの取得について説明する。足首関節にn個の筋を備えた筋骨格ヒューマノイドにおいて、x,y,z軸周りの関節トルクベクトルT(∈R2)は、足首における筋張力ベクトルF(∈Rn)、筋力ヤコビアンG(∈Rn×2）を用いて、以下の変換式から求められる。
2017144512.tif 000009
ここで、
T：関節トルクベクトル、
F：ワイヤ張力ベクトル、
G：ヤコビアン(ワイヤ長−関節角度)、
τm,x：x軸周りの関節トルク、
τm,y：y軸周りの関節トルク、
fn：各ワイヤ張力、
である。
【００２７】
筋ヤコビアンは、筋長ベクトルＬと関節角度ベクトルθを関連付けるヤコビアン行列であり、筋ヤコビアンは、
2017144512.tif 000010
から求められる。
【００２８】
図５に示すように、本実施形態に係るヒューマノイドは、足首について６つの筋（番号：7,8,9,23,24,25）を備えており、足首関節のトルクは、この６つの筋の筋張力（ワイヤ張力）から取得することができる。ワイヤ張力の取得手段としては、ロードセル等の１軸力センサを用いた張力計測機構が当業者に知られている。このような張力計測機構の具体例が、
Yuki Asano, Toyotaka Kozuki, Soichi Ookubo, Koji Kawasaki, Takuma Shirai, Kohei Kimura, Kei Okada and Masayuki Inaba: A sensor-driver integrated muscle module with high-tension measurability and flexibility for tendon-driven robots, Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference, pp.5960−5965
に記載されており、適宜参照することができる。
【００２９】
図７に、筋張力（ワイヤ張力）から求めた関節トルクと６軸力センサによって取得されたトルクを対比して示す。小型の６軸力センサは、ヒューマノイドの負荷に対して定格が低く、大きなトルクが計測できなかったことが示されている。なお、本実施形態に係るヒューマノイドは、足リンクに６軸センサを備えているが、本実施形態に係る筋ZMPの算出においては、６軸力センサは、床反力fzのみを取得するために用いられる。
【００３０】
［Ｃ−２］筋ZMPの計算
通常、ZMPの位置を算出するためには、床面と接触するリンクでのTx、Ty、Fx、Fy、Fzが必要である。本実施形態では、ZMPの位置を算出において、水平力Fx、Fyが及ぼす影響は少ないことを仮定して（リンクに設けられた力センサが床面に近い場合には、これらの影響は小さくなると考えられる。）、従来６軸力センサによって取得されていた水平力Fx、Fyを省くことを考えた。したがって、ZMPの位置を算出に用いる値はTx、Ty、Fzとなる。
【００３１】
Tx、Tyとして、６軸力センサにより取得されるトルクの代わりに、筋張力fmuscleから取得する関節トルクτm,x,τm,yを用いる。Fzとして、１軸以上の力センサ(FT sensor)により計測された床反力fzを用いる。通常のZMP計算式(2)、(3)において、fx、fyの値が微小であると仮定して、これらを無視する。式(2)、(3)を書き換えることによって、筋ZMPの位置pm,x、pm,yの算出式(5)、(6)を得る。足首の幾何条件を図８に示す。
【００３２】
［Ｃ−３］筋ZMPの計算式（一般的な場合）
2017144512.tif 000011
ここで、
pm,x：筋ZMPのx座標、
pm,y：筋ZMPのy座標、
pjx：各力センサのx座標、
pjy：各力センサのy座標、
fjz：各力センサでのz方向の力、
τm,jx：各関節での張力に基づくx軸周りのトルク、
τm,jy：各関節での張力に基づくy軸周りのトルク、
j(1,…,N)：床面と接触するリンク番号、
である。
【００３３】
［Ｄ−４］筋ZMPの計算式（二脚の具体的な場合）
二脚（N=2）の場合は、以下の式によって、筋ZMPを算出する。
2017144512.tif 000012
ここで、
pm,x：筋ZMPのx座標、
pm,y：筋ZMPのy座標、
pLx：左足力センサのx座標、
pLy：左足センサのy座標、
pRx：右足力センサのx座標、
pRy：右足力センサのy座標、
fLz：左足力センサでのz方向の力、
fRz：右足力センサでのz方向の力、
τm,Lx：左足首関節での張力に基づくx軸周りのトルク、
τm,Ly：左足首関節での張力に基づくy軸周りのトルク、
τm,Rx：右足首関節での張力に基づくx軸周りのトルク、
τm,Ry：右足首関節での張力に基づくy軸周りのトルク、
である。
【００３４】
図９に本実施形態に係るZMPの位置の算出のブロック図を示す。ZMPの算出装置は、床面と接触する各リンクに設けられた床反力取得手段と、床面と接触する各リンクの関節トルク（床面と接触する第１リンクと、第１リンクに隣接する第２リンクと、を接続する関節のトルク）を取得する関節トルク取得手段と、ZMP算出手段と、とからなる。
【００３５】
本実施形態では、床反力取得手段は、各リンクついて１個の１軸以上の力センサ（図では「１軸力センサ」を例示）であり、当該センサによって床反力fizが取得される。各１軸以上の力センサの位置pjx,pjyは既知あるいは計算可能となっている。関節トルク取得手段は、筋張力と、筋張力を関節トルクに変換するヤコビアンと、を用いて関節トルクτm,jx，τm,jyを取得する。ZMPの算出手段は、fiz、pjx,pjy、τm,jx，τm,jyを用いて、筋ZMPの位置pm,x，pm,yを算出する。
【００３６】
このようなZMPの算出装置は、床反力取得手段としての１軸以上の力センサ、筋張力取得手段としての１軸力センサ（ロードセル等）、関節トルクの計算やZMP算出手段を構成するコンピュータ（少なくとも記憶部、演算部を備える）から構成することができる。
【００３７】
図１０に、本実施形態に係る筋骨格ヒューマノイドを、直立姿勢の重心軌跡が円に沿うように運動させた時の６軸力センサを用いて取得したZMPと、筋張力に基づく関節トルク及び床反力からした筋ZMPと、を対比して示す。両者の値はほぼ同じであることが確認された。なお、水平力fx,fyの影響について検証したところ、本実施形態においては、Tyの値に対してfxの値は小さく、一方、Txの値に対してfyの値は比較的大きいことがわかった。したがって、正確性においては、筋ZMPxが高く、筋ZMPyは、筋ZMPxに比べて低いと言える。
【００３８】
筋ZMPの位置pmは、安定化制御の指標として用いられ得る。１つの態様では、得られた筋ZMPを目標ZMP(転倒しない安定的な位置)に追従させる制御を行う。具体的には、筋ZMPと目標ZMPの偏差を小さくするように所定の関節（後述する実施形態では背骨関節であるが、これには限定されず、股関節や足首関節等であってもよい）を駆動することで、ヒューマノイドのバランス制御を行う。
【００３９】
［Ｅ］バランス制御のための背骨スタビライザ
［Ｅ−１］背骨スタビライザ
ヒューマノイドは、重心の床面への投影点がいわゆる支持多角形内にある時には転倒しない。図１１左図に示すように、ヒューマノイドが直立姿勢にある時には、全身の重心の床面への投影点は支持多角形内に位置している。左図の状態から、足首及び股関節のみ運動により前傾して中央図のように姿勢が変化すると、全身の重心の床面への投影点は支持多角形その外に位置することになる。背骨スタビライザは、重心の床面への投影点が支持多角形内に位置するように全身を制御する。本実施形態に係る背骨スタビライザは、図１１右図に示すように、脚の運動によって生じた全身の重心移動を、上半身の重心を移動するように背骨を制御することで補償し、全身の重心が支持多角形内に位置するようにする。すなわち、背骨スタビライザは、脚部の重心移動を、背骨の可動範囲を用いて上半身重心を移動させることで補償する。
【００４０】
図１２に、背骨スタビライザを用いた制御のフローチャートを示す。スタビライザシーケンスの開始において、ヒューマノイドを床面上で直立した状態でセットし、筋ZMPを算出して、目標筋ZMPrefとする。次いで、各筋（ワイヤ）の筋張力（ワイヤ張力）を用いて各関節トルクを推定して、関節トルクを得る。背骨スタビライザによる制御において、筋ZMPと、このサイクルにおける目標筋ZMPrefと筋ZMPとの誤差に基づくPI制御によって決定される目標背骨角度と、を計算する。ヒューマノイドは、決定された背骨角度に基づいて動作し、筋長、筋張力、床反力が更新される。制御サイクルは、8[ms]とした。さらに、背骨スタビライザは、各関節角度を用いて生成されたヒューマノイド動作を補償する。
【００４１】
［Ｅ−２］筋ZMPに基づく背骨スタビライザ
背骨スタビライザは、背骨のロールとピッチ運動によってヒューマノイドのバランスを補償するように実行される。この運動は、目標ZMPと現在の筋ZMPの誤差に基づく背骨角度のPI制御によって生成される。すなわち、背骨は以下のように制御される。
2017144512.tif 000013
ここで、e(t)は、各制御サイクルにおけるZMP誤差であり、pxref,pyrefは、目標ZMPのx要素、y要素である。目標ZMPとしては、背骨スタビライザシーケンスの初期値を用いた。
【００４２】
［Ｅ−３］前屈み制御、後反り制御、
矢状面に沿った前屈み実験、後反り実験を行った。実験において、オペレータによって決定された足首、股関節の角度が入力され、スタビライザは、ヒューマノイドのバランスを補償する。実験において、頭部、骨盤の角度は、各リンクに搭載された慣性計測装置（IMU）によって取得した。図１３は、前屈み実験の姿勢の変化を示し、図１４は実験データのプロットを示す。図１５は、後反り実験の姿勢の変化を示し、図１６は実験データのプロットを示す。本実施形態に係る筋ZMPを用いることで、図１３、図１５に示すようなヒューマノイドのバランス動作が可能となった。筋ZMPで現時点のZMPを算出し、安定な目標ZMPに対してPI制御で追従させるような制御を行った。図１４、図１６に示すように、目標ZMPであるZMPrefに対して、現時点の筋ZMPが追従するような挙動となっており、筋ZMPがバランス制御に有効であることがわかる。
【００４３】
［Ｅ−４］重りを伴うバランス制御
ヒューマノイドに重りを用いて負荷を与えて検証実験を行った。ヒューマノイドのエンドエフェクタに取り付けた袋に重りを加えて実験を行った。ヒューマノイドは、15[kgf]の重りが負荷された状態で立ち続けることができた。図１７は、実験の姿勢を示し（右図に15[kgf]の重りが負荷された状態を示す）、図１８は実験データのプロットを示す。背骨スタビライザが適切に動作することを確認した。床反力について、床反力が0[s]で477[N]、110[s]で622[N]であったことから、15[kgf](147[N])がヒューマノイドに負荷されたことを確認した。
【図１】
2017144512.tif 000016
【図２】
2017144512.tif 000017
【図３】
2017144512.tif 000018
【図４】
2017144512.tif 000019
【図５】
2017144512.tif 000020
【図６】
2017144512.tif 000021
【図７】
2017144512.tif 000022
【図８】
2017144512.tif 000023
【図９】
2017144512.tif 000024
【図１０】
2017144512.tif 000025
【図１１】
2017144512.tif 000026
【図１２】
2017144512.tif 000027
【図１３】
2017144512.tif 000028
【図１４】
2017144512.tif 000029
【図１５】
2017144512.tif 000030
【図１６】
2017144512.tif 000031
【図１７】
2017144512.tif 000032
【図１８】
2017144512.tif 000033