(57)【要約】ロボット１の上体３等の所定の部位の実姿勢の検出値又は推定値と、目標歩容の姿勢との偏差を基に、その偏差の時間的変化量としての姿勢回転偏差変化量を求め、この回転偏差変化量だけ、ロボット１が、ある回転中心回りに回転したと想定して、ロボット１の位置（例えば、接地位置）を推定する。さらに、この推定位置と加速度センサ等を用いる慣性航法的によるロボット１の推定位置との差に応じて慣性航法によるロボット１の推定位置を補正することで、推定位置の精度を高める。
2003090980.tif 000002
【特許請求の範囲】
【請求項１】
決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記ロボットの位置の推定値たる推定位置を決定する位置推定手段とを備えることを特徴とする脚式移動ロボットの自己位置推定装置。
【請求項２】
前記位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第１座標系から見たロボット位置と前記第２座標系から見たロボットの推定位置とが同一になるようにグローバル座標系から見たロボットの推定位置を決定することを特徴とする請求の範囲第１項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３】
前記所定の部位は、前記ロボットの上体であることを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項４】
前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項５】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項６】
前記姿勢回転偏差算出手段は、各瞬間の前記姿勢回転偏差変化量を逐次求める手段であり、前記位置推定手段は、その各瞬間の姿勢回転偏差変化量を用いて前記ロボットの各瞬間の推定位置を逐次決定することを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項７】
前記姿勢回転偏差算出手段は、前記ロボットの着地動作により該ロボットの脚体が着地する毎に、前回の脚体の着地から今回の脚体の着地までの期間における姿勢回転偏差変化量である着地間姿勢回転偏差変化量を求める手段であり、前記位置推定手段は、該着地間姿勢回転偏差変化量を用いて各着地毎に前記ロボットの推定位置を決定することを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項８】
前記位置推定手段が推定するロボットの位置は、該ロボットの着地動作により着地した脚体の接地位置であることを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項９】
前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、少なくともその推定された変形量を用いて前記ロボットの推定位置を決定することを特徴とする請求の範囲第１項又は第２項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１０】
決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
前記ロボットに搭載された、並進加速度を検出する加速度センサと、
前記ロボットに搭載された、慣性空間に対する角速度を検出する角速度センサと、
前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかを基に前記ロボットの所定の部位又は前記ロボットの重心の鉛直位置の幾何学的推定値としての第１の推定位置を決定する幾何学的位置推定手段と、
少なくとも前記加速度センサの検出値と前記角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロボットの全体重心の鉛直位置の慣性航法的推定値としての第２の推定位置を決定すると共に、少なくとも前記第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とする脚式移動ロボットの自己位置推定装置。
【請求項１１】
前記所定の部位は、前記ロボットの上体であることを特徴とする請求の範囲第１０項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１２】
前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定した変形量を用いて前記第１の推定位置を決定することを特徴とする請求の範囲第１０項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１３】
前記慣性航法的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することを特徴とする請求の範囲第１０項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１４】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることを特徴とする請求の範囲第１０項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１５】
決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
前記ロボットに搭載された、並進加速度を検出する加速度センサと、
前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の位置の推定値たる第１の推定位置を決定する幾何学的位置推定手段と、
少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は前記ロボットの全体重心の位置の推定値としての第２の推定位置を算出すると共に、少なくとも第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とする脚式移動ロボットの自己位置推定装置。
【請求項１６】
前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちのの少なくともいずれかから把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置と前記第２座標系から見た前記所定の部位又は全体重心の第１の推定位置とが同一になるようにグローバル座標系から見た前記所定の部位又は全体重心の第１の推定位置を決定することを特徴とする請求の範囲第１５項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１７】
前記所定の部位は、前記ロボットの上体であることを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１８】
前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項１９】
前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第１の推定位置を決定することを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２０】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２１】
前記慣性航法的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２２】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることを特徴とする請求の範囲第１５項又は第１６項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２３】
決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
前記ロボットに作用する床反力を検出する床反力検出手段と、
前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の推定位置としての第１の推定位置を決定する幾何学的位置推定手段と、
少なくとも前記床反力検出手段の検出値と前記実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記ロボットの全体重心の推定位置としての第２の推定位置を算出すると共に、少なくとも第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する動力学的位置推定手段とを備えることを特徴とする脚式移動ロボットの自己位置推定装置。
【請求項２４】
前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置と前記第２座標系から見た前記所定の部位又は全体重心の第１の推定位置とが同一になるようにグローバル座標系から見た前記所定の部位又は全体重心の第１の推定位置を決定することを特徴とする請求の範囲第２３項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２５】
前記所定の部位は、前記ロボットの上体であることを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２６】
前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２７】
前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第１の推定位置を決定することを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２８】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項２９】
前記動力学的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３０】
前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記動力学的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることを特徴とする請求の範囲第２３項又は第２４項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３１】
少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物に対するロボットの相対位置関係情報を基に、前記所定の部位の実姿勢の検出値又は推定値と、前記ロボットの推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求の範囲第１項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３２】
少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求の範囲第１０項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３３】
少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置と前記所定の部位の実姿勢の検出値又は推定値とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求の範囲第１５項又は第２３項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３４】
少なくとも、前記位置推定手段により推定された前記ロボットの位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求の範囲第１項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３５】
少なくとも、前記第２の推定位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求の範囲第１０項、第１５項および第２３項のいずれか１項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３６】
少なくとも前記ロボットの推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求の範囲第１項に記載の脚式移動ロボットの自己位置推定装置。
【請求項３７】
少なくとも前記第２の推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求の範囲第１０項、第１５項および第２３項のいずれか１項に記載の脚式移動ロボットの自己位置推定装置。
【発明の詳細な説明】
技術分野
本発明は、脚式移動ロボットの自己位置を推定する装置に関する。
背景技術
脚式移動ロボットでは、移動中に脚体を振ることによる反力によって、足平（脚体先端部）と床の間の摩擦力が限界となって、足平と床の間で回転滑り（スピン）が生じ、ロボット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれるという問題が従来よりあった。
補足すると、上体が常に鉛直（直立）を維持して、直線歩行する目標歩容だけが生成されるとは限らない。目標歩容においても、ロボット全体または上体は旋回したり、前後左右に傾いたりする。すなわち、目標歩容においても、全体の姿勢回転（または上体等の代表部位の姿勢回転）が存在する。そこで、本明細書では、目標歩容における姿勢回転を目標姿勢回転と呼ぶ。本明細書で主に問題とする現象は、実際のロボットの全体の姿勢回転（または上体等の代表部位の姿勢回転）が、前記目標姿勢回転からずれることである。この現象を、厳密に表現するならば、「目標姿勢回転からの摂動」あるいは「姿勢回転摂動」と呼ぶべきであるが、目標姿勢回転と混同する恐れがない場合には、以降、これを「姿勢回転」と省略する。
なお、以降、特に、ロボット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれる現象をスピンと呼ぶ。
また、ロボットの走行を行う場合には、全脚体が床から離れて空中に存在する時期（空中期）において、ロボットの鉛直方向の位置も目標歩容からずれ易かった。
ロボットの目標歩容の軌道、すなわち目標経路からロボットがずれないようにするためには、自己位置・姿勢を精度良く推定することが必要である。
また、ビデオカメラ等の環境認識装置を搭載していても、地図情報にあらかじめ登録してあるランドマーク等の注視すべき対象物を画像のある所定の位置（画像の中央等）に捉えるためには、自己位置・姿勢を精度良く推定することが必要である。
また、ビデオカメラ等の環境認識装置によって、環境に設定されたグローバル座標系（ロボットの移動環境の床に対して固定された座標系）における対象物の位置を計測するためには、グローバル座標系における自己位置・姿勢を精度良く推定することが必要である。
ビデオカメラ等の環境認識装置を用いないで、自己位置・姿勢を推定する方法としては、ロケットなどの誘導に用いられている慣性航法装置が知られている。
しかし、慣性航法装置においては、加速度センサの検出値を２階積分することにより位置が推定されるので、積分誤差の蓄積が大きくなりやすい。そのため、加速度センサに極めて高い検出精度が要求される。また、慣性航法装置を用いるためには、初期状態においてロボットを静的に床（地面）に固定することによって、ロボットの位置、速度の初期値を０としてから、移動を開始する必要がある。しかし、脚式移動ロボットにおいては、直立静止時でも、姿勢を安定化するために、常に各関節が微小ながら動き続けなければならないので、完全静止状態にすることが困難であった。したがって、位置、速度の初期値が完全には０にならない状態から慣性航法演算を開始することとなり、推定誤差が大きくなりがちであった。
また、ロケットや航空機においては、衛星、地上のレーダーやビーコンなど、外部装置との相対位置関係から自己位置姿勢を修正するグローバル・ポジショニング・システム（ＧＰＳ）が用いられている。ロボットにおいて、同様のシステムを導入することも可能ではあろうが、ロボットの足平（脚先端部）の床からの高さ等をミリメートルやセンチメートルのオーダーで測定する程の精度を低コストで実現することは困難である。
本発明は、かかる背景に鑑みてなされたものであり、ロボットの自己位置を精度良く推定することができる自己位置推定装置を提供することを目的とする。特に、ロボットの運動加速度が水平方向や鉛直方向に激しく変動している状態、走行時などのように全脚体が床から離れている状態、または足底と床の間の回転滑りなどによってロボット全体に姿勢回転（またはスピン）が生じている状態においても、自己位置姿勢を精度良く推定することができる自己位置推定装置を提供することを目的とする。
また、ロボットの自己位置を精度よく推定した上で、その推定自己位置を利用して、ロボットに搭載されたビデオカメラ等の環境認識装置により対象物を環境認識装置の画像の適切な位置に捉えるように該環境認識装置の方向を制御する注視制御を適切に行うことをも可能とした自己位置推定装置を提供することを目的とする。
また、ランドマーク等あらかじめ正確な位置情報を有する対象物をビデオカメラ等の環境認識手段によって認識しつつ、それによって得た情報を基にロボットの推定自己位置の精度を高めることができる自己位置推定装置を提供することを目的とする。
さらに、推定した自己位置姿勢を利用することで、ビデオカメラ等の環境認識手段により、床面や障害物などの位置姿勢あるいは形状を精度良く認識することができる自己位置推定装置を提供することを目的とする。
発明の開示
本発明の脚式移動ロボットの自己位置推定装置の第１発明は、前記の目的を達成するために、決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記ロボットの位置の推定値たる推定位置を決定する位置推定手段とを備えることを特徴とするものである。
この第１発明では、前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の変化は、ある回転中心の回りにロボットの全体が前記姿勢回転偏差変化量で回転したことと同等であると想定される。実際、例えばロボットの足平（脚体先端部）と床との間で回転すべり（スピン）が発生した場合には、それが、上記姿勢回転偏差の変化となって現れる。そして、本発明では、上記想定の基で、ロボットの推定位置を決定するので、上記回転すべり等によるロボットの実際の位置の変化を定量的に把握して、その実際の位置の推定値たる推定位置を決定できる。その結果、精度のよいロボットの自己位置推定が可能となる。
なお、前記所定の部位の実姿勢の検出値又は推定値は、例えば傾斜計、ジャイロセンサ等のセンサを用いて検出もしくは推定したものでよいが、本発明で求めるロボットの推定位置、加速度センサの加速度検出値等を補助的に用いて適宜、補正したものであってもよい。所定の部位の実姿勢の検出値又は推定値は、それを得る手段を問わず、基本的には所定の部位の実姿勢を比較的精度よく表すものであればよい。このことは、第１発明以外の本発明においても同様である。
前記第１発明では、より具体的には、前記位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第１座標系から見たロボット位置と前記第２座標系から見たロボットの推定位置とが同一になるようにグローバル座標系から見たロボットの推定位置を決定することが好ましい（第２発明）。なお、グローバル座標系はロボットが移動する環境の床（地面）に対して固定された座標系である。
かかる第２発明によれば、前記第１座標系から見たロボット位置、すなわち、第１座標系上で、ロボットが回転すべり等を生じることなく、目標歩容の目標運動もしくは関節変位検出値もしくは関節変位目標値の通りに運動しているとした場合に該目標運動と関節変位検出値と関節変位目標値とのうちの少なくともいずれかから把握されるロボットの位置（第１座標系から見たロボットの位置）と、該第１座標系を前記回転中心回りに前記姿勢回転偏差変化量で回転させてなる第２座標系から見たロボットの推定位置とが同一になる（各座標系上での位置の各座標成分が両座標系で同じになる）ように、グローバル座標系から見たロボットの推定位置が決定される。その結果、姿勢回転偏差の変化を適切に反映させてロボットの推定位置を決定でき、精度のよい推定位置を得ることができる。補足すると、前記第１座標系は、例えばロボットの着地動作により着地する脚体の足平の接地面近傍等に該ロボットの着地毎に設定される座標系（後述の実施形態で説明する支持脚座標系）でもよいが、前記グローバル座標系と同じ座標系でもよい。
上記第１又は第２発明では、前記所定の部位は、ロボットの上体（脚体が延設される基体）であることが好適である（第３発明）。これは、ロボットの回転すべり等による影響は、ロボットの上体の姿勢の変化として現れやすいからである。
また、第１〜第３発明では、前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことが好適である（第４発明）。これは、ロボットの位置は、ヨー方向の姿勢回転偏差の変化の影響を受け易いからである。
また、前記第１〜第４発明で、前記ロボットの目標歩容が、走行等のように、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第５発明）。すなわち、空中期では、前記姿勢回転偏差の変化は、概ねロボットの重心を中心とする回転となる。空中期以外の時期、すなわち、いずれかの脚体が接地している時期では、前記姿勢回転偏差の変化の回転中心は、概ね実床反力中心点（ロボットに実際に作用する全床反力の中心点）および目標ＺＭＰのいずれかの点の付近に存在する。従って、上記のように回転中心を決定することで、その回転中心が実状に合った適切なものとなり、ひいては、ロボットの推定位置の精度を適切に確保することができる。
なお、目標歩容が、ロボットの歩行等のように空中期を持たない歩容（常時、いずれかの脚体が接地する歩容）であるときには、前記回転中心は、例えば実床反力中心点および目標ＺＭＰのいずれかの点又はその近傍に決定すればよい。
第１〜第５発明の一つの形態では、例えば前記姿勢回転偏差算出手段は、各瞬間の前記姿勢回転偏差変化量を逐次求める手段であり、前記位置推定手段は、その各瞬間の姿勢回転偏差変化量を用いて前記ロボットの各瞬間の推定位置を逐次決定する（第６発明）。これによれば、各瞬間の推定位置を逐次決定するので、ロボットの自己位置をほぼ連続的に把握することができる。なお、この第６発明では、姿勢回転偏差変化量は、姿勢回転偏差の変化速度に相当するものとなる。
あるいは、他の一つの形態では、前記姿勢回転偏差算出手段は、前記ロボットの着地動作により該ロボットの脚体が着地する毎に、前回の脚体の着地から今回の脚体の着地までの期間における姿勢回転偏差変化量である着地間姿勢回転偏差変化量を求める手段であり、前記位置推定手段は、該着地間姿勢回転偏差変化量を用いて各着地毎に前記ロボットの推定位置を決定する（第７発明）。
これによれば、ロボットの位置の安定性が高いものとなるロボットの着地毎にロボットの推定位置を決定するので、その推定位置の信頼性を高めることできる。
また、第１〜第７発明では、前記位置推定手段が推定するロボットの位置は、該ロボットの着地動作により着地した脚体の接地位置であることが好適である（第８発明）。ロボットの脚体の接地位置は、ロボットの上体等の部位に比べて微小変動を生じたりし難いため、安定性が高い。従って、その接地位置をロボットの位置として推定することで、信頼性の高い推定位置を得ることができる。
さらに、前記第１〜第８発明では、前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、少なくともその推定された変形量を用いて前記ロボットの推定位置を決定することが好適である（第９発明）。これによれば、ロボットに作用する床反力によるロボットの足平等の変形量を考慮してロボットの推定位置を決定するので、該推定位置の精度を高めることができる。
次に、本発明の自己位置推定装置の第１０発明は、決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、前記ロボットに搭載された、並進加速度を検出する加速度センサと、前記ロボットに搭載された、慣性空間に対する角速度を検出する角速度センサと、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかを基に前記ロボットの所定の部位又は前記ロボットの重心の鉛直位置の幾何学的推定値としての第１の推定位置を決定する幾何学的位置推定手段と、少なくとも前記加速度センサの検出値と前記角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロボットの全体重心の鉛直位置の慣性航法的推定値としての第２の推定位置を決定すると共に、少なくとも前記第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とするものである。
この第１０発明では、少なくとも前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とを基に前記所定の部位又は前記ロボットの重心の鉛直位置の幾何学的推定値（キネマティクス演算等によって求められる鉛直位置）としての第１の推定位置が決定される。さらに、少なくとも加速度センサの検出値と角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロボットの全体重心の鉛直位置の慣性航法的推定値としての第２の推定位置が決定される。そして、このとき、第２の推定位置は、第１の推定位置との差を基に補正される。つまり、慣性航法とは異なる幾何学的な手法によって決定される第１の推定位置を用いて、慣性航法を基礎とする第２の推定位置が補正される。これにより、前記第２の推定位置に含まれやすいドリフト（積分誤差の蓄積）を防止しつつ、第２の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の鉛直位置の推定精度を高めることができる。
かかる第１０発明では、前記第３発明と同様、前記所定の部位は、前記ロボットの上体であることが好ましい（第１１発明）。
また、第１０又は第１１発明では、前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定した変形量を用いて前記第１の推定位置を決定することが好ましい（第１２発明）。これによれば、前記第９発明と同様に床反力によるロボットの変形量を考慮して第１の推定位置を決定するので、該第１の推定位置の精度を高めることができる。その結果、前記第２の推定位置のより適切な補正が可能となり、該第２の推定位置の精度も高めることができる。
また、第１０〜第１２発明では、前記慣性航法的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することが好ましい（第１３発明）。これにより、第２推定位置の補正を、該第２推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。
さらに、第１０〜第１３発明では、前記ロボットの目標歩容が、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記慣性航法的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることが好ましい（第１４発明）。すなわち、空中期では、ロボットが非接地となるので、ロボットの所定の部位又は全体重心の鉛直位置の幾何学的推定位置たる前記第１の推定位置の精度が低下しやすい。そこで、第１４発明では、空中期では、第２の推定位置の補正量を０にする。これにより、空中期で第２の推定位置が、信頼性の低い第１の推定位置を用いて不適切に補正されてしまうのを防止して、該第２の推定位置の長期的な精度を安定して確保することができる。
次に、本発明の自己位置推定装置の第１５発明は、決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、前記ロボットに搭載された、並進加速度を検出する加速度センサと、前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の位置の推定値たる第１の推定位置を決定する幾何学的位置推定手段と、少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は前記ロボットの全体重心の位置の推定値としての第２の推定位置を算出すると共に、少なくとも第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とするものである。
この第１５発明では、前記第１発明で説明した手法を用いて、ロボットの所定の部位又は全体重心の位置の推定値たる第１の推定位置が決定される。なお、このとき、より具体的には、ロボットの着地動作により着地した脚体の接地位置を前記第８発明で説明した如く求めた上で、その接地位置を基準として、目標歩容の目標運動、ロボットの関節の変位検出値、及び該関節の変位目標値のうちの少なくともいずれかを基に前記第１の推定位置を決定することが好適である。そして、第１５発明では、少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は全体重心の位置の推定値としての第２の推定位置が算出され、この第２の推定位置が、第１の推定位置との差を基に補正される。つまり、慣性航法を基礎とする第２の推定位置が、ロボットの回転すべり等によるロボットの位置変動を考慮して幾何学的に決定される第１の推定位置を用いて補正される。これにより、前記第２の推定位置に含まれやすいドリフト（積分誤差の蓄積）を防止しつつ、第２の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の位置の推定精度を高めることができる。
かかる第１５発明では、より具体的には、前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置と前記第２座標系から見た前記所定の部位又は全体重心の第１の推定位置とが同一になるようにグローバル座標系から見た前記所定の部位又は全体重心の第１の推定位置を決定することが好ましい（第１６発明）。これにより、第１の推定位置を決定することに関して、前記第２発明と同様の作用効果を奏することができる。
また、第１５又は第１６発明では、前記第３発明と同様に、前記所定の部位は、前記ロボットの上体であることが好ましい（第１７発明）。
さらに、第１５〜第１７発明では、前記第４発明と同様に、前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことが好ましい（第１８発明）。
また、第１５〜第１８発明では、前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第１の推定位置を決定することが好ましい（第１９発明）。これによれば、前記第９発明と同様に床反力によるロボットの変形量を考慮して第１の推定位置を決定するので、該第１の推定位置の精度を高めることができる。その結果、前記第２の推定位置のより適切な補正が可能となり、該第２の推定位置の精度も高めることができる。
また、第１５〜第１９発明では、前記ロボットの目標歩容が、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第２０発明）。これによれば、前記第５発明と同様、前記回転中心を実状に合った適切なものとして、前記第１の推定位置の精度を適切に確保することができる。
さらに、第１５〜第２０発明では、前記慣性航法的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することが好ましい（第２１発明）。これにより、前記第１３発明と同様、第２推定位置の補正を、該第２推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。
また、第１５〜第２１発明では、前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることが好ましい（第２２発明）。
これによれば、前記第１４発明と同様、空中期では、第２の推定位置の補正量を０にするので、空中期で第２の推定位置が、信頼性の低い第１の推定位置を用いて不適切に補正されてしまうのを防止して、該第２の推定位置の長期的な精度を安定して確保することができる。
次に本発明の自己位置推定装置の第２３発明は、決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、前記ロボットに作用する床反力を検出する床反力検出手段と、前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段、前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の推定位置としての第１の推定位置を決定する幾何学的位置推定手段、少なくとも前記床反力検出手段の検出値と前記実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記ロボットの全体重心の推定位置としての第２の推定位置を算出すると共に、少なくとも第１の推定位置と第２の推定位置との差を基に第２の推定位置を補正する動力学的位置推定手段とを備えることを特徴とするものである。
この第２３発明では、前記第１の推定位置は、前記第１５発明と全く同様に決定される。一方、第２の推定位置は、少なくとも前記床反力検出手段の検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記ロボットの全体重心の推定位置としての第２の推定位置が算出され、この第２の推定位置が第１の推定位置との差を基に補正される。ここで、床反力をロボットの全体質量で割った値から、重力加速度を減算したものの符号を反転させたものが、前記第１５発明でいうところの加速度検出値に相当するものとなるので、結果的には、第１５発明と同様に第２の推定位置が決定・補正されることとなる。従って、第２３発明によれば、第１５発明と同様に、前記第２の推定位置に含まれやすいドリフト（積分誤差の蓄積）を防止しつつ、第２の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の位置の推定精度を高めることができる。
かかる第２３発明では、より具体的には、前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置と前記第２座標系から見た前記所定の部位又は全体重心の第１の推定位置とが同一になるようにグローバル座標系から見た前記所定の部位又は全体重心の第１の推定位置を決定することが好ましい（第２４発明）。これにより、第１の推定位置を決定することに関して、前記第２発明と同様の作用効果を奏することができる。
また、第２３又は第２４発明では、前記第３発明と同様に、前記所定の部位は、前記ロボットの上体であることが好ましい（第２５発明）。
さらに、第２３〜第２５発明では、前記第４発明と同様に、前記姿勢回転偏差は、少なくとも前記所定の部位のヨー方向の姿勢回転偏差成分を含むことが好ましい（第２６発明）。
また、第２３〜第２６発明では、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第１の推定位置を決定することが好ましい（第２７発明）。これによれば、前記第９発明と同様に床反力によるロボットの変形量を考慮して第１の推定位置を決定するので、該第１の推定位置の精度を高めることができる。その結果、前記第２の推定位置のより適切な補正が可能となり、該第２の推定位置の精度も高めることができる。
また、第２３〜第２７発明では、前記ロボットの目標歩容が、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第２８発明）。これによれば、前記第５発明と同様、前記回転中心を実状に合った適切なものとして、前記第１の推定位置の精度を適切に確保することができる。
さらに、第２３〜第２８発明では、前記動力学的位置推定手段は、前記第１の推定位置と第２の推定位置との差を０に近づけるように前記第２の推定位置を補正することが好ましい（第２９発明）。これにより、前記第１３発明と同様、第２推定位置の補正を、該第２推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。
また、第２３〜第２９発明では、前記ロボットの目標歩容が、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記動力学的位置推定手段は、前記空中期では、前記第２の推定位置の補正量を略０にすることが好ましい（第３０発明）。
これによれば、前記第１４発明と同様、空中期では、第２の推定位置の補正量を０にするので、空中期で第２の推定位置が、信頼性の低い第１の推定位置を用いて不適切に補正されてしまうのを防止して、該第２の推定位置の長期的な精度を安定して確保することができる。
さらに、前述した第１〜第９発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物に対するロボットの相対位置関係情報を基に、前記所定の部位の実姿勢の検出値又は推定値と、前記ロボットの推定位置とのうちの少なくともいずれかを補正することが好ましい（第３１発明）。
同様に、前記第１０〜第１４発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置とのうちの少なくともいずれかを補正する手段を備えることが好ましい（第３２発明）。
同様に、前記第１５〜第２９発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置と前記所定の部位の実姿勢の検出値又は推定値とのうちの少なくともいずれかを補正する手段を備えることが好ましい（第３３発明）。
これらの第３１〜第３３発明によれば、地図上で、位置情報が明確に確定しているランドマーク等の対象物を環境認識手段によって認識し、その対象物とロボットとの相対位置関係情報を基に、前記推定位置（第１０〜第３０発明では第１もしくは第２の推定位置）、あるいは該推定位置を決定するために必要な前記所定の部位の実姿勢の検出値又は推定値（第１〜第９発明、又は第１５〜第３０発明の場合）が、対象物を認識したときに補正されるので、推定位置（第１０〜第３０発明では第２の推定位置）の精度を高めることができる。
また、前記第１〜第９発明では、少なくとも、前記位置推定手段により推定された前記ロボットの位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えることが好ましい（第３４発明）。
同様に、前記第１０〜第３０発明では、少なくとも、前記第２の推定位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えることが好ましい（第３５発明）。
これらの第３４、第３５発明によれば、前述の如く決定したロボットの自己位置たる推定位置（第１０〜第３０発明では、第２の推定位置）を用いることで、対象物の位置を精度よく推定することができる。
また、第１〜第９発明では、少なくとも前記ロボットの推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えることが好ましい（第３６発明）。
同様に、第１０〜第３０発明では、少なくとも前記第２の推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えることが好ましい（第３７発明）。
これらの第３６、第３７発明によれば、前述の如く決定したロボットの自己位置たる推定位置（第１０〜第３０発明では、第２の推定位置）を用いるので、対象物とロボットとの位置関係を精度よく認識することができる。その結果、環境認識手段の注視方向を注視すべき方向（例えば環境認識手段を構成する撮像装置の画像の中央等の所定位置に対象物が存在するような方向）に精度よく制御することができる。
発明を実施するための最良の形態
以下、添付図面を参照してこの発明の実施形態に係る脚式移動ロボットの自己位置推定装置を説明する。尚、脚式移動ロボットとしては２足移動ロボットを例にとる。
図１は、この実施形態に係る脚式移動ロボットとしての２足移動ロボットを全体的に示す概略図である。
図示の如く、２足移動ロボット（以下、ロボットという）１は上体（ロボット１の基体）３から下方に延設された左右一対の脚体（脚部リンク）２，２を備える。両脚体２，２は同一構造であり、それぞれ６個の関節を備える。その６個の関節は上体３側から順に、股（腰部）の回旋（回転）用（上体３に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌ（符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、股（腰部）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。
各脚体２の足首の２つの関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には、各脚体２の先端部を構成する足平（足部）２２Ｒ（Ｌ）が取着されると共に、両脚体２，２の最上位には、各脚体２の股の３つの関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して前記上体３が取り付けられている。上体３の内部には、詳細を後述する制御ユニット６０などが格納される。なお、図１では図示の便宜上、制御ユニット６０を上体３の外部に記載している。
上記構成の各脚体２においては、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、膝関節は関節１６Ｒ（Ｌ）から構成され、足首関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２４Ｒ（Ｌ）で連結され、膝関節と足首関節とは下腿リンク２６Ｒ（Ｌ）で連結される。
尚、上体３の上部の両側部には左右一対の腕体５，５が取り付けられると共に、上体３の上端部には頭部４が配置される。これらの腕体５，５及び頭部４は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。
上記の構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体３に対して６つの自由度を与えられている。そして、ロボット１の歩行等の移動中に、両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行うことができる。これにより、ロボット１は任意に３次元空間を移動することができる。
図１に示す如く、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下方には足平２２Ｒ（Ｌ）との間に公知の６軸力センサ５０が介装されている。該６軸力センサ５０は、各脚体２の足平２２Ｒ（Ｌ）の着地の有無、および各脚体２に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の３方向成分Ｆｘ，Ｆｙ，Ｆｚ並びにモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚの検出信号を制御ユニット６０に出力する。また、上体３には、Ｚ軸（鉛直方向（重力方向））に対する上体３の傾き（姿勢角）およびその角速度等を検出するための傾斜センサ５４が備えられ、その検出信号が該傾斜センサ５４から制御ユニット６０に出力される。この傾斜センサ５４は、図示を省略する３軸方向の加速度センサおよび３軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体３の傾きおよびその角速度を検出するために用いられると共に、ロボット１の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット１の各関節には、それを駆動するための電動モータ６４（図５参照）と、その電動モータ６４の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ）６５（図５参照）とが設けられ、該エンコーダ６５の検出信号が該エンコーダ６５から制御ユニット６０に出力される。
さらに、図１では図示を省略するが、ロボット１の適宜な位置にはジョイスティック（操作器）７３（図５参照）が設けられ、そのジョイスティック７３を操作することで、直進移動しているロボット１を旋回させるなど、ロボット１の歩容に対する要求を必要に応じて制御ユニット６０に入力できるように構成されている。
図２は本実施形態における各脚体２の先端部分（各足平２２Ｒ（Ｌ）を含む）の基本構成を概略的に示す図である。同図に示すように、各足平２２Ｒ（Ｌ）の上方には、前記６軸力センサ５０との間にばね機構７０が装備されると共に、足底（各足平２２Ｒ，Ｌの底面）にはゴムなどからなる足底弾性体７１が貼られている。これらのばね機構７０及び足底弾性体７１によりコンプライアンス機構７２が構成されている。ばね機構７０は詳細は後述するが、足平２２Ｒ（Ｌ）の上面部に取り付けられた方形状のガイド部材（図２では図示省略）と、足首関節１８Ｒ（Ｌ）（図２では足首関節２０Ｒ（Ｌ）を省略している）および６軸力センサ５０側に取り付けられ、前記ガイド部材に弾性材（ゴムやばね）を介して微動自在に収納されるピストン状部材（図２では図示省略）とから構成されている。
図２に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示している。各脚体２が床反力を受けると、コンプライアンス機構７２のばね機構７０と足底弾性体７１とがたわみ、足平２２Ｒ（Ｌ）は図中に点線で例示したような位置姿勢に移る。このコンプラインアス機構７２の構造は、例えば本出願人が先に提案した特開平５−３０５５８４号公報に詳細に説明されている如く、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。
上記コンプライアンス機構７２を含めた足平２２Ｒ（Ｌ）（以下、足平機構２２Ｒ（Ｌ）と称することがある。）のより詳細な構成を図３及び図４を参照してさらに説明する。図３は足平機構２２Ｒ（Ｌ）の側面示の断面図、図４は該足平機構２２Ｒ（Ｌ）の底面側から見た平面図である。
足平機構２２Ｒ（Ｌ）は、大略平板状の足平プレート部材１０２を骨格部材として備えている。この足平プレート部材１０２は、その前端部（つま先部）と後端部（踵部）とが若干上方に湾曲されているが、他の部分は平坦な平板状になっている。また、足平プレート部材１０２の上面部には、横断面方形状のガイド部材１０３がその軸心を上下方向に向けて固設されている。このガイド部材１０３の内部には、該ガイド部材１０３の内周面に沿うようにして略上下方向に移動可能に設けられた可動板（ピストン状部材）１０４が設けられ、該可動板１０４が足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）に６軸力センサ５０を介して連結されている。
また、可動板１０４は、その下面の周縁部がばね、ゴム等の弾性材からなる複数の弾性部材１０６（図ではばねとして記載している）を介して足平プレート部材１０２の上面部に連結されている。従って、足平プレート部材１０２は、弾性部材１０６、可動板１０４及び６軸力センサ５０を介して足首関節１８Ｒ（Ｌ）に連結されている。尚、ガイド部材１０３の内部（可動板１０４の下側の空間）は、図示を省略する穴や隙間を介して大気側に開放されており、大気中の空気がガイド部材１０３の内部に入出自在となっている。また、上記ガイド部材１０３、可動板１０４、及び弾性部材１０６は前記図２に示したばね機構７０を構成するものである。
足平プレート部材１０２の底面（下面）には、前記図２に示した足底弾性体７１としての接地部材７１が取着されている。該接地部材７１は、足平機構２２Ｒ（Ｌ）の接地状態で、該足平プレート部材１０２と床面との間に介在させる弾性部材（床面に直接的に接触する弾性部材）であり、本実施形態では、足平プレート部材１０２の接地面の四隅（足平プレート部材１０２のつま先部の両側部並びに踵部の両側部）に固着されている。
また、接地部材７１は、本実施形態では、比較的軟質のゴム材から成る軟質層１０７ａと、比較的硬質のゴム材から成る硬質層１０７ｂとを上下に重合してなる２層構造に形成され、硬質層１０７ｂが、脚体２の着床時に直接的に床面に接触する接地面部として最下面側に設けられている。
足平機構２２Ｒ（Ｌ）には、上記の構成の他、着地衝撃緩衝装置１０８が備えられている。この着地衝撃緩衝装置１０８は、足平プレート部材１０２の底面に取着された袋状部材１０９と、該袋状部材１０９の内部に対して圧縮性流体としての空気（大気中の空気）を入出させるための流通路１１０とを備えている。
袋状部材１０９は、その周囲に前記接地部材７１が存するようにして、足平プレート部材１０２の底面の大略中央部に設けられている。この袋状部材１０９は、ゴム等の弾性材により変形自在に構成されており、外力による弾性変形が生じていない自然状態では、図３に実線で示すように、上方に開口した円筒容器形状を呈する。そして、該袋状部材１０９は、その開口端部が全周にわたって足平プレート部材１０２の底面に固着され、該足平プレート部材１０２により閉蓋されている。また、袋状部材１０９は、円筒容器形状を呈する自然状態では、該袋状部材１０９の底部が前記接地部材７１よりも下方に突出するように設けられている。つまり、該袋状部材１０９の高さ（足平プレート部材１０２の下面から袋状部材１０９の底部までの距離）は、接地部材７１の厚さよりも大きいものとされている。従って、足平プレート部材１０２が接地部材７１を介して接地した状態（脚部２の着床状態）では、袋状部材１０９は、図３に仮想線で示すように、床反力により袋状部材１０９の高さ方向に圧縮される。
尚、本実施形態では、袋状部材１０９が円筒容器形状を呈する自然状態は該袋状部材１０９の膨張状態である。そして、袋状部材１０９は、弾性材により構成されているため、圧縮されたとき、自然状態の形状（円筒容器形状）への形状復元力を有する。
前記流通路１１０は、袋状部材１０９に対する空気の流入・流出を行う流入・流出手段を構成するものであり、本実施形態では、袋状部材１０９の内部と前記ガイド部材１０３の内部とを連通させるように足平プレート部材１０２に穿設された流通孔である。この場合、前述のように、ガイド部材１０３の内部は大気側に開放されているので、該流通路１１０は、袋状部材１０９の内部を大気側に連通させていることとなる。従って、袋状部材１０９の内部には、大気中の空気が流通路１１０を介して入出自在となっており、該袋状部材１０９の膨張状態（自然状態）では、該袋状部材１０９内には空気が充填され、その内部の圧力は大気圧と同等になる。また、流通路１１０は絞り通路となっており、袋状部材１０９の内部に空気が入出する際には流体抵抗を生じるようになっている。
図５は制御ユニット６０の構成を示すブロック図である。該制御ユニット６０はマイクロコンピュータにより構成されており、ＣＰＵからなる第１の演算装置９０及び第２の演算装置９２、Ａ／Ｄ変換器８０、カウンタ８６、Ｄ／Ａ変換器９６、ＲＡＭ８４、ＲＯＭ９４、並びにこれらの間のデータ授受を行うバスライン８２を備えている。この制御ユニット６０では、各脚体２の６軸力センサ５０、傾斜センサ５４（加速度センサおよびレートジャイロセンサ）、ジョイスティック７３等の出力信号はＡ／Ｄ変換器８０でデジタル値に変換された後、バスライン８２を介してＲＡＭ８４に送られる。またロボット１の各関節のエンコーダ６５（ロータリーエンコーダ）の出力は、カウンタ８６を介してＲＡＭ８４に入力される。
前記第１の演算装置９０は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ６４の回転角の指令値）を算出し、ＲＡＭ８４に送出する。また第２の演算装置９２はＲＡＭ８４から関節角変位指令と、前記エンコーダ６５の出力信号とに基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器９６とサーボアンプ６４ａとを介して各関節を駆動する電動モータ６４に出力する。
図６は、この実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図６中の「実ロボット」の部分以外の部分が制御ユニット６０が実行する処理機能（主として第１の演算装置９０及び第２の演算装置９２の機能）によって構成されるものである。なお、以下の説明では、脚体２の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。
以下説明すると、制御ユニット６０は、ロボット１の目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置２００、自己位置姿勢推定部２１４等を備えている。自己位置姿勢推定部２１４は、詳細は後述するが、本発明の特徴部分に係わる処理を実行し、ロボット１の位置・姿勢（グローバル座標系での位置・姿勢）を推定するものである。
歩容生成装置２００が出力する目標歩容は、目標上体位置姿勢軌道（上体３の目標位置及び目標姿勢の軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体５の目標姿勢の軌道）、目標全床反力中心点（目標ＺＭＰ）軌道、目標全床反力軌道から構成される。なお、脚体２や腕体５以外に上体３に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。
ここで、本発明の実施形態での用語の意味あるいは定義について補足しておく。上記歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、各部位の「姿勢」は、該部位の傾きと向きとを総称したものである。ここで、「傾き」は該部位の鉛直方向となす角度であり、「向き」は該部位の前方向を示すベクトルを水平面に投影したベクトルの向きである。例えば上体姿勢のうちの傾きは、Ｚ軸（鉛直軸）に対するロール方向（Ｘ軸回り）の上体３の傾斜角（姿勢角）と、Ｚ軸に対するピッチ方向（Ｙ軸回り）の上体３の傾斜角（姿勢角）とからなる。また、上体３の向きは、上体３の前方向を示すベクトルを水平面に投影したベクトルのヨー方向（Ｚ軸回り）の回転角で表される。なお、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。また、特に、足平２２の着地姿勢に関しては、その着地姿勢は、基本的には着地した足平２２の向きを表し、具体的には着地した足平２２のかかとからつま先に向かうベクトルを水平面に投影したベクトルの向きを表す。また、目標腕姿勢は、腕体５のすべての部位に関する上体３に対する相対的な姿勢で表される。
上体位置は、上体３の所定位置、具体的には上体３のあらかじめ定めた代表点の位置を意味する。同様に、足平位置は、各足平２２Ｒ，２２Ｌのあらかじめ定めた代表点の位置を意味する。なお、上体速度は、上体３の上記代表点の移動速度を意味し、足平速度は、各足平２２Ｒ，２２Ｌの上記代表点の移動速度を意味する。
目標上体位置姿勢等の目標歩容に関し、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット１の運動に係わる歩容を総称的に「運動」という。
各足平２２Ｒ，Ｌの床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全ての（２本の）足平２２Ｒ，２２Ｌの床反力の合力を「全床反力」と呼ぶ。ただし、以下の説明においては、各足平床反力に関してはほとんど言及しないので、断らない限り、「床反力」は「全床反力」と同義として扱う。
目標床反力は、一般的には、作用点とその点に作用する力（並進力）と力のモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に前述の目標床反力中心点を作用点にして目標床反力を表現すると、力のモーメントは、鉛直軸成分を除けば、０になる。
尚、動力学的平衡条件を満足する歩容では、目標運動軌道から算出されるＺＭＰ（目標運動軌道から算出されるロボット１の慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直軸成分を除いて０になる点）と目標全床反力中心点は一致することから、目標全床反力中心点軌道の代わりに目標ＺＭＰ軌道を与えると言っても同じことである（詳細は、例えば本出願人によるＰＣＴ公開公報ＷＯ／０２／４０２２４を参照）。
このような背景から、ＰＣＴ公開公報ＷＯ／０２／４０２２４の明細書では目標歩容を次のように定義していた。
ａ）広義の目標歩容とは、１歩ないしは複数歩の期間の目標運動軌道とその目標床反力軌道との組である。
ｂ）狭義の目標歩容とは、１歩の期間の目標運動軌道とそのＺＭＰ軌道との組である。
ｃ）一連の歩容は、いくつかの歩容がつながったものとする。
ロボット１の歩行を行う場合においては、本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法によって上体鉛直位置（上体高さ）が決定されると、床反力の並進力成分は従属的に決定されるので、目標歩容の床反力に関して明示的に設定すべき物理量としては、ＺＭＰだけで十分であった。したがって、ＰＣＴ公開公報ＷＯ／０２／４０２２４の明細書では、狭義の目標歩容としては、上記のｂ）で十分であった。それに対し、ロボット１の走行を行う場合には、床反力鉛直成分も制御上重要であるので、該床反力鉛直成分を明示的に設定することが好ましい。そこで、本願出願人が先に提案したＰＣＴ出願（ＰＣＴ／ＪＰ０２／１３５９６）等では、狭義の目標歩容として、次のｂ’）を採用した。
ｂ’）狭義の目標歩容とは、１歩の期間の目標運動軌道とそのＺＭＰ軌道と床反力鉛直成分軌道の組である。
この明細書では以降、特にことわらない限り、目標歩容は狭義の目標歩容の意味で使用する。また、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してからもう一方の脚体２が着地するまでの意味で使用する。
歩容における両脚支持期とは言うまでもなく、ロボット１がその自重を両脚体２，２で支持する期間、片脚支持期とはいずれか一方のみの脚体２でロボット１の自重を支持する期間、空中期とは両脚体２，２が床から離れている（空中に浮いている）期間を言う。
片脚支持期においてロボット１の自重を支持しない側の脚体２を「遊脚」と呼び、自重を支持する側の脚体２を「支持脚」と呼ぶ。ロボット１の歩行では、両脚支持期と片脚支持期とが交互に繰り返され、ロボット１の走行では片脚支持期と空中期とが交互に繰り返される。この場合、走行の空中期では、両脚体２，２とも、ロボット１の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体２、支持脚であった脚体２をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。
また、目標上体姿勢、目標上体位置、目標足平位置姿勢、目標腕姿勢等、目標歩容におけるロボット１の各部の位置姿勢は支持脚座標系で記述される。支持脚座標系とは、支持脚の足平２２の接地面辺りに原点を持つ床面に固定された座標系である。より詳細には、支持脚座標系は、本出願人の特許３２７３４４３号に記載されているように、支持脚の足平２２を接地面との間で滑らさないで、水平姿勢になるまで回転させた時の、該支持脚の足首関節の中心から接地面への垂直投影点を原点とし、該支持脚足平２２のつま先に向かう水平軸（足平２２の前後方向の軸）をＸ軸として、鉛直軸をＺ軸、これらのＸ軸、Ｚ軸に直交する座標軸（足平２２の左右方向の軸）をＹ軸とする座標系である。
本発明の実施形態に係る歩容生成装置２００は、２歩先までの遊脚の足平２２の着地位置姿勢、着地時刻の要求値（目標値）を入力として、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標ＺＭＰ軌道、目標床反力鉛直成分軌道、及び目標腕姿勢軌道から構成される目標歩容を生成する。このとき、これらの軌道を規定するパラメータ（これを歩容パラメータと呼ぶ）の一部は、歩容の継続性を満足するように修正される。
目標歩容の生成にはロボット１の動力学モデルが用いられる。その動力学モデルとしては、例えば、前記ＰＣＴ公開公報ＷＯ／０２／４０２２４号に記載の単純化モデルあるいは、本出願人が提案した特開２００２−３２６１７３号公報に記載の多質点モデル（フルモデル）などを用いれば良い。
また、歩容生成装置２００は、ロボット１の片方の脚体２が着地してから他方の脚体２が着地するまでの１歩分の目標歩容（前記狭義の意味での目標歩容）を単位として、その１歩分の目標歩容を順番に生成する。ここで、現在あるいはこれから生成しようとしている歩容を「今回歩容」、その次の歩容を「次回歩容」、さらにその次の歩容を「次次回歩容」と呼ぶ。また、「今回歩容」の１つ前に生成した目標歩容を「前回歩容」と呼ぶ。
歩容生成装置２００が生成する目標歩容の一部を例示的に概説すると、例えば目標足平位置姿勢軌道は、本出願人による特許３２３３４５０号に開示した有限時間整定フィルタを用いて生成される。この有限時間整定フィルタによる足平位置姿勢軌道の生成処理では、例えば足平位置軌道は、目標着地位置（着地位置の要求値）に向かって足平２２を徐々に加速しながら移動を開始し、目標着地時刻（着地時刻の要求値）までに徐々に速度を０またはほぼ０にまで減速し、該目標着地時刻に目標着地位置に到達して停止するように生成される。足平姿勢軌道についても同様である。これにより生成される目標足平位置姿勢軌道は、着地瞬間における対地速度が０またはほぼ０になるため、特にロボット１の走行を行う場合に、前記空中期からの着地時における着地衝撃を小さくできる。
また、図７に示すように人間が走行を行う場合と同様の形態でロボット１の走行を行う場合には、例えば目標床反力鉛直成分軌道および目標ＺＭＰ軌道（詳しくは支持脚座標系のＸ軸方向（支持脚足平２２の前後方向）での目標ＺＭＰ軌道）は、それぞれ図８（ａ）、図８（ｂ）に実線で示すようなパターンで設定される。なお、図７の第１番目〜第３番目の図は、それぞれ片脚支持期の開始時、中間時点、終了時におけるロボット１の両脚体２，２の運動状態を模式的に示し、第４番目及び第５番目の図は、それぞれ空中期の中間時点、空中期の終了時（次の片脚支持期の開始時）におけるロボット１の両脚体２，２の運動状態を模式的に示している。
ロボット１の走行を行う場合には、目標床反力鉛直成分軌道は、基本的には、片脚支持期では上に凸のパターンとなり、空中期では０に維持される。また、ロボット１の歩行を行う場合には、目標床反力鉛直成分軌道は、例えば図８（ａ）に二点鎖線で示すように設定される。この場合、二点鎖線のうちの上に凸の部分が両脚支持期に対応し、下に凸の部分が片脚支持期に対応する。また、目標ＺＭＰは走行、歩行のいずれであっても、基本的には、ロボット１の脚体２の接地面内（より詳しくは所謂、支持多角形内）の中央付近に設定される。
図９は、前記歩容生成装置２００の歩容生成処理、ならびに、図６に示す自己位置姿勢推定部２１４の自己位置姿勢推定処理を示すフローチャート（構造化フローチャート）である。
まずＳ０１０において時刻ｔを０に初期化するなど種々の初期化作業を行う。
次いでＳ０１２を経てＳ０１４に進み、制御周期毎のタイマ割り込みを待つ。制御周期はΔｔである。
次いでＳ０１６に進み、自己位置姿勢推定部２１４による自己位置姿勢推定を行う。Ｓ０１６における処理がこの出願に係る脚式移動ロボットの自己位置推定装置の特徴をなすが、この説明は後述することとする。
次いでＳ０１８に進み、歩容切り替わり目（前回歩容の生成が終了し、新たな今回歩容の生成を開始すべき時刻）であるか否かが判断される。そして、その判断結果がＹＥＳであるときはＳ０２０に進み、ＮＯであるときはＳ０３２に進む。なお、以下に説明するＳ０２０以降の処理は、本願出願人が先に提案したＰＣＴ公開公報ＷＯ／０２／４０２２４あるいは前記ＰＣＴ出願ＰＣＴ／ＪＰ０２／１３５９６に詳細に説明されているので、本明細書では簡略的な説明に留める。
Ｓ０２０に進むときは時刻ｔを０に初期化する。次いでＳ０２２に進み、次回歩容支持脚座標系（詳しくはその位置および向き）、次次回歩容支持脚座標系（詳しくはその位置および向き）、今回歩容周期および次回歩容周期を読み込む。
上記次回歩容支持脚座標系および次次回歩容支持脚座標系は、それぞれ、前記ジョイスティック７３の操作等によって指定される１歩目の遊脚足平２２（今回歩容の遊脚足平２２）の着地位置姿勢の要求値（目標着地位置姿勢）、２歩目の遊脚足平２２（次回歩容の遊脚足平２２）の着地位置姿勢の要求値（目標着地位置姿勢）に応じて、前記した支持脚座標系の定義に従って決定される。
また、今回歩容周期、次回歩容周期は、それぞれ、１歩目の遊脚足平２２の着地時刻の要求値（目標着地時刻）、２歩目の遊脚足平２２の着地時刻の要求値（目標着地時刻）に応じて決定される。
上記した遊脚足平２２の着地位置姿勢の要求値並びに着地時刻の要求値、あるいは支持脚座標系の位置および向き並びに歩容周期は、あらかじめ歩行スケジュールとして記憶しておいても良く、あるいはジョイスティック７３などの操縦装置からの指令（要求）とそのときまでの歩行履歴を基に決定しても良い。
次いでＳ０２４に進み、今回歩容につながる定常旋回歩容の歩容パラメータが、Ｓ０２２で決定された次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期等に基づいて決定される。主に、目標足平位置姿勢軌道を規定する足平軌道パラメータ、目標上体姿勢の基準軌道を規定する基準上体姿勢軌道パラメータ、目標腕姿勢軌道を規定する腕姿勢軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータが決定される。例えば床反力鉛直成分軌道パラメータに関して例示すると、前記図８（ａ）に示したパターンの折れ点の時刻や値が床反力鉛直成分軌道パラメータとして決定される。
ここで、前記定常旋回歩容は、その歩容を繰り返したときに歩容の境界においてロボット１の運動状態に不連続が生じないような周期的歩容を意味する（以降、「定常旋回歩容」を「定常歩容」と略す場合もある）。
定常旋回歩容の１周期分の歩容は、第１旋回歩容と第２旋回歩容とからなる。第１旋回歩容は、今回歩容の支持脚座標系に対応する支持脚足平２２を次次回歩容支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当し、第２旋回歩容は、次回歩容支持脚座標系に対応する支持脚足平２２を次次次回支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当する。この場合、次次次回歩容支持脚座標系は、第２旋回歩容の遊脚足平２２の目標着地位置姿勢に対応するものである。そして、該次次次回歩容支持脚座標系は、次次回歩容支持脚座標系（第２旋回歩容の支持脚座標系）から見た該次次次回歩容支持脚座標系の位置姿勢（位置及び向き）が、今回歩容支持脚座標系から見た次回歩容支持脚座標系（今回歩容の遊脚足平２２の着地位置姿勢）の位置姿勢（位置及び向き）に一致するように設定される。尚、定常旋回歩容に関して「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。
定常旋回歩容は、歩容生成装置２００で今回歩容の終端における発散成分や上体鉛直位置速度を決定するために暫定的に作成される仮想的な周期的歩容であり、ロボット１を実際に制御するために歩容生成装置２００からそのまま出力されるものではない。
尚、「発散」とは、上体の位置が両足部（足平）の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、２足移動ロボットの上体の位置が両足部（足平）の位置（厳密には、支持脚接地面に設定された支持脚座標系の原点からかけ離れていく具合を表す数値であり、上体３の水平方向の位置及びその速度の関数で表される。
本実施形態では、これから生成する今回歩容の後につながる定常歩容を移動要求（前記２歩先までの遊脚の足平２２の着地位置姿勢、着地時刻などの要求値）に応じて設定し、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致するように、今回歩容を生成するようにした。Ｓ０２４の詳細は、本出願人が提案した前記ＰＣＴ公開公報ＷＯ／０２／４０２２４、あるいはＰＣＴ／ＪＰ０２／１３５９６に説明されているので、これ以上の説明を省略する。
Ｓ０１０からＳ０２４までに示す処理を行って後、Ｓ０２６に進み、定常旋回歩容の初期状態（初期上体水平位置速度成分、初期上体鉛直位置速度、初期発散成分、初期上体姿勢角および角速度）を決定する。Ｓ０２６の詳細は、ＰＣＴ公開公報ＷＯ／０２／４０２２４、あるいはＰＣＴ／ＪＰ０２／１３５９６に説明しているので、ここでは、これ以上の説明を省略する。
次いで、Ｓ０２８に進み、今回歩容の歩容パラメータを決定（一部仮決定）する。この場合、決定される今回歩容の歩容パラメータは、定常旋回歩容の歩容パラメータと同様、主に、足平軌道パラメータ、基準上体姿勢軌道パラメータ、腕姿勢軌道パラメータ、目標ＺＭＰ軌道パラメータ、目標床反力鉛直成分軌道パラメータであり、それぞれのパラメータにより規定される軌道が、定常旋回歩容の軌道に連続するように決定される。ただし、これらのパラメータのうち、目標ＺＭＰ軌道パラメータは暫定的なものである。このＳ０２８の処理の詳細は、前記ＰＣＴ公開公報ＷＯ／０２／４０２２４、あるいはＰＣＴ／ＪＰ０２／１３５９６号等に説明されているので、ここではこれ以上の説明を省略する。
次いでＳ０３０に進み、今回歩容の終端発散成分が定常歩容の初期発散成分に一致するように、今回歩容の歩容パラメータを修正する。ここで修正される歩容パラメータは、目標ＺＭＰ軌道パラメータである。
Ｓ０３０において今回歩容パラメータを修正した後、あるいはＳ０１８の判断結果がＮＯである場合には、Ｓ０３２に進み、今回歩容瞬時値を決定する。この処理の詳細は、前記ＰＣＴ公開公報ＷＯ／０２／４０２２４、あるいはＰＣＴ／ＪＰ０２／１３５９６号等に説明しているので、ここでは、これ以上の説明を省略する。
次いでＳ０３４に進み、ＰＣＴ出願ＰＣＴ／ＪＰ０２／１３５９６号の実施形態のＳ０３２の処理と同様、スピン力をキャンセルするための腕振り動作を決定する。
次いでＳ０３６に進み、歩容生成用時刻ｔをΔｔだけ増やし、Ｓ０１４に戻り、以上のごとく歩容生成を続ける。
以上が、歩容生成装置２００における目標歩容生成処理、ならびに、自己位置姿勢推定部２１４の自己位置姿勢推定処理である。
図６を参照してこの実施形態に係る制御ユニット６０の制御処理をさらに説明すると、歩容生成装置２００において、上記したように目標歩容が生成される。生成された目標歩容のうち、目標上体位置姿勢（軌道）および目標腕姿勢軌道は、ロボット幾何学モデル（逆キネマティクス演算部）２０２に直接送られる。
また、目標足平位置姿勢（軌道）、目標ＺＭＰ軌道（目標全床反力中心点軌道）、および目標全床反力（軌道）（目標床反力水平成分と目標床反力鉛直成分）は、複合コンプライアンス動作決定部２０４に直接送られる一方、目標床反力分配器２０６にも送られる。目標床反力分配器２０６では、目標全床反力は各足平２２Ｒ，２２Ｌに分配され、目標各足平床反力中心点および目標各足平床反力が決定される。その決定された目標各足平床反力中心点および目標各足平床反力が複合コンプライアンス動作決定部２０４に送られる。
複合コンプライアンス動作決定部２０４では、機構変形補償付き修正目標足平位置姿勢軌道が生成され、それがロボット幾何学モデル２０２に送られる。ロボット幾何学モデル２０２は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）が入力されると、それらを満足する脚体２，２の１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出して変位コントローラ２０８に送る。変位コントローラ２０８は、ロボット幾何学モデル２０２で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
ロボット１に生じた床反力（詳しくは実各足平床反力）は６軸力センサ５０によって検出される。その検出値は前記複合コンプライアンス動作決定部２０４に送られる。また、前記図９のＳ０１６で後述する如く求められた推定上体姿勢と歩容生成装置２００が生成した目標上体姿勢との差のうちの傾き成分、すなわち実上体姿勢角偏差θｅｒｒｘ，θｅｒｒｙが姿勢安定化制御演算部２１２に送られる。なお、θｅｒｒｘはロール方向（Ｘ軸回り）の傾き成分であり、θｅｒｒｙはピッチ方向（Ｙ軸回り）の傾き成分である。この姿勢安定化制御演算部２１２で、ロボット１の上体姿勢の傾きを目標歩容の上体姿勢の傾きに復元するための目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄが算出され、この補償全床反力モーメントＭｄｍｄが複合コンプライアンス動作決定部２０４に与えられる。複合コンプライアンス動作決定部２０４は、入力値に基づいて目標足平位置姿勢を修正する。具体的には、複合コンプライアンス動作決定部２０４では、実全床反力（すべての実足平床反力の合力で、並進力およびモーメントの両者を含む）が、目標各足平床反力の合力である目標全床反力と補償全床反力モーメントＭｄｍｄとの合力に一致するように、歩容生成装置２００から与えられた目標足平位置姿勢を修正して、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすべての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、足平位置姿勢及び床反力の各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の２乗）の重み付き平均が最小になるように制御する。
補足すると、機構変形補償付き修正目標足平位置姿勢（軌道）は、複合コンプライアンス動作決定部２０４によって修正された床反力の目標値を発生させるために必要な足平の変形機構（円柱状ゴム、足底スポンジおよび衝撃吸収用の袋状のエアダンパー）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求めて、その変形量が発生するように修正した、目標足平位置姿勢（軌道）である。
以下に、第１実施形態におけるＳ０１６の自己位置姿勢推定処理について、そのフローチャートである図１０を用いて詳説する。なお、以下に説明する第１実施形態の自己位置姿勢推定処理は、ロボット１の歩行を行っている場合の処理である。
まずＳ２０００において、上体３の傾斜センサ５４に備えたジャイロセンサの検出値、すなわち上体３の角速度（３軸方向の角速度）の検出値を積分器により積分して推定上体姿勢を求める。この推定上体姿勢はグローバル座標系で記述される。なお、推定上体姿勢のうちの傾き成分に関しては、ジャイロセンサの検出値の積分誤差の蓄積（ドリフト）を抑制するために、傾斜センサ５４に備えた加速度センサによって検出される重力方向を用いてドリフト補正が行われる。具体的には、加速度センサの検出値から求められる重力方向に対する上体３の相対角度と、上記推定上体姿勢のうちの傾きとの差を求め、この差が０に収束するように、該差からＰＩ制御則等のフィードバック制御則により求めた値を、ジャイロセンサ検出値を積分するための積分器に追加的に入力することにより、補正が行われる。この補正手法は公知であるので、これ以上の説明は省略する。
次いでＳ２００２に進み、ロボット１の着地が確定したか否かを判定する。この判定においては、以下に列挙したいずれかの方法のように、目標歩容の時期から判定しても良いし、あるいは、６軸力センサ５０（床反力センサ）や加速度センサの検出値を基に判定しても良い。あるいは、それらの検出値や目標歩容の時期から総合的に判断しても良い。
ａ）片脚支持期が開始してから所定時間後の時点か否か。
ｂ）６軸力センサ５０（床反力センサ）の検出値がある所定の値となってから所定時間が経過した時点か否か。
ｃ）加速度センサの検出値がある所定の値となってから所定時間が経過した時点か否か。
ただし、Ｓ２００２の判定結果がＹＥＳになった直後に、極力、着地した足平２２が滑ったり、再び床から離れることがないように、着地瞬間から適度な時間が経過した時点を着地が確定した時点とすることが好ましい。従って、上記ａ）〜ｃ）の所定時間は、このような条件を満たすように設定することが好ましい。
Ｓ２００２の判定結果がＮＯである場合には、以上をもってＳ０１６の自己位置推定処理を終了し、前述のごとく図９のＳ０１８に進む。
Ｓ２００２の判定結果がＹＥＳである場合には、以下の処理を行う。すなわち、まずＳ２００４に進み、グローバル座標系から見た推定上体姿勢と、グローバル座標系から見た目標歩容の上体姿勢（目標上体姿勢）との差を姿勢回転偏差変化量として算出する。なお、グローバル座標系から見た目標歩容の上体姿勢とは、現在の推定支持脚座標系上で、１歩の間、上体３の姿勢傾きやスピン（目標上体姿勢からのずれ）がなくロボット１が目標歩容通りに運動したと仮定した場合のグローバル座標系から見た上体姿勢のことである。ただし、推定支持脚座標系とは、実際のロボット１の支持脚足平２２の推定位置姿勢に対応した支持脚座標系である。すなわち、推定支持脚座標系は、前記した支持脚座標系の定義によって、より具体的には、実際のロボット１の支持脚足平２２を、その推定位置姿勢から、接地面との間で滑らさないで水平まで回転させた時の、該支持脚の足首中心から接地面への垂直投影点を原点とし、該支持脚足平２２のつま先に向かう水平軸をＸ軸にとり、鉛直軸をＺ軸、これらに直交する座標軸をＹ軸にとった座標系である。第１実施形態においては、後述するごとく推定支持脚座標系は、着地が確定した時にのみ更新されるので、着地時の実際のロボットの支持脚足平２２の推定位置姿勢に対応する支持脚座標系となる。結局、第１実施形態においてはロボット１の自己位置の推定値として推定支持脚座標系の位置姿勢が推定される。
なお、Ｓ２００４において、着地確定時における推定上体姿勢と目標歩容の上体姿勢との差を姿勢回転偏差変化量として算出するようにしたが、厳密には、１歩の間の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として算出するのが良い。ただし、第１実施形態においては、以下のアルゴリズムを実行することによって、自動的に、１歩の初めにおいて推定上体姿勢と目標歩容の上体姿勢とは一致するので、推定上体姿勢と目標歩容の上体姿勢との差と、１歩の間の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差は同一となるので、いずれの差を用いても構わない。
次いでＳ２００６に進み、姿勢回転中心を決定する。具体的には、前回歩容の目標ＺＭＰのうち、例えば前回歩容の後側の支持脚足平２２がつま先接地になった状態での目標ＺＭＰを姿勢回転中心として決定する。
次いでＳ２００８に進み、現在の推定支持脚座標系（詳しくは前回歩容の支持脚足平２２の着地確定時に決定された推定支持脚座標系で、図１１に示す滑りが発生する前の推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、Ｓ２００６で決定した姿勢回転中心まわりに回転させた位置・姿勢を改めて現在の推定支持脚座標系（図１１に例示する滑りが発生した後の推定支持脚座標系）と決定する。
なお、推定支持脚座標系の原点および座標軸の向きは、グローバル座標系によって表されるものとする。
また、Ｓ０１０の初期化処理において、推定支持脚座標系の初期値（グローバル座標系での初期位置姿勢）がセットされているものとする。
次いでＳ２０１０に進み、現在の推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置姿勢関係が目標歩容（前回歩容）における支持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系（図１１に例示する次回歩容推定支持脚座標系）を決定する。なお、ここで言う次回歩容支持脚座標系は、これから生成しようとしている今回歩容に対する次回歩容支持脚座標系ではなく、前回歩容の次の歩容（つまり今回歩容）の支持脚座標系のことである。
次いでＳ２０１２に進み、Ｓ２０１０で決定した次回歩容推定支持脚座標系の位置姿勢（原点の位置及び座標軸の向き）を現在の推定支持脚座標系の位置姿勢に代入する。以上をもってＳ０１６の自己位置推定処理を終了し、前述のごとくＳ０１８に進む。
以上が、第１実施形態におけるＳ０１６の自己位置推定処理である。
推定支持脚座標系の回転中心は、空中期のない通常の歩行においては、支持脚足平２２の滑り回転の回転中心を意味する。なお、走りなど、１歩の途中に支持脚も床から離れている空中期においては、狭義の意味では支持脚足平２２の滑りが存在するわけではないので、空中での姿勢回転（あるいはスピン）は、推定支持脚座標系の回転中心まわりの回転で表現するのが望ましい。しかし、広義の意味で「支持脚足平の滑り」を推定支持脚座標系の回転中心まわりの姿勢回転と定義しても良いことである。
補足すると、支持脚座標系は、支持脚足平２２の接地面に対して設定するもので、その原点は、前述のように支持脚の足首中心から接地面への垂直投影点にしなくても良い。すなわち、支持脚座標系は、ロボットの運動を記述するための支持脚足平２２の近傍の仮想の床に設定されたローカル座標系である。結局、ロボット１の姿勢回転（あるいはスピン）現象は、ロボット１が前記仮想の床上で相対的には姿勢回転（あるいはスピン）しないで運動している状態を保ったまま、ロボット１ごと前記仮想の床を、グローバル座標系のなかである所定の点を回転中心として姿勢回転（あるいはスピン）させた現象とみなされる。
さらに言い替えると、ロボット１の運動は、ローカル座標系において目標歩容あるいは関節変位検出値に従って運動しているロボット１の全体が、ローカル座標系ごと、グローバル座標系に対して、ある所定の点（姿勢回転中心）を回転中心として摂動回転をしているとみなすことができる。
したがって、姿勢回転中心は、推定支持脚座標系の回転中心であり、また、前記摂動回転の回転中心であるとも言える。
第１実施形態のＳ２００８において、上体３の姿勢の傾斜成分の差も含めて、現在推定支持脚座標系を前記姿勢回転偏差変化量だけ回転させると、一般的には、現在推定支持脚座標系のＺ軸は鉛直方向を向かなくなる。
本実施形態では、現在推定支持脚座標系は、着地毎にしか更新されないので、着地瞬間の上体姿勢によって現在推定支持脚座標系が傾いたままであることは、好ましいことではない。したがって、現在推定支持脚座標系は、Ｚ軸の向きが鉛直方向になるように決定されるべきである。そこで、第１実施形態では、Ｓ２００８において、現在の推定支持脚座標系を回転させた後に、その回転後の現在の推定支持脚座標系のＺ軸の向きを鉛直方向に戻す。具体的には、回転後の現在の推定支持脚座標系を、そのＺ軸の鉛直方向に対する傾斜角度分だけ、該推定支持脚座標系の原点回りに回転させることで、該推定支持脚座標系のＺ軸の向きを鉛直方向に戻す。あるいは、Ｓ２００８において、現在の推定支持脚座標系を、前記姿勢回転偏差変化量だけ回転させる代わりに、この姿勢回転偏差変化量のうちの鉛直軸まわり成分（スピン成分）だけ回転させても良い。または、Ｓ２０１０において、次回歩容推定支持脚座標系のＺ軸の向きを鉛直方向に戻しても良い。
以降、第２実施形態までは、同様に、現在推定支持脚座標系は、Ｚ軸の向きが鉛直方向になるように決定するのが良い。第３実施形態以降においても、スピンによる自己位置ずれを重視するならば、同様にしても良い。
上記のごとく、第１実施形態においては、実際のロボット１の挙動を、ロボット１の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、姿勢検出手段（Ｓ２０００の処理）によって得られた上体３の姿勢（実姿勢の推定値）と上体３の目標姿勢との差の１歩の間の変化量としての前記姿勢回転偏差変化量（あるいは該姿勢回転偏差変化量の鉛直軸回り成分）の分だけ、姿勢回転中心決定手段（Ｓ２００６の処理）によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚足平２２の位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち足平２２の着地点の位置及び向き、さらに言い換えると足跡（ｆｏｏｔ  ｐｒｉｎｔ）が、１歩毎（着地毎）に推定される。
なお、以上説明した第１実施形態は、本発明の第１発明〜第５発明、並びに、第７、第８発明に対応するものである。
次に、本発明の第２実施形態を図１２及び図１３を参照して説明する。なお、第２実施形態は、前記図９のＳ０１６の自己位置姿勢推定処理のみが第１実施形態と相違し、これ以外の構成及び制御ユニット６０の処理は、第１実施形態と同じである。
図１２は第２実施形態での自己位置姿勢推定処理を示すフローチャートである。なお、以下に説明する第２実施形態の自己位置推定処理は、ロボット１の歩行を行っている場合の処理である。
以下説明すると、まずＳ２１００において、第１実施形態と同様、上体３の傾斜センサ５４に備えたジャイロセンサの検出値（角速度検出値）を積分器により積分して推定上体姿勢を求める。次いでＳ２１０２に進み、第１実施形態と同様、着地が確定したか否かを判定する。
Ｓ２１０２の判定結果がＮＯである場合には、以上をもって図９のＳ０１６の自己位置推定処理を終了し、前述のごとくＳ０１８に進む。
Ｓ２１０２の判定結果がＹＥＳである場合には、以下の処理を行う。すなわち、まずＳ２１０４に進み、第１実施形態と同様、グローバル座標系から見た推定上体姿勢とグローバル座標系から見た目標歩容の上体姿勢との差を姿勢回転偏差変化量として算出する。
次いでＳ２１０６に進み、第１実施形態と同様、姿勢回転中心を決定する。
次いでＳ２１０８に進み、第１実施形態と同様、現在の推定支持脚座標系を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置・姿勢を改めて現在の推定支持脚座標系と決定する。図１３に、回転させる前の現在推定支持脚座標系と回転させた後の現在推定支持脚座標系とを例示する。
次いでＳ２１１０に進み、少なくとも関節変位検出値を基にキネマティクス演算によって、現在の推定支持脚座標系における着地時の実遊脚足平位置・姿勢（図１３の例ではロボット１の前側の足平２２の位置姿勢）を推定する。以降、推定された着地時の実遊脚足平位置・姿勢を推定着地時遊脚足平位置姿勢と呼ぶ。
ただし、この時、ロボット１が、現在の推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平２２が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。
補足すると、前記姿勢回転偏差変化量だけ前記姿勢回転中心まわりに回転させた現在推定支持脚座標系上で、ロボット１が、目標歩容通りの上体姿勢を維持している姿を想定した理由は、この瞬間において、想定した姿における上体姿勢を、グローバル座標系において、推定上体姿勢に一致させるためである。したがって、現在推定支持脚座標系を、前記差だけ姿勢回転させる代わりに、前記姿勢回転偏差変化量の内の鉛直軸まわり成分（スピン成分）だけ回転させる場合には、ロボット１が、現在の推定支持脚座標系上で、目標歩容通りの上体向きを維持しつつ、上体傾きは、前記姿勢回転偏差変化量の傾き成分に一致し、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平２２が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。
また、この時、６軸力センサ５０による床反力検出値と目標歩容の床反力とのうちの少なくともいずれかを基に、足平２２の変形機構（前記図３及び図４に示した弾性部材１０６、接地部材（足底弾性体）７１、袋状部材１０９等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求め、その変形量を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めても良い。より具体的には、図１３に示すロボット１の姿のように、現在の推定支持脚座標系において上体姿勢を目標上体姿勢とし、関節変位を関節変位検出値（第ｊ関節変位検出値θｊ、ｊ＝１，２…）とし、変形機構の変形量を床反力検出値を基に推定した変形量とした場合の全体姿（その瞬間のロボット１の全体の姿）を求め、この全体姿における支持脚（図１３ではロボット１の後ろ側の脚体２）に対する遊脚足平位置姿勢を求め、これをもって推定着地時遊脚足平位置姿勢とするようにしても良い。また、モータ電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めるようにしても良い。
または、少なくとも目標歩容の関節変位とコンプライアンス補償量（本出願人が先に提案した特開平１０−２７７９６９号を参照のこと）を基に、キネマティクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。
これらにより、より一層精度良く推定着地時遊脚足平位置姿勢を求めることができる。
次いでＳ２１１２に進み、前記推定着地時遊脚足平位置姿勢を基に、図１３に示すように、現在推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置・姿勢を求める。なお、推定着地時遊脚足平位置姿勢、すなわち次回歩容の支持脚足平２２の位置姿勢と、次回歩容推定支持脚座標系の位置姿勢の対応関係は、前述のごとく支持脚足平２２と支持脚座標系との対応関係と同一にする。
次いでＳ２１１４に進み、図１３に示すように、回転させた後の現在推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置・姿勢と、グローバル座標系から見た、回転させた後の現在推定支持脚座標系の位置姿勢とを基に、グローバル座標系から見た次回歩容推定支持脚座標系の位置姿勢を求める。
次いでＳ２１１６に進み、上記の如く求めた次回歩容推定支持脚座標系の位置姿勢を現在の推定支持脚座標系の位置姿勢に代入する。以上をもって図９のＳ０１６の自己位置推定処理を終了し、前述のごとくＳ０１８に進む。
上記のごとく、第２実施形態においては、実際のロボット１の挙動を、ロボット１の運動を記述するローカル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動しているロボット１が、１歩の間に、姿勢検出手段（Ｓ２１００の処理）によって得られた上体３の姿勢（実姿勢の推定値）と上体３の目標姿勢との差の１歩の間の変化量としての前記姿勢回転偏差変化量だけ、姿勢回転中心決定手段（Ｓ２１０６の処理）によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち足平２２の着地点の位置及び向き、さらに言い換えると足跡（ｆｏｏｔ  ｐｒｉｎｔ）が、１歩毎（着地毎）に推定される。
なお、以上説明した第２実施形態は、本発明の第１〜第５発明及び第７〜第９発明に対応するものである。
次に本発明の第３実施形態を図１４及び図１５を参照して説明する。なお、第３実施形態は、前記図９のＳ０１６の自己位置姿勢推定処理のみが第１実施形態と相違し、これ以外の構成及び制御ユニット６０の処理は、第１実施形態と同じである。
図１４は、第３実施形態における図９のＳ０１６の自己位置推定処理のフローチャートである。
第３実施形態においては、第１、第２実施形態と異なり、制御周期毎に、推定支持脚座標系の姿勢回転（またはスピン）を推定し、推定支持脚座標系を更新するようにした。また、制御周期毎の各瞬間における上体姿勢を推定すると共に、さらに上体位置（厳密には上体の代表点の位置）も推定するようにした。
詳細を説明すると、まず、Ｓ２２００において、第１実施形態と同様、推定上体姿勢を求める。
次いでＳ２２０２に進み、制御周期の間におけるグローバル座標系から見た推定上体姿勢の変化量と、該制御周期の間におけるグローバル座標系から見た目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量としてを算出する。すなわち、前記第１、第２実施形態では、１歩の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として求めたが、第３実施形態では、制御周期毎の両変化量の差を姿勢回転偏差変化量として求める。
次いでＳ２２０４に進み、第１実施形態と同様、姿勢回転中心を決定する。具体的には、その瞬間の目標ＺＭＰ（目標ＺＭＰの現在値）を姿勢回転中心とする。
次いでＳ２２０６に進み、現在の推定支持脚座標系（前回の制御周期で決定された推定支持脚座標系で、図１５に示す時刻ｔ−Δｔにおける推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置・姿勢を改めて現在の推定支持脚座標系（図１５に示す時刻ｔにおける推定支持脚座標系）と決定する。回転のさせ方は第１実施形態と同様である。
次いでＳ２２０８に進み、着地時刻であるか否か、すなわち歩容の切り替わり目であるか否かを判定する。
Ｓ２２０８の判定結果がＹＥＳである場合には、以下の処理を行う。すなわち、まずＳ２２１０に進み、第１実施形態と同様、現在の推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置姿勢関係が、目標歩容（前回歩容）における支持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系を決定する。
次いでＳ２２１２に進み、第１実施形態と同様、次回歩容推定支持脚座標系の位置姿勢を現在の推定支持脚座標系の位置姿勢に代入する。
Ｓ２２１２の後、あるいはＳ２２０８において否定された場合には、Ｓ２２１４に進み、現在の推定支持脚座標系の位置姿勢に対応して推定上体位置を決定する。ただし、現在の推定支持脚座標系に対する推定上体位置の関係は、目標歩容（前回の制御周期で決定された目標歩容）の支持脚座標系に対する上体位置の関係に一致させる。
以上をもってＳ０１６の自己位置推定処理を終了し、前述のごとくＳ０１８に進む。
上記のごとく、第３実施形態においては、実際のロボットの挙動を、ロボットの運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、各瞬間に、姿勢検出手段（Ｓ２２００の処理）によって得られた上体姿勢の変化量（変化速度）と目標歩容の上体姿勢の変化量（変化速度）との差としての姿勢回転偏差変化量で、姿勢回転中心決定手段（Ｓ２２０４の処理）によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなす。そして、その上で、新たな推定支持脚座標系と推定上体位置とが各瞬間（制御周期毎）に決定（更新）されるとともに、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向き、さらに言い換えると新たな足跡（ｆｏｏｔ  ｐｒｉｎｔ）が、１歩毎（着地毎）に推定される。
なお、本実施形態は、ロボット１の歩行を行う場合だけでなく、ロボット１の走行を行う場合でもよい。但し、走行を行う場合にＳ２２０４で姿勢回転中心を決定するときには、片脚支持期では、姿勢回転中心を各瞬間（現在）の目標ＺＭＰとすればよいが、ロボット１の両脚体２，２が空中に浮く空中期では、姿勢回転中心を、各瞬間（現在）の目標歩容におけるロボット１の重心の位置に決定する。
また、ロボット１の歩行を行なう場合には、Ｓ２２１４においては、少なくとも現在の推定支持脚座標系と関節変位検出値とを基に、Ｓ２１１０において推定着地時遊脚足平位置姿勢を求めた手法と同様の手法で、支持脚座標系に対する上体位置を求め、この位置関係に、推定支持脚座標系に対する推定上体位置が一致するように、推定上体位置を求めても良い。この場合、推定上体位置、推定支持脚座標系および関節変位検出値の関係は、図１３に示すようになる。
または、少なくとも目標歩容の関節変位とコンプライアンス補償量（本出願人が先に提案した特開平１０−２７７９６９号を参照のこと）を基に、キネマティクス演算によって支持脚座標系に対する上体位置・姿勢を求め、これに推定支持脚座標系に対する推定上体位置姿勢の関係を一致させても良い。
これらにより、より一層精度良く推定上体位置姿勢を決定することができる。
また、ロボット１の歩行を行なう場合には、Ｓ２２１０の代わりに、第２実施形態における図１２のＳ２１１０からＳ２１１４の処理を実行しても良い。これにより、より一層精度良く、着地時の推定支持脚座標系を決定することができる。
またさらに、Ｓ２２１０の代わりに、第２実施形態におけるＳ２１１０からＳ２１１４の処理を実行すると共に、Ｓ２２１４において、上記のごとく少なくとも関節変位検出値を基にキネマティクス演算によって推定上体位置・姿勢を求めても良い。このとき、前記第２実施形態で説明した如く、床反力検出値を基に、足平２２の変形量を考慮して、推定着地時遊脚足平位置姿勢を求め、これを用いて次回歩容推定支持脚座標系を決定するようにすることがより好ましい。
言い換えると、実際のロボットの挙動を、ロボットの運動を記述するローカル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動しているロボットが、各瞬間（各制御周期）に、姿勢検出手段（Ｓ２２００の処理）によって得られた上体姿勢の変化量（制御周期当たりの変化量でこれは変化速度に相当する）と目標歩容の上体姿勢の変化量（制御周期当たりの変化量でこれは変化速度に相当する）との差としての姿勢回転偏差変化量で、姿勢回転中心決定手段（Ｓ２２０４の処理）によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなす。そして、その上で各瞬間に（制御周期毎に）新たな推定支持脚座標系と推定上体位置を決定（更新）するとともに、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向き、さらに言い換えると新たな足跡（ｆｏｏｔ  ｐｒｉｎｔ）が、１歩毎（着地毎）に推定されるようにしても良い。
これにより、より一層精度良く、着地時の推定支持脚座標系を決定することができるとともに、推定上体位置姿勢が不連続になることを防止することができる。
なお、以上説明した第３実施形態は、本発明の第１〜第６発明、並びに、第８、第９発明に対応するものである。
次に本発明の第４実施形態を図１６〜図１９を参照して説明する。なお、図９のＳ０１６の自己位置姿勢推定処理のみが前記第１実施形態と相違し、これ以外の構成及び制御ユニット６０の処理は、第１実施形態と同じである。
前記第３実施形態の如く決定された推定上体位置を、以降、「幾何学的推定上体位置」という。幾何学的推定上体位置は、グローバル座標系から見た位置である。
一方、加速度センサとジャイロセンサの検出値を基に、公知の技術である慣性航法によって推定されるグローバル座標系から見た上体の位置・姿勢を慣性航法的推定上体位置姿勢と呼ぶ。なお、第３実施形態のＳ２２００で求められる推定上体姿勢は、慣性航法的推定上体姿勢である。
幾何学的推定上体位置は、前記第３実施形態で説明した如く、床を基準にして幾何学的に推定される上体位置であるので、走行などの空中期においては、推定精度が低下し易い。
一方、慣性航法的推定上体位置姿勢は、歩行、走行を問わず、片脚支持脚期、両脚支持期および空中期のいずれの時期においても、推定原理が同一であるので、いかなる時期においても短期的な精度が低下することはないが、積分が用いられるため、長期的なドリフトが発生しやすい。
これらの特性を考慮し、第４実施形態は、慣性航法的推定上体位置姿勢を幾何学的推定上体位置で補正するようにした。
また、幾何学的推定上体位置及び推定上体姿勢の運動から算出されるロボット１の運動加速度と、加速度センサの検出値とを用いて、ジャイロセンサの検出ドリフトを補正すると共に、推定上体姿勢のうちの傾き成分のドリフトを補正するようにした。さらに、状況によって、ジャイロセンサの検出値のヨー方向のドリフト補正も行うようにした。
図１６は、第４実施形態における図９のＳ０１６の自己位置推定処理のフローチャート、図１７は、同推定処理のブロック図である。
図１６および図１７を用いて、第４実施形態における図９のＳ０１６の自己位置推定処理を説明すると、まず図１６のＳ２３００において、ジャイロセンサの検出値を積分して推定上体姿勢を求める。ただし、前制御周期（前回以前の制御周期）に求めた幾何学的推定上体位置の運動から算出される運動加速度と、加速度センサの検出値を用いて、ジャイロセンサの検出ドリフトを補正することで、推定上体姿勢のうちの傾き成分のドリフトを補正する。
Ｓ２３００の処理を詳説すると、まず、前回の制御周期およびそれ以前に、後述するＳ２３０２において決定した幾何学的推定上体位置の運動を基に、幾何学的推定上体位置の２階微分である幾何学的推定上体加速度を算出する。この処理は、図１７のブロック３０１で実行される。なお、幾何学的推定上体位置を求める処理は、図１７のブロック３００で実行される。補足すると、上体代表点と加速度センサの位置が一致していれば、幾何学的推定上体加速度（厳密には、幾何学的に推定される上体３の中の加速度センサ位置の加速度）を算出する上で、推定上体姿勢は必要ない。
次いで、加速度センサ検出値（上体加速度検出値）を、推定上体姿勢を用いてグローバル座標系に変換してなる加速度センサ検出値グローバル座標系換算値を求める。この処理は、図１７のブロック３０２で実行される。なお、説明を簡単にするために、ここでは、加速度センサの位置と上体代表点の位置は一致しているものとする。異なる場合には、キネマティクス演算によって、加速度センサ検出値およびジャイロセンサ検出値を上体代表点における加速度・角速度に変換するか、あるいは、上体代表点における加速度・角速度を加速度センサの位置における加速度、ジャイロセンサの位置における角速度に変換すれば良い。
次いで、図１８（ａ）および図１８（ｂ）に示すように、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じる（ベクトルの減算を行う）ことにより、推定重力加速度を求める。加速度センサ検出値グローバル座標系換算値には、加速度センサに作用する重力による加速度成分が含まれるが、幾何学的推定上体加速度には、重力による加速度成分が含まれない。従って、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じることで、推定重力加速度が求められる。この処理は、図１７のブロック３０３で実行される。なお、図１８（ａ）は、推定上体姿勢に誤差がない場合、図１８（ｂ）は、推定上体姿勢に誤差（以降、これを推定上体姿勢誤差、あるいは推定上体姿勢誤差角度と呼ぶ）がある場合を示す。
一方、図１８（ａ）および図１８（ｂ）に示すように、推定上体姿勢を基にロボット１が推定したグローバル座標系の鉛直方向（Ｚ軸方向）に発生すると想定される重力加速度を想定重力加速度と呼ぶ。
前述したように、幾何学的推定上体加速度はグローバル座標系で記述される。また、実際のロボット１は、推定上体姿勢が実際の上体姿勢を正しく推定しているとみなした上で、目標歩容に追従するように運動している。言い換えると、推定上体位置姿勢を基に推定されるグローバル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。より厳密に表現するならば、ロボット１の自己位置姿勢推定処理においては、推定上体位置姿勢を基に推定されるグローバル座標系が正しいと信じ、実際のロボット１の挙動を、正しいと信じているグローバル座標系に設定されるローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、各瞬間に、姿勢検出手段によって得られた（検出あるいは推定された）上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差としての姿勢回転偏差変化量で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなしている。
したがって、推定上体姿勢が正しく、かつ加速度センサの検出値に誤差がなければ、図１８（ａ）に示すように、前記想定重力加速度と前記推定重力加速度とは一致する。
これに対し、加速度センサの検出値に誤差がなくても、推定上体姿勢に誤差があれば、図１８（ｂ）に示すように、前記想定重力加速度の向きと前記推定重力加速度の向きとには、推定上体姿勢誤差角度だけずれが生じる。すなわち、式４２が成立する。
推定上体姿勢誤差角度  ＝  想定重力加速度と推定重力加速度との間の角度
…式４２
図１８（ａ）および図１８（ｂ）に示すように、推定重力加速度と想定重力加速度との差を重力加速度推定誤差と呼ぶと、上記の関係から、重力加速度推定誤差は、加速度センサの検出値に誤差がなければ、推定上体姿勢の誤差によって発生し、推定上体姿勢を基にロボット１が推定したグローバル座標系から見た重力加速度推定誤差の水平成分（推定重力加速度の想定重力加速度に直交する成分）と推定上体姿勢誤差角度とには、式４３の関係があることがわかる。ただし、式４３は、重力加速度推定誤差の前後方向成分（Ｘ成分）と推定上体姿勢誤差角度Ｙ軸まわり成分との関係を表す。重力加速度推定誤差の左右方向成分（Ｙ成分）と推定上体姿勢誤差角度Ｘ軸まわり成分の関係を表す場合においては、右辺のマイナス記号を削除すれば良い。ここで、重力加速度は、正である。
重力加速度推定誤差の水平成分
＝  −ｔａｎ（推定上体姿勢誤差角度）＊重力加速度
…式４３
本実施形態においては、式４２あるいは式４３から算出される推定上体姿勢誤差角度を用いて、推定上体姿勢誤差が０に収束するように推定上体姿勢を補正するようにした。
Ｓ２３００の説明に戻ると、式４２を用いて、現在の瞬間（厳密には、１制御周期前の瞬間）における想定重力加速度と推定重力加速度との間の角度から推定上体姿勢誤差角度を算出する。あるいは、式４３を用いて、現在の瞬間（厳密には、１制御周期前の瞬間）における重力加速度推定誤差の水平成分から推定上体姿勢誤差角度を算出しても良い。推定上体姿勢誤差角度の算出処理は、図１７のブロック３０４で実行される。
次いで、前記推定上体姿勢誤差角度を、図１７のブロック３０５にて、センサ座標系（座標軸をジャイロセンサの検出軸に合わせた座標系）に変換した後、ブロック３０６にて、その変換後の値に積分ゲインＫａを乗じた値を積分することにより、推定ジャイロセンサドリフト（ジャイロセンサのドリフトの推定値）を求める。さらに、この推定ジャイロセンサドリフトを、ブロック３０７にて、ジャイロセンサ検出値ωｉｎ（上体角速度検出値）から減じることにより、ドリフトを補正した角速度を求める。なお、ブロック３０７では、ヨーレートのドリフトも適宜減算されるが、これについては後述する。さらに、このドリフトを補正した角速度を、ブロック３５０にて、推定上体姿勢を用いてグローバル座標系に変換することにより、グローバル上体角速度ωｇｌを求める。
次いで、前記推定上体姿勢誤差角度にゲインＫｂを図１７のブロック３０９で乗じた値をブロック３０８にてグローバル上体角速度ωｇｌから減じ、その減算後の値（ブロック３０８の出力）をブロック３１０（積分器）で積分することにより新たな推定上体姿勢θｅｓｔｍを求める。
なお、推定上体姿勢θｅｓｔｍや角速度などは、クォータニオン、回転行列、あるいはオイラー角などで表現される。
ジャイロセンサのドリフト（推定ジャイロセンサドリフト）を上記のごとき構成により推定する場合、推定が効果的に働くためには、推定上体姿勢誤差角度のセンサ座標系への変換値（ベクトル）のそれぞれの要素（それぞれのセンサ検出軸まわりの推定上体姿勢誤差角度、言い換えるとセンサローカル推定上体姿勢誤差角度）が、その要素に対応するジャイロセンサのドリフトの影響のみを受けて、他のジャイロセンサのドリフトの影響を受けない、あるいは、ほとんど受けないことが前提条件となる。
言い換えると、推定上体姿勢誤差角度のＸ（Ｙ）軸まわり成分の誤差は、Ｘ（Ｙ）軸用ジャイロセンサのドリフトの影響を受けるが、Ｙ（Ｘ）軸用ジャイロセンサのドリフトの影響を受けないことが前提条件となる。
もし、Ｘ軸用ジャイロセンサのドリフトの影響により、推定上体姿勢誤差角度のＸ軸まわり成分が発生した後、上体をセンサ座標系のＺ軸まわりに急に９０度回転させると、推定上体姿勢誤差角度は、グローバル座標系において蓄積されたままであるので、結果的に推定上体姿勢誤差角度のセンサローカルＸ軸まわり成分が、推定上体姿勢誤差角度のセンサローカルＹ軸まわり成分に移る。したがって、前記前提条件が成立するためには、センサ座標系Ｚ軸まわりの回転速度の絶対値が十分に小さいことが必要条件となる。
よって、Ｚ軸（上体の上下方向軸）用ジャイロセンサ検出値の絶対値が大きい時には、積分ゲインＫａを小さくするか、あるいは０にする（すなわちジャイロセンサのドリフト補正を行わない）ようにするのが望ましい。
また、通常、ロボット１の移動時や作業時には、上体３の上下軸（体幹軸）は鉛直姿勢あるいはその近傍の姿勢になっているので、ジャイロセンサの角速度検出値をグローバル座標系に変換したグローバル上体角速度ωｇｌｚは、Ｚ軸用ジャイロセンサの角速度検出値の値に近い値となる。
よって、図１９に示すように、ジャイロセンサの角速度検出値をグローバル座標系に変換したグローバル上体角速度の鉛直軸まわり角速度ωｇｌｚの絶対値が大きい時には、積分ゲインＫａを小さくするか、あるいは０にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。
また、ジャイロセンサの角速度検出値の代わりに、上体３の目標歩容の上体回転速度の鉛直軸まわり成分あるいは上体３の上下方向軸まわり成分の絶対値が大きい時には、積分ゲインＫａを小さくするか、あるいは０にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。
以上をまとめて言い換えると、ジャイロセンサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体３の上下方向軸まわり成分の絶対値が大きい時には、積分ゲインＫａを小さくするか、あるいは０にする（すなわちジャイロセンサのドリフト補正を行わない）と良い。
また、加速度センサ検出値グローバル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい場合には、センサの検出誤差やロボット１本体の剛性不足などにより、推定重力加速度の誤差が大きくなる傾向があるので、上記ゲインＫａを小さく、あるいは０に設定するのが良い。
また、前述したごとく、実際のロボット１は、推定上体位置姿勢を基に推定されるグローバル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。したがって、支持脚が接地している状態においては、推定上体姿勢に大きな誤差があっても、実際の上体加速度は、実際の重力加速度に支配されず、前記推定されたグローバル座標系上での幾何学的推定上体加速度に、ほぼ一致する。一方、空中期においては、実際の上体加速度は、実際の重力加速度に支配されて加速するので、前記推定されたグローバル座標系上での幾何学的推定上体加速度の方向と大きくずれ、図１８（ｂ）の関係が成立しなくなる。したがって、空中期においては、幾何学的推定上体位置の精度は、比較的高精度の加速度センサとジャイロセンサを基に慣性航法によって求めた上体位置の精度よりも低下しがちとなる。したがって、空中期においても、上記ゲインＫａを小さく、あるいは０に設定するのが良い。
また、前述のように足平２２の変形を考慮して上体位置を推定する場合には、空中期でなくとも、脚体先端部の足平２２が床と良好に接触していない状態では、足底付近の変形量推定の誤差が大きくなる傾向があるので、幾何学的推定上体加速度の精度は低下する。したがって、支持脚の足平２２と床との間の接触面積が小さい場合にも、上記ゲインＫａを小さく、あるいは０に設定するのが良い。
なお、脚体２の足平２２が床と良好に接触している状態とは、具体的には、以下のような状態の少なくともいずれかが満足する状態、あるいは以下のうちの複数が同時に満足する状態を指す。
ａ）足平２２の底面と床との間の接触面積が大きい状態、例えば、足平２２が床にべたに接触している状態（いわゆるべた足状態）
ｂ）床反力の分布圧が比較的均一である状態
ｃ）接地箇所が多い状態
ｄ）目標ＺＭＰ（あるいは実床反力中心点）が足平２２の中央あるいは中央に近い状態
ｅ）接地圧または床反力が高い状態
ｆ）足平２２の位置姿勢変化率が小さい状態、厳密には、足平２２の底面と床の相対位置姿勢関係の変化が小さい状態
上記状態の判定には、例えば目標歩容の時期（フェーズ）、目標ＺＭＰ、床反力検出値を基に行えばよい。あるいは、本実施例では備えていないが、分布圧センサや接触センサの検出値を基に上記状態の判定を行ってもよい。
以上のゲイン設定法をまとめると、ジャイロセンサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体３の上下方向軸まわり成分の絶対値が大きい状態、加速度センサ検出値グローバル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい状態、空中期、あるいは、脚先端部（足平）が床と良好に接触していない状態では、上記ゲインＫａを小さく、あるいは０に設定するのが良い。
すなわち、これらの状態を基に総合的に判断して積分ゲインＫａを決定するのが良い。
またこれらの状態の瞬時値、あるいは長期的傾向に応じて積分ゲインＫａを決定しても良い。
同じ理由により、積分ゲインＫｂも前記ゲインＫａと同様に設定することが好ましい。
本実施形態でのＳ２３００においては、以上の処理の他に、以下のように、状況によりヨーレート補正（ヨー方向のドリフト補正）も行われる。
すなわち、まず、ヨーレート補正を行うか否かを、図１７のブロック３１１で判定する。
具体的には、以下の条件の少なくともいずれかひとつあるいは複数を判断条件として用意し、この判断条件を満足している場合には、ヨーレート補正を行うと判断する。
ａ）検出された床反力の鉛直軸まわりモーメント（あるいはその微分値）の絶対値がある所定の値以内である状況
ｂ）目標動作がほぼ静止である状況
ｃ）足底と床の間の接触状況が良好で滑りが生じていないと考えられる状況
ｄ）目標歩容の姿勢角速度とジャイロ検出値（姿勢角速度検出値）の差の絶対値がある所定の値以内である状況
ｅ）目標歩容の姿勢角速度の絶対値がある所定の値以内である状況
ｆ）ジャイロ検出値（姿勢角速度検出値）の絶対値がある所定の値以内である状況
基本的には、支持脚足平２２と床との接地面で滑り（回転滑り）が生じないかもしくは生じ難いという状況で、ヨーレート補正を行うと判断する。
なお、ｄ）およびｆ）におけるジャイロ検出値は、ジャイロセンサの検出値そのもの（ジャイロセンサの生の出力そのものが表す姿勢角速度検出値）をヨーレート補正した値であることが望ましい。
次いで、ヨーレート補正を行うと判定された場合には、図１７に示すように、ブロック３１２にて、目標歩容、目標上体姿勢、目標関節変位あるいは関節変位検出値の少なくともいずれかと最新の着地時に記憶した推定支持脚座標系（以降、着地時推定支持脚座標系と呼ぶ）を基に、推定支持脚座標系に対応する足平２２と床との間に滑りが生じていないと想定した場合の上体姿勢（以降、滑りなし推定上体姿勢と呼ぶ）を算出する。そして、前記推定上体姿勢と前記滑りなし推定上体姿勢との差をブロック３１３で求め、この差をブロック３１４でセンサ座標系に変換したものを、フィードバック制御則のブロック３１５に入力することで、ヨーレートドリフトを求める。ブロック３１５のフィードバック制御則としては、例えばＰＩＤ制御則が用いられ、前記差（ブロック３１４の出力）の時間微分値（または制御周期の間の前記差の変化量）にゲインＫｅを乗じた値と、前記差（ブロック３１４の出力）にゲインＫｆを乗じた値と、前記差（ブロック３１４の出力）の積分値にゲインＫｇを乗じた値との和を求めることによりヨーレートドリフトが求められる。そして、この求めたヨーレートドリフトを、前記ブロック３０７で前記角速度センサ検出値ωｉｎから減じる。また、ヨーレート補正を行わないと判定された場合には、フィードバック制御則のブロック３１５への入力を遮断して（図１７のスイッチ３１６を開成する）、直前のヨーレートドリフト値を保持し、その値を前記角速度センサ検出値ωｉｎから減じるようにする。なお、滑りなし推定上体姿勢を求める際には、図１２のＳ２１１０において着地時の遊脚足平位置・姿勢を推定する際と同様、床反力検出値および／または目標歩容の床反力を基に、足平２２の変形機構（前記図３及び図４に示した弾性部材１０６、接地部材（足底弾性体）７１、袋状部材１０９等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求め、前記変形量を含めて（考慮して）、滑りなし推定上体姿勢を求めても良い。また、モータ電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、滑りなし推定上体姿勢を求めるようにしても良い。
または、少なくとも目標歩容の関節変位とコンプライアンス補償量（本出願人が先に提案した特開平１０−２７７９６９号を参照）を基に、キネマティクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。
これらにより、より一層精度良く推定着地時遊脚足平位置姿勢を求めることができる。
図１６の説明に戻ると、以上のごとくＳ２３００の処理を完了した後、次いで、Ｓ２３０２に進み、前述の第３実施形態の図１４のＳ２２０２からＳ２２１４を実行して、推定支持脚座標系の位置姿勢および幾何学的推定上体位置を決定する。なお、このとき、第３実施形態に関して補足した如く、図１４のＳ２２１０の代わりに、第２実施形態におけるＳ２１１０からＳ２１１４の処理を実行すると共に、Ｓ２２１４において、上記のごとく少なくとも関節変位検出値を基にキネマティクス演算によって推定上体位置・姿勢を求めても良い。このとき、前記第２実施形態で説明した如く、床反力検出値を基に、足平２２の変形量を考慮して、推定着地時遊脚足平位置姿勢を求め、これを用いて次回歩容推定支持脚座標系を決定するようにすることがより好ましい。
次いで、Ｓ２３０４に進み、加速度センサとジャイロセンサを基に慣性航法により慣性航法的推定上体位置姿勢を求めつつ、幾何学的推定上体位置と慣性航法的推定上体位置との差が０に収束するように慣性航法的上体位置を修正する。
具体的には、図１７に示すごとく慣性航法的推定上体位置Ｘｉｎｅｒｔｅｓｔｍの前回値と前記幾何学的推定上体位置との差をブロック３１７で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲインＫｃを乗じた値と、該差にゲインＫｄを乗じた値との和をブロック３１８で求める。つまり、上記差からフィードバック制御則としてのＰＤ制御則より、上記和を求める。また、前記ブロック３０２の出力である前記加速度センサ検出値グローバル座標系換算値と想定重力加速度Ｇの差をブロック３１９で求める。そして、この差（ブロック３１９の出力）から上記和（ブロック３１８の出力）をブロック３６０にて減じた値を、ブロック３２０にて２階積分することにより、新たな慣性航法的推定上体位置Ｘｉｎｅｒｔｅｓｔｍを決定する。
なお、ゲインＫｃおよびＫｄは、幾何学的推定上体位置の誤差が大きく発生し易い状況では小さく、あるいは０に設定するのが良い。支持脚の足平２２の底面と床との間の接触面積が小さい場合、幾何学的推定上体位置の誤差は、大きくなる。したがって、支持脚の足平２２の底面と床との間の接触面積が小さい場合には、上記ゲインＫｃおよびＫｄを小さく、あるいは０に設定するのが良い。また、特に、足平２２が床にべたに接触している状態（いわゆるべた足状態）の時に、上記ゲインＫｃおよびＫｄを大きくするのが良い。
以上より、例えば、走行の場合には、補正ゲインＫａ、Ｋｂ、ＫｃおよびＫｄは、図８（ｃ）に示す補正ゲインＫのグラフのように、足底全面接地期間では高く、空中期には、０または概ね０にすれば良い。なお図８（ｃ）の補正ゲインＫのグラフは、Ｋａ、Ｋｂ、ＫｃおよびＫｄの大きさの変化の傾向を表してはいるが、厳密な値を表しているわけではない。また、図８（ｃ）では補正ゲインＫの最大値が１となるように規格化されている。したがって、Ｋは、補正ゲインＫａ、Ｋｂ、Ｋｃ、Ｋｄの絞り（減衰器）を意味すると考えても良い。
本実施例においては、前記式４２あるいは式４３を用いて算出される推定上体姿勢誤差を基に推定上体姿勢を補正するようにしたが、式４２および式４３を用いず、直接、重力加速度推定誤差の水平成分を基に推定上体姿勢を補正するようにしても良い。すなわち、式４３の代わりに次の式４４を用いても良い。
重力加速度推定誤差の水平成分  ＝−推定上体姿勢誤差角度＊重力加速度
…式４４
なお、ゲインＫａの設定について補足すると、空中期においては、推定上体姿勢の誤差がいかなる値であろうとも、ロボット１とともに放物運動をする座標系から見ればロボット１全体は無重力状態にいるのと同一であって、加速度センサの出力は、推定上体姿勢の誤差の影響を受けない。したがって、加速度センサの検出精度が高く、目標歩容に対する実際のロボット１の追従性が高く、ロボット１の剛性が高く、且つ目標歩容を生成するために用いられるロボット動力学モデルのパラメータが実際のロボット１とほぼ一致しているという条件（以下、ここでは条件Ａという）が満足されるならば、推定重力加速度と想定重力加速度とは常にほぼ一致する。厳密には、推定上体姿勢の誤差に応じて、推定重力加速度と想定重力加速度とは同じ量だけ真値からずれるので、結果的に推定重力加速度と想定重力加速度とは常にほぼ一致する。したがって、本質的に、空中期においては、重力方向を推定することはできない。しかし、推定重力加速度と想定重力加速度とは常にほぼ一致するので、その瞬間の推定上体姿勢誤差はほぼ０となり、上記ゲインＫａを小さく設定しなくても、推定上体姿勢誤差にゲインＫａを乗じて求められる補正量もほぼ０となり、推定上体姿勢に大きく悪影響を与える恐れは低い。
また、上記条件Ａを満足する場合には、ゲインＫａを一定にしておいた方が、むしろ推定上体姿勢の低周波数成分（ＤＣ成分）の精度が高くなる場合もある。なぜなら、上記のごとく、ゲインＫａを乗じた値を積分した値を０にするように補正した構成は、ゲインＫａを一定にすると、理論上、重力加速度推定誤差の水平成分の長時間平均値を０にするように作用し、また、推定上体姿勢誤差が０の時には、重力加速度推定誤差の水平成分の長時間平均値は理論上０になるからである。しかし、通常は、上記条件Ａを満足することは難しいので、図８（ｃ）のようにゲインＫａを設定する方が望ましい。
なお、以上説明した第４実施形態は、本発明の第１〜第６発明、第８〜第２２発明に対応するものである。
次に本発明の第５実施形態を説明する。図２０及び図２１を参照して説明する。図２０は、第５実施形態における図９のＳ０１６の自己位置推定処理のフローチャート、図２１は、第５実施形態におけるＳ０１６の自己位置推定処理のうちの、全体重心位置を推定する手段を示すブロック図である。なお、第５実施形態は、第４実施形態において慣性航法的推定上体位置Ｘｉｎｅｒｔｅｓｔｍを決定する代わりに、後述する慣性航法的推定全体重心位置ＸＧｉｎｅｒｔｅｓｔｍを、制御周期毎に決定するようにした。これ以外は、第４実施形態と同じである。また、図２１では、図１７と同一構成部分については、図１７と同一の参照符号を用いている。
図２０および図２１を参照して、第５実施形態におけるＳ０１６の自己位置推定処理を説明すると、まず、図２０のＳ２４００において、第４実施例の図１６のＳ２３００と同様に、ジャイロセンサの検出値を積分して推定上体姿勢を求めつつ、ジャイロセンサの検出ドリフトと推定上体姿勢のうちの傾き成分のドリフトを補正する。
次いでＳ２４０２に進み、前述の図１４のＳ２２０２からＳ２２１４を実行して、推定支持脚座標系の位置姿勢および幾何学的推定上体位置を決定する。
次いでＳ２４０４に進み、前記推定支持脚座標系の位置姿勢、目標上体位置姿勢（あるいは前記幾何学的推定上体位置）および関節変位検出値（あるいは目標値）を基に、前記第３実施形態のごとく実際のロボットの挙動（姿勢回転）を考慮して、全体重心位置をキネマティクス演算によって算出する。この処理は、図２１のブロック３３０で実行される。このようにして算出された全体重心位置を幾何学的推定全体重心位置と呼ぶ。
次いでＳ２４０６に進み、関節角検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）および前記加速度センサ検出値グローバル座標系換算値を基に、全体重心の加速度をキネマティクス演算によって算出する。この処理は、図２１のブロック３３１で実行される。以降、このようにして求められた全体重心の加速度を慣性航法的推定全体重心加速度と呼ぶ。
次いでＳ２４０８に進み、前記慣性航法的推定全体重心加速度を基に慣性航法により慣性航法的推定全体重心位置ＸＧｉｎｅｒｔｅｓｔｍを求めつつ、幾何学的推定全体重心位置と慣性航法的推定全体重心位置の差が０に収束するように慣性航法的推定全体重心位置を修正する。より具体的には、図２１に示すように、前制御周期（前回以前の制御周期）に決定した慣性航法的推定全体重心位置ＸＧｉｎｅｒｔｅｓｔｍと前記幾何学的推定上体位置との差をブロック３３２で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲインＫｃを乗じた値と、該差にゲインＫｄを乗じた値との和をブロック３３３で求める。つまり、上記差からフィードバック制御則としてのＰＤ制御則より、上記和を求める。そして、ブロック３３１の出力である慣性航法的推定全体重心加速度から上記和（ブロック３３３の出力）をブロック３３４で減じた値をブロック３３５にて２階積分することにより、新たな慣性航法的推定全体重心位置ＸＧｉｎｅｒｔｅｓｔｍを決定する。
最後にＳ２４１０に進み、前記慣性航法的推定全体重心位置、関節変位検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）を基に、グローバル座標系における推定上体位置を求める。この処理は、図２１のブロック３３６で実行される。
以上が、第５実施形態における図９のＳ０１６の自己位置推定処理である。
第５実施形態においては、空中期であるか否かを判定して、空中期である場合には、加速度センサを用いずに、全体重心は放物運動を行うという想定のもとで、前記慣性航法的推定全体重心加速度を決定しても良い。
ロボット１が環境から外力を受けないことが明白な場合には、空中期においては、全体重心は放物運動を行うので、これにより、より一層精度良く全体重心位置を推定することができる。
なお、空中期であるか否かは、目標歩容の時期（フェーズ）、目標歩容の床反力あるいは、床反力検出値の少なくともいずれかを用いて判定すれば良い。
特に、全体重心位置の推定精度を極力高くしたい場合には、床反力検出値が所定の値以下であるか否かによって空中期であるか否かを判定すれば良い。
なお、以上説明した第５実施形態は、本発明の第１〜第６発明、第８〜第２２発明に対応するものである。
次に本発明の第６実施形態を図２２を参照して説明する。図２２は、第６実施例における図９のＳ０１６の自己位置推定処理のうちの、全体重心位置を推定する手段を示すブロック図である。
第６実施形態においては、第５実施形態において加速度センサ検出値を基に慣性航法的推定全体重心位置を算出する代わりに、前記６軸力センサ（床反力センサ）５０の床反力検出値を基に動力学的推定全体重心位置を算出するようにした。これ以外は、第５実施例と同じである。また、図２２では、図２１と同一構成部分については、図２１と同一の参照符号を用いている。
図２２を用いて、制御周期毎の全体重心位置の推定処理を説明すると、まず、床反力センサ（６軸力センサ５０）の検出値を、関節角検出値（あるいは目標値）および推定上体姿勢（あるいは目標上体姿勢）を用いて、ブロック３４０にてグローバル座標系の値に換算する。以降、この換算された床反力センサ検出値を、床反力センサ検出値グローバル座標系換算値と呼ぶ。
次いで、前記床反力センサ検出値グローバル座標系換算値をブロック３４１にてロボット１の全体質量で割った値から、ブロック３４２にて重力加速度を減じることにより、全体重心加速度の推定値を求める。以降、このようにして求めた全体重心加速度を動力学的推定全体重心加速度と呼ぶ。
次いで、第５実施形態と同様、ブロック３３０にて幾何学的推定全体重心位置を求める。
次いで、前記動力学的推定全体重心加速度を２階積分することで動力学的推定全体重心位置を求めつつ、幾何学的推定全体重心位置姿勢と慣性航法的推定全体重心位置との差が０に収束するように前記動力学的全体重心位置を修正する。
より具体的には、図２２に示すように、前制御周期（前回以前の制御周期）に決定した前記動力学的推定全体重心位置と前記幾何学的推定全体重心位置との差をブロック３４３で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲインＫｃを乗じた値と、該差にゲインＫｄを乗じた値の和をブロック３４４で求める。つまり、上記差からフィードバック制御則としてのＰＤ制御則より、上記和を求める。そして、ブロック３４２の出力である前記動力学的推定全体重心加速度から上記和（ブロック３４４の出力）をブロック３４５で減じた値をブロック３４６にて２階積分することにより、新たな動力学的推定全体重心位置ＸＧｉｎｅｒｔｅｓｔｍを決定する。
最後に、前記動力学的推定全体重心位置、関節角検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）を基に、ブロック３４７にて、グローバル座標系における上体位置を推定する。
以上が、第６実施形態におけるＳ０１６の自己位置推定処理のうちの、全体重心位置および上体位置を推定する手段である。
第６実施形態においても、空中期であるか否かを判定して、空中期である場合には、床反力センサ検出値を用いずに、全体重心は放物運動を行うという想定のもとで、前記動力学的推定全体重心加速度を決定しても良い。
ロボットが環境から外力を受けないことが明白な場合には、空中期においては、全体重心は放物運動を行うので、これにより、より一層精度良く全体重心位置を推定することができる。
なお、空中期であるか否かは、目標歩容の時期（フェーズ）、目標歩容の床反力あるいは、床反力検出値の少なくともいずれかを用いて判定すれば良い。
特に、全体重心位置の推定精度を極力高くしたい場合には、床反力検出値が所定の値以下であるか否かによって空中期であるか否かを判定すれば良い。
なお、以上説明した第６実施形態は、本発明の第１〜第６発明、第８、第９発明、並びに、第２３〜第３０発明に対応するものである。
前記各実施形態で決定した姿勢回転中心について、以下に補足する。いずれかの脚体２が接地し、十分大きい床反力が発生している時期においては、いわゆる支持多角形（接地面を含む最小凸多角形、ＺＭＰ存在可能範囲あるいは全床反力中心点存在可能範囲）の中に、姿勢回転中心があると考えられる。
この理由を以下に述べると、姿勢回転中心が支持多角形の外にあると、足平２２の底面のあらゆる点が床に対して相対移動していることになるので、足平２２の底面と床との間のあらゆる接触点には、動摩擦が働く。動摩擦は、ロボット１が床反力を制御しようとして、脚体２を動かしても、一定値のままで、任意の値に制御することはできない。すなわち、氷の上でつるつる滑ってうまく歩けない状態と同じである。したがって、ロボット１が床反力を制御しつつ安定に移動している場合には、足平２２の底面と床との間のある領域においては、滑りが生じないで静摩擦が作用していると考えれる。すなわち、支持多角形の中に、姿勢回転中心があると考えられる。補足すると、ロボット１の足平２２が完全剛体であるならば、姿勢回転中心点以外のあらゆる接触点は滑りが生じるが、実際の足平２２の底面は、ゴムなどの弾性体でできているので、姿勢回転中心の近傍でも滑りは生じないと考えれる。
また、空中期においては、ロボットは、重心を中心として摂動運動をすると考えられる。
また、上記以外の時期、すなわちいずれかの脚体２が接地しているが、床反力が十分に発生していない時期においては、運動の連続性を考慮すると、姿勢回転中心は、支持多角形と全体重心位置（あるいは上体代表点位置）との間に存在すると考えられる。
以上の考察から、第３実施形態以降で実行されるＳ２２０４（図１４参照）において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい。
現在時刻ｔにおける
ａ）目標ＺＭＰ、
ｂ）実ＺＭＰ（すなわち、実床反力中心点、あるいは床反力の圧力中心点）、
ｃ）支持脚座標系の所定の点、例えば原点
ｄ）全体重心点、
ｅ）上体代表点
ｆ）上記のいずれかの複数の点の内分点
いずれかの脚が接地し、十分大きい床反力が発生している時期においては、姿勢回転中心は、支持多角形の中に設定するのが良い。具体的には、上記ａ）およびｂ）に設定すれば良い。あるいは、姿勢回転中心が支持多角形の中に含まれるように、上記ｃ）における所定の点を設定すれば良い。例えば支持脚座標系の原点（通常、足首関節の下）に設定すれば良い。
空中期においては、姿勢回転中心は、上記ｄ）のように設定するのが良いが、全体重心位置は上体代表点位置の近くに存在するので、上記ｅ）のように設定しても良い。
上記以外の時期、すなわちいずれかの脚体２が接地しているが、床反力が十分に発生していない時期においては、上記ｆ）のように設定するのが良い。
さらに、全期間での運動の連続性を考慮すると、姿勢回転中心点は、連続的に変化するように設定するのが望ましい。
いずれにしても、概ねすべての各瞬間において、ロボット１の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。
同様に、第１実施形態の図１０のＳ２００６および第２実施例の図１２のＳ２１０６において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい。
ａ）遊脚着地時刻ｎの、あるいは１歩の間の平均の目標ＺＭＰ位置
ｂ）遊脚着地時刻ｎの、あるいは１歩の間の平均の実ＺＭＰ（すなわち、実床反力中心点、あるいは床反力の圧力中心点）位置
ｃ）支持脚座標系の所定の点、例えば原点
ｄ）遊脚着地時刻ｎの、あるいは１歩の間の平均の全体重心点位置
ｅ）遊脚着地時刻ｎの、あるいは１歩の間の平均の上体代表点位置
ｆ）上記のいずれかの複数の点の内分点
いずれにしても、ロボット１の全体が最新の１歩の間の運動によって掃引した領域を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。
または、ある瞬間における、例えば着地瞬間における、ロボット１の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。
第４実施形態においては、慣性航法的推定上体位置を幾何学的推定上体位置で補正する際に、慣性航法的推定上体位置を算出するための積分器（図１７のブロック３２０）に補正入力（図１７のブロック３１８の出力）を付加的に加えていた。したがって、慣性航法的推定上体位置は、制御周期毎に修正された。これに対し、ある所定の期間、補正を加えずに慣性航法的推定上体位置を決定し、次の式４５により、慣性航法的推定上体位置と幾何学的推定上体位置とを内挿した推定上体位置である内挿的推定上体位置を求めて、これを最終的に決定した推定上体位置として出力するようにしても良い。
内挿的推定上体位置
＝  α＊慣性航法的推定上体位置＋  （１−α）＊幾何学的推定上体位置
…式４５
ただし、αは加重平均の重みであり、前記所定の期間の間に１から徐々に０に変化する。
αが０になったときに、幾何学的推定上体位置を慣性航法的推定上体位置に代入（上書き）し、αを１に戻す。
また、所定の期間は、空中期以外の期間に設定する。
第５および６実施形態においても、第４実施形態と同様にして、慣性航法的あるいは動力学的推定全体重心位置と幾何学的推定全体重心位置とを内挿した推定全体重心位置である内挿的推定全体重心位置を決定するようにしても良い。
第４〜第６実施形態において、補正される慣性航法的推定上体位置もいくは慣性航法的推定重心位置もしくは動力学的推定重心位置は鉛直位置成分（離床面に対する）のみであっても良い。あるいは水平成分のみ補正しても良い。
補正ゲインＫａ，Ｋｂ，ＫｃもしくはＫｄの値を、定常カルマンフィルタあるいは非定常カルマンフィルタの手法を用いて決定しても良い。ただし、本実施形態のロボット１においては、特に移動中においては、システム雑音（外乱）と観測雑音の性質が、カルマンフィルタの前提条件を十分に満足してはいないので、十分な効果を生むとは限らない。
加速度センサおよびまたはジャイロセンサは、上体３以外の部位、例えば頭部４に搭載（内蔵）されていても良い。さらに頭部４と上体３との間に首関節がある場合、首関節の変位指令（目標変位）または変位検出値を基に、加速度センサおよびまたはジャイロセンサの検出値をキネマティクス演算によって、上体代表点の加速度と角加速度とに変換しさえすれば、残りは、前記各実施形態と同様に自己位置推定を行えば良い。
推定位置姿勢の成分（位置鉛直成分、位置水平成分、鉛直軸まわりの向き成分、傾き成分）によって、第１実施形態から第６実施形態までの自己位置推定処理のうちの異なる処理を選択しても良い。例えば、上体３の水平位置に関しては、第３実施形態の自己位置推定処理に従って幾何学的に推定し、鉛直位置に関しては、第４実施形態の自己位置推定処理に従って慣性航法的推定上体鉛直位置を幾何学的推定上体鉛直位置により補正しても良い。
推定支持脚座標系および幾何学的推定上体位置を決定する際に関節変位を用いる実施形態においては、その関節変位を、目標歩容の関節変位または、関節変位検出値としたが、これらの重み付き平均でも良い。また、このときの重みは周波数特性を持たせても良い。
推定支持脚座標系や推定上体位置姿勢など、推定される位置姿勢を表現する時には、前記各実施形態のごとくグローバル座標系を基準として表現する代わりに、目標位置姿勢からの摂動分で表現しても良い。
第４実施形態以降においては、前記図１７に示すごとく、グローバル座標系における推定重力加速度を基に、グローバル座標系における推定上体姿勢誤差角度を求め、グローバル座標系における推定上体姿勢誤差角度にゲインＫｂを乗じた値を、グローバル上体角速度ωｇｌを積分する積分器（図１７のブロック３１０）に付加的に入力することによって、推定上体姿勢を補正していた。すなわち、グローバル座標系において、推定上体姿勢を補正していたが、この代わりに、ジャイロセンサのローカル座標系（上体３に固定された座標系）上で補正しても良い。具体的には、図１７において、ゲインＫｂのブロック３０９と、その出力をグローバル上体角速度ωｇｌから減じる加算器（ブロック３０８）を削除し、積分器Ｋａ／Ｓ（ブロック３０６）を、Ｋａ／Ｓ＋Ｋｂ、すなわち、ＰＩ制御則のブロックに変更しても良い。
原理上、このようにローカル座標系で補正しても、ヨーレートが高い旋回（その場旋回含む）時に、Ｋｂによって傾斜ドリフトの収束が悪くなることはない。
推定上体姿勢の補正において、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じることにより推定重力加速度を求める代わりに、加速度センサ検出値グローバル座標系換算値から目標歩容の上体加速度を減じることにより推定重力加速度を求めるようにしても良い。
実際には、目標歩容通りに運動しようとしているロボットには、姿勢回転中心まわりの姿勢回転が発生しているので、姿勢回転によって、上体加速度は、目標歩容からずれる。しかし、姿勢回転の内の姿勢傾斜は、前後左右に振動しても平均的には、０である。また、通常、姿勢回転の内のスピンに関しては、１歩毎にスピンの向きが逆に切り替わるので、平均的には０に近い。したがって、遠心力など、姿勢回転の回転方向によらずほぼ同一方向に作用する力を除けば、姿勢回転による上体加速度への影響は、正の影響と負の影響とが相殺し、長期的には、ほぼ０になる。なお、ここで、「長期」とは、推定上体姿勢の補正の整定時間以上の時間を指す。
よって、推定上体姿勢の補正のために、幾何学的推定上体加速度を用いる代わりに、目標歩容の上体加速度を用いても、補正の効果が大きく低下することはない。
第１から第３実施形態においても、第４実施形態のように、幾何学的推定上体位置姿勢運動から算出される運動加速度と、加速度センサの検出値を用いて、ジャイロセンサの検出ドリフトを補正するようにしても良い。また、第１から第３実施形態においても、第４実施形態のように、推定上体姿勢のうちの傾き成分のドリフトを補正するようにしても良い。また、第１から第３実施形態においても、第４実施形態のように、ヨーレート補正を行っても良い。
次に本発明の第７実施形態を図２３〜図２５を参照して説明する。図２３および図２４は、第７実施形態におけるロボット１の頭部４の内部構造を表す。図２３は正面視、図２４は側面視の図である。頭部４は、パン・チルト方向に回転する首関節１２０を介して、上体３の上部に連接されている。
首関節１２０にも、他の関節と同じく、エンコーダ（関節変位検出器）付きのモータ１２１，１２２と減速機１２３，１２４とを備え、図示を省略するモータ制御装置を介して前記制御ユニット６０からの関節変位指令に追従するように制御される。
頭部４には、環境認識手段としての左右２台のビデオカメラ１２５，１２５が備えられ、対象物を立体視することができるようになっている。前記図５では図示を省略しているが、ビデオカメラ１２５，１２５の出力（撮像情報）は、制御ユニット６０に入力され、該制御ユニット６０で撮像情報内の対象物までの距離等が認識される。
左右２台のビデオカメラ１２５，１２５の代わりに、以下のような環境認識手段を備えても良い。
ａ）３台以上のカメラによる立体視
ｂ）１台カメラで対象物の多点を認識し、三角測量の原理で距離を推定する手段
ｃ）レンジファインダ、スキャン式レーザー距離計等の非接触多点距離計測装置
第７実施形態は、上記した頭部４の構成の他、図９のＳ０１６の自己位置推定処理が第４実施形態と相違している。その他の構成及び処理は、第４実施形態と同じである。なお、その他の構成及び処理は、第４実施形態の代わりに、例えば第５または第６実施形態と同じにしても良い。
図２５は第７実施形態における図９のＳ０１６の自己位置推定処理を示すフローチャートである。第７実施形態においては、第４実施形態における推定自己位置姿勢を用いて、対象物を前記ビデオカメラ１２５，１２５の画像（以下、単にカメラ画像という）の中央あるいは適切な位置に捉えるように、首関節１２０を制御して、ビデオカメラ１２５，１２５の方向を制御する注視制御を行う。また、ビデオカメラ１２５，１２５（あるいはレンジファインダ等）により、あらかじめグローバル座標系における正確な位置を記憶しているランドマーク等を認識して、前記推定自己位置姿勢を補正する。また、ビデオカメラ１２５，１２５により、ある対象物を認識して、ビデオカメラ１２５，１２５によって得た情報と前記推定自己位置姿勢から、対象物のグローバル座標系における位置姿勢または形状を認識する。
なお、第４実施形態における推定自己位置姿勢に代わりに、第３、第５および第６のいずれかの実施形態の推定自己位置姿勢を用いても良い。
以下に、図２５を用いて、第７実施形態のその動作の詳細を説明する。
まず、Ｓ２５００からＳ２５０４まで、第４実施形態のＳ２３００からＳ２３０４と同様の処理を行い、推定上体姿勢、推定支持脚座標系の位置姿勢、幾何学的推定上体位置および慣性航法的上体位置を決定する。
次いでＳ２５０６に進み、注視制御をすべきか否かを判断する。具体的には、現在の推定上体姿勢及び慣性航法的上体位置、並びにロボット１の目標経路および地図情報を基に、注視制御をすべきか否かを判断し、また、注視制御する場合には、いずれの対象物を注視するかを決定する。具体的には、地図情報に、各対象物に対して、その対象物を観測でき、かつロボット１と対象物の相対位置関係が測定できる領域（以降、観測可能領域と呼ぶ）が記述してある。そして、現在の推定上体姿勢及び慣性航法的上体位置と今後の目標経路とに基づき、現在および今後の所定の期間、いずれかの観測可能領域にいるか否かを予想し、しばらくの間、観測可能領域にいると予想される場合には、注視制御をすべきと判断する。
注視すべき対象物の指定を、あらかじめ、ロボット１の移動制御部（図６に示した部分）の上階層に位置するロボット１の行動計画部（図示しない）が、移動経路に応じて決定するか、あるいは、オペレータがマンマシンインターフェースを介して指定しておいても良い。
なお、地図情報は、ランドマーク、障害物、出発地、目的地、移動可能領域、道などに関する、位置、形状特徴および観測可能領域などの情報であり、移動前に、あらかじめメモリに記憶されているものとする。
次いでＳ２５０８に進み、前記注視すべき対象物の地図上の位置と前記推定上体位置姿勢との相対関係を求める。
次いでＳ２５１０に進み、前記相対関係から、カメラ画像の中央に対象物が映るように目標首関節変位をキネマティクス演算により決定する。
次いでＳ２５１２に進み、目標首関節変位に追従するように、首関節モータ１２１，１２２を制御する。具体的には、首関節モータ１２１，１２２のモータ制御装置（モータドライバ）に対して目標首関節変位を制御ユニット６０から出力する。そして、このとき首関節モータ１２１，１２２のモータ制御装置（モータドライバ）が、目標首関節変位に首関節変位検出値を追従させるように首関節モータ１２１，１２２の電流を制御する。
次いでＳ２５１４に進み、対象物がランドマークであるか否かを判定する。具体的には、検索した対象物の属性の情報からランドマークであるか否かを判定する。
Ｓ２５１４において肯定される場合には、Ｓ２５１６に進み、カメラ画像、地図情報、および推定上体位置姿勢、首関節変位から、幾何学演算によって、現在の実際のロボット１の支持脚足平位置姿勢に対応する支持脚座標系（以降、視覚推定支持脚座標系と呼ぶ）のグローバル座標系のおける位置姿勢を推定する。
Ｓ２５１６の後、さらにＳ２５１８に進み、前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差が０に収束するように、推定支持脚座標系の位置姿勢を修正する。
Ｓ２５１８の処理をより詳細に説明すると、まず前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差を求める。次いで、この差と、ある所定のゲインＫｅとの積を求め、該積を推定支持脚座標系の位置姿勢に加えた位置姿勢を新たな推定支持脚座標系の位置姿勢とする。
すなわち、次の式４６によって新たな推定支持脚座標系を決定する。言い換えると、前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との内挿（すなわち内分あるいは重み付き平均）によって、新たな推定支持脚座標系の位置姿勢を決定する。
新たな推定支持脚座標系
＝  推定支持脚座標系の位置姿勢
＋  Ｋｅ＊（視覚推定支持脚座標系の位置姿勢−推定支持脚座標系の位置姿勢）
…式４６
ゲインＫｅは、各推定値の確からしさを基に、決定される新たな推定支持脚座標系が、真値として最も確率が高くなる値になるように決定しても良い。
Ｓ２５１４において否定される場合には、Ｓ２５２０に進み、カメラ画像、地図情報、および推定上体位置姿勢、首関節変位から対象物形状・位置を算出する。以降、前記算出された対象物形状・位置を視覚推定対象物形状・位置と呼ぶ。
Ｓ２５２０の後、Ｓ２５２２に進み、視覚推定対象物形状・位置により、地図情報の対象物形状・位置を修正する。具体的には、視覚推定対象物形状・位置と地図情報の対象物形状・位置との差が０に収束するように地図情報の対象物形状・位置を修正する。
より具体的には、まず前記視覚推定対象物形状・位置と地図情報の対象物形状・位置との差を求める。次いで、その差と、ある所定のゲインＫｆとの積を求め、この積を地図情報の対象物形状・位置に加えた形状・位置を新たな地図情報の対象物形状・位置とする。
すなわち、次の式４７によって新たな地図情報の対象物形状・位置を決定する。言い換えると、前記視覚推定対象物形状・位置と地図情報の対象物形状・位置の内挿（すなわち内分あるいは重み付き平均）によって、新たな地図情報の対象物形状・位置を決定する。
新たな地図情報の対象物形状・位置
＝  地図情報の対象物形状・位置
＋  Ｋｆ＊（視覚推定対象物形状・位置−地図情報の対象物形状・位置）
…式４７
Ｓ２５１８またはＳ２５２２の処理の完了をもって、Ｓ０１６の自己位置推定処理の完了とする。
視覚推定支持脚座標系の位置は、慣性航法的推定上体位置を基に推定されており、さらに、慣性航法的推定上体位置は、推定支持脚座標系を基に算出される幾何学的推定上体位置姿勢に収束するように決定されるので、Ｓ２５１８の処理により、推定支持脚座標系の位置姿勢を変更すると、視覚推定支持脚座標系の位置も遅れて変更される。したがって、Ｓ２５１８の処理においては、収束を速めようとしてゲインＫｅを大きくし過ぎると発振する恐れがある。ゲインＫｅを大きくしても、発振しにくくするためには、視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差に応じて推定支持脚座標系の位置姿勢を修正すると共に、直接的に慣性航法的推定上体位置も修正して、視覚推定支持脚座標系の位置の変更の遅れを短くすれば良い。
推定上体位置姿勢と視覚推定上体位置姿勢の差、または、視覚推定対象物形状・位置と地図情報の対象物形状・位置の差がある所定の許容値を越えているならば、誤認識と判断して、その値による推定上体位置姿勢または地図情報の対象物形状・位置の修正をしないようにしても良い。
なお、以上説明した第７実施形態は、本発明の第３１〜第３７発明に対応するものである。
以上説明した如く、第１実施形態においては、実際のロボット１の挙動を、ロボット１の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、１歩の間に、姿勢検出手段によって検出あるいは推定された姿勢と目標姿勢の差（あるいは前記差の鉛直成分）の分だけ、姿勢回転中心決定手段によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち着地点の位置と向きとを、１歩毎（着地毎）に推定するようにしたので、着地点の位置と向きを精度良く推定することができる。特に、加速度が水平方向や鉛直方向に激しく変動している状態、走行時の空中期などのように全脚体２，２が床から離れている状態、または足平２２と床との間の回転滑りなどによってロボット１の全体に姿勢回転（またはスピン）が生じている状態においても、着地点の位置と向きを精度良く推定することができる。
第２実施形態においては、実際のロボット１の挙動を、ロボット１の運動を記述するローカル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動しているロボットが、１歩の間に、姿勢検出手段によって検出あるいは推定された姿勢と目標姿勢の差の分だけ、姿勢回転中心決定手段によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち着地点の位置と向きを、１歩毎（着地毎）に推定するようにしたので、着地点の位置と向きをより一層精度良く推定することができる。
第３実施形態においては、実際のロボット１の挙動を、ロボット１の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、各瞬間に、姿勢検出手段によって検出あるいは推定された上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差の変化速度で、すなわち推定上体姿勢変化速度と目標歩容の上体姿勢変化速度の差の変化速度で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、新たな推定支持脚座標系と推定上体位置姿勢を各瞬間（制御周期毎）に決定（更新）する。また、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向きを、１歩毎（着地毎）に推定したようにしたので、着地点の位置と向きをより一層精度良く推定すると共に、ロボット１の自己位置姿勢を連続的に精度良く推定することができる。
第４実施形態においては、慣性航法によって推定上体位置（慣性航法的推定上体位置）と推定上体姿勢とを求めると共に、第３実施形態によって決定される推定上体位置である幾何学的推定上体位置を用いて慣性航法的な推定上体位置及び推定上体姿勢を補正するようにしたので、ロボット１の自己位置姿勢ならびに着地位置・向き（推定支持脚座標系の位置・向き）をより一層精度良く推定することができる。
第５実施形態においては、第４実施形態において慣性航法的推定上体位置Ｘｉｎｅｒｔｅｓｔｍを決定する代わりに、慣性航法による推定全体重心位置を、制御周期毎に決定するようにしたのようにしたので、ロボット１の自己位置姿勢ならびに着地位置・向き（推定支持脚座標系の位置・向き）を第４実施形態と同様に、精度良く推定することができる。
第６実施形態においては、第５実施形態において加速度センサ検出値を基に慣性航法的推定全体重心位置を算出する代わりに、床反力センサ検出値を基に動力学的推定全体重心位置を算出するようにしたのようにしたので、ロボット１の自己位置姿勢ならびに着地位置・向き（推定支持脚座標系の位置・向き）を第５実施形態と同様に、精度良く推定することができる。
第７実施形態においては、幾何学自己位置推定演算を用いることによって、あるいは幾何学自己位置推定演算と慣性航法演算を複合的に用いることによって、連続的に（制御周期毎に）推定される自己位置姿勢と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物のグローバル座標系における位置を基に、ロボット１に搭載されたビデオカメラ１２５，１２５等の環境認識手段が注視すべき方向を決定しているので、前記対象物を、連続的に前記環境認識手段の画像の中央あるいは適切な位置に捉えることができる。
また、前記環境認識手段が前記対象物を認識する時に、前記環境認識手段によって得た情報と前記推定した自己位置姿勢から、グローバル座標系における対象物の位置姿勢または形状を精度良く認識することができる。
また、さらに、前記対象物の地図上の位置・形状を連続する複数の画像を基に補正するので、ノイズや誤認識の影響を受けにくい。
また、前記環境認識手段が、ランドマーク等あらかじめ正確な位置情報を記憶している対象物の位置情報と、前記環境認識手段によって得た前記対象物に対するロボット１の相対位置情報に基づき、幾何学自己位置推定演算を用いることによって推定した自己位置姿勢、あるいは幾何学自己位置推定演算と慣性航法演算を複合的に用いることによって推定した自己位置姿勢を補正し、前記自己位置姿勢の推定の精度を高めることができる。
また、さらに、前記自己位置姿勢を連続する複数の画像を基に補正するので、ノイズや誤認識の影響を受けにくい。
また、第４実施形態においては、運動加速度の影響を受けにくく、精度良くロボット１の姿勢傾斜を推定することができる。また、これは他の実施形態にも適用できる。
また、第４実施形態においては、ヨーレートが補正され、より一層精度良くロボット１の姿勢（特に水平方向成分の向き）並びに位置を推定することができる。また、これは他の実施形態にも適用できる。
産業上の利用可能性
以上の如く、本発明は、２足移動ロボット等の脚式移動ロボットの位置を精度よく推定することができる技術を提供するものとして有用である。
【図面の簡単な説明】
図１は、本発明の実施形態における脚式移動ロボットとしての２足移動ロボットの全体的構成の概略を示す概略図、図２は図１の各脚体の足平部分の構成を概略的に示す概略図、図３及び図４はそれぞれ各脚体の足平部分の詳細構成を示す側面視の断面図及び底面図、図５は図１のロボットに備えた制御ユニットの構成を示すブロック図、図６は図５の制御ユニットの機能的構成を示すブロック図である。図７はロボットの走行歩容を示す説明図、図８（ａ），（ｂ），（ｃ）はそれぞれ目標歩容の床反力鉛直成分、目標ＺＭＰ、自己位置姿勢推定用のゲインの設定例を示すグラフ、図９は第１実施形態における制御ユニットの要部の処理を示すフローチャート、図１０は図９のフローチャートの自己位置姿勢推定処理を示すフローチャート、図１１は図９のフローチャートの自己位置姿勢推定処理を説明するための図である。図１２は第２実施形態の自己位置姿勢推定処理を示すフローチャート、図１３は第２実施形態の自己位置姿勢推定処理を説明するための図である。図１４は第３実施形態の自己位置姿勢推定処理を示すフローチャート、図１５は第３実施形態の自己位置姿勢推定処理を説明するための図である。図１６は第４実施形態の自己位置姿勢推定処理を示すフローチャート、図１７は第４実施形態の自己位置姿勢推定処理の要部の処理を示すブロック図、図１８（ａ），（ｂ）は第４実施形態の自己位置姿勢推定処理を説明するための図、図１９は第４実施形態で用いるゲインの設定例を示すグラフである。図２０は第５実施形態の自己位置姿勢推定処理を示すフローチャート、図２１は第５実施形態の自己位置姿勢推定処理の要部の処理を示すブロック図である。図２２は第６実施形態の自己位置姿勢推定処理の要部の処理を示すブロク図である。図２３及び図２４は第７実施形態におけるロボットの頭部の内部構造をそれぞれ正面視、側面視で示す図、図２５は第７実施形態の自己位置姿勢推定処理を示すフローチャートである。
【図１】
2003090980.tif 000003
【図２】
2003090980.tif 000004
【図３】
2003090980.tif 000005
【図４】
2003090980.tif 000006
【図５】
2003090980.tif 000007
【図６】
2003090980.tif 000008
【図７】
2003090980.tif 000009
【図８（ａ）】
2003090980.tif 000010
【図８（ｂ）】
2003090980.tif 000011
【図８（ｃ）】
2003090980.tif 000012
【図９】
2003090980.tif 000013
【図１０】
2003090980.tif 000014
【図１１】
2003090980.tif 000015
【図１２】
2003090980.tif 000016
【図１３】
2003090980.tif 000017
【図１４】
2003090980.tif 000018
【図１５】
2003090980.tif 000019
【図１６】
2003090980.tif 000020
【図１７】
2003090980.tif 000021
【図１８（ａ）】
2003090980.tif 000022
【図１８（ｂ）】
2003090980.tif 000023
【図１９】
2003090980.tif 000024
【図２０】
2003090980.tif 000025
【図２１】
2003090980.tif 000026
【図２２】
2003090980.tif 000027
【図２３】
2003090980.tif 000028
【図２４】
2003090980.tif 000029
【図２５】
2003090980.tif 000030
【国際調査報告】
2003090980.tif 000031
2003090980.tif 000032