JP4181114B2 - 脚式移動ロボットの自己姿勢推定装置 - 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との合力に一致するように、歩容生成装置２００から与えられた目標足平位置姿勢を修正して、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすべての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、足平位置姿勢及び床反力の各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の２乗）の重み付き平均が最小になるように制御する。
【００８９】
補足すると、機構変形補償付き修正目標足平位置姿勢（軌道）は、複合コンプライアンス動作決定部２０４によって修正された床反力の目標値を発生させるために必要な足平の変形機構（円柱状ゴム、足底スポンジおよび衝撃吸収用の袋状のエアダンパー）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求めて、その変形量が発生するように修正した、目標足平位置姿勢（軌道）である。
【００９０】
以下に、本実施形態におけるＳ０１６の自己位置姿勢推定処理について、そのフローチャートである図１０を用いて詳説する。
【００９１】
まず図１０のＳ２２００において、上体３の前記傾斜センサ５４に備えたジャイロセンサの検出値、すなわち上体３の角速度（３軸方向の角速度）の検出値を積分器により積分して推定上体姿勢（以下、慣性航法的推定上体姿勢ということもある）を求める。この推定上体姿勢はグローバル座標系で記述される。なお、この処理では、前制御周期（前回以前の制御周期）で、後述するＳ２２０２からＳ２２１４の処理により求めた上体位置の推定値たる推定上体位置等を用いてジャイロセンサの検出値の積分誤差の蓄積によるドリフトを補正するドリフト補正も行われる。その詳細については後述する。
【００９２】
次いで、Ｓ２２０２に進み、制御周期の間（前回制御周期と今回制御周期との間）におけるグローバル座標系から見た推定上体姿勢（Ｓ２２００で求まる推定上体姿勢）の変化量と、該制御周期の間におけるグローバル座標系から見た目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として算出する。換言すれば、グローバル座標系から見た推定上体姿勢の変化速度と、目標上体姿勢の変化速度との差を姿勢回転偏差変化量として算出する。
【００９３】
なお、グローバル座標系から見た目標歩容の上体姿勢とは、現在の推定支持脚座標系上（より詳しくは、前回の制御周期に決定された推定支持脚座標系上）で、制御周期の間、上体３の姿勢傾きやスピン（目標上体姿勢からのずれ）がなくロボット１が目標歩容通りに運動したと仮定した場合のグローバル座標系から見た上体姿勢のことである。ただし、推定支持脚座標系とは、実際のロボット１の支持脚足平２２の推定位置姿勢に対応した支持脚座標系である。すなわち、推定支持脚座標系は、前記した支持脚座標系の定義によって、より具体的には、実際のロボット１の支持脚足平２２を、その推定位置姿勢から、接地面との間で滑らさないで水平まで回転させた時の、該支持脚の足首中心から接地面への垂直投影点を原点とし、該支持脚足平２２のつま先に向かう水平軸をＸ軸にとり、鉛直軸をＺ軸、これらに直交する座標軸をＹ軸にとった座標系である。
【００９４】
次いでＳ２２０４に進み、姿勢回転中心を決定する。具体的には、その瞬間の目標ＺＭＰ（目標ＺＭＰの現在値）を姿勢回転中心として決定する。なお、ロボット１の走行における空中期では、例えば目標歩容におけるロボット１の重心を姿勢回転中心として決定する。
【００９５】
次いでＳ２２０６に進み、現在の推定支持脚座標系（前回の制御周期で決定された推定支持脚座標系で、図１１に示す時刻ｔ−Δｔにおける推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置・姿勢を改めて現在の推定支持脚座標系（図１１に示す時刻ｔにおける推定支持脚座標系）と決定する。
【００９６】
なお、推定支持脚座標系の原点および座標軸の向きは、グローバル座標系によって表されるものとする。また、Ｓ０１０の初期化処理において、推定支持脚座標系の初期値（グローバル座標系での初期位置姿勢）がセットされているものとする。
【００９７】
また、推定支持脚座標系の回転中心は、空中期のない通常の歩行においては、支持脚足平２２の滑り回転の回転中心を意味する。なお、走りなど、１歩の途中に支持脚も床から離れている空中期においては、狭義の意味では支持脚足平２２の滑りが存在するわけではないので、空中での姿勢回転（あるいはスピン）は、推定支持脚座標系の回転中心まわりの回転で表現するのが望ましい。しかし、広義の意味で「支持脚足平の滑り」を推定支持脚座標系の回転中心まわりの姿勢回転と定義しても良いことである。
【００９８】
補足すると、支持脚座標系は、支持脚足平２２の接地面に対して設定するもので、その原点は、前述のように支持脚の足首中心から接地面への垂直投影点にしなくても良い。すなわち、支持脚座標系は、ロボット１の運動を記述するための支持脚足平２２の近傍の仮想の床に設定されたローカル座標系である。結局、ロボット１の姿勢回転（あるいはスピン）現象は、ロボット１が前記仮想の床上で相対的には姿勢回転（あるいはスピン）しないで運動している状態を保ったまま、ロボット１ごと前記仮想の床を、グローバル座標系のなかである所定の点を回転中心として姿勢回転（あるいはスピン）させた現象とみなされる。
【００９９】
さらに言い替えると、ロボット１の運動は、ローカル座標系において目標歩容あるいは関節変位検出値に従って運動しているロボット１の全体が、ローカル座標系ごと、グローバル座標系に対して、ある所定の点（姿勢回転中心）を回転中心として摂動回転をしているとみなすことができる。
【０１００】
したがって、姿勢回転中心は、推定支持脚座標系の回転中心であり、また、前記摂動回転の回転中心であるとも言える。
【０１０１】
また、Ｓ２２０６における姿勢回転後の現在の推定支持脚座標系のＺ軸の向きは一般には、鉛直方向を向かない。そこで、姿勢回転後に、現在の推定支持脚座標系のＺ軸を鉛直方向に戻す（該Ｚ軸の鉛直方向に対する傾斜角度分だけ、次回歩容推定支持脚座標系をその原点回りに回転させる）ようにしてもよい。あるいは、Ｓ２２０６の姿勢回転処理の際に、前記姿勢回転偏差変化量のうち、鉛直軸回りの成分（スピン成分）だけ、推定支持脚座標系を回転させるようにしてもよい。
【０１０２】
次いでＳ２２０８に進み、着地時刻であるか否か、すなわち歩容の切り替わり目であるか否かを判定する。
【０１０３】
Ｓ２２０８の判定結果がＹＥＳである場合には、以下の処理を行う。すなわち、まずＳ２２１０に進み、図１２に示す如く、現在の推定支持脚座標系（Ｓ２２０６での姿勢回転後の推定支持脚座標系）に対する次回歩容推定支持脚座標系の相対位置姿勢関係が、目標歩容（前回歩容）における支持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系を決定する。なお、ここで言う次回歩容支持脚座標系は、これから生成しようとしている今回歩容に対する次回歩容支持脚座標系ではなく、前回歩容の次の歩容（つまり今回歩容）の支持脚座標系のことである。また、目標歩容の支持脚座標系はそのＺ軸が鉛直方向を向く座標系であるので、姿勢回転後の現在の推定支持脚座標系に対して上記の相対位置姿勢関係を満たす次回歩容推定支持脚座標系のＺ軸は、一般には鉛直方向を向かない。そこで、本実施形態では、姿勢回転後の現在の推定支持脚座標系に対して上記の相対位置姿勢関係を満たす次回歩容推定支持脚座標系のＺ軸を鉛直方向に戻す（該Ｚ軸の鉛直方向に対する傾斜角度分だけ、次回歩容推定支持脚座標系をその原点回りに回転させる）ことで、最終的に次回歩容推定支持脚座標系を決定するようにしている。なお、Ｓ２２０６において、前述の如く推定支持脚座標のＺ軸を鉛直方向に向けるようにした場合には、Ｓ２２１０で次回歩容推定支持脚座標系のＺ軸を鉛直方向に戻す処理は不要である。
【０１０４】
次いでＳ２２１２に進み、Ｓ２２１０で決定した次回歩容推定支持脚座標系の位置姿勢（原点の位置及び座標軸の向き）を現在の推定支持脚座標系の位置姿勢に代入する。つまり、現在の推定支持脚座標系の位置姿勢を次回歩容推定支持脚座標系の位置姿勢に置き換える。
【０１０５】
Ｓ２２１２の後、あるいはＳ２２０８の判定結果がＮＯである場合には、Ｓ２２１４に進み、現在の推定支持脚座標系の位置姿勢に対応して、グローバル座標系での上体位置の推定値たる推定上体位置を決定する。ただし、現在の推定支持脚座標系に対する推定上体位置の関係は、目標歩容（前回の制御周期で決定された目標歩容）の支持脚座標系に対する上体位置（目標上体位置）の関係に一致させる。
【０１０６】
次いで、Ｓ２２１６に進み、前記傾斜センサ５４に備えた加速度センサとジャイロセンサを基に慣性航法により慣性航法的推定上体位置姿勢を求めつつ、幾何学的推定上体位置と慣性航法的推定上体位置との差が０に収束するように慣性航法的上体位置を修正する。この処理の詳細についても後述する。
【０１０７】
以上が図９のＳ０１６の自己位置姿勢推定処理の概要である。
【０１０８】
なお、Ｓ２２１０では、次回歩容推定支持脚座標系を決定するために目標歩容を用いたが、ロボット１の歩行を行なう場合には、次のようにして次回歩容推定支持脚座標系を決定するようにしてもよい。
【０１０９】
すなわち、まず、現在（着地瞬間）のロボット１の関節変位検出値を基にキネマティクス演算によって、現在の推定支持脚座標系における着地時の実遊脚足平位置・姿勢（図１２の例ではロボット１の前側の足平２２の位置姿勢）を推定する。以降、この推定された着地時の実遊脚足平位置・姿勢を推定着地時遊脚足平位置姿勢と呼ぶ。
【０１１０】
ただし、この時、ロボット１が、現在の推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平２２が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。
【０１１１】
補足すると、前記姿勢回転偏差変化量だけ前記姿勢回転中心まわりに回転させた現在推定支持脚座標系上で、ロボット１が、目標歩容通りの上体姿勢を維持している姿を想定した理由は、この瞬間において、想定した姿における上体姿勢を、グローバル座標系において、推定上体姿勢に一致させるためである。したがって、現在推定支持脚座標系を、前記差だけ姿勢回転させる代わりに、前記姿勢回転偏差変化量の内の鉛直軸まわり成分（スピン成分）だけ回転させる場合には、ロボット１が、現在の推定支持脚座標系上で、目標歩容通りの上体向きを維持しつつ、上体傾きは、前記姿勢回転偏差変化量の傾き成分に一致し、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平２２が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。
【０１１２】
また、この時、６軸力センサ５０による床反力検出値と目標歩容の床反力とのうちの少なくともいずれかを基に、足平２２の変形機構（前記図３及び図４に示した弾性部材１０６、接地部材（足底弾性体）７１、袋状部材１０９等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求め、その変形量を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めても良い。より具体的には、図１２に示すロボット１の姿のように、現在の推定支持脚座標系において上体姿勢を目標上体姿勢とし、関節変位を関節変位検出値（第ｊ関節変位検出値θｊ、ｊ＝１，２…）とし、変形機構の変形量を床反力検出値を基に推定した変形量とした場合の全体姿（その瞬間のロボット１の全体の姿）を求め、この全体姿における支持脚（図１２ではロボット１の後ろ側の脚体２）に対する遊脚足平位置姿勢を求め、これをもって推定着地時遊脚足平位置姿勢とするようにしても良い。また、モータ電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めるようにしても良い。
【０１１３】
または、少なくとも目標歩容の関節変位（関節変位目標値）とコンプライアンス補償量（本出願人が先に提案した特開平10-277969号を参照のこと）を基に、キネマティクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。
【０１１４】
これらにより、より一層精度良く推定着地時遊脚足平位置姿勢を求めることができる。
【０１１５】
また、ロボット１の歩行を行っている場合には、Ｓ２２１４の処理では、前記推定着地時遊脚足平位置姿勢を求めた手法と同様の手法を用い、現在のロボット１の各関節の関節変位検出値を基に、キネマティク演算演算によって支持脚座標系に対する上体位置を求め、この位置関係に、現在の推定支持脚座標系に対する推定上体位置の位置関係が一致するように、推定上体位置を決定するようにしてもよい。この場合、推定上体位置、推定支持脚座標系および関節変位検出値の関係は、図１２に示すような関係になる。
【０１１６】
または、少なくとも目標歩容の関節変位（関節変位目標値）とコンプライアンス補償量（本出願人が先に提案した特開平10-277969号を参照のこと）を基に、キネマティクス演算によって支持脚座標系に対する上体位置を求め、これに推定支持脚座標系に対する推定上体位置の関係を一致させても良い。
【０１１７】
これらにより、より一層精度良く推定上体位置を決定することができる。
【０１１８】
次に、Ｓ２２００の処理及びＳ２２１６の処理を図１３を参照しつつ詳説する。なお、以降の説明では、Ｓ２２１４で前述の如く決定される推定上体位置を幾何学的推定上体位置という。
【０１１９】
Ｓ２２００の処理を詳説すると、まず、前回の制御周期およびそれ以前に、前述の如く決定した幾何学的推定上体位置の運動を基に、幾何学的推定上体位置の２階微分である幾何学的推定上体加速度を算出する。この処理は、図１３のブロック３０１で実行される。なお、幾何学的推定上体位置を求める処理は、図１３のブロック３００で実行されるものとする。補足すると、上体代表点と加速度センサの位置が一致していれば、幾何学的推定上体加速度（厳密には幾何学的に推定される上体３の中の加速度センサ位置の加速度）を算出する上で、推定上体姿勢は必要ない。
【０１２０】
次いで、加速度センサ検出値（上体加速度検出値）を、推定上体姿勢を用いてグローバル座標系に変換してなる加速度センサ検出値グローバル座標系換算値を求める。この処理は、図１３のブロック３０２で実行される。なお、説明を簡単にするために、ここでは、加速度センサの位置と上体代表点の位置は一致しているものとする。異なる場合には、キネマティクス演算によって、加速度センサ検出値およびジャイロセンサ検出値を上体代表点における加速度・角速度に変換するか、あるいは、上体代表点における加速度・角速度を加速度センサの位置における加速度、ジャイロセンサの位置における角速度に変換すれば良い。
【０１２１】
次いで、図１４（ａ）および図１４（ｂ）に示すように、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じる（ベクトルの減算を行う）ことにより、推定重力加速度を求める。加速度センサ検出値グローバル座標系換算値には、加速度センサに作用する重力による加速度成分が含まれるが、幾何学的推定上体加速度には、重力による加速度成分が含まれない。従って、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じることで、推定重力加速度が求められる。この処理は、図１３のブロック３０３で実行される。なお、図１４（ａ）は、推定上体姿勢に誤差がない場合、図１４（ｂ）は、推定上体姿勢に誤差（以降、これを推定上体姿勢誤差、あるいは推定上体姿勢誤差角度と呼ぶ）がある場合を示す。
【０１２２】
一方、図１４（ａ）および図１４（ｂ）に示すように、推定上体姿勢を基にロボット１が推定したグローバル座標系の鉛直方向（Ｚ軸方向）に発生すると想定される重力加速度を想定重力加速度と呼ぶ。
【０１２３】
前述したように、幾何学的推定上体加速度はグローバル座標系で記述される。また、実際のロボット１は、推定上体姿勢が実際の上体姿勢を正しく推定しているとみなした上で、目標歩容に追従するように運動している。言い換えると、推定上体位置姿勢を基に推定されるグローバル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。より厳密に表現するならば、ロボット１の自己位置姿勢推定処理においては、推定上体位置姿勢を基に推定されるグローバル座標系が正しいと信じ、実際のロボット１の挙動を、正しいと信じているグローバル座標系に設定されるローカル座標系である推定支持脚座標系上で目標歩容通りに運動しているロボット１が、各瞬間に、姿勢検出手段によって得られた（検出あるいは推定された）上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差としての姿勢回転偏差変化量で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなしている。
【０１２４】
したがって、推定上体姿勢が正しく、かつ加速度センサの検出値に誤差がなければ、図１４（ａ）に示すように、前記想定重力加速度と前記推定重力加速度とは一致する。
【０１２５】
これに対し、加速度センサの検出値に誤差がなくても、推定上体姿勢に誤差があれば、図１４（ｂ）に示すように、前記想定重力加速度の向きと前記推定重力加速度の向きとには、推定上体姿勢誤差角度だけずれが生じる。すなわち、式４２が成立する。
【０１２６】
推定上体姿勢誤差角度 ＝ 想定重力加速度と推定重力加速度との間の角度
…式４２
図１４（ａ）および図１４（ｂ）に示すように、推定重力加速度と想定重力加速度との差を重力加速度推定誤差と呼ぶと、上記の関係から、重力加速度推定誤差は、加速度センサの検出値に誤差がなければ、推定上体姿勢の誤差によって発生し、推定上体姿勢を基にロボット１が推定したグローバル座標系から見た重力加速度推定誤差の水平成分（推定重力加速度の想定重力加速度に直交する成分）と推定上体姿勢誤差角度とには、式４３の関係があることがわかる。ただし、式４３は、重力加速度推定誤差の前後方向成分（Ｘ成分）と推定上体姿勢誤差角度Ｙ軸まわり成分との関係を表す。重力加速度推定誤差の左右方向成分（Ｙ成分）と推定上体姿勢誤差角度Ｘ軸まわり成分の関係を表す場合においては、右辺のマイナス記号を削除すれば良い。ここで、重力加速度は、正である。
【０１２７】
重力加速度推定誤差の水平成分
＝ −tan( 推定上体姿勢誤差角度 )＊重力加速度
…式４３
本実施形態においては、式４２あるいは式４３から算出される推定上体姿勢誤差角度を用いて、推定上体姿勢誤差が０に収束するように推定上体姿勢を補正するようにした。
【０１２８】
Ｓ２２００の処理の説明に戻ると、式４２を用いて、現在の瞬間（厳密には、１制御周期前の瞬間）における想定重力加速度と推定重力加速度との間の角度から推定上体姿勢誤差角度を算出する。あるいは、式４３を用いて、現在の瞬間（厳密には、１制御周期前の瞬間）における重力加速度推定誤差の水平成分から推定上体姿勢誤差角度を算出しても良い。推定上体姿勢誤差角度の算出処理は、図１３のブロック３０４で実行される。
【０１２９】
次いで、前記推定上体姿勢誤差角度を、図１３のブロック３０５にて、センサ座標系（座標軸をジャイロセンサの検出軸に合わせた座標系）に変換した後、ブロック３０６にて、その変換後の値に積分ゲインKaを乗じた値を積分することにより、推定ジャイロセンサドリフト（ジャイロセンサのドリフトの推定値）を求める。さらに、この推定ジャイロセンサドリフトを、ブロック３０７にて、ジャイロセンサ検出値ωin（上体角速度検出値）から減じることにより、ドリフトを補正した角速度を求める。なお、ブロック３０７では、ヨーレートのドリフトも適宜減算されるが、これについては後述する。さらに、このドリフトを補正した角速度を、ブロック３５０にて、推定上体姿勢を用いてグローバル座標系に変換することにより、グローバル上体角速度ωglを求める。
【０１３０】
次いで、前記推定上体姿勢誤差角度にゲインKbを図１３のブロック３０９で乗じた値を、グローバル上体角速度から減じた値をブロック３１０（積分器）で積分することにより、新たな推定上体姿勢θ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)ジャイロ検出値（姿勢角速度検出値）の絶対値がある所定の値以内である状況
基本的には、支持脚足平２２と床との接地面で滑り（回転滑り）が生じないかもしくは生じ難いという状況で、ヨーレート補正を行うと判断する。
【０１５１】
なお、ｄ）およびｆ）におけるジャイロ検出値は、ジャイロセンサの検出値そのもの（ジャイロセンサの生の出力そのものが表す姿勢角速度検出値）をヨーレート補正した値であることが望ましい。
【０１５２】
次いで、ヨーレート補正を行うと判定された場合には、図１３に示すように、前記６軸力センサ５０（床反力センサ）による床反力検出値を基に、ブロック３１１にて、足平２２の変形機構（前記図３及び図４に示した弾性部材１０６、接地部材（足底弾性体）７１、袋状部材１０９等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求める。そして、ブロック３１２にて、この求めた変形量（機構変形量）と、目標歩容（目標運動）とを基に、ロボット１の最新の着地時（着地瞬間）に記憶した推定支持脚座標系（図１０の最新のＳ２２１２で決定した推定支持脚座標系）に対応する足平２２と床との間に滑りが生じていないと想定した場合の上体姿勢（以降、滑りなし推定上体姿勢と呼ぶ）を算出する。つまり、最新の着地瞬間以降、姿勢回転（姿勢回転偏差の変化）を生じなかったと仮定して（着地瞬間以降、推定支持脚座標系の位置姿勢が変化していないと仮定して）、前記機構変形量を考慮しつつ、目標歩容（目標運動）を基に、現在の滑りなし推定上体姿勢を求める。なお、目標歩容（目標運動）の代わりに、ロボット１の関節の変位検出値もしくは変位目標値を用いて、滑りなし推定上体姿勢を求めてもよい。また、モータ電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、滑りなし推定上体姿勢を求めるようにしても良い。または、少なくとも目標歩容の関節変位とコンプライアンス補償量（本出願人が先に提案した特開平10-277969号を参照）を基に、キネマティクス演算によって滑りなし推定上体姿勢を求めるようにしても良い。これらにより、より一層精度良く滑りなし推定上体姿勢を求めることができる。
【０１５３】
次いで、上記の如く求めた滑りなし推定上体姿勢の現在値及び前制御周期（前回以前の制御周期）の値を用いて該滑りなし推定上体姿勢の微分値たる滑りなし推定上体姿勢角速度をブロック３１３で求める。さらに、前記推定上体姿勢を出力するブロック（積分器）３１０の入力、すなわちブロック３０８から出力される上体姿勢角速度と上記滑りなし推定上体姿勢角速度との差をブロック３１４で求め、この差のうちの、ヨーレートセンサ（ヨーレート検出用ジャイロセンサ）の検出方向成分（以下、ヨーレートセンサ検出方向角速度偏差成分という）をブロック３１５で抽出する。なお、このヨーレートセンサ検出方向角速度偏差成分の抽出には、推定上体姿勢（前回の制御周期で求めた値）が用いられる。該推定上体姿勢の代わりに、目標歩容の上体姿勢を用いてもよい。なお、上体姿勢が鉛直または鉛直に近い場合には、前記差（ブロック３１４の出力）の鉛直方向成分の値をヨーレートセンサ検出方向角速度偏差成分として用いてもよい。
【０１５４】
次いで、ヨーレートセンサ検出方向角速度偏差成分を、フィードバック制御則のブロック３１８に入力することで、ヨーレートドリフトを求める（推定する）。ブロック３１８のフィードバック制御則としては、例えばＰＩ制御則が用いられ、前記ヨーレートセンサ検出方向角速度偏差成分（ブロック３１５の出力）にゲインKfを乗じた値と、該ヨーレートセンサ検出方向角速度偏差成分の積分値にゲインKgを乗じた値との和を求めることによりヨーレートドリフトが求められる。そして、この求めたヨーレートドリフトを、前記ブロック３０７で前記角速度センサ検出値ωinから減じる。また、ヨーレート補正を行わないと判定された場合には、フィードバック制御則のブロック３１８ヘの入力を遮断して（図１３のスイッチ３１７をブロック３１５の出力側から切り離し、「０」入力側に閉成する）、直前のヨーレートドリフト値を保持し、その値を前記角速度センサ検出値ωinから減じるようにする。
【０１５５】
以上が図１０のＳ２２００におけるヨーレート補正の処理の詳細である。
【０１５６】
次に、図１０のＳ２２１６の処理を図１３を参照して詳説する。Ｓ２２１６の処理では、図１３に示すごとく慣性航法的推定上体位置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を設定する方が望ましい。
【０１６２】
また、補正ゲインKa，Kb，KcもしくはKdの値を、定常カルマンフィルタあるいは非定常カルマンフィルタの手法を用いて決定しても良い。ただし、本実施形態のロボット１においては、特に移動中においては、システム雑音（外乱）と観測雑音の性質が、カルマンフィルタの前提条件を十分に満足してはいないので、十分な効果を生むとは限らない。
【０１６３】
次に、前記実施形態で決定した姿勢回転中心について、以下に補足する。いずれかの脚体２が接地し、十分大きい床反力が発生している時期においては、いわゆる支持多角形（接地面を含む最小凸多角形、ＺＭＰ存在可能範囲あるいは全床反力中心点存在可能範囲）の中に、姿勢回転中心があると考えられる。
【０１６４】
この理由を以下に述べると、姿勢回転中心が支持多角形の外にあると、足平２２の底面のあらゆる点が床に対して相対移動していることになるので、足平２２の底面と床との間のあらゆる接触点には、動摩擦が働く。動摩擦は、ロボット１が床反力を制御しようとして、脚体２を動かしても、一定値のままで、任意の値に制御することはできない。すなわち、氷の上でつるつる滑ってうまく歩けない状態と同じである。したがって、ロボット１が床反力を制御しつつ安定に移動している場合には、足平２２の底面と床との間のある領域においては、滑りが生じないで静摩擦が作用していると考えれる。すなわち、支持多角形の中に、姿勢回転中心があると考えられる。補足すると、ロボット１の足平２２が完全剛体であるならば、姿勢回転中心点以外のあらゆる接触点は滑りが生じるが、実際の足平２２の底面は、ゴムなどの弾性体でできているので、姿勢回転中心の近傍でも滑りは生じないと考えれる。
【０１６５】
また、空中期においては、ロボットは、重心を中心として摂動運動をすると考えられる。
【０１６６】
また、上記以外の時期、すなわちいずれかの脚体２が接地しているが、床反力が十分に発生していない時期においては、運動の連続性を考慮すると、姿勢回転中心は、支持多角形と全体重心位置（あるいは上体代表点位置）との間に存在すると考えられる。
【０１６７】
以上の考察から、前記図１０のＳ２２０４において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい。
現在時刻ｔにおける
a)目標ＺＭＰ、
b)実ＺＭＰ（すなわち、実床反力中心点、あるいは床反力の圧力中心点）、
c)支持脚座標系の所定の点、例えば原点
d)全体重心点、
e)上体代表点
f)上記のいずれかの複数の点の内分点
いずれかの脚が接地し、十分大きい床反力が発生している時期においては、姿勢回転中心は、支持多角形の中に設定するのが良い。具体的には、上記a)およびb)に設定すれば良い。あるいは、姿勢回転中心が支持多角形の中に含まれるように、上記c)における所定の点を設定すれば良い。例えば支持脚座標系の原点（通常、足首関節の下）に設定すれば良い。
【０１６８】
空中期においては、姿勢回転中心は、上記d)のように設定するのが良いが、全体重心位置は上体代表点位置の近くに存在するので、上記e)のように設定しても良い。
【０１６９】
上記以外の時期、すなわちいずれかの脚体２が接地しているが、床反力が十分に発生していない時期においては、上記f)のように設定するのが良い。
【０１７０】
さらに、全期間での運動の連続性を考慮すると、姿勢回転中心点は、連続的に変化するように設定するのが望ましい。
【０１７１】
いずれにしても、概ねすべての各瞬間において、ロボット１の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。
【０１７２】
なお、以上説明した実施形態では、加速度センサおよびまたはジャイロセンサは、上体３以外の部位、例えば頭部４に搭載（内蔵）されていても良い。さらに頭部４と上体３との間に首関節がある場合、首関節の変位指令（目標変位）または変位検出値を基に、加速度センサおよびまたはジャイロセンサの検出値をキネマティクス演算によって、上体代表点の加速度と角加速度とに変換しさえすれば、残りは、前記各実施形態と同様に自己位置推定を行えば良い。
【０１７３】
また、推定支持脚座標系および幾何学的推定上体位置を決定する際に関節変位を用いる場合においては、その関節変位を、目標歩容の関節変位または、関節変位検出値としたが、これらの重み付き平均でも良い。また、このときの重みは周波数特性を持たせても良い。
【０１７４】
推定支持脚座標系や推定上体位置姿勢など、推定される位置姿勢を表現する時には、前記実施形態のごとくグローバル座標系を基準として表現する代わりに、目標位置姿勢からの摂動分で表現しても良い。
【０１７５】
また、前記実施形態においては、前記図１３に示したごとく、グローバル座標系における推定重力加速度を基に、グローバル座標系における推定上体姿勢誤差角度を求め、グローバル座標系における推定上体姿勢誤差角度にゲインKbを乗じた値を、グローバル上体角速度ωglを積分する積分器（図１３のブロック３１０）に付加的に入力することによって、推定上体姿勢を補正していた。すなわち、グローバル座標系において、推定上体姿勢を補正していたが、この代わりに、ジャイロセンサのローカル座標系（上体３に固定された座標系）上で補正しても良い。具体的には、図１３において、ゲインKbのブロック３０９と、その出力をグローバル上体角速度ωglから減じる加算器（ブロック３０８）を削除し、積分器Ka／S（ブロック３０６）を、Ka/S＋Kb、すなわち、ＰＩ制御則のブロックに変更しても良い。
【０１７６】
原理上、このようにローカル座標系で補正しても、ヨーレートが高い旋回（その場旋回含む）時に、Kbによって傾斜ドリフトの収束が悪くなることはない。
【０１７７】
推定上体姿勢の補正において、加速度センサ検出値グローバル座標系換算値から幾何学的推定上体加速度を減じることにより推定重力加速度を求める代わりに、加速度センサ検出値グローバル座標系換算値から目標歩容の上体加速度を減じることにより推定重力加速度を求めるようにしても良い。
【０１７８】
実際には、目標歩容通りに運動しようとしているロボットには、姿勢回転中心まわりの姿勢回転が発生しているので、姿勢回転によって、上体加速度は、目標歩容からずれる。しかし、姿勢回転の内の姿勢傾斜は、前後左右に振動しても平均的には、０である。また、通常、姿勢回転の内のスピンに関しては、１歩毎にスピンの向きが逆に切り替わるので、平均的には０に近い。したがって、遠心力など、姿勢回転の回転方向によらずほぼ同一方向に作用する力を除けば、姿勢回転による上体加速度への影響は、正の影響と負の影響とが相殺し、長期的には、ほぼ０になる。なお、ここで、「長期」とは、推定上体姿勢の補正の整定時間以上の時間を指す。
【０１７９】
よって、推定上体姿勢の補正のために、幾何学的推定上体加速度を用いる代わりに、目標歩容の上体加速度を用いても、補正の効果が大きく低下することはない。
【０１８０】
以上説明した如く、前記実施形態においては、慣性航法によって推定上体位置（慣性航法的推定上体位置）と推定上体姿勢とを求めると共に、幾何学的推定上体位置を用いて慣性航法的な推定上体位置及び推定上体姿勢を補正するようにしたので、ロボット１の自己位置姿勢ならびに着地位置・向き（推定支持脚座標系の位置・向き）を精度良く推定することができる。また、特に、ロボット１の停止時等、足平２２と床との間の滑りが生じていないと判断される状況では、ヨーレート補正を行うようにしたことによって、ロボット１の自己姿勢（上体姿勢）を精度よく推定することができる。
【０１８１】
産業上の利用可能性
以上の如く、本発明は、２足移動ロボット等の脚式移動ロボットの上体等の所定の部位の姿勢を精度よく推定することができる技術を提供するものとして有用である。
【図面の簡単な説明】
【図１】本発明の実施形態における脚式移動ロボットとしての２足移動ロボットの全体的構成の概略を示す概略図。
【図２】図１の各脚体の足平部分の構成を概略的に示す概略図。
【図３】各脚体の足平部分の詳細構成を示す側面視の断面図。
【図４】各脚体の足平部分の詳細構成を示す底面図。
【図５】図１のロボットに備えた制御ユニットの構成を示すブロック図。
【図６】図５の制御ユニットの機能的構成を示すブロック図。
【図７】ロボットの走行歩容を示す説明図。
【図８】図８（ａ），（ｂ），（ｃ）はそれぞれ目標歩容の床反力鉛直成分、目標ＺＭＰ、自己位置姿勢推定用のゲインの設定例を示すグラフ。
【図９】第１実施形態における制御ユニットの要部の処理を示すフローチャート。
【図１０】図９のフローチャートの自己位置姿勢推定処理を示すフローチャート。
【図１１】図９のフローチャートの自己位置姿勢推定処理を説明するための図。
【図１２】図９のフローチャートの自己位置姿勢推定処理を説明するための図。
【図１３】図９のフローチャートの自己位置姿勢推定処理の要部の処理を示すブロック図。
【図１４】図１４（ａ），（ｂ）は図１３の処理を説明するための図。
【図１５】図１３の処理で用いるゲインの設定例を示すグラフ。

Claims (4)

1. 決定された目標歩容に追従するように制御される脚式移動ロボットにおいて、
   前記ロボットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、
   少なくとも前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちのいずれかを含む該ロボットの運動状態量を基に、前記ロボットと床との接触面に滑りが無いと仮定して、前記所定の部位の姿勢角速度を推定する無滑り姿勢角速度推定手段と、
   少なくとも前記姿勢角速度検出手段の検出値のうちのヨー方向の成分と前記無滑り姿勢角速度推定手段が推定した姿勢角速度のうちのヨー方向の成分とに応じて、前記姿勢角速度検出手段の検出値に対するドリフト補正値を決定するドリフト補正値決定手段と、
   少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備え、
   前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度のうちのヨー方向の成分と前記無滑り姿勢角速度推定手段が推定した姿勢角速度のうちのヨー方向の成分との偏差を０に近づけるように新たなドリフト補正値を決定することを特徴とする脚式移動ロボットの自己姿勢推定装置。
2. 前記ドリフト補正値決定手段は、前記ロボットと床との接触面に回転滑りが生じているか否かを判断する手段を備え、該回転滑りが生じていると判断したときには、前記ドリフト補正値の値を保持することを特徴とする請求項１記載の脚式移動ロボットの自己姿勢推定装置。
3. 脚式移動ロボットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、
   少なくとも前記ロボットの運動停止状態における前記姿勢角速度検出手段の検出値のうちのヨー方向の成分に応じて、該検出値に対するドリフト補正値を決定するドリフト補正値決定手段と、
   前記ロボットの運動中に少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備えたことを特徴とする脚式移動ロボットの自己姿勢推定装置。
4. 前記所定の部位は、前記ロボットの上体であることを特徴とする請求項１又は３記載の脚式移動ロボットの自己姿勢推定装置。

Images