JP4246696B2 - 脚式移動ロボットの自己位置推定装置 - Google Patents

Description

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

Claims (30)

1. 決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
   前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢とのグローバル座標系での差である姿勢回転偏差であって、少なくともヨー方向の姿勢回転偏差成分を含む姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
   前記姿勢回転偏差の変化が前記ロボットの回転すべりによる変化であるとして、該姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
   前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記グローバル座標系でのロボットの位置の推定値たる推定位置を決定する位置推定手段とを備え、
   前記位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記グローバル座標系で前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見たロボットの位置の各座標成分の値と、決定しようとするロボットの推定位置を前記第２座標系から見た場合における該推定位置の各座標成分の値とが同一になるように前記グローバル座標系でのロボットの推定位置を決定することを特徴とする脚式移動ロボットの自己位置推定装置。
2. 前記所定の部位は、前記ロボットの上体であることを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
3. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点に前記回転中心を決定することを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
4. 前記姿勢回転偏差算出手段は、各瞬間の前記姿勢回転偏差変化量を逐次求める手段であり、前記位置推定手段は、その各瞬間の姿勢回転偏差変化量を用いて前記ロボットの各瞬間の推定位置を逐次決定することを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
5. 前記姿勢回転偏差算出手段は、前記ロボットの着地動作により該ロボットの脚体が着地する毎に、前回の脚体の着地から今回の脚体の着地までの期間における姿勢回転偏差変化量である着地間姿勢回転偏差変化量を求める手段であり、前記位置推定手段は、該着地間姿勢回転偏差変化量を用いて各着地毎に前記ロボットの推定位置を決定することを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
6. 前記位置推定手段が推定するロボットの位置は、該ロボットの着地動作により着地した脚体の接地位置であることを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
7. 前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれか と前記推定された変形量とから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見たロボットの位置の各座標成分の値と、決定しようとするロボットの推定位置を前記第２座標系から見た場合における該推定位置の各座標成分の値とが同一になるように前記グローバル座標系でのロボットの推定位置を決定することを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
8. 決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
   前記ロボットに搭載された、並進加速度を検出する加速度センサと、
   前記ロボットに搭載された、慣性空間に対する角速度を検出する角速度センサと、
   前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかを基に前記ロボットの所定の部位又は前記ロボットの全体重心のグローバル座標系での鉛直位置の幾何学的推定値としての第１の推定位置を決定する幾何学的位置推定手段と、
   少なくとも前記加速度センサの検出値と前記角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロボットの全体重心の前記グローバル座標系での鉛直位置の慣性航法的推定値としての第２の推定位置を決定する慣性航法的位置推定手段と、
   前記決定された第１の推定位置と前記決定された第２の推定位置との差からフィードバック制御則により、該差を０に近づけるための操作量を決定する手段とを備え、
   前記慣性航法的位置推定手段は、新たに決定しようとする第２の推定位置に、前記決定された操作量に応じた補正成分を付加して、該第２の推定位置を決定することを特徴とする脚式移動ロボットの自己位置推定装置。
9. 前記所定の部位は、前記ロボットの上体であることを特徴とする請求項８記載の脚式移動ロボットの自己位置推定装置。
10. 前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定した変形量と、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかとから前記第１の推定位置を決定することを特徴とする請求項８記載の脚式移動ロボットの自己位置推定装置。
11. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、新たに決定する第２の推定位置に付加する前記補正成分を０にすることを特徴とする請求項８記載の脚式移動ロボットの自己位置推定装置。
12. 決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
    前記ロボットに搭載された、並進加速度を検出する加速度センサと、
    前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢とのグローバル座標系での差である姿勢回転偏差であって、少なくともヨー方向の姿勢回転偏差成分を含む姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
    前記姿勢回転偏差の変化が前記ロボットの回転すべりによる変化であるとして、該姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
    前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の前記グローバル座標系での位置の幾何学的推定値としての第１の推定位置を決定する幾何学的位置推定手段と、
    少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は前記ロボットの全体重心の前記グローバル座標系での位置の慣性航法的推定値としての第２の推定位置を決定する慣性航法的位置推定手段と、
    前記決定された第１の推定位置と第２の推定位置との差からフィードバック制御則により、該差を０に近づけるための操作量を決定する手段とを備え、
    前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記グローバル座標系で前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置の各座標成分の値と、決定しようとする前記所定の部位又は全体重心の第１の推定位置を前記第２座標系から見た場合における該第１の推定位置の各座標成分の値とが同一になるように前記グローバル座標系での前記所定の部位又は全体重心の第１の推定位置を決定し、
    前記慣性航法的位置推定手段は、新たに決定しようとする第２の推定位置に、前記決定された操作量に応じた補正成分を付加して、該第２の推定位置を決定することを特徴とする脚式移動ロボットの自己位置推定装置。
13. 前記所定の部位は、前記ロボットの上体であることを特徴とする請求項１２記載の脚式移動ロボットの自己位置推定装置。
14. 前記ロボットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかと前記推定された変形量とから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置の各座標成分の値と、決定しようとする前記所定の部位又は全体重心の第１の推定位置を前記第２座標系から見た場合における該第１の推定位置の各座標成分の値とが同一になるように前記グローバル座標系での第１の推定位置を決定することを特徴とする請求項１２記載の脚式移動ロボットの自己位置推定装置。
15. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点に前記回転中心を決定することを特徴とする請求項１２記載の脚式移動ロボットの自己位置推定装置。
16. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、新たに決定しようとする第２の推定位置に付加する前記補正成分を０にすることを特徴とする請求項１２記載の脚式移動ロボットの自己位置推定装置。
17. 決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
    前記ロボットに作用する床反力を検出する床反力検出手段と、
    前記ロボットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢とのグローバル座標系での差である姿勢回転偏差であって、少なくともヨー方向の姿勢回転偏差成分を含む姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、
    前記姿勢回転偏差の変化が前記ロボットの回転すべりによる変化であるとして、該姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、
    前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の前記グローバル座標系での位置の幾何学 的推定値としての第１の推定位置を決定する幾何学的位置推定手段と、
    少なくとも前記床反力検出手段の検出値と前記実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記ロボットの全体重心の前記グローバル座標系での位置の推定値としての第２の推定位置を決定する動力学的位置推定手段と、
    前記決定された第１の推定位置と第２の推定位置との差からフィードバック制御則により、該差を０に近づけるための操作量を決定する手段とを備え、
    前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第１の座標系を前記グローバル座標系で前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第２の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置の各座標成分の値と、決定しようとする前記所定の部位又は全体重心の第１の推定位置を前記第２座標系から見た場合における該第１の推定位置の各座標成分の値とが同一になるように前記グローバル座標系での前記所定の部位又は全体重心の第１の推定位置を決定し、
    前記動力学的位置推定手段は、新たに決定しようとする第２の推定位置に、前記決定された操作量に応じた補正成分を付加して、該第２の推定位置を決定することを特徴とする脚式移動ロボットの自己位置推定装置。
18. 前記所定の部位は、前記ロボットの上体であることを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。
19. 前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかと前記推定された変形量とから前記ロボットの回転すべりが無いと仮定して把握される、前記第１座標系から見た前記所定の部位又は全体重心の位置の各座標成分の値と、決定しようとする前記所定の部位又は全体重心の第１の推定位置を前記第２座標系から見た場合における該第１の推定位置の各座標成分の値とが同一になるように前記グローバル座標系での第１の推定位置を決定することを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。
20. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標ＺＭＰのいずれかの点に前記回転中心を決定することを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。
21. 前記ロボットの目標歩容は、ロボットの全脚体を空中に浮かせる空中期を有する歩容であり、前記動力学的位置推定手段は、前記空中期では、新たに決定しようとする第２の推定位置に付加する前記補正成分を０にすることを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。
22. 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物に対するロボットの相対位置関係情報を基に、前記所定の部位の実姿勢の検出値又は推定値と、前記ロボットの推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
23. 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求項８記載の脚式移動ロボットの自己位置推定装置。
24. 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロボットとの相対位置関係情報を基に、前記第１の推定位置と第２の推定位置と前記所定の部位の実姿勢の検出値又は推定値とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求項１２又は１７記載の脚式移動ロボットの自己位置推定装置。
25. 少なくとも、前記位置推定手段により推定された前記ロボットの位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
26. 少なくとも、前記慣性航法的位置推定手段により決定された第２の推定位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求項８又は１２記載の脚式移動ロボットの自己位置推定装置。
27. 少なくとも、前記動力学的位置推定手段により決定された第２の推定位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。
28. 少なくとも前記ロボットの推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求項１記載の脚式移動ロボットの自己位置推定装置。
29. 少なくとも前記慣性航法的位置推定手段により決定された第２の推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求項８又は１２記載の脚式移動ロボットの自己位置推定装置。
30. 少なくとも前記動力学的位置推定手段により決定された第２の推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロボットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求項１７記載の脚式移動ロボットの自己位置推定装置。

Images