WO2003090980A1

WO2003090980A1 - Dispositif d'estimation automatique de position pour robots mobiles montes sur des jambes

Info

Publication number: WO2003090980A1
Application number: PCT/JP2003/005448
Authority: WO
Inventors: Toru Takenaka; Takashi Matsumoto
Original assignee: Honda Giken Kogyo Kabushiki Kaisha
Priority date: 2002-04-26
Filing date: 2003-04-28
Publication date: 2003-11-06
Also published as: DE60332233D1; JPWO2003090979A1; DE60336519D1; KR100956539B1; KR20050000410A; EP1508408B1; KR100956520B1; EP1504855A4; EP1502712A1; EP1504855A1; JPWO2003090980A1; US20050126833A1; JP4181114B2; EP2106886B1; EP1508408A1; DE60336518D1; EP1502712B1; WO2003090981A1; JP4181113B2; EP1508408A4

Description

明細書脚式移動ロポッ卜の自己位置推定装置技術分野

本発明は、脚式移動ロポッ卜の自己位置を推定する装置に関する。背景技術

脚式移動ロボットでは、移動中に脚体を振ることによる反力によって足平（脚体先端部）と床の間の摩擦力が限界となって、足平と床の間で回転滑り（スピン）が生じ、口ポット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれるという問題が従来よりあった。

補足すると、上体が常に鉛直（直立）を維持して、直線歩行する目標歩容だけが生成されるとは限らない。目標歩容においても、口ポット全体または上体は旋回したり、前後左右に傾いたりする。すなわち、目標歩容においても、全体の姿勢回転（または上体等の代表部位の姿勢回転）が存在する。そこで、本明細書では、目標歩容における姿勢回転を目標姿勢回転と呼ぶ。本明細書で主に問題とする現象は、実際のロポットの全体の姿勢回転（または上体等の代表部位の姿勢回転）が、前記目標姿勢回転からずれることである。この現象を、厳密に表現するならば.

「目標姿勢回転からの摂動」あるいは「姿勢回転摂動」と呼ぶべきであるが、目標姿勢回転と混同する恐れがない場合には、以降、これを「姿勢回転」と省略する。

なお、以降、特に、口ポット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれる現象をスピンと呼ぶ。

また、口ポットの走行を行う場合には、全脚体が床から離れて空中に存在する時期（空中期）において、ロボットの鉛直方向の位置も目標歩容からずれ易かつた。

ロボッ卜の目標歩容の軌道、すなわち目標経路からロポットがずれないようにするためには、自己位置 ·姿勢を精度良く推定することが必要である。

また、ビデオカメラ等の環境認識装置を搭載していても、地図情報にあらかじめ登録してあるランドマ一ク等の注視すべき対象物を画像のある所定の位置（画像の中央等）に捉えるためには、自己位置 ·姿勢を精度良く推定することが必要である。

また、ビデオカメラ等の環境認識装置によって、環境に設定されたグ口一バル座標系（口ポットの移動環境の床に対して固定された座標系）における対象物の位置を計測するためには、グローバル座標系における自己位置 ·姿勢を精度良く推定することが必要である。

ビデオ力メラ等の環境認識装置を用いないで、自己位置，姿勢を推定する方法としては、ロケットなどの誘導に用いられている慣性航法装置が知られている。

しかし、慣性航法装置においては、加速度センサの検出値を 2階積分することにより位置が推定されるので、積分誤差の蓄積が大きくなりやすい。そのため、加速度センサに極めて高い検出精度が要求される。また、慣性航法装置を用いるためには、初期状態において口ポットを静的に床（地面）に固定することによって、口ポットの位置、速度の初期値を 0 としてから、移動を開始する必要がある。しかし、脚式移動ロポッ卜においては、直立静止時でも、姿勢を安定化するために、常に各関節が微小ながら動き続けなければならないので、完全静止状態にすることが困難であった。したがって、位置、速度の初期値が完全には 0にならない状態から慣性航法演算を開始することとなり、推定誤差が大きくなりがちであった。

また、ロケットや航空機においては、衛星、地上のレーダーやビーコンなど、外部装置との相対位置関係から自己位置姿勢を修正するグロ一バル ' ポジショニング ' システム（G P S ) が用いられている。ロボットにおいて、同様のシステムを導入することも可能ではあろうが、ロボットの足平（脚先端部）の床からの高さ等をミリメートルやセンチメ一トルのオーダーで測定する程の精度を低コス卜で実現することは困難である。

本発明は、かかる背景に鑑みてなされたものであり、ロボットの自己位置を精度良く推定することができる自己位置推定装置を提供することを目的とする。特に、口ポットの運動加速度が水平方向や鉛直方向に激しく変動している状態、走行時などのように全脚体が床から離れている状態、または足底と床の間の回転滑りなどによってロポット全体に姿勢回転（またはスピン）が生じている状態においても、自己位置姿勢を精度良く推定することができる自己位置推定装置を提供することを目的とする。

また、口ポットの自己位置を精度よく推定した上で、その推定自己位置を利用して、ロポットに搭載されたビデオカメラ等の環境認識装置により対象物を環境認識装置の画像の適切な位置に捉えるように該環境認識装置の方向を制御する注視制御を適切に行うことをも可能とした自己位置推定装置を提供することを目的とする。

また、ランドマーク等あらかじめ正確な位置情報を有する対象物をビデォカメラ等の環境認識手段によって認識しつつ、それによつて得た情報を基にロボッ卜の推定自己位置の精度を高めることができる自己位置推定装置を提供することを目的とする。

さらに、推定した自己位置姿勢を利用することで、ビデオカメラ等の環境認識手段により、床面や障害物などの位置姿勢あるいは形状を精度良く認識することができる自己位置推定装置を提供することを目的とする。発明の開示

本発明の脚式移動ロポッ卜の自己位置推定装置の第 1発明は、前記の目的を達成するために、決定された目標歩容に追従するように制御される脚式移動ロポットにおいて、前記ロポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記口ポットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記ロポットの位置の推定値たる推定位置を決定する位置推定手段とを備えることを特徴とするものである。

この第 1発明では、前記ロボッ卜の所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の変化は、ある回転中心の回りにロポットの全体が前記姿勢回転偏差変化量で回転したことと同等であると想定される。実際、例えば口ポットの足平（脚体先端部）と床との間で回転すベり（スピン）が発生した場合には、それが、上記姿勢回転偏差の変化となって現れる, そして、本発明では、上記想定の基で、口ポットの推定位置を決定するので、上記回転すベり等によるロポットの実際の位置の変化を定量的に把握して、その実際の位置の推定値たる推定位置を決定できる。その結果、精度のよい口ポットの自己位置推定が可能となる。

なお、前記所定の部位の実姿勢の検出値又は推定値は、例えば傾斜計. ジャィ口センサ等のセンサを用いて検出もしくは推定したものでよいが本発明で求めるロポットの推定位置、加速度センサの加速度検出値等を補助的に用いて適宜、補正したものであってもよい。所定の部位の実姿勢の検出値又は推定値は、それを得る手段を問わず、基本的には所定の部位の実姿勢を比較的精度よく表すものであればよい。このことは、第 1発明以外の本発明においても同様である。

前記第 1発明では、より具体的には、前記位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え. 前記目標歩容の目標運動と前記ロポットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第 1座標系から見たロボット位置と前記第 2座標系から見たロポッ卜の推定位置とが同一になるようにグロ一バル座標系から見たロポットの推定位置を決定することが好ましい（第 2発明）。なお、グロ一バル座標系は口ポットが移動する環境の床（地面）に対して固定された座標系である。かかる第 2発明によれば、前記第 1座標系から見た口ポット位置、すなわち、第 1座標系上で、口ポットが回転すベり等を生じることなく、目標歩容の目標運動もしくは関節変位検出値もしくは関節変位目標値の通りに運動しているとした場合に該目標運動と関節変位検出値と関節変位目標値とのうちの少なくともいずれかから把握されるロポットの位置 (第 1座標系から見た口ポットの位置）と、該第 1座標系を前記回転中心回りに前記姿勢回転偏差変化量で回転させてなる第 2座標系から見たロポットの推定位置とが同一になる（各座標系上での位置の各座標成分が両座標系で同じになる）ように、グローバル座標系から見たロボットの推定位置が決定される。その結果、姿勢回転偏差の変化を適切に反映させてロボットの推定位置を決定でき、精度のよい推定位置を得ることができる。補足すると、前記第 1座標系は、例えばロボットの着地動作により着地する脚体の足平の接地面近傍等に該ロポットの着地毎に設定される座標系（後述の実施形態で説明する支持脚座標系）でもよいが、前記グローバル座標系と同じ座標系でもよい。

上記第 1又は第 2発明では、前記所定の部位は、口ポットの上体（脚体が延設される基体）であることが好適である（第 3発明）。これは、ロポットの回転すベり等による影響は、ロポッ卜の上体の姿勢の変化として現れやすいからである。

また、第 1〜第 3発明では、前記姿勢回転偏差は、少なくとも前記所定の部位のョ一方向の姿勢回転偏差成分を含むことが好適である（第 4 発明）。これは、口ポットの位置は、ョー方向の姿勢回転偏差の変化の影響を受け易いからである。

また、前記第 1〜第 4発明で、前記口ポットの目標歩容が、走行等のように、ロポットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記口ポットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Pのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第 5発明）。すなわち、空中期では、前記姿勢回転偏差の変化は、概ねロポットの重心を中心とする回転となる, 空中期以外の時期、すなわち、いずれかの脚体が接地している時期では. 前記姿勢回転偏差の変化の回転中心は、概ね実床反力中心点（口ポットに実際に作用する全床反力の中心点）および目標 Z M Pのいずれかの点の付近に存在する。従って、上記のように回転中心を決定することで、その回転中心が実状に合った適切なものとなり、ひいては、口ポットの推定位置の精度を適切に確保することができる。

なお、目標歩容が、口ポットの歩行等のように空中期を持たない歩容 (常時、いずれかの脚体が接地する歩容）であるときには、前記回転中心は、例えば実床反力中心点および目標 Z M Pのいずれかの点又はその近傍に決定すればよい。

第 1〜第 5発明の一つの形態では、例えば前記姿勢回転偏差算出手段は、各瞬間の前記姿勢回転偏差変化量を逐次求める手段であり、前記位置推定手段は、その各瞬間の姿勢回転偏差変化量を用いて前記ロポットの各瞬間の推定位置を逐次決定する（第 6発明）。これによれば、各瞬間の推定位置を逐次決定するので、ロポットの自己位置をほぼ連続的に把握することができる。なお、この第 6発明では、姿勢回転偏差変化量は、姿勢回転偏差の変化速度に相当するものとなる。

あるいは、他の一つの形態では、前記姿勢回転偏差算出手段は、前記ロポットの着地動作により該ロボッ卜の脚体が着地する毎に、前回の脚体の着地から今回の脚体の着地までの期間における姿勢回転偏差変化量である着地間姿勢回転偏差変化量を求める手段であり、前記位置推定手段は、該着地間姿勢回転偏差変化量を用いて各着地毎に前記口ポットの推定位置を決定する（第 7発明）。

これによれば、ロポットの位置の安定性が高いものとなるロポットの着地毎にロポットの推定位置を決定するので、その推定位置の信頼性を高めることできる。

また、第 1〜第 7発明では、前記位置推定手段が推定する口ポットの位置は、該ロポッ卜の着地動作により着地した脚体の接地位置であることが好適である（第 8発明）。口ポットの脚体の接地位置は、口ポットの上体等の部位に比べて微小変動を生じたりし難いため、安定性が高い ₍ 従って、その接地位置を口ポットの位置として推定することで、信頼性の高い推定位置を得ることができる。

さらに、前記第 1〜第 8発明では、前記口ポットに作用する床反力を検出する床反力検出手段を備え、前記位置推定手段は、前記検出された床反力を基に前記口ポットの変形量を推定し、少なくともその推定された変形量を用いて前記ロボットの推定位置を決定することが好適である (第 9発明）。これによれば、口ポットに作用する床反力によるロポットの足平等の変形量を考慮して口ポットの推定位置を決定するので、該推定位置の精度を高めることができる。

次に、本発明の自己位置推定装置の第 1 0発明は、決定された目標歩容に追従するように制御される脚式移動ロポットにおいて、前記ロポットに搭載された、並進加速度を検出する加速度センサと、前記ロボットに搭載された、慣性空間に対する角速度を検出する角速度センサと、前記目標歩容の目標運動と前記ロポットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかを基に前記ロボットの所定の部位又は前記ロポッ卜の重心の鉛直位置の幾何学的推定値としての第 1の推定位置を決定する幾何学的位置推定手段と、少なくとも前記加速度センサの検出値と前記角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロボッ卜の全体重心の鉛直位置の慣性航法的推定値としての第 2の推定位置を決定すると共に、少なくとも前記第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とするものである。

この第 1 0発明では、少なくとも前記目標歩容の目標運動と前記ロボッ卜の関節の変位検出値と該関節の変位目標値とを基に前記所定の部位又は前記ロポットの重心の鉛直位置の幾何学的推定値（キネマテイクス演算等によって求められる鉛直位置）としての第 1の推定位置が決定される。さらに、少なくとも加速度センサの検出値と角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロポッ卜の全体重心の鉛直位置の慣性航法的推定値としての第 2の推定位置が決定される _: そして、このとき、第 2の推定位置は、第 1の推定位置との差を基に補正される。つまり、慣性航法とは異なる幾何学的な手法によって決定される第 1の推定位置を用いて、慣性航法を基礎とする第 2の推定位置が補正される。これにより、前記第 2の推定位置に含まれやすいドリフト (積分誤差の蓄積）を防止しつつ、第 2の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の鉛直位置の推定精度を高めることができる。

かかる第 1 0発明では、前記第 3発明と同様、前記所定の部位は、前記ロポッ卜の上体であることが好ましい（第 1 1発明）。

また、第 1 0又は第 1 1発明では、前記口ポットに作用する床反力を検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロポットの変形量を推定し、その推定した変形量を用いて前記第 1の推定位置を決定することが好ましい（第 1 2発明）。これによれば、前記第 9発明と同様に床反力による口ポットの変形量を考慮して第 1の推定位置を決定するので、該第 1の推定位置の精度を高めることができる。その結果、前記第 2の推定位置のより適切な補正が可能となり、該第 2の推定位置の精度も高めることができる。

また、第 1 0〜第 1 2発明では、前記慣性航法的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2の推定位置を補正することが好ましい（第 1 3発明）。これにより、第 2推定位置の補正を、該第 2推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。

さらに、第 1 0〜第 1 3発明では、前記ロポットの目標歩容が、ロボットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記慣性航法的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることが好ましい（第 1 4発明）。すなわち、空中期では、口ポットが非接地となるので、口ポットの所定の部位又は全体重心の鉛直位置の幾何学的推定位置たる前記第 1の推定位置の精度が低下しゃすい。そこで、第 1 4発明では、空中期では、第 2の推定位置の補正量を 0にする。これにより、空中期で第 2の推定位置が、信頼性の低い第 1の推定位置を用いて不適切に補正されてしまうのを防止して、該第 2の推定位置の長期的な精度を安定して確保することができる。

次に、本発明の自己位置推定装置の第 1 5発明は、決定された目標歩容に追従するように制御される脚式移動ロポットにおいて、前記ロボットに搭載された、並進加速度を検出する加速度センサと、前記口ポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロポットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロポッ卜の全体重心の位置の推定値たる第 1の推定位置を決定する幾何学的位置推定手段と、少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は前記ロポットの全体重心の位置の推定値としての第 2の推定位置を算出すると共に、少なくとも第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とするものである。

この第 1 5発明では、前記第 1発明で説明した手法を用いて、ロポットの所定の部位又は全体重心の位置の推定値たる第 1の推定位置が決定される。なお、このとき、より具体的には、口ポットの着地動作により着地した脚体の接地位置を前記第 8発明で説明した如く求めた上で、その接地位置を基準として、目標歩容の目標運動、口ポットの関節の変位検出値、及び該関節の変位目標値のうちの少なくともいずれかを基に前記第 1の推定位置を決定することが好適である。そして、第 1 5発明では、少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は全体重心の位置の推定値としての第 2の推定位置が算出され、この第 2の推定位置が、第 1の推定位置との差を基に補正される。つまり、慣性航法を基礎とする第 2の推定位置が、ロポットの回転すベり等によるロポットの位置変動を考慮して幾何学的に決定される第 1の推定位置を用いて補正される。これにより、前記第 2の推定位置に含まれやすいドリフト (積分誤差の蓄積）を防止しつつ、第 2の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の位置の推定精度を高めることができる。

かかる第 1 5発明では、より具体的には、前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第 1座標系から見た前記所定の部位又は全体重心の位置と前記第 2座標系から見た前記所定の部位又は全体重心の第 1の推定位置とが同一になるようにグロ一バル座標系から見た前記所定の部位又は全体重心の第 1の推定位置を決定することが好ましい（第 1 6発明）。これにより、第 1の推定位置を決定することに関して、前記第 2発明と同様の作用効果を奏することができる。

また、第 1 5又は第 1 6発明では、前記第 3発明と同様に、前記所定の部位は、前記口ポットの上体であることが好ましい（第 1 7発明）。さらに、第 1 5〜第 1 7発明では、前記第 4発明と同様に、前記姿勢回転偏差は、少なくとも前記所定の部位のョー方向の姿勢回転偏差成分を含むことが好ましい（第 1 8発明）。

また、第 1 5〜第 1 8発明では、前記口ポットに作用する床反カを検出する床反力検出手段を備え、前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第 1の推定位置を決定することが好ましい（第 1 9発明）。これによれば、前記第 9発明と同様に床反力による口ポットの変形量を考慮して第 1の推定位置を決定するので、該第 1の推定位置の精度を高めることができる。その結果、前記第 2の推定位置のより適切な補正が可能となり、該第 2の推定位置の精度も高めることができる。

また、第 1 5〜第 1 9発明では、前記ロポットの目標歩容が、ロポットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記口ポットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Pのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第 2 0発明）。これによれば、前記第 5発明と同様、前記回転中心を実状に合った適切なものとして、前記第 1の推定位置の精度を適切に確保することができる。

さらに、第 1 5〜第 2 0発明では、前記慣性航法的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2.の推定位置を補正することが好ましい（第 2 1発明）。これにより . 前記第 1 3発明と同様、第 2推定位置の補正を、該第 2推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。

また、第 1 5〜第 2 1発明では、前記ロボットの目標歩容は、ロポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることが好ましい（第 2 2発明）。

これによれば、前記第 1 4発明と同様、空中期では、第 2の推定位置の補正量を 0にするので、空中期で第 2の推定位置が、信頼性の低い第 1の推定位置を用いて不適切に補正されてしまうのを防止して、該第 2 の推定位置の長期的な精度を安定して確保することができる。

次に本発明の自己位置推定装置の第 2 3発明は、決定された目標歩容に追従するように制御される脚式移動ロポッ卜において、前記ロボットに作用する床反力を検出する床反力検出手段と、前記ロポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段、前記口ポットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロポットの全体重心の推定位置としての第 1の推定位置を決定する幾何学的位置推定手段、少なくとも前記床反力検出手段の検出値と前記実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記ロボットの全体重心の推定位置としての第 2の推定位置を算出すると共に、少なくとも第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する動力学的位置推定手段とを備えることを特徴とするものである。

この第 2 3発明では、前記第 1 の推定位置は、前記第 1 5発明と全く同様に決定される。一方、第 2の推定位置は、少なくとも前記床反カ検出手段の検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に, 動力学演算によって前記所定の部位又は前記ロポットの全体重心の推定位置としての第 2の推定位置が算出され、この第 2の推定位置が第 1の推定位置との差を基に補正される。ここで、床反力を口ポットの全体質量で割った値から、重力加速度を減算したものの符号を反転させたものが、前記第 1 5発明でいうところの加速度検出値に相当するものとなるので、結果的には、第 1 5発明と同様に第 2の推定位置が決定 ·補正されることとなる。従って、第 2 3発明によれば、第 1 5発明と同様に、前記第 2の推定位置に含まれやすいドリフト（積分誤差の蓄積）を防止しつつ、第 2の推定位置の精度を高めることができる。すなわち、前記所定の部位又は全体重心の位置の推定精度を高めることができる。

かかる第 2 3発明では、より具体的には、前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロポッ卜の関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第 1座標系から見た前記所定の部位又は全体重心の位置と前記第 2座標系から見た前記所定の部位又は全体重心の第 1の推定位置とが同一になるようにグロ一バル座標系から見た前記所定の部位又は全体重心の第 1の推定位置を決定することが好ましい（第 2 4発明）。これにより、第 1の推定位置を決定することに関して、前記第 2発明と同様の作用効果を奏することができる。

また、第 2 3又は第 2 4発明では、前記第 3発明と同様に、前記所定の部位は、前記口ポットの上体であることが好ましい（第 2 5発明）。

さらに、第 2 3〜第 2 5発明では、前記第 4発明と同様に、前記姿勢回転偏差は、少なくとも前記所定の部位のョー方向の姿勢回転偏差成分を含むことが好ましい（第 2 6発明）。

また、第 2 3〜第 2 6発明では、前記幾何学的位置推定手段は、前記検出された床反力を基に前記口ポットの変形量を推定し、その推定された変形量を用いて前記第 1の推定位置を決定することが好ましい（第 2 7発明）。これによれば、前記第 9発明と同様に床反力による口ポットの変形量を考慮して第 1の推定位置を決定するので、該第 1の推定位置の精度を高めることができる。その結果、前記第 2の推定位置のより適切な補正が可能となり、該第 2の推定位置の精度も高めることができる < また、第 2 3〜第 2 7発明では、前記ロポットの目標歩容が、ロポットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記回転中心決定手段は、前記空中期では、前記口ポットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Pのいずれかの点又はその近傍に前記回転中心を決定することが好ましい（第 2 8発明）。これによれば、前記第 5発明と同様、前記回転中心を実状に合った適切なものとして、前記第 1の推定位置の精度を適切に確保することができる。

さらに、第 2 3〜第 2 8発明では、前記動力学的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2の推定位置を補正することが好ましい（第 2 9発明）。これにより、前記第 1 3発明と同様、第 2推定位置の補正を、該第 2推定位置の精度が高まる（誤差を小さくする）ように適切に行うことができる。

また、第 2 3〜第 2 9発明では、前記ロポットの目標歩容が、ロポットの全脚体を空中に浮かせる空中期を有する歩容であるときには、前記動力学的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることが好ましい（第 3 0発明）。

さらに、前述した第 1〜第 9発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物に対するロポットの相対位置関係情報を基に、前記所定の部位の実姿勢の検出値又は推定値と、前記ロポットの推定位置とのうちの少なくともいずれかを補正することが好ましい（第 3 1発明）。

同様に、前記第 1 0〜第 1 4発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物とロポッ卜との相対位置関係情報を基に、前記第 1の推定位置と第 2の推定位置とのうちの少なくともいずれかを補正する手段を備えることが好ましい（第 3 2発明）。

同様に、前記第 1 5〜第 2 9発明では、少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された前記対象物と口ポットとの相対位置関係情報を基に、前記第 1の推定位置と第 2の推定位置と前記所定の部位の実姿勢の検出値又は推定値とのうちの少なくともいずれかを補正する手段を備えることが好ましい（第 3 3発明）。

これらの第 3 1〜第 3 3発明によれば、地図上で、位置情報が明確に確定しているランドマーク等の対象物を環境認識手段によって認識し、その対象物と口ポットとの相対位置関係情報を基に、前記推定位置（第 1 0〜第 3 0発明では第 1 もしくは第 2の推定位置）、あるいは該推定位置を決定するために必要な前記所定の部位の実姿勢の検出値又は推定値（第 1〜第 9発明、又は第 1 5〜第 3 0発明の場合）が、対象物を認識したときに補正されるので、推定位置（第 1 0〜第 3 0発明では第 2 の推定位置）の精度を高めることができる。また、前記第 1〜第 9発明では、少なくとも、前記位置推定手段により推定された前記ロポットの位置と、前記ロポットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えることが好ましい（第 3 4発明）。

同様に、前記第 1 0〜第 3 0発明では、少なくとも、前記第 2の推定位置と、前記ロポットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロポットの相対位置関係情報を基に、対象物の位置を推定する手段を備えることが好ましい（第 3 5発明）。

これらの第 3 4、第 3 5発明によれば、前述の如く決定した口ポットの自己位置たる推定位置（第 1 0〜第 3 0発明では、第 2の推定位置）を用いることで、対象物の位置を精度よく推定することができる。

また、第 1〜第 9発明では、少なくとも前記口ポットの推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記口ポットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えることが好ましい（第 3 6発明）。

同様に、第 1 0〜第 3 0発明では、少なくとも前記第 2の推定位置と. あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロポットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えることが好ましい（第 3 7発明）。

これらの第 3 6、第 3 7発明によれば、前述の如く決定したロボットの自己位置たる推定位置（第 1 0〜第 3 0発明では、第 2の推定位置）を用いるので、対象物とロボッ卜との位置関係を精度よく認識することができる。その結果、環境認識手段の注視方向を注視すべき方向（例えば環境認識手段を構成する撮像装置の画像の中央等の所定位置に対象物が存在するような方向）に精度よく制御することができる。図面の簡単な説明

図 1は、本発明の実施形態における脚式移動ロポットとしての 2足移動ロポットの全体的構成の概略を示す概略図、図 2は図 1の各脚体の足平部分の構成を概略的に示す概略図、図 3及び図 4はそれぞれ各脚体の足平部分の詳細構成を示す側面視の断面図及び底面図、図 5は図 1の口ポットに備えた制御ュニットの構成を示すプロック図、図 6は図 5の制御ュニットの機能的構成を示すブロック図である。図 7はロポットの走行歩容を示す説明図、図 8 ( a ) , ( b ) , ( c ) はそれぞれ目標歩容の床反力鉛直成分、目標 Z M P、自己位置姿勢推定用のゲインの設定例を示すグラフ、図 9は第 1実施形態における制御ュニットの要部の処理を示すフローチャート、図 1 0は図 9のフローチャートの自己位置姿勢推定処理を示すフローチヤ一ト、図 1 1は図 9のフローチャートの自己位置姿勢推定処理を説明するための図である。図 1 2は第 2実施形態の自己位置姿勢推定処理を示すフローチャート、図 1 3は第 2実施形態の自己位置姿勢推定処理を説明するための図である。図 1 4は第 3実施形態の自己位置姿勢推定処理を示すフローチャート、図 1 5は第 3実施形態の自己位置姿勢推定処理を説明するための図である。図 1 6は第 4実施形態の自己位置姿勢推定処理を示すフローチヤ一ト、図 1 7は第 4実施形態の自己位置姿勢推定処理の要部の処理を示すブロック図、図 1 8 ( a ) , ( b ) は第 4実施形態の自己位置姿勢推定処理を説明するための図、図 1 9は第 4実施形態で用いるゲインの設定例を示すグラフである図 2 0は第 5実施形態の自己位置姿勢推定処理を示すフローチヤ一ト、図 2 1は第 5実施形態の自己位置姿勢推定処理の要部の処理を示すプロック図である。図 2 2は第 6実施形態の自己位置姿勢推定処理の要部の処理を示すブロク図である。図 2 3及び図 2 4は第 7実施形態におけるロボットの頭部の内部構造をそれぞれ正面視、側面視で示す図、図 2 5 は第 7実施形態の自己位置姿勢推定処理を示すフローチャートである。発明を実施するための最良の形態

以下、添付図面を参照してこの発明の実施形態に係る脚式移動ロポッ卜の自己位置推定装置を説明する。尚、脚式移動口ポットとしては 2足移動ロポットを例にとる。

図 1は、この実施形態に係る脚式移動ロボットとしての 2足移動ロボットを全体的に示す概略図である。

図示の如く、 2足移動口ポット（以下、口ポットという） 1 は上体 (口ポット 1の基体） 3から下方に延設された左右一対の脚体（脚部リンク） 2， 2を備える。両脚体 2 , 2は同一構造であり、それぞれ 6個の関節を備える。その 6個の関節は上体 3側から順に、股（腰部）の回旋（回転）用（上体 3に対するョ一方向の回転用）の関節 1 0 R， 1 0 L (符号 R， Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）のロール方向（X 軸まわり）の回転用の関節 1 2 R, 1 2 Lと、股（腰部）のピッチ方向 (Y軸まわり）の回転用の関節 1 4 R, 1 4 L、膝部のピッチ方向の回転用の関節 1 6 R， 1 6 Lと、足首のピッチ方向の回転用の関節 1 8 R_: 1 8 Lと、足首のロール方向の回転用の関節 2 0 R， 2 0 Lとから構成される。

各脚体 2の足首の 2つの関節 1 8 R (L ), 2 O R (L ) の下部には. 各脚体 2の先端部を構成する足平（足部） 2 2 R (L) が取着されると共に、両脚体 2 , 2の最上位には、各脚体 2の股の 3つの関節 1 0 R (L), 1 2 R (L), 1 4 R (L) を介して前記上体 3が取り付けられている。上体 3の内部には、詳細を後述する制御ユニット 6 0などが格納される。なお、図 1では図示の便宜上、制御ユニット 6 0を上体 3の外部に記載している。

上記構成の各脚体 2においては、股関節（あるいは腰関節）は関節 1 O R (L)， 1 2 R (L)， 1 4 R (L) から構成され、膝関節は関節 1 6 R (L) から構成され、足首関節は関節 1 8 R (L), 2 0 R (L) から構成される。また股関節と膝関節とは大腿リンク 2 4 R (L) で連結され、膝関節と足首関節とは下腿リンク 2 6 R (L) で連結される。

尚、上体 3の上部の両側部には左右一対の腕体 5， 5が取り付けられると共に、上体 3の上端部には頭部 4が配置される。これらの腕体 5 , 5及び頭部 4は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。

上記の構成により、各脚体 2の足平 2 2 R (L) は、上体 3に対して 6つの自由度を与えられている。そして、口ポット 1の歩行等の移動中に、両脚体 2 , 2を合わせて 6 * 2 = 1 2個（この明細書で「 *」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平 2 2 R, 2 2 L の所望の運動を行うことができる。これにより、口ポット 1は任意に 3 次元空間を移動することができる。

図 1 に示す如く、各脚体 2の足首関節 1 8 R ( L ), 2 0 R (L ) の下方には足平 2 2 R (L) との間に公知の 6軸力センサ 5 0が介装されている。該 6軸力センサ 5 0は、各脚体 2の足平 2 2 R (L) の着地の有無、および各脚体 2に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の 3方向成分 F x， F y , F z並びにモーメントの 3方向成分 Mx, My, M zの検出信号を制御ユニット 6 0に出力する。また、上体 3には、 Z軸（鉛直方向（重力方向））に対する上体 3の傾き（姿勢角）およびその角速度等を検出するための傾斜センサ 5 4が備えられ、その検出信号が該傾斜センサ 5 4から制御ュニット 6 0に出力される。この傾斜センサ 5 4は、図示を省略する 3軸方向の加速度センサおよび 3軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体 3の傾きおよびその角速度を検出するために用いられると共に、口ポット 1の自己位置姿勢を推定するために用いられる, また、詳細構造の図示は省略するが、口ポット 1の各関節には、それを駆動するための電動モータ 6 4 (図 5参照）と、その電動モ一夕 6 4の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ） 6 5 (図 5参照）とが設けられ、該エンコーダ 6 5の検出信号が該エンコーダ 6 5から制御ュニット 6 0に出力される。

さらに、図 1では図示を省略するが、ロボット 1の適宜な位置にはジョィスティック（操作器） 7 3 (図 5参照）が設けられ、そのジョイスティック 7 3を操作することで、直進移動しているロボット 1を旋回させるなど、口ポット 1の歩容に対する要求を必要に応じて制御ュニット 6 0に入力できるように構成されている。

図 2は本実施形態における各脚体 2の先端部分（各足平 2 2 R ( L ) を含む）の基本構成を概略的に示す図である。同図に示すように、各足平 2 2 R ( L ) の上方には、前記 6軸力センサ 5 0との間にばね機構 7 0が装備されると共に、足底（各足平 2 2 R , Lの底面）にはゴムなどからなる足底弾性体 7 1が貼られている。これらのばね機構 7 0及び足底弾性体 7 1によりコンプライアンス機構 7 2が構成されている。ばね機構 7 0は詳細は後述するが、足平 2 2 R ( L ) の上面部に取り付けられた方形状のガイド部材（図 2では図示省略）と、足首関節 1 8 R ( L ) (図 2では足首関節 2 O R ( L ) を省略している）および 6軸力センサ 5 0側に取り付けられ、前記ガイド部材に弾性材（ゴムやばね）を介して微動自在に収納されるピストン状部材（図 2では図示省略）とから構成されている。 ·

図 2に実線で表示された足平 2 2 R (L) は、床反力を受けていないときの状態を示している。各脚体 2が床反力を受けると、コンプライアンス機構 7 2のばね機構 7 0と足底弾性体 Ί 1 とがたわみ、足平 2 2 R (L) は図中に点線で例示したような位置姿勢に移る。このコンプラインァス機構 7 2の構造は、例えば本出願人が先に提案した特開平 5— 3 0 5 5 8 4号公報に詳細に説明されている如く、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。

上記コンプライアンス機構 7 2を含めた足平 2 2 R (L) (以下、足平機構 2 2 R (L) と称することがある。）のより詳細な構成を図 3及び図 4を参照してさらに説明する。図 3は足平機構 2 2 R (L) の側面示の断面図、図 4は該足平機構 2 2 R (L) の底面側から見た平面図でめる。

足平機構 2 2 R (L) は、大略平板状の足平プレート部材 1 0 2を骨格部材として備えている。この足平プレート部材 1 0 2は、その前端部 (つま先部）と後端部（踵部）とが若干上方に湾曲されているが、他の部分は平坦な平板状になっている。また、足平プレート部材 1 0 2の上面部には、横断面方形状のガイド部材 1 0 3がその軸心を上下方向に向けて固設されている。このガイド部材 1 0 3の内部には、該ガイド部材 1 0 3の内周面に沿うようにして略上下方向に移動可能に設けられた可動板（ピストン状部材） 1 0 4が設けられ、該可動板 1 0 4が足首関節 1 8 R (L ), 2 0 R (L) に 6軸力センサ 5 0を介して連結されている。

また、可動板 1 0 4は、その下面の周縁部がばね、ゴム等の弾性材からなる複数の弾性部材 1 0 6 (図ではばねとして記載している）を介して足平プレート部材 1 0 2の上面部に連結されている。従って、足平プレ一ト部材 1 0 2は、弾性部材 1 0 6、可動板 1 0 4及び 6軸力センサ 5 0を介して足首関節 1 8 R ( L ) に連結されている。尚、ガイド部材 1 0 3の内部（可動板 1 0 4の下側の空間）は、図示を省略する穴や隙間を介して大気側に開放されており、大気中の空気がガイド部材 1 0 3 の内部に入出自在となっている。また、上記ガイド部材 1 0 3、可動板 1 0 4、及び弾性部材 1 0 6は前記図 2に示したばね機構 7 0を構成するものである。

足平プレート部材 1 0 2の底面（下面）には、前記図 2に示した足底弾性体 7 1としての接地部材 7 1が取着されている。該接地部材 7 1は. 足平機構 2 2 R ( L ) の接地状態で、該足平プレート部材 1 0 2と床面との間に介在させる弹性部材（床面に直接的に接触する弾性部材）であり、本実施形態では、足平プレート部材 1 0 2の接地面の四隅（足平プレート部材 1 0 2のつま先部の両側部並びに踵部の両側部）に固着されている。

また、接地部材 7 1は、本実施形態では、比較的軟質のゴム材から成る軟質層 1 0 7 aと、比較的硬質のゴム材から成る硬質層 1 0 7 bとを上下に重合してなる 2層構造に形成され、硬質層 1 0 7 bが、脚体 2の着床時に直接的に床面に接触する接地面部として最下面側に設けられている。

足平機構 2 2 R ( L ) には、上記の構成の他、着地衝撃緩衝装置 1 0 8が備えられている。この着地衝撃緩衝装置 1 0 8は、足平プレート部材 1 0 2の底面に取着された袋状部材 1 0 9 と、該袋状部材 1 0 9の内部に対して圧縮性流体としての空気（大気中の空気）を入出させるための流通路 1 1 0とを備えている。

袋状部材 1 0 9は、その周囲に前記接地部材 7 1が存するようにして- 足平プレート部材 1 0 2の底面の大略中央部に設けられている。この袋状部材 1 0 9は、ゴム等の弹性材により変形自在に構成されており、外力による弾性変形が生じていない自然状態では、図 3に実線で示すように、上方に開口した円筒容器形状を呈する。そして、該袋状部材 1 0 9 は、その開口端部が全周にわたって足平プレート部材 1 0 2の底面に固着され、該足平プレート部材 1 0 2により閉蓋されている。また、袋状部材 1 0 9は、円筒容器形状を呈する自然状態では、該袋状部材 1 0 9 の底部が前記接地部材 7 1よりも下方に突出するように設けられている, つまり、該袋状部材 1 0 9の高さ（足平プレート部材 1 0 2の下面から袋状部材 1 0 9の底部までの距離）は、接地部材 7 1の厚さよりも大きいものとされている。従って、足平プレート部材 1 0 2が接地部材 7 1 を介して接地した状態（脚部 2の着床状態）では、袋状部材 1 0 9は、図 3に仮想線で示すように、床反力により袋状部材 1 0 9の高さ方向に圧縮される。

尚、本実施形態では、袋状部材 1 0 9が円筒容器形状を呈する自然状態は該袋状部材 1 0 9の膨張状態である。そして、袋状部材 1 0 9は、弹性材により構成されているため、圧縮されたとき、自然状態の形状 (円筒容器形状）への形状復元力を有する。

前記流通路 1 1 0は、袋状部材 1 0 9に対する空気の流入 · 流出を行う流入 ·流出手段を構成するものであり、本実施形態では、袋状部材 1 0 9の内部と前記ガイド部材 1 0 3の内部とを連通させるように足平プレート部材 1 0 2に穿設された流通孔である。この場合、前述のように. ガイド部材 1 0 3の内部は大気側に開放されているので、該流通路 1 1 0は、袋状部材 1 0 9の内部を大気側に連通させていることとなる。従つて、袋状部材 1 0 9の内部には、大気中の空気が流通路 1 1 0を介して入出自在となっており、該袋状部材 1 0 9の膨張状態（自然状態）では、該袋状部材 1 0 9内には空気が充填され、その内部の圧力は大気圧と同等になる。また、流通路 1 1 0は絞り通路となっており、袋状部材 1 0 9の内部に空気が入出する際には流体抵抗を生じるようになっている。

図 5は制御ュニッ卜 6 0の構成を示すプロック図である。該制御ユエット 6 0はマイクロコンピュータにより構成されており、 C PUからなる第 1の演算装置 9 0及び第 2の演算装置 9 2、 AZD変換器 8 0、力ゥン夕 8 6、 DZA変換器 9 6、 RAM 8 4 , ROM 9 4、並びにこれらの間のデータ授受を行うバスライン 8 2を備えている。この制御ュニット 6 0では、各脚体 2の 6軸力センサ 5 0、傾斜センサ 5 4 (加速度センサおよびレートジャイロセンサ）、ジョイスティック 7 3等の出力信号は A/D変換器 8 0でデジタル値に変換された後、バスライン 8 2 を介して RAM 8 4に送られる。またロボット 1の各関節のエンコーダ 6 5 (ロータリ一エンコーダ）の出力は、カウンタ 8 6を介して RAM 84に入力される。

前記第 1の演算装置 9 0は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ 6 4の回転角の指令値）を算出し、 RAM 8 4に送出する。また第 2の演算装置 9 2は R AM 8 4から関節角変位指令と、前記エンコーダ 6 5の出力信号とに基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出して DZ A変換器 9 6とサーボアンプ 6 4 aとを介して各関節を駆動する電動モ一夕 64に出力する。

図 6は、この実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図 6中の「実口ポット」の部分以外の部分が制御ュニット 6 0が実行する処理機能（主として第 1の演算装置 9 0及び第 2の演算装置 9 2の機能）によって構成されるものである。なお、以下の説明では、脚体 2の左右を特に区別する必要がないときは、前記符号 R , Lを省略する。

以下説明すると、制御ユニット 6 0は、ロボット 1 の目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置 2 0 0、自己位置姿勢推定部 2 1 4等を備えている。自己位置姿勢推定部 2 1 4は、詳細は後述するが、本発明の特徴部分に係わる処理を実行し、口ポット 1の位置 ·姿勢（グローバル座標系での位置 ·姿勢）を推定するものである。歩容生成装置 2 0 0が出力する目標歩容は、目標上体位置姿勢軌道 (上体 3の目標位置及び目標姿勢の軌道）、目標足平位置姿勢軌道（各足平 2 2の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体 5 の目標姿勢の軌道）、目標全床反力中心点（目標 Z M P ) 軌道、目標全床反力軌道から構成される。なお、脚体 2や腕体 5以外に上体 3に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。

ここで、本発明の実施形態での用語の意味あるいは定義について補足しておく。上記歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、各部位の「姿勢」は、該部位の傾きと向きとを総称したものである。ここで、「傾き」は該部位の鉛直方向となす角度であり、「向き」は該部位の前方向を示すベクトルを水平面に投影したベクトルの向きである。例えば上体姿勢のうちの傾きは、 Z軸 (鉛直軸）に対するロール方向（X軸回り）の上体 3 の傾斜角（姿勢角）と、 Z軸に対するピッチ方向（Y軸回り）の上体 3の傾斜角（姿勢角）とからなる。また、上体 3の向きは、上体 3の前方向を示すべクトルを水平面に投影したベクトルのョー方向（Z軸回り）の回転角で表される。なお、足平姿勢は各足平 2 2に固定的に設定された 2軸の空間的な方位角で表される。また、特に、足平 2 2の着地姿勢に関しては、その着地姿勢は、基本的には着地した足平 2 2の向きを表し、具体的には着地した足平 2 2のかかとからつま先に向かうべクトルを水平面に投影したベクトルの向きを表す。また、目標腕姿勢は、腕体 5のすベての部位に関する上体 3に対する相対的な姿勢で表される。

上体位置は、上体 3の所定位置、具体的には上体 3のあらかじめ定めた代表点の位置を意味する。同様に、足平位置は、各足平 2 2 R， 2 2 Lのあらかじめ定めた代表点の位置を意味する。なお、上体速度は、上体 3の上記代表点の移動速度を意味し、足平速度は、各足平 2 2 R， 2 2 Lの上記代表点の移動速度を意味する。

目標上体位置姿勢等の目標歩容に関し、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、口ポット 1 の運動に係わる歩容を総称的に「運動」という。

各足平 2 2 R， Lの床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、口ポット 1の全ての（ 2本の）足平 2 2 R， 2 2 Lの床反力の合力を「全床反力」と呼ぶ。ただし、以下の説明においては、各足平床反力に関してはほとんど言及しないので、断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する力（並進力）と力のモーメントによつて表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に前述の目標床反力中心点を作用点にして目標床反力を表現すると、力のモーメントは、鉛直軸成分を除けば、 0になる。

尚、動力学的平衡条件を満足する歩容では、目標運動軌道から算出される Z M P (目標運動軌道から算出されるロボ、、ノト 1 の慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直軸成分を除いて 0 になる点）と目標全床反力中心点は一致することから、目標全床反力中心点軌道の代わりに目標 ZMP軌道を与えると言っても同じことである (詳細は、例えば本出願人による P C T公開公報 WO/02/40224を参照）。

このような背景から、 P C T公開公報 WOZ02Z40224の明細書では目標歩容を次のように定義していた。

a) 広義の目標歩容とは、 1歩ないしは複数歩の期間の目標運動軌道とその目標床反力軌道との組である。

b) 狭義の目標歩容とは、 1歩の期間の目標運動軌道とその Z MP軌道との組である。

c)一連の歩容は、いくつかの歩容がつながったものとする。

口ポット 1の歩行を行う場合においては、本出願人が先に特開平 10- 86080号公報で提案した上体高さ決定手法によって上体鉛直位置（上体高さ）が決定されると、床反力の並進力成分は従属的に決定されるので. 目標歩容の床反力に関して明示的に設定すべき物理量としては、 Z MP だけで十分であった。したがって、 P C T公開公報 WO/02Z40224の明細書では、狭義の目標歩容としては、上記の b) で十分であった。それに対し、口ポット 1の走行を行う場合には、床反力鉛直成分も制御上重要であるので、該床反力鉛直成分を明示的に設定することが好ましい, そこで、本願出願人が先に提案した P C T出願（ PCT / JP02 / 13596) 等では、狭義の目標歩容として、次の b ') を採用した。

b ') 狭義の目標歩容とは、 1歩の期間の目標運動軌道とその Z M P軌道と床反力鉛直成分軌道の組である。

この明細書では以降、特にことわらない限り、目標歩容は狭義の目標歩容の意味で使用する。また、目標歩容の「 1歩」は、口ポット 1の片方の脚体 2が着地してからもう一方の脚体 2が着地するまでの意味で使用する。

歩容における両脚支持期とは言うまでもなく、口ポット 1がその自重を両脚体 2， 2で支持する期間、片脚支持期とはいずれか一方のみの脚体 2で口ポット 1の自重を支持する期間、空中期とは両脚体 2 , 2が床から離れている（空中に浮いている）期間を言う。

片脚支持期においてロポット 1の自重を支持しない側の脚体 2を「遊脚」と呼び、自重を支持する側の脚体 2を「支持脚」と呼ぶ。口ポット 1の歩行では、両脚支持期と片脚支持期とが交互に繰り返され、ロボット 1の走行では片脚支持期と空中期とが交互に繰り返される。この場合. 走行の空中期では、両脚体 2， 2とも、口ポット 1の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体 2、支持脚であった脚体 2をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。

また、目標上体姿勢、目標上体位置、目標足平位置姿勢、目標腕姿勢等、目標歩容におけるロポット 1の各部の位置姿勢は支持脚座標系で記述される。支持脚座標系とは、支持脚の足平 2 2の接地面辺りに原点を持つ床面に固定された座標系である。より詳細には、支持脚座標系は、本出願人の特許 3273443 号に記載されているように、支持脚の足平 2 2を接地面との間で滑らさないで、水平姿勢になるまで回転させた時の. 該支持脚の足首関節の中心から接地面への垂直投影点を原点とし、該支持脚足平 2 2のつま先に向かう水平軸（足平 2 2の前後方向の軸）を X 軸として、鉛直軸を Z軸、これらの X軸、 Z軸に直交する座標軸（足平 2 2の左右方向の軸）を Y軸とする座標系である。

本発明の実施形態に係る歩容生成装置 2 0 0は、 2歩先までの遊脚の足平 2 2の着地位置姿勢、着地時刻の要求値（目標値）を入力として、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標 Z M P軌道、目標床反力鉛直成分軌道、及び目標腕姿勢軌道から構成される目標歩容を生成する。このとき、これらの軌道を規定するパラメ一夕（これを歩容パラメ一夕と呼ぶ）の一部は、歩容の継続性を満足するように修正される, 目標歩容の生成には口ポット 1の動力学モデルが用いられる。その動力学モデルとしては、例えば、前記 P C T公開公報 WOZ 02Z 40224号に記載の単純化モデルあるいは、本出願人が提案した特開 2002- 326173 号公報に記載の多質点モデル (フルモデル) などを用いれば良い。

また、歩容生成装置 2 0 0は、口ポット 1の片方の脚体 2が着地してから他方の脚体 2が着地するまでの 1歩分の目標歩容（前記狭義の意味での目標歩容）を単位として、その 1歩分の目標歩容を順番に生成する, ここで、現在あるいはこれから生成しょうとしている歩容を「今回歩容」、その次の歩容を「次回歩容」、さらにその次の歩容を「次次回歩容」と呼ぶ。また、「今回歩容」の 1つ前に生成した目標歩容を「前回歩容」と呼ぶ。

歩容生成装置 2 0 0が生成する目標歩容の一部を例示的に概説すると例えば目標足平位置姿勢軌道は、本出願人による特許 3233450 号に開示した有限時間整定フィルタを用いて生成される。この有限時間整定フィルタによる足平位置姿勢軌道の生成処理では、例えば足平位置軌道は目標着地位置（着地位置の要求値）に向かって足平 2 2を徐々に加速しながら移動を開始し、目標着地時刻（着地時刻の要求値）までに徐々に速度を 0またはほぼ 0にまで減速し、該目標着地時刻に目標着地位置に到達して停止するように生成される。足平姿勢軌道についても同様である。これにより生成される目標足平位置姿勢軌道は、着地瞬間における対地速度が 0またはほぼ 0になるため、特に口ポット 1の走行を行う場合に、前記空中期からの着地時における着地衝撃を小さくできる。

また、図 7に示すように人間が走行を行う場合と同様の形態でロポット 1の走行を行う場合には、例えば目標床反力鉛直成分軌道および目標 ZMP軌道（詳しくは支持脚座標系の X軸方向（支持脚足平 2 2の前後方向）での目標 ZMP軌道）は、それぞれ図 8 ( a)、図 8 (b) に実線で示すようなパターンで設定される。なお、図 7の第 1番目〜第 3番目の図は、それぞれ片脚支持期の開始時、中間時点、終了時における口ポット 1の両脚体 2， 2の運動状態を模式的に示し、第 4番目及び第 5 番目の図は、それぞれ空中期の中間時点、空中期の終了時（次の片脚支持期の開始時）における口ポット 1の両脚体 2， 2の運動状態を模式的に示している。

口ポット 1の走行を行う場合には、目標床反力鉛直成分軌道は、基本的には、片脚支持期では上に凸のパターンとなり、空中期では 0に維持される。また、ロボット 1の歩行を行う場合には、目標床反力鉛直成分軌道は、例えば図 8 ( a) に二点鎖線で示すように設定される。この場合、二点鎖線のうちの上に凸の部分が両脚支持期に対応し、下に凸の部分が片脚支持期に対応する。また、目標 ZMPは走行、歩行のいずれであっても、基本的には、口ポット 1の脚体 2の接地面内（より詳しくは所謂、支持多角形内）の中央付近に設定される。

図 9は、前記歩容生成装置 2 0 0の歩容生成処理、ならびに、図 6に示す自己位置姿勢推定部 2 1 4の自己位置姿勢推定処理を示すフローチャ一ト（構造化フローチャート）である。

まず S 0 1 0において時刻 t を 0に初期化するなど種々の初期化作業を行う。

次いで S 0 1 2を経て S 0 1 4に進み、制御周期毎のタイマ割り込みを待つ。制御周期は A tである。次いで S 0 1 6に進み、自己位置姿勢推定部 2 1 4による自己位置姿勢推定を行う。 S 0 1 6における処理がこの出願に係る脚式移動ロポットの自己位置推定装置の特徴をなすが、この説明は後述することとする, 次いで S 0 1 8に進み、歩容切り替わり目（前回歩容の生成が終了し. 新たな今回歩容の生成を開始すべき時刻）であるか否かが判断される。そして、その判断結果が Y E Sであるときは S 0 2 0に進み、 N Oであるときは S 0 3 2に進む。なお、以下に説明する S 0 2 0以降の処理は. 本願出願人が先に提案した P C T公開公報 WOノ 02, 40224あるいは前記 P C T出願 PCT/JP02Z 13596 に詳細に説明されているので、本明細書では簡略的な説明に留める。

S 0 2 0に進むときは時刻 tを 0に初期化する。次いで S 0 2 2に進み、次回歩容支持脚座標系（詳しくはその位置および向,き）、次次回歩容支持脚座標系（詳しくはその位置および向き）、今回歩容周期および次回歩容周期を読み込む。

上記次回歩容支持脚座標系および次次回歩容支持脚座標系は、それぞれ、前記ジョィスティック 7 3の操作等によって指定される 1歩目の遊脚足平 2 2 (今回歩容の遊脚足平 2 2 ) の着地位置姿勢の要求値（目標着地位置姿勢）、 2歩目の遊脚足平 2 2 (次回歩容の遊脚足平 2 2 ) の着地位置姿勢の要求値（目標着地位置姿勢）に応じて、前記した支持脚座標系の定義に従って決定される。

また、今回歩容周期、次回歩容周期は、それぞれ、 1歩目の遊脚足平 2 2の着地時刻の要求値（目標着地時刻）、 2歩目の遊脚足平 2 2の着地時刻の要求値（目標着地時刻）に応じて決定される。

上記した遊脚足平 2 2の着地位置姿勢の要求値並びに着地時刻の要求値、あるいは支持脚座標系の位置および向き並びに歩容周期は、あらかじめ歩行スケジュールとして記憶しておいても良く、あるいはジョイスティック 7 3などの操縦装置からの指令（要求）とそのときまでの歩行履歴を基に決定しても良い。

次いで S 0 2 4に進み、今回歩容につながる定常旋回歩容の歩容パラメータが、 S 0 2 2で決定された次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期等に基づいて決定される, 主に、目標足平位置姿勢軌道を規定する足平軌道パラメ一夕、目標上体姿勢の基準軌道を規定する基準上体姿勢軌道パラメ一夕、目標腕姿勢軌道を規定する腕姿勢軌道パラメータ、目標 Z M P軌道を規定する Z M P 軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメ一夕が決定される。例えば床反力鉛直成分軌道パラメータに関して例示すると、前記図 8 ( a ) に示したパターンの折れ点の時刻や値が床反力鉛直成分軌道パラメ一夕として決定される。

ここで、前記定常旋回歩容は、その歩容を繰り返したときに歩容の境界において口ポット 1の運動状態に不連続が生じないような周期的歩容を意味する（以降、「定常旋回歩容」を「定常歩容」と略す場合もある） _t 定常旋回歩容の 1周期分の歩容は、第 1旋回歩容と第 2旋回歩容とからなる。第 1旋回歩容は、今回歩容の支持脚座標系に対応する支持脚足平 2 2を次次回歩容支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当し、第 2旋回歩容は、次回歩容支持脚座標系に対応する支持脚足平 2 2を次次次回支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当する。この場合、次次次回歩容支持脚座標系は、第 2旋回歩容の遊脚足平 2 2の目標着地位置姿勢に対応するものである。そして、該次次次回歩容支持脚座標系は、次次回歩容支持脚座標系（第 2旋回歩容の支持脚座標系）から見た該次次次回歩容支持脚座標系の位置姿勢 (位置及び向き）が、今回歩容支持脚座標系から見た次回歩容支持脚座標系（今回歩容の遊脚足平 2 2の着地位置姿勢）の位置姿勢（位置及び向き）に一致するように設定される。尚、定常旋回歩容に関して「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。

定常旋回歩容は、歩容生成装置 2 0 0で今回歩容の終端における発散成分や上体鉛直位置速度を決定するために暫定的に作成される仮想的な周期的歩容であり、口ポット 1を実際に制御するために歩容生成装置 2 0 0からそのまま出力されるものではない。

尚、「発散」とは、上体の位置が両足部（足平）の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、 2足移動口ポットの上体の位置が両足部（足平）の位置（厳密には、支持脚接地面に設定された支持脚座標系の原点からかけ離れていく具合を表す数値であり、上体 3の水平方向の位置及びその速度の関数で表される。

本実施形態では、これから生成する今回歩容の後につながる定常歩容を移動要求（前記 2歩先までの遊脚の足平 2 2の着地位置姿勢、着地時刻などの要求値）に応じて設定し、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致するように、今回歩容を生成するようにした。 S O 2 4の詳細は、本出願人が提案した前記 P C T公開公報 WOZ 02Z 40224、あるいは PCT/ JP02/ 13596に説明されているので、これ以上の説明を省略する。

S 0 1 0から S 0 2 4までに示す処理を行って後、 S 0 2 6に進み、定常旋回歩容の初期状態（初期上体水平位置速度成分、初期上体鉛直位置速度、初期発散成分、初期上体姿勢角および角速度）を決定する。 S 0 2 6の詳細は、 P C T公開公報 WOZ 02Z 40224、あるいは PCTZ JP02Z 13596 に説明しているので、ここでは、これ以上の説明を省略する。

次いで、 S 0 2 8に進み、今回歩容の歩容パラメータを決定（一部仮決定）する。この場合、決定される今回歩容の歩容パラメ一夕は、定常旋回歩容の歩容パラメ一夕と同様、主に、足平軌道パラメ一夕、基準上体姿勢軌道パラメ一夕、腕姿勢軌道パラメ一夕、目標 Z M P軌道パラメ一夕、目標床反力鉛直成分軌道パラメ一夕であり、それぞれのパラメ一夕により規定される軌道が、定常旋回歩容の軌道に連続するように決定される。ただし、これらのパラメータのうち、目標 Z M P軌道パラメ一夕は暫定的なものである。この S 0 2 8の処理の詳細は、前記 P C T公開公報 WOZ 02Z 40224、あるいは PCT/JP02Z 13596 号等に説明されているので、ここではこれ以上の説明を省略する。

次いで S 0 3 0に進み、今回歩容の終端発散成分が定常歩容の初期発散成分に一致するように、今回歩容の歩容パラメータを修正する。ここで修正される歩容パラメ一夕は、目標 Z M P軌道パラメータである。

S 0 3 0において今回歩容パラメ一タを修正した後、あるいは S 0 1 8の判断結果が N〇である場合には、 S 0 3 2に進み、今回歩容瞬時値を決定する。この処理の詳細は、前記 P C T公開公報 WO/ 02Z 40224, あるいは PCT/JP02/ 13596 号等に説明しているので、ここでは、これ以上の説明を省略する。

次いで S 0 3 4に進み、 P C T出願 PCTZ JP02Z 13596 号の実施形態の S O 3 2の処理と同様、スピン力をキャンセルするための腕振り動作を決定する。

次いで S O 3 6に進み、歩容生成用時刻 tを Δ tだけ増やし、 S 0 1 4に戻り、以上のごとく歩容生成を続ける。

以上が、歩容生成装置 2 0 0における目標歩容生成処理、ならびに、自己位置姿勢推定部 2 1 4の自己位置姿勢推定処理である。

図 6を参照してこの実施形態に係る制御ュニッ卜 6 0の制御処理をさらに説明すると、歩容生成装置 2 0 0において、上記したように目標歩容が生成される。生成された目標歩容のうち、目標上体位置姿勢（軌道）および目標腕姿勢軌道は、口ポット幾何学モデル（逆キネマテイクス演算部） 2 0 2に直接送られる。

また、目標足平位置姿勢（軌道）、目標 Z M P軌道（目標全床反力中心点軌道）、および目標全床反力（軌道）（目標床反力水平成分と目標床反力鉛直成分）は、複合コンプライアンス動作決定部 2 0 4に直接送られる一方、目標床反力分配器 2 0 6にも送られる。目標床反力分配器 2 0 6では、目標全床反力は各足平 2 2 R， 2 2 Lに分配され、目標各足平床反力中心点および目標各足平床反力が決定される。その決定された目標各足平床反力中心点および目標各足平床反力が複合コンプライアンス動作決定部 2 0 4に送られる。

複合コンプライアンス動作決定部 2 0 4では、機構変形補償付き修正目標足平位置姿勢軌道が生成され、それがロポット幾何学モデル 2 0 2 に送られる。口ポット幾何学モデル 2 0 2は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）が入力されると, それらを満足する脚体 2 , 2の 1 2個の関節（ 1 0 R ( L ) など）の関節変位指令（値）を算出して変位コン卜ローラ 2 0 8に送る。変位コントローラ 2 0 8は、ロポット幾何学モデル 2 0 2で算出された関節変位指令（値）を目標値としてロポット 1の 1 2個の関節の変位を追従制御する。

口ポット 1に生じた床反力（詳しくは実各足平床反力）は 6軸力センサ 5 0によって検出される。その検出値は前記複合コンプライアンス動作決定部 2 0 4に送られる。また、前記図 9の S 0 1 6で後述する如く求められた推定上体姿勢と歩容生成装置 2 0 0が生成した目標上体姿勢との差のうちの傾き成分、すなわち実上体姿勢角偏差 0 errx , Θ erry が姿勢安定化制御演算部 2 1 2に送られる。なお、 0 _{e rrx} はロール方向（X軸回り）の傾き成分であり、 0 erry はピッチ方向（Y軸回り）の傾き成分である。この姿勢安定化制御演算部 2 1 2で、口ポット 1の上体姿勢の傾きを目標歩容の上体姿勢の傾きに復元するための目標全床反力中心点（目標 Z M P ) まわりの補償全床反力モーメント Mdmd が算出され、この補償全床反力モーメント Mdmd が複合コンプライアンス動作決定部 2 0 4に与えられる。複合コンプライアンス動作決定部 2 0 4は、入力値に基づいて目標足平位置姿勢を修正する。具体的には、複合コンプライアンス動作決定部 2 0 4では、実全床反力（すべての実足平床反力の合力で、並進力およびモーメントの両者を含む）が、目標各足平床反力の合力である目標全床反力と補償全床反力モーメント Mdmd との合力に一致するように、歩容生成装置 2 0 0から与えられた目標足平位置姿勢を修正して、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすベての状態を目標に一致させることは事実上不可能であるので、これらの間に卜レードオフ関係を与えて妥協的になるベく一致させる。すなわち、足平位置姿勢及び床反力の各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の 2 乗）の重み付き平均が最小になるように制御する。

補足すると、機構変形補償付き修正目標足平位置姿勢（軌道）は、複合コンプライアンス動作決定部 2 0 4によって修正された床反力の目標値を発生させるために必要な足平の変形機構（円柱状ゴム、足底スポンジおよび衝撃吸収用の袋状のエアダンパー）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求めて、その変形量が発生するように修正した、目標足平位置姿勢（軌道）である。

以下に、第 1実施形態における S 0 1 6の自己位置姿勢推定処理について、そのフローチャートである図 1 0を用いて詳説する。なお、以下に説明する第 1実施形態の自己位置姿勢推定処理は、口ポット 1の歩行を行っている場合の処理である。

まず S 2 0 0 0において、上体 3の傾斜センサ 5 4に備えたジャイロセンサの検出値、すなわち上体 3の角速度（ 3軸方向の角速度）の検出値を積分器により積分して推定上体姿勢を求める。この推定上体姿勢はグローバル座標系で記述される。なお、推定上体姿勢のうちの傾き成分に関しては、ジャイロセンサの検出値の積分誤差の蓄積（ドリフト）を抑制するために、傾斜センサ 5 4に備えた加速度センサによつて検出される重力方向を用いてドリフト補正が行われる。具体的には、加速度センサの検出値から求められる重力方向に対する上体 3の相対角度と、上記推定上体姿勢のうちの傾きとの差を求め、この差が 0に収束するように、該差から P I制御則等のフィ一ドバック制御則により求めた値を、ジャィ口センサ検出値を積分するための積分器に追加的に入力することにより、補正が行われる。この補正手法は公知であるので、これ以上の説明は省略する。

次いで S 2 0 0 2に進み、ロボット 1の着地が確定したか否かを判定する。この判定においては、以下に列挙したいずれかの方法のように、目標歩容の時期から判定しても良いし、あるいは、 6軸力センサ 5 0 (床反力センサ）や加速度センサの検出値を基に判定しても良い。あるいは、それらの検出値や目標歩容の時期から総合的に判断しても良い。 a)片脚支持期が開始してから所定時間後の時点か否か。

b) 6軸力センサ 5 0 (床反力センサ) の検出値がある所定の値となつてから所定時間が経過した時点か否か。

c)加速度センサの検出値がある所定の値となつてから所定時間が経過した時点か否か。

ただし、 S 2 0 0 2の判定結果が Y E Sになった直後に、極力、着地した足平 2 2が滑ったり、再び床から離れることがないように、着地瞬間から適度な時間が経過した時点を着地が確定した時点とすることが好ましい。従って、上記 a)〜c)の所定時間は、このような条件を満たすように設定することが好ましい。

S 2 0 0 2の判定結果が N Oである場合には、以上をもって S 0 1 6 の自己位置推定処理を終了し、前述のごとく図 9の S O 1 8に進む。

S 2 0 0 2の判定結果が Y E Sである場合には、以下の処理を行う。すなわち、まず S 2 0 0 4に進み、グロ一バル座標系から見た推定上体姿勢と、グローバル座標系から見た目標歩容の上体姿勢（目標上体姿勢）との差を姿勢回転偏差変化量として算出する。なお、グローバル座標系から見た目標歩容の上体姿勢とは、現在の推定支持脚座標系上で、 1歩の間、上体 3の姿勢傾きやスピン（目標上体姿勢からのずれ）がなく口ポット 1が目標歩容通りに運動したと仮定した場合のグローバル座標系から見た上体姿勢のことである。ただし、推定支持脚座標系とは、実際の口ポット 1の支持脚足平 2 2の推定位置姿勢に対応した支持脚座標系である。すなわち、推定支持脚座標系は、前記した支持脚座標系の定義によって、より具体的には、実際の口ポット 1の支持脚足平 2 2を. その推定位置姿勢から、接地面との間で滑らさないで水平まで回転させた時の、該支持脚の足首中心から接地面への垂直投影点を原点とし、該支持脚足平 2 2のつま先に向かう水平軸を X軸にとり、鉛直軸を Z軸、これらに直交する座標軸を Y軸にとった座標系である。第 1実施形態においては、後述するごとく推定支持脚座標系は、着地が確定した時にのみ更新されるので、着地時の実際のロポットの支持脚足平 2 2の推定位置姿勢に対応する支持脚座標系となる。結局、第 1実施形態においては口ポット 1の自己位置の推定値として推定支持脚座標系の位置姿勢が推定される。

なお、 S 2 0 0 4において、着地確定時における推定上体姿勢と目標歩容の上体姿勢との差を姿勢回転偏差変化量として算出するようにしたが、厳密には、 1歩の間の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として算出するのが良い。ただし、第 1実施形態においては、以下のアルゴリズムを実行することによって、自動的に、 1歩の初めにおいて推定上体姿勢と目標歩容の上体姿勢とは一致するので、推定上体姿勢と目標歩容の上体姿勢との差と、 1歩の間の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差は同一となるので、いずれの差を用いても構わない。

次いで S 2 0 0 6に進み、姿勢回転中心を決定する。具体的には、前回歩容の目標 Z M Pのうち、例えば前回歩容の後側の支持脚足平 2 2がつま先接地になった状態での目標 Z M Pを姿勢回転中心として決定する次いで S 2 0 0 8に進み、現在の推定支持脚座標系（詳しくは前回歩容の支持脚足平 2 2の着地確定時に決定された推定支持脚座標系で、図 1 1 に示す滑りが発生する前の推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、 S 2 0 0 6で決定した姿勢回転中心まわりに回転させた位置 ·姿勢を改めて現在の推定支持脚座標系（図 1 1に例示する滑りが発生した後の推定支持脚座標系）と決定する。

なお、推定支持脚座標系の原点および座標軸の向きは、グローバル座標系によって表されるものとする。

また、 S 0 1 0の初期化処理において、推定支持脚座標系の初期値

(グロ一バル座標系での初期位置姿勢）がセットされているものとする ₍ 次いで S 2 0 1 0に進み、現在の推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置姿勢関係が目標歩容（前回歩容）における玄持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系（図 1 1 に例示する次回歩容推定支持脚座標系）を決定する。なお、ここで言う次回歩容支持脚座標系は、これから生成しょうとしている今回歩容に対する次回歩容支持脚座標系ではなく、前回歩容の次の歩容（つまり今回歩容）の支持脚座標系のことである。

次いで S 2 0 1 2に進み、 S 2 0 1 0で決定した次回歩容推定支持脚座標系の位置姿勢（原点の位置及び座標軸の向き）を現在の推定支持脚座標系の位置姿勢に代入する。以上をもって S 0 1 6の自己位置推定処理を終了し、前述のごとく S 0 1 8に進む。

以上が、第 1実施形態における S 0 1 6の自己位置推定処理である。推定支持脚座標系の回転中心は、空中期のない通常の歩行においては. 支持脚足平 2 2の滑り回転の回転中心を意味する。なお、走りなど、 1 歩の途中に支持脚も床から離れている空中期においては、狭義の意味では支持脚足平 2 2の滑りが存在するわけではないので、空中での姿勢回転（あるいはスピン）は、推定支持脚座標系の回転中心まわりの回転で表現するのが望ましい。しかし、広義の意味で「支持脚足平の滑り」を推定支持脚座標系の回転中心まわりの姿勢回転と定義しても良いことである。

補足すると、支持脚座標系は、支持脚足平 2 2の接地面に対して設定するもので、その原点は、前述のように支持脚の足首中心から接地面への垂直投影点にしなくても良い。すなわち、支持脚座標系は、ロボットの運動を記述するための支持脚足平 2 2の近傍の仮想の床に設定されたローカル座標系である。結局、口ポット 1 の姿勢回転（あるいはスピン）現象は、口ポット 1が前記仮想の床上で相対的には姿勢回転（あるいはスピン）しないで運動している状態を保ったまま、口ポット 1 ごと前記仮想の床を、グローバル座標系のなかである所定の点を回転中心として姿勢回転（あるいはスピン）させた現象とみなされる。さらに言い替えると、口ポット 1の運動は、ローカル座標系において目標歩容あるいは関節変位検出値に従って運動しているロボット 1の全体が、ローカル座標系ごと、グロ一バル座標系に対して、ある所定の点 (姿勢回転中心）を回転中心として摂動回転をしているとみなすことができる。

したがって、姿勢回転中心は、推定支持脚座標系の回転中心であり、また、前記摂動回転の回転中心であるとも言える。

第 1実施形態の S 2 0 0 8において、上体 3の姿勢の傾斜成分の差も含めて、現在推定支持脚座標系を前記姿勢回転偏差変化量だけ回転させると、一般的には、現在推定支持脚座標系の Z軸は鉛直方向を向かなくなる。

本実施形態では、現在推定支持脚座標系は、着地毎にしか更新されないので、着地瞬間の上体姿勢によって現在推定支持脚座標系が傾いたままであることは、好ましいことではない。したがって、現在推定支持脚座標系は、 Z軸の向きが鉛直方向になるように決定されるべきである。そこで、第 1実施形態では、 S 2 0 0 8において、現在の推定支持脚座標系を回転させた後に、その回転後の現在の推定支持脚座標系の Z軸の向きを鉛直方向に戻す。具体的には、回転後の現在の推定支持脚座標系を、その Z軸の鉛直方向に対する傾斜角度分だけ、該推定支持脚座標系の原点回りに回転させることで、該推定支持脚座標系の Z軸の向きを鉛直方向に戻す。あるいは、 S 2 0 0 8において、現在の推定支持脚座標系を、前記姿勢回転偏差変化量だけ回転させる代わりに、この姿勢回転偏差変化量のうちの鉛直軸まわり成分（スピン成分）だけ回転させても良い。または、 S 2 0 1 0において、次回歩容推定支持脚座標系の Z軸の向きを鉛直方向に戻しても良い。

以降、第 2実施形態までは、同様に、現在推定支持脚座標系は、 Z軸の向きが鉛直方向になるように決定するのが良い。第 3実施形態以降においても、スピンによる自己位置ずれを重視するならば、同様にしても良い。

上記のごとく、第 1実施形態においては、実際の口ポット 1の挙動を. 口ポット 1の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロポット 1が、姿勢検出手段（S 2 0 0 0の処理）によって得られた上体 3の姿勢（実姿勢の推定値）と上体 3 の目標姿勢との差の 1歩の間の変化量としての前記姿勢回転偏差変化量 (あるいは該姿勢回転偏差変化量の鉛直軸回り成分）の分だけ、姿勢回転中心決定手段（ S 2 0 0 6の処理）によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚足平 2 2の位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち足平 2 2の着地点の位置及び向き、さらに言い換えると足跡（foot print) が、 1歩毎（着地毎）に推定される。

なお、以上説明した第 1実施形態は、本発明の第 1発明〜第 5発明、並びに、第 7、第 8発明に対応するものである。

次に、本発明の第 2実施形態を図 1 2及び図 1 3を参照して説明する, なお、第 2実施形態は、前記図 9の S 0 1 6の自己位置姿勢推定処理のみが第 1実施形態と相違し、これ以外の構成及び制御ュニット 6 0の処理は、第 1実施形態と同じである。

図 1 2は第 2実施形態での自己位置姿勢推定処理を示すフローチヤ一トである。なお、以下に説明する第 2実施形態の自己位置推定処理は、ロボット 1の歩行を行っている場合の処理である。

以下説明すると、まず S 2 1 0 0において、第 1実施形態と同様、上体 3の傾斜センサ 5 4に備えたジャイロセンサの検出値（角速度検出値）を積分器により積分して推定上体姿勢を求める。次いで S 2 1 0 2 に進み、第 1実施形態と同様、着地が確定したか否かを判定する。

S 2 1 0 2の判定結果が N 0である場合には、以上をもって図 9の S

0 1 6の自己位置推定処理を終了し、前述のごとく S 0 1 8に進む。

S 2 1 0 2の判定結果が Y E Sである場合には、以下の処理を行う。すなわち、まず S 2 1 0 4に進み、第 1実施形態と同様、グローバル座標系から見た推定上体姿勢とグロ一バル座標系から見た目標歩容の上体姿勢との差を姿勢回転偏差変化量として算出する。

次いで S 2 1 0 6に進み、第 1実施形態と同様、姿勢回転中心を決定する。

次いで S 2 1 0 8に進み、第 1実施形態と同様、現在の推定支持脚座標系を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置 · 姿勢を改めて現在の推定支持脚座標系と決定する。図 1 3 に、回転させる前の現在推定支持脚座標系と回転させた後の現在推定支持脚座標系とを例示する。

次いで S 2 1 1 0に進み、少なくとも関節変位検出値を基にキネマテイクス演算によって、現在の推定支持脚座標系における着地時の実遊脚足平位置 ·姿勢（図 1 3の例ではロポット 1の前側の足平 2 2の位置姿勢）を推定する。以降、推定された着地時の実遊脚足平位置 ·姿勢を推定着地時遊脚足平位置姿勢と呼ぶ。

ただし、この時、口ポット 1が、現在の推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平 2 2が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。

補足すると、前記姿勢回転偏差変化量だけ前記姿勢回転中心まわりに回転させた現在推定支持脚座標系上で、口ポット 1が、目標歩容通りの上体姿勢を維持している姿を想定した理由は、この瞬間において、想定した姿における上体姿勢を、グローバル座標系において、推定上体姿勢に一致させるためである。したがって、現在推定支持脚座標系を、前記差だけ姿勢回転させる代わりに、前記姿勢回転偏差変化量の内の鉛直軸まわり成分（スピン成分）だけ回転させる場合には、口ポット 1が、現在の推定支持脚座標系上で、目標歩容通りの上体向きを維持しつつ、上体傾きは、前記姿勢回転偏差変化量の傾き成分に一致し、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平 2 2が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。

また、この時、 6軸力センサ 5 0による床反力検出値と目標歩容の床反力とのうちの少なくともいずれかを基に、足平 2 2の変形機構（前記図 3及び図 4に示した弾性部材 1 0 6、接地部材（足底弾性体） 7 1 、袋状部材 1 0 9等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求め、その変形量を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めても良い。より具体的には、図 1 3に示すロボット 1の姿のように、現在の推定支持脚座標系において上体姿勢を目標上体姿勢とし、関節変位を関節変位検出値（第 j 関節変位検出値 0 j 、 j = l , 2 、とし、変形機構の変形量を床反力検出値を基に推定した変形量とした場合の全体姿（その瞬間の口ポット 1の全体の姿）を求め，この全体姿における支持脚（図 1 3ではロポット 1の後ろ側の脚体 2 ) に対する遊脚足平位置姿勢を求め、これをもって推定着地時遊脚足平位置姿勢とするようにしても良い。また、モー夕電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機ゃリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めるようにしても良い。

または、少なくとも目標歩容の関節変位とコンプライアンス補償量 (本出願人が先に提案した特開平 10-277969 号を参照のこと）を基にキネマティクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。

これらにより、より一層精度良く推定着地時遊脚足平位置姿勢を求めることができる。

次いで S 2 1 1 2に進み、前記推定着地時遊脚足平位置姿勢を基に、図 1 3に示すように、現在推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置 ·姿勢を求める。なお、推定着地時遊脚足平位置姿勢、すなわち次回歩容の支持脚足平 2 2の位置姿勢と、次回歩容推定支持脚座標系の位置姿勢の対応関係は、前述のごとく支持脚足平 2 2と支持脚座標系との対応関係と同一にする。

次いで S 2 1 1 4に進み、図 1 3に示すように、回転させた後の現在推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置 ·姿勢と、グローバル座標系から見た、回転させた後の現在推定支持脚座標系の位置姿勢とを基に、グローバル座標系から見た次回歩容推定支持脚座標系の位置姿勢を求める。

次いで S 2 1 1 6に進み、上記の如く求めた次回歩容推定支持脚座標系の位置姿勢を現在の推定支持脚座標系の位置姿勢に代入する。以上をもって図 9の S O 1 6の自己位置推定処理を終了し、前述のごとく S 0 1 8に進む。

上記のごとく、第 2実施形態においては、実際の口ポット 1の挙動を口ポット 1の運動を記述するローカル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動しているロボット 1が、 1歩の間に、姿勢検出手段（S 2 1 0 0の処理）によって得られた上体 3の姿勢（実姿勢の推定値）と上体 3の目標姿勢との差の 1歩の間の変化量としての前記姿勢回転偏差変化量だけ、姿勢回転中心決定手段（S 2 1 0 6の処理）によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち足平 2 2の着地点の位置及び向き、さらに言い換えると足跡（foot print) が、 1歩毎（着地毎）に推定される。

なお、以上説明した第 2実施形態は、本発明の第 1〜第 5発明及び第

7〜第 9発明に対応するものである。

次に本発明の第 3実施形態を図 1 4及び図 1 5を参照して説明する。なお、第 3実施形態は、前記図 9の S 0 1 6の自己位置姿勢推定処理のみが第 1実施形態と相違し、これ以外の構成及び制御ュニット 6 0の処理は、第 1実施形態と同じである。

図 1 4は、第 3実施形態における図 9の S 0 1 6の自己位置推定処理のフローチヤ一卜である。

第 3実施形態においては、第 1、第 2実施形態と異なり、制御周期毎に、推定支持脚座標系の姿勢回転（またはスピン）を推定し、推定支持脚座標系を更新するようにした。また、制御周期毎の各瞬間における上体姿勢を推定すると共に、さらに上体位置（厳密には上体の代表点の位置）も推定するようにした。

詳細を説明すると、まず、 S 2 2 0 0において、第 1実施形態と同様. 推定上体姿勢を求める。

次いで S 2 2 0 2に進み、制御周期の間におけるグロ一バル座標系から見た推定上体姿勢の変化量と、該制御周期の間におけるグローバル座標系から見た目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量としてを算出する。すなわち、前記第 1、第 2実施形態では、 1歩の期間における推定上体姿勢の変化量と目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として求めたが、第 3実施形態では、制御周期毎の両変化量の差を姿勢回転偏差変化量として求める。

次いで S 2 2 0 4に進み、第 1実施形態と同様、姿勢回転中心を決定する。具体的には、その瞬間の目標 Z M P (目標 Z M Pの現在値）を姿勢回転中心とする。

次いで S 2 2 0 6に進み、現在の推定支持脚座標系（前回の制御周期で決定された推定支持脚座標系で、図 1 5に示す時刻 t 一 Δ tにおける推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置 ·姿勢を改めて現在の推定支持脚座標系（図 1 5に示す時刻 t における推定支持脚座標系）と決定する。回転のさせ方は第 1実施形態と同様である。

次いで S 2 2 0 8に進み、着地時刻であるか否か、すなわち歩容の切り替わり目であるか否かを判定する。

S 2 2 0 8の判定結果が Y E Sである場合には、以下の処理を行う。すなわち、まず S 2 2 1 0に進み、第 1実施形態と同様、現在の推定支持脚座標系に対する次回歩容推定支持脚座標系の相対位置姿勢関係が、目標歩容（前回歩容）における支持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系を決定する。

次いで S 2 2 1 2に進み、第 1実施形態と同様、次回歩容推定支持脚座標系の位置姿勢を現在の推定支持脚座標系の位置姿勢に代入する。

S 2 2 1 2の後、あるいは S 2 2 0 8において否定された場合には、 S 2 2 1 4に進み、現在の推定支持脚座標系の位置姿勢に対応して推定上体位置を決定する。ただし、現在の推定支持脚座標系に対する推定上体位置の関係は、目標歩容（前回の制御周期で決定された目標歩容）の支持脚座標系に対する上体位置の関係に一致させる。以上をもって S 0 1 6の自己位置推定処理を終了し、前述のごとく S 0 1 8に進む。

上記のごとく、第 3実施形態においては、実際のロボットの挙動を、ロボットの運動を記述する口一カル座標系である推定支持脚座標系上で目標歩容通りに運動している口ポット 1が、各瞬間に、姿勢検出手段 ( S 2 2 0 0の処理）によって得られた上体姿勢の変化量（変化速度）と目標歩容の上体姿勢の変化量（変化速度）との差としての姿勢回転偏差変化量で、姿勢回転中心決定手段（S 2 2 0 4の処理）によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなす。そして、その上で、新たな推定支持脚座標系と推定上体位置とが各瞬間（制御周期毎）に決定（更新）されるとともに、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向き、さらに言い換えると新たな足跡（foot print) が、 1歩毎（着地毎）に推定される。なお、本実施形態は、口ポット 1の歩行を行う場合だけでなく、ロボット 1の走行を行う場合でもよい。但し、走行を行う場合に S 2 2 0 4 で姿勢回転中心を決定するときには、片脚支持期では、姿勢回転中心を各瞬間（現在）の目標 Z M Pとすればよいが、ロボット 1の両脚体 2 , 2が空中に浮く空中期では、姿勢回転中心を、各瞬間（現在）の目標歩容における口ポット 1の重心の位置に決定する。

また、口ポット 1の歩行を行なう場合には、 S 2 2 1 4においては、少なくとも現在の推定支持脚座標系と関節変位検出値とを基に、 S 2 1 1 0において推定着地時遊脚足平位置姿勢を求めた手法と同様の手法で支持脚座標系に対する上体位置を求め、この位置関係に、推定支持脚座標系に対する推定上体位置が一致するように、推定上体位置を求めても良い。この場合、推定上体位置、推定支持脚座標系および関節変位検出値の関係は、図 1 3に示すようになる。

または、少なくとも目標歩容の関節変位とコンプライアンス補償量 (本出願人が先に提案した特開平 10-277969 号を参照のこと）を基に. キネマティクス演算によって支持脚座標系に対する上体位置 ·姿勢を求め、これに推定支持脚座標系に対する推定上体位置姿勢の関係を一致させても良い。

これらにより、より一層精度良く推定上体位置姿勢を決定することができる。

また、口ポット 1の歩行を行なう場合には、 S 2 2 1 0の代わりに、第 2実施形態における図 1 2の S 2 1 1 0から S 2 1 1 4の処理を実行しても良い。これにより、より一層精度良く、着地時の推定支持脚座標系を決定することができる。

またさらに、 S 2 2 1 0の代わりに、第 2実施形態における S 2 1 1 0から S 2 1 1 4の処理を実行すると共に、 S 2 2 1 4において、上記のごとく少なくとも関節変位検出値を基にキネマティクス演算によって推定上体位置 · 姿勢を求めても良い。このとき、前記第 2実施形態で説明した如く、床反力検出値を基に、足平 2 2の変形量を考慮して、推定着地時遊脚足平位置姿勢を求め、これを用いて次回歩容推定支持脚座標系を決定するようにすることがより好ましい。

言い換えると、実際の口ポットの挙動を、口ポットの運動を記述するローカル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動しているロボットが、各瞬間（各制御周期）に、姿勢検出手段（S 2 2 0 0の処理）によって得られた上体姿勢の変化量（制御周期当たりの変化量でこれは変化速度に相当する）と目標歩容の上体姿勢の変化量（制御周期当たりの変化量でこれは変化速度に相当する）との差としての姿勢回転偏差変化量で、姿勢回転中心決定手段（S 2 2 0 4の処理）によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に- 回転したものであるとみなす。そして、その上で各瞬間に（制御周期毎に）新たな推定支持脚座標系と推定上体位置を決定（更新）するとともに、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向き、さらに言い換えると新たな足跡（foot print) が、 1歩毎（着地毎）に推定されるようにしても良い。

これにより、より一層精度良く、着地時の推定支持脚座標系を決定することができるとともに、推定上体位置姿勢が不連続になることを防止することができる。

なお、以上説明した第 3実施形態は、本発明の第 1〜第 6発明、並びに、第 8、第 9発明に対応するものである。

次に本発明の第 4実施形態を図 1 6〜図 1 9を参照して説明する。なお、図 9の S 0 1 6の自己位置姿勢推定処理のみが前記第 1実施形態と相違し、これ以外の構成及び制御ユニット 6 0の処理は、第 1実施形態と同じである。

前記第 3実施形態の如く決定された推定上体位置を、以降、「幾何学的推定上体位置」という。幾何学的推定上体位置は、グロ一バル座標系から見た位置である。

一方、加速度センサとジャイロセンサの検出値を基に、公知の技術である慣性航法によって推定されるグロ一バル座標系から見た上体の位置 · 姿勢を慣性航法的推定上体位置姿勢と呼ぶ。なお、第 3実施形態の S 2 2 0 0で求められる推定上体姿勢は、慣性航法的推定上体姿勢である。

幾何学的推定上体位置は、前記第 3実施形態で説明した如く、床を基準にして幾何学的に推定される上体位置であるので、走行などの空中期においては、推定精度が低下し易い。

一方、慣性航法的推定上体位置姿勢は、歩行、走行を問わず、片脚支持脚期、両脚支持期および空中期のいずれの時期においても、推定原理が同一であるので、いかなる時期においても短期的な精度が低下することはないが、積分が用いられるため、長期的なドリフトが発生しやすい, これらの特性を考慮し、第 4実施形態は、慣性航法的推定上体位置姿勢を幾何学的推定上体位置で補正するようにした。

また、幾何学的推定上体位置及び推定上体姿勢の運動から算出される口ポット 1の運動加速度と、加速度センサの検出値とを用いて、ジャィ口センサの検出ドリフトを補正すると共に、推定上体姿勢のうちの傾き成分のドリフトを補正するようにした。さらに、状況によって、ジャィ口センサの検出値のョー方向のドリフト補正も行うようにした。

図 1 6は、第 4実施形態における図 9の S 0 1 6の自己位置推定処理のフローチャート、図 1 7は、同推定処理のブロック図である。

図 1 6および図 1 7を用いて、第 4実施形態における図 9の S 0 1 6 の自己位置推定処理を説明すると、まず図 1 6の S 2 3 0 0において、ジャイロセンサの検出値を積分して推定上体姿勢を求める。ただし、前制御周期（前回以前の制御周期）に求めた幾何学的推定上体位置の運動から算出される運動加速度と、加速度センサの検出値を用いて、ジャィ口センサの検出ドリフトを補正することで、推定上体姿勢のうちの傾き成分のドリフ卜を補正する。

S 2 3 0 0の処理を詳説すると、まず、前回の制御周期およびそれ以前に、後述する S 2 3 0 2において決定した幾何学的推定上体位置の運動を基に、幾何学的推定上体位置の 2階微分である幾何学的推定上体加速度を算出する。この処理は、図 1 7のブロック 3 0 1で実行される。なお、幾何学的推定上体位置を求める処理は、図 1 7のブロック 3 0 0 で実行される。補足すると、上体代表点と加速度センサの位置が一致していれば、幾何学的推定上体加速度（厳密には、幾何学的に推定される上体 3の中の加速度センサ位置の加速度）を算出する上で、推定上体姿勢は必要ない。

次いで、加速度センサ検出値（上体加速度検出値）を、推定上体姿勢を用いてグロ一バル座標系に変換してなる加速度センサ検出値グロ一バル座標系換算値を求める。この処理は、図 1 7のブロック 3 0 2で実行される。なお、説明を簡単にするために、ここでは、加速度センサの位置と上体代表点の位置は一致しているものとする。異なる場合には、キネマティクス演算によって、加速度センサ検出値およびジャィ口センサ検出値を上体代表点における加速度 · 角速度に変換するか、あるいは、上体代表点における加速度 · 角速度を加速度センサの位置における加速度、ジャィ口センサの位置における角速度に変換すれば良い。

次いで、図 1 8 ( a ) および図 1 8 ( b ) に示すように、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じる (ベクトルの減算を行う）ことにより、推定重力加速度を求める。加速度センサ検出値グローバル座標系換算値には、加速度センサに作用する重力による加速度成分が含まれるが、幾何学的推定上体加速度には、重力による加速度成分が含まれない。従って、加速度センサ検出値グロ一バル座標系換算値から幾何学的推定上体加速度を減じることで、推定重力加速度が求められる。この処理は、図 1 7のブロック 3 0 3で実行される。なお、図 1 8 ( a ) は、推定上体姿勢に誤差がない場合、図 1 8 ( b ) は、推定上体姿勢に誤差（以降、これを推定上体姿勢誤差、あるいは推定上体姿勢誤差角度と呼ぶ）がある場合を示す。

一方、図 1 8 ( a ) および図 1 8 ( b ) に示すように、推定上体姿勢を基に口ポット 1が推定したグローバル座標系の鉛直方向（Z軸方向）に発生すると想定される重力加速度を想定重力加速度と呼ぶ。

前述したように、幾何学的推定上体加速度はグローバル座標系で記述される。また、実際の口ポット 1は、推定上体姿勢が実際の上体姿勢を正しく推定しているとみなした上で、目標歩容に追従するように運動している。言い換えると、推定上体位置姿勢を基に推定されるグローバル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。より厳密に表現するならば、口ポット 1の自己位置姿勢推定処理においては、推定上体位置姿勢を基に推定されるグロ一バル座標系が正しいと信じ、実際のロポット 1の挙動を、正しいと信じているグローバル座標系に設定されるロー力ル座標系である推定支持脚座標系上で目標歩容通りに運動しているロポット 1 が、各瞬間に、姿勢検出手段によって得られた（検出あるいは推定された）上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差としての姿勢回転偏差変化量で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなしている。

したがって、推定上体姿勢が正しく、かつ加速度センサの検出値に誤差がなければ、図 1 8 ( a ) に示すように、前記想定重力加速度と前記推定重力加速度とは一致する。

これに対し、加速度センサの検出値に誤差がなくても、推定上体姿勢に誤差があれば、図 1 8 ( b ) に示すように、前記想定重力加速度の向きと前記推定重力加速度の向きとには、推定上体姿勢誤差角度だけずれが生じる。すなわち、式 4 2が成立する。推定上体姿勢誤差角度 = 想定重力加速度と推定重力加速度との間の角度

…式 4 2 図 1 8 ( a ) および図 1 8 ( b ) に示すように、推定重力加速度と想定重力加速度との差を重力加速度推定誤差と呼ぶと、上記の関係から、重力加速度推定誤差は、加速度センサの検出値に誤差がなければ、推定上体姿勢の誤差によって発生し、推定上体姿勢を基にロポット 1が推定したグローバル座標系から見た重力加速度推定誤差の水平成分（推定重力加速度の想定重力加速度に直交する成分）と推定上体姿勢誤差角度とには、式 4 3の関係があることがわかる。ただし、式 4 3は、重力加速度推定誤差の前後方向成分（X成分）と推定上体姿勢誤差角度 Y軸まわり成分との関係を表す。重力加速度推定誤差の左右方向成分（Y成分）と推定上体姿勢誤差角度 X軸まわり成分の関係を表す場合においては、右辺のマイナス記号を削除すれば良い。ここで、重力加速度は、正である。

重力加速度推定誤差の水平成分

= - tan( 推定上体姿勢誤差角度）*重力加速度

…式 4 3 本実施形態においては、式 4 2あるいは式 4 3から算出される推定上体姿勢誤差角度を用いて、推定上体姿勢誤差が 0に収束するように推定上体姿勢を補正するようにした。

S 2 3 0 0の説明に戻ると、式 4 2を用いて、現在の瞬間（厳密には. 1制御周期前の瞬間）における想定重力加速度と推定重力加速度との間の角度から推定上体姿勢誤差角度を算出する。あるいは、式 4 3を用いて、現在の瞬間（厳密には、 1制御周期前の瞬間）における重力加速度推定誤差の水平成分から推定上体姿勢誤差角度を算出しても良い。推定上体姿勢誤差角度の算出処理は、図 1 7のブロック 3 0 4で実行される, 次いで、前記推定上体姿勢誤差角度を、図 1 7のブロック 3 0 5にてセンサ座標系（座標軸をジャイロセンサの検出軸に合わせた座標系）に変換した後、ブロック 3 0 6にて、その変換後の値に積分ゲイン Ka を乗じた値を積分することにより、推定ジャイロセンサドリフト（ジャィ口センサのドリフトの推定値）を求める。さらに、この推定ジャイロセンサドリフトを、ブロック 3 0 7にて、ジャィ口センサ検出値 co in (上体角速度検出値）から減じることにより、ドリフトを補正した角速度を求める。なお、ブロック 3 0 7では、ョーレートのドリフトも適宜減算されるが、これについては後述する。さらに、このドリフトを補正した角速度を、ブロック 3 5 0にて、推定上体姿勢を用いてグローバル座標系に変換することにより、グローバル上体角速度 co glを求める。

次いで、前記推定上体姿勢誤差角度にゲイン Kb を図 1 7のブロック 3 0 9で乗じた値をブロック 3 0 8にてグロ一バル上体角速度 o gl から減じ、その減算後の値（ブロック 3 0 8の出力）をブロック 3 1 0 (積分器）で積分することにより新たな推定上体姿勢 6> _{e s}tm を求める, なお、推定上体姿勢 0 estm や角速度などは、クォータ二オン、回転行列、あるいはオイラー角などで表現される。

ジャイロセンサのドリフト（推定ジャイロセンサドリフト）を上記のごとき構成により推定する場合、推定が効果的に働くためには、推定上体姿勢誤差角度のセンサ座標系への変換値（ベクトル）のそれぞれの要素（それぞれのセンサ検出軸まわりの推定上体姿勢誤差角度、言い換えるとセンサローカル推定上体姿勢誤差角度）が、その要素に対応するジャイロセンサのドリフ卜の影響のみを受けて、他のジャイロセンサのドリフトの影響を受けない、あるいは、ほとんど受けないことが前提条件となる。

言い換えると、推定上体姿勢誤差角度の X ( Y ) 軸まわり成分の誤差は、 X ( Y ) 軸用ジャイロセンサのドリフトの影響を受けるが、 Y ( X ) 軸用ジャイロセンサのドリフトの影響を受けないことが前提条件となる。

もし、 X軸用ジャイロセンサのドリフトの影響により、推定上体姿勢誤差角度の X軸まわり成分が発生した後、上体をセンサ座標系の Z軸まわりに急に 9 0度回転させると、推定上体姿勢誤差角度は、グローバル座標系において蓄積されたままであるので、結果的に推定上体姿勢誤差角度のセンサ口一カル X軸まわり成分が、推定上体姿勢誤差角度のセンサローカル Y軸まわり成分に移る。したがって、前記前提条件が成立するためには、センサ座標系 Z軸まわりの回転速度の絶対値が十分に小さいことが必要条件となる。

よって、 Z軸（上体の上下方向軸）用ジャイロセンサ検出値の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする (すなわちジャィ口センサのドリフト補正を行わない）ようにするのが望ましい。

また、通常、口ポット 1の移動時や作業時には、上体 3の上下軸（体幹軸）は鉛直姿勢あるいはその近傍の姿勢になっているので、ジャイロセンサの角速度検出値をグローバル座標系に変換したグローバル上体角速度 c glz は、 Z軸用ジャイロセンサの角速度検出値の値に近い値となる。

よって、図 1 9に示すように、ジャイロセンサの角速度検出値をグロ —バル座標系に変換したグローバル上体角速度の鉛直軸まわり角速度 ω glz の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。

また、ジャイロセンサの角速度検出値の代わりに、上体 3の目標歩容の上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。

以上をまとめて言い換えると、ジャィ口センサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）と良い。

また、加速度センサ検出値グロ一バル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい場合には、センサの検出誤差や口ポット 1本体の剛性不足などにより、推定重力加速度の誤差が大きくなる傾向があるので、上記ゲイン

Kaを小さく、あるいは 0に設定するのが良い。

また、前述したごとく、実際の口ポット 1は、推定上体位置姿勢を基に推定されるグロ一バル座標系が正しいとみなした上で、前記推定されたグロ一バル座標系上で目標歩容に追従するように運動している。したがって、支持脚が接地している状態においては、推定上体姿勢に大きな誤差があっても、実際の上体加速度は、実際の重力加速度に支配されず. 前記推定されたグローバル座標系上での幾何学的推定上体加速度に、ほぼ一致する。一方、空中期においては、実際の上体加速度は、実際の重力加速度に支配されて加速するので、前記推定されたグローバル座標系上での幾何学的推定上体加速度の方向と大きくずれ、図 1 8 ( b ) の関係が成立しなくなる。したがって、空中期においては、幾何学的推定上体位置の精度は、比較的高精度の加速度センサとジャィ口センサを基に慣性航法によって求めた上体位置の精度よりも低下しがちとなる。したがって、空中期においても、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

また、前述のように足平 2 2の変形を考慮して上体位置を推定する場合には、空中期でなくとも、脚体先端部の足平 2 2が床と良好に接触していない状態では、足底付近の変形量推定の誤差が大きくなる傾向があるので、幾何学的推定上体加速度の精度は低下する。したがって、支持脚の足平 2 2と床との間の接触面積が小さい場合にも、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

なお、脚体 2の足平 2 2が床と良好に接触している状態とは、具体的には、以下のような状態の少なくともいずれかが満足する状態、あるいは以下のうちの複数が同時に満足する状態を指す。

a)足平 2 2の底面と床との間の接触面積が大きい状態、例えば、足平 2 が床にベたに接触している状態（いわゆるベた足状態）

b)床反力の分布圧が比較的均一である状態

c)接地箇所が多い状態

d)目標 Z M P (あるいは実床反力中心点）が足平 2 2の中央あるいは中央に近い状態

e)接地圧または床反力が高い状態

f)足平 2 2の位置姿勢変化率が小さい状態、厳密には、足平 2 2の底面と床の相対位置姿勢関係の変化が小さい状態

上記状態の判定には、例えば目標歩容の時期（フェーズ）、目標 Z M P、床反力検出値を基に行えばよい。あるいは、本実施例では備えていないが、分布圧センサゃ接触センサの検出値を基に上記状態の判定を行つてもよい。

以上のゲイン設定法をまとめると、ジャィ口センサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい状態、加速度センサ検出値グローバル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい状態、空中期、あるいは、脚先端部（足平）が床と良好に接触していない状態では、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

すなわち、これらの状態を基に総合的に判断して積分ゲイン Ka を決定するのが良い。

またこれらの状態の瞬時値、あるいは長期的傾向に応じて積分ゲイン Kaを決定しても良い。

同じ理由により、積分ゲイン Kb も前記ゲイン Ka と同様に設定することが好ましい。

本実施形態での S 2 3 0 0においては、以上の処理の他に、以下のように、状況によりョ一レート補正（ョ一方向のドリフト補正）も行われる。

すなわち、まず、ョ一レート補正を行うか否かを、図 1 7のブロック 3 1 1で判定する。

具体的には、以下の条件の少なくともいずれかひとつあるいは複数を判断条件として用意し、この判断条件を満足している場合には、ョーレ一卜補正を行うと判断する。

a)検出された床反力の鉛直軸まわりモーメント（あるいはその微分値）の絶対値がある所定の値以内である状況

b)目標動作がほぼ静止である状況 c)足底と床の間の接触状況が良好で滑りが生じていないと考えられる状況

d)目標歩容の姿勢角速度とジャイロ検出値（姿勢角速度検出値）の差の絶対値がある所定の値以内である状況

e)目標歩容の姿勢角速度の絶対値がある所定の値以内である状況 f)ジャイロ検出値（姿勢角速度検出値）の絶対値がある所定の値以内である状況

基本的には、支持脚足平 2 2と床との接地面で滑り（回転滑り）が生じないかもしくは生じ難いという状況で、ョーレ一ト補正を行うと判断する。

なお、 d ) および f ) におけるジャイロ検出値は、ジャイロセンサの検出値そのもの（ジャィ口センサの生の出力そのものが表す姿勢角速度検出値）をョ一レート補正した値であることが望ましい。

次いで、ョ一レート補正を行うと判定された場合には、図 1 7に示すように、ブロック 3 1 2にて、目標歩容、目標上体姿勢、目標関節変位あるいは関節変位検出値の少なくともいずれかと最新の着地時に記憶した推定支持脚座標系（以降、着地時推定支持脚座標系と呼ぶ）を基に、推定支持脚座標系に対応する足平 2 2と床との間に滑りが生じていないと想定した場合の上体姿勢（以降、滑りなし推定上体姿勢と呼ぶ）を算出する。そして、前記推定上体姿勢と前記滑りなし推定上体姿勢との差をブロック 3 1 3で求め、この差をブロック 3 1 4でセンサ座標系に変換したものを、フィ一ドバック制御則のプロック 3 1 5に入力することで、ョーレートドリフトを求める。ブロック 3 1 5のフィードバック制御則としては、例えば P I D制御則が用いられ、前記差（ブロック 3 1 4の出力）の時間微分値（または制御周期の間の前記差の変化量）にゲイン Ke を乗じた値と、前記差（ブロック 3 1 4の出力）にゲイン Kf を乗じた値と、前記差（ブロック 3 1 4の出力）の積分値にゲイン Kg を乗じた値との和を求めることによりョ一レートドリフ卜が求められるそして、この求めたョ一レートドリフトを、前記ブロック 3 0 7で前記角速度センサ検出値 co in から減じる。また、ョーレ一ト補正を行わないと判定された場合には、フィードバック制御則のブロック 3 1 5への入力を遮断して（図 1 7のスィッチ 3 1 6を開成する）、直前のョ一レートドリフト値を保持し、その値を前記角速度センサ検出値 o in から減じるようにする。なお、滑りなし推定上体姿勢を求める際には、図 1 2の S 2 1 1 0において着地時の遊脚足平位置 ·姿勢を推定する際と同様、床反力検出値および Zまたは目標歩容の床反力を基に、足平 2 2の変形機構（前記図 3及び図 4に示した弹性部材 1 0 6、接地部材（足底弾性体） 7 1、袋状部材 1 0 9等）の変形量を変形機構の力学モデル (ばねダンパ一モデル等）を用いて求め、前記変形量を含めて（考慮して）、滑りなし推定上体姿勢を求めても良い。また、モータ電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機ゃリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、滑りなし推定上体姿勢を求めるようにしても良い。

または、少なくとも目標歩容の関節変位とコンプライアンス補償量 (本出願人が先に提案した特開平 10-277969 号を参照）を基に、キネマティクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。

図 1 6の説明に戻ると、以上のごとく S 2 3 0 0の処理を完了した後，次いで、 S 2 3 0 2に進み、前述の第 3実施形態の図 1 4の S 2 2 0 2 から S 2 2 1 4を実行して、推定支持脚座標系の位置姿勢および幾何学的推定上体位置を決定する。なお、このとき、第 3実施形態に関して補足した如く、図 1 4の S 2 2 1 0の代わりに、第 2実施形態における S 2 1 1 0から S 2 1 1 4の処理を実行すると共に、 S 2 2 1 4において、上記のごとく少なくとも関節変位検出値を基にキネマティクス演算によつて推定上体位置 ·姿勢を求めても良い。このとき、前記第 2実施形態で説明した如く、床反力検出値を基に、足平 2 2の変形量を考慮して、推定着地時遊脚足平位置姿勢を求め、これを用いて次回歩容推定支持脚座標系を決定するようにすることがより好ましい。

次いで、 S 2 3 0 4に進み、加速度センサとジャイロセンサを基に慣性航法により慣性航法的推定上体位置姿勢を求めつつ、幾何学的推定上体位置と慣性航法的推定上体位置との差が 0に収束するように慣性航法的上体位置を修正する。

具体的には、図 1 7 に示すごとく慣性航法的推定上体位置 Xinertestm の前回値と前記幾何学的推定上体位置との差をブロック 3 1 7で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲイン Kc を乗じた値と、該差にゲイン Kd を乗じた値との和をブロック 3 1 8で求める。つまり、上記差からフィ一ドバック制御則としての P D制御則より、上記和を求める。また、前記ブロック 3 0 2の出力である前記加速度センサ検出値グロ一バル座標系換算値と想定重力加速度 G の差をプロック 3 1 9で求める。そして、この差（ブロック 3 1 9の出力）から上記和（ブロック 3 1 8の出力）をブロック 3 6 0 にて減じた値を、ブロック 3 2 0にて 2階積分することにより、新たな慣性航法的推定上体位置 Xinertestmを決定する。

なお、ゲイン Kc および Kd は、幾何学的推定上体位置の誤差が大きく発生し易い状況では小さく、あるいは 0に設定するのが良い。支持脚の足平 2 2の底面と床との間の接触面積が小さい場合、幾何学的推定上体位置の誤差は、大きくなる。したがって、支持脚の足平 2 2の底面と床との間の接触面積が小さい場合には、上記ゲイン Kc および Kd を小さく、あるいは 0に設定するのが良い。また、特に、足平 2 2が床にベたに接触している状態（いわゆるベた足状態）の時に、上記ゲイン Kc および Kdを大きくするのが良い。

以上より、例えば、走行の場合には、補正ゲイン Ka、 Kb、 Kc および Kd は、図 8 ( c ) に示す補正ゲイン Kのグラフのように、足底全面接地期間では高く、空中期には、 0または概ね 0にすれば良い。なお図 8 ( c ) の補正ゲイン Kのグラフは、 Ka、 Kb、 Kcおよび Kdの大きさの変化の傾向を表してはいるが、厳密な値を表しているわけではない。また、図 8 ( c ) では補正ゲイン Kの最大値が 1 となるように規格化されている。したがって、 Kは、補正ゲイン Ka、 Kb、 Kc、 Kd の絞り (減衰器）を意味すると考えても良い。

本実施例においては、前記式 4 2あるいは式 4 3を用いて算出される推定上体姿勢誤差を基に推定上体姿勢を補正するようにしたが、式 4 2 および式 4 3を用いず、直接、重力加速度推定誤差の水平成分を基に推定上体姿勢を補正するようにしても良い。すなわち、式 4 3の代わりに次の式 4 4を用いても良い。重力加速度推定誤差の水平成分 =一推定上体姿勢誤差角度 *重力加式 4 4 なお、ゲイン Ka の設定について補足すると、空中期においては、推定上体姿勢の誤差がいかなる値であろうとも、ロポット 1 とともに放物運動をする座標系から見ればロポット 1全体は無重力状態にいるのと同一であって、加速度センサの出力は、推定上体姿勢の誤差の影響を受けない。したがって、加速度センサの検出精度が高く、目標歩容に対する実際の口ポット 1の追従性が高く、口ポット 1の剛性が高く、且つ目標歩容を生成するために用いられるロポット動力学モデルのパラメータが実際の口ポット 1 とほぼ一致しているという条件（以下、ここでは条件 Aという）が満足されるならば、推定重力加速度と想定重力加速度とは常にほぼ一致する。厳密には、推定上体姿勢の誤差に応じて、推定重力加速度と想定重力加速度とは同じ量だけ真値からずれるので、結果的に推定重力加速度と想定重力加速度とは常にほぼ一致する。したがって、本質的に、空中期においては、重力方向を推定することはできない。しかし、推定重力加速度と想定重力加速度とは常にほぼ一致するので、その瞬間の推定上体姿勢誤差はほぼ 0となり、上記ゲイン Ka を小さく設定しなくても、推定上体姿勢誤差にゲイン Ka を乗じて求められる補正量もほぼ 0となり、推定上体姿勢に大きく悪影響を与える恐れは低い。

また、上記条件 Aを満足する場合には、ゲイン Ka を一定にしておいた方が、むしろ推定上体姿勢の低周波数成分（D C成分）の精度が高くなる場合もある。なぜなら、上記のごとく、ゲイン Ka を乗じた値を積分した値を 0にするように補正した構成は、ゲイン Ka を一定にすると. 理論上、重力加速度推定誤差の水平成分の長時間平均値を 0にするように作用し、また、推定上体姿勢誤差が 0の時には、重力加速度推定誤差の水平成分の長時間平均値は理論上 0になるからである。しかし、通常は、上記条件 Aを満足することは難しいので、図 8 ( c ) のようにゲイン K aを設定する方が望ましい。

なお、以上説明した第 4実施形態は、本発明の第 1〜第 6発明、第 8 〜第 2 2発明に対応するものである。次に本発明の第 5実施形態を説明する。図 2 0及び図 2 1を参照して説明する。図 2 0は、第 5実施形態における図 9の S O 1 6の自己位置推定処理のフ口一チャート、図 2 1は、第 5実施形態における S 0 1 6 の自己位置推定処理のうちの、全体重心位置を推定する手段を示すプロック図である。なお、第 5実施形態は、第 4実施形態において慣性航法的推定上体位置 Xinertestm を決定する代わりに、後述する慣性航法的推定全体重心位置 XGinertestm を、制御周期毎に決定するようにした, これ以外は、第 4実施形態と同じである。また、図 2 1では、図 1 7と同一構成部分については、図 1 7と同一の参照符号を用いている。

図 2 0および図 2 1を参照して、第 5実施形態における S 0 1 6の自己位置推定処理を説明すると、まず、図 2 0の S 2 4 0 0において、第 4実施例の図 1 6の S 2 3 0 0と同様に、ジャイロセンサの検出値を積分して推定上体姿勢を求めつつ、ジャイロセンサの検出ドリフトと推定上体姿勢のうちの傾き成分のドリフ卜を補正する。

次いで S 2 40 2に進み、前述の図 1 4の S 2 2 0 2から S 2 2 1 4 を実行して、推定支持脚座標系の位置姿勢および幾何学的推定上体位置を決定する。

次いで S 2 40 4に進み、前記推定支持脚座標系の位置姿勢、目標上体位置姿勢（あるいは前記幾何学的推定上体位置）および関節変位検出値（あるいは目標値）を基に、前記第 3実施形態のごとく実際のロポットの挙動（姿勢回転）を考慮して、全体重心位置をキネマテイクス演算によって算出する。この処理は、図 2 1のブロック 3 3 0で実行される, このようにして算出された全体重心位置を幾何学的推定全体重心位置と呼ぶ。

次いで S 2 4 0 6に進み、関節角検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）および前記加速度センサ検出値グロ一バル座標系換算値を基に、全体重心の加速度をキネマティクス演算によつて算出する。この処理は、図 2 1のブロック 3 3 1で実行される。以降、このようにして求められた全体重心の加速度を慣性航法的推定全体重心加速度と呼ぶ。

次いで S 2 4 0 8に進み、前記慣性航法的推定全体重心加速度を基に慣性航法により慣性航法的推定全体重心位置 XGinertestm を求めつつ，幾何学的推定全体重心位置と慣性航法的推定全体重心位置の差が 0に収束するように慣性航法的推定全体重心位置を修正する。より具体的には、図 2 1に示すように、前制御周期（前回以前の制御周期）に決定した慣性航法的推定全体重心位置 XGinertestm と前記幾何学的推定上体位置との差をブロック 3 3 2で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲイン Kc を乗じた値と、該差にゲイン Kd を乗じた値との和をブロック 3 3 3で求める。つまり、上記差からフィ —ドバック制御則としての P D制御則より、上記和を求める。そして、ブロック 3 3 1の出力である慣性航法的推定全体重心加速度から上記和 (ブロック 3 3 3の出力）をブロック 3 3 4で減じた値をブロック 3 3 5 にて 2階積分することにより、新たな慣性航法的推定全体重心位置 XGinertestmを決定する。

最後に S 2 4 1 0に進み、前記慣性航法的推定全体重心位置、関節変位検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）を基に、グローバル座標系における推定上体位置を求める。この処理は、図 2 1のブロック 3 3 6で実行される。

以上が、第 5実施形態における図 9の S 0 1 6の自己位置推定処理でめ

第 5実施形態においては、空中期であるか否かを判定して、空中期である場合には、加速度センサを用いずに、全体重心は放物運動を行うという想定のもとで、前記慣性航法的推定全体重心加速度を決定しても良い。

ロポット 1が環境から外力を受けないことが明白な場合には、空中期においては、全体重心は放物運動を行うので、これにより、より一層精度良く全体重心位置を推定することができる。

なお、空中期であるか否かは、目標歩容の時期（フェーズ）、目標歩容の床反力あるいは、床反力検出値の少なくともいずれかを用いて判定すれば良い。

特に、全体重心位置の推定精度を極力高くしたい場合には、床反カ検出値が所定の値以下であるか否かによって空中期であるか否かを判定すれば良い。

なお、以上説明した第 5実施形態は、本発明の第 1〜第 6発明、第 8 〜第 2 2発明に対応するものである。

次に本発明の第 6実施形態を図 2 2を参照して説明する。図 2 2は、第 6実施例における図 9の S 0 1 6の自己位置推定処理のうちの、全体重心位置を推定する手段を示すブロック図である。

第 6実施形態においては、第 5実施形態において加速度センサ検出値を基に慣性航法的推定全体重心位置を算出する代わりに、前記 6軸カセンサ（床反力センサ） 5 0の床反力検出値を基に動力学的推定全体重心位置を算出するようにした。これ以外は、第 5実施例と同じである。また、図 2 2では、図 2 1 と同一構成部分については、図 2 1 と同一の参照符号を用いている。

図 2 2を用いて、制御周期毎の全体重心位置の推定処理を説明するとまず、床反力センサ（ 6軸力センサ 5 0 ) の検出値を、関節角検出値 (あるいは目標値）および推定上体姿勢（あるいは目標上体姿勢）を用いて、ブロック 3 4 0にてグロ一バル座標系の値に換算する。以降、この換算された床反力センサ検出値を、床反力センサ検出値グローバル座標系換算値と呼ぶ。

次いで、前記床反力センサ検出値グローバル座標系換算値をブロック 3 4 1 にて口ポット 1の全体質量で割った値から、ブロック 3 4 2にて重力加速度を減じることにより、全体重心加速度の推定値を求める。以降、このようにして求めた全体重心加速度を動力学的推定全体重心加速度と呼ぶ。

次いで、第 5実施形態と同様、ブロック 3 3 0にて幾何学的推定全体重心位置を求める。

次いで、前記動力学的推定全体重心加速度を 2階積分することで動力学的推定全体重心位置を求めつつ、幾何学的推定全体重心位置姿勢と慣性航法的推定全体重心位置との差が 0に収束するように前記動力学的全体重心位置を修正する。

より具体的には、図 2 2に示すように、前制御周期（前回以前の制御周期）に決定した前記動力学的推定全体重心位置と前記幾何学的推定全体重心位置との差をブロック 3 4 3で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲイン Kc を乗じた値と、該差にゲイン Kd を乗じた値の和をブロック 3 4 4で求める。つまり、上記差からフィードバック制御則としての P D制御則より、上記和を求める。そして、ブロック 3 4 2の出力である前記動力学的推定全体重心加速度から上記和（ブロック 3 4 4の出力）をブロック 3 4 5で減じた値をブロック 3 4 6にて 2階積分することにより、新たな動力学的推定全体重心位置 XGinertestmを決定する。

最後に、前記動力学的推定全体重心位置、関節角検出値（あるいは目標値）、推定上体姿勢（あるいは目標上体姿勢）を基に、ブロック 3 4 7にて、グローバル座標系における上体位置を推定する。以上が、第 6実施形態における S 0 1 6の自己位置推定処理のうちの. 全体重心位置および上体位置を推定する手段である。

第 6実施形態においても、空中期であるか否かを判定して、空中期である場合には、床反力センサ検出値を用いずに、全体重心は放物運動を行うという想定のもとで、前記動力学的推定全体重心加速度を決定しても良い。

ロポッ卜が環境から外力を受けないことが明白な場合には、空中期においては、全体重心は放物運動を行うので、これにより、より一層精度良く全体重心位置を推定することができる。

なお、以上説明した第 6実施形態は、本発明の第 1〜第 6発明、第 8 . 第 9発明、並びに、第 2 3〜第 3 0発明に対応するものである。

前記各実施形態で決定した姿勢回転中心について、以下に補足する。いずれかの脚体 2が接地し、十分大きい床反力が発生している時期においては、いわゆる支持多角形（接地面を含む最小凸多角形、 Z M P存在可能範囲あるいは全床反力中心点存在可能範囲）の中に、姿勢回転中心があると考えられる。

この理由を以下に述べると、姿勢回転中心が支持多角形の外にあると足平 2 2の底面のあらゆる点が床に対して相対移動していることになるので、足平 2 2の底面と床との間のあらゆる接触点には、動摩擦が働く , 動摩擦は、口ポット 1が床反力を制御しょうとして、脚体 2を動かしても、一定値のままで、任意の値に制御することはできない。すなわち、氷の上でつるつる滑ってうまく歩けない状態と同じである。したがって. 口ポット 1が床反力を制御しつつ安定に移動している場合には、足平 2 2の底面と床との間のある領域においては、滑りが生じないで静摩擦が作用していると考えれる。すなわち、支持多角形の中に、姿勢回転中心があると考えられる。補足すると、ロボット 1の足平 2 2が完全剛体であるならば、姿勢回転中心点以外のあらゆる接触点は滑りが生じるが、実際の足平 2 2の底面は、ゴムなどの弾性体でできているので、姿勢回転中心の近傍でも滑りは生じないと考えれる。

また、空中期においては、口ポットは、重心を中心として摂動運動をすると考えられる。

また、上記以外の時期、すなわちいずれかの脚体 2が接地しているが. 床反力が十分に発生していない時期においては、運動の連続性を考慮すると、姿勢回転中心は、支持多角形と全体重心位置（あるいは上体代表点位置）との間に存在すると考えられる。

以上の考察から、第 3実施形態以降で実行される S 2 2 0 4 (図 1 4 参照）において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい。

現在時刻 t における

a)目標 Z M P、

b)実 Z M P (すなわち、実床反力中心点、あるいは床反力の圧力中心点）、

c)支持脚座標系の所定の点、例えば原点

d)全体重心点、

e)上体代表点

f)上記のいずれかの複数の点の内分点いずれかの脚が接地し、十分大きい床反力が発生している時期においては、姿勢回転中心は、支持多角形の中に設定するのが良い。具体的には、上記 a)および b)に設定すれば良い。あるいは、姿勢回転中心が支持多角形の中に含まれるように、上記 c)における所定の点を設定すれば良い。例えば支持脚座標系の原点（通常、足首関節の下）に設定すれば良い。

空中期においては、姿勢回転中心は、上記 d)のように設定するのが良いが、全体重心位置は上体代表点位置の近くに存在するので、上記 e)のように設定しても良い。

上記以外の時期、すなわちいずれかの脚体 2が接地しているが、床反力が十分に発生していない時期においては、上記 f)のように設定するのが良い。

さらに、全期間での運動の連続性を考慮すると、姿勢回転中心点は、連続的に変化するように設定するのが望ましい。

いずれにしても、概ねすベての各瞬間において、口ポット 1の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。

同様に、第 1実施形態の図 1 0の S 2 0 0 6および第 2実施例の図 1 2の S 2 1 0 6において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい。

a)遊脚着地時刻 nの、あるいは 1歩の間の平均の目標 Z M P位置

b)遊脚着地時刻 nの、あるいは 1歩の間の平均の実 Z M P (すなわち、実床反力中心点、あるいは床反力の圧力中心点）位置

c)支持脚座標系の所定の点、例えば原点

d)遊脚着地時刻 nの、あるいは 1歩の間の平均の全体重心点位置

e)遊脚着地時刻 nの、あるいは 1歩の間の平均の上体代表点位置 f)上記のいずれかの複数の点の内分点

いずれにしても、口ポット 1の全体が最新の 1歩の間の運動によって掃引した領域を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。

または、ある瞬間における、例えば着地瞬間における、口ポット 1の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。

第 4実施形態においては、慣性航法的推定上体位置を幾何学的推定上体位置で補正する際に、慣性航法的推定上体位置を算出するための積分器（図 1 7のブロック 3 2 0 ) に補正入力（図 1 7のブロック 3 1 8の出力）を付加的に加えていた。したがって、慣性航法的推定上体位置は. 制御周期毎に修正された。これに対し、ある所定の期間、補正を加えずに慣性航法的推定上体位置を決定し、次の式 4 5により、慣性航法的推定上体位置と幾何学的推定上体位置とを内挿した推定上体位置である内揷的推定上体位置を求めて、これを最終的に決定した推定上体位置として出力するようにしても良い。内挿的推定上体位置

= ひ *慣性航法的推定上体位置 + ( 1 — α ) *幾何学的推定上体位置

…式 4 5 ただし、ひは加重平均の重みであり、前記所定の期間の間に 1から徐々に 0に変化する。

αが 0になったときに、幾何学的推定上体位置を慣性航法的推定上体位置に代入（上書き）し、ひを 1に戻す。 5448

7 4 また、所定の期間は、空中期以外の期間に設定する。

第 5および 6実施形態においても、第 4実施形態と同様にして、慣性航法的あるいは動力学的推定全体重心位置と幾何学的推定全体重心位置とを内挿した推定全体重心位置である内挿的推定全体重心位置を決定するようにしても良い。

第 4〜第 6実施形態において、補正される慣性航法的推定上体位置もいくは慣性航法的推定重心位置もしくは動力学的推定重心位置は鉛直位置成分（離床面に対する）のみであっても良い。あるいは水平成分のみ補正しても良い。

補正ゲイン Ka, Kb, Kc もしくは Kd の値を、定常カルマンフィルタあるいは非定常カルマンフィル夕の手法を用いて決定しても良い。ただし、本実施形態の口ポット 1においては、特に移動中においては、システム雑音（外乱）と観測雑音の性質が、カルマンフィル夕の前提条件を十分に満足してはいないので、十分な効果を生むとは限らない。

加速度センサおよびまたはジャイロセンサは、上体 3以外の部位、例えば頭部 4に搭載（内蔵）されていても良い。さらに頭部 4と上体 3との間に首関節がある場合、首関節の変位指令（目標変位）または変位検出値を基に、加速度センサおよびまたはジャィ口センサの検出値をキネマティクス演算によって、上体代表点の加速度と角加速度とに変換しさえすれば、残りは、前記各実施形態と同様に自己位置推定を行えば良い, 推定位置姿勢の成分（位置鉛直成分、位置水平成分、鉛直軸まわりの向き成分、傾き成分）によって、第 1実施形態から第 6実施形態までの自己位置推定処理のうちの異なる処理を選択しても良い。例えば、上体 3の水平位置に関しては、第 3実施形態の自己位置推定処理に従って幾何学的に推定し、鉛直位置に関しては、第 4実施形態の自己位置推定処理に従って慣性航法的推定上体鉛直位置を幾何学的推定上体鉛直位置により補正しても良い。

推定支持脚座標系および幾何学的推定上体位置を決定する際に関節変位を用いる実施形態においては、その関節変位を、目標歩容の関節変位または、関節変位検出値としたが、これらの重み付き平均でも良い。また、このときの重みは周波数特性を持たせても良い。

推定支持脚座標系や推定上体位置姿勢など、推定される位置姿勢を表現する時には、前記各実施形態のごとくグローバル座標系を基準として表現する代わりに、目標位置姿勢からの摂動分で表現しても良い。

第 4実施形態以降においては、前記図 1 7に示すごとく、グロ一バル座標系における推定重力加速度を基に、グローバル座標系における推定上体姿勢誤差角度を求め、グローバル座標系における推定上体姿勢誤差角度にゲイン Kbを乗じた値を、グローバル上体角速度 co glを積分する積分器（図 1 7のブロック 3 1 0 ) に付加的に入力することによって、推定上体姿勢を補正していた。すなわち、グローバル座標系において、推定上体姿勢を補正していたが、この代わりに、ジャイロセンサのローカル座標系（上体 3に固定された座標系）上で補正しても良い。具体的には、図 1 7において、ゲイン Kb のブロック 3 0 9と、その出力をグ口一バル上体角速度 c gl から減じる加算器（ブロック 3 0 8 ) を削除し、積分器 KaZ S (ブロック 3 0 6 ) を、 Ka/S + Kb、すなわち、 P I 制御則のブロックに変更しても良い。

原理上、このようにローカル座標系で補正しても、ョ一レートが高い旋回（その場旋回含む）時に、 Kb によって傾斜ドリフトの収束が悪くなることはない。

推定上体姿勢の補正において、加速度センサ検出値グロ一バル座標系換算値から幾何学的推定上体加速度を減じることにより推定重力加速度を求める代わりに、加速度センサ検出値グローバル座標系換算値から目標歩容の上体加速度を減じることにより推定重力加速度を求めるようにしても良い。

実際には、目標歩容通りに運動しょうとしている口ポットには、姿勢回転中心まわりの姿勢回転が発生しているので、姿勢回転によって、上体加速度は、目標歩容からずれる。しかし、姿勢回転の内の姿勢傾斜は，前後左右に振動しても平均的には、 0である。また、通常、姿勢回転の内のスピンに関しては、 1歩毎にスピンの向きが逆に切り替わるので、平均的には 0に近い。したがって、遠心力など、姿勢回転の回転方向によらずほぼ同一方向に作用する力を除けば、姿勢回転による上体加速度への影響は、正の影響と負の影響とが相殺し、長期的には、ほぼ 0になる。なお、ここで、「長期」とは、推定上体姿勢の補正の整定時間以上の時間を指す。

よって、推定上体姿勢の補正のために、幾何学的推定上体加速度を用いる代わりに、目標歩容の上体加速度を用いても、補正の効果が大きく低下することはない。

第 1から第 3実施形態においても、第 4実施形態のように、幾何学的推定上体位置姿勢運動から算出される運動加速度と、加速度センサの検出値を用いて、ジャイロセンサの検出ドリフトを補正するようにしても良い。また、第 1から第 3実施形態においても、第 4実施形態のように. 推定上体姿勢のうちの傾き成分のドリフトを補正するようにしても良い, また、第 1から第 3実施形態においても、第 4実施形態のように、ョーレート補正を行っても良い。

次に本発明の第 7実施形態を図 2 3〜図 2 5を参照して説明する。図

2 3および図 2 4は、第 7実施形態における口ポット 1の頭部 4の内部構造を表す。図 2 3は正面視、図 2 4は側面視の図である。頭部 4は、パン · チルト方向に回転する首関節 1 2 0を介して、上体 3の上部に連接されている。

首関節 1 2 0 にも、他の関節と同じく、エンコーダ（関節変位検出器）付きのモ一夕 1 2 1， 1 2 2と減速機 1 2 3 , 1 2 4とを備え、図示を省略するモータ制御装置を介して前記制御ュニッ卜 6 0からの関節変位指令に追従するように制御される。

頭部 4には、環境認識手段としての左右 2台のビデオカメラ 1 2 5， 1 2 5が備えられ、対象物を立体視することができるようになつている, 前記図 5では図示を省略しているが、ビデオカメラ 1 2 5 , 1 2 5の出力（撮像情報）は、制御ユニット 6 0に入力され、該制御ユニット 6 0 で撮像情報内の対象物までの距離等が認識される。

左右 2台のビデオカメラ 1 2 5 , 1 2 5の代わりに、以下のような環境認識手段を備えても良い。

a) 3台以上のカメラによる立体視

b) 1台カメラで対象物の多点を認識し、三角測量の原理で距離を推定する手段

c)レンジファインダ、スキャン式レーザー距離計等の非接触多点距離計測装置

第 7実施形態は、上記した頭部 4の構成の他、図 9の S 0 1 6の自己位置推定処理が第 4実施形態と相違している。その他の構成及び処理は. 第 4実施形態と同じである。なお、その他の構成及び処理は、第 4実施形態の代わりに、例えば第 5または第 6実施形態と同じにしても良い。

図 2 5は第 7実施形態における図 9の S 0 1 6の自己位置推定処理を示すフローチャートである。第 7実施形態においては、第 4実施形態における推定自己位置姿勢を用いて、対象物を前記ビデオカメラ 1 2 5 , 1 2 5の画像（以下、単にカメラ画像という）の中央あるいは適切な位置に捉えるように、首関節 1 2 0を制御して、ビデオカメラ 1 2 5 , 1 2 5の方向を制御する注視制御を行う。また、ビデオカメラ 1 2 5 , 1 2 5 (あるいはレンジファインダ等) により、あらかじめグローバル座標系における正確な位置を記憶しているランドマ一ク等を認識して、前記推定自己位置姿勢を補正する。また、ビデオカメラ 1 2 5， 1 2 5により、ある対象物を認識して、ビデオカメラ 1 2 5 , 1 2 5によって得た情報と前記推定自己位置姿勢から、対象物のグローバル座標系における位置姿勢または形状を認識する。

なお、第 4実施形態における推定自己位置姿勢に代わりに、第 3、第 5および第 6のいずれかの実施形態の推定自己位置姿勢を用いても良い, 以下に、図 2 5を用いて、第 7実施形態のその動作の詳細を説明する, まず、 S 2 5 0 0力ら S 2 5 0 4まで、第 4実施形態の S 2 3 0 0から S 2 3 0 4と同様の処理を行い、推定上体姿勢、推定支持脚座標系の位置姿勢、幾何学的推定上体位置および慣性航法的上体位置を決定する, 次いで S 2 5 0 6に進み、注視制御をすべきか否かを判断する。具体的には、現在の推定上体姿勢及び慣性航法的上体位置、並びに口ポット 1の目標経路および地図情報を基に、注視制御をすべきか否かを判断しまた、注視制御する場合には、いずれの対象物を注視するかを決定する, 具体的には、地図情報に、各対象物に対して、その対象物を観測でき、かつ口ポット 1 と対象物の相対位置関係が測定できる領域（以降、観測可能領域と呼ぶ）が記述してある。そして、現在の推定上体姿勢及び慣性航法的上体位置と今後の目標経路とに基づき、現在および今後の所定の期間、いずれかの観測可能領域にいるか否かを予想し、しばらくの間、観測可能領域にい.ると予想される場合には、注視制御をすべきと判断する。

注視すべき対象物の指定を、あらかじめ、口ポット 1の移動制御部 (図 6に示した部分）の上階層に位置する口ポット 1 の行動計画部（図示しない）が、移動経路に応じて決定するか、あるいは、オペレータがマンマシンインターフエ一スを介して指定しておいても良い。

なお、地図情報は、ランドマーク、障害物、出発地、目的地、移動可能領域、道などに関する、位置、形状特徴および観測可能領域などの情報であり、移動前に、あらかじめメモリに記憶されているものとする。次いで S 2 5 0 8に進み、前記注視すべき対象物の地図上の位置と前記推定上体位置姿勢との相対関係を求める。

次いで S 2 5 1 0に進み、前記相対関係から、カメラ画像の中央に対象物が映るように目標首関節変位をキネマティクス演算により決定する, 次いで S 2 5 1 2に進み、目標首関節変位に追従するように、首関節モ一夕 1 2 1 , 1 2 2を制御する。具体的には、首関節モータ 1 2 1， 1 2 2のモ一夕制御装置（モ一夕ドライバ）に対して目標首関節変位を制御ュニット 6 0から出力する。そして、このとき首関節モータ 1 2 1 , 1 2 2のモータ制御装置（モ一夕ドライバ）が、目標首関節変位に首関節変位検出値を追従させるように首関節モ一夕 1 2 1， 1 2 2の電流を制御する。

次いで S 2 5 1 4に進み、対象物がランドマークであるか否かを判定する。具体的には、検索した対象物の属性の情報からランドマークであるか否かを判定する。

S 2 5 1 4において肯定される場合には、 S 2 5 1 6に進み、カメラ画像、地図情報、および推定上体位置姿勢、首関節変位から、幾何学演算によって、現在の実際のロボット 1の支持脚足平位置姿勢に対応する支持脚座標系（以降、視覚推定支持脚座標系と呼ぶ）のグローバル座標系のおける位置姿勢を推定する。

S 2 5 1 6の後、さらに S 2 5 1 8に進み、前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差が 0に収束するように、推定支持脚座標系の位置姿勢を修正する。

S 2 5 1 8の処理をより詳細に説明すると、まず前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差を求める。次いで、この差と、ある所定のゲイン Ke との積を求め、該積を推定支持脚座標系の位置姿勢に加えた位置姿勢を新たな推定支持脚座標系の位置姿勢とする。

すなわち、次の式 4 6によって新たな推定支持脚座標系を決定する。言い換えると、前記視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との内挿（すなわち内分あるいは重み付き平均）によって. 新たな推定支持脚座標系の位置姿勢を決定する。新たな推定支持脚座標系

= 推定支持脚座標系の位置姿勢

+ Ke * (視覚推定支持脚座標系の位置姿勢一推定支持脚座標系の位置姿勢）

…式 4 6 ゲイン K e は、各推定値の確からしさを基に、決定される新たな推定支持脚座標系が、真値として最も確率が高くなる値になるように決定しても良い。

S 2 5 1 4において否定される場合には、 S 2 5 2 0に進み、カメラ画像、地図情報、および推定上体位置姿勢、首関節変位から対象物形状 · 位置を算出する。以降、前記算出された対象物形状 ·位置を視覚推定対象物形状 ·位置と呼ぶ。

S 2 5 2 0の後、 S 2 5 2 2に進み、視覚推定対象物形状 ·位置により、地図情報の対象物形状 ·位置を修正する。具体的には、視覚推定対象物形状 · 位置と地図情報の対象物形状 ·位置との差が 0に収束するように地図情報の対象物形状 · 位置を修正する。

より具体的には、まず前記視覚推定対象物形状 ·位置と地図情報の対象物形状 · 位置との差を求める。次いで、その差と、ある所定のゲイン Kf との積を求め、この積を地図情報の対象物形状 · 位置に加えた形状 ·位置を新たな地図情報の対象物形状 ·位置とする。

すなわち、次の式 4 7によって新たな地図情報の対象物形状 ·位置を決定する。言い換えると、前記視覚推定対象物形状 · 位置と地図情報の対象物形状 · 位置の内挿（すなわち内分あるいは重み付き平均）によつて、新たな地図情報の対象物形状 ·位置を決定する。新たな地図情報の対象物形状 ·位置

= 地図情報の対象物形状 · 位置

+ Kf * (視覚推定対象物形状 ·位置一地図情報の対象物形状 ·位置） …式 4 7

S 2 5 1 8または S 2 5 2 2の処理の完了をもって、 S O 1 6の自己位置推定処理の完了とする。

視覚推定支持脚座標系の位置は、慣性航法的推定上体位置を基に推定されており、さらに、慣性航法的推定上体位置は、推定支持脚座標系を基に算出される幾何学的推定上体位置姿勢に収束するように決定されるので、 S 2 5 1 8の処理により、推定支持脚座標系の位置姿勢を変更すると、視覚推定支持脚座標系の位置も遅れて変更される。したがって、 S 2 5 1 8の処理においては、収束を速めようとしてゲイン Ke を大きくし過ぎると発振する恐れがある。ゲイン Ke を大きくしても、発振しにくくするためには、視覚推定支持脚座標系の位置姿勢と推定支持脚座標系の位置姿勢との差に応じて推定支持脚座標系の位置姿勢を修正すると共に、直接的に慣性航法的推定上体位置も修正して、視覚推定支持脚座標系の位置の変更の遅れを短くすれば良い。

推定上体位置姿勢と視覚推定上体位置姿勢の差、または、視覚推定対象物形状 ·位置と地図情報の対象物形状 ·位置の差がある所定の許容値を越えているならば、誤認識と判断して、その値による推定上体位置姿勢または地図情報の対象物形状 ·位置の修正をしないようにしても良い, なお、以上説明した第 7実施形態は、本発明の第 3 1〜第 3 7発明に対応するものである。

以上説明した如く、第 1実施形態においては、実際の口ポット 1の挙動を、口ポット 1の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動している口ポット 1が、 1歩の間に、姿勢検出手段によって検出あるいは推定された姿勢と目標姿勢の差（あるいは前記差の鉛直成分）の分だけ、姿勢回転中心決定手段によって決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち着地点の位置と向きとを、 1歩毎 (着地毎）に推定するようにしたので、着地点の位置と向きを精度良く推定することができる。特に、加速度が水平方向や鉛直方向に激しく変動している状態、走行時の空中期などのように全脚体 2， 2が床から離れている状態、または足平 2 2と床との間の回転滑りなどによってロボット 1の全体に姿勢回転（またはスピン）が生じている状態においても. 着地点の位置と向きを精度良く推定することができる。

第 2実施形態においては、実際の口ポット 1の挙動を、ロボット 1の運動を記述する口一カル座標系である推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、少なくとも関節変位検出値の通りに運動している口ポットが、 1歩の間に、姿勢検出手段によって検出あるいは推定された姿勢と目標姿勢の差の分だけ、姿勢回転中心決定手段によつて決定された所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、着地時の遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち着地点の位置と向きを、 1 歩毎（着地毎）に推定するようにしたので、着地点の位置と向きをより一層精度良く推定することができる。

第 3実施形態においては、実際のロボット 1の挙動を、ロボット 1の運動を記述するローカル座標系である推定支持脚座標系上で目標歩容通りに運動している口ポット 1が、各瞬間に、姿勢検出手段によって検出あるいは推定された上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差の変化速度で、すなわち推定上体姿勢変化速度と目標歩容の上体姿勢変化速度の差の変化速度で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなして、新たな推定支持脚座標系と推定上体位置姿勢を各瞬間（制御周期毎）に決定（更新）する。また、着地時には、遊脚位置姿勢に応じた新たな推定支持脚座標系の位置姿勢、すなわち新たな着地点の位置と向きを、 1歩毎（着地毎）に推定したようにしたので、着地点の位置と向きをより一層精度良く推定すると共に、口ポット 1の自己位置姿勢を連続的に精度良く推定することができる。第 4実施形態においては、慣性航法によって推定上体位置（慣性航法的推定上体位置）と推定上体姿勢とを求めると共に、第 3実施形態によつて決定される推定上体位置である幾何学的推定上体位置を用いて慣性航法的な推定上体位置及び推定上体姿勢を補正するようにしたので、口ポット 1の自己位置姿勢ならびに着地位置 · 向き（推定支持脚座標系の位置 ' 向き）をより一層精度良く推定することができる。第 5実施形態においては、第 4実施形態において慣性航法的推定上体位置 Xinertestm を決定する代わりに、慣性航法による推定全体重心位置を、制御周期毎に決定するようにしたのようにしたので、ロボット 1 の自己位置姿勢ならびに着地位置 · 向き（推定支持脚座標系の位置 · 向き）を第 4実施形態と同様に、精度良く推定することができる。

第 6実施形態においては、第 5実施形態において加速度センサ検出値を基に慣性航法的推定全体重心位置を算出する代わりに、床反力センサ検出値を基に動力学的推定全体重心位置を算出するようにしたのようにしたので、口ポット 1の自己位置姿勢ならびに着地位置 · 向き（推定支持脚座標系の位置 · 向き）を第 5実施形態と同様に、精度良く推定することができる。

第 7実施形態においては、幾何学自己位置推定演算を用いることによつて、あるいは幾何学自己位置推定演算と慣性航法演算を複合的に用いることによって、連続的に（制御周期毎に）推定される自己位置姿勢と. あらかじめ記憶しておいた地図上の床面や障害物などの対象物のグロ一バル座標系における位置を基に、口ポット 1に搭載されたビデオカメラ 1 2 5 , 1 2 5等の環境認識手段が注視すべき方向を決定しているので. 前記対象物を、連続的に前記環境認識手段の画像の中央あるいは適切な位置に捉えることができる。

また、前記環境認識手段が前記対象物を認識する時に、前記環境認識手段によって得た情報と前記推定した自己位置姿勢から、グローバル座標系における対象物の位置姿勢または形状を精度良く認識することがでさる。

また、さらに、前記対象物の地図上の位置 · 形状を連続する複数の画像を基に補正するので、ノイズや誤認識の影響を受けにくい。

また、前記環境認識手段が、ランドマーク等あらかじめ正確な位置情報を記憶している対象物の位置情報と、前記環境認識手段によって得た前記対象物に対するロポット 1の相対位置情報に基づき、幾何学自己位置推定演算を用いることによって推定した自己位置姿勢、あるいは幾何学自己位置推定演算と慣性航法演算を複合的に用いることによって推定した自己位置姿勢を補正し、前記自己位置姿勢の推定の精度を高めることができる。

また、さらに、前記自己位置姿勢を連続する複数の画像を基に補正するので、ノイズや誤認識の影響を受けにくい。

また、第 4実施形態においては、運動加速度の影響を受けにくく、精度良く口ポット 1の姿勢傾斜を推定することができる。また、これは他の実施形態にも適用できる。

また、第 4実施形態においては、ョーレートが補正され、より一層精度良くロボット 1の姿勢（特に水平方向成分の向き）並びに位置を推定することができる。また、これは他の実施形態にも適用できる。産業上の利用可能性

以上の如く、本発明は、 2足移動口ポット等の脚式移動口ポットの位置を精度よく推定することができる技術を提供するものとして有用である。

Claims

BH 求の範囲

1 . 決定された目標歩容に追従するように制御される脚式移動ロポッ卜において、

前記ロポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロポットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記ロポットの位置の推定値たる推定位置を決定する位置推定手段とを備えることを特徴とする脚式移動ロポットの自己位

2 . 前記位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え、前記目標歩容の目標運動と前記口ポットの関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第 1座標系から見たロポット位置と前記第 2座標系から見たロポットの推定位置とが同一になるようにグロ一バル座標系から見たロボットの推定位置を決定することを特徴とする請求の範囲第 1項に記載の脚式移動ロポットの自己位置推定装置。

3 . 前記所定の部位は、前記口ポットの上体であることを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポットの自己位置推定装置。 '

4 . 前記姿勢回転偏差は、少なくとも前記所定の部位のョ一方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポッ卜の自己位置推定装置。

5 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では、前記ロボッ卜の重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Pのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポットの自己位置推定装置。

6 . 前記姿勢回転偏差算出手段は、各瞬間の前記姿勢回転偏差変化量を逐次求める手段であり、前記位置推定手段は、その各瞬間の姿勢回転偏差変化量を用いて前記ロポットの各瞬間の推定位置を逐次決定することを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロボットの自己位置推定装置。

7 . 前記姿勢回転偏差算出手段は、前記口ポットの着地動作により該ロポットの脚体が着地する毎に、前回の脚体の着地から今回の脚体の着地までの期間における姿勢回転偏差変化量である着地間姿勢回転偏差変化量を求める手段であり、前記位置推定手段は、該着地間姿勢回転偏差変化量を用いて各着地毎に前記ロボットの推定位置を決定することを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポットの自己位

8 . 前記位置推定手段が推定する口ポットの位置は、該ロポットの着地動作により着地した脚体の接地位置であることを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポットの自己位置推定装置。

9 . 前記ロボッ卜に作用する床反力を検出する床反力検出手段を備え、前記位置推定手段は、前記検出された床反力を基に前記ロポットの変形量を推定し、少なくともその推定された変形量を用いて前記ロポットの推定位置を決定することを特徴とする請求の範囲第 1項又は第 2項に記載の脚式移動ロポットの自己位置推定装置。

1 0 . 決定された目標歩容に追従するように制御される脚式移動ロポットにおいて、

前記ロボットに搭載された、並進加速度を検出する加速度センサと、前記ロボットに搭載された、慣性空間に対する角速度を検出する角速度センサと、

前記目標歩容の目標運動と前記ロポッ卜の関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかを基に前記ロポットの所定の部位又は前記ロポットの重心の鉛直位置の幾何学的推定値としての第 1の推定位置を決定する幾何学的位置推定手段と、

少なくとも前記加速度センサの検出値と前記角速度センサの検出値とを基に慣性航法によって前記所定の部位又は前記ロポットの全体重心の鉛直位置の慣性航法的推定値としての第 2の推定位置を決定すると共に，少なくとも前記第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とする脚式移動ロポッ卜の自己位置推定装置。

1 1 . 前記所定の部位は、前記口ポットの上体であることを特徴とする請求の範囲第 1 0項に記載の脚式移動ロポットの自己位置推定装置。

1 2 . 前記ロボットに作用する床反力を検出する床反力検出手段を備え. 前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロポットの変形量を推定し、その推定した変形量を用いて前記第 1の推定位置を決定することを特徴とする請求の範囲第 1 0項に記載の脚式移動ロボッ卜の自己位置推定装置。

1 3 . 前記慣性航法的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2の推定位置を補正することを特徴とする請求の範囲第 1 0項に記載の脚式移動ロボットの自己位置推定装置。

1 4 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることを特徴とする請求の範囲第 1 0項に記載の脚式移動ロボッ卜の自己位置推定装置。

1 5 . 決定された目標歩容に追従するように制御される脚式移動ロポッ卜において、

前記ロポッ卜に搭載された、並進加速度を検出する加速度センサと、前記ロポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の目標姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロボットが、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロポッ卜の全体重心の位置の推定値たる第 1の推定位置を決定する幾何学的位置推定手段と、少なくとも前記加速度センサの検出値と前記所定の部位の実姿勢の検出値又は推定値とを基に、慣性航法によって該所定の部位又は前記ロボットの全体重心の位置の推定値としての第 2の推定位置を算出すると共に、少なくとも第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する慣性航法的位置推定手段とを備えることを特徴とする脚式移動ロポッ卜の自己位置推定装置。

1 6 . 前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロポットの関節の変位検出値と該関節の変位目標値とのうちのの少なくともいずれかから把握される、前記第 1座標系から見た前記所定の部位又は全体重心の位置と前記第 2座標系から見た前記所定の部位又は全体重心の第 1の推定位置とが同一になるようにグロ一バル座標系から見た前記所定の部位又は全体重心の第 1の推定位置を決定することを特徴とする請求の範囲第 1 5項に記載の脚式移動ロポッ卜の自己位置推定装置。

1 7 . 前記所定の部位は、前記口ポットの上体であることを特徴とする請求の範囲第 1 5項又は第 1 6項に記載の脚式移動ロポットの自己位置推定装置。

1 8 . 前記姿勢回転偏差は、少なくとも前記所定の部位のョー方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第 1 5項又は第 1 6 項に記載の脚式移動ロポッ卜の自己位置推定装置。

1 9 . 前記口ポットに作用する床反力を検出する床反力検出手段を備え，前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロボットの変形量を推定し、その推定された変形量を用いて前記第 1の推定位置を決定することを特徴とする請求の範囲第 1 5項又は第 1 6項に記載の脚式移動ロポットの自己位置推定装置。

2 0 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では. 前記ロポッ卜の重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Pのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第 1 5項又は第 1 6項に記載の脚式移動ロボッ卜の自己位置推定装置。

2 1 . 前記慣性航法的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2の推定位置を補正することを特徴とする請求の範囲第 1 5項又は第 1 6項に記載の脚式移動ロボッ卜の自己位置推定装置。

2 2 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記慣性航法的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることを特徴とする請求の範囲第 1 5項又は第 1 6項に記載の脚式移動ロポットの自己位置推

2 3 . 決定された目標歩容に追従するように制御される脚式移動ロポットにおいて、

前記ロボットに作用する床反力を検出する床反力検出手段と、前記ロポットの所定の部位の実姿勢の検出値又は推定値と前記目標歩容における前記所定の部位の姿勢との差である姿勢回転偏差の時間的変化量を姿勢回転偏差変化量として求める姿勢回転偏差算出手段と、前記姿勢回転偏差の変化の回転中心を決定する回転中心決定手段と、前記ロポッ卜が、前記回転中心まわりに前記姿勢回転偏差変化量で回転したと想定して、前記所定の部位又は前記ロボットの全体重心の推定位置としての第 1の推定位置を決定する幾何学的位置推定手段と、少なくとも前記床反力検出手段の検出値と前記実姿勢の検出値又は推定値とを基に、動力学演算によって前記所定の部位又は前記口ポットの全体重心の推定位置としての第 2の推定位置を算出すると共に、少なくとも第 1の推定位置と第 2の推定位置との差を基に第 2の推定位置を補正する動力学的位置推定手段とを備えることを特徴とする脚式移動ロボッ卜の自己位置推定装置。

2 4 . 前記幾何学的位置推定手段は、前記目標歩容を記述する座標系である第 1の座標系を前記姿勢回転偏差変化量だけ前記回転中心回りに回転させてなる第 2の座標系を求める手段を備え、前記目標歩容の目標運動と前記ロボッ卜の関節の変位検出値と該関節の変位目標値とのうちの少なくともいずれかから把握される、前記第 1座標系から見た前記所定の部位又は全体重心の位置と前記第 2座標系から見た前記所定の部位又は全体重心の第 1の推定位置とが同一になるようにグローバル座標系から見た前記所定の部位又は全体重心の第 1の推定位置を決定することを特徴とする請求の範囲第 2 3項に記載の脚式移動ロポットの自己位置推

2 5 . 前記所定の部位は、前記口ポットの上体であることを特徴とする請求の範囲第 2 3項又は第 2 4項に記載の脚式移動ロポッ卜の自己位置推定装置。

2 6 . 前記姿勢回転偏差は、少なくとも前記所定の部位のョー方向の姿勢回転偏差成分を含むことを特徴とする請求の範囲第 2 3項又は第 2 4 項に記載の脚式移動ロポットの自己位置推定装置。

2 7 . 前記幾何学的位置推定手段は、前記検出された床反力を基に前記ロポットの変形量を推定し、その推定された変形量を用いて前記第 1の推定位置を決定することを特徴とする請求の範囲第 2 3項又は第 2 4項に記載の脚式移動ロポットの自己位置推定装置。

2 8 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記回転中心決定手段は、前記空中期では前記ロポットの重心に前記回転中心を決定し、該空中期以外の時期では実床反力中心点および目標歩容の目標 Z M Fのいずれかの点又はその近傍に前記回転中心を決定することを特徴とする請求の範囲第 2 3項又は第 2 4項に記載の脚式移動ロボットの自己位置推定装置。

2 9 . 前記動力学的位置推定手段は、前記第 1の推定位置と第 2の推定位置との差を 0に近づけるように前記第 2の推定位置を補正することを特徴とする請求の範囲第 2 3項又は第 2 4項に記載の脚式移動ロポットの自己位置推定装置。

3 0 . 前記口ポットの目標歩容は、口ポットの全脚体を空中に浮かせる空中期を有する歩容であり、前記動力学的位置推定手段は、前記空中期では、前記第 2の推定位置の補正量を略 0にすることを特徴とする請求の範囲第 2 3項又は第 2 4項に記載の脚式移動ロボットの自己位置推定

3 1 . 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロポットに搭載された撮像装置等の環境認識手段によって認識された前記対象物に対するロボットの相対位置関係情報を基に、前記所定の部位の実姿勢の検出値又は推定値と、前記口ポットの推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求の範囲第 1項に記載の脚式移動ロボットの自己位置推定装置。

3 2 . 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロボットに搭載された撮像装置等の環境認識手段によつて認識された前記対象物と口ポットとの相対位置関係情報を基に、前記第 1の推定位置と第 2の推定位置とのうちの少なくともいずれかを補正する手段を備えたことを特徵とする請求の範囲第 1 0項に記載の脚式移動ロボットの自己位置推定装置。

3 3 . 少なくとも、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置情報と、前記ロポットに搭載された撮像装置等の環境認識手段によつて認識された前記対象物とロボットとの相対位置関係情報を基に、前記第 1の推定位置と第 2の推定位置と前記所定の部位の実姿勢の検出値又は推定値とのうちの少なくともいずれかを補正する手段を備えたことを特徴とする請求の範囲第 1 5項又は第 2 3項に記載の脚式移動ロボッ卜の自己位置推定装置。

3 4 . 少なくとも、前記位置推定手段により推定された前記ロボットの位置と、前記ロボットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記口ポットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求の範囲第 1項に記載の脚式移動ロボッ卜の自己位置推定装置。

3 5 . 少なくとも、前記第 2の推定位置と、前記口ポットに搭載された撮像装置等の環境認識手段によって認識された対象物に対する前記ロボットの相対位置関係情報を基に、対象物の位置を推定する手段を備えたことを特徴とする請求の範囲第 1 0項、第 1 5項および第 2 3項のいずれか 1項に記載の脚式移動ロポットの自己位置推定装置。

3 6 . 少なくとも前記口ポットの推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロポットに搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求の範囲第 1項に記載の脚式移動ロボットの自己位置推定装置。

3 7 . 少なくとも前記第 2の推定位置と、あらかじめ記憶しておいた地図上の床面や障害物などの対象物の位置とを基に、前記ロポッ卜に搭載された撮像装置等の環境認識手段が注視すべき方向を決定する手段を備えたことを特徴とする請求の範囲第 1 0項、第 1 5項および第 2 3項のいずれか 1項に記載の脚式移動ロポットの自己位置推定装置。