(57)【要約】ロボット１の上体姿勢角等の実状態量と目標状態量との偏差に基づいて、目標床反力（目標床反力モーメント）の修正操作量を逐次決定しつつ、その修正操作量と動力学モデルとを用いてロボット１の目標運動を逐次決定する。このとき、並進床反力水平成分等、ロボット１と床との間の摩擦力を規定する摩擦力成分を制限対象量とし、その制限対象量の許容範囲を設定する。目標運動を決定するときには、制限対象量が許容範囲内に収まり、且つ目標床反力を修正操作量により修正してなる床反力に、動力学モデル上でロボット１の運動が発生する慣性力と重力との合力が釣り合うように目標運動を決定する。目標運動は、床反力モーメントと並進床反力の発生比率が互いに異なる複数の運動モードを調整して決定する。
2003061917.tif 000002
【特許請求の範囲】
【請求項１】
上体から延設された脚体を運動させて移動する脚式移動ロボットの目標運動の瞬時値を、少なくとも該ロボットの運動と床反力との関係を表す動力学モデルを用いて逐次決定しつつ、その決定した目標運動の瞬時値に追従させるように前記ロボットの動作制御を行なう脚式移動ロボットの制御装置において、
少なくとも前記ロボットに作用する並進床反力水平成分又は並進床反力の床面平行成分又は該ロボットの全体重心加速度又は全体重心加速度の床面平行成分を制限対象量とし、該制限対象量の許容範囲を設定する許容範囲設定手段と、
少なくとも前記決定された目標運動の瞬時値に対応するロボットの姿勢の目標状態量と該ロボットの姿勢の実状態量との偏差に基づいて、前記目標運動の新たな瞬時値を、該新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まるように決定する目標瞬時値決定手段とを備えたことを特徴とする脚式移動ロボットの制御装置。
【請求項２】
前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる前記ロボットの複数の運動モードを含み、前記目標瞬時値決定手段は、前記動力学モデルの運動の複数の運動モードを少なくとも前記許容範囲に応じて調整して前記目標運動の新たな瞬時値を決定することを特徴とする請求の範囲第１項に記載の脚式移動ロボットの制御装置。
【請求項３】
前記運動モードは、前記ロボットの重心を略一定に維持しつつ該ロボットの所定の部位の姿勢を変化させる姿勢変化運動と、該ロボットの上体の並進運動とを含むことを特徴とする請求の範囲第２項に記載の脚式移動ロボットの制御装置。
【請求項４】
少なくとも前記動力学モデルの状態量に応じて前記ロボットの目標床反力及び／又は目標運動の現在以降のパターンを決定するための歩容パラメータを修正する手段を備えたことを特徴とする請求の範囲第１項に記載の脚式移動ロボットの制御装置。
【請求項５】
前記ロボットの目標床反力の基準瞬時値を逐次決定する手段が備えられ、前記目標瞬時値決定手段は、少なくとも前記偏差と前記制限対象量の許容範囲とに応じて前記目標床反力の修正操作量を決定すると共に、該修正操作量により前記基準瞬時値を修正してなる床反力に、前記目標運動の新たな瞬時値により前記動力学モデル上で発生する慣性力と重力との合力を釣り合わせるように前記目標運動の新たな瞬時値を決定することを特徴とする請求の範囲第１項に記載の脚式移動ロボットの制御装置。
【請求項６】
前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる前記ロボットの複数の運動モードを含み、前記目標瞬時値決定手段は、前記動力学モデルの運動の複数の運動モードを少なくとも前記許容範囲に応じて調整して前記目標運動の新たな瞬時値を決定することを特徴とする請求の範囲第５項に記載の脚式移動ロボットの制御装置。
【請求項７】
前記運動モードは、前記ロボットの重心を略一定に維持しつつ該ロボットの所定の部位の姿勢を変化させる姿勢変化運動と、該ロボットの上体の並進運動とを含むことを特徴とする請求の範囲第６項に記載の脚式移動ロボットの制御装置。
【請求項８】
前記目標瞬時値決定手段は、前記目標床反力の修正操作量により前記基準瞬時値を修正してなる床反力に前記合力を釣り合わせるために、前記姿勢変化運動および前記上体の並進運動のうち、該上体の並進運動を優先的に調整して前記目標運動の新たな目標瞬時値を決定することを特徴とする請求の範囲第７項に記載の脚式移動ロボットの制御装置。
【請求項９】
前記目標床反力の修正操作量を少なくとも前記偏差と前記制限対象量の許容範囲と前記動力学モデルの状態量とに応じて決定する手段を備えたことを特徴とする請求の範囲第５項に記載の脚式移動ロボットの制御装置。
【請求項１０】
前記目標運動の瞬時値を決定するために用いる前記目標床反力の修正操作量及び／又は該修正操作量による前記動力学モデルの状態量の変化に応じて前記ロボットの目標床反力及び／又は目標運動の現在以降のパターンを決定するための歩容パラメータを修正する手段を備えたことを特徴とする請求の範囲第５項に記載の脚式移動ロボットの制御装置。
【請求項１１】
前記偏差に応じて該偏差が０に近づくように床反力の第１要求操作量を決定する第１手段と、少なくとも該第１要求操作量に基づき前記ロボットの実床反力の目標値を規定する実床反力操作量と前記目標床反力の修正操作量とを決定する第２手段と、前記実床反力操作量と前記目標運動の瞬時値とに応じて前記ロボットの動作制御を行なう第３手段とを備えたことを特徴とする請求の範囲第５項に記載の脚式移動ロボットの制御装置。
【請求項１２】
前記第１要求操作量、前記実床反力操作量、および前記目標床反力の修正操作量は、いずれも床反力作用点回りの床反力モーメント又は該床反力作用点の位置又はＺＭＰに関する操作量であることを特徴とする請求の範囲第１１項に記載の脚式移動ロボットの制御装置。
【請求項１３】
前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が前記第１要求操作量の増加に伴い単調に変化するように前記実床反力操作量と前記目標床反力の修正操作量とを決定することを特徴とする請求の範囲第１１項または第１２項に記載の脚式移動ロボットの制御装置。
【請求項１４】
前記第２手段は、前記実床反力操作量を前記基準瞬時値に加えたものが所定の実床反力許容範囲内に収まるように該実床反力操作量を決定することを特徴とする請求の範囲第１１項に記載の脚式移動ロボットの制御装置。
【請求項１５】
前記第２手段は、前記第１要求操作量を前記基準瞬時値に加えたものが前記所定の実床反力許容範囲内の値であるときには、該第１要求操作量を前記実床反力操作量として決定すると共に、前記目標床反力の修正操作量を略０又は前記動力学モデルの状態量に応じて定めた値に決定し、前記第１要求操作量を前記基準瞬時値に加えたものが前記所定の実床反力許容範囲を逸脱する値であるときには、該実床反力許容範囲の限界値から前記基準瞬時値を差し引いたものを前記実床反力操作量として決定すると共に、少なくとも前記実床反力操作量と前記第１要求操作量との差分に応じて前記目標床反力の修正操作量を決定することを特徴とする請求の範囲第１４項に記載の脚式移動ロボットの制御装置。
【請求項１６】
前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記第１要求操作量に応じた値になるように該実床反力操作量および目標床反力の修正操作量の仮決定値を求める手段と、その求めた目標床反力の修正操作量の仮決定値を用いて前記目標運動の新たな瞬時値を決定したと仮定した場合における当該目標運動の新たな瞬時値に対応する前記制限対象量の推定値を求める手段と、前記求めた実床反力操作量の仮決定値を前記基準瞬時値に加えたものと前記制限対象量の推定値とを前記所定の実床反力許容範囲及び前記制限対象量の許容範囲と比較し、その比較結果に基づき該実床反力許容範囲及び該制限対象量の許容範囲の制限を満たす実床反力操作量および制限対象量の組を決定する手段と、その決定した実床反力操作量から前記第１要求操作量を差し引いたものと制限対象量とに基づき前記目標床反力の修正操作量を決定する手段とから構成されていることを特徴とする請求の範囲第１４項に記載の脚式移動ロボットの制御装置。
【請求項１７】
前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる複数の運動モードを含んでおり、前記目標床反力の修正操作量は、各運動モードにそれぞれ対応する複数の操作量から構成されていることを特徴とする請求の範囲第１６項に記載の脚式移動ロボットの制御装置。
【請求項１８】
前記動力学モデルの状態量に応じて該状態量を所定の状態量に近づけるように床反力の第２要求操作量を決定する第４手段を備え、前記第２手段は、少なくとも前記第１要求操作量と前記第２要求操作量と前記許容範囲とに基づき前記実床反力操作量と前記目標床反力の修正操作量とを決定することを特徴とする請求の範囲第１１項に記載の脚式移動ロボットの制御装置。
【請求項１９】
前記第１および第２要求操作量、前記実床反力操作量並びに前記目標床反力の修正操作量は、いずれも床反力作用点回りの床反力モーメント又は該床反力作用点の位置又はＺＭＰに関する操作量であることを特徴とする請求の範囲第１８項に記載の脚式移動ロボットの制御装置。
【請求項２０】
前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が、前記第１要求操作量の増加に伴い単調に変化するように前記実床反力操作量および前記目標床反力の修正操作量を決定することを特徴とする請求の範囲第１８項又は第１９項に記載の脚式移動ロボットの制御装置。
【請求項２１】
前記第２手段は、前記実床反力操作量を前記基準瞬時値に加えたものが所定の実床反力許容範囲内に収まるように該実床反力操作量を決定することを特徴とする請求の範囲第１８項に記載の脚式移動ロボットの制御装置。
【請求項２２】
前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記第１要求操作量に応じた値になり、且つ前記目標床反力の修正操作量が前記第２要求操作量に応じた値になるように該実床反力操作量および目標床反力の修正操作量の仮決定値を求める手段と、その求めた目標床反力の修正操作量の仮決定値を用いて前記目標運動の新たな瞬時値を決定したと仮定した場合における当該目標運動の新たな瞬時値に対応する前記制限対象量の推定値を求める手段と、前記求めた実床反力操作量の仮決定値を前記基準瞬時値に加えたものと前記制限対象量の推定値とを前記所定の実床反力許容範囲及び前記制限対象量の許容範囲と比較し、その比較結果に基づき該実床反力許容範囲及び該制限対象量の許容範囲の制限を満たす実床反力操作量および制限対象量の組を決定する手段と、その決定した実床反力操作量から前記第１要求操作量を差し引いたものと制限対象量とに基づき前記目標床反力の修正操作量を決定する手段とから構成されていることを特徴とする請求の範囲第２１項に記載の脚式移動ロボットの制御装置。
【請求項２３】
前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる複数の運動モードを含んでおり、前記目標床反力の修正操作量は、各運動モードにそれぞれ対応する複数の操作量から構成されていることを特徴とする請求の範囲第２２項に記載の脚式移動ロボットの制御装置。
【発明の詳細な説明】
技術分野
本発明は脚式移動ロボットの制御装置に関し、より具体的にはロボットの目標歩容を生成する内部モデルを備え、ロボット（実機）の挙動を内部モデルの挙動に追従させるようにしたものに関する。
背景技術
脚式移動ロボット、特に２足の脚式移動ロボットとしては、例えば特開昭６２−９７００５号公報、特開昭６３−１５０１７６号公報に記載されたものなどが知られている。また脚式移動ロボットを含むロボットの制御については、例えば「ロボット工学ハンドブック」（日本ロボット学会編、１９９０年１０月２０日）に詳しく説明されている。
ところで、脚式移動ロボット、特に２足の脚式移動ロボットは本来的に安定性が低いことから、外乱などを受けると姿勢が不安定となる。そこでロボットの上体の傾き角度（姿勢角）やその変化速度を検出し、それらの検出値に応じてロボットの姿勢制御を行なうことが望ましい。その意図から本出願人は先に特開平４−２０１１８７号公報等において、複数の着地候補位置を予め用意しておき、それらの着地候補位置のなかから、一歩ごとの制御周期でいずれかの着地候補位置を選択して目標歩容を切り換える制御を提案している。
しかし、この制御によるときは、姿勢が崩れても、次の着地までは姿勢を復元させる効果が発生しないので、次の着地までに大きく姿勢が崩れてしまって、転倒してしまうおそれがある。また、姿勢が崩れた結果、ＺＭＰ（垂直床反力の作用重心点で、床反力モーメントの水平成分が０となる点。Ｚｅｒｏ  Ｍｏｍｅｎｔ  Ｐｏｉｎｔ）が存在可能範囲の限界付近に偏って接地性が失われても、次の一歩で歩容が修正されて姿勢が復元するまではＺＭＰが限界付近に偏ったままなので、それまで接地性を回復できないと言う不都合がある。
そこで本出願人は例えば特開平５−３０５５７９号公報において、次のような技術を提案している。この技術では、ロボットの力学モデルを基に力学的平衡状態が保証される様に床反力を設定した目標歩容が生成される。そして、モデル化誤差や外乱によってロボットの傾き角度と傾き角速度とがそれぞれの目標値と実際値との間で偏差を生じたときは、実ロボットの接地部の変位あるいはモーメントが基準歩容から故意にずらされる。これにより、実ロボットの床反力（実床反力）がモデルから計算される目標歩容の床反力から故意にずらされ、実ロボットの傾きをモデルの傾きに近づける復元力が得られる。なお、ここで言う「床反力」は、各脚体に作用する床反力を全て合成した、ある作用点に働く合力と合モーメントとを意味する。
しかし、この技術によるときは、例えば２足移動ロボットの片脚支持期では、実ロボットの床反力を故意に大きくずらそうとして足首角度を大きくずらしても、ＺＭＰが足平接地面を超える全床反力を発生することができず、足平の一部が浮いてしまうことがある。従って、実ロボットがモデルに対して大きく傾いてしまった場合には、実ロボットの傾斜を正常に戻すために必要な十分な復元力を得ることができないことがある。そして、そのときは実ロボットが転倒する。即ち、特開平５−３０５５７９号公報の技術では、目標値と実際値との偏差を縮める復元作用がＺＭＰの存在範囲に制約される。
図を用いて説明すると、実ロボットの関節変位をモデルの通りに追従する制御においては、実ロボットの傾き角度と傾き角速度とがそれぞれモデルのそれらと一致していれば、図６１に実線で示す実ロボットのＺＭＰはモデルから決定される目標ＺＭＰに一致し、ロボットは所期の姿勢で歩行する。しかし、実際には先に述べた様にモデル化誤差や外乱によって破線で示す様に傾き角度と傾き角速度とに偏差が生じ、しかもそのずれは重力の作用によって偏差が大きくなるほど益々大きくなろうとする発散傾向を持つ。ところで実ＺＭＰが目標ＺＭＰからずれると、図６２に示す様に、その間の距離ｘに力Ｆを乗じたモーメントＭが目標ＺＭＰのまわりに生じる。換言すれば、実ＺＭＰが故意に目標ＺＭＰからずれる様に関節変位を制御することにより、所望のモーメントを生じさせることができる。先に提案した技術ではその思想に基づき、傾き（角度と角速度）偏差が生じたときは図６２に示す様に、故意に実ＺＭＰをずらして姿勢復元方向にモーメントを生じさせる様にした。しかし、この技術によるときは同図に示す如く、実ＺＭＰをずらすことができる範囲は、足平領域ｘｓｏｌｅ内に限られる。従って、姿勢復元方向に発生させ得るモーメントには限界があり、ロボットの姿勢復元を適切に行なうことができない場合がある。
そこで、さらに本願出願人は、特開平５−３３７８４９号公報等にて、次のような技術を提案している。すなわち、この技術では、ロボットの傾き偏差が生じたときには、上記の如く実ＺＭＰをずらして、目標ＺＭＰ回りに許容範囲内で姿勢復元方向のモーメントを発生させると共に、目標歩容を生成するために用いるモデル上で、目標ＺＭＰ回りにモーメントを発生させるように目標歩容の運動を決定する。このように、モデル上でモーメントを意図的に発生させることにより、実際のロボット上で姿勢復元方向のモーメントを発生させることと同等の効果を得ることができる。しかもこの場合、モデル上でのモーメントは、ＺＭＰの存在可能範囲を無視して任意に発生させることができる。このため、実際のロボット上で姿勢復元方向のモーメントを発生させることが困難な場合であっても、モデル上でのモーメントの発生によって、実際のロボットの姿勢を復元方向に適切に安定化することが可能となる。
しかしながら、この技術では、モデル上でのモーメントの発生は、ロボットの上体の水平方向並進加速度を調整する（ひいては、ロボットに作用する並進床反力水平成分を調整する）ことで行なわれる。このため、摩擦力の低い床でのロボットの移動を行なう場合や、ロボットの走行を行うときのように並進床反力の鉛直成分がかなり小さくなる（結果的に発生させ得る摩擦力も小さくなる）ような時期を有するロボットの移動を行なう場合には、ロボットのスリップを生じる恐れがあった。
従って、本発明の目的は、ロボットのスリップを生じることなく、安定した姿勢でロボットの移動を行なうことができる脚式移動ロボットの制御装置を提案することにある。
発明の開示
本発明の脚式移動ロボットの制御装置は、上記の目的を達成するために、上体から延設された脚体を運動させて移動する脚式移動ロボットの目標運動の瞬時値を、少なくとも該ロボットの運動と床反力との関係を表す動力学モデルを用いて逐次決定しつつ、その決定した目標運動の瞬時値に追従させるように前記ロボットの動作制御を行なう脚式移動ロボットの制御装置において、少なくとも前記ロボットに作用する並進床反力水平成分又は並進床反力の床面平行成分又は該ロボットの全体重心加速度又は全体重心加速度の床面平行成分を制限対象量とし、該制限対象量の許容範囲を設定する許容範囲設定手段と、少なくとも前記決定された目標運動の瞬時値に対応するロボットの姿勢の目標状態量と該ロボットの姿勢の実状態量との偏差に基づいて、前記目標運動の新たな瞬時値を、該新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まるように決定する目標瞬時値決定手段とを備えたことを特徴とするものである（第１発明）。
なお、本発明（第１発明以外の発明を含む）において、前記並進床反力の水平成分又は該並進床反力の床面平行成分（詳しくは床面に平行な成分）又はロボットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分は、いずれも摩擦力に比例もしくはほぼ比例するものである。そこで、以下の説明では、便宜上、これらを総称的に摩擦力成分ということがある。
かかる第１発明によれば、目標運動の新たな瞬時値は、ロボットの姿勢の目標状態量と該ロボットの姿勢の実状態量との偏差に基づいて、その新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量（摩擦力成分）が許容範囲内に収まるように決定されるので、前記制限対象量、すなわち摩擦力成分が許容範囲に収まり、且つロボットの姿勢の状態量（例えばロボットの上体の姿勢角や、重心位置等の状態量）に関する前記偏差を０に近づけることが可能となる目標運動の瞬時値を逐次決定できる。このため、この目標運動の瞬時値に追従させるようにロボットの動作制御を行うことにより、摩擦力の小さい床面や、並進床反力の鉛直成分が小さくなるよう時期を有するような歩容（例えば走行歩容）でロボットを移動させる場合においても、ロボットのスリップを生じることなく、安定した姿勢でロボットの移動を行なうことが可能となる。
かかる第１発明では、前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる前記ロボットの複数の運動モードを含み、前記目標瞬時値決定手段は、前記動力学モデルの運動の複数の運動モードを少なくとも前記許容範囲に応じて調整して前記目標運動の新たな瞬時値を決定することが好ましい（第２発明）。
この第２発明によれば、上記複数の運動モードを調整することによって、床反力モーメントと並進床反力とがそれぞれ任意の所望の値になるようなロボットの目標運動を決定することができることとなる。従って、前記制限対象量の許容範囲を考慮しつつ、前記複数の運動モードを少なくとも該制限対象量の許容範囲に応じて調整することによって、前記制限対象量としての摩擦力成分（これは並進床反力に依存する）が許容範囲に収まり、且つ、前記偏差を０に近づけるような床反力モーメントを発生させ得る目標運動の瞬時値を決定できることとなる。例えば、ある１つの運動モードで、前記偏差を０に近づける方向の床反力モーメントを発生させている状態で、前記摩擦力成分が許容範囲を越えそうな場合には、当該１つの運動モードと、他の運動モードとを調整することによって、所望の床反力モーメントを維持しつつ、摩擦力成分を許容範囲内に収めることが可能となる。
上記第２発明ではさらに、前記運動モードは、前記ロボットの重心を略一定に維持しつつ該ロボットの所定部位（例えばロボットの上体）の姿勢を変化させる姿勢変化運動と、該ロボットの上体の並進運動とを含むことが好適である（第３発明）。
これによれば、前記上体の並進運動によって、主に並進床反力を調整でき、また、前記姿勢変化運動によって並進床反力をほとんど変化させずに床反力モーメントを調整できるので、所望の床反力モーメントと所望の並進床反力（ひいては所望の摩擦力成分）とを発生させ得る目標運動の瞬時値の決定処理を容易に行なうことが可能となる。
さらに、上記第１〜第３発明では、少なくとも前記動力学モデルの状態量に応じて前記ロボットの目標床反力及び／又は目標運動の現在以降のパターンを決定するための歩容パラメータを修正する手段を備えることが好ましい（第４発明）。
これによれば、ロボットの目標床反力や目標運動の基本的なパターン（形態）を規定する歩容パラメータ（例えば目標ＺＭＰ（目標床反力中心点）の軌道を規定するパラメータ等）を、前記動力学モデルの状態量に応じて修正するので、動力学モデルの挙動の継続的な安定性を保ちながら、目標床反力や目標運動を決定することが可能となる。
また、前記第１発明では、より具体的には、前記ロボットの目標床反力の基準瞬時値を逐次決定する手段が備えられ、前記目標瞬時値決定手段は、少なくとも前記偏差と前記制限対象量の許容範囲とに応じて前記目標床反力の修正操作量を決定すると共に、該修正操作量により前記基準瞬時値を修正してなる床反力に、前記目標運動の新たな瞬時値により前記動力学モデル上で発生する慣性力と重力との合力を釣り合わせるように前記目標運動の新たな瞬時値を決定することが好ましい（第５発明）。
これによれば、前記偏差と制限対象量とを操作するための要素として、前記目標床反力の修正操作量が用いられる。そして、目標床反力は、特に制限対象量である摩擦力成分に対して直接的に影響する物理量であるから、制限対象量である摩擦力成分を許容範囲内に収めつつ、前記偏差を０に近づけ得るような好適な修正操作量を比較的容易に決定することが可能となる。その結果、摩擦力成分を許容範囲内に収めつつ、前記偏差を０に近づける上で好適な目標運動の新たな瞬時値を、演算負荷を軽減しつつ決定することが可能となる。
かかる第５発明では、前記第２発明や第３発明と同様の形態を採用することが好ましい。すなわち、前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる前記ロボットの複数の運動モードを含み、前記目標瞬時値決定手段は、前記動力学モデルの運動の複数の運動モードを少なくとも前記許容範囲に応じて調整して前記目標運動の新たな瞬時値を決定することが好ましい（第６発明）。さらに、該第６発明において、前記運動モードは、前記ロボットの重心を略一定維持しつつ該ロボットの所定の部位の姿勢を変化させる姿勢変化運動と、該ロボットの上体の並進運動とを含むことが好ましい（第７発明）。
上記第６発明及び第７発明によれば、それぞれ前記第２発明及び第３発明と同様の作用効果を奏することができる。
さらに、前記第７発明では、前記目標瞬時値決定手段は、前記目標床反力の修正操作量により前記基準瞬時値を修正してなる床反力に前記合力を釣り合わせるために、前記姿勢変化運動および前記上体の並進運動のうち、該上体の並進運動を優先的に調整して前記目標運動の新たな目標瞬時値を決定することが好適である（第８発明）。
すなわち、上体の姿勢変化運動等、ロボットの重心回りの姿勢変化運動は、床反力モーメントを変化させるロボットの姿勢の変動が大きくなりやすい。これに対して、上体の並進運動は、その変化に対して並進床反力や床反力モーメントの変化の感度が比較的高い。従って、上体の並進運動の調整を優先することで、ロボットの姿勢の変動を極力抑えることができる。
また、前記第５〜第８発明では、前記目標床反力の修正操作量を少なくとも前記偏差と前記制限対象量の許容範囲と前記動力学モデルの状態量とに応じて決定する手段を備えることが好ましい（第９発明）。
これによれば、前記目標床反力の修正操作量には、前記偏差（すなわち、実際のロボットの姿勢の状態量の偏差）だけでなく、動力学モデルの状態量が加味されることとなる。このため、実際のロボットの姿勢の安定性だけでなく、動力学モデルの挙動の安定性も確保することが可能となり、ひいては、ロボットの実際の安定した移動を行なう上で好適な目標運動の瞬時値を決定できることとなる。
なお、動力学モデルの状態量は、動力学モデル上でのロボットの重心位置や上体姿勢角、それらの変化速度等が挙げられる。
また、前記第５〜第９発明では、前記目標運動の瞬時値を決定するために用いる前記目標床反力の修正操作量及び／又は該修正操作量による前記動力学モデルの状態量の変化に応じて前記ロボットの目標床反力及び／又は目標運動の現在以降のパターンを決定するための歩容パラメータを修正する手段を備えることが好ましい（第１０発明）。
これによれば、ロボットの目標床反力や目標運動の基本的なパターン（形態）を規定する歩容パラメータ（例えば目標ＺＭＰ（目標床反力中心点）の軌道を規定するパラメータ等）を、前記目標床反力の修正操作量やその修正操作量による動力学モデルの状態量の変化に応じて修正するので、動力学モデルの挙動の継続的な安定性を確保することが可能となる。
また、前記第５〜第１０発明では、前記偏差に応じて該偏差が０に近づくように床反力の第１要求操作量を決定する第１手段と、少なくとも該第１要求操作量に基づき前記ロボットの実床反力の目標値を規定する実床反力操作量と前記目標床反力の修正操作量とを決定する第２手段と、前記実床反力操作量と前記目標運動の瞬時値とに応じて前記ロボットの動作制御を行なう第３手段とを備えることが好ましい（第１１発明）。
これによれば、前記目標床反力の修正操作量によって動力学モデル上での床反力が操作されることに加えて、前記実床反力操作量によって実際のロボット上での実床反力が操作されることとなる。この結果、前記偏差を０に近づけるための床反力の操作を実際のロボットと、動力学モデルとに分配することができ、実際のロボットの挙動の変化と動力学モデルの挙動の変化とを必要限に留めることができる。ひいては、実際のロボットの挙動をより一層安定化することができる。
この第１１発明では、前記第１要求操作量、前記実床反力操作量、および前記目標床反力の修正操作量は、いずれも床反力作用点回りの床反力モーメント又は該床反力作用点の位置又はＺＭＰに関する操作量であることが好適である（第１２発明）。すなわち、ロボットの姿勢の安定性は、床反力作用点回りの床反力モーメント又は該床反力作用点の位置又はＺＭＰ（その点まわりの床反力モーメントの水平成分が０になる点）の位置が大きく影響するので、それらに関する操作量を第１要求操作量、前記実床反力操作量、および前記目標床反力の修正操作量として用いることにより、ロボットの姿勢の安定性の確保を確実に行なうことが可能となる。
さらに、上記第１１あるいは第１２発明では、前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が前記第１要求操作量の増加に伴い単調に変化するように前記実床反力操作量と前記目標床反力の修正操作量とを決定することが好ましい（第１３発明）。
これによれば、前記偏差を適正に０に近づけることが可能な実床反力操作量と目標床反力の修正操作量とを決定できる。なお、前記実床反力操作量と前記目標床反力の修正操作量との差が前記第１要求操作量の増加に伴い単調に変化するようにするためには、例えば、該差が前記第１要求操作量にほぼ一致するか、もしくは比例するように実床反力操作量と目標床反力の修正操作量とを決定すればよい。
上記第１１〜第１３発明では、前記第２手段は、前記実床反力操作量を前記基準瞬時値に加えたものが所定の実床反力許容範囲内に収まるように該実床反力操作量を決定することが好ましい（第１４発明）。これによれば、前記摩擦力成分（制限対象量）だけでなく前記実床反力操作量に対応する床反力成分、すなわち、床反力作用点回りの床反力モーメントや、該床反力作用点の位置、ＺＭＰを所望の許容範囲内に収めることが可能となる。従って、ロボットの挙動の安定性をさらに高めることができる。
上記第１４発明では、より具体的には、例えば前記第２手段は、前記第１要求操作量を前記基準瞬時値に加えたものが前記所定の実床反力許容範囲内の値であるときには、該第１要求操作量を前記実床反力操作量として決定すると共に、前記目標床反力の修正操作量を略０又は前記動力学モデルの状態量に応じて定めた値に決定し、前記第１要求操作量を前記基準瞬時値に加えたものが前記所定の実床反力許容範囲を逸脱する値であるときには、該実床反力許容範囲の限界値から前記基準瞬時値を差し引いたものを前記実床反力操作量として決定すると共に、少なくとも前記実床反力操作量と前記第１要求操作量との差分に応じて前記目標床反力の修正操作量を決定する（第１５発明）。
これによれば、実床反力操作量の仮決定値を前記基準瞬時値に加えたもの（これはロボットの実床反力に相当する。以下、ここでは、実床反力推定値という）が前記実床反力許容範囲内に存在する場合には、該実床反力操作量によって実際のロボットの実床反力を操作することで、前記偏差が０に近づくように制御される。そして、上記実床反力推定値が実床反力許容範囲を逸脱した場合には、実床反力推定値が実床反力許容範囲内に収まるように制限した実床力操作量が決定される。このため、この実床反力操作量に応じてロボットの実床反力を操作したとき、実床反力を所定の実床反力許容範囲に収めることができる。さらに、実床反力推定値が実床反力許容範囲を逸脱した場合には、実床反力操作量が制限されるが、前記実床反力操作量と前記第１要求操作量との差分、すなわち、前記偏差を０に近づけるための第１要求操作量のうち、実床反力操作量ではまかないきれない余剰分に応じて、目標床反力の修正操作量を決定するので、実床反力操作量に応じた実床反力の操作と、目標床反力の修正操作量に応じた動力学モデル上での床反力操作（目標運動の瞬時値の調整）とを併せて、前記偏差を０に近づけるようにすることができる。なお、実床反力推定値が実床反力許容範囲内に存在する場合において、目標床反力の修正操作量を動力学モデルの状態量に応じて決定したときには、該動力学モデルの挙動を安定化できる。
前記第１４発明では、前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記第１要求操作量に応じた値になるように該実床反力操作量および目標床反力の修正操作量の仮決定値を求める手段と、その求めた目標床反力の修正操作量の仮決定値を用いて前記目標運動の新たな瞬時値を決定したと仮定した場合における当該目標運動の新たな瞬時値に対応する前記制限対象量の推定値を求める手段と、前記求めた実床反力操作量の仮決定値を前記基準瞬時値に加えたものと前記制限対象量の推定値とを前記所定の実床反力許容範囲及び前記制限対象量の許容範囲と比較し、その比較結果に基づき該実床反力許容範囲及び該制限対象量の許容範囲の制限を満たす実床反力操作量および制限対象量の組を決定する手段と、その決定した実床反力操作量から前記第１要求操作量を差し引いたものと制限対象量とに基づき前記目標床反力の修正操作量を決定する手段とから構成するようにしてもよい（第１６発明）。
この第１６発明によれば、前記実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記偏差を０に近づけるための第１要求操作量に応じた値になるように該実床反力操作量及び目標床反力の修正操作量の仮決定値を求めた上で、修正操作量の仮決定値を用いて目標運動の新たな瞬時値を決定したと過程した場合における前記制限対象量の推定値が求められる。そして、実床反力操作量の仮決定値を前記基準瞬時値に加えたもの（以下、ここでは仮実床反力という）と前記制限対象量の推定値とが実床反力許容範囲及び制限対象量の許容範囲と比較され、それらの許容範囲の制限を満たす実床反力操作量および制限対象量の組が決定される。さらに、実床反力操作量から第１要求操作量を差し引いたものと制限対象量とに基づいて、目標床反力の修正操作量が決定される。
これにより、前記偏差を０に近づけ、且つ、実床反力操作量と制限対象量とをそれぞれの許容範囲に適正に収め得る実床反力操作量と修正操作量との組を決定できる。ひいては、ロボットの挙動の安定性を高めることができる。
なお、前記実床反力許容範囲および制限対象量の許容範囲の制限を満たす前記実床反力操作量と目標床反力の修正操作量との組は、探索的に決定することも可能である。
上記第１６発明では、前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる複数の運動モードを含んでおり、前記目標床反力の修正操作量は、各運動モードにそれぞれ対応する複数の操作量から構成されていることが好ましい（第１７発明）。
これによれば、各運動モードに対応する各操作量を調整することで、前記動力学モデル上で所望の並進床反力と床反力モーメントとを発生させることができるので、特に制限対象量をそれに対する許容範囲内に収める上で好適な修正操作量を決定することが可能となる。なお、この第１７発明では、前記複数の運動モードは、前記第３発明あるいは第７発明と同様であることが好ましい。
前記第１１発明では、前記動力学モデルの状態量に応じて該状態量を所定の状態量（基本的には動力学モデルの挙動を安定化できる状態量）に近づけるように床反力の第２要求操作量を決定する第４手段を備え、前記第２手段は、少なくとも前記第１要求操作量と前記第２要求操作量と前記許容範囲とに基づき前記実床反力操作量と前記目標床反力の修正操作量とを決定することがより好ましい（第１８発明）。
これによれば、前記偏差を０に近づけ、且つ、動力学モデルの状態を安定化し、且つ、実床反力が実床反力許容範囲に収まり、且つ、制限対象量（摩擦力成分）がそれに対する許容範囲に収まるような目標運動の瞬時値を適正に決定し得る実床反力操作量と目標床反力の修正操作量とを適正に決定することが可能となる。
この第１８発明では、前記第１２発明の場合と同様、前記第１および第２要求操作量、前記実床反力操作量並びに前記目標床反力の修正量は、いずれも床反力作用点回りの床反力モーメント又は該床反力作用点の位置又はＺＭＰに関する操作量であることが好ましい（第１９発明）。これによれば、第１２発明と同様に、ロボットの姿勢の安定性の確保を確実に行なうことが可能となる。
さらに、第１８発明および第１９発明では、前記第１３発明の場合と同様に、前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が前記第１要求操作量の増加に伴い単調に変化するように前記実床反力操作量と前記目標床反力の修正操作量とを決定することが好ましい（第２０発明）。
これによれば、前記第１３発明と同様に、前記偏差を適正に０に近づけることが可能な実床反力操作量と目標床反力の修正操作量とを決定できる。
さらに、上記第１８〜第２０発明では、前記第１４発明の場合と同様に、前記第２手段は、前記実床反力操作量を前記基準瞬時値に加えたものが所定の実床反力許容範囲内に収まるように該実床反力操作量を決定することが好ましい（第２１発明）。
これによれば、第１４発明と同様に、前記摩擦力成分（制限対象量）だけでなく前記実床反力操作量に対応する床反力成分、すなわち、床反力作用点回りの床反力モーメントや、該床反力作用点の位置、ＺＭＰを所望の許容範囲内に収めることが可能となる。従って、ロボットの挙動の安定性をさらに高めることができる。
また、上記第２１発明では、より具体的には、前記第１６発明と同様の形態が好ましい。すなわち、前記第２手段は、前記実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記第１要求操作量に応じた値になり、且つ前記目標床反力の修正操作量が前記第２要求操作量に応じた値になるように該実床反力操作量および目標床反力の修正操作量の仮決定値を求める手段と、その求めた目標床反力の修正操作量の仮決定値を用いて前記目標運動の新たな瞬時値を決定したと仮定した場合における当該目標運動の新たな瞬時値に対応する前記制限対象量の推定値を求める手段と、前記求めた実床反力操作量の仮決定値を前記基準瞬時値に加えたものと前記制限対象量の推定値とを前記所定の実床反力許容範囲及び前記制限対象量の許容範囲と比較し、その比較結果に基づき該実床反力許容範囲及び該制限対象量の許容範囲の制限を満たす実床反力操作量および制限対象量の組を決定する手段と、その決定した実床反力操作量から前記第１要求操作量を差し引いたものと制限対象量とに基づき前記目標床反力の修正操作量を決定する手段とから構成されていることが好ましい（第２２発明）。
この第２２発明は、前記第１要求操作量に加えて、前記動力学モデルの状態量を所定の状態量に近づけるための第２要求操作量が考慮され、前記実床反力操作量および目標床反力の修正操作量の仮決定値が、実床反力操作量と前記目標床反力の修正操作量との差が少なくとも前記第１要求操作量に応じた値になり、且つ前記目標床反力の修正操作量が前記第２要求操作量に応じた値になるように決定される。そして、これ以外は、前記１６発明と同様である。
従って、第２２発明によれば、前記偏差を０に近づけ、且つ、動力学モデルの挙動の安定性を高め、且つ、実床反力操作量と制限対象量とをそれぞれの許容範囲に適正に収め得る実床反力操作量と修正操作量との組を決定できる。ひいては、ロボットの挙動の安定性を高めることができる。
なお、前記実床反力許容範囲および制限対象量の許容範囲の制限を満たす前記実床反力操作量と目標床反力の修正操作量との組は、探索的に決定することも可能である。
また、上記第２２発明では、前記第１７発明と同様、前記動力学モデルの運動は、少なくとも床反力モーメントと並進床反力との発生比率が互いに異なる複数の運動モードを含んでおり、前記目標床反力の修正操作量は、各運動モードにそれぞれ対応する複数の操作量から構成されていることが好ましい（第２３発明）。
これによれば、前記第１７発明と同様、特に制限対象量をそれに対する許容範囲内に収める上で好適な修正操作量を決定することが可能となる。なお、この第２３発明においても、前記複数の運動モードは、前記第３発明あるいは第７発明と同様であることが好ましい。
発明を実施するための最良の形態
以下、脚式移動ロボットとして２足移動ロボットを例にとって、本発明の実施形態を説明する。図１はそのロボット１を全体的に示す説明スケルトン図である。図示の如く、ロボット１は上体２４から下方に延設された左右一対の脚体（脚部リンク）２，２を備える。両脚体２，２は同一構造であり、それぞれ６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。該６個の関節は上体２４側から順に、腰（股）の回旋用（上体２４に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌ（符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、腰（股）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、腰（股）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌと、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首部のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首部のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。
各脚体２の足首部の２つの関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には、各脚体２の先端部を構成する足平２２Ｒ（Ｌ）が取着されると共に、両脚体２，２の最上位には各脚体２の股の３つの関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して前記上体（筐体）２４が設けられている。上体２４の内部には制御ユニット２６などが格納されている。なお、図１では図示の便宜上、制御ユニット２６を上体２４の外部に記載している。
上記構成の各脚体２においては、腰関節（股関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、膝関節は関節１６Ｒ（Ｌ）から構成され、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また、腰関節（股関節）と膝関節とは大腿リンク３２Ｒ（Ｌ）で連結され、膝関節と足関節とは下腿リンク３４Ｒ（Ｌ）で連結される。
なお、図示は省略するが、上体２４の上部の両側部には左右一対の腕対が取り付けられると共に、上体２４の上端部には頭部が配置される。これらの腕体および頭部は、本発明の要旨と直接的な関連を有しないので詳細な説明は省略するが、各腕体は、それに備える複数の関節によって、該腕体を上体２４に対して前後に振る等の運動を行なうことが可能となっている。
また、上記した各関節は電動モータからなり、更にはその出力を倍力する減速機などを備える。その詳細は先に本出願人が提案した特願平１−３２４２１８号、特開平３−１８４７８２号などに述べられており、それ自体はこの発明の要旨とするところではないので、これ以上の説明は省略する。
各脚体２の上記構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体２４に対して６つの自由度を与えられている。そして、ロボット１の移動に際して両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算においては乗算を示し、ベクトルに対する演算においては外積を示す）の関節をそれぞれ適宜な角度に駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行なうことができる。これによりロボット１は任意に３次元空間を移動することができる。
尚、この明細書で後述する上体２４の位置および速度は、上体２４の所定位置、具体的には上体２４のあらかじめ定めた代表点（例えば左右の股関節の間の中央点等）の位置およびその移動速度を意味する。同様に、各足平２２Ｒ，２２Ｌの位置及び速度は、各足平２２Ｒ，２２Ｌのあらかじめ定めた代表点の位置及びその移動速度を意味する。この場合、本実施形態では各足平２２Ｒ，２２Ｌの代表点は、例えば各足平２２Ｒ，２２Ｌの底面上（より具体的には各脚体２の足首関節の中心から各足平２２Ｒ，２２Ｌの底面への垂線が該底面と交わる点等）に設定される。
図１に示すロボット１において、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）と足平２２Ｒ（Ｌ）との間には公知の６軸力センサ３６が介装されている。該６軸力センサ３６は、各脚体２の足平２２Ｒ（Ｌ）の着地の有無と支持脚に床から作用する力の大きさと方向とを検出するためのものであり、床から足平２２Ｒ（Ｌ）を介して各脚体２に伝達される床反力のＸ，Ｙ，Ｚ方向の並進力成分Ｆｘ，Ｆｙ，Ｆｚとその３方向まわりのモーメント成分Ｍｘ，Ｍｙ，Ｍｚの検出信号を制御ユニット２６に出力する。また、各足平２２Ｒ（Ｌ）の四隅には、各足平２２Ｒ（Ｌ）の接地の有無を検出するための静電容量型の接地スイッチ３８（図１では図示省略）が設けられ、その検出信号が該接地スイッチ３８から制御ユニット２６に出力される。さらに、上体２４には、Ｘ−Ｚ平面内とＹ−Ｚ平面内のｚ軸に対する、即ち、重力方向（鉛直方向）に対する上体２４の傾斜角度（姿勢角）とその角速度を検出するための傾斜センサ４０が設置され、その検出信号が該傾斜センサ４０から制御ユニット２６に出力される。また各関節の電動モータ４６（図２参照）には、その回転量を検出するためのエンコーダ（ロータリエンコーダ）４７（図２参照）が設けられ、該エンコーダ４７の検出信号が該エンコーダ４７から制御ユニット２６に出力される。更に、図１では省略するが、ロボット１の適宜な位置には傾斜センサ４０の出力を補正するための原点スイッチ４２と、フェール対策用のリミットスイッチ４４とが設けられ、これらの出力が制御ユニット２６に送られる。
さらに、図示を省略するが、ロボット１の外部には、該ロボット１の操縦用のジョイスティック（操作器）が設けられ、そのジョイスティックを操作することで、直進移動しているロボット１を旋回させるなど、ロボット１の歩容に対する要求を必要に応じて制御ユニット２６に入力できるように構成される。この場合、入力できる要求は、例えばロボット１の移動時の歩容形態（歩行、走行等）、遊脚の着地位置姿勢や着地時刻、あるいはこれらの着地位置姿勢や着地時刻を規定する指令データ（例えばロボット１の移動方向、移動速度等）である。
図２は制御ユニット２６の構成を示すブロック図である。該制御ユニット２６はマイクロコンピュータにより構成されており、ＣＰＵからなる第１の演算装置６０及び第２の演算装置６２、Ａ／Ｄ変換器５０、カウンタ５６、Ｄ／Ａ変換器６６、波形成形回路５８、ＲＡＭ５４、ＲＯＭ６４、並びにこれらの間のデータ授受を行なうバスライン５２を備えている。この制御ユニット２６では、前記各脚体２の６軸力センサ３６、傾斜センサ４０等の出力信号はＡ／Ｄ変換器５０でデジタル値に変換された後、バスライン５２を介してＲＡＭ５４に入力される。また、ロボット１の各関節のエンコーダ（ロータリエンコーダ）４７の出力はカウンタ５６を介してＲＡＭ５４に入力される。さらに、前記接地スイッチ３８、原点スイッチ４２、リミットスイッチ４４などの出力は波形整形回路５８を経てＲＡＭ５４に入力される。
前記第１の演算装置６０は後述の如く、目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ３２の回転角の目標値）を算出し、ＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４から関節角変位指令と、前記エンコーダ４７の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器６６とサーボアンプ４６ａとを介して各関節を駆動する電動モータ４６に出力する。
以上説明した構成は、以下に説明するいずれの実施形態においても共通の構成である。
図３は、本発明の第１実施形態におけるロボット１の制御装置の機能的構成を全体的に示すブロック図である。この図３中の「実ロボット」の部分以外の部分が制御ユニット２６が実行する処理機能（主として第１の演算装置６０及び第２の演算装置６２の機能）によって構成されるものである。尚、以下の説明では、脚体２の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。
同図３を参照して、本実施形態の制御装置の動作の概要を説明すると、制御ユニット２６は、歩容生成装置１００により後述する如く目標歩容を生成する。この歩容生成装置１００が生成して出力する目標歩容は、目標上体位置姿勢軌道（上体２４の目標位置及び目標姿勢の軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体の目標姿勢の軌道）、目標ＺＭＰ（目標全床反力中心点）軌道、および目標全床反力軌道から構成される。尚、脚体２や腕体以外に、上体２４に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。
ここで、上記歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、歩容生成装置１００の制御周期（演算処理周期）毎に算出される瞬時値の時系列により構成される。以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はＺ軸（鉛直軸）に対するロール方向（Ｘ軸まわり）の上体２４の傾斜角（姿勢角）とピッチ方向（Ｙ軸まわり）の上体２４の傾斜角（姿勢角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。また、広義では、上体姿勢にヨー方向（Ｚ軸まわり）の上体２４の回転を含む場合もある。
尚、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる要素（目標ＺＭＰおよび目標全床反力）以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット１の各部の運動に係わる歩容を総称的に「運動」という。また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。
目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、本明細書の実施形態では、目標ＺＭＰ（目標床反力中心点）を目標床反力の作用点とする。このとき、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Ｚ軸）まわりのモーメント）を除いて零になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（Ｘ軸及びＹ軸）回りのモーメント）は零になる。
歩容生成装置１００により生成される目標歩容のうち、目標上体位置姿勢（軌道）と目標腕姿勢（軌道）とは、ロボット幾何学モデル（キネマティクス演算部）１０２に直接送られる。
また、目標足平位置姿勢軌道、目標ＺＭＰ軌道（目標全床反力中心点軌道）、および目標全床反力軌道（詳細には目標並進床反力鉛直成分軌道、目標並進床反力水平成分軌道、目標ＺＭＰまわりの目標床反力モーメント軌道）は、複合コンプライアンス動作決定部１０４に直接送られると共に、目標床反力分配器１０６にも送られる。そして、該目標床反力分配器１０６にて、目標床反力は各足平２２Ｒ，２２Ｌに分配され、目標各足平床反力中心点および目標各足平床反力が決定される。この決定された目標各足平床反力中心点および目標各足平床反力は複合コンプライアンス動作決定部１０４に送られる。
複合コンプライアンス動作決定部１０４では、目標足平位姿勢（軌道）を修正してなる機構変形補償付き修正目標足平位置姿勢（軌道）が求められ、この修正目標足平位置姿勢（軌道）がロボット幾何学モデル１０２に送られる。ロボット幾何学モデル１０２は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）を入力されると、それらを満足する両脚体２，２の１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）をロボット１のキネマティクスモデル（剛体リンクモデル）基づく逆キネマティクス演算によって算出し、その算出した関節変位指令を変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令（値）を目標値としてロボット１の両脚体２，２の１２個の関節の変位を追従制御する。また、ロボット幾何学モデル１０２は、目標腕姿勢を満足する腕関節の変位指令（値）を算出して変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された腕関節の関節変位指令（値）を目標値としてロボット１の腕体の１０（もしくは８）個の関節の変位を追従制御する。
上記のような追従制御によるロボット１の実際の運動によって該ロボット１に生じた床反力（実各足床反力）は前記６軸力センサ３６によって検出される。その検出値は前記複合コンプライアンス動作決定部１０４に送られる。
また、ロボット１に生じた実上体姿勢角偏差（目標上体姿勢角に対する実上体姿勢角の偏差）が前記傾斜センサ３６によって検出され、その検出値は姿勢安定化制御演算部１１２に送られる。そして、該姿勢安定化制御演算部１１２は、与えられた実上体姿勢角偏差に基づいて、ロボット１の実上体姿勢角を目標上体姿勢角に復元する（実上体姿勢角偏差を０に近づける）ための目標ＺＭＰまわりの補償全床反力モーメントＭｄｍｄを適宜のフィードバック制御則により算出する。
より具体的には、本実施形態では、補償全床反力モーメントＭｄｍｄは、例えばＰＤ制御則を用いて次式により決定される。
2003061917.tif 000003
ここで、実上体姿勢角速度偏差は、実上体姿勢角偏差の時間微分値であり、目標上体姿勢角速度に対する実上体姿勢角速度の偏差を意味する。また、実上体姿勢角偏差は、より詳しくは、ロボット１の上体２４のロール方向（Ｘ軸回り）の姿勢角偏差と、ピッチ方向（Ｙ軸回り）の姿勢角偏差とからなるベクトルであり、このことは補償全床反力モーメントＭｄｍｄも同様である。
上記の如く決定された補償全床反力モーメントＭｄｍｄは、補償全床反力モーメント分配器１１０により、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントとに分配される。換言すれば、実上体姿勢角偏差を基に、姿勢安定化制御演算部１１２及び補償全床反力モーメント分配器１１０により、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントとが決定される。
コンプライアンス制御用目標床反力モーメントは、実ロボット１の姿勢を目標歩容（正確には修正歩容）に収束させて安定化させる（復元させる）ために実ロボット１に発生させる目標ＺＭＰまわりの床反力モーメントである。モデル操作床反力モーメントは、コンプライアンス制御用目標床反力モーメントと同じ目的のために後述する歩容生成用のロボット動力学モデルに発生させる目標ＺＭＰまわりの床反力モーメントである。モデル操作床反力モーメントは、言い換えると、最終的に決定された目標歩容（修正歩容）の運動が目標ＺＭＰまわりに発生する床反力モーメントである。
これらの床反力モーメントは、以下のように決定される。まず、モデル操作床反力モーメントが次式で決定される。なお、床反力モーメント許容範囲は、後述するごとく歩容生成装置１００において決定される。
2003061917.tif 000004
なお、上記式ｄ２６では、補償全床反力モーメントＭｄｍｄそのものを床反力モーメント許容範囲と比較しているが、本来は、上記式ｄ２６で床反力モーメント許容範囲と比較すべき対象は、Ｍｄｍｄを目標ＺＭＰまわりの床反力モーメントの基準瞬時値に加えたものである。この場合、本実施形態では、目標ＺＭＰまわりのモーメント（詳しくは水平成分）の基準瞬時値（歩容生成装置１００が出力する目標全床反力のモーメントの水平成分）は常に「０」であるため、これにＭｄｍｄを加えたものは、Ｍｄｍｄに等しい。このため、上記の如くＭｄｍｄを直接的に床反力モーメント許容範囲と比較するようにしている。
次に、コンプライアンス制御用目標床反力モーメントが次式で決定される。
2003061917.tif 000005
従って、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントとの差がＭｄｍｄに等しくなるようにそれらの床反力モーメントが決定される。
上記のような演算を行なう補償全床反力モーメント分配器１１０をブロック図で表すと、図４に示すようになる。
図３の説明に戻って、歩容生成装置１００には前記決定されたモデル操作床反力モーメントが入力される。そして、詳細は後述するが、歩容生成装置１００は、該歩容生成装置１００において決定される目標ＺＭＰまわりの床反力モーメントがモデル操作床反力モーメントとなるように動力学モデルを用いて目標歩容の運動（特に上体位置姿勢軌道）を生成する。
また、前記決定されたコンプライアンス制御用目標床反力モーメントは、複合コンプライアンス動作決定部１０４に入力される。そして、複合コンプライアンス動作決定部１０４は、歩容生成装置１００が生成する目標歩容の運動にロボット１の運動を追従させつつ、目標ＺＭＰまわりの実床反力モーメントがコンプライアンス制御用目標床反力モーメントに近づくように、目標足平位置姿勢を修正することにより、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。この場合、ロボット１の足平位置姿勢や床反力の全ての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の２乗）の重み付き平均が最小になるように機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。換言すれば、目標ＺＭＰまわりの実床反力モーメントと、ロボット１の実際の足平位置姿勢とがそれぞれコンプライアンス制御用目標床反力モーメント、目標足平位置姿勢にできるだけ近づくように機構変形補償付き修正目標足平位置姿勢（軌道）が決定される。そして、複合コンプライアンス動作決定部１０４は、この修正目標足平位置姿勢をロボット幾何学モデル１０２に足平位置姿勢の最終的な目標値として出力することで、ロボット１の動作を制御する。
なお、この本発明の要旨は主に、歩容生成装置１００における歩容生成の手法にあり、上記した複合コンプライアンス動作決定部１０４などの構成および動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されているので、説明を以上に止める。
次に、図５に示す走行歩容を例にして、歩容生成装置１００が生成する目標歩容の概要を説明する。なお、以降の説明では、「床反力鉛直成分」は「並進床反力鉛直成分」を意味するものとし、床反力のうちのモーメントの鉛直成分（鉛直軸回り成分）は、「モーメント」という用語を用いて「床反力鉛直成分」と区別をする。同様に、「床反力水平成分」は「並進床反力水平成分」を意味するものとする。また、歩容における両脚支持期とは言うまでもなく、ロボット１がその自重を両脚体２，２で支持する期間、片脚支持期とはいずれか一方のみの脚体２でロボット１の自重を支持する期間、空中期とは両脚体２，２が床から離れている（空中に浮いている）期間を言う。片脚支持期においてロボット１の自重を支持しない側の脚体２を「遊脚」と呼ぶ。但し、本実施形態で主に説明する走行歩容では、両脚支持期は無く、片脚支持期（着地期）と空中期とが交互に繰り返される。この場合、空中期では両脚２，２とも、ロボット１の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体２、支持脚であった脚体２をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。
まず、図５に示す走行歩容を説明すると、この走行歩容は、人間の通常的な走行歩容と同様の歩容である。この走行歩容では、ロボット１の左右いずれか一方のみの脚体２（支持脚）の足平２２が着地（接地）する片脚支持期と、両脚体２，２が空中に浮く空中期とが交互に繰り返される。図５の１番目の状態は片脚支持期の開始時（初期）の状態、２番目の状態は片脚支持期の中間時点の状態、３番目の状態は片脚支持期に続く空中期の開始時（片脚支持期の終了時）の状態、４番目の状態は空中期の中間時点の状態、５番目の状態は空中期の終了時（次の片脚支持期の開始時）の状態を示している。
この走行歩容では、ロボット１は、図５の１番目の状態で示すように、片脚支持期の開始時において支持脚（ロボット１の進行方向前側の脚体２）側の足平２２のかかとで着地する。続いて、図５の２番目の状態で示すように、ロボット１は、着地した足平２２（支持脚側の足平２２）の底面のほぼ全面を着地させた後、図５の３番目の状態で示すように、その支持脚側の足平２２（図５の３番目の状態におけるロボット１の進行方向後側の脚体２の足平２２）のつま先で床を蹴って空中に飛び上がる。これより片脚支持期が終了すると共に空中期が開始する。尚、片脚支持期における遊脚は、図５の１番目の状態で示すように、片脚支持期の開始時では、支持脚の後側に存在するが、図５の２番目及び３番目の状態で示すように次の着地予定位置に向かって支持脚の前側に振り出される。次いで、図５の４番目の状態で示す空中期を経た後、ロボット１は、遊脚（空中期の開始直前の片脚支持期で遊脚となっていた脚体２）の足平２２のかかとで着地し、次の片脚支持期が開始される。
図５の走行歩容を考慮しつつ、前記歩容生成装置１００が生成する目標歩容の基本的な概要を説明する。なお、歩容に関する詳細は、先に本願出願人が提案した特開平１０−８６０８１号公報にも記載されているので、以下では、特開平１０−８６０８１号公報に記載されていない内容を主に説明する。
詳細は後述するが、歩容生成装置１００が目標歩容を生成するとき、遊脚側の足平２２の着地位置姿勢（着地予定位置姿勢）や着地時刻（着地予定時刻）等の目標歩容生成用の基本的な要求値（要求パラメータ）が、ジョイスティック（ロボット１の操作器）等の所要の操作等に応じて歩容生成装置１００に与えられる。そして、歩容生成装置１００は、その要求パラメータを用いて目標歩容を生成する。より詳しく言えば、歩容生成装置１００は、上記要求パラメータに応じて、目標歩容の目標足平位置姿勢軌道、目標床反力鉛直成分軌道等、目標歩容の一部の構成要素を規定するパラメータ（歩容パラメータという）を決定した上で、その歩容パラメータを用いて目標歩容の瞬時値を逐次決定し、該目標歩容の時系列パターンを生成する。
この場合、目標足平位置姿勢軌道（より詳しくは、足平の位置及び姿勢の空間的な各成分（Ｘ軸成分等）の目標軌道）は、例えば本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に生成される。この有限時間整定フィルタは、可変時定数の１次遅れフィルタ、すなわち、伝達関数が１／（１＋τｓ）の形で表されるフィルタ（τは可変の時定数。以下、このフィルタを単位フィルタという）を複数段（本実施形態では３段以上）、直列に接続したものであり、所望の指定時刻に指定値に到達するような軌道を生成・出力することができるものである。この場合、各段の単位フィルタの時定数τは、いずれも、有限時間整定フィルタの出力生成を開始してから、上記指定時刻までの残時間に応じて逐次可変的に設定される。より詳しくは、該残時間が短くなるに伴いτの値が所定の初期値（＞０）から減少されていき、最終的には、該残時間が０になる指定時刻にて、τの値が０になるように設定される。そして、有限時間整定フィルタには、前記指定値（より詳しくは、有限時間整定フィルタの出力の初期値から前記指定値への変化量）に応じた高さのステップ入力が与えられる。このような有限時間整定フィルタは、指定時刻にて指定値に達するような出力が生成されるだけでなく、指定時刻における有限時間整定フィルタの出力の変化速度を０もしくはほぼ０にすることができる。特に、単位フィルタを３段以上（３段でよい）、接続した場合には、有限時間整定フィルタの出力の変化加速度（変化速度の微分値）をも０もしくはほぼ０にすることができる。
このような有限時間整定フィルタを用いる足平位置姿勢軌道（足平２２が着地してから次に着地するまでの位置姿勢軌道）の生成は、例えば次のように行なわれる。例えばＸ軸方向（前後方向）の目標足平位置軌道は次のように生成される。すなわち、前記要求パラメータにより定まる各足平２２の次の着地予定位置のＸ軸方向位置（より詳しくは、次の着地予定位置のひとつ前の着地位置に対するＸ軸方向の変化量（移動量）。これは前記指定値に相当する）に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に前記時定数τが所定の初期値に初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、足平２２のＸ軸方向位置の軌道生成が開始される。そして、この軌道生成時には、前記時定数τは、足平２２の着地予定時刻（これは前記指定時刻に相当する）までに初期値から０まで減少していくように、逐次可変設定される。これにより、着地予定時刻で着地予定位置に達するような、足平２２のＸ軸方向の位置の軌道が生成される。
また、Ｚ軸方向（鉛直方向）の目標足平位置軌道は、例えば次のように生成される。すなわち、まず、足平２２の次の着地予定位置及び着地予定時刻に応じて、該足平２２の高さ（鉛直位置）が最大になるときの該足平２２のＺ軸方向位置（以下、最高点位置という）とその最高点位置への到達時刻とが決定される。そして、その最高点位置（これは前記指定値に相当する）に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に時定数τが初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、前記最高点位置までのＺ軸方向の足平位置軌道が逐次生成される。この際、時定数τは、最高点位置への到達時刻（前記指定時刻に相当）までに初期値から０まで減少するように逐次可変設定される。さらに、最高点位置までのＺ軸方向位置の軌道の生成が終了したら、時定数τを初期化すると共にいままでのステップ入力と逆極性のステップ入力（より詳しくは、最高点位置から次の着地予定位置までのＺ軸方向の変化量（これは前記指定値に相当する）に応じた高さの逆極性のステップ入力）が有限時間整定フィルタに入力され、該最高点位置から着地予定位置までのＺ軸方向の足平位置の軌道が逐次生成される。この際、時定数τは足平２２の着地予定時刻までに初期値から０まで減少するように逐次可変設定される。
尚、Ｚ軸方向の足平位置軌道の生成においては、時定数τを軌道生成開始時刻から足平２２の着地予定時刻まで、初期値から０まで継続的に減少するように可変設定すると共に、最高点位置への到達時刻もしくはその近傍時刻で、ステップ入力の極性を逆極性に切り替えることで、Ｚ軸方向の足平位置軌道を生成するようにしてもよい。この場合には、足平２２を所望の最高点位置に精度よく到達させることはできないが、着地予定時刻での着地予定位置への到達は問題なく行なうことができる。
足平姿勢軌道についても、上述した足平位置軌道と同様に有限時間整定フィルタを用いて生成することができる。この場合、足平姿勢の空間的な各成分のうち、その姿勢の角度変化が単調的（単調増加もしくは単調減少）なものとなる成分については、前記したＸ軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。また、姿勢の角度変化が極大値もしくは極小値をもつような成分については、前記したＺ軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。
尚、上記のように有限時間整定フィルタにより生成される目標足平位置姿勢軌道は、床面に固定された後述の支持脚座標系での各足平２２の目標位置姿勢軌道である。
上述のように生成される目標足平位置姿勢軌道は、各足平２２の位置が、その初期接地状態（目標歩容の初期時刻の状態）から着地予定位置に向かって徐々に加速しながら移動を開始するように生成される。そして、該目標足平位置姿勢軌道は、最終的に着地予定時刻までに徐々に位置の変化速度を０またはほぼ０にまで減速し、着地予定時刻にて着地予定位置に到達して停止するように生成される。このため、各足平２２の着地瞬間における対地速度（床に固定された支持脚座標系での各足平２２の位置の変化速度）が０またはほぼ０になる。したがって、走行歩容において同時に全脚体２，２が空中に存在する状態（空中期での状態）から着地しても、着地衝撃が小さくなる。
前記走行歩容においては、ロボット１に作用する重力によって空中期後半から上体２４の鉛直速度は下向きになり、着地時でも下向きのままである。したがって、上記のように各足平２２の着地瞬間における対地速度が０またはほぼ０になるように目標足平位置姿勢軌道を生成すると共に、後述する如く動力学的平衡条件を満たすように上体２４の目標位置姿勢軌道を生成したとき、着地直前において、上体２４に対する遊脚側の足平２２の相対速度は、上向きになる。すなわち、走行歩容の着地瞬間では、ロボット１の目標歩容は遊脚側の脚体２２を上体２４側に引っ込めながら着地するような歩容となる。言い換えれば、本実施例での目標歩容では、ロボット１は、着地瞬間において、遊脚側の足平２２の対地速度が０またはほぼ０になるように、上体２４から見て該足平２２を引き上げるようにして着地する。これによって、着地衝撃は小さくなり、着地衝撃が過大になるのを防止するようにしている。
また、本実施形態では、有限時間整定フィルタは、単位フィルタを３段以上（例えば３段）、直列に接続したものであるため、着地予定時刻までに各足平２２の速度（足平位置の変化速度）が０またはほぼ０になるだけでなく、各足平２２は、その加速度も着地予定時刻にて０またはほぼ０になって停止する。つまり、着地瞬間における対地加速度も０またはほぼ０になる。したがって、着地衝撃がより一層小さくなる。特に、実際のロボット１の着地時刻が目標の着地時刻からずれても、衝撃があまり増大しなくなる。補足すると、着地予定時刻にて各足平２２の対地速度を０またはほぼ０にする上では、有限時間整定フィルタの単位フィルタの段数は２段でもよいが、この場合には、着地予定時刻での各足平２２の加速度は一般には０にならない。
尚、足平姿勢に関しては、各足平２２が着地予定時刻にてその踵で着地した後、該足平２２の底面のほぼ全面が床に接地するまで動き続ける。このため、該足平２２の底面のほぼ全面が床に接地する時刻を前記指定時刻に設定して、前記有限時間整定フィルタにより足平姿勢軌道が生成される。
また、本実施形態では、有限時間整定フィルタを用いて足平位置軌道を生成したが、着地予定時刻での足平位置の変化速度が０またはほぼ０になる（足平位置の時間微分値が０になる）ように、さらには、該着地予定時刻での足平位置の変化加速度（変化速度の時間微分値）が０またはほぼ０になるように設定された多項式などの関数を用いて目標足平位置軌道を生成するようにしても良い。このことは、目標足平姿勢軌道の生成に関しても同様である。但し、該目標足平姿勢軌道の生成に関しては、上述の如く、各足平２２の底面のほぼ全面が床に接地する時刻にて、各足平２２の姿勢の変化速度、さらにはその変変化加速度が０またはほぼ０になるように多項式などの関数が設定される。
歩容生成装置１００は目標床反力鉛直成分を明示的に設定する。この目標床反力鉛直成分軌道は、例えば図６のように設定される。本実施形態では、走行歩容における目標床反力鉛直成分軌道の形状（詳しくは片脚支持期での形状）は、台形状（床反力鉛直成分の増加側に凸の形状）に定められており、その台形の高さ、折れ点の時刻を目標床反力鉛直成分軌道を規定する歩容パラメータとして、それらの歩容パラメータ（床反力鉛直成分軌道パラメータ）が決定される。尚、走行歩容の空中期では、目標床反力鉛直成分は定常的に０に設定される。この例のように、目標床反力鉛直成分軌道は、実質的に連続になるように（値が不連続にならないように）設定するのが良い。これは床反力を制御する際のロボット１の関節の動作を滑らかにするためである。ここで、「実質的に連続」というのは、アナログ的に連続な軌道（真の意味での連続な軌道）を離散時間系でデジタル表現したときに必然的に生じる値の飛びは、該軌道の連続性を失わせるものではないということを意味するものである。
目標ＺＭＰ軌道は次のように設定される。図５の走行歩容においては、前記したように支持脚側足平２２のかかとで着地し、次にその支持脚側足平２２のつま先で蹴って空中に飛び上がり、最後に遊脚側足平２２のかかとで着地する。したがって、片脚支持期での目標ＺＭＰ軌道は、図７の上段図に示すように、支持脚側足平２２のかかとを初期位置として、次に支持脚側足平２２の底面のほぼ全面が接地する期間において該足平２２の前後方向の中央に移動し、その後、離床時までに支持脚側足平２２のつまさきに移動するように設定される。ここで、図７の上段図は、Ｘ軸方向（前後方向）の目標ＺＭＰ軌道を示すものであり、図７の下段図はＹ軸方向（左右方向）の目標ＺＭＰ軌道を示すものである。尚、片脚支持期におけるＹ軸方向の目標ＺＭＰ軌道は、図７の下段図に示すように、Ｙ軸方向での支持脚側脚体２の足首関節の中心位置と同じ位置に設定される。
図７の上段図に示すように、空中期における目標ＺＭＰ軌道のＸ軸方向位置は、次の遊脚側脚体２の着地までに支持脚側足平２２のつまさきから遊脚側足平２２のかかとの着地位置まで連続的に移動するようにした。また、図７の下段図に示すように、空中期における目標ＺＭＰ軌道のＹ軸方向位置は、次の遊脚側脚体２の着地までに支持脚側脚体２の足首関節の中心のＹ軸方向位置から遊脚側脚体２の足首関節の中心のＹ軸方向位置まで連続的に移動するようにした。すなわち、歩容の全期間において目標ＺＭＰ軌道を連続（実質的に連続）にした。ここで、上記したＺＭＰ軌道の「実質的に連続」の意味は、前記床反力鉛直成分軌道の場合と同様である。
なお、目標歩容の生成に用いる動力学モデルの近似精度等を考慮すると、目標歩容の目標運動をできるだけ連続的なものとする上で、目標ＺＭＰ軌道は上記のように空中期でも実質的に連続であることが望ましい。但し、本実施形態で用いる後述の動力学モデルでは、必ずしも目標ＺＭＰが連続的である必要はない。また、本実施形態では、図７に示したような目標ＺＭＰ軌道の折れ点の位置や時刻が、ＺＭＰ軌道パラメータ（目標ＺＭＰ軌道を規定するパラメータ）として設定される。
ＺＭＰ軌道パラメータは、安定余裕が高く、かつ急激な変化をしないように決定される。ここで、ロボット１の接地面を含む最小の凸多角形（いわゆる支持多角形）の中央付近に目標ＺＭＰが存在する状態を安定余裕が高いと言う（詳細は特開平１０−８６０８１号公報を参照）。図７の目標ＺＭＰ軌道はこのような条件を満たすように設定したものである。
また、目標腕姿勢は、上体２４に対する相対姿勢で表される。
また、目標上体位置姿勢、目標足平位置姿勢および後述の基準上体姿勢はグローバル座標系で記述される。グローバル座標系は床に固定された座標系であり、より具体的には、後述する支持脚座標系が用いられる。
本実施形態では、歩容生成装置１００は、目標上体姿勢だけでなく、基準上体姿勢も生成する。この基準上体姿勢は、歩容に対する要求（歩容生成装置１００に対する行動計画部などの装置または外部（前記ジョイスティック等）からの要求）にそのまま従って生成される上体姿勢である。
目標上体姿勢（以降、「基準」が付いていない場合には、目標上体姿勢を表す）は、長期的には基準上体姿勢に追従するかまたは一致するように生成される。
目標上体姿勢は、歩行においては、本願出願人による特願２０００−３５２０１１号の明細書に記載した実施形態のごとく通常、常に基準上体姿勢に一致させておけば良い。特願２０００−３５２０１１号では、基準上体姿勢という概念は記載されていないが、目標上体姿勢パターンを、明示的かつ優先的に与えていたので、基準上体姿勢に目標上体姿勢が常に一致していることと同じことである。
しかし、走行など空中期がある歩容や低摩擦床面での歩行においては、単に上体水平加速度を調整するだけでは、目標歩容の床反力水平成分が許容範囲以内（あるいは摩擦限界以内）に存在するようにしつつ動力学的平衡条件を満足することができない。
そこで、本実施形態においては、目標上体姿勢を必要に応じて基準上体姿勢から故意にずらすこととした。より具体的には、以下に説明するふたつの運動モードを複合的に発生させることにより、目標歩容の床反力水平成分が許容範囲以内（あるいは摩擦限界以内）に存在するようにした。
図８のように、ロボット１のある運動状態から、上体水平加速度だけを摂動（微小変化）させると、ロボット１の全体重心水平加速度と全体重心まわりの角運動量が摂動する。すなわち、上体水平加速度の摂動は、それによって発生するロボット１の慣性力と重力との合力に対して動力学的に釣り合う床反力鉛直成分を摂動させずに（ロボット１の全体重心鉛直加速度を摂動させずに）、目標ＺＭＰまわりの床反力モーメント成分（ただし鉛直軸まわり成分を除く）と床反力水平成分とを摂動させる。このようにロボット１の上体水平加速度を摂動させる運動モードを上体並進モードと呼ぶ。
言いかえると、床反力鉛直成分を変化させずに、目標ＺＭＰまわりの床反力モーメント成分（ただし鉛直軸まわり成分を除く）と床反力水平成分とを変化させる運動を上体並進モードと呼ぶ。
この時の単位加速度当たりの床反力モーメント成分の変化をΔＭｐ、単位加速度当たりの床反力水平成分の変化をΔＦｐとする。図８に示す状況で上体２４を前方に水平加速すると、ΔＭｐとΔＦｐは図８に示す矢印の向きに作用する。
感覚的に判り易くするために、運動によって発生する慣性力と重力との合力に釣り合う床反力を用いて表現したが、慣性力と重力との合力を用いて表現した方が、理論的には的確である。なお、上記合力と床反力とは、互いに大きさが同じで向きが逆になっている。
それに対し、図９のように、ロボット１のある運動状態から、ある点Ｐｒまわりに上体姿勢角加速度（上体２４の姿勢角の角加速度）を摂動させると、ロボット１の全体重心は摂動せずに、全体重心まわりの角運動量が摂動する。すなわち、点Ｐｒまわりの上体姿勢角加速度の摂動は、床反力鉛直成分と床反力水平成分とを摂動させずに、目標ＺＭＰまわりの床反力モーメント成分（鉛直軸まわり成分を除く）を摂動させる。このようにロボット１の上体姿勢角加速度を摂動させる運動モードを上体回転モードと呼ぶ。
言いかえると、床反力鉛直成分と床反力水平成分を変化させずに、目標ＺＭＰまわりの床反力モーメント成分（鉛直軸まわり成分を除く）を変化させる運動を上体回転モードと呼ぶ。
この時の単位角加速度当たりの床反力モーメント成分の変化をΔＭｒ、単位角加速度当たりの床反力水平成分の変化をΔＦｒとする。ΔＦｒは零である。図９に示す状況で上体２４が前傾するように上体姿勢角の角加速度を発生させると、ΔＭｒは図９に示す矢印の向きに作用する。
次に、本実施形態で歩容生成に用いられるロボット動力学モデルについて説明する。本実施形態では単純化された（近似された）以下に示す動力学モデルを用いる。ただし、以下に示す動力学モデルに関しては、キネマティクスモデル（関節、リンクの構造や寸法を表すモデルであり、言い換えると、関節変位とリンクの位置姿勢との関係を表すモデルである。）も必要である。
図１０は、本実施形態に用いられるロボット動力学モデルである。図示の如く、この動力学モデルは、ロボット１の各脚体２にそれぞれ対応する２つの質点２ｍ，２ｍ、及び上体２４に対応する質点２４ｍからなる合計３質点と、イナーシャがあって質量のないフライホイールＦＨとから構成されるモデルである。この動力学モデルでは、デカップルド、即ち、脚体２，２の動力学（各質点２ｍ，２ｍの動力学）と上体２４の動力学（質点２４ｍ及びフライホイールＦＨの動力学）とが相互に非干渉に構成されると共に、ロボット１全体の動力学は、それらの線形結合で表される。また、さらに、上体２４の運動と床反力との関係も、上体２４の並進運動（上体並進モード）と床反力との関係、並びに上体２４の回転運動（上体回転モード）と床反力との関係に分離される。具体的には、上体質点２４ｍの水平運動によって発生する床反力は、上体２４の水平方向並進運動（上体並進モード）によって発生する床反力に対応し、フライホイールの回転運動によって発生する床反力は、上体２４の姿勢角の回転運動（上体回転モード）によって発生する床反力に対応する。
尚、ロボット１の腕体の質量は上体２４に含まれるものとし、上体質点２４ｍは腕体の質量を含む質量をもつ。本実施形態では、目標歩容における腕体の運動（腕振り運動）は、後述するように、ロボット１の腕振り以外の運動によってロボット１に発生する鉛直軸まわりの慣性力のモーメントを打ち消しつつも、上体２４に対する両腕体の全体の重心の相対位置が動かないように行なわれるので、腕体の腕振り運動による（鉛直軸まわり成分を除く）床反力モーメントへの影響と床反力水平成分への影響とは無視する。
以下に上記単純化モデルを数式化する。ただし、説明を簡単にするために、サジタルプレーン（前後軸（Ｘ軸）と鉛直軸（Ｚ軸）を含む平面）での運動方程式のみを記述し、ラテラルプレーン（左右軸（Ｙ軸）と鉛直軸（Ｚ軸）を含む平面）での運動方程式を省略した。
説明の便宜上、動力学モデルに関する変数およびパラメータを以下のように定義する。なお、各質点２ｍ，２ｍ，２４ｍはそれが対応する部位の代表点であるか、あるいはその部位の位置姿勢から幾何学的に一義的に決定される点に相当する。例えば、支持脚側脚体２の質点２ｍの位置は、該脚体２の足平２２の底面の前記代表点から所定の距離だけ上方の点とする。
Ｚｓｕｐ：支持脚質点鉛直位置
Ｚｓｗｇ：遊脚質点鉛直位置
Ｚｂ：上体質点鉛直位置（上体鉛直位置とは一般的には異なる。）
ＺＧｔｏｔａｌ：全体重心鉛直位置
Ｘｓｕｐ：支持脚質点水平位置
Ｘｓｗｇ：遊脚質点水平位置
Ｘｂ：上体質点水平位置（前記点Ｐｒから前後にある所定の距離だけオフセットした点である。オフセットは、直立時などにおいて、厳密モデルの重心位置と、本動力学モデルの重心位置がなるべく一致するように、決定される。上体水平位置とは一般的には異なる。）
ＸＧｔｏｔａｌ：全体重心水平位置
θｂｙ：鉛直方向に対するＹ軸回りの上体姿勢角（傾斜角）
ｍｂ：上体質点質量
ｍｓｕｐ：支持脚質点質量
ｍｓｗｇ：遊脚質点質量
ｍｔｏｔａｌ：ロボット総質量（＝ｍｔｏｔａｌ＋ｍｓｕｐ＋ｍｓｗｇ）
Ｊ：上体慣性モーメント（上体回転モードにおける等価慣性モーメントである。実ロボット１の上体２４部位の慣性モーメントとは一般的には一致しない。）
Ｆｘ：床反力水平成分（詳しくは並進床反力の前後方向（Ｘ軸）成分）
Ｆｚ：床反力鉛直成分（詳しくは並進床反力の鉛直方向（Ｚ軸）成分。
本実施形態では、これは目標並進床反力鉛直成分に等しい）
Ｍｙ：目標ＺＭＰまわりの床反力モーメント（詳しくは床反力モーメントの左右軸（Ｙ軸）まわり成分）
尚、本実施形態では、各脚体２の質点２ｍの位置と、その脚体２の足平２２の位置（足平２２のあらかじめ定めた代表点の位置）との位置関係はあらかじめ定められており、一方の位置が決まれば、他方の位置は一義的に定まるものとされている。また、上体質点２４ｍと、上体２４の位置（上体２４のあらかじめ定められた代表点の位置）との位置関係は、上体２４の姿勢角に応じてあらかじめ定められており、一方の位置と姿勢角とが決まれば、他方の位置は一義的に定まるものとされている。
また、任意の変数Ｘに対して、ｄＸ／ｄｔはＸの１階微分を表わし、ｄ２Ｘ／ｄｔ２はＸの２階微分を表わす。したがって、変数Ｘが変位ならば、ｄＸ／ｄｔは速度、ｄ２Ｘ／ｄｔ２は加速度を意味する。ｇは重力加速度定数を示す。ここでは、ｇは正の値とする。
上記動力学モデルの運動方程式（動力学的平衡条件を表す式）は、式０１、式０２、式０３で表される。
2003061917.tif 000006
また、ロボット全体重心位置には次の関係式が成立する。
2003061917.tif 000007
以下に上記動力学モデルと前記ΔＦｐ、ΔＭｐ、ΔＦｒ、ΔＭｒの関係を示す。
前記ΔＦｐは、式０２において、ｄ２Ｘｂ／ｄｔ２を単位量だけ摂動させたときのＦｘの摂動量であるから、次式によって求められる。
2003061917.tif 000008
すなわち、上体並進モードの単位加速度当たりの床反力の水平成分の変化ΔＦｐは、上記動力学モデルの上体質点２４ｍの質量に相当する。
前記ΔＭｐは、式０３において、ｄ２Ｘｂ／ｄｔ２を単位量だけ摂動させたときのＭｙの摂動量であるから、次式によって求められる。
2003061917.tif 000009
すなわち、上体並進モードの単位加速度当たりの床反力モーメント成分の変化ΔＭｐは、上記動力学モデルの上体質点質量に、上体質点２４ｍの目標ＺＭＰからの高さ（鉛直位置）を乗じたものになる。このときの上体質点２４ｍ及び目標ＺＭＰの位置と、上体質点２４ｍの運動との関係は、上体質点２４ｍを倒立振子質点、目標ＺＭＰを倒立振子支点に対応づけたときの倒立振子の挙動に相当する。
前記ΔＦｒは、式０２において、ｄ２θｂｙ／ｄｔ２を単位量だけ摂動させたときのＦｘの摂動量であるから、次式によって求められる。
2003061917.tif 000010
すなわち、上体回転モードの単位加速度当たりの並進床反力水平成分の変化ΔＦｒは、０である。
前記ΔＭｒは、式０３において、ｄ２θｂｙ／ｄｔ２を単位量だけ摂動させたときのＭｙの摂動量であるから、次式によって求められる。
2003061917.tif 000011
すなわち、上体回転モードの単位加速度当たりの床反力モーメント成分の変化ΔＭｒは、フライホイールの慣性モーメントに相当する。
本実施形態における歩容生成装置１００は、ロボット１の片方の脚体２が着地してから他方の脚体２が着地するまでの１歩分の目標歩容を単位として、その１歩分の目標歩容を順番に生成する。従って、本実施形態で生成する図５の走行歩容では、該目標歩容は片脚支持期の開始時から、これに続く空中期の終了時（次の片脚支持期の開始時）までの目標歩容が順番に生成される。ここで、本明細書では、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してからもう一方の脚体２が着地するまでの意味で使用する。また、新たに生成しようとしている目標歩容を「今回歩容」、その次の目標歩容を「次回歩容」、さらにその次の目標歩容を「次次回歩容」、というように呼ぶ。また、「今回歩容」の一つ前に生成した目標歩容を「前回歩容」と呼ぶ。
また、歩容生成装置１００が今回歩容を新たに生成するとき、該歩容生成装置１００には、ロボット１の２歩先までの遊脚側足平２２の着地予定位置姿勢、着地予定時刻の要求値（要求）が歩容に対する要求パラメータとして入力される（あるいは歩容生成装置１００が記憶装置から要求パラメータを読み込む）。そして、歩容生成装置１００は、これらの要求パラメータを用いて、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標ＺＭＰ軌道、目標床反力鉛直成分軌道、目標腕姿勢軌道等を生成する。このとき、これらの軌道を規定する歩容パラメータの一部は、歩行の継続性を確保するように適宜修正される。
以下に図５の走行歩容を生成することを主要例にして、歩容生成装置１００の歩容生成処理の詳細を図１２〜図４１を参照しつつ説明する。図１２は、その歩容生成装置１００が実行する歩容生成処理を示すフローチャート（構造化フローチャート）である。
まずＳ０１０において時刻ｔを０に初期化するなど種々の初期化作業が行なわれる。この処理は、歩容生成装置１００の起動時等に行なわれる。次いで、Ｓ０１２を経てＳ０１４に進み、歩容生成装置１００は、制御周期（図１２のフローチャートの演算処理周期）毎のタイマ割り込みを待つ。制御周期はΔｔである。
次いで、Ｓ０１６に進み、歩容の切り替わり目であるか否かが判断され、歩容の切り替わり目であるときはＳ０１８に進むと共に、切り替わり目でないときはＳ０３０に進む。ここで、上記「歩容の切り替わり目」は、前回歩容の生成が完了し、今回歩容の生成を開始するタイミングを意味し、例えば前回歩容の生成を完了した制御周期の次の制御周期が歩容の切り替わり目になる。
Ｓ０１８に進むときは時刻ｔが０に初期化され、次いでＳ０２０に進み、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期が読み込まれる。これらの支持脚座標系及び歩容周期は、前記要求パラメータにより定まるものである。すなわち、本実施形態では、歩容生成装置１００にジョイスティック等から与えられる要求パラメータは、２歩先までの遊脚側足平２２の着地予定位置姿勢（足平２２が着地してから足底を床面にほぼ全面的に接触させるように、滑らさずに回転させた状態での足平位置姿勢）、着地予定時刻の要求値を含んでおり、その１歩目の要求値、２歩目の要求値がそれぞれ、今回歩容、次回歩容に対応するものとして、今回歩容の生成開始時（前記Ｓ０１６の歩容の切り替わり目）以前に歩容生成装置１００に与えられたものである。なお、これらの要求値は今回歩容の生成途中でも変更することは可能である。
そして、上記要求パラメータにおける１歩目の遊脚側足平２２（今回歩容での遊脚側足平２２）の着地予定位置姿勢の要求値に対応して次回歩容支持脚座標系が定まる。
例えば図１５を参照して、今回歩容（１歩目）に係わる遊脚側足平２２（図では２２Ｌ）の着地予定位置姿勢の要求値が、今回歩容の支持脚側足平２２（図では２２Ｒ）の着地位置姿勢に対して、今回歩容支持脚座標系のＸ軸方向（今回歩容の支持脚側足平２２Ｒの前後方向）及びＹ軸方向（今回歩容の支持脚側足平２２Ｒの左右方向）に、それぞれｘｎｅｘｔ、ｙｎｅｘｔだけ移動し、且つＺ軸回り（鉛直軸回り）にθｚｎｅｘｔだけ回転させた位置姿勢であるとする。ここで、支持脚座標系は、支持脚側足平２２を水平姿勢（より一般的には床面に平行な姿勢）にして該支持脚側足平２２の底面のほぼ全面を床面に接触（密着）させた状態において、該支持脚側脚体２の足首中心から床面に延ばした垂線が該床面と交わる点（この点は、本実施形態の例では支持脚側足平２２の底面のほぼ全面を床面に接触させた状態では、該足平２２の代表点と合致する）を原点とし、その原点を通る水平面をＸＹ平面とする、床に固定された座標系である。この場合、Ｘ軸方向、Ｙ軸方向は、それぞれ支持脚側足平２２の前後方向、左右方向である。尚、支持脚座標系の原点は、必ずしも支持脚側足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の代表点（足平２２の位置を代表する点）に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。
このとき、次回歩容支持脚座標系は、図示のように今回歩容の遊脚側足平２２Ｌの着地予定位置姿勢の要求値に従って該足平２２を着地させた場合（足平２２の代表点を着地予定位置の要求値に一致させ、且つ、足平２２の姿勢（向き）を着地予定姿勢の要求値に一致させた場合）における該足平２２Ｌの代表点（より詳しくは該代表点に合致する床上の点）を原点とし、該原点を通る水平面内における該足平２２Ｌの前後方向、左右方向をそれぞれＸ’軸方向、Ｙ’軸方向とする座標系である。
上記と同様に、２歩目の遊脚側足平２２の着地予定位置姿勢の要求値に応じて次々回歩容支持脚座標系（図１５のＸ”Ｙ”座標を参照）が定まる。また、今回歩容周期は、今回歩容の支持脚側足平２２の着地予定時刻（要求値）から、１歩目（今回歩容）の遊脚側足平２２の着地予定時刻（要求値）までの時間として定まり、次回歩容周期は、１歩目の遊脚側足平２２の着地予定時刻（要求値）から２歩目の遊脚側足平２２の着地予定時刻（要求値）までの時間として定まる。
尚、前記要求パラメータは、本実施形態ではジョイスティックの所要の操作によって歩容生成装置１００に入力されるものとしたが、あらかじめ該要求パラメータあるいはこれに対応する上記の支持脚座標系の位置姿勢及び歩容周期をロボット１の移動スケジュールとして記憶しておいても良い。あるいは、ジョイスティックなどの操縦装置からの指令（要求）とそのときまでのロボットの１の移動履歴とを基に前記次回及び次次回歩容支持脚座標系、並びに今回及び次回歩容周期を決定しても良い。
次いでＳ０２２に進み、歩容生成装置１００は、今回歩容に続く仮想的な周期的歩容としての定常旋回歩容の歩容パラメータを決定する。該歩容パラメータは、定常旋回歩容における目標足平位置姿勢軌道を規定する足平軌道パラメータ、基準とする上体姿勢軌道を規定する基準上体姿勢軌道パラメータ、目標腕姿勢軌道を規定する腕軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータを含む。さらに、目標床反力水平成分許容範囲を規定するパラメータも歩容パラメータに含まれる。
尚、この明細書で「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界（本実施形態では１歩毎の歩容の境界）においてロボット１の運動状態（足平位置姿勢、上体位置姿勢等の状態）に不連続が生じないような周期的歩容を意味するものとして使用する。以降、「定常旋回歩容」を「定常歩容」と略す場合もある。
周期的歩容である定常旋回歩容は、本実施形態では、ロボット１の２歩分の歩容、すなわち今回歩容に続く第１旋回歩容と該第１旋回歩容に続く第２旋回歩容とからなる歩容を該定常旋回歩容の１周期分の歩容として、その１周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。また、生成する目標歩容が前記図５の走行歩容であると、その目標歩容の今回歩容は、片脚支持期及び空中期を有する走行歩容であるので、定常旋回歩容の第１旋回歩容及び第２旋回歩容は、いずれも今回歩容と同様に片脚支持期と空中期とを有する歩容である。つまり、第１旋回歩容及び第２旋回歩容の基本的な歩容形態は今回歩容と同一である。
定常旋回歩容について補足すると、２足移動ロボットでは、定常旋回歩容の１周期分は、前記した狭義の意味での歩容が、少なくとも２歩分、必要である。そして、３歩以上の歩容を１周期分の歩容とする複雑な定常旋回歩容を設定することも可能である。但し、定常旋回歩容は、後述の如く、今回歩容の終端（終了時刻）における発散成分（詳細は後述する）を決定するためだけに用いられる。このため、３歩以上の歩容を１周期とする定常旋回歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本実施形態での定常旋回歩容の１周期分の歩容を２歩分の歩容（第１及び第２旋回歩容）により構成するようにしている。尚、３足以上の脚式移動ロボットにあっては、それに応じて定常旋回歩容を定義するに足る歩容数が増加する。以下では、説明の便宜上、複数の狭義の歩容（本実施形態では２歩分の歩容）から成る定常旋回歩容を１歩の歩容とみなす。
定常旋回歩容は、歩容生成装置１００で今回歩容の終端における発散成分や上体鉛直位置速度、上体姿勢角及びその角速度等のロボット１の運動状態を決定するために暫定的に作成されるものであり、歩容生成装置１００からそのまま出力されるものではない。
尚、「発散」とは、図１３に示すように、２足移動ロボット１の上体２４の位置が両足平２２，２２の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、２足移動ロボット１の上体２４の位置が両足平２２，２２の位置（より具体的には、支持脚側足平２２の接地面に設定されたグローバル座標系（支持脚座標系）の原点）からかけ離れていく具合を表す数値である。
本実施形態では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成するようにした。ただし、継続的な歩容の代表例である定常歩容（歩容の軌道の不連続を発生することなく、同じパターンの歩容を繰り返すことができる周期的歩容であり、原理上、無限回繰り返しても発散しない歩容）の初期発散成分（定常歩容の初期時刻での発散成分）であっても、単純に０ではなく、定常歩容のパラメータが変われば、その初期発散成分も変わる。すなわち、歩き方あるいは走り方等の歩容形態によって、適切な発散成分が変わる。そこで、本実施形態では、これから生成しようとする今回歩容の後に続く定常歩容が前記今回歩容に係わる要求パラメータに応じて設定され、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる（より一般的には、今回歩容を定常歩容に連続させもしくは近づける）ように、今回歩容を生成する。これのような歩容生成の基本的な指針は本出願人が先に提案した特願２０００−３５２０１１号のものと同様である。
本発明の実施形態では、特願２０００−３５２０１１号の第１実施形態で用いたような線形な３質点動力学モデルを用いていないが、次式で定義される発散成分および収束成分の概念は、図１０に示したモデルのような非線形な動力学モデルの挙動の摂動分に対しても、近似的に十分な精度で適用することができる。
2003061917.tif 000012
ただし、ここでの上体質点水平位置は、図１０に示す前記動力学モデルにおいては、上体質点水平位置Ｘｂを表す。
また、ω０はある所定の値であり、走行時では、特願２０００−３５２０１１号の歩行歩容生成時の値に対して若干変更する必要がある。
発散成分および収束成分に関するその他の詳細は、特願２０００−３５２０１１号に記述されているので、ここではこれ以上言及しない。
本実施形態においては、特願２０００−３５２０１１号に示した方式に加え、さらに、後述するように、目標床反力鉛直成分軌道を規定する歩容パラメータを設定し、この目標床反力鉛直成分を動力学的に満足するようにロボット１の全体重心鉛直位置を決定する。この場合、床反力鉛直成分の２階積分値がロボット１の全体重心鉛直位置を規定するものになるので、目標床反力鉛直成分の設定が悪いと、ロボット１の全体重心鉛直位置もしくは上体鉛直位置が高くなり過ぎたり低くなり過ぎたりする。したがって、目標床反力鉛直成分の設定方法も重要な課題である。しかし、床反力鉛直成分と上体鉛直位置との関係は、ＺＭＰと上体水平位置との関係に似ているので、上体水平位置速度を適切にするための目標ＺＭＰ決定手法の一部を、以下の本実施形態に示すように、少し変更するだけで、上体鉛直位置速度を適切にするための目標床反力鉛直成分の決定手法に適用することができる。
本題に戻り、Ｓ０２２では、図１４に示すフローチャートに従って、以下の処理が行われる。
まず、Ｓ１００において、今回歩容、第１旋回歩容、第２旋回歩容の順に足平位置姿勢軌道がつながるように、定常歩容の歩容パラメータのうちの足平軌道パラメータが決定される。以下に具体的な設定方法を図１５を参照して説明する。尚、以降の説明では、支持脚側の脚体２の足平２２を支持脚足平と称し、遊脚側の脚体２の足平２を遊脚足平を称する。また、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。
足平軌道パラメータは、第１旋回歩容および第２旋回歩容の初期及び終端のそれぞれにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成される。この足平軌道パラメータのうち、第１旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。この場合、走行歩容では、今回歩容終端における支持脚足平２２は、空中に移動している。そして、今回歩容終端支持脚足平位置姿勢は、今回歩容初期支持脚足平位置姿勢（＝前回歩容終端遊脚足平位置姿勢）から、前記要求パラメータにおける２歩目の遊脚足平２２の着地予定位置姿勢の要求値（今回歩容の支持脚足平２２の次回歩容における着地予定位置姿勢の要求値）もしくは該要求値に対応する次次回歩容支持脚座標系に応じて定まる次回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道（詳しくは次回歩容支持脚座標系から見た軌道）を、今回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
尚、次回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平２２を接地させたまま、つま先を下げるように該足平２２を水平姿勢までピッチ方向に所定角度回転させたときの該足平の位置姿勢が次次回歩容支持脚座標系の位置姿勢に合致するように決定される。換言すれば、次回歩容終端遊脚足平位置姿勢は、前記要求パラメータにおける２歩目の遊脚足平２２の着地位置姿勢の要求値から、該足平２２をすべらないように接地させたまま、つま先を持ち上げるように該足平２２をピッチ方向に所定角度回転させた状態（つま先を上げてかかとを着地させた状態）での該足平２２の位置姿勢である。
また、第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。この場合、今回歩容終端遊脚足平位置姿勢は、前記次回歩容終端遊脚足平位置姿勢の場合と同様、前記次回歩容支持脚座標系もしくはこれに対応する前記要求パラメータの１歩目（今回歩容）の遊脚着地予定位置姿勢の要求値に応じて定まるものである。すなわち、該今回歩容終端遊脚足平位置姿勢は、その位置姿勢から、足平２２を接地させたまま、つま先を下げるように該足平２２を回転させて該足平２２の底面のほぼ全面を床面に接地させたときの該足平の代表点が次回歩容支持脚座標系の原点に合致するように決定される。
第１旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢や次回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に基づいて決定される。より具体的には、第１旋回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平２２を接地させたまま、すべらないように該足平２２を水平姿勢まで所定角度回転させたときの足平位置姿勢が、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に合致するように設定される。
第１旋回歩容終端では、支持脚足平２２は離床して空中にある。支持脚足平２２が離床してから後の軌道を決定するために、第１旋回歩容支持脚足平着地予定位置姿勢が設定される。第１旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢に基づいて設定される。より具体的には、第１旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢である。なお、次次次回歩容支持脚座標系は、次次回歩容支持脚座標系と次次次回歩容支持脚座標系との相対的位置姿勢関係が、今回歩容支持脚座標系と次回歩容支持脚座標系との相対的位置姿勢関係と一致するように設定される。
第１旋回歩容終端支持脚足平位置姿勢は、第１旋回歩容初期支持脚足平位置姿勢を求めた場合と同様に、第１旋回歩容初期支持脚足平位置姿勢から、前記第１旋回歩容支持脚足平着地予定位置姿勢に至る足平位置姿勢軌道（より詳しくは次回歩容支持脚座標系から見た軌道）を、第１旋回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
第２旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端支持脚足平位置姿勢とされる。第２旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端遊脚足平位置姿勢とされる。
第２旋回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。第２旋回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。
第１旋回歩容および第２旋回歩容の歩容周期は、次回歩容周期と同一に設定される。これらの第１旋回歩容及び第２旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第１旋回歩容および第２旋回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む）は、上記決定されたパラメータに応じて、歩容の条件（アクチュエータの速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど）を満足するように適宜決定する。
次に、Ｓ１０２に進み、目標上体姿勢が追従すべき基準上体姿勢軌道を規定する基準上体姿勢軌道パラメータが決定される。基準上体姿勢は、定常歩容の初期（第１旋回歩容の初期）と終端（第２旋回歩容の終端）とでつながるように（定常歩容の初期、終端での基準上体姿勢の姿勢角及びその角速度が一致するように）に設定されている限り、一定姿勢である必要はないが、本実施形態では理解を容易にするため、基準上体姿勢は、直立姿勢（鉛直姿勢）に設定される。つまり、本実施形態では、基準上体姿勢は、定常歩容の全期間において直立姿勢に設定される。従って、本実施形態では、基準上体姿勢の姿勢角の角速度及び角加速度は０である。
次に、Ｓ１０４に進み、腕姿勢軌道パラメータ、より詳しくは鉛直軸（あるいは上体体幹軸）まわりの両腕体の角運動量変化に関すること以外の腕姿勢軌道パラメータが決定される。たとえば、上体２４に対する腕体の手先の相対高さや腕全体の相対重心位置などの腕姿勢軌道パラメータが決定される。尚、本実施形態では、腕全体の相対重心位置は、上体に対して一定に維持されるように設定される。
次に、Ｓ１０６に進み、床反力鉛直成分軌道パラメータが設定される。この場合、該パラメータにより規定される床反力鉛直成分軌道が、第１旋回歩容および第２旋回歩容のいずれにおいても図６のように実質的に連続なものとなるように床反力鉛直成分軌道パラメータが設定される。すなわち、定常旋回歩容の目標床反力鉛直成分軌道は図１６のようなパターンで設定される。そのパターンでは、第１旋回歩容及び第２旋回歩容のいずれにおいても、片脚支持期では床反力鉛直成分が台形状に変化し、空中期では床反力鉛直成分が０に維持される。そして、このパターンの折れ点の時刻や、台形部分の高さ（ピーク値）が床反力鉛直成分軌道パラメータとして設定される。
この床反力鉛直成分軌道パラメータの設定に際しては、床反力鉛直成分の歩容全期間（第１旋回歩容と第２旋回歩容両方の期間を合わせた期間で定常歩容の１周期の期間）における平均値をロボット１の自重と一致させる。すなわち、床反力鉛直成分の平均値がロボット１に作用する重力と同じ大きさで反対向きになるようにする。
上記のごとく床反力鉛直成分軌道を設定することは、定常歩容の条件を満たすために必要である。定常歩容の条件とは、支持脚座標系（支持脚側足平２２の接地面に設定された座標系）から見た歩容のあらゆる状態変数（ロボット１の各部の位置、姿勢、速度等）の初期状態（第１旋回歩容の初期状態）と次の支持脚座標系（次の第１旋回歩容の支持脚座標系）から見た歩容の終端状態（第２旋回歩容の終端状態）が一致していることである（以下、この条件を定常歩容の境界条件ということがある）。したがって、定常歩容終端におけるロボット１の全体重心鉛直速度と定常歩容初期における全体重心鉛直速度との差（詳細には、第２旋回歩容終端の全体重心鉛直速度と第１旋回歩容初期の全体重心鉛直速度との差）も零でなければならない。上記差は床反力鉛直成分と重力との差の積分値（１階積分値）であるから、上記差を零にするためには、上記のごとく床反力鉛直成分軌道を設定する必要がある。
尚、本実施形態では、床反力鉛直成分の第１旋回歩容、第２旋回歩容それぞれの期間における平均値をロボット１の自重と一致させるようにした。より具体的には、例えば第１旋回歩容及び第２旋回歩容の歩容周期に応じて各旋回歩容における床反力鉛直成分軌道の台形部分の折れ点の時刻を設定した上で、その台形部分の高さを、床反力鉛直成分の第１旋回歩容、第２旋回歩容それぞれの期間における平均値がロボット１の自重と一致するように決定した（台形の高さを未知数として、上記平均値及び自重の一致条件を表す方程式を解くことで台形の高さを求める）。
こうすることにより、第１旋回歩容終端の全体重心鉛直速度と第１旋回歩容初期の全体重心鉛直速度との差も０、第２旋回歩容終端の全体重心鉛直速度と第２旋回歩容初期の全体重心鉛直速度との差も０になる。ただし、このようにする必然性はない。例えば、第１旋回歩容と第２旋回歩容との境界あたりで上体鉛直位置が高くなり過ぎたり低くなり過ぎたりして無理な姿勢になりそうな場合には、各旋回歩容で上記平均値及び自重を一致させた状態から各旋回歩容の床反力鉛直成分軌道の台形の高さ等を修正するようにしてもよい。
次に、Ｓ１０８に進み、上記のごとく図１６のように設定された床反力鉛直成分軌道に応じて、床反力水平成分の許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］（より詳しくはこれを規定するパラメータ）が図１７のように設定される。図１７の負側の折れ線が床反力水平成分許容下限値Ｆｘｍｉｎ、正側の折れ線が床反力水平成分許容上限値Ｆｘｍａｘを表す。これらの設定方法に関して以下に補足する。以下では、床面が水平である場合について説明する。
床反力水平成分は、床と足平２２との間の摩擦によって発生するが、摩擦はいくらでも発生できるわけではなく、限界がある。したがって、生成された目標歩容に従って実際のロボット１が移動した時にスリップしないようにするためには、目標歩容の床反力水平成分が、常に摩擦限界以内になければならない。そこで、この条件を満足させるために、床反力水平成分許容範囲を設定し、後述するように、目標歩容の床反力水平成分がこの許容範囲内になるように、目標歩容を生成することとした。
床と足平２２との間の摩擦係数をμとすると、Ｆｘｍｉｎは、常に−μ＊床反力鉛直成分以上に、Ｆｘｍａｘはμ＊床反力鉛直成分以下に設定されなければならない。最も単純な設定方法は、次式による設定である。ただし、ｋａは１より小さい正の定数である。
2003061917.tif 000013
図１７の床反力水平成分許容範囲は、式１２にしたがって設定した例である。床反力水平成分許容範囲を規定するパラメータとして、図１７の台形波形などの折れ点での値と時刻を設定してもよいが、式１２により床反力水平成分許容範囲を決定するときには、単に式１２における（ｋａ＊μ）の値をパラメータとして設定するだけでも良い。
尚、上記条件（目標歩容の床反力水平成分を常に摩擦限界以内にするという条件）を満足しているならば、他の設定方法で床反力水平成分許容範囲を設定するようにしても構わない。また、Ｓ１０８で設定する床反力水平成分許容範囲は、今回歩容の終端条件を決定するための定常歩容を生成するために用いるものであるため、必ずしも厳密な摩擦限界に基づいて決定する必要はない。例えば片脚支持期の床反力水平成分許容範囲を摩擦限界を越えるような範囲に設定してもよい。このことは空中期でも同様である。
次に、Ｓ１１０に進み、第１旋回歩容および第２旋回歩容を合わせた定常歩容のＺＭＰ軌道を規定するＺＭＰ軌道パラメータが設定される。この場合、目標ＺＭＰ軌道は、前述したように安定余裕が高くかつ急激な変化をしないように設定される。
さらに詳細には、図５の走行歩容においては、支持脚足平２２のかかとで着地した後、しばらくして該支持脚足平２２の底面のほぼ全面が接地し、さらにしばらくして、支持脚足平２２のつま先だけが接地する。そして、次に支持脚足平２２のつま先で蹴って空中に飛び上がり、最後に遊脚足平２２のかかとで着地する。また、目標ＺＭＰは接地面内に存在しなければならない。そこで、本実施形態では、定常歩容の第１旋回歩容及び第２旋回歩容のそれぞれの目標ＺＭＰのＸ軸方向の位置は、前記図７の上段図に示したように、支持脚足平２２のかかとを初期位置として、該足平２２の底面のほぼ全体が接地するまでその位置に留まるように設定される。次に、目標ＺＭＰは支持脚足平２２の中央に移動し、該足平２２がつま先接地状態になるまでにつま先に移動し、その後、離床時まで支持脚足平２２のつまさきに留まるように設定される。さらにその後は、目標ＺＭＰは、前述のごとく、次の遊脚足平２２の着地までに目標ＺＭＰが、支持脚足平２２のつまさきから遊脚足平２２のかかとの着地位置まで連続的に移動するように設定される。したがって、第１旋回歩容と第２旋回歩容とから成る定常歩容の目標ＺＭＰ軌道（Ｘ軸方向の軌道）は、図１８のようになる。そして、この目標ＺＭＰ軌道の折れ点の時刻及び位置がＺＭＰ軌道パラメータとして設定される。この場合、折れ点の時刻は、前記要求パラメータに応じて定めた第１旋回歩容及び第２旋回歩容の歩容周期に応じて設定され、該折れ点の位置は、次回歩容支持脚座標系及び次次回歩容支持脚座標系の位置姿勢あるいはこれらの座標系を規定する要求パラメータの１歩目及び２歩目の遊脚側足平着地予定位置姿勢の要求値に応じて設定される。尚、ＺＭＰ軌道のＹ軸方向の位置は、前記図７の下段図に示したものと同様に設定される。より詳しくは、第１旋回歩容における目標ＺＭＰのＹ軸方向位置の軌道は、図７の下段図のものと同一パターンで設定され、第２旋回歩容における目標ＺＭＰのＹ軸方向位置の軌道は、第１旋回歩容のものと同じ形の軌道で、該軌道の終端につながるものに設定される。
最後に、Ｓ１１２に進み、定常歩容１歩（１周期）の初期時刻、終端時刻、期間が以下のように再定義される。
定常歩容は、その初期と終端での状態変数が連続的につながる歩容でなければならない。このような歩容の決定を容易にするために、本実施形態では、定常歩容を決定するまでは、前述の狭義の歩容の定義とは異なり、定常歩容の１歩の初期、終端、期間を便宜上、図１６のように定める。すなわち、第１旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻を定常歩容の初期時刻Ｔｓとして設定する。なお、初期時刻Ｔｓは、図７に示すように、支持脚足平２２の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前の時刻（図７の足底全面接地期間の終了時刻もしくはその直前の時刻）に設定するのが望ましい。また、Ｓ１１０において設定した図１８（または図７）に示す目標ＺＭＰと時刻Ｔｓとの関係を説明すると、第１旋回歩容において支持脚足平２２の底面のほぼ全面が接地した後に、目標ＺＭＰが支持脚足平２２の中央に移動し、つま先接地状態になるまでにつま先に移動を完了した瞬間が、時刻Ｔｓとなっているのが望ましい。このような初期時刻Ｔｓは例えば先に設定した目標ＺＭＰ軌道パラメータに応じて設定される。上記のごとく初期時刻Ｔｓを設定する理由は後述する。
また、図１６に示すように、定常歩容の周期Ｔｃｙｃは第１旋回歩容と第２旋回歩容の歩容周期の和である。定常歩容の終端時刻をＴｅとする。Ｔｅは、ＴｓにＴｃｙｃを加えた時刻に設定される。
定常歩容が決定された時点（後述の図１９のＳ２０４のループを抜けた時点）から、改めて、歩容の初期、終端等の定義を、前述の狭義の歩容の定義に戻すこととする。以下の説明では、前述の狭義の歩容の定義による初期時刻（支持脚足平２２の着地当初の時刻）を０とし、定常歩容を決定するまで用いる上記初期時刻Ｔｓを参照符号Ｔｓを用いて（単に「Ｔｓ」と省略することもある）本来の初期時刻０と区別する。
図１２のＳ０１０からＳ０２２までに示す処理が行われた後、Ｓ０２４に進み、定常歩容の初期状態が算出される。ここで算出される初期状態は、定常歩容の初期上体水平位置速度（水平方向での初期上体位置及び初期上体速度）、初期上体鉛直位置速度（鉛直方向での初期上体位置及び初期上体速度）、初期発散成分、初期上体姿勢角およびその角速度である。この初期状態の算出は、図１９のフローチャートに従って、探索的に行なわれる。
図１９フローチャートにおいては、先ず、Ｓ２００において、定常歩容の歩容パラメータ（前記図１２のＳ０２２で設定したパラメータ）に基づいて、目標足平位置姿勢、目標腕姿勢、および目標上体姿勢角（姿勢角と方向）の初期状態（初期時刻Ｔｓでの状態）が決定される。ここで状態とは、位置や姿勢角とその変化率（時間微分）とを表す。
この場合、支持脚側の目標足平位置姿勢の初期状態は、前記図１４のＳ１００で決定した足平軌道パラメータの第１旋回歩容初期支持脚足平位置姿勢から、第２旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道（次回歩容支持脚座標系から見た軌道）を時刻Ｔｓまで有限時間整定フィルタにより生成することにより決定される。遊脚側の目標足平位置姿勢の初期状態は、次回歩容支持脚座標系から見た今回歩容初期支持脚足平位置姿勢から、第１旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道を時刻Ｔｓまで有限時間整定フィルタにより生成することにより決定される。また、目標腕姿勢の初期状態は、図１４のＳ１０４で決定した腕姿勢軌道パラメータに基づいて時刻Ｔｓにおける腕姿勢（上体２４に対する両腕体の全体重心位置等）を求めることで決定される。また、目標上体姿勢角の初期状態は、図１４のＳ１０２で決定した基準上体姿勢軌道パラメータにより定まる時刻Ｔｓにおける基準上体姿勢（姿勢角）及びその角速度が該目標上体姿勢角の初期状態として決定される。なお、本実施形態では、基準状態姿勢角は鉛直姿勢であるので、目標上体姿勢角の初期状態（姿勢角及びその角速度）は０である。また、本実施形態では、定常歩容の目標足平位置姿勢軌道、床反力鉛直成分軌道、及び目標ＺＭＰ軌道は、それぞれ、図１４のフローチャートで決定した足平軌道パラメータ、床反力鉛直成分軌道パラメータ、ＺＭＰ軌道パラメータにより互いに独立的に決定される。例えば定常歩容の瞬時瞬時の目標足平位置姿勢は、床反力鉛直成分の瞬時値に依存することなく足平軌道パラメータに応じて決定される。
次いでＳ２０２において初期上体水平位置速度の候補（すなわち初期時刻Ｔｓでの上体水平位置速度候補）である（Ｘｓ，Ｖｘｓ）（Ｘｓ：水平位置、Ｖｘｓ：水平速度）が仮決めされる。ここで仮決めする候補（Ｘｓ，Ｖｘｓ）は、任意でよいが、例えば前回歩容の生成時に求めた定常歩容の初期状態における上体水平位置速度を仮決め候補（Ｘｓ，Ｖｘｓ）とすればよい。
尚、説明を簡略にするため、サジタルプレーン上でＸ方向（前後方向）での定常歩容の初期状態を探索する場合を例にとるが、実際には位置・速度ともＸ方向（前後方向），Ｙ方向（左右方向）を別々にあるいは同時に定常歩容の初期状態（定常歩容の前記境界条件を満たす初期状態）を探索する必要がある。
探索的な決定手法としては、擬似ヤコビアン（感度マトリックス）を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。本実施例では、最急降下法を用いる。
次いで、Ｓ２０４を経てＳ２０６において、初期（時刻Ｔｓ）上体鉛直位置速度（Ｚｓ、Ｖｚｓ）（Ｚｓ：鉛直位置、Ｖｚｓ：鉛直速度）が決定される。
初期上体鉛直速度Ｖｚｓは、本実施形態では以下のように解析的に決定される。
ロボット１には、動力学的関係として、次式が成立する。
2003061917.tif 000014
（ただし、重力加速度は負の値とする。）
また、定常歩容では、終端全体重心鉛直位置と初期全体重心鉛直位置は一致するので、上式１３の右辺は零にならなければならない。したがって、これらの関係から初期全体重心鉛直速度を求めることができる。具体的には、まず、前記図１４のＳ１０４で設定した床反力鉛直成分パラメータによって算出される床反力鉛直成分をロボット１の全体質量で割った値を１歩の期間（時刻Ｔｓから時刻Ｔｅまで）２階積分することにより、床反力鉛直成分による全体重心移動量（式１３の右辺第１項）が求められる。さらに、重力加速度を一歩の期間２階積分することにより、重力による全体重心移動量（式１３の右辺第２項）が求められる。そして、上記求めた床反力による全体重心移動量と重力による全体重心移動量との和を符号反転し、これを１歩の時間（定常歩容の１周期Ｔｃｙｃ１の時間）で割ることにより、初期全体重心鉛直速度が求められる。
次に、初期上体鉛直位置Ｚｓを求めるために、次式を用いて、時刻０の全体重心鉛直速度が求められる。この際、次式における時刻Ｔｓの全体重心鉛直速度には、式１３に基づいて上記の通り求めた値を代入し、積分期間は時刻０から時刻Ｔｓまでとする。
2003061917.tif 000015
（ただし、重力加速度は負の値とする。）
次に、本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法を用いて時刻０における（すなわち着地瞬間における）上体高さ（上体鉛直位置）が決定される。このとき、時刻０における足平位置姿勢（前記図１４のＳ１００で決定した第１旋回歩容初期（時刻０）支持脚足平位置姿勢及び第１旋回歩容初期（時刻０）遊脚足平位置姿勢）と、各脚体２の膝部の曲げ角に関する所定の幾何学的条件とに基づいて、時刻０での各脚体２，２の膝部が伸び切らないような上体鉛直位置（上体高さ）が決定される。具体的には、支持脚側脚体２の膝曲げ角をθｓｕｐ、遊脚側脚体２の膝曲げ角をθｓｗｇとしたとき、例えばそれらの膝曲げ角θｓｕｐ，θｓｗｇの正弦値の逆数の和がある所定値（有限値）になるように、上体鉛直位置が決定される。ここで、膝曲げ角θｓｕｐ，θｓｗｇは、各脚体２の大腿部の軸心に対する下腿部の軸心の角度であり、各脚体２が伸びきった状態から膝部を曲げていくに伴い、０から増加していく角度である。なお、このような上体２４の鉛直位置の決定手法は、上記特開平１０−８６０８０号公報に詳細に説明されているので、ここでは以上の説明に留める。
次に、このように決定した時刻０における上体鉛直位置や、足平位置姿勢などから時刻０における全体重心鉛直位置がロボット１のモデルを用いて求められる。例えば図１０のモデルに対応する前記式０４のキネマティクスモデルを用いることで、時刻０における全体重心鉛直位置が求められる。具体的には、時刻０での上体鉛直位置と上体姿勢角（本実施形態では０）とから上体質点２４ｍの鉛直位置が求められ、時刻０での支持脚足平位置姿勢及び遊脚足平位置姿勢（これらは、前記図１４のＳ１００で設定した第１旋回歩容初期支持脚足平位置姿勢及び第１旋回歩容初期遊脚足平位置姿勢である）とからそれぞれ支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置が求められる。そして、これらの求めた上体質点２４ｍ、支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置を、前記式０４のＺｂ、Ｚｓｕｐ、Ｚｓｗｇに代入することで、全体重心鉛直位置（式０４のＺＧｔｏｔａｌ）が求められる。尚、ロボット１のより厳密なモデル（例えば図１１に示すような多質点モデル）を用い、時刻０での上体水平位置や腕姿勢をも考慮して、時刻０での全体重心鉛直位置を求めるようにしてもよい。
次に、前記式１３において、上記求めた時刻０における全体重心鉛直位置を同式１３の左辺の初期全体重心鉛直位置に代入し、上記求めた時刻０の全体重心鉛直速度を同式１３の右辺の初期全体重心鉛直速度に代入し、さらに、同式１３の１歩の時間をＴｓとし、積分期間を時刻０から時刻Ｔｓまでとすることで、式１３の左辺の終端全体重心鉛直位置の値が算出され、この算出値が初期（時刻Ｔｓ）全体重心鉛直位置として得られる。さらに、この求めた初期（時刻Ｔｓ）全体重心鉛直位置と時刻Ｔｓにおける足平位置姿勢（Ｓ２００で求めたもの）などから、ロボット１のモデル（例えば式０４のキネマティクスモデル）を用いて、初期（時刻Ｔｓ）上体鉛直位置Ｚｓが求められる。具体的には、時刻Ｔｓにおける支持脚及び遊脚の各平位置姿勢から、図１０のモデルの支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置が求められ、これらの鉛直位置と、上記求めた初期（時刻Ｔｓ）全体重心鉛直位置とを式０４に適用することで、上体質点２４ｍの鉛直位置（式０４のＺｂ）が求められる。そして、この上体質点２４ｍの鉛直位置と時刻Ｔｓでの上体姿勢（本実施形態では０）とから逆算的に初期（時刻Ｔｓ）上体鉛直位置Ｚｓが求められる。尚、この場合も、より厳密なモデルを用い、上体水平位置や腕姿勢をも考慮して初期上体鉛直位置Ｚｓを求めるようにしてもよい。
最後に、前記Ｓ２００で決定した足平位置姿勢の初期状態や、Ｓ２０６で求めた初期全体重心鉛直速度等から、ロボット１のモデル（例えば式０４のキネマティクスモデル）を用いて、初期上体鉛直速度が求められる。具体的には、式０４の両辺を時間微分してなる式に、前記式１３に基づいて求めた初期全体重心鉛直速度と、支持脚側及び遊脚側の各足平位置姿勢の初期状態により定まる支持脚質点２ｍ及び遊脚質点２ｍの鉛直速度とを適用することで、上体質点２２ｍの鉛直速度が求められる。そして、この上体質点２２ｍの鉛直速度と上体姿勢角の初期状態（Ｓ２００もしくは後述のＳ２０８で決定されたもの）とから、上体鉛直速度が求められる。尚、各足平位置姿勢の初期状態や、初期全体重心鉛直速度のほか、腕姿勢の初期状態（Ｓ２００で決定されたもの）、仮決定した上体水平位置の初期状態（Ｓ２０２もしくは後述のＳ２１６もしくはＳ２１８で仮決定された最新のもの）、上記求めた初期上体鉛直位置を考慮し、上記求めた初期全体重心鉛直速度を満足するような初期上体鉛直速度を、ロボット１のより厳密なモデルを用いて求めるようにしてもよい。
補足すると、ロボット１のモデルが図１１のような複雑なもの（厳密なモデル）であっても、ロボット１は、片脚当たり６自由度であることから、足平位置姿勢軌道および上体位置姿勢軌道の初期状態（位置姿勢とその変化率）が与えられれば、ロボットの初期状態は一義的にすべて決定されることになる。したがって、初期全体重心鉛直速度もこれらが与えられれば一義的に決定される。逆に、片脚当たり６自由度であることから、足平位置姿勢軌道および上体位置姿勢軌道の初期状態の内で速度に関する状態のひとつ（例えば初期上体鉛直速度）が未定であっても、初期全体重心鉛直速度が与えられれば、未定の初期状態は一義的に決定される。
Ｓ２０６の処理の後、次いでＳ２０８に進み、定常旋回歩容が仮に生成される。より具体的には、前記図１２のＳ０２２で決定した定常歩容の歩容パラメータを基に、初期時刻Ｔｓから終端時刻Ｔｅまでの各瞬間における目標ＺＭＰ、目標床反力鉛直成分、目標足平位置姿勢、基準上体姿勢、目標腕姿勢および床反力水平成分許容範囲が逐次求められる。そして、求めた目標ＺＭＰと目標床反力鉛直成分とに関する動力学的平衡条件、並びに床反力水平成分許容範囲の条件を満足するように、前記動力学モデル（図１０のモデル）を用いて、前記上体水平位置速度（Ｘｓ，Ｖｘｓ）、上体鉛直位置速度（Ｚｓ、Ｖｚｓ）を上体２４の初期（時刻Ｔｓ）状態として、上体位置姿勢を逐次決定することによって、時刻Ｔｓから終端時刻Ｔｅまでの歩容を生成する。また、この時、上体姿勢は、基準上体姿勢になるべく一致するように生成される。
尚、この定常歩容の歩容生成は歩容生成装置１００の内部で行われるだけであって、実際のロボット１を駆動するための目標値として、前記複合コンプライアンス動作決定部１０４に出力されることはない。
以下に、Ｓ２０８の処理である逐次演算による定常歩容生成処理の詳細を説明する。
図２０はその処理を示すサブルーチンフローチャートである。
以下説明すると、Ｓ３００において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻ｋに初期時刻Ｔｓを代入する。さらに、上体水平位置速度に、現在仮決定されている（Ｘｓ，Ｖｘｓ）（図１９のＳ２０２もしくは後述のＳ２１６もしくはＳ２１８で決定されたもの）が代入され、上体鉛直位置速度に、前記Ｓ２０６で求めた最新の（Ｚｓ、Ｖｚｓ）が代入される。また、目標上体姿勢角には、基準上体姿勢角初期値（時刻Ｔｓでの基準上体姿勢角）、目標上体姿勢角速度には、基準上体姿勢角速度初期値（時刻Ｔｓでの基準上体姿勢角速度）を代入する。補足すると、後述するごとく最終的には初期目標上体姿勢角速度は変更されるので、基準上体姿勢角速度初期値と異なる値を代入しても良い。
次いで、Ｓ３０２を経てＳ３０４において、仮歩容生成用時刻ｋが歩容終端時刻以前であるか否か（ｋ≦Ｔｓ＋Ｔｃｙｃであるか否か）が判断され、その判断結果がＹＥＳである場合には、Ｓ３０６の歩容瞬時値決定サブルーチンに進み、歩容瞬時値が決定される。次いで歩容生成装置１００の処理は、Ｓ３０８に進み、仮歩容生成用時刻ｋをΔｋだけ増やした後、Ｓ３０４に戻る。
ここで、Δｋは、仮歩容生成のインターバルであり、通常、制御周期Δｔと一致させておけば良い。なお、Δｋは、仮歩容の動力学的精度を落としても良いならば、演算量を低減するために、ΔｋをΔｔよりも長く設定しても良い。
Ｓ３０４の判断結果がＮＯである場合には、Ｓ３１０に進む。以上の処理により、Ｓ３１０に進むまでに定常歩容が初期から終端まで生成される。
以下に、Ｓ３０６の歩容瞬時値決定サブルーチンの詳細を図２１を参照して説明する。
まず図２１のＳ４００において、定常歩容パラメータ（床反力鉛直成分軌道パラメータ）を基に、前記図１６に示した目標床反力鉛直成分の時刻ｋにおける値（今回値）が求められる。さらに、Ｓ４０２において、定常歩容パラメータ（ＺＭＰ軌道パラメータ）を基に、前記図１８に示した目標ＺＭＰ軌道の時刻ｋにおける値（今回値）が求められる。
次いでＳ４０４に進み、定常歩容パラメータ（足平軌道パラメータ、基準上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ）を基に、時刻ｋにおける目標両足平位置姿勢（支持脚側及び遊脚側の両者の目標足平位置姿勢）、基準上体姿勢および目標腕姿勢の値（今回値）が求められる。ただし、目標腕姿勢に関しては、より詳しくは両腕体の全体重心位置が決定されるが、鉛直軸（または上体体幹軸）まわりの角運動量を変化させる腕体の運動（腕振り運動）は、未だ決定されていない。尚、目標足平位置姿勢の今回値（時刻ｋでの値）は、前記図１９のＳ２００で初期時刻Ｔｓでの足平位置姿勢を求めた場合と同様に求められる。
次いでＳ４０６に進み、目標床反力鉛直成分を満足する（ロボット１の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる）ような時刻ｋにおける全体重心鉛直位置速度の値（今回値）が算出される。具体的には、例えば前記図１０の動力学モデルに係わる前記式０１と式０４とに基づいて、全体重心鉛直位置速度が算出される。すなわち、式０１と式０４とによれば、ロボット１の運動による全体重心鉛直加速度と重力加速度との和にロボット１の全体質量を乗算したものが床反力鉛直成分に等しいという関係式（ロボット１の全体重心の鉛直方向に関する運動方程式）が得られる。従って、この関係式と目標床反力鉛直成分とから、全体重心鉛直加速度が求められる。
尚、この関係式自体は、ロボット１のモデルに依存することなく一般的に成立するものである。そして、この求めた全体鉛直加速度を積分することにより、全体重心鉛直速度が算出され、さらにその全体重心鉛直速度を積分することにより、全体重心鉛直位置が算出される。これらの算出は、より一般的には、次式１５および式１６で表される動力学関係式（ニュートンの運動方程式を離散化した式）を用いて行なわれる。
2003061917.tif 000016
次いでＳ４０８に進み、全体重心鉛直位置を満足する上体鉛直位置が算出される。具体的には、例えば前記図１０のモデルに係わる式０４を用いて上体鉛直位置が算出される。すなわち、支持脚側及び遊脚側の目標足平位置姿勢の今回値から、図１０のモデルの支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置が求められる。そして、これらの求めた支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置と、Ｓ４０７で求めた全体重心鉛直位置の今回値とを式０４に適用することで、上体質点２４ｍの鉛直位置が求められる。さらに、この求めた上体質点２４ｍの鉛直位置と目標上体姿勢角の現在値（Ｓ４０４で設定された基準上体姿勢角または後述のＳ４１４で決定された前回（時刻ｋ−Δｋ）の目標上体姿勢角）とから上体鉛直位置が求められる。
尚、より厳密なモデル（例えば図１１に示すようにロボット１の各リンクに対応する質点をもたせたような多質点モデル）を用いて上体鉛直位置を求めてもよい。この場合には、前記求めた目標両足平位置姿勢の今回値（時刻ｋの値）、時刻（ｋ−Δｋ）の目標上体姿勢（または前記求めた基準上体姿勢の今回値）、前記求めた目標腕姿勢（両腕の重心位置）の今回値、時刻（ｋ−Δｋ）の上体水平位置と、求めたい上体鉛直位置とから決定される全体重心鉛直位置が、前記求めた全体重心鉛直位置の今回値に一致するように上体鉛直位置が求められる。
この場合、時刻ｋの上体姿勢と上体水平位置は、未だ決定されていないので、代わりに時刻（ｋ−Δｋ）の値を用いた。より精度を高めるために、時刻ｋでの推定値を、時刻（ｋ−Δｋ）での歩容状態から外挿によって求めても良い。
次いでＳ４１０に進み、前記図１４のＳ１０８で決定した歩容パラメータ（定常歩容の床反力水平成分許容範囲を規定するパラメータ）に基づいて、図１７に示した床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］の時刻ｋにおける値（今回値）が求められる。
次いでＳ４１２に進み、目標ＺＭＰに関する動力学的平衡条件（ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０であるという条件）を満足するように、上体水平加速度と上体姿勢角加速度との今回値が決定される。ただし、床反力水平成分Ｆｘが［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えず、かつ、上体姿勢角速度が定常歩容の初期と終端とで一致するように、上体水平加速度と上体姿勢角加速度とが決定される。以下にこの詳細を説明する。
この時点において、前記の如く足平位置姿勢および上体鉛直位置の瞬時値（今回値）が決定されているので、残りの上体水平位置と上体姿勢角とを決定すれば、鉛直軸まわりの角運動量を変化させる腕体の運動自由度を除いて、ロボット１の目標運動は一義的に決定できる。したがって、上体水平位置と上体姿勢角とを決定すれば、床反力の鉛直軸まわりのモーメントを除き、すべての床反力も一義的に決定される。なお、本実施形態では、定常歩容の目標床反力鉛直成分及び目標ＺＭＰは、それぞれ前記図１２のＳ０２２で決定した床反力鉛直成分軌道パラメータ及び目標ＺＭＰ軌道パラメータによって規定される。したがって、上体水平位置と上体姿勢角の決定に応じて従属的に決定される床反力は、床反力水平成分のみである。また、補足すると、前述したように、ロボット１は片脚当たり６自由度であることから、目標足平位置姿勢と目標上体位置姿勢とから脚体２の各部の位置姿勢は一義的に決定される。なお、鉛直軸まわりの角運動量を変化させる腕の運動自由度は、後述するように、スピン力を打ち消すために用いられる。
ところで、歩容生成において、前記上体並進モードをあまり用いずに主に上体回転モードを用いることにより、目標ＺＭＰを満足させる（目標ＺＭＰまわりの床反力モーメントの水平成分を０にする）と、上体姿勢角が大きく振れ過ぎてしまう恐れがある。したがって、これを防ぐためには、なるべく上体並進モードを優先的に用いるべきである。しかし、上体並進モードは、床反力水平成分の変化を伴うので、床反力水平成分許容範囲が狭い場合には、上体並進モードを強く働かせるとスリップする恐れがある。この場合には、上体回転モードに頼るしかない。特に、前記走行歩容のように床反力水平成分許容範囲が０となる期間では、床反力水平成分を発生させるような歩容を生成することはできないので、上体回転モードに頼らざるを得ない。以上のことを考慮し、本実施形態では、図２２に示すフローチャートに従って、上体水平加速度と上体姿勢角加速度とが決定される。
まず、Ｓ５００において、初期時刻Ｔｓから第１旋回歩容の空中期を経て第２旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻Ｔｍが設定される。さらに、第２旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻Ｔｓ２が設定される。さらに、第２旋回歩容の空中期を経て第１旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻Ｔｍ２が設定される。
図１６にこれらの時刻を示す。時刻Ｔｍは、支持脚足平２２の底面のほぼ全面が接地した瞬間あるいはその直後に設定するのが望ましい。時刻Ｔｍ２も同様である。また、時刻Ｔｓ２は、前記初期時刻Ｔｓ同様、足平２２の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前に設定するのが望ましい。
また、前記図１４のＳ１１０において設定した図１８の目標ＺＭＰとこれらの時刻Ｔｍ，Ｔｓ２，Ｔｍ２との関係を説明すると、第２旋回歩容において、目標ＺＭＰが、支持脚足平２２のかかとを初期位置として、該足平２２の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平２２の中央に移動し始める瞬間を時刻Ｔｍとすることが望ましい。その後、支持脚足平２２のつま先だけ接地した状態になるまでに、目標ＺＭＰがつま先に移動を完了した瞬間を、時刻Ｔｓ２とすることが望ましい。さらに次の第１旋回歩容において、目標ＺＭＰが、支持脚足平２２のかかとを初期位置として該足平２２の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平２２の中央に移動し始める瞬間を時刻Ｔｍ２とすることが望ましい。
上記のごとく設定する理由は後述する。
次にＳ５０２において現在時刻（定常歩容作成用タイマーの値）ｋが、時刻ＴｓからＴｍの間または時刻Ｔｓ２からＴｍ２の間にあるか否かが判定される。Ｓ５０２の判定結果がＹＥＳであるならば、Ｓ５０４に進み、ＮＯであるならばＳ５１８に進む。
Ｓ５０４では、ロボット１の前回瞬時歩容状態（時刻ｋ−１の歩容状態）から、仮に、上体回転モードの角加速度を０にして、上体並進モードの運動をロボット１にさせた場合に今回（時刻ｋ）の目標ＺＭＰを満足するために必要な上体水平加速度αｔｍｐが求められる。このαｔｍｐは、例えば前記図１０の動力学モデルに係わる前記式０３を用いて求められる。より具体的には、例えば、現在時刻ｋまでに求められた目標足平位置姿勢の時系列値を用いて、現在時刻ｋにおける支持脚質点２ｍ及び遊脚質点２ｍの鉛直加速度が求められると共に、現在時刻ｋ（今回）の目標足平位置姿勢を用いて支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置が求められる。また、現在時刻ｋ（今回）の床反力鉛直位置を用いて上体質点２４ｍの鉛直位置が求められると共に、現在時刻ｋまでに求めた目標上体鉛直位置の時系列値を用いて現在時刻ｋにおける上体質点２４ｍの鉛直加速度が求められる。そして、これらの求めた値を前記式０３に代入すると共に、同式０３のＭｙ、ｄ２θｂｙ／ｄｔ２を０にしてなる式を、ｄ２Ｘｂ／ｄｔ２について解くことにより、上体質点水平加速度ｄ２Ｘｂ／ｄｔ２が上体水平加速度αｔｍｐとして求められる。尚、より厳密な動力学モデルを用いて、目標ＺＭＰ回りの床反力モーメントの水平成分を零にするような上体水平加速度αｔｍｐを探索的に求めるようにしてもよい。また、本実施形態では、基準上体姿勢が鉛直姿勢で、基準上体姿勢による上体姿勢角加速度が０であるため、上体回転モードの角加速度を０にして上体水平加速度αｔｍｐを求めるようにした。但し、基準上体姿勢が変化するように該基準上体姿勢軌道パラメータを設定した場合で、それにより定まる現在時刻ｋにおける基準上体姿勢角加速度が０でない場合には、上体回転モードの角加速度をその０でない基準上体姿勢角加速度の値にして、上体水平加速度αｔｍｐを動力学モデルを用いて求める（例えば式０３のｄ２θｂｙ／ｄｔ２を０でない基準上体姿勢角加速度に設定して上記と同様に上体水平加速度αｔｍｐを求める）ようにすればよい。
次に、Ｓ５０６に進み、上体水平加速度がαｔｍｐの場合の時刻ｋにおける床反力水平成分Ｆｘｔｍｐが動力学モデルを用いて求められる。本実施形態においては、前記動力学モデルの式０２を用いてＦｘｔｍｐが求められる。すなわち、次式１７によってＦｘｔｍｐが求められる。但し、ｄ２Ｘｓｕｐ／ｄｔ２およびｄ２Ｘｓｗｇ／ｄｔ２はそれぞれ時刻ｋにおける支持脚足平質点水平加速度、遊脚足平質点水平加速度を表す。
2003061917.tif 000017
このようにして求められるＦｘｔｍｐの例を図２３に示す。図２３に、Ｆｘｔｍｐが床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えている部分を斜線で示す。
次に、Ｓ５０８に進み、上体並進モードの上体水平加速度α、これによって発生する床反力水平成分Ｆｘ、上体回転モードの上体角加速度βが次のように決定される（Ｓ５０８〜Ｓ５１６）。
すなわち、
Ｆｘｔｍｐ＞Ｆｘｍａｘならば、Ｓ５１０に進み、次式によりＦｘが決定される。
2003061917.tif 000018
Ｆｘｔｍｐ＜Ｆｘｍｉｎならば、Ｓ５１２に進み、次式によりＦｘが決定される。
2003061917.tif 000019
それ以外、すなわち、Ｆｘｔｍｐが、床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］内に存するならば、Ｓ５１４に進み、次式によりＦｘが決定される。
2003061917.tif 000020
いずれの場合においても、次いでＳ５１６に進み、次式により上体水平加速度α、上体姿勢角加速度βが決定される。
2003061917.tif 000021
ただし、ΔＦｐ、ΔＭｐおよびΔＭｒはそれぞれ前記式０６、０７、および式０９により決定される。
補足すると、より動力学演算の精度を高めようとするならば、上記のごとく上体角加速度βを求めた後、上体並進モードと上記求めた上体角加速度βの上体回転モードとを合成した運動が目標ＺＭＰを満足するように、上体並進モードの上体水平加速度αを、より厳密な動力学モデルを用いて解析的あるいは探索的に決定した方が良い。なお、探索的な決定法としては、擬似ヤコビアン（感度マトリックス）を求め、擬似ニュートン法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。
さらに床反力水平成分Ｆｘが床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を厳密に越えないようにするために、Ｓ５１０においては、Ｆｘ＝Ｆｘｍａｘかつ目標ＺＭＰまわりの床反力モーメントの水平成分が０になるように、Ｓ５１２においては、Ｆｘ＝Ｆｘｍｉｎかつ目標ＺＭＰまわりの床反力モーメントの水平成分が０になるように、上体水平加速度αと上体角加速度βとの組を探索的に求めても良い。
上記のように求められるＦｘを図２４に示す。Ｆｘは、Ｆｘｔｍｐの値を床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］から越えないように制限（飽和）させたものになっている。より詳しくは、上体並進モードのみによる上体水平加速度αｔｍｐに応じたＦｘｔｍｐが許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］内にある場合には、ＦｘｔｍｐがそのままＦｘとなる。また、上体並進モードのみによる上体水平加速度αｔｍｐに応じたＦｘｔｍｐが許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］の上限を超え、もしくは下限を下回った場合には、それぞれＦｘは、それぞれ強制的にＦｘｍａｘ，Ｆｘｍｉｎに制限される。特に走行歩容の空中期では、常に、Ｆｘｍａｘ＝Ｆｘｍｉｎ＝０であるので、Ｆｘ＝０となる。
また、上記のように求められる上体姿勢角加速度βを図２５に示す。上体並進モードによって発生するＦｘが許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えないように、上体並進モードの加速度を制限したことによって不足した床反力モーメント（より詳しくは目標ＺＭＰ回りの床反力モーメントの水平成分を０にする上で必要な慣性力モーメントから、制限した上体水平運動と両脚体２，２の運動とによるモーメント成分を差し引いたモーメント）を、上体回転モードで補ったことになる。なお、走行歩容の空中期では、上体並進モードによる上体水平加速度αは常に０に制限されることとなるので、上体回転モードによる上体姿勢角加速度βのみによって、上記不足分の床反力モーメントが補われることとなる。
以上が、時刻ｋが時刻ＴｓからＴｍの間または時刻Ｔｓ２からＴｍ２の間の場合の処理である。
Ｓ５０２の判定結果がＮＯである場合には以下の処理が行われる。まず、Ｓ５１８に進み、上体姿勢角速度を、現在時刻ｋが時刻Ｔｓ２以前ならば時刻Ｔｓ２までに、そうでないならば時刻Ｔｅまでに、初期値（現在時刻ｋが時刻Ｔｓ２以前ならば時刻Ｔｓでの値、そうでないならば時刻Ｔｓ２での値）に戻すための上体姿勢角加速度を発生させる床反力モーメントのＺＭＰ換算値（以降、上体姿勢復元モーメントＺＭＰ換算値と呼び、ＺＭＰｒｅｃと略す）のパターンを設定する。
これについて以下に詳細に説明する。現在時刻ｋが時刻Ｔｍ２以降の時期の処理は、時刻Ｔｓ２以前の時期の処理と同様なので、現在時刻ｋが時刻Ｔｓ２以前までの時期の処理だけを解説する。
時刻Ｔｍから時刻Ｔｓ２までに、上体回転モードを用いて上体姿勢角加速度を発生させることにより、上体姿勢角速度を初期値（時刻Ｔｓでの値）に戻すことを考える。このための上体姿勢角加速度パターンを、β（ｋ）とする。
上体回転モードでは、上体姿勢角加速度β（ｋ）を発生させようとすると、床反力モーメントβ（ｋ）＊ΔＭｒが発生する。この結果、その瞬間における床反力鉛直成分がＦｚ（ｋ）であるとすると、（目標ＺＭＰでなく）運動から算出されるＺＭＰ（ｋ）は、次式で求められるΔＺＭＰだけずれる。
2003061917.tif 000022
したがって、ΔＭｒのパターンとＦｚ（ｋ）のパターンが決まっていれば（既知であれば）、ΔＺＭＰ（ｋ）のパターンを適当に設定することによって、式２３を満足する上体姿勢角加速度パターンを発生させて、上体姿勢角速度を初期値（時刻Ｔｓでの値）、すなわち基準上体姿勢軌道の初期（時刻Ｔｓ）状態における上体姿勢角速度に戻すことができる。
前記上体姿勢復元モーメントＺＭＰ換算値（ＺＭＰｒｅｃ）は、そのように適切に設定されたΔＺＭＰ（ｋ）を意味する。前記式２３を用いて上体姿勢復元モーメントＺＭＰ換算値を設定する際に、ΔＭｒは厳密には変動するが、近似的に一定値としても良い。定常歩容は仮に生成されるだけであり、実際のロボットをこの歩容に追従させるわけではないので、定常歩容の動力学的精度はあまり高くなくても良いからである。
図２６にＺＭＰｒｅｃの一例を示す。図２６では、そのパターンは台形パターンとされている。その台形部の折れ点の時刻は、時刻Ｔｍと時刻Ｔｓ２との間の期間における目標ＺＭＰパターンの折れ点の時刻（図１８参照）に一致させてある。これは、後述するが、今回歩容の目標ＺＭＰパターンの修正が簡単になるからである。式２３のΔＺＭＰ（ｋ）にＺＭＰｒｅｃを代入することにより次式が得られる。
2003061917.tif 000023
したがって、この式２４で求められるβ（ｋ）は、図２７の実線で示すようになる。なお、図２７の点線で示すものは、時刻Ｔｓから時刻Ｔｍまでの上体姿勢角加速度（図２５の実線示のもの）である。
ところで、定常歩容では、時刻Ｔｓ２での上体姿勢角速度が時刻Ｔｓでの上体姿勢角速度に一致していることが望ましい。一致していると、上体姿勢角の変動が小さくなるからである。
そこで、時刻Ｔｓ２での上体姿勢角速度を時刻Ｔｓでの上体姿勢角速度に一致させるため、時刻ＴｓからＴｓ２までの上体姿勢角加速度βの積分値が０になるように、上体姿勢角加速度パターンを設定する。すなわち、時刻Ｔｍから時刻Ｔｓ２までの期間のβ（ｋ）の積分値（図２７の実線部の積分値）が、時刻Ｔｓから時刻Ｔｍまでの上体姿勢角加速度（これは図２２のＳ５０４〜Ｓ５１６で前述のように求められる）の積分値（図２７の点線部の積分値、言い換えれば時刻Ｔｍにおける上体姿勢角速度と時刻Ｔｓにおける上体姿勢角速度の差）に−１を乗じた値になるように設定される。
より具体的には、本実施形態では、図２６のＺＭＰｒｅｃの台形パターンにおいて、台形の折れ点の時刻（これは前述のように定常歩容の目標ＺＭＰ軌道に基づいて決定される）を既知とし、台形高さを未知数とすることで、上記関係を満足するＺＭＰｒｅｃの台形高さ（ＺＭＰｒｅｃパターンを規定するパラメータ）が求められる。
第１旋回歩容におけるＺＭＰｒｅｃの台形高さを第１旋回歩容上体姿勢復元モーメントＺＭＰ換算値台形高さａｃｙｃ１と呼ぶ。第２旋回歩容におけるＺＭＰｒｅｃの台形高さを第２旋回歩容上体姿勢復元モーメントＺＭＰ換算値台形高さａｃｙｃ２と呼ぶ。なお、第２旋回歩容におけるＺＭＰｒｅｃは、時刻Ｔｍ２からＴｅまでの期間で設定される台形パターンである。
補足すると、今回歩容を１歩生成している途中で歩容パラメータが変更されない限り、時刻ＴｍとＴｓ２との間の期間、又は時刻Ｔｍ２とＴｅとの間の期間においてＳ５１８で決定されるＺＭＰｒｅｃパターンは変わらない（同一パラメータ値が算出される）ので、Ｓ５１８の処理は、時刻Ｔｍと時刻Ｔｍ２でのみ実行しても良い。
次いでＳ５２０に進み、時刻ｋにおける上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃの瞬間値（今回値）が、上記のごとく決定したパラメータ（台形高さ）を基に算出される。
次いでＳ５２２に進み、前記式２４を用いて、時刻ｋにおける上記上体姿勢角加速度パターンの瞬間値（今回値）βが算出される。
次いでＳ５２４に進み、ロボットの前回瞬時目標歩容状態（時刻ｋ−１の目標歩容状態）から、仮に上体回転モードをさせずに上体並進モードの運動のみをさせた場合に、今回（時刻ｋ）の目標ＺＭＰを満足するために必要な上体水平加速度αｔｍｐが、ロボット１の動力学モデルを用いて求められる。この算出は、Ｓ５０４と同様に行なわれる。
次いでＳ５２６に進み、次式を用いて、上体並進モードの上体水平加速度αが算出される。
2003061917.tif 000024
さらにＳ５２８に進み、上体水平加速度が上記の如く求めたαである場合の時刻ｋにおける床反力水平成分Ｆｘが、前記式１７と同様の次式２６によって求められる。
2003061917.tif 000025
補足すると、より動力学演算の精度を高めようとするならば、上記のごとく上体角加速度βを求めた後、Ｓ５２６において、式２５を用いた上体水平加速度αの算出を実行する代わりに、Ｓ５１６の補足で解説した手法と同様、上体並進モードと上記求めた上体角加速度βの上体回転モードを合成した運動が目標ＺＭＰを満足するように、上体並進モードの上体水平加速度αを、より厳密な動力学モデルを用いて、解析的あるいは探索的に決定した方が良い。
以上が、時刻ｋが時刻ＴｓからＴｍの間または時刻Ｔｓ２からＴｍ２の間のいずれでもない場合の処理である。
以上の処理を時刻Ｔｓ２まで完了した時の、床反力水平成分Ｆｘを図２８に示す。さらに、以上の処理を時刻Ｔｅまで完了した時の、床反力水平成分Ｆｘを図２９に、上体姿勢角加速度βを図３０に示す。なお、以上に示したＳ４１２の処理（上体水平加速度と上体姿勢角加速度を決定するサブルーチン）は、時刻ｋにおける上体水平加速度と上体姿勢角加速度の瞬間値が決定されるのであって、時刻Ｔｅまでのこれらの時系列が一度に生成されるわけではない。
なお、本実施形態では、基準上体姿勢が鉛直姿勢で、基準上体姿勢による上体姿勢角加速度が０であるため、上記の如く求めた上体回転モードの角加速度βをＳ４１２の処理で決定される上体姿勢角加速度とした。但し、基準上体姿勢が変化するように該基準上体姿勢軌道パラメータを設定した場合には、上記の如く求めた上体回転モードの角加速度βと基準上体姿勢角加速度の和をもって、Ｓ４１２の処理で決定される上体姿勢角加速度とすべきである。
時刻Ｔｓ、Ｔｍ、Ｔｓ２およびＴｍ２を前記のごとく設定した理由は、時刻ＴｍからＴｓ２までの期間と時刻Ｔｍ２からＴｅまでの期間とにおいて、上体姿勢角速度を基準上体姿勢軌道の初期角速度に戻すように上体姿勢角加速度βを発生させても、床反力水平成分Ｆｘが許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えないようにするためである。換言すれば、時刻ＴｍからＴｓ２までの期間と時刻Ｔｍ２からＴｅまでの期間とにおいては、床反力水平成分許容範囲が十分に大きいため、目標ＺＭＰを満足しつつ、上体姿勢角速度を戻すように上体姿勢角加速度βを発生させても、床反力水平成分Ｆｘは、許容範囲を越えることがない。
また、時刻Ｔｓでの上体姿勢角速度を時刻Ｔｓでの上体姿勢角速度に一致させることを考えず、ａｃｙｃ１とａｃｙｃ２を同一値として（ａｃｙｃ１＝ａｃｙｃ２として）、時刻Ｔｅでの上体姿勢角速度と時刻Ｔｓでの上体姿勢角速度に一致させるようにａｃｙｃ１およびａｃｙｃ２の値を決定しても良い。
次いで図２１のＳ４１４に進み、Ｓ４１２で求めた上体水平加速度を逐次積分する（時刻Ｔｓから現在時刻ｋまで累積加算する）ことにより上体水平速度が求められ、さらにその上体水平速度を逐次積分する（時刻Ｔｓから現在時刻ｋまで累積加算する）ことにより、上体水平位置（今回値）が求められる。また、Ｓ４１２で求めた上体姿勢角加速度を逐次積分する（時刻Ｔｓから現在時刻ｋまで累積加算する）ことにより上体姿勢角速度が求められ、さらにその上体姿勢角速度を逐次積分する（時刻Ｔｓから現在時刻ｋまで累積加算する）ことにより、上体姿勢角（今回値）が求められる。
以上のごとく図２０のＳ３０６の定常歩容瞬時値決定サブルーチンが実行された後、Ｓ３０８に進み、歩容生成用時刻ｋの値が歩容生成インターバルΔｋだけ増加される。次いでＳ３０４に戻り、Ｓ３０４に示す条件が成立している限り、Ｓ３０６とＳ３０８の処理が繰り返され、Ｓ３０４に示す条件が成立しなくなったら、すなわち終端（時刻Ｔｅ＝Ｔｓ＋Ｔｃｙｃ）まで仮定常歩容の生成が完了したら、Ｓ３１０に進む。Ｓ３１０では、次式に従って初期（時刻Ｔｓ）上体姿勢角と初期（時刻Ｔｓ）上体姿勢角速度が、上記求めた仮歩容の終端（時刻Ｔｅ）上体姿勢角などを基に修正される。この修正は、上体姿勢角及びその角速度に関する定常歩容の境界条件（定常歩容の初期と終端とで上体姿勢角及び角速度を一致させる）を満たすようにするためのものである。尚、ここでの説明では、ロボット１の歩容は、該ロボット１を直線的に移動させる歩容であるとする。
この場合、次式の新たな値が修正された値である。
2003061917.tif 000026
ここで、上記式３０により初期状態姿勢角速度を修正する理由を以下に説明する。定常歩容では、それを繰り返したときに境界でロボット１の運動状態が不連続にならないように（前記境界条件を満たすように）初期状態姿勢角及びその角速度を決定する必要がある。この場合、上述のように求めた仮定常歩容の終端上体姿勢角速度は、前述した上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃに対応する上体姿勢角加速度パターンの設定によって、初期上体姿勢角速度初期値（基準上体姿勢角速度初期値）に一致するように生成されているので、残りの条件である次式を満たせば、上体姿勢角及び角速度に関する上記条件を満足する。
2003061917.tif 000027
また、
2003061917.tif 000028
であるから、この式３３の両辺を定常歩容の初期から終端まで２階積分することにより、次式の関係を得る。
2003061917.tif 000029
一方、定常歩容生成においては、前記の如く目標ＺＭＰを満足するために、基本的には上体並進運動モードを用いて上体２４の水平加速度（水平加減速）を調整するが、目標ＺＭＰを満足するための上体２４の水平加速度が、十分に発生できない状況（十分な摩擦力が発生できない状況）では、上体回転運動モードを用いて全体重心加速度が変わらないようにしつつ上体姿勢角の角加速度を調整することで、上体並進運動モードによる上体水平加速度の不足分が補われる。
上記上体回転運動モードを用いて上体姿勢角加速度を調整する動作は、図１０に示すフライホイールＦＨが備わったロボット１のモデルにおいて、そのフライホイールＦＨの角加速度を調整することと近似的に等価である。フライホイールＦＨの回転速度が０に近いならば、ジャイロ効果は発生せず、フライホイールＦＨが発生する力は、フライホイールＦＨの角加速度だけで決定される。したがって、上体２４にフライホイールＦＨが備わったロボット１においては、フライホイールＦＨの初期角度・角速度が多少変わっても、該フライホイールＦＨを同じパターンの角加速度で動かせば、発生する力はほとんど変わらない。それ故、ロボット動力学方程式である式０１、式０２、式０３ではジャイロ効果を無視している。したがって、本実施形態のロボット１において、式０１、式０２、式０３、あるいは、より一般的で厳密な動力学モデルのいずれを用いた場合でも、全体重心加速度が変わらないようにしつつ上体姿勢角を同じパターンの角加速度で動かせば（すなわち上体回転運動モードを用いて、上体姿勢角を同じパターンの角加速度で動かせば）、初期上体姿勢角度・角速度を少々変えても、発生する慣性力と重力との合力はほぼ同じになる。言い換えれば、初期上体姿勢角度・角速度を少々変えても、上記のフローチャートに従って生成される歩容の上体姿勢角加速度は同じになる。
つまり、式３４における上体姿勢角加速度の２階積分値は、初期上体姿勢角度・角速度に依存しなくなる。すなわち、式３４における上体姿勢角加速度の２階積分値は、初期上体姿勢角度・角速度の値に関わらず、一定値として扱うことができる。
初期上体姿勢角の値が仮歩容の初期上体姿勢角、かつ初期上体姿勢角速度の値が仮歩容の初期上体姿勢角速度であった場合に、終端上体姿勢角が仮歩容の終端上体姿勢角になったのであるから、この関係を式３４に代入して次式を得る。
2003061917.tif 000030
これより、
2003061917.tif 000031
上記議論により上体姿勢角加速度の２階積分値は定数扱いできる（初期上体姿勢角度・角速度に依存しない）ので、上式を式３４に代入し、次式が得られる。
2003061917.tif 000032
式３７と式３２とから前記式３０が得られる。
なお、初期上体姿勢角は、本実施形態では、式３１のように基準上体姿勢角初期値に一致させるようにしたが、定常歩容の上体姿勢角の平均値が基準上体姿勢角初期値に一致するように初期上体姿勢角を設定するようにしてもよい。あるいは、定常歩容の上体姿勢角の最大値と最小値の平均値が基準上体姿勢角初期値に一致するように初期上体姿勢角を設定しても良い。
また、前記ではロボット１を直進移動させる場合に限定した例を説明したが、旋回移動を含む定常歩容を決定する場合には、その初期と終端とではロボット１の進行方向が変わる。このため初期上体姿勢角速度は初期上体姿勢角に依存するようになる。したがって、これを考慮して初期上体姿勢角および角速度を決定する必要がある。この場合には、例えば次の式３７ａ、３７ｂを満たすように初期上体姿勢角及び初期上体姿勢角速度を更新するようにすればよい。
2003061917.tif 000033
なお、式３７ａ、３７ｂのそれぞれの右辺第１項の積分期間は、時刻ＴｓからＴｍまでの期間と、Ｔｓ２からＴｍ２までの期間とを合わせた期間であり、右辺第２項の積分期間は、時刻ＴｍからＴｓ２までの期間とＴｍ２からＴｅまでの期間とを合わせた期間である。
より具体的に説明すると定常歩容では、第１旋回歩容の支持脚座標系（次回歩容支持客座標系）から見た初期状態姿勢角及び角速度が、それぞれ、次の第１旋回歩容の支持脚座標系（次次次回歩容支持脚座標系）から見た終端上体姿勢角および角速度に一致していなければならない。そこで、本実施形態では、新たな初期上体姿勢角は、前記式３１により決定し、この新たな初期上体姿勢角と、これを定常歩容におけるロボット１のトータルの旋回角（鉛直軸回りの旋回角）に応じたマトリクス（回転座標変換のマトリクス）により次回歩容支持脚座標系から見た値に座標変換したものとをそれぞれ式３７ａの左辺の初期上体姿勢角、終端上体姿勢角に代入する。また、式３７ａ，３７ｂのそれぞれの右辺第１項の積分に係る上体姿勢角加速度は、前記図２２のＳ５１６で逐次求められたものが用いられる。
そして、式３７ａ，３７ｂの初期上体姿勢角速度と、式３７ａ，３７ｂの右辺第２項の積分に係るＺＭＰｒｅｃ（図２６の台形状のパターン）の台形高さをと未知数として（但し、ＺＭＰｒｅｃの台形パターンの折れ点の時刻はあらかじめ定める。また、第１旋回歩容のＺＭＰｒｅｃの台形高さａｃｙｃ１と第２旋回歩容のＺＭＰｒｅｃの台形高さａｃｙｃ２を同一値とする。）、それらの未知数を含む式３７ａ，３７ｂの連立方程式を解くことにより求められる初期上体姿勢角速度を新たな初期上体姿勢角速度として決定する。この場合、式３７ｂの終端上体姿勢角速度は、未知数である初期上体姿勢角速度を、定常歩容の上記トータルの旋回角に応じたマトリクスにより次回支持脚座標系から見た値に座標変換したものである。
ロボット１の移動が旋回を含む移動である場合には、上記のようにして、新たな初期上体姿勢角および角速度を決定すればよい。
なお、定常歩容の境界条件を満たすような初期上体姿勢角速度を探索的に求めることも可能である。
図２０のＳ３１０の処理を完了した後、図１９のＳ２１０に進み、生成した歩容（仮定常歩容）の終端上体水平位置・速度を、その瞬間の支持脚に対応する支持脚座標系（図３１のＸ’’’，Ｙ’’’，Ｚ’’’座標系）から見た値に変換し、その値を（Ｘｅ，Ｖｘｅ）とする（Ｘｅ：終端上体水平位置、Ｖｘｅ：終端上体水平速度）。
次いでＳ２１２に進み、図示の如く、初期上体水平位置速度（Ｘｓ，Ｖｘｓ）と終端上体水平位置速度（Ｘｅ，Ｖｘｅ）との差を算出する。この差（Ｘｓ−Ｘｅ，Ｖｘｓ−Ｖｘｅ）を上体水平位置速度境界条件誤差（ｅｒｒｘ，ｅｒｒｖｘ）と呼ぶ。定常歩容では、前記境界条件を満たす必要があるので、（Ｘｓ，Ｖｘｓ）と（Ｘｅ，Ｖｘｅ）とが一致していなければならない。従って、上体水平位置速度境界条件誤差（ｅｒｒｘ，ｅｒｒｖｘ）が零またはほぼ零でなければならない。本実施形態では、以下のように、上体水平位置速度境界条件誤差（ｅｒｒｘ，ｅｒｒｖｘ）がほぼ零となる（Ｘｓ，Ｖｘｓ）が探索的に求められる。
次いでＳ２１４に進み、算出した上体水平位置速度境界条件誤差（ｅｒｒｘ，ｅｒｒｖｘ）が、あらかじめ適宜設定された許容範囲内にあるか否かが判断される。尚、このように上体水平位置速度境界条件誤差の許容範囲を設定する代わりに、初期発散成分（Ｘｓ＋Ｖｘｓ／ω０）と終端発散成分（Ｘｅ＋Ｖｘｅ／ω０）との差、および初期収束成分（Ｘｓ−Ｖｘｓ／ω０）と終端収束成分（Ｘｅ−Ｖｘｅ／ω０）との差がそれぞれある許容範囲内にあるか否か判断するようにしても良い。ただしω０は、前述したように、ある所定の値である。
Ｓ２１４の判断結果がＮＯであるときにはＳ２１６に進む。このＳ２１６では、（Ｘｓ，Ｖｘｓ）の付近に複数（本実施形態では２つ）の初期値候補（Ｘｓ＋ΔＸｓ，Ｖｘｓ），（Ｘｓ，Ｖｘｓ＋ΔＶｘｓ）が決定される。ここで、ΔＸｓ，ΔＶｘｓは、それぞれＸｓ，Ｖｘｓに対する所定の微小変化量を意味している。そして、これらの初期値候補のそれぞれを上体水平位置速度の初期状態として、前記Ｓ２０８と同様の処理によって定常歩容が歩容パラメータを用いて生成される。さらに、その生成された定常歩容の終端上体位置速度を、その瞬間の支持脚に対応する支持脚座標系（図３１のＸ’’’，Ｙ’’’，Ｚ’’’座標系）から見た値に変換した値（Ｘｅ＋ΔＸｅ１，Ｖｘｅ＋ΔＶｘｅ１），（Ｘｅ＋ΔＸｅ２，Ｖｘｅ＋ΔＶｘｅ２）が求められる。ここで、（Ｘｅ＋ΔＸｅ１，Ｖｘｅ＋ΔＶｘｅ１）は（Ｘｓ＋ΔＸｓ，Ｖｘｓ）に対応する終端上体位置速度を意味し、（Ｘｅ＋ΔＸｅ２，Ｖｘｅ＋ΔＶｘｅ２）は（Ｘｓ，Ｖｘｓ＋ΔＶｘｓ）に対応する終端上体位置速度を意味している。なお、この場合の定常歩容（仮歩容）の生成処理では、上体水平位置速度以外の変数の初期状態（時刻Ｔｓでの状態）は、例えば上体水平位置速度の初期値候補を（Ｘｓ，Ｖｘｓ）とした場合と同一に設定すればよい。Ｓ２１６ではさらに、前記Ｓ２１０と同様の処理によって、各初期値候補とこれに対応する終端上体位置速度との差、すなわち、各初期値候補（Ｘｓ＋ΔＸｓ，Ｖｘｓ），（Ｘｓ，Ｖｘｓ＋ΔＶｘｓ）のそれぞれに対応する上体水平位置速度境界条件誤差が求められる。
次いでＳ２１８に進み、（Ｘｓ，Ｖｘｓ）およびその付近の初期値候補（Ｘｓ＋ΔＸｓ，Ｖｘｓ），（Ｘｓ，Ｖｘｓ＋ΔＶｘｓ）のそれぞれに対する上体水平位置速度境界条件誤差を基に、（Ｘｓ，Ｖｘｓ）の次の初期値候補が探索法（擬似ヤコビアン（感度マトリックス）を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法など）により決定される。すなわち、（Ｘｓ，Ｖｘｓ）およびその付近の初期値候補（Ｘｓ＋ΔＸｓ，Ｖｘｓ），（Ｘｓ，Ｖｘｓ＋ΔＶｘｓ）のそれぞれに対する上体水平位置速度境界条件誤差によって、上体水平位置及び上体水平速度をそれぞれ初期値候補（Ｘｓ，Ｖｘｓ）から微小変化させたときの、上体水平位置速度境界条件誤差の変化度合いを示す感度マトリックスが求められ、その感度マトリックスに基づいて、上体水平位置速度境界条件誤差をより小さくするような初期値候補（Ｘｓ，Ｖｘｓ）が新たに決定される。そして、このように上体水平位置速度の新たな初期値候補（Ｘｓ，Ｖｘｓ）が決定された後、Ｓ２０６に戻る。
Ｓ２１４の判断結果がＮＯとなる限りは上記した処理（Ｓ２０６〜Ｓ２１８の処理）が繰り返される。なお、この場合、上体水平位置速度の新たな初期値候補（Ｘｓ，Ｖｘｓ）に対応する定常歩容の生成処理（Ｓ２０８）におけるＳ３００（図２０参照）では、上体姿勢角速度の初期値は、基準上体姿勢角速度の初期値ではなく、上体水平位置速度の前回の初期値候補（Ｘｓ，Ｖｘｓ）に対応するＳ２０８の処理におけるＳ３１０（図２０参照）で決定したものに設定される。そして、Ｓ２１４の判断結果がＹＥＳになると、繰り返しループ（Ｓ２０４）を抜けてＳ２２０に進む。なお、Ｓ２０４の繰り返しループを抜けたときの直前に生成された仮定常歩容が、境界条件を満たす定常歩容として得られることとなる。
Ｓ２２０では、本来の初期時刻０（今回歩容の終端時刻）における初期上体水平位置速度（Ｘ０，Ｖ０）と初期時刻０における初期上体鉛直位置速度（Ｚ０、Ｖｚ０）と初期時刻０における初期上体姿勢角および角速度が求められる。
具体的には、（Ｘ０，Ｖ０）と（Ｚ０，Ｖｚ０）は、前記図２１に示した定常歩容瞬時値決定サブルーチンのＳ４０８およびＳ４１４において、第２旋回歩容から第２旋回歩容に切り換わった瞬間の時刻、すなわち、時刻ｋ＝Ｔｃｙｃ（時刻Ｔｅ−Ｔｓ）であるときに決定された上体水平位置速度と上体鉛直位置速度とを、それぞれ時刻Ｔｃｙｃから始まる一歩（すなわち２度目の第１旋回歩容）の支持脚に対応する支持脚座標系（図３１のＸ’’’Ｙ’’’，Ｚ’’’座標系）から見た値に変換した値に決定される。同様に、初期状態姿勢角および角速度は、時刻ｋ＝Ｔｃｙｃ（時刻Ｔｅ−Ｔｓ）であるときに決定された上体姿勢角及び角加速度を、時刻Ｔｃｙｃから始まる一歩（すなわち２度目の第１旋回歩容）の支持脚に対応する支持脚座標系（図３１のＸ’’’，Ｙ’’’，Ｚ’’’座標系）から見た値に変換した値に決定される。
次いでＳ２２２に進み、定常歩容初期発散成分ｑ［０］が次式から求められる。
2003061917.tif 000034
ただしω０は、発散についての解説で述べたように、ある所定の値である。
さらに、Ｓ２２４に進み、定常歩容初期発散成分ｑ［０］が今回歩容支持脚座標系から見た値に変換され、これがｑ”［０］として求められる。また、初期上体鉛直位置速度（Ｚ０，Ｖｚ０）が今回歩容支持脚座標系から見た値に変換され、これが（Ｚ０”，Ｖｚ０”）として求められる。
補足すると、（Ｚ０”，Ｖｚ０”）は、第２旋回歩容の支持脚座標系（図１５のＸ”，Ｙ”，Ｚ”座標系）から見た第２旋回歩容終端上体鉛直位置速度に一致している。また、ｑ”［０］も第２旋回歩容の支持脚座標系（図１５のＸ”，Ｙ”，Ｚ”座標系）から見た第２旋回歩容終端発散成分に一致している。したがって、これらの性質を用いて、（Ｚ０”，Ｖｚ０”）およびｑ”［０］を算出するようにしても良い。
以上により、図１２のＳ０２４の処理、すなわち定常歩容の初期状態を求めるサブルーチン処理が終了する。
次いで、図１２のＳ０２６に進み、今回歩容の歩容パラメータを決定（一部は仮決定）する。Ｓ０２６では、より具体的には、図３２に示すフローチャートに従って、以下の処理が行われる。
まず、Ｓ６００において、今回歩容の足平位置姿勢軌道が定常歩容の足平位置姿勢軌道につながるように、今回歩容の足平軌道パラメータが設定される。
具体的には、今回歩容初期遊脚足平位置姿勢（今回歩容遊脚足平位置姿勢の初期値）は、今回歩容支持脚座標系から見た現在遊脚位置姿勢（前回歩容終端遊脚位置姿勢）に設定される。今回歩容初期支持脚足平位置姿勢（今回歩容支持脚足平位置姿勢の初期値）は、今回歩容支持脚座標系から見た現在支持脚足平位置姿勢（前回歩容終端支持脚足平位置姿勢）に設定される。また、今回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た次回歩容支持脚座標系（今回歩容に係わる１歩目の遊脚着床位置姿勢の要求値）に対応して決定される。すなわち、今回歩容終端遊脚足平位置姿勢から遊脚足平２２を床に接触させたまま、すべらないように該足平２２をその底面のほぼ全面が接地するまで回転させたときの、該足平２２の代表点が今回歩容支持脚座標系から見た次回歩容支持脚座標系の原点に合致するように今回歩容終端遊脚足平位置姿勢が決定される。
今回歩容終端では、支持脚足平２２は離床して空中にある。支持脚足平２２が離床してから後の軌道を決定するために、支持脚足平着地予定位置姿勢を設定する。支持脚足平着地予定位置姿勢は、今回歩容支持脚座標から見た次次回歩容支持脚座標（今回歩容に係わる２歩目の遊脚足平位置姿勢の要求値）に対応して設定される。より具体的には、支持脚足平着地予定位置姿勢は、その位置姿勢から、足平２２を床に接触させたまま、すべらないように該足平２２をその底面のほぼ全面が床に接地するまで回転させたときの該足平２２の代表点が今回歩容支持脚座標から見た次次回歩容支持脚座標の原点に合致するように設定される。
今回歩容終端支持脚足平位置姿勢は、現在支持脚位置姿勢（今回歩容初期支持脚足平位置姿勢）から、次回歩容支持脚座標系に対応する足平着地予定位置姿勢（前記要求パラメータにおける２歩目の遊脚側足平着地位置姿勢の要求値）に至る足平位置姿勢軌道を、今回歩容終端まで前記有限時間整定フィルタにより生成することにより求められる。
次いで、Ｓ６０２に進み、今回歩容の基準上体姿勢軌道パラメータが、定常歩容の第１旋回歩容や第２旋回歩容と同様に決定される。ただし、今回歩容の基準上体姿勢軌道が前記定常歩容の基準上体姿勢軌道に連続してつながる（今回歩容終端での基準上体姿勢角及び角速度がそれぞれ定常歩容初期の基準上体姿勢角及び角速度に一致する）ように上記パラメータが設定される。なお、本実施形態では、基準上体姿勢は、今回歩容及び定常歩容のいずれでも定常的な鉛直姿勢である。
次いで、Ｓ６０４に進み、今回歩容の腕姿勢軌道パラメータが、定常歩容の第１旋回歩容や第２旋回歩容と同様に決定される。ただし、今回歩容の腕姿勢軌道が前記定常歩容の腕姿勢軌道に連続してつながるように上記パラメータを設定する。なお、ここで決定される腕姿勢軌道パラメータは、定常歩容パラメータの決定（図１４のＳ１０４）の場合と同様、鉛直軸（あるいは上体体幹軸）まわりの両腕体の角運動量変化に関すること以外の運動パラメータであり、両腕体の重心位置の軌道を規定するパラメータである。
次いで、Ｓ６０６に進み、今回歩容の床反力鉛直成分軌道パラメータが、定常歩容の第１旋回歩容や第２旋回歩容と同様、該パラメータにより規定される床反力鉛直成分軌道が前記図６のように実質的に連続な（値がステップ状に飛ばない）軌道になるように設定される。
ただし、床反力鉛直成分軌道パラメータは、今回歩容の全体重心鉛直位置速度と床反力鉛直成分軌道とのいずれもが、前記定常歩容と連続してつながるように決定される。
具体的には、まず、前記図１２のＳ０２４の処理（定常歩容初期状態の決定処理）で最終的に求めた定常歩容の初期上体鉛直位置速度を今回歩容支持脚座標系から見た値に変換した値（Ｚ０”，Ｖｚ０”）、すなわち、図１９のＳ２２４で求めた（Ｚ０”，Ｖｚ０”）等を基に、例えば前記式０４（あるいはロボット１のキネマティクスモデル）を用いて、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置速度が求められる。具体的には、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置は、Ｓ２２４で求めた定常歩容の上体鉛直位置Ｚ０”に対応する図１０のモデルの上体質点鉛直位置と、定常歩容初期の各足平位置を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直位置とを式０４に代入することで求められる。また、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直速度は、Ｓ２２４で求めた定常歩容の上体鉛直速度Ｖｚ０”に対応する図１０のモデルの上体質点鉛直速度と、定常歩容初期の各足平鉛直速度を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直速度とを式０４の両辺を微分してなる式に代入することにより求められる。なお、この初期全体重心鉛直位置速度の算出は、より厳密なモデルを用いて行なうようにしてもよい。
そして、このようにして求めた定常歩容の初期全体重心鉛直位置速度を、前記式１３と下記式４１の終端全体重心鉛直位置速度に代入すると共に、前回の目標歩容瞬時値（より厳密には、前回目標歩容の終端状態を今回支持脚座標系に変換したもの）の全体重心鉛直位置および速度を、前記式１３と下記式４１の初期全体重心鉛直位置および速度に代入し、式１３と式４１の関係を満足するように、今回歩容の床反力鉛直成分パターン（より具体的にはパラメータ値）が決定される。ただし、式１３と式４１における積分値は今回歩容の初期から終端までの期間の積分値とする。
2003061917.tif 000035
ただし、重力加速度は負の値とする。
より具体的には、まず、図６のような床反力鉛直成分パターンを規定する床反力鉛直成分パラメータ（折れ点の時刻等）のうちの、少なくとも２つのパラメータを独立な未知変数として、その未知変数の値を、式１３と式４１とからなる連立方程式を解いて決定する。
未知変数とする床反力鉛直成分パラメータには、例えば、図６の台形の高さ（床反力鉛直成分のピーク値）と幅（片脚支持期時間）とを選択すれば良い。この場合、図６の台形の両側部分の傾きは今回歩容周期等に応じてあらかじめ定めた値とし、あるいは、片脚支持期から空中期に移行する時刻を除く床反力鉛直成分パターンの折れ点の時刻を今回歩容周期等に応じてあらかじめ定めた値とする。補足すると、未知変数を１つとすると、一般的には式１３と式４１の連立方程式を満足する解が存在しない。
次いで、Ｓ６０８に進み、床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］（具体的には該床反力水平成分許容範囲のパターンを規定するパラメータ）が、定常歩容の第１旋回歩容や第２旋回歩容と同様に設定される。たとえば図３３に示すようなパターンで床反力水平成分許容範囲が設定される。本実施形態では、先にＳ６０６で決定した床反力鉛直成分パターンに応じて前記式１２に基づいて床反力水平成分許容範囲（詳しくはＸ軸方向、Ｙ軸方向の各軸方向の許容範囲）が設定される。なお、ここで設定する床反力水平成分許容範囲は、確実に摩擦限界内に収まるように設定することが望ましい。
次いで、Ｓ６１０に進み、今回歩容のＺＭＰ軌道（具体的にはＺＭＰ軌道を規定するパラメータで、軌道の折れ点の時刻や位置）が、定常歩容の第１旋回歩容や第２旋回歩容と同様、安定余裕が高くかつ急激な変化をしないように、前記図７に示すごとく設定される。ただし、今回歩容のＺＭＰ軌道が前記定常歩容のＺＭＰ軌道に連続してつながるように上記パラメータを設定される。つまり、今回歩容終端におけるＺＭＰの位置が定常歩容初期のＺＭＰ位置に一致するように、ＺＭＰ軌道パラメータが決定される。この場合、走行歩容では、片脚支持期におけるＺＭＰ軌道の折れ点の時刻や位置の設定の仕方は、前述した定常歩容のＺＭＰ軌道パラメータの設定の仕方と同様でよい。そして、空中期における目標ＺＭＰ軌道が、空中期開始時から、定常歩容初期のＺＭＰ位置まで、直線的に連続して変化するようにＺＭＰ軌道パラメータを設定すればよい。
なお、Ｓ６１０で決定される今回歩容のＺＭＰ軌道パラメータは、仮決定されただけであり、後述するように修正される。そこで、上記のごとく設定した今回歩容のＺＭＰ軌道を、以降、今回歩容の仮目標ＺＭＰ軌道と呼ぶこととする。
最後に、Ｓ６１２に進み、上体姿勢角復元期間［Ｔａ，Ｔｂ］が設定される。上体姿勢角復元開始時刻Ｔａは、定常歩容の第２旋回歩容におけるＴｍに相当し、上体姿勢角復元修了時刻Ｔｂは、定常歩容の第２旋回歩容におけるＴｓ２に相当するものである。これらの時刻Ｔａ，Ｔｂの設定の仕方は、それぞれＴｍおよびＴｓ２の設定の仕方と同様である。
図１２の説明に戻って、上記のごとくＳ０２６に示す処理（今回歩容の歩容パラメータ決定処理）を行って後、次いでＳ０２８に進み、今回歩容の歩容パラメータ（ＺＭＰ軌道パラメータ）が修正される。この処理では、上体位置姿勢軌道を定常歩容に連続させ、もしくは近づけるべくＺＭＰ軌道パラメータが修正される。
図３４はその処理を示すサブルーチン・フロー・チャートである。
まずＳ７００を経てＳ７０２に進み、仮目標ＺＭＰパターンとその他の今回歩容パラメータを基に、今回歩容終端時刻までの仮今回歩容が仮生成される。
Ｓ７０２では、図３５に示すフローチャートに従って、以下の処理が行われる。
以下説明すると、Ｓ８００において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻ｋに０が代入される。また、前回目標歩容の終端状態（より具体的には、上体水平位置速度、上体鉛直位置速度、上体姿勢角及びその角速度、目標足平位置姿勢、目標腕姿勢などの歩容状態の終端値）を今回支持脚座標系に変換したものを、今回歩容初期状態とする。
次いで、Ｓ８０２を経てＳ８０４において、仮歩容生成用時刻ｋが今回歩容終端時刻Ｔｃｕｒｒ以前であるか否か（ｋ≦Ｔｃｕｒｒであるか否か）が判断され、その判断結果がＹＥＳある場合には、Ｓ８０６の今回歩容瞬時値決定サブルーチンに進み、今回歩容の時刻ｋの瞬時値が決定される。次いでＳ８０８に進み、仮歩容生成用時刻ｋがΔｋだけ増加され、Ｓ８０４に戻る。
Ｓ８０４の判断結果がＮＯである場合には、図３５に示すフローチャートの処理が完了する。
以上の処理により、仮の今回歩容が初期から終端まで生成される。
Ｓ８０６の歩容瞬時値決定サブルーチンでは、図３６に示す処理が行われる。
以下にこれを説明する。
まずＳ９００において、今回歩容パラメータの床反力鉛直成分軌道パラメータを基に、図６に示した目標床反力鉛直成分の時刻ｋにおける値（今回値）が求められる。次いで、Ｓ９０２において、今回歩容パラメータのＺＭＰ軌道パラメータを基に、図７に示す目標ＺＭＰ軌道（仮目標ＺＭＰ軌道）の時刻ｋにおける値（今回値）を求める。
次いでＳ９０４に進み、今回歩容パラメータ（足平軌道パラメータ、基準上体姿勢軌道パラメータおよび腕姿勢軌道パラメータ）を基に、時刻ｋにおける目標両足平位置姿勢、基準上体姿勢および目標腕姿勢の値（今回値）がそれぞれ求められる。ただし、目標腕姿勢に関しては、腕全体重心位置は決定されるが、鉛直軸（または上体体幹軸）まわりの角運動量を変化させる運動自由度は、未だ決定されていない。なお、定常歩容の場合と同様、今回歩容の目標足平位置姿勢軌道及び床反力鉛直成分軌道は、それぞれ、図３２のフローチャートで決定した足平軌道パラメータ、床反力鉛直成分軌道パラメータに応じて互いに独立的に決定される。
次いでＳ９０６に進み、Ｓ９００で求めた目標床反力鉛直成分の今回値を満足する（ロボット１の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる）ような時刻ｋにおける全体重心鉛直位置が算出される。この算出は、具体的には前記図２１のＳ４０６と同様の処理により行なわれる。
次いでＳ９０８に進み、上記の如く求めた全体重心鉛直位置を満足する上体鉛直位置の今回値が算出される。この算出は、具体的には、前記図２１のＳ４０８と同様の処理により行なわれる。
次いでＳ９１０に進み、前記図３２のＳ６０８で決定した床反力水平成分許容範囲のパラメータに基づいて、図３３に示す床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］の時刻ｋにおける値（今回値）が求められる。
次いでＳ９１２に進み、目標ＺＭＰを満足する（目標ＺＭＰ回りに発生するモーメント水平成分が０になる）ように、今回仮歩容の上体水平加速度と上体姿勢角加速度が決定される。ただし、これらの加速度は、床反力水平成分Ｆｘが［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えないように決定される。この処理は、図２１のＳ４１２の詳細を示す図２２のフローチャートの処理とは若干異なる。
Ｓ９１２では、具体的には、図３７のフローチャートに示す処理が行われる。図３７のフローチャートでは、まずＳ１０００おいて、時刻ｋが前記図３２のＳ６１２で設定した上体姿勢角復元期間［Ｔａ，Ｔｂ］にあるか否かが判断される。
この判断結果がＮＯである場合（床反力水平成分許容範囲が０もしくは狭い場合）には、Ｓ１００２からＳ１０１４までの処理が実行される。Ｓ１００２からＳ１０１４までの処理は、前述の図２２のＳ５０４からＳ５１６までの処理と同じであるので、説明を省略する。
Ｓ１０００の判断結果がＹＥＳである場合には、Ｓ１０１６に進み、上体姿勢復元モーメントＺＭＰ換算値パターンＺＭＰｒｅｃが設定される。この場合、定常歩容の生成に係わる前記図２２のＳ５１８と異なり、ＺＭＰｒｅｃは、一定値０のパターン（すなわち図２６に示す台形パターンの高さが０であるパターン）に設定される。次に、Ｓ１０１８からＳ１０２６までの処理が行われる。Ｓ１０１８からＳ１０２６までの処理は、前記図２２のＳ５２０からＳ５２８までの処理と同じであるので説明を省略する。なお、Ｓ１０１６〜Ｓ１０２６の処理では、上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃの瞬時値は常に０になるので、Ｓ１０２０で求められる上体回転モードの上体姿勢角加速度βも０になる。従って、β＝０として、Ｓ１０２２、Ｓ１０２４及びＳ１０２６の処理のみを行なうようにしてもよい。Ｓ１０００の判断結果がＹＥＳである場合、すなわち、上体姿勢角復元期間［Ｔａ，Ｔｂ］では、上記のようにβ＝０となることから、Ｓ９１２の処理では、上体姿勢角復元期間［Ｔａ，Ｔｂ］の期間（床反力水平成分許容範囲が十分に大きい期間）で、定常歩容生成の場合と異なり、上体姿勢角速度の復元方向への上体姿勢変化の運動軌道は生成されないこととなる。
なお、本実施形態では、基準上体姿勢が鉛直姿勢で、基準上体姿勢による上体姿勢角加速度が０であるため、上記の如く求めた上体回転モードの角加速度βをＳ９１２の処理で決定される上体姿勢角加速度とした。但し、基準上体姿勢が変化するように該基準上体姿勢軌道パラメータを設定した場合には、上記の如く求めた上体回転モードの角加速度βと基準上体姿勢角加速度の和をもって、Ｓ９１２の処理で決定される上体姿勢角加速度とすべきである。
以上のごとく図３６のＳ９１２の処理を完了した後、次いでＳ９１４に進み、上記Ｓ９１２で求めた上体水平加速度を逐次積分する（今回歩容初期（時刻ｋ＝０）から現在時刻ｋまで累積加算する）ことによって上体水平速度が求められ、さらにその上体水平速度を逐次積分する（今回歩容初期（時刻ｋ＝０）から現在時刻ｋまで累積加算する）ことによって、上体水平位置（今回値）が求められる。また、上記Ｓ９１２で求めた上体姿勢角加速度を逐次積分する（今回歩容初期（時刻ｋ＝０）から現在時刻ｋまで累積加算する）ことによって、上体姿勢角速度が求められ、さらにその上体姿勢角速度を逐次積分する（今回歩容初期（時刻ｋ＝０）から現在時刻ｋまで累積加算する）ことによって、上体姿勢角の今回値が求められる。このＳ９１４の処理は図２１のＳ４１４と同様の処理である。
以上のごとく図３５のＳ８０６の今回歩容瞬時値決定サブルーチンが実行され、次いでＳ８０８に進み、歩容生成用時刻ｋの値が歩容生成インターバルΔｋだけ増加される。次いでＳ８０４に戻り、Ｓ８０４に示す条件が成立している限り、Ｓ８０６とＳ８０８の処理が繰り返される。そして、Ｓ８０４に示す条件が成立しなくなったら、すなわち終端まで仮今回歩容の生成が完了したら、図３４のＳ７０４に進む。
Ｓ７０４では、上記のごとくＳ７０２で求めた今回歩容終端での上体水平位置速度（Ｘｅ，Ｖｘｅ）から、終端発散成分ｑ０［ｋ］（ｋ＝Ｔｃｕｒｒ）が図示の式（前記式１０）を用いて求められる。
次いでＳ７０６に進み、今回歩容終端発散成分ｑ０［ｋ］と定常歩容初期発散成分ｑ”（図１９のＳ２２４で求めたもの）との差である終端発散成分誤差ｅｒｒｑが図示の式を用いて求められる。さらに、Ｓ７０８に進んで、その求めた終端発散成分誤差ｅｒｒｑが許容範囲内（０近傍の範囲）にあるか否か判断する。
Ｓ７０８の判断結果がＮＯであるときはＳ７１０に進み、ａ＝Δａ（Δａは所定の微小量）とし、図３８の関係に従って現在の仮目標ＺＭＰパターンに台形状の修正を加えた目標ＺＭＰを基に、前記Ｓ７０２と同様に、終端までの仮今回歩容を計算する。ここで、図３８を参照して、ａは、今回歩容終端発散成分を定常歩容初期発散成分にできるだけ一致させる（今回歩容の上体水平位置姿勢軌道を定常歩容の上体水平位置姿勢軌道に近づける）ために、仮目標ＺＭＰを修正するための台形パターンの高さである。この場合、本実施形態では、仮目標ＺＭＰの修正は、支持脚足平２２の底面のほぼ全面が接地する期間（足底全面接地期間）、すなわち、床反力水平成分許容範囲が十分に広いものとなる期間において行なうようにしており、上記台形パターンの折れ点の時刻は、足底全面接地期間における仮目標ＺＭＰの折れ点の時刻に合わせて設定されている。なお、Ｓ７１０でａ＝Δａとするのは、現在の仮目標ＺＭＰ軌道を上記台形パターンによって微小量、修正したときの前記終端発散成分誤差ｅｒｒｑの変化を観測するためである。
上記のようにＳ７１０にて、ａ＝Δａとして仮目標ＺＭＰ軌道を修正したときの仮今回歩容を終端まで生成した後、さらにＳ７１２に進み、Ｓ７１０で求めた仮今回歩容終端での上体水平位置速度（Ｘｅ１，Ｖｘｅ１）を基に、この仮今回歩容における終端発散成分ｑ１［ｋ］が図示の式（前記式１０）を用いて求められる。
尚、Ｓ７１０においてΔａは、本実施形態では、適宜に設定される微小量の定数としたが、以下に説明する繰り返し演算によって終端発散成分誤差ｅｒｒｑが小さくなるにつれてΔａを減少させるように該Δａを設定するようにしても良い。ただし、定数にしておいても、数回の繰り返し演算で終端発散成分誤差ｅｒｒｑを許容範囲内に収めることができる。
次いでＳ７１４に進み、パラメータ感度ｒ（Δａに対する終端発散成分誤差の変化の割合）が図示の式から求められる。さらに、Ｓ７１６に進み、ａ＝−ｅｒｒｑ／ｒ、すなわちＳ７０６で求めた終端発散成分誤差ｅｒｒｑを、Ｓ７１４で求めたパラメータ感度ｒで割った値を高さａとする台形パターンの修正量を図３８の関係に従って仮目標ＺＭＰパターンに加えることにより、仮目標ＺＭＰパターンが修正される（新たな仮目標ＺＭＰパターンが決定される）。
次いでＳ７０２に戻る。そして、Ｓ７０８の判断結果がＮＯである限りは、上記したＳ７０２〜Ｓ７１６の処理が繰り返され、Ｓ７０８の判断結果がＹＥＳになったときには、その繰り返しループ（Ｓ７００）を抜けてＳ７１８に進む。
Ｓ７１８では、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差、および仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差などを基に、今回歩容の上体姿勢角が定常歩容の上体姿勢角に近づくように、今回歩容の上体姿勢復元モーメントＺＭＰ換算値（ＺＭＰｒｅｃ）のパターンが決定される。ここで決定するＺＭＰｒｅｃは、後述する今回歩容瞬時値の生成処理で、床反力水平成分許容範囲が十分に大きいものとなる期間（片脚支持期内の期間）において、上体姿勢角軌道を定常歩容につながらせる（近づける）ように上体姿勢角加速度を発生させても、今回歩容終端発散成分と定常歩容初期発散成分との一致性（Ｓ７０８の条件）を維持できるように、仮目標ＺＭＰを修正するためのものである。
このＺＭＰｒｅｃは、前記定常歩容生成処理で説明したものと同様に台形状のパターンであり、具体的には次のように決定される。すなわち、図２６に示したＺＭＰｒｅｃの台形パターンにおいて、台形の頂点（折れ点）の時刻は既知とする（より具体的には台形の折れ点の時刻を目標ＺＭＰの折れ点時刻に合わせる）と共に、台形高さを未知数とし、以下のようにＺＭＰｒｅｃの台形高さ（パラメータ）が求められる。ただし、Ｓ７１８では図２６におけるＴｍおよびＴｓ２をそれぞれＴａ，Ｔｂに置き換えるものとする。
上記のように上体姿勢復元モーメントＺＭＰ換算値パターンの未知パラメータがひとつの場合には、今回歩容の終端において上体姿勢角と上体姿勢角速度の両方を定常歩容に連続につなげることは一般的にはできない。そこで、本実施形態では、複数歩に渡って徐々に、生成される歩容の状態が定常歩容の状態に近づけるように、未知パラメータを決定することとする。
補足すると、１歩容の中でのＺＭＰｒｅｃパターンを複雑にし、未知パラメータを２つ以上にして、今回歩容の終端において上体姿勢角と上体姿勢角速度との両者を定常歩容に連続につなげても良いが、ＺＭＰｒｅｃパターンがジグザグに大きく変動し過ぎる恐れがある。
以下では、算出原理を説明した後、算出手順を説明する。
前述のごとくＳ７０２でＺＭＰｒｅｃパターンの台形高さを０として求めた仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差を求め、これをθｅｒｒとする。また、仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差を求め、これをｖθｅｒｒとする。
ここで、ＺＭＰｒｅｃパターンの台形高さをある値ｂｃｕｒｒとして今回歩容が生成され、引き続いて、今回歩容と同様のアルゴリズムによって第１旋回歩容が生成されると想定する。ただし、第１旋回歩容の上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃパターンは、図２２のＳ５１８において求められた第１旋回歩容のＺＭＰｒｅｃパターン（高さが前記ａｃｙｃ１である図２６の台形パターン）とある値ｂ１の和であるとする。
このようにして生成された歩容をＺＭＰｒｅｃ修正歩容と呼び、その終端（第１旋回歩容の終端）上体姿勢角と角速度をそれぞれθ１、ｖθ１とする。
Ｓ０２４における定常歩容の初期状態を求めるサブルーチンの処理が完了した時点で求められた本来の定常歩容（最終的にＳ３１０の式３０、式３１によって決定された定常歩容初期上体姿勢角および角速度を初期値とし、ＺＭＰｒｅｃパターンをＳ５１８において求められたパターン（高さがａｃｙｃ１である図２６の台形パターン）とした場合の定常歩容）の第１旋回歩容終端上体姿勢角と角速度をそれぞれθ１ｏｒｇ、ｖθ１ｏｒｇとする。
ここで、Δθ１、Δｖθ１を次のように定義する。
2003061917.tif 000036
2003061917.tif 000037
Δθ１およびΔｖθ１は、ＺＭＰｒｅｃ修正歩容と本来の定常歩容を第１旋回歩容の終端まで生成した時点において、両者間における上体姿勢角の差及び角速度の差を意味する。もし、Δθ１およびΔｖθが０になれば、ＺＭＰｒｅｃ修正歩容に引き続いて、今回歩容と同様のアルゴリズムによって、ＺＭＰｒｅｃパターンの台形高さを前記ａｃｙｃ２として第２旋回歩容が生成されると、この歩容は、本来の定常歩容に一致する。
したがって、Δθ１およびΔｖθ１が０となる今回歩容台形高さｂｃｕｒｒ、第１旋回歩容台形高さｂ１を求め、求めたｂｃｕｒｒを今回歩容の台形高さとして最終的に決定すれば良い。
ところで、ロボット１の上体姿勢角に関する動力学モデルが図１０に示すフライホイールＦＨのような線形特性を持つことから、Δθ１およびΔｖθは、今回歩容台形高さｂｃｕｒｒ、第１旋回歩容台形高さｂ１、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角の差θｅｒｒ、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角速度の差ｖθｅｒｒと以下の関係を持つ。
2003061917.tif 000038
だだし、ｃ１１、ｃ１２、ｃ２１、ｃ２２、ｅ１、ｅ２は、今回歩容、第１旋回歩容の歩容周期、上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃパターンのパラメータ（特に時間に関するパラメータ）などによって一義的に決定される係数である。
以上の原理に基づき、算出手順として、まず、仮今回歩容と定常歩容との境界での上体姿勢角の差θｅｒｒと角速度の差Ｖθｅｒｒが求められる。
次いで、式５２，５３の係数であるｃ１１、ｃ１２、ｃ２１、ｃ２２、ｅ１、ｅ２が、今回歩容、第１旋回歩容の歩容周期、上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃパターンのパラメータ（特に時間に関するパラメータ）などを基に求められる。
次いで式５２、式５３の右辺が０となるように今回歩容台形高さｂｃｕｒｒ、第１旋回歩容台形高さｂ１が決定される。すなわち、式５２、式５３の右辺を０とした連立方程式を解くことによって、ｂｃｕｒｒ、ｂ１が求められる。
最後に、今回歩容の上体姿勢復元モーメントＺＭＰ換算値（ＺＭＰｒｅｃ）の台形パターンの台形高さが、上記求めた今回歩容台形高さｂｃｕｒｒに設定される。
次いでＳ７２０に進み、現在の仮目標ＺＭＰパターン（Ｓ７００の繰り返しループを抜けたときの仮目標ＺＭＰパターン）に、Ｓ７１８で上記のように求めた上体姿勢復元モーメントＺＭＰ換算値パターンを加算してなるパターンが今回歩容の目標ＺＭＰパターンとして決定される。なお、この処理は、Ｓ７１０においてΔａの高さの台形パターンを仮目標ＺＭＰパターンに加える処理と同様である。
以下に仮目標ＺＭＰパターンに上体姿勢復元モーメントＺＭＰ換算値パターンを加算した理由を述べる。
Ｓ７００のループにおいて生成される仮今回歩容は、前述の如く、上体姿勢復元モーメントＺＭＰ換算値ＺＭＰｒｅｃを０（ＺＭＰｒｅｃの台形パターンの高さパラメータを０）にして生成される。このようにしてＳ７００のループで最終的に生成された仮今回歩容では、その上体位置速度は、定常歩容に連続し、もしくは近づくが、上体姿勢角は、定常歩容の上体姿勢角からずれ、場合によっては発散してしまう。
Ｓ７１８で求められた上体姿勢復元モーメントＺＭＰ換算値パターンは、上体姿勢角の定常歩容に対するずれを０に近づけるための上体姿勢角加速度を発生させるものである。
しかし、Ｓ７００のループで最終的に得られた仮目標ＺＭＰパターンを修正することなく、Ｓ７１８で求められた上体姿勢復元モーメントＺＭＰ換算値パターンに応じた上体姿勢角加速度を発生させると、動力学的平衡条件（ロボットの重力と慣性力の合力が目標ＺＭＰに作用するモーメントが鉛直成分を除いて０になること）を満足させるために、上体水平位置軌道を前記仮今回歩容の上体水平位置軌道からずらさざるを得なくなる。そこで、本実施例では、上体水平位置軌道をＳ７００のループで最終的に得られるものからずらさなくて済むように、ＺＭＰｒｅｃによって仮目標ＺＭＰパターンを修正することとした。
前記仮今回歩容の運動に加えて、Ｓ７１８で求めた上体姿勢復元モーメントＺＭＰ換算値パターンに対応する上体姿勢角加速度を発生させると、ＺＭＰ（運動によって発生する重力と慣性力の合力の鉛直成分を除くモーメントが０になる点）は上体姿勢復元モーメントＺＭＰ換算値だけずれる。したがって、逆に、仮目標ＺＭＰパターンに上体姿勢復元モーメントＺＭＰ換算値パターンを加算したパターンを目標ＺＭＰパターンとして、Ｓ７１８で求めた上体姿勢復元モーメントＺＭＰ換算値パターンに対応する上体回転モードの上体姿勢角加速度を発生させながら、目標ＺＭＰパターンを満足する今回歩容を生成すれば、前記仮今回歩容と同じ上体並進運動となる。
以上が、仮目標ＺＭＰパターンに上体姿勢復元モーメントＺＭＰ換算値パターンを加算したパターンを目標ＺＭＰパターンとした理由である。
図１２に戻って、上記のごとくＳ０２８において今回歩容パラメータを修正した後、Ｓ０２９に進み、目標ＺＭＰまわりの床反力モーメント（より詳しくは床反力モーメントの水平成分）の許容範囲を規定するパラメータが決定される。なお、床反力モーメントを、床反力鉛直成分で割ったものが、ＺＭＰ（床反力中心点）の目標ＺＭＰからのずれ量を表すものとなる。従って、床反力モーメント許容範囲を、床反力鉛直成分で割ることによって、床反力中心点に変換したＺＭＰ許容範囲（床反力中心点許容範囲）のパラメータを設定するようにしてもよい。
Ｓ０２９で決定する床反力モーメント許容範囲は、前記複合コンプライアンス動作決定部１０４の制御処理（コンプライアンス制御）によって制御される目標ＺＭＰまわりの実床反力モーメントの許容範囲である。この床反力モーメント許容範囲について以下に説明する。
複合コンプライアンス動作決定部１０４の処理によるコンプライアンス制御は、目標ＺＭＰまわりに発生する床反力モーメントがコンプライアンス制御用目標床反力モーメントになるように足平２２の位置姿勢を制御する。コンプライアンス制御が目標通りに忠実に作動すると、実床反力中心点は、目標床反力モーメントを鉛直床反力で割った値を目標ＺＭＰに加えた点もしくは目標ＺＭＰから減じた点にずれる。実床反力中心点は、いわゆる支持多角形で表されるＺＭＰ存在可能範囲（厳密な表現では、実床反力作用点の存在可能範囲）を越えることができないが、実床反力中心点が越えそうになると（すなわちＺＭＰ存在可能範囲の境界に近づき過ぎると）、足平２２の本来の接地感を低下させたり、足平２２の底面が浮いてしまうような不具合が発生する。
床反力モーメント許容範囲は、このような不具合を防止するために、コンプライアンス制御の目標床反力モーメントに制限を加えるものである。厳密には、床反力モーメント許容範囲は、後述の第２実施形態で説明する復元条件１のために設定するが、本実施形態のように前記式ｄ２７を満足させる場合には、コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲を越えないように制限することと等価になる。
したがって、床反力モーメント許容範囲は、これを床反力鉛直成分で割ることによって求められる床反力中心点許容範囲が、ＺＭＰ存在可能範囲に含まれるように決定されることが望ましい。
より具体的には、床反力中心点許容範囲（ＺＭＰ許容範囲）は、支持多角形に応じて決定されるべきで、一般的には、複雑な形状になるが、ここでは、演算を簡素化するために、基本的には床面上の長方形の領域とする。すなわち、床反力中心点許容範囲、ひいては、これを床反力モーメントに変換してなる床反力モーメント許容範囲が、前後方向（Ｘ軸方向）成分と左右方向（Ｙ軸方向）成分とでそれぞれ独立に範囲が設定される。
例えば、支持多角形と目標ＺＭＰが図３９のような状況にある場合には、図示のごとく、ＺＭＰ許容範囲は支持多角形に内包されるように設定される。また同時に、ＺＭＰ許容範囲は目標ＺＭＰを内包するように設定される。なお、図３９の例での支持多角形は、歩行歩容（歩行歩容に関する具体的な歩容生成については後に補足説明を行なう）における両脚支持期のものであり、両端の長方形部分が各脚体２の足平２２の位置を表しており、右足平２２はつま先が接地、左足平２２は踵が接地しているものとする。
歩行歩容あるいは前記図５の走行歩容における片脚支持期では、支持多角形は、支持脚足平２２の接地面（床面との接触面）である。この場合においては、ＺＭＰ許容範囲は、目標ＺＭＰを内包しつつ、該支持多角形に内包され、もしくは、その支持多角形とほぼ一致するように設定される。また、前記図５の走行歩容の空中期では、ＺＭＰ許容範囲は、例えば目標ＺＭＰを中心として、Ｘ軸、Ｙ軸両方向に幅が０の領域、すなわち、目標ＺＭＰそのものの点（これは面積が０の領域を意味する）に設定される。
図１２に戻って、上記のごとくＳ０２９において床反力モーメン許容範囲を決定した後、あるいはＳ０１６の判断結果がＮＯである場合には、Ｓ０３０に進み、今回歩容瞬時値が決定される。Ｓ０３０では、目標ＺＭＰまわりに前記式ｄ２６に従って決定されるモデル操作床反力モーメントが発生するように歩容瞬時値が決定される。
具体的には、図４０、図４１のフローチャートにしたがって歩容瞬時値が決定される。すなわち、Ｓ０３０では、まず、図４０のＳ１０００からＳ１０１０までの処理が実行される。これらのＳ１０００からＳ１０１０までの処理は、前述した図３６のＳ９００からＳ９１０までの処理と全く同一である。
次いで、Ｓ１０１１に進み、前記図１２のＳ０２９で決定した床反力モーメント許容範囲パラメータを基に、床反力モーメント許容範囲の瞬時値（現在時刻ｔの今回値）が求められる。この求められた床反力モーメント許容範囲は、前記補償全床反力モーメント分配器１１０（図３参照）に送られる。そして、この分配器１１０で前記式ｄ２６により算出されたモデル操作床反力モーメントの今回値（現在時刻ｔの値）が歩容生成装置１００に与えられる。
次いで、歩容生成装置１００の処理は、Ｓ１０１２に進み、補償全床反力モーメント分配器１１０から与えられたモデル操作床反力モーメントが目標ＺＭＰまわりに発生するように、今回歩容の上体水平加速度と上体姿勢角加速とが決定される。ただし、床反力水平成分Ｆｘが、前記図３２のＳ６０８で決定された床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えないように上体水平加速度および上体姿勢角加速度が決定される。
言い換えると、慣性力と重力との合力が目標ＺＭＰまわりに作用するモーメントがモデル操作床反力モーメントの符号を反転したモーメントになるように、今回歩容の上体水平加速度と上体姿勢角加速とが決定される。ただし、慣性力水平成分の符号を反転した力が床反力水平成分許容範囲［Ｆｘｍｉｎ，Ｆｘｍａｘ］を越えないように上体水平加速度および上体姿勢角加速度が決定される。
Ｓ１０１２では、具体的には、図４１に示すフローチャートに従って、上体水平加速度と上体姿勢角加速度とが決定される。これを説明すると、まずＳ１１００において、現在時刻ｔが前記図３３のＳ６１２の処理に関して説明した上体傾斜角復元期間［Ｔａ，Ｔｂ］にあるか否か、すなわち、床反力鉛直成分が十分に大きく、床反力水平成分が十分に大きく発生することができる期間であるか否かが判定される。
Ｓ１１００の判断結果がＮＯである場合には、Ｓ１１０２に進み、仮に上体並進モードの運動をさせた場合に、今回時刻ｔの目標ＺＭＰまわりにモデル操作床反力モーメントが発生するために必要な上体水平加速度αｔｍｐを求める。このαｔｍｐは、例えば前記図１０の動力学モデルに係わる前記式０３を用いて求められる。より具体的には、例えば、現在時刻ｋまでに求められた目標足平位置姿勢の時系列値を用いて、現在時刻ｋにおける支持脚質点２ｍ及び遊脚質点２ｍの加速度が求められると共に、現在時刻ｋ（今回）の目標足平位置姿勢を用いて支持脚質点２ｍ及び遊脚質点２ｍの位置が求められる。また、現在時刻ｋ（今回）の目標床反力鉛直成分を用いて現在時刻ｋの目標全体重心鉛直位置が求められ、支持脚質点２ｍ及び遊脚質点２ｍの鉛直位置と目標全体重心鉛直位置とから上体質点２４ｍの鉛直位置が求められると共に、現在時刻ｋまでに求めた目標上体鉛直位置の時系列値を用いて現在時刻ｋにおける上体質点２４ｍの鉛直加速度が求められる。そして、これらの求めた値を前記式０３に代入すると共に、同式０３のＭｙをモデル操作床反力モーメントとすると共に同式０３のｄ２θｂｙ／ｄｔ２を０にしてなる式を、ｄ２Ｘｂ／ｄｔ２について解くことにより、上体質点水平加速度ｄ２Ｘｂ／ｄｔ２が上体水平加速度αｔｍｐとして求められる。尚、より厳密な動力学モデルを用いて、目標ＺＭＰ回りの床反力モーメントの水平成分をモデル操作床反力モーメントにするような上体水平加速度αｔｍｐを探索的に求めるようにしてもよい。また、本実施形態では、基準上体姿勢が鉛直姿勢で、基準上体姿勢による上体姿勢角加速度が０であるため、上体回転モードの角加速度を０にして上体水平加速度αｔｍｐを求めるようにした。但し、基準上体姿勢が変化するように該基準上体姿勢軌道パラメータを設定した場合で、それにより定まる現在時刻ｋにおける基準上体姿勢角加速度が０でない場合には、上体回転モードの角加速度をその０でない基準上体姿勢角加速度にして、上体水平加速度αｔｍｐを動力学モデルを用いて求める（例えば式０３のｄ２θｂｙ／ｄｔ２を基準上体姿勢角加速度に設定して上記と同様に上体水平加速度αｔｍｐを求める）ようにすればよい。
次いでＳ１１０４からＳ１１１４まで進み、前記図３７のＳ１００４からＳ１０１４までと同一の処理が実行される。
Ｓ１１００の判断結果がＹＥＳである場合には、Ｓ１１１６に進み、上体姿勢復元モーメントＺＭＰ換算値（ＺＭＰｒｅｃ）パターンが設定される。このＳ１１００では、ＺＭＰｒｅｃパターンは、図３７のＳ１０１６と異なり、図３４のＳ７１８で設定した上体姿勢復元モーメントＺＭＰ換算値（ＺＭＰｒｅｃ）パターンに設定される。
次いで、Ｓ１１１８およびＳ１１２０に進み、図３７のＳ１０１８およびＳ１０２０と同じ処理が実行される。但し、この場合、ＺＭＰｒｅｃパターンは０でないので、Ｓ１１２０で求められる上体姿勢復元用の上体姿勢角加速度βは一般的には０にはならない。
次いで、Ｓ１１２２に進み、仮に上体並進モードの運動をさせた場合に今回時刻の目標ＺＭＰまわりにモデル操作床反力モーメントが発生するために必要な上体水平加速度αｔｍｐが求められる。この処理は、Ｓ１１０２の処理と同様である。
次いで、Ｓ１１２４およびＳ１１２６に進み、図３７のＳ１０２４およびＳ１０２６と同一の処理が実行される。これにより図４０のＳ１０１２の処理が完了する。
以上のごとくＳ１０１２の処理を行った後、Ｓ１０１４に進み、図３６のＳ９１４と同一の処理が実行され、今回歩容の上体水平位置および上体姿勢角（詳しくはそれらの現在時刻ｔにおける今回値）が決定される。これにより、図１２のＳ０３０の処理が完了する。
次いでＳ０３２に進み、スピン力をキャンセルする（ロボット１の腕以外の運動によって目標ＺＭＰ回りに発生する床反力モーメント鉛直成分を略零にする）ための腕動作が決定される。具体的には、腕を振らなかった場合の目標ＺＭＰにおける床反力モーメント鉛直成分軌道（厳密には、腕を振らずに歩容を生成した場合において、ロボットの重力と慣性力の合力が目標ＺＭＰに作用するモーメント鉛直成分軌道の各瞬時値の符号を反転したもの）が求められる。すなわち、Ｓ０３０の処理によって生成された歩容の運動（これには腕振りの運動は含まれていない）の瞬時値に釣り合う目標ＺＭＰ（瞬時値）回りの床反力モーメント鉛直成分の瞬時値が求められる。そして、これを、腕振り運動の等価慣性モーメントで割ることにより、スピン力キャンセルに必要な腕振り動作の角加速度が求められる。なお、補足すると、腕の振りが大き過ぎる場合には、等価慣性モーメントよりも大きな値で割れば良い。
次に、この角加速度を２階積分し、これを積分値が過大になるのを防ぐためのローカットフィルタに通して得た角度を腕振り動作角とする。ただし、腕振り動作では、左右の腕を前後逆方向に振り、両腕体の重心位置を変化させないようにする。なお、スピン力をキャンセルするための腕振り運動を定常歩容でも生成しておき、これにつながるように、今回歩容における腕振り運動を決定するようにしてもよい。
次いでＳ０３４に進み、時刻ｔに制御周期Δｔを加え、再び、Ｓ０１４に戻り、制御周期毎のタイマー割り込みを待つ。
なお、以上説明した目標歩容の生成処理においては、前記図５の走行歩容を例にとって説明したが、ロボット１の歩行歩容を生成する場合でも、上記と同様に、目標歩容を生成できる。この場合には、床反力鉛直成分軌道（１歩分の軌道）は、例えば図４２に示すような折れ線状のパターンで設定すればよい。この例では、より詳しくは、歩行歩容用の床反力鉛直成分軌道は、両脚支持期では、床反力鉛直成分の増加側に凸（上に凸）の台形状に設定され、片脚支持期では床反力鉛直成分の減少側に凸（下に凸）の台形状に設定される。この場合、床反力鉛直成分軌道の２つの台形部分の高さＣ１，Ｃ２等は、次のように決定することが好ましい。すなわち、本願出願人が先に提案した上体高さ決定法（特開平１０−８６０８０号公報。より具体的には同公報の図６の手法等）を用いて、ロボット１の脚体２，２の関節（具体的には膝関節）の変位に関する所定の幾何学的条件（各脚体２，２が伸び切ることが無いようにするための条件）を満たす上体鉛直位置軌道を基準上体鉛直位置軌道として求めておく。そして、床反力鉛直成分軌道を歩容初期から歩容終端まで２階積分して得られるロボット１の全体重心鉛直位置軌道に対応する上体鉛直位置軌道の形状的な特徴量（例えば最大値と最小値との差）が、上記基準上体鉛直位置軌道の特徴量（例えば最大値と最小値との差）に合致するように、床反力鉛直成分軌道の２つの台形部分の高さＣ１，Ｃ２を決定する。尚、この場合、走行歩容用の床反力鉛直成分軌道の設定の場合と同様、歩行歩容用の床反力鉛直成分軌道においても、その１周期内の平均値がほぼロボット１の自重に一致するようにする。
以上のごとく、歩容生成装置１００における目標歩容生成処理が行なわれ、目標上体位置姿勢、目標足平位置姿勢、目標腕姿勢、目標ＺＭＰおよび目標全床反力の瞬時値が逐次決定されて出力される。この場合、目標全床反力は、コンプライアンス制御に必要な成分だけを出力しても良い。なお、目標ＺＭＰは、目標全床反力に含まれるが特に重要であるので、あえて出力として挙げた。また、複合コンプライアンス制御装置（図４の点線で囲った部分）に対しては、モデル操作床反力モーメントは、目標床反力として出力されない。すなわち、コンプライアンス制御に対しては、目標ＺＭＰまわりの床反力モーメントが０になることを目標とする目標床反力が、歩容生成装置１００から出力される。
以上説明した本実施形態の作用１として、目標歩容の運動は、モデル操作床反力モーメントが目標ＺＭＰまわりに発生するように生成されつつ、実際のロボット１の床反力には、モデル操作床反力モーメントが付加されないように制御される。したがって、差し引きモデル操作床反力モーメント分だけ、目標歩容の運動と床反力の不平衡（アンバランス）を生じる。このことは、実ロボット１の上体姿勢角と目標歩容の上体姿勢角との差を０に収束させる効果に関して言えば、モデル操作床反力モーメントの符号を反転した床反力モーメントを実ロボット１に作用させることと等価である。
つまり、モデル操作床反力モーメントを適宜決定することにより、実ロボット１を、修正目標歩容に収束させることができる。つまり、実ロボット１の姿勢を安定化させることができる。
作用２として、モデル操作床反力モーメントの符号を反転したモーメントとコンプライアンス制御用目標床反力モーメントとの和がトータルの復元力になる。すなわち、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントとの差がトータルの復元力になる。
また、作用３として、モデル操作床反力モーメントは、ＺＭＰ存在可能範囲を無視していかなる値でも取ることが可能であるので、非常に高い姿勢復元力を発生することができる。
なお、作用１、２および３は、本願出願人が先に提案した特開平５−３３７８４９号公報の技術と同様である。
また、作用４として、床反力水平成分が、床反力水平成分許容範囲を越えないように上体並進モードの上体並進加速度と上体回転モードの上体姿勢角加速度とが決定されるので、走行歩容における支持脚側脚体２の離床直前や着床直後など、大きな床反力水平成分を発生できない時期や、摩擦係数の小さい床をロボット１が移動する場合においてもロボット１のスリップを防止することができる。
また、作用５として、床反力の並進力鉛直成分が０の時期、すなわち、両脚がいずれも接地していない時期では、床反力水平成分の許容範囲が０に設定されることにより、前記した本実施形態のアルゴリズムにより自動的に、上体並進モードに依存せずに上体回転モードに依存した姿勢復元が行われ、床と足底の間の摩擦力に依存せずに姿勢復元が行われる。したがって、この時期（空中期）においても、単に、上体並進モードを修正するだけの方式と異なり、有効に姿勢復元作用が働く。なお、この時には、床反力水平成分が０になるように生成されるから、歩容の全体重心水平加速度は０となる。
また、作用６として、上記したごとく、コンプライアンス制御に対しては、モデル操作床反力モーメントは、目標床反力として出力されない。すなわち、目標ＺＭＰまわりにモデル操作床反力モーメントを発生させるように歩容生成が行なわれても、コンプライアンス制御に対しては、目標ＺＭＰまわりの床反力モーメントが０になることを目標とする目標床反力が、歩容生成装置１００から与えられる。したがって、コンプライアンス制御による床反力制御を妨げず、コンプライアンス制御による床反力制御を適切に行うことができる。より具体的には、足平２２の本来の接地性が低下したり、足平２２の底面が浮いてしまうような不具合が発生することを防止または抑制することができる。
なお、後述するように、第２実施例以降でも、目標ＺＭＰまわりのコンプライアンス制御用目標床反力モーメントは、床反力モーメント許容範囲を越えないように決定される。
作用７として、目標ＺＭＰまわりに発生するモーメントが０になるように生成したとした場合の歩容を元歩容、前記実施形態の如く目標ＺＭＰまわりに発生するモーメントがモデル操作床反力モーメントになるように生成される歩容を修正歩容と呼ぶと、通常、元歩容と修正歩容は異なる歩容となる。元歩容は定常歩容に漸近するように設定されているので、修正歩容は、通常、定常歩容に漸近しない歩容となる。
しかし、今回歩容（修正歩容）の生成が完了した直後に、再びＳ０２０からＳ０２８が実行され、修正歩容の終端状態を新たな初期状態とした新たな今回歩容が、新たに設定される定常歩容に漸近するように、新たな今回歩容パラメータが決定されるので、継続的に（長期的に）安定性が保証された歩容を生成し続けることができる。
ここで、前述した第１実施形態と本発明との対応関係を説明しておく。第１実施形態は、前記第１〜１５発明に対応している。この場合、第１実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、モデル操作床反力モーメントが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当する。また、動力学モデルとして、図１０のものが用いられ、そのモデルの運動は上体並進モードと上体回転モードを含んでいる。また、歩容パラメータは図１２のＳ０２８の処理で前回歩容の終端状態に応じて適宜修正される。
なお、本実施形態では、補償全床反力モーメントＭｄｍｄが床反力モーメント許容範囲内の値であるときには、モデル操作床反力モーメントは０になるが、このときのモデル操作床反力モーメントを図１０の動力学モデルの状態量（例えば動力学モデル上でのロボットの重心位置や、上体質点２４ｍの位置等）に応じて設定するようにしてもよい。
次に本発明の第２実施形態を図４３〜図４６を参照して説明する。なお、本実施形態の説明において、第１実施形態と同一構成部分もしくは同一機能部分については前記第１実施形態と同一の参照符号を用いて説明を省略する。
本実施形態の概要を説明すると、元歩容と修正歩容が同時に生成される。修正歩容は、実ロボット１の姿勢安定化のために修正されると共に、コンプライアンス制御により姿勢復元に必要な床反力モーメントを発生してもまだ余裕がある（目標ＺＭＰまわりに発生可能な床反力モーメントに余裕がある）場合には、この余裕を用いて、可能な範囲で元歩容に収束するように決定される。
本実施形態における制御ユニット２６の機能ブロック図を図４３に示す。本実施形態では、姿勢安定化制御演算部１１２が求める補償全床反力モーメントＭｄｍｄは、歩容生成装置１００に入力される。そして、このＭｄｍｄを基にモデル操作床反力モーメントとコンプライアンス制御用目標床反力モーメントとを決定する補償全床反力モーメント分配器１１０は、歩容生成装置１００に組み込まれ、該歩容生成装置１００からコンプライアンス制御用目標床反力モーメントが複合コンプライアンス動作決定部１０４に出力される。そして、歩容生成装置１００内の補償全床反力モーメント分配器１１０は後述するように、第１実施形態のものよりも複雑な処理を行う。これ以外の、制御ユニット２６の機能的構成は、前記第１実施形態と同一である。
図４４に本実施形態における歩容生成装置１００の処理のメインフローチャートを示す。
同図４４おいて、Ｓ２０１０からＳ２０２９までは、第１実施形態のメインフローチャート（図１２）のＳ０１０からＳ０２９までの処理と同一の処理が行なわれる。なお、、Ｓ０２８（本実施形態ではＳ２０２８）のサブルーチンである図３５のフローチャートのＳ８００における初期化においては、今回歩容初期状態は、前回修正歩容（歩容生成装置１００が最終的に出力する歩容）の終端状態を今回支持脚座標系に変換したものであり、Ｓ２０３０で決定される元歩容の終端状態は、Ｓ８００では用いられない。
Ｓ２０２９の処理を完了した後、あるいはＳ２０１６の判断結果がＮＯである場合には、Ｓ２０３０に進み、元歩容の瞬時値（時刻ｔの今回値）が決定される。元歩容は、目標ＺＭＰまわりの床反力モーメントが０になるように生成される歩容である。
この元歩容は、前記第１実施形態の図１２のＳ０３０のサブルーチン処理の一部を変更したアルゴリズムによって生成される。すなわち、Ｓ０３０内のサブルーチン処理（詳しくはＳ０３０のサブルーチン処理である図４０のＳ１０１２のサブルーチン処理）である図４１のＳ１１０２およびＳ１１２２において、モデル操作床反力モーメントを０として（目標ＺＭＰまわりの目標床反力モーメントの水平成分を０として）、上体水平加速度αｔｍｐが求められる。これ以外の処理は、図１２のＳ０３０の処理と同一でよい。
次いで、Ｓ２０３１に進み、修正歩容の瞬時値を決定する。なお、修正歩容は、歩容生成装置１００から最終的に出力される目標歩容である。
Ｓ２０３１の処理は、図４５のサブルーチン処理により行なわれ、以下にその詳細を説明する。
まずＳ２１００からＳ２１１１までは、第１実施形態で説明した図４０のＳ１０００からＳ１０１１と同一の処理が実行される。
次いで、Ｓ２１１２に進み、床反力モーメント許容範囲と床反力水平成分許容範囲との条件を満足するように、モデル操作床反力モーメント、コンプライアンス制御用目標床反力モーメント、上体水平加速度および上体姿勢角加速が決定される。
Ｓ２１１２の詳細を、その処理を示す図４６のフローチャートを用いて、以下に説明する。
まず、Ｓ２２００において、修正歩容の上体水平位置と元歩容の上体水平位置との差であるモデル間上体水平位置差が求められる。この時点では、修正歩容の上体水平位置の今回値（時刻ｔでの値）は未だ求められていない。そこで、Ｓ２２００では、修正歩容の上体水平位置の前回値（時刻ｔ−Δｔの制御周期で最終的に求められた値）と、元歩容の上体水平位置の前回値（時刻ｔ−Δｔの制御周期でＳ２０３０において求められた値）あるいは今回値（時刻ｔの制御周期でＳ２０３０において求められた値）とを用いて、モデル間上体水平位置差が算出される。
次いで、Ｓ２２０２に進み、修正歩容の上体姿勢角と元歩容の上体姿勢角との差であるモデル間上体姿勢角差が求められる。このＳ２２０２では、Ｓ２２００のモデル間上体水平位置差の算出処理の場合と同様、修正歩容の上体姿勢角の前回値と、元歩容の上体姿勢角の前回値又は今回値とを用いてモデル間上体姿勢角差が求められる。
次いで、Ｓ２２０４に進み、モデル間上体水平位置差を基に、その差を０に収束させるために必要なモデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄが決定される。単に修正歩容の上体並進モードの上体水平加速度を発生させる床反力モーメントを元歩容の上体並進モードの上体水平加速度を発生させる床反力モーメントに一致させると、モデル間上体水平位置差は発散する。モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄは、前記上体並進モードによって修正歩容の上体水平位置を元歩容の上体水平位置に戻すよう動作を行った時に、これに伴って発生する床反力モーメントから元歩容の上体並進運動モードの上体水平加速度を発生させる床反力モーメントを減じたモーメントとしての意味を持つ。
具体的には、モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄは、例えば次式のフィードバック制御則によって決定される。この例では、フィードバック制御則として、ＰＤ制御則を用いているが、ＰＩＤ等、他のフィードバック制御則を用いてもよい。
2003061917.tif 000039
但し、Ｋｍｐ、Ｋｍｐｖはフィードバックゲイン（比例ゲイン、微分ゲイン）である。
次いで、Ｓ２２０６に進み、モデル間上体姿勢角差を基に、その差を０に収束させるために必要なモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄが決定される。単に修正歩容の上体回転モードの上体姿勢角加速度を発生させる床反力モーメントを元歩容の上体回転モードの上体姿勢角加速度を発生させる床反力モーメントに一致させると、モデル間上体姿勢角差は０に収束しない。モデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄは、上体回転モードによって修正歩容の上体姿勢角を元歩容の上体姿勢角に戻すよう動作を行った時に、これに伴って発生する床反力モーメントから元歩容の上体回転モードの上体姿勢角加速度を発生させる床反力モーメントを減じたモーメントとしての意味を持つ。
具体的には、モデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄは、例えば次式のフィードバック制御則によって決定される。この例では、フィードバック制御則として、ＰＤ制御則を用いているが、ＰＩＤ等、他のフィードバック制御則を用いてもよい。
2003061917.tif 000040
但し、Ｋｍｒ、Ｋｍｒｖはフィードバックゲイン（比例ゲイン、微分ゲイン）である。
ところで、最終的に決定された修正歩容の上体並進運動モードに伴って発生する床反力モーメントから元歩容の上体並進運動モードの上体水平加速度を発生させる床反力モーメントを減じたモーメントを、モデル上体水平位置安定化床反力モーメントと呼ぶ。また、最終的に決定された修正歩容の上体回転運動モードに伴って発生する床反力モーメントから元歩容の上体回転運動モードの上体姿勢角加速度を発生させる床反力モーメントを減じたモーメントを、モデル上体姿勢角安定化床反力モーメントと呼ぶ。
一方、摂動運動と摂動床反力とには線形性が近似的に成立する、すなわち、異なる摂動運動を元歩容の運動に加えた運動の床反力は、元歩容の床反力にそれぞれの摂動運動が発生する摂動床反力を加えたものにほぼ一致する。したがって、次式が近似的に成立する。
2003061917.tif 000041
したがって、式ｄ３０が近似的に成立することを考慮しつつ、モデル上体水平位置安定化床反力モーメントをモデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄに一致またはなるべく近くなるように決定し、かつ、モデル上体姿勢角安定化床反力モーメントをモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄに一致またはなるべく近くなるように決定すれば、修正歩容に適切なモデル操作床反力モーメントを発生させつつ、修正歩容の上体水平加速度と上体姿勢角加速度とを、それぞれ元歩容の上体水平加速度と上体姿勢角加速度とに可能な範囲で収束させることができる。
そこで、Ｓ２２０６の後、Ｓ２２０８に進み、以下の条件（これらを復元条件と呼ぶ）をできる限り満足するようにモデル上体水平位置安定化床反力モーメント（上体並進モードの床反力モーメント）とモデル上体姿勢角安定化床反力モーメント（上体回転モードの床反力モーメント）とを決定する。さらに、上記のモデル上体水平位置安定化床反力モーメントとモデル上体姿勢角安定化床反力モーメントとの定義を満足するように、修正歩容の上体水平加速度と上体姿勢角加速度とを決定する。なお、以下に示す復元条件は、番号が小さい条件ほど優先度が高いものとする。つまり相反して両立できない条件がある場合には、番号が小さい条件を優先して満足（成立）させる。ただし、復元条件１と２は必ず満足（成立）させる。
復元条件１）補償全床反力モーメントＭｄｍｄとモデル操作床反力モーメントの和（これは、前記式ｄ２７が成立する場合には、コンプライアンス制御用目標床反力モーメントに相当する）が床反力モーメント許容範囲を越えないこと。
復元条件２）修正歩容の床反力水平成分が床反力水平成分許容範囲を越えないこと。
復元条件３）モデル上体姿勢角安定化床反力モーメントが、モデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄに一致またはできる限り近いこと。この条件は、修正歩容の上体姿勢角が元歩容（当初予定した歩容）の上体姿勢角に収束するための条件である。
復元条件４）モデル上体水平位置安定化床反力モーメントが、モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄに一致またはできる限り近いこと。この条件は、修正歩容の上体水平位置が元歩容（当初予定した歩容）の上体水平位置に収束するための条件である。
復元条件５）モデル上体姿勢角安定化床反力モーメントおよびモデル上体水平位置安定化床反力モーメントがそれぞれ連続であること。
上記のような復元条件１〜５を満足するような上体水平加速度、上体姿勢角加速度等を決定するＳ２２０８の処理は具体的には例えば次のように実行される。
まず、コンプライアンス制御用目標床反力モーメントを、補償全床反力モーメントＭｄｍｄと、前記モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄと、モデル上体姿勢角安定化床反力モーメントＭｒｆｄｍｄとの和に仮に一致させたとした場合における該コンプライアンス制御用目標床反力モーメントの仮値（＝Ｍｄｍｄ＋Ｍｐｆｄｍｄ＋Ｍｒｆｄｍｄ。以下、本実施形態の説明ではこれに参照符号Ｍｉｎを付する）が求められる。このＭｉｎは、コンプライアンス制御用目標床反力モーメントの基本的な要求値としての意味をもつ。
さらに、上体並進モードよってＭｐｆｄｍｄを発生させると共に上体回転モードによってＭｒｆｄｍｄを発生させると仮定して歩容の運動を生成した場合に前記式０２，０３の動力学モデル上で発生する床反力水平成分が仮値Ｆｉｎとして求められる。このＦｉｎは、モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄによる床反力水平成分の要求値としての意味をもつ。この床反力水平成分の仮値Ｆｉｎは、例えば前記第１実施形態で説明した図４１のＳ１１０２およびＳ１１０４と同様の手法によって求めることができる。より具体的には、式０３の左辺のＭｙを（Ｍｐｆｄｍｄ＋Ｍｒｆｄｍｄ）に設定する（厳密にはＭｐｆｄｍｄ＋Ｍｒｆｄｍｄに目標ＺＭＰまわりの床反力モーメントの基準値を加えたものをＭｙとするのであるが、本実施形態では該基準値は０である）と共に、同式０３の右辺のＪ＊ｄ２θｂｙ／ｄｔ２をＭｒｆｄｍｄに設定してなる式をｄ２Ｘｂ／ｄｔ２について解くことにより、上体質点２４ｍの水平加速度、ひいては、上体水平加速度（これはＳ１１０２のαｔｍｐに相当する）が求められる。そして、この求めた上体水平加速度を用いて前記式１７に基づき、床反力水平成分の仮値Ｆｉｎ（これは式１７のＦｘｔｍｐに相当する）が求められる。
次いで、これらのＭｉｎ、Ｆｉｎの組が、床反力モーメント許容範囲（詳しくは現在時刻ｔの制御周期において図４５のＳ２１１１で求めた今回値）と、床反力水平成分許容範囲（詳しくは現在時刻ｔの制御周期において図４５のＳ２１１０で求めた今回値）との組と比較され、これらの許容範囲の制限を満たすコンプライアンス制御用目標床反力モーメントおよび床反力水平成分の組（詳しくは床反力モーメント許容範囲内に収まるコンプライアンス制御用目標床反力モーメントと床反力水平成分許容範囲内に収まる床反力水平成分との組）が決定される。なお、床反力モーメント許容範囲による制限対象となるのは、厳密には、目標ＺＭＰまわりの床反力モーメントの基準値（基準瞬時値）にコンプライアンス制御用目標床反力モーメントを加えたものであるが、本実施形態では該基準値は０であるため、コンプライアンス制御用目標床反力モーメントが直接的に床反力モーメント許容範囲による制限対象とされる。
ここで、上記許容範囲の制限を満たすコンプライアンス制御用目標床反力モーメント（以下、これを制限済コンプライアンス制御用目標床反力モーメントＭｌｔｄという）と床反力水平成分（以下、これを制限済床反力水平成分Ｆｌｔｄという）との組の決定手法は、後述する第３実施形態で図５２〜図５５を用いて詳細に説明する手法と同じである。そこで、その手法の詳細は、第３実施形態で詳細に説明することとし、本実施形態における以下の説明では、図５２〜図５５を参照して簡略的に説明する。
図５２〜図５５のように、床反力モーメントと床反力水平成分とを２軸とする座標平面を想定したとき、床反力モーメント許容範囲及び床反力水平成分許容範囲の組は、図示の長方形の領域（以下、ここでの説明では許容領域という）で表現される。図５２のように、（Ｆｉｎ，Ｍｉｎ）の組（図の白丸点）が上記許容領域内に存在する場合には、図の黒丸点で表される（Ｆｌｔｄ，Ｍｌｔｄ）の組の如く、Ｆｉｎがそのまま、床反力水平成分許容範囲の制限を満たす床反力水平成分Ｆｌｔｄとして決定されると共に、Ｍｉｎがそのまま、床反力モーメント許容範囲の制限を満たすコンプライアンス制御用目標床反力モーメントＭｌｔｄとして決定される。
また、図５３〜図５５のように、（Ｆｉｎ，Ｍｉｎ）の組（図の白丸点）が許容領域を逸脱している場合には、上記許容領域を満足するように最終的に決定される（Ｆｌｔｄ，Ｍｌｔｄ）の組から後述する式ｅ０１、式ｅ０２を用いて求められるモデル上体姿勢角安定化床反力モーメントがモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄに一致または最も近いことを優先し、その上で、モデル上体水平位置安定化モーメントがモデル上体水平位置安定化モーメント要求値Ｍｐｆｄｍｄに最も近いように（Ｍｌｔｄ，Ｆｌｔｄ）の組が決定される。これは前記復元条件３），４）を満たすためである。
このような（Ｆｌｔｄ，Ｍｌｔｄ）の組は、例えば次のように決定される。前記上体並進モードによって発生する床反力モーメントΔＭｐと床反力水平成分ΔＦｐとの比（＝ΔＭｐ／ΔＦｐ）を上体並進モード床反力比率ｈと言うこととし、図５３〜図５５の座標平面上でこの上体並進モード床反力比率ｈの傾きを持ち、且つ点（Ｆｉｎ，Ｍｉｎ）を通る直線を想定する。なお、上体並進モード床反力比率ｈは、前記式０６、０７に基づいてｈ＝Ｚｂ−Ｚｚｍｐ（これは上体質点２４ｍの目標ＺＭＰからの高さである）によって定まる値である。
このとき、図５３及び図５４のように、上記の傾きｈの直線と、前記許容領域（図示の長方形の領域）との交点が存在する場合には、その交点のうち、点（Ｆｉｎ，Ｍｉｎ）に最も近い交点（図の黒丸点）に対応する床反力水平成分および床反力モーメントの値が（Ｆｌｔｄ，Ｍｌｔｄ）として決定される。このように（Ｆｌｔｄ，Ｍｌｔｄ）の組を決定したとき、後述する式ｅ０１、ｅ０２を用いて求められるモデル上体姿勢角安定化床反力モーメントがモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄに一致することが優先され、その上で、モデル上体水平位置安定化床反力モーメントがモデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄに最も近いものとなる。なお、この原理の詳細は、後述の第３実施形態で説明する。
また、図５５のように、点（Ｆｉｎ，Ｍｉｎ）（図の白丸点）を通る傾きｈの直線が前記許容領域と交わらない場合には、その直線と同じ傾きｈを持ち、且つ、前記許容領域と交わる直線のうち、点（Ｆｉｎ，Ｍｉｎ）を通る傾きｈの直線に最も近い直線と、前記許容領域との交点（図の黒丸点）に対応する床反力水平成分および床反力モーメントの値が（Ｆｌｔｄ，Ｍｌｔｄ）として決定される。このように（Ｆｌｔｄ，Ｍｌｔｄ）の組を決定したとき、後述する式ｅ０１、ｅ０２を用いて求められるモデル上体姿勢角安定化床反力モーメントがモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄに一致しないものの、該Ｍｒｆｄｍｄに最も近いことが優先され、その上で、モデル上体水平位置安定化床反力モーメントがモデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄに最も近いものとなる。なお、この原理の詳細も後述の第３実施形態で説明する。
なお、前記復元条件３）、４）を満たす（Ｆｌｔｄ，Ｍｌｔｄ）の組は、前記式０２，０３に基づいて探索的に求めるようにするうことも可能である。
上述のように（Ｆｌｔｄ，Ｍｌｔｄ）の組を求めた後、前記モデル上体水平位置安定化床反力モーメント、すなわち、上体並進モードで発生すべき床反力モーメントと、前記モデル上体姿勢角安定化床反力モーメント、すなわち上体回転モードで発生すべき床反力モーメントとが決定される。
この場合、モデル上体水平位置安定化床反力モーメントは、例えば次式により決定される。
2003061917.tif 000042
また、モデル上体姿勢角安定化床反力モーメントは、例えば次式により決定される。
2003061917.tif 000043
次いで、上体水平加速度の今回値（時刻ｔでの値）と上体姿勢角加速度の今回値（時刻ｔでの値）とが決定される。この場合、上体水平加速度は、例えば、前記式０２の左辺のＦｘをＦｌｔｄに設定した式をｄ２Ｘｂ／ｄｔ２について解くことにより求められる上体質点２４ｍの水平加速度から決定される。また、上体姿勢角加速度は、例えば、前記の如く求めたモデル上体姿勢角安定化床反力モーメントを前記上体慣性モーメントＪで割ることにより求められる。あるいは、例えば式０３の左辺のＭｙを上記の如く求めたモデル上体水平位置安定化床反力モーメントとモデル上体姿勢角安定化床反力モーメントとの和に設定し、且つ、式０３の右辺第２項のｄ２Ｘｂ／ｄｔ２を上記の如く求めた上体質点２４ｍの水平加速度（今回値）に設定してなる式をｄ２θｂｙ／ｄｔ２について解くことにより求められる。なお、この場合、式０３の右辺第１項のＸｂ（上体質点２４ｍの水平位置）に対応する上体水平位置の今回値は未だ求められていないが、例えば上体水平位置の前回値（時刻ｔ−Δｔの制御周期で求めた値）に対応するＸｂを用いればよい。あるいは、上記の如く求めた上体水平加速度の今回値を用いて上体水平加速度を現在時刻ｔまで２階積分してなる上体水平位置の今回値に対応する上体質点２４ｍの水平位置をＸｂとして用いてもよい。
以上説明した処理により前記復元条件１〜５を満たすようにモデル上体水平位置安定化床反力モーメントおよびモデル上体姿勢角安定化床反力モーメントが決定され、さらに上体水平加速度および上体姿勢角加速度が決定される。
上述したようにＳ２２０８の処理を実行した後、Ｓ２２１０に進み、前記式ｄ３０により、モデル操作床反力モーメントが決定される。すなわち、Ｓ２２０８で求めたモデル上体水平位置安定化床反力モーメントとモデル上体姿勢角安定化床反力モーメントとの和がモデル操作床反力モーメントとして決定される。なお、最終的に決定される修正歩容の運動の今回瞬時値に基づいて目標ＺＭＰまわりの床反力モーメントを直接算出して、これをもってモデル操作床反力モーメントとしても良い。
次いで、Ｓ２２１２に進み、前記式ｄ２７により、コンプライアンス制御用目標床反力モーメントが決定される。すなわち、補償全床反力モーメントＭｄｍｄとＳ２２１０で求めたモデル操作床反力モーメントとの和がコンプライアンス制御用目標床反力モーメントとして決定される。
以上により、図４５のＳ２１１２の処理が終了し、次に、Ｓ２１１４に進む。このＳ２１１４の処理は、前記第１実施形態における図４０のＳ１０１４と同一であり、上体水平加速度の２階積分により上体水平位置の今回値が決定されると共に、上体姿勢角加速度の２階積分により上体姿勢角の今回値が決定される。
次いで、図４４のＳ２０３２、Ｓ２０３４に進んで、前記第１実施形態における図１２のＳ０３２、Ｓ０３４と同一の処理が実行され、歩容生成処理が繰り返される。
なお、補足すると、本実施形態における動力学モデルによる歩容瞬時値決定においては、動力学モデルの状態量（あるいは前回、前前回の歩容瞬時値）も必要であるので、修正歩容生成用と元歩容生成用との２つの動力学モデルが必要である。
本実施形態においては、上記のごとく、元歩容と修正歩容とが並列的に生成され、修正歩容は、実ロボット１の姿勢安定化のために修正されると共に、コンプライアンス制御により姿勢復元に必要な床反力モーメントを発生してもまだ余裕がある場合には、この余裕を用いて、可能な範囲で元歩容に収束するようにしている。このため、前記第１実施形態の作用効果に加え、当初設定した元歩容に近い、すなわち、当初の要求通りの歩容に近い歩容を生成することができる。したがって、あらかじめ設定された移動経路がある場合には、移動経路から大きくずれることを防止することが可能となる。また、修正歩容の上体姿勢角が元歩容（当初決定した歩容）の上体姿勢角に収束することを、修正歩容の上体水平位置が元歩容（当初決定した歩容）の上体水平位置に収束することよりも優先した（床反力水平成分許容範囲を満たす範囲で可能な限り上体並進モードの運動を調整した）ので、上体姿勢角が大きく変動することを抑制することができる。
ここで、前述した第２実施形態と本発明との対応関係を説明しておく。第２実施形態は、前記第１〜１４発明、第１６〜２３発明に対応している。この場合、第２実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、モデル操作床反力モーメントが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当し、モデル上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄおよびモデル上体姿勢角安定化床反力モーメント要求値Ｍｒｆｄｍｄが第２要求操作量に相当する。また、動力学モデルとして、図１０のものが用いられ、そのモデルの運動は上体並進モードと上体回転モードを含んでいる。また、歩容パラメータは図４４のＳ２０２８の処理で修正歩容の状態量に応じて適宜修正される。
次に本発明の第３実施形態を図４７〜図５５を参照して説明する。なお、本実施形態の説明で、前記第１実施形態もしくは第２実施形態と同一構成部分もしくは同一機能部分については第１実施形態もしくは第２実施形態と同一の参照符号を用いて詳細な説明を省略する。
本実施形態では、制御ユニット２６の機能的構成は、前記第２実施形態のもの、すなわち前記図４３に示したものと同一である。但し、本実施形態では、歩容生成装置１００が実行する歩容生成のアルゴリズムは、前記第２実施形態のものと相違している。そして、歩容生成装置１００以外の各部の処理は、前記第２実施形態と同一である。
図４７は、本実施形態における歩容生成装置１００の処理の概要を示すブロック図である。この図４７を参照して、歩容生成装置１００の処理の概要を以下に説明する。なお、図４７を用いて以下に説明する処理の概要は、後述する第４〜第６実施形態についても同様である。
図示の如く、歩容生成装置１００は歩容パラメータ決定部１００ａを備える。歩容パラメータ決定部１００ａは、目標歩容のパラメータ（目標歩容を規定するパラメータ）の値あるいは時系列テーブルを決定する。これは、後述の図４８のフローチャートのＳ３０１８からＳ３０３０までの処理に相当する。
詳細は後述するが、歩容パラメータ決定部１００ａが決定するパラメータには、目標足平位置姿勢軌道、目標腕姿勢軌道、基準上体姿勢軌道、目標ＺＭＰ軌道、目標床反力鉛直成分軌道等を規定するパラメータが含まれる他、床反力水平成分許容範囲を規定するパラメータと、ＺＭＰ許容範囲を規定するパラメータとが含まれる。この場合、本実施形態で設定する床反力水平成分許容範囲は、後述のＳ３０２６の処理で設定される単純化モデル歩容用のものと、Ｓ３０３０で設定される歩容補正用のものとの２種類がある。一方、ＺＭＰ許容範囲は、Ｓ３０３０処理で設定される歩容補正用のものだけである。
ここで、ＺＭＰ許容範囲は、本実施形態における床反力モーメント許容範囲に対応するものであり、前記第１実施形態で図３９を参照して説明した如く設定される。
歩容パラメータ決定部１００ａで決定された歩容パラメータは目標瞬時値発生部１００ｂに入力される。目標瞬時値発生部１００ｂは入力された歩容パラメータに基づき、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分、目標腕姿勢、目標全体重心鉛直位置、目標上体鉛直位置、床反力水平成分許容範囲、ＺＭＰ許容範囲および基準上体姿勢角の、現在時刻ｔにおける瞬時値を逐次算出（発生）する（図４７では一部の目標瞬時値のみを記載している）。この目標瞬時値発生部１００ｂの処理は、後述の図４８のフローチャートＳ３０３２の処理のなかで実行される図４０のＳ１０００〜Ｓ１０１０の処理、並びに、図４８のＳ３０３４、Ｓ３０３６の処理に相当する。なお、本実施形態では、目標瞬時値発生部１００ｂが算出する目標瞬時値のうち、一部の瞬時値（具体的には目標上体鉛直位置の瞬時値）は仮値であり、後に修正される。また、目標瞬時値発生部１００ｂが算出する床反力水平成分許容範囲の瞬時値は、単純化モデル歩容用の瞬時値と歩容補正用の瞬時値とがある。
目標瞬時値発生部１００ｂで算出（発生）された目標瞬時値（一部は仮瞬時値）は、フルモデル補正部１００ｃに入力される。また、フルモデル補正部１００ｃには、前記姿勢安定化制御演算部１１２（図４３を参照）で求められる補償全床反力モーメントＭｄｍｄも入力される。フルモデル補正部１００ｃは、動力学モデルとして、単純化モデル１００ｃ１とフルモデル１００ｃ２とを備える。該フルモデル補正部１００ｃは、単純化モデル１００ｃ１に基づいて、入力値から目標上体位置姿勢の仮瞬時値等を決定すると共に、さらに決定された上体位置姿勢の仮瞬時値等をフルモデル１００ｃ２を用いて修正する。
尚、単純化モデル１００ｃ１をフルモデル補正部１００ｃ２に含ませない構成も可能である。また、フルモデル１００ｃ２は、後述するように、逆フルモデル（逆動力学フルモデル）と順フルモデル（順動力学フルモデル）とのいずれかを含む。
フルモデル補正部１００ｃは、基本的には、次のＡ１〜Ａ３の条件を満足するように、Ｂの処理を実行する。すなわち、フルモデル補正部１００ｃは、
Ａ１）単に単純化モデルに基づいて生成した歩容（以降、これを単純化モデル歩容と呼ぶ）よりも高い精度で動力学的平衡条件を満足する。
Ａ２）真のＺＭＰ（目標ＺＭＰのまわりにコンプライアンス制御用目標床反力モーメントを発生させることによって修正された本来の定義を満足するＺＭＰ）は、ＺＭＰ許容範囲（安定余裕が十分維持できる許容範囲）に存在する。
Ａ３）床反力水平成分は歩容補正用の床反力水平成分許容範囲内になる。という条件を満足するように、
Ｂ）単純化モデルを用いて決定された単純化モデル歩容の上体位置姿勢を補正すると共に、目標ＺＭＰまわりのコンプライアンス制御用目標床反力モーメントを出力する。
なお、上記条件Ａ２は、目標ＺＭＰまわりに発生させる床反力モーメントをＺＭＰ許容範囲に対応する床反力モーメント許容範囲内に収めることと同等である。
ここで、上記単純化モデル１００ｃ１とフルモデル１００ｃ２とについて説明しておく。単純化モデル１００ｃ１は、動力学的精度よりも、演算量低減や挙動解析のし易さに重点を置いた動力学モデルであり、動力学的に一部無視があったり（例えば、重心まわりの角運動量変化を無視したり）、矛盾があるもの（厳密さを欠くもの）でも良い。本実施形態では前記第１実施形態で説明した図１０の動力学モデル（前記式０１〜０５で記述される動力学モデル）が単純化モデル１００ｃ１として用いられる。
また、フルモデル１００ｃ２は、単純化モデル１００ｃ１とは異なるロボット動力学モデルを意味する。これは、単純化モデル１００ｃ１よりも近似精度の高いロボット動力学モデルであることが望ましい。図示例で説明すれば、本実施形態では先に述べたように前記図１０に示した動力学モデルを単純化モデル１００ｃ１として用いるので、それよりも、近似精度の高い、例えば図１１に示す多質点モデル（ロボット１の各リンクにそれぞれ質点を有するモデル）のようなロボット動力学モデルをフルモデル１００ｃ２として用いることが望ましい。この場合、該フルモデル１００ｃ２は、質点のまわりに慣性モーメントを設定するものであっても良い。
しかし、単純化モデル１００ｃ１とフルモデル１００ｃ２とは、必ずしもモデルの近似精度が異なる必要はない。単純化モデル１００ｃ１とフルモデル１００ｃ２とは、動力学方程式が同じで、床反力水平成分許容範囲が異なっている、すなわち、単純化モデル歩容用の許容範囲と、歩容補正用の許容範囲とが異なっているだけでも良い。たとえば、単純化モデル１００ｃ１を用いた歩容を生成する際の床反力水平成分許容範囲を大きくとり（摩擦限界を越えていても良い）、フルモデル１００ｃ２を用いて歩容を修正する際の床反力水平成分許容範囲をスリップしにくいように狭く設定するだけでも良い。
尚、この明細書において、目標足平位置姿勢、目標床反力（特に、目標ＺＭＰと目標床反力鉛直成分）に基づいて（入力して）上体位置姿勢を算出する（出力させる）のに使用するモデルを「順動力学モデル」と呼び、目標足平位置姿勢、目標上体姿勢、目標上体位置に基づいて（入力して）床反力（特に、目標ＺＭＰもしくは目標ＺＭＰまわりの床反力モーメントと床反力水平成分）を算出する（出力させる）のに使用するモデルを「逆動力学モデル」と呼ぶ。順動力学モデルの入力には、少なくとも目標床反力が含まれ、逆動力学モデルの入力には、少なくとも目標運動が含まれる。
フルモデル補正部１００ｃが備えるフルモデル１００ｃ２は、逆動力学フルモデル（しばしば「逆フルモデル」と略称）または順動力学フルモデル（しばしば「順フルモデル」と略称）を備える。一般的には、逆動力学モデルの演算に比べ、順動力学モデルの演算は、演算量が多くなりがちである。
以上が歩容生成装置１００における処理の概要である。
次に、本実施形態における歩容生成装置１００の処理を詳説する。本実施形態における歩容生成装置１００は、図４８のフローチャートに示す処理を実行して、歩容を生成する。
まず、Ｓ３０１０からＳ３０２８までにて、前記第１実施形態で説明した図１２のＳ０１０からＳ０２８までの処理と同一の処理が実行される。なお、Ｓ３０２６のサブルーチンである図３２のＳ６０８で決定する今回歩容の床反力水平成分許容範囲は、前記第１実施形態や第２実施形態の場合よりも厳密に摩擦力限界を考慮せずともよく、摩擦力限界を超えるような範囲に設定してもよい。これは、後述するフルモデル補正によって、床反力水平成分は最終的に歩容補正用の床反力水平成分許容範囲によって制限されることとなるからである。
次いで、Ｓ３０３０に進み、歩容補正用の床反力水平成分許容範囲とＺＭＰ許容範囲（床反力中心点許容範囲）とをそれぞれ規定するパラメータを決定する。この場合、歩容補正用の床反力水平成分許容範囲は、例えば前記単純化モデル歩容用の床反力水平成分許容範囲と同様、Ｘ軸方向（前後方向）、Ｙ軸方向（左右方向）のそれぞれについて、床反力鉛直成分軌道と、前記式１２とに基づいて、前記図３３に示したようなパターンに設定される。そして、例えば、前記式１２のｋａ＊μの値が歩容補正用の床反力水平成分許容範囲を規定するパラメータとして設定される。但し、この床反力水平成分許容範囲は、式１２の係数ｋａの値を、単純化モデル歩容用の床反力水平成分許容範囲よりも小さめに設定する等して、確実に摩擦限界内の範囲に設定することが望ましい。
また、ＺＭＰ許容範囲は、前記第１実施形態における図１２のＳ０２９で床反力モーメント許容範囲の設定に関して説明した場合と同様に設定される。なお、ＺＭＰ許容範囲に、床反力鉛直成分を乗算することで、ＺＭＰ許容範囲をこれと等価な床反力モーメント許容範囲に変換してもよいことはもちろんである。
次いで、Ｓ３０３２に進み今回歩容瞬時値（現在時刻ｔの値）が前記単純化モデル（図１０の動力学モデル）を用いて決定される。このＳ３０３２の処理は、一部の処理を除いて第１実施形態における図１２のＳ０３０の処理と同一である。さらに詳細には、Ｓ３０３２の処理では、モデル操作床反力モーメントを０として、換言すれば、目標ＺＭＰまわりに発生する床反力モーメントが０になるように今回歩容瞬時値が決定される。すなわち、Ｓ３０３２の処理では、その処理内のサブルーチン処理である図４１のＳ１１０２、Ｓ１１２２の処理が、モデル操作床反力モーメントを０として実行される。これ以外は、第１実施形態のＳ０３０の処理と同一である。
次いでＳ３０３４に進んで、スピン力をキャンセルするための腕動作が決定される。この処理は、第１実施形態における図１２のＳ０３２の処理と同一である。
以上説明したＳ３０３４までの処理によって生成された現在時刻ｔにおける歩容の瞬時値を、以降、単純化モデル歩容瞬時値という。本実施形態では、この単純化モデル歩容瞬時値は、単純化モデル（図１０の動力学モデル）を用いて、ロボット１にその運動によって発生する慣性力と重力との合力が目標ＺＭＰまわりに発生する床反力モーメントが０になるように（目標ＺＭＰに関する動力学的平衡条件を満たすように）決定される。
この場合、単純化モデル歩容瞬時値のうちの、上体水平位置および上体姿勢角の瞬時値と、上体鉛直位置の瞬時値とは仮瞬時値であり、後述するフルモデル補正によって補正されるものである。また、本実施形態における単純化モデル歩容瞬時値のうち、目標ＺＭＰまわりの目標床反力モーメントの瞬時値は定常的に０であるが、後述するフルモデル補正によって目標ＺＭＰまわりに発生させる床反力モーメントの目標値としてのコンプライアンス制御用目標床反力モーメントが生成される。
次に、Ｓ３０３６に進み、歩容補正用の床反力水平成分許容範囲とＺＭＰ許容範囲を規定するパラメータ（Ｓ３０３０で設定したもの）を基に、歩容補正用の床反力水平成分許容範囲と、ＺＭＰ許容範囲との瞬時値（現在時刻ｔの値）が求められる。
次いで、Ｓ３０３８に進み、フルモデルを用いて補正歩容の発生（歩容の補正）を行い、最終的な目標歩容の瞬時値が決定される。すなわち、前記図４７を参照して説明したように、補正目標上体位置姿勢や、目標ＺＭＰまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメントの算出（決定）などが実行される。
次いで、Ｓ３０４０に進み、時刻ｔをΔｔだけ増やし、再びＳ３０１４に戻り、Ｓ３０１４からＳ３０４０までの処理が繰り返される。
上記Ｓ３０３８の処理は、本実施形態の特徴をなす部分であり、以下にその処理を詳説する。本実施形態に係る装置の歩容補正手法は、フルモデルフィードフォワード補正型である。また、逆動力学フルモデル（逆フルモデル）を用いる手法で、かつ単純化モデル歩容の入力を補正しない手法であり、かつ摂動モデルを用いる手法である。
図４９は、本実施形態に係る歩容生成装置１００の動作、具体的には、図４８のフロー・チャートのＳ３０３８の歩容の修正手法を説明する機能ブロック図である。ただし、図４９の単純化モデル２００は、単に動力学モデルだけでなく、前述のＳ３０１０からＳ３０３４の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表す。したがって、図４９において、単純化モデル２００から先の部分がＳ３０３８の処理に相当する。
尚、歩容補正用の床反力水平成分許容範囲とＺＭＰ許容範囲の瞬時値を決定処理は、図４８のフローチャートの参照符号Ｓ３０３６を用いて示している。
実際の処理はひとつのコンピュータで実行されるので、ブロック図を離散化した後、ブロック図の上流側から下流側（歩容出力側）に向かって順に実行される。ただし、上流側に戻るフィードバック量は、前回制御周期（現在時刻ｔに対して時刻ｔ−Δｔ）に算出した値（状態量）を用いることとする。尚、以降、前回制御周期（時刻ｔ−Δｔ）に算出した値を前回値と略す。
Ｓ３０３８の処理が実行される毎に、ブロック図の１制御周期分の演算が実行される。
Ｓ３０３８では、まず、上記のごとく得られた単純化モデル歩容の目標上体姿勢角（以降、単純化モデル上体姿勢角と呼ぶ）、目標上体水平位置（以降、単純化モデル上体水平位置と呼ぶ）、目標重心位置、目標足平位置姿勢、目標腕姿勢などの運動を表す変数（これを運動変数と呼ぶ）の瞬時値と、目標ＺＭＰの瞬時値とが前記逆動力学フルモデル（逆フルモデル）２０１に入力される。入力された運動変数で表される運動に釣り合う（すなわち運動によってフルモデルが発生する）床反力水平成分と目標ＺＭＰまわりの床反力モーメントが、逆フルモデル２０１の演算処理によって算出される。なお、単純化モデル歩容における目標ＺＭＰまわりの床反力モーメントは０であるので、逆フルモデル２０１によって算出される目標ＺＭＰまわりの床反力モーメントは、単純化モデル歩容の誤差としての意味を持つものである。なお、逆フルモデル２０１により求められた床反力水平成分と床反力モーメントをそれぞれ「フルモデル床反力水平成分」、「フルモデル床反力モーメント」と呼ぶ。また、以降、しばしば、フルモデル床反力水平成分をＦｆｕｌｌ、フルモデル床反力モーメントをＭｆｕｌｌと略す。
また、前記逆フルモデル２０１は、目標重心位置を満足する上体鉛直位置を算出する。さらに、図示は省略しているが、逆フルモデル２０１は、重心水平位置も算出する。
補足すると、フルモデルには目標全体重心鉛直位置を入力しており、また目標床反力鉛直成分は目標全体重心鉛直位置の２階微分値から求められるので、フルモデルには、特に目標床反力鉛直成分を入力する必要はない。冗長であっても演算を減らすためなどの理由で、フルモデルに目標床反力鉛直成分を入力しても良い。
ここで、歩容補正に用いられる摂動モデルについて説明する。
摂動モデルは、上体水平位置補正用摂動モデル２０２および上体姿勢角補正用摂動モデル２０３から成る。尚、摂動モデルは、このようにふたつのモデルに分離しないで、図１０のようなひとつのモデルにしても良い。
上体水平位置補正用摂動モデル２０２は、前記上体並進モードにおける床反力の摂動と上体水平位置の摂動との関係を表す。
上体水平位置補正用摂動モデル２０２は、目標床反力モーメントの補正量を入力し、これに動力学的に釣り合う目標上体水平位置の補正量を算出する。この入力（目標床反力モーメントの補正量）を、上体水平位置補正用摂動モデルモーメントＭｐと呼ぶ。また、上体水平位置補正用摂動モデル２０２の出力（目標上体水平位置の補正量）を、補正用摂動モデル上体水平位置Ｘｃと呼ぶ。また、上体水平位置補正用摂動モデル２０２が発生する床反力水平成分を上体水平位置補正用摂動モデル床反力水平成分Ｆｐと呼ぶ。
上体水平位置補正用摂動モデル２０２は、図５０のように、支点と倒立振子質点（上体質点）とこれらを結合する伸縮自在な支持棒から成る倒立振子で表される。支点の水平位置は前記した今回歩容の支持脚座標系の原点の水平位置に一致し、支点の鉛直位置は目標ＺＭＰの鉛直位置に一致するように支点の位置が設定されている。倒立振子質点の質量ｍｂは、図１０に示す前記単純化モデル（３質点＋フライホイールのモデル）の上体質点質量と同じである。倒立振子質点の鉛直位置Ｚｃは、単純化歩容に対応する図１０に示す単純化モデルの上体質点位置の鉛直位置Ｚｂと同一とする。
この上体水平位置補正用摂動モデル２０２は、前記単純化モデル（３質点モデル）の動力学方程式の式０１、式０２、式０３において、床反力モーメントの摂動ΔＭｙと上体質点水平位置の摂動ΔＸｂの関係を表す。
そこで、Ｍｙ、Ｘｂ、Ｚｂ以外は定数とみなして、床反力モーメントの摂動ΔＭｙと上体質点水平位置の摂動ΔＸｂの関係を式０３から求めると、次式が得られる。
2003061917.tif 000044
同様に、Ｆｘ、Ｘｂ以外は定数とみなして、床反力水平成分の摂動ΔＦｘと上体質点水平位置の摂動ΔＸｂの関係を式ａ９から求めると、次式が得られる。
2003061917.tif 000045
上体水平加速度によって発生するΔＭｐとΔＦｐの比である前記上体並進モード床反力比率ｈは、式ａ１２右辺のうちの上体水平加速によって発生する項（すなわち第２項）と式ａ１３との比であるから、次式が得られる。
2003061917.tif 000046
すなわち、上体並進モード床反力比率ｈは、単純化モデルの上体質点（倒立振子質点）の支点からの高さに相当する。
よって、式ａ１２と式ａ１４より次式が得られる。
2003061917.tif 000047
一方、上体質点（倒立振子質点）に作用する重力と慣性力との合力の並進力鉛直成分に釣り合う床反力鉛直成分を上体床反力鉛直成分Ｆｂｚと呼ぶ。すなわち、上体床反力鉛直成分Ｆｂｚは、次式で定義される。
2003061917.tif 000048
式ａ８より、上体床反力鉛直成分Ｆｂｚは次式となる。
2003061917.tif 000049
言い換えると、上体床反力鉛直成分は、床反力鉛直成分Ｆｚと図１０に示す前記単純化モデル（３質点＋フライホイールのモデル）の両脚質点に作用する重力と慣性力との合力の並進力鉛直成分との和に等しい。
式ａ１６を式ａ１５に代入することにより、次式が得られる。
2003061917.tif 000050
式ａ１８のΔＭｙを上体水平位置補正用摂動モデルモーメントＭｐ、ΔＸｂを補正用摂動モデル上体水平位置Ｘｃに対応させることにより（式ａ１８のΔＭｙに上体水平位置補正用摂動モデルモーメントＭｐ、ΔＸｂに補正用摂動モデル上体水平位置Ｘｃを代入することにより）、次式が得られる。
2003061917.tif 000051
すなわち、上体水平位置補正用摂動モデル２０２は、式ａ１４によって求められる上体並進モード床反力比率ｈと式ａ１７によって求められる上体床反力鉛直成分Ｆｂｚを用いて、式ａ１９で表される。
また、式ａ１３のΔＦｘを上体水平位置補正用摂動モデル床反力水平成分Ｆｐに対応させることにより、次式が得られる。
2003061917.tif 000052
すなわち、式ａ１４、式ａ１７、式ａ１９、式ａ２０によって上体水平位置補正用摂動モデル２０２が記述される。
補足すると、ここでは、上体質点位置の摂動と上体位置（上体代表点位置）の摂動は一致するものとみなしているが、厳密には、一致するとは限らない。従って、Ｍｐ、ＦｐとＸｃの関係を求めるためには、さらに上体質点水平位置と上体位置の幾何学的関係を表すモデルが必要である。
一方、上体姿勢角補正用摂動モデル２０３は、前記上体回転モードにおける床反力の摂動と上体姿勢角の摂動の関係を表す。
上体姿勢角補正用摂動モデル２０３は、床反力モーメントの補正量が入力され、これに動力学的に釣り合う目標上体姿勢角の補正量を算出する。この入力（床反力モーメントの補正量）を、上体姿勢角補正用摂動モデルモーメントＭｒと呼ぶ。また、上体姿勢角補正量摂動モデル２０３の出力（目標上体姿勢角の補正量）を補正用摂動モデル上体姿勢角θｃと呼ぶ。また、上体姿勢角補正用摂動モデルが発生する床反力水平成分を上体水平位置補正用摂動モデル床反力水平成分Ｆｒと呼ぶ。Ｆｒは前述したごとく０である。すなわち、次式が常に成立する。
2003061917.tif 000053
上体姿勢角補正用摂動モデル２０３は、図５１のように、フライホイールＦＨで表される。フライホイールのイナーシャは、図１０に示した前記単純化モデル（３質点モデル＋フライホイール）のフライホイールＦＨと同じである。この上体姿勢角補正用摂動モデル２０３のフライホイールの回転角度が補正用摂動モデル上体姿勢角θｃ、そのフライホイールによって発生する床反力モーメントが上体姿勢角補正用摂動モデルモーメントＭｒに相当する。
この上体姿勢角補正用摂動モデル２０３は、前記単純化モデル（３質点＋フライホイールのモデル）の動力学方程式の前記式０３において、床反力モーメントの摂動ΔＭｙと上体姿勢角の摂動Δθｂｙの関係を表している。
そこで、Ｍｙ、θｂｙ以外は定数とみなして、床反力モーメントの摂動ΔＭｙと上体姿勢角の摂動Δθｂｙの関係を式０３から求めると、次式が得られる。
2003061917.tif 000054
式ａ２２のΔＭｙを上体姿勢角補正用摂動モデルモーメント  Ｍｒ、Δθｂｙを補正用摂動モデル上体姿勢角θｃに対応させることにより、次式が得られる。
2003061917.tif 000055
すなわち、上体姿勢角補正用摂動モデル２０３は、式ａ２３で表される。また、上体水平位置補正用摂動モデル床反力水平成分Ｆｒは上記したごとく式ａ２１のようになる（Ｆｒ＝０）。
後述するように、Ｓ３０３８では、最終的に、補正歩容（より詳しくは、単純化モデル歩容の一部の瞬時値を補正した目標瞬時値）が生成（出力）される。補正歩容の目標上体姿勢角（これを以降、補正目標上体姿勢角と呼ぶ）は、前記求めた単純化モデル上体姿勢角の瞬時値（Ｓ３０３２で求めた今回歩容の現在時刻ｔでの目標上体姿勢角の瞬時値）に前記補正用摂動モデル上体姿勢角θｃ（現在時刻ｔの制御周期で求められる値）を演算部２０４で加えることにより得られる。補正歩容の目標上体水平位置（これを以降、補正目標上体水平位置と呼ぶ）は、前記求めた単純化モデル上体水平位置の瞬時値（Ｓ０３２で求めた今回歩容の現在時刻ｔでの目標上体水平位置の瞬時値）に補正用摂動モデル上体水平位置Ｘｃ（現在時刻ｔの制御周期で求められる値）を演算部２０５で加えることにより得られる。
補正歩容の目標床反力も修正される。具体的には、目標ＺＭＰまわりの床反力モーメント水平成分が０でなくなり、コンプライアンス制御用目標床反力モーメントが目標値として出力される。また、目標床反力水平成分も、補正目標床反力水平成分に修正され、出力される。
上記のごとく、補正歩容の運動は、単純化モデル歩容の運動に摂動モデルの運動（詳しくは上体位置姿勢補正用摂動モデルと上体姿勢角補正用摂動モデルの運動）を加えた（合成した）運動である。
一般に、ある基準運動に、ある摂動運動を加えた運動が発生する床反力は、基準運動が発生する床反力（運動によって発生する重力と慣性力に釣り合う床反力）と摂動運動によって発生する床反力の摂動分の和で近似される。
したがって、補正歩容の運動によって発生する床反力（厳密には、補正歩容の運動によって発生する床反力を、逆フルモデル２０１を用いて算出した値）は、逆フルモデル２０１の床反力（単純化モデル歩容の運動によって発生する床反力を、逆フルモデル２０１を用いて算出した値）に上体位置姿勢補正用摂動モデル２０２の入力モーメント（前記上体水平位置補正用摂動モデルモーメントＭｐ）と上体姿勢角補正用摂動モデル２０３の入力モーメント（前記上体姿勢角補正用摂動モデルモーメントＭｒ）とを加えたものとなる。
補正歩容が、動力学的平衡条件を精度良く満足するということは、補正歩容の運動によって発生する床反力（詳しくは補正歩容の運動によって逆フルモデル２０１上で発生する床反力）が、補正目標床反力とほぼ一致することであるから、この条件を満足するためには、以下のふたつの式を満足しなければならない。
2003061917.tif 000056
また、補正歩容の真のＺＭＰは、補正目標床反力モーメントを目標床反力鉛直成分で割った値だけ、単純化モデル歩容の目標ＺＭＰ（理想目標ＺＭＰ）からずれた点に変更される。
2003061917.tif 000057
尚、補正歩容の真のＺＭＰのＸ方向（前後方向）成分を算出する場合には、補正目標床反力モーメントのＹ軸（左右方向軸）まわり成分を用いる。また、補正歩容の真のＺＭＰのＹ方向成分を算出する場合には、補正目標床反力モーメントのＸ軸（前後軸）まわり成分を用いる。ただし、補正歩容の真のＺＭＰのＹ方向成分を算出する場合には、式ａ７の右辺の「＋」は「−」にしなければならない。
式ａ７から求められる補正歩容の真のＺＭＰは、ＺＭＰ許容範囲内になければならない。これをＺＭＰ制約条件と呼ぶ。
また、補正目標床反力水平成分は、歩容補正用の床反力水平成分許容範囲内になければならない。これを床反力水平成分制約条件と呼ぶ。
以上のように、補正歩容は、式ａ５、式ａ６、ＺＭＰ制約条件（式ａ７から得られる補正歩容の真のＺＭＰの存在範囲条件）および床反力水平成分制約条件を満足しなければならない。
しかし、単に、これらの式や条件を満足するだけでは、前記補正用摂動モデル上体位置と前記補正用摂動モデル上体姿勢角が発散してしまう。
そこで、前記上体水平位置補正用摂動モデル２０２と前記上体姿勢角補正用摂動モデル２０３の状態量（より具体的には、補正用摂動モデル上体水平位置速度、補正用摂動モデル上体姿勢角・角速度など）を基に、これらの状態量がある所定の状態に収束する（安定する）ように、補正用摂動モデル２０２，２０３の安定化制御を行う。
まず、上体水平位置補正用摂動モデル２０２の安定化制御について詳細を説明する。
先に本出願人が提案した特願２００１−１３３６２１号では、倒立振子型の摂動モデルを用い、補正歩容の瞬間姿勢を基にフルモデルを用いて計算される重心水平位置ＸＧｆを、単純化モデル歩容の瞬間姿勢を基に単純化モデルを用いて計算される重心水平位置ＸＧｓに収束させる制御則を持ち、その制御則で決定されたフィードバック量（フィードバック操作量）を上体水平位置補正用摂動モデルに付加的に入力していた。
具体的には、式ａ８で与えられる目標整定位置に収束するように式ａ９の制御則が与えられていた。ただし、ｍｔｏｔａｌは前記ロボット総重量、ｍｂは前記上体質点質量（倒立振子質量）であり、ＸＧｆは、単純化モデル歩容の瞬間姿勢を基にフルモデルを用いて計算される重心水平位置、すなわち、前記逆フルモデルが算出する重心水平位置である。また、Ｋｐ、Ｋｖはフィードバック制御のゲインである。
2003061917.tif 000058
尚、補足すると、実際にコンピュータ上で計算を実行する時には、上記右辺の変数の値には、前回値が用いられる。以降の制御則でも同様である。
本出願人が今回提案するフルモデル補正手法に用いられる上体水平位置補正用摂動モデルは、非線形性は強いものの、前述のごとく、倒立振子型のモデルであるという点では、先に本願出願人が提案した特願２００１−１３３６２１号に用いられる上体水平位置補正用摂動モデルと類似している。
そこで、今回提案するフルモデル補正手法でも、目標整定位置は同様に決定し、目標整定位置に収束させるための制御則だけを変更することとする。
上体水平位置補正用摂動モデル２０２を目標整定位置に収束（安定）させるための制御則を上体水平位置補正用摂動モデル制御則２０６と呼び、この制御則が決定するフィードバック量（操作量）を上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄと呼ぶ。ここで「要求値」を付けたのは、後述するように、真のＺＭＰが前記ＺＭＰ許容範囲内に存在し、かつ、床反力水平成分が床反力水平成分許容範囲内になるように、上記制御則で決定された値に制限が加えられて修正されるからである。制限が加えられて修正されたモーメントを、上体水平位置補正用摂動モデル安定化モーメントＭｐｆと呼ぶ。
上体水平位置補正用摂動モデル制御則２０６には、式ａ９の代わりに、次式を用いれば良い。
2003061917.tif 000059
ただし、Ｋｐｐ、Ｋｐｖはフィードバック制御のゲインである。
上体水平位置補正用摂動モデル２０２の説明で述べたごとく、上体質点（倒立振子質点）には鉛直方向に上体床反力鉛直成分Ｆｂｚが作用している。このため、上体質点には、上体質点位置が原点（支点位置）からずれていくと、ますますずれようとするモーメントが作用する。式ａ１０の右辺第３項は、それを打ち消すためのモーメントである。
上体水平位置補正用摂動モデル安定化モーメント要求値ＭｐｆｄｍｄのＸ軸（前後方向軸）まわり成分を求める場合には、補正用摂動モデル上体水平位置速度および目標整定位置はＹ軸方向（左右方向）成分を用いる。
上体水平位置補正用摂動モデル安定化モーメント要求値ＭｐｆｄｍｄのＹ軸（左右方向軸）まわり成分を求める場合には、補正用摂動モデル上体水平位置速度および目標整定位置はＸ軸方向（前後方向）成分を用い、右辺第３項の「−」を「＋」に代える。
尚、目標整定位置は、０（倒立振子支点の水平位置）にしても良い。また、上体水平位置補正用摂動モデル制御則２０６は、先に本出願人が提案した特願２００１−１３３６２１号に述べたような各種方法のいずれかにしても良い。また、それら以外でも、少なくとも上体水平位置補正用摂動モデル２０２の状態量のいずれか（より具体的には補正用摂動モデル上体水平位置速度など）を基に、この状態量がある所定の状態に収束する（安定する）ように、上体水平位置補正用摂動モデルの安定化制御則２０６を決定すれば良い。
次に、上体姿勢角補正用摂動モデル２０３の安定化制御について詳細を説明する。
目標瞬時値発生部１００ｂが出力する基準上体姿勢角（図４０のＳ１００４で求められたもの）あるいは単純化モデルによる目標上体姿勢角（図４０のＳ１０１４で求められたもの）に、補正目標上体姿勢角、すなわち、単純化モデルによる目標上体姿勢角に補正用摂動モデル上体姿勢角を加えたものが整定あるいは追従するように、上体姿勢角補正用摂動モデル２０３の状態に応じて、ＰＩ制御などのフィードバック制御則によってフィードバック量（操作量）を決定し、これを上体姿勢角補正用摂動モデル２０３に付加的に入力する。
この制御則を上体姿勢角補正用摂動モデル制御則２０７と呼び、このフィードバック量（操作量）を上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄと呼ぶ。ここで「要求値」を付けたのは、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄと同様の理由からである。制限が加えられて修正されたモーメントを、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆと呼ぶ。
上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄを決定する上体姿勢角補正用摂動モデル制御則２０７は、具体的には、次式のようにすれば良い。
2003061917.tif 000060
ただし、Ｋｒｐ、Ｋｒｖはフィードバック制御のゲインである。
なお、式ａ１１において、（基準上体姿勢角−単純化モデルによる目標上体姿勢角）の代わりに０としてもよい。
再び、図４９の機能ブロック図を用いてＳ３０３８の処理を説明すると、上記のごとく、前記上体水平位置補正用摂動モデル制御則２０６（式ａ１０）によって上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄが求められる。また、前記上体姿勢角補正用摂動モデル制御則２０７（式ａ１１）によって上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄが求められる。
次いで、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが０であったと仮定した場合の上体位置補正用摂動モデルの床反力推定（算出）値ＦＯがＦＯ演算部２０８により求められる。後述するように、上体水平位置補正用摂動モデル２０２には、フルモデル床反力モーメントＭｆｕｌｌに、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを加えられたものが入力される。したがって、ＦＯは、上体水平位置補正用摂動モデル２０２にフルモデル床反力モーメントＭｆｕｌｌの符号を反転させたものだけが入力された場合に上体水平位置補正用摂動モデル２０２が発生する床反力である。
具体的には、次式によってＦＯが求められる。
2003061917.tif 000061
右辺第１項は、前回（時刻ｔ−Δｔ）の上体水平位置補正用摂動モデル２０２の床反力水平成分を表す。
右辺第２項は、前回の上体水平位置補正用摂動モデル安定化モーメントＭｐｆによって、上体水平位置補正用摂動モデル２０２に直接的に発生した（すなわち直達項の）床反力水平成分を表す。
すなわち、上体水平位置補正用摂動モデル２０２の前回の上体質点水平加速度に上体質点の質量ｍｂを乗じた値から、前回の上体水平位置補正用摂動モデル安定化モーメントＭｐｆを上体並進モード床反力比率ｈで割った値を減じることによって、Ｍｐｆが０であったと仮定した場合の上体位置補正用摂動モデルの床反力ＦＯの推定値が求められる。
次に、前記制約（床反力水平成分正条件およびＺＭＰ制約条件）を無視して、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致させ、かつ、上体姿勢角補正用摂動モデル安定化モーメント  Ｍｒｆを、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致させると共に、目標ＺＭＰまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメントを補償全床反力モーメントＭｄｍｄとＭｐｆとＭｒｆとの総和に一致させたと仮定した場合に、目標ＺＭＰまわりに発生する床反力モーメントＭｉｎがＭｉｎ演算部２０９により求められる。この床反力モーメントを無制限時補正目標床反力モーメントＭｉｎと呼ぶ。無制限時補正目標床反力モーメントＭｉｎは、次式によって求められる。
2003061917.tif 000062
すなわち、無制限時補正目標床反力モーメントＭｉｎは、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄと上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄと補償全床反力モーメントＭｄｍｄとを加算することにより得られる。
次いで、前記制約（床反力水平成分制約条件およびＺＭＰ制約条件）を無視して、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致させ、かつ、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆを、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致させると共に、コンプライアンス制御用目標床反力モーメントを補償全床反力モーメントＭｄｍｄとＭｐｆとＭｒｆとの総和に一致させたと仮定した場合に、発生する床反力水平成分ＦｉｎがＦｉｎ演算部２１０により求められる。この床反力水平成分を無制限時補正目標床反力水平成分Ｆｉｎと呼ぶ。
補正目標床反力水平成分は、前記式ａ６によって得られるが、前述のごとく、上体姿勢角補正用摂動モデル２０３の挙動によって上体姿勢角補正用摂動モデル２０３には床反力水平成分は発生しない、すなわちＦｒは０である。したがって、無制限時補正目標床反力水平成分Ｆｉｎは、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが０であったと仮定した場合の補正目標床反力水平成分に、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを０から上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに変更したことによって増加する床反力水平成分を加えたものになる。
ところで、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを０から上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに変更したことによって増加する床反力水平成分は、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄを上体並進モード床反力比率ｈで割った値になる。
したがって、無制限時補正目標床反力水平成分Ｆｉｎは、式ａ１５に示すように、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄを上体並進モード床反力比率ｈで割った値に、前記求めたフルモデル床反力水平成分Ｆｆｕｌｌを加算し、さらに、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが０であったと仮定した場合の上体位置補正用摂動モデルの床反力の算出値ＦＯを加算することによって得られる。
2003061917.tif 000063
次いで、後に詳説する制限手段（制限処理部）２１１によって、無制限時補正目標床反力モーメントＭｉｎと無制限時補正目標床反力水平成分Ｆｉｎとから、これらに制限を加えた値である（目標ＺＭＰまわりの）制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄが決定される。本実施形態では、コンプライアンス制御用目標床反力モーメントは、制限補正目標床反力モーメントＭｌｔｄに一致し、補正歩容の床反力力水平成分は制限補正目標床反力水平成分Ｆｌｔｄに一致する。
制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄとは、補正歩容（コンプライアンス制御用目標床反力モーメントを含む）の真のＺＭＰが前記ＺＭＰ許容範囲内に存在し、かつ、補正歩容の床反力水平成分が床反力水平成分許容範囲内になるように決定される。すなわち、ＭｌｔｄおよびＦｌｔｄはＺＭＰ制約条件および床反力水平成分制約条件を満足するように決定される。
また、上記制約条件下で、上体水平位置補正用摂動モデル安定化モーメントＭｐは、可能な限り、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致または近い値になるように決定される。同様に、上体姿勢角補正用摂動モデル安定化モーメントＭｒは、可能な限り上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致または近い値になるように決定される。これにより、前記補正用摂動モデル上体位置Ｘｃと前記補正用摂動モデル上体姿勢角θｃとを安定化し、発散することを防止する。
次いで、以下の式により、上体水平位置補正用摂動モデル安定化モーメントＭｐｆ、および上体姿勢角補正用摂動モデル安定化モーメントＭｒｆがそれぞれＭｐｆ演算部２１２、Ｍｒｆ演算部２１３により求められる。
2003061917.tif 000064
すなわち、Ｍｐｆ演算部２１２では、制限補正目標床反力水平成分Ｆｌｔｄから、フルモデル床反力水平成分Ｆｆｕｌｌを減じ、Ｍｐが０であったと仮定した場合の上体位置補正用摂動モデル２０２の床反力の算出値ＦＯを減じた値に、上体並進モード床反力比率ｈを乗じることにより、前記上体水平位置補正用摂動モデル安定化モーメントＭｐｆが得られる。また、Ｍｒｆ演算部２１３では、目標ＺＭＰまわりの制限補正目標床反力モーメントＭｌｔｄから前記上体水平位置補正用摂動モデル安定化モーメントＭｐｆと補償全床反力モーメントＭｄｍｄとを減じることにより、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆが得られる。
次いで、以下の式により、上体水平位置補正用摂動モデル床反力モーメントＭｐ、および上体姿勢角補正用摂動モデル床反力モーメントＭｒが求められる。
2003061917.tif 000065
すなわち、Ｍｐ演算部２１４にて、上体水平位置補正用摂動モデル安定化モーメントＭｐｆからフルモデル床反力モーメントＭｆｕｌｌを減じることにより、上体水平位置補正用摂動モデル床反力モーメントＭｐが得られる。また、上体姿勢角補正用摂動モデル床反力モーメントＭｒは、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆと同一の値とされる。
次いで、上体水平位置補正用摂動モデル床反力モーメントＭｐが上体位置補正用摂動モデル２０２に入力され、その入力された床反力モーメントに釣り合う補正用摂動モデル上体位置Ｘｃが算出される。
また、上体姿勢角補正用摂動モデル床反力モーメントＭｒが上体姿勢角補正用摂動モデル２０３に入力され、その入力された床反力モーメントに釣り合う補正用摂動モデル上体姿勢角θｃが算出される。
次いで、演算部２０５，２０４にて、それぞれ次式ａ２４、式ａ２５に従って、補正目標上体水平位置と補正目標上体姿勢角とが決定され、これらがそれぞれ上体水平位置姿勢の最終的な目標瞬時値として出力される。
2003061917.tif 000066
すなわち、単純化モデル上体水平位置に補正用摂動モデル上体位置Ｘｃを加えることにより補正目標上体水平位置が得られ、これが最終的な目標上体水平位置として出力される。また、単純化モデル上体姿勢角に補正用摂動モデル上体姿勢角θｃを加えることにより補正目標上体姿勢角が得られ、これが最終的な目標上体姿勢角として出力される。
また、目標ＺＭＰまわりのコンプライアンス制御用目標床反力モーメントとして制限補正目標床反力モーメントＭｌｔｄが出力されると共に、補正目標床反力水平成分として制限補正目標床反力水平成分Ｆｌｔｄが出力される。
すなわち、次式ａ２６、式ａ２７に従って、補正目標床反力水平成分と目標ＺＭＰまわりの補正目標床反力モーメントとがそれぞれ、床反力水平成分および床反力モーメント（目標ＺＭＰまわりのモーメント）の最終的な目標瞬時値として決定され、これらが出力される。
2003061917.tif 000067
以上のごとく、Ｓ３０３８の歩容補正が行われる。
尚、補足すると、本実施形態は、フィードフォワード型の補正であり、また、摂動動力学モデルは厳密モデルではない。このため、上記のごとく式ａ５および式ａ６を満足するように歩容を補正しても、厳密には、動力学平衡条件を満足するするわけではなく、近似的に動力学平衡条件を満足する。
また、本実施形態では、例えば１歩の歩容の終端（今回歩容終端）において、上体水平位置補正用摂動モデル２０２の上体運動と上体姿勢角補正用摂動モデル２０３との状態量、例えば上体水平位置補正用摂動モデル２０２の上体質点（倒立振子質点）の水平位置や、上体姿勢角補正用摂動モデル２０３のフライホールの回転角は、単純化モデル２００の状態量として追加される。つまり、今回歩容終端における単純化モデル２００の状態量が、上体水平位置補正用摂動モデル２０２の上体運動と上体姿勢角補正用摂動モデル２０３との状態量を加えたものに修正される。さらに、各摂動モデル２０２，２０３の状態量は初期化される（例えば上体水平位置補正用摂動モデル２０２の上体質点（倒立振子質点）の水平位置や、上体姿勢角補正用摂動モデル２０３のフライホールの回転角が０にリセットされる）。そして、上記の如く修正した単純化モデル２００の状態量を次回歩容の初期値として、次回歩容の生成が行なわれると共に、各摂動モデル２０２，２０３による補正用摂動モデル上体水平位置Ｘｃ及び補正用摂動モデル上体姿勢角θｃの算出が行なわれる。これにより各摂動モデル２０２，２０３の挙動の安定性をより一層高めることができる。なお、上記のような単純化モデル２００の状態量の修正は、歩容の生成途中で適宜行なってもよい。また、上記のような単純化モデル２００の状態量の修正は、後述する第４、第５実施形態においても同様に行なわれる。
次に、前述の制限手段の詳細、すなわち制限処理部２１１の処理の詳細を説明する。
制限手段（制限処理部２１１）では、前述した機能を持たせるために、以下の処理が行われる。
まず、目標瞬時値発生部１００ｂが決定したＺＭＰ許容範囲の瞬時値、すなわち前記図４８のＳ３０３６で決定されたＺＭＰ許容範囲の瞬時値が、目標ＺＭＰまわりの床反力モーメント許容範囲の瞬時値に変換される。
具体的には、次式の関係を用いて床反力モーメント許容範囲が求められる。
2003061917.tif 000068
ただし、式ａ３２においては、変数はすべてベクトルである。床反力鉛直成分ベクトルは、鉛直成分以外の成分を０にした並進床反力ベクトルである。また、「＊」は外積を意味する。
ＺＭＰ制約条件と床反力水平成分制約条件とを考慮せず、動力学的平衡条件（特に、重力と慣性力の合力が目標ＺＭＰまわりに作用するモーメントの水平成分が０であること）だけを満足する歩容の運動は、サジタルプレーン上での運動と、ラテラルプレーン上での運動とに分離して、それぞれを動力学的平衡条件を満足するように生成してから合成しても、動力学的平衡条件を近似的に満足することが知られている。また、以上のごとくＺＭＰ許容範囲と歩容補正用の床反力水平成分許容範囲とを設定することにより、ＺＭＰ制約条件および床反力水平成分制約条件も、サジタルプレーン上での制約条件とラテラルプレーン上での制約条件とに分離できる。したがって、ＺＭＰ制約条件、床反力水平成分制約条件および動力学的平衡条件のすべてを考慮した歩容の運動も、サジタルプレーン上での運動と、ラテラルプレーン上での運動とに分離して運動を生成し、後に合成することができる。
そこで、以下では、理解を容易にするために、サジタルプレーン上の運動に限定して制限手段の動作を説明するが、ラテラルプレーン上での運動に関しても同様である。
以上のように簡単化することにより、補正歩容により定まるコンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲内に存在すること（このことは補正歩容の運動による真のＺＭＰが前記ＺＭＰ許容範囲に存在することを意味する）は、次式で表現される。ただし、Ｍｍａｘは床反力モーメント許容範囲の上限値、Ｍｍｉｎは下限値である。
2003061917.tif 000069
また、補正歩容の床反力水平成分が歩容補正用の床反力水平成分許容範囲内に存在することは、次式で表現される。ただし、Ｆｍａｘは床反力水平成分許容範囲の上限値、Ｆｍｉｎは下限値である。
2003061917.tif 000070
また、本実施形態では、コンプライアンス制御用目標床反力モーメントは制限補正目標床反力モーメントＭｌｔｄ、補正歩容の床反力水平成分は制限補正目標床反力水平成分Ｆｌｔｄに一致またはほぼ一致する。
したがって、式ａ３３および式ａ３４は、近似的にそれぞれ次式ａ３５および式ａ３６に変換される。
2003061917.tif 000071
図５２に示す、横軸に床反力水平成分のＸ軸方向成分（前後軸方向成分）、縦軸に床反力モーメントのＹ軸まわり成分（左右軸まわり成分）をとった座標系において、制限補正目標床反力水平成分Ｆｌｔｄと制限補正目標床反力モーメントＭｌｔｄの組（Ｆｌｔｄ，Ｍｌｔｄ）を座標系上の点とすると、式ａ３５、式ａ３６を満足する点の集合（許容範囲）は、図５２の長方形の境界およびその内部である。
図５２の白丸（同図では、白丸と後述の黒丸とが重なっている）のように、無制限時補正目標床反力モーメントＭｉｎが床反力モーメント許容範囲内に存在し、かつ、無制限時補正目標床反力水平成分Ｆｉｎが床反力水平成分許容範囲内に存在している場合、すなわち、床反力モーメント許容範囲と床反力水平成分許容範囲で囲まれた長方形の範囲内に点（Ｆｉｎ，Ｍｉｎ）が存在している場合には、同図の黒丸で示す如く、Ｍｌｔｄは無制限時補正目標床反力モーメントＭｉｎにし、Ｆｌｔｄは無制限時補正目標床反力水平成分Ｆｉｎにする。当然のごとく、このようにするだけで、式ａ３５と式ａ３６が成立する。すなわち、コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲内に存在し、かつ補正歩容の床反力水平成分が床反力水平成分許容範囲内に存在する。しかも、無制限時補正目標床反力モーメントＭｉｎ、無制限時補正目標床反力水平成分Ｆｉｎに制限をかけずに（修正せずに）そのままＭｌｔｄ、Ｆｌｔｄとして出力されるので、前記補正用摂動モデル上体位置と前記補正用摂動モデル上体姿勢角を適切に安定化し、発散を防止することができる。
図５３〜図５５の白丸のように、無制限時補正目標床反力モーメントＭｉｎが床反力モーメント許容範囲内に存在していないか、あるいは、無制限時補正目標床反力水平成分Ｆｉｎが床反力水平成分許容範囲内に存在していない場合、すなわち、床反力モーメント許容範囲と床反力水平成分許容範囲とで囲まれた長方形の範囲に点（Ｆｉｎ，Ｍｉｎ）が存在していない場合には、次のようにＭｌｔｄとＦｌｔｄとが決定される。
まず、ＭｌｔｄおよびＦｌｔｄの決定法を説明する前に、その決定における方針を説明する。
単に、Ｍｌｔｄを無制限時補正目標床反力モーメントＭｉｎにし、Ｆｌｔｄを無制限時補正目標床反力水平成分Ｆｉｎにすると、コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲内に存在しない、あるいは、補正歩容の床反力水平成分が床反力水平成分許容範囲内に存在しなくなる。この時の上体水平位置補正用摂動モデル安定化モーメントＭｐｆと上体姿勢角補正用摂動モデル安定化モーメントＭｒｆは前記式ａ２０および式ａ２１において、ＭｌｔｄにＭｉｎ、ＦｌｔｄにＦｉｎを代入することで得られる。コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲内に存在し、かつ補正歩容の床反力水平成分が床反力水平成分許容範囲内に存在するためには、このようにして得られたＭｐｆまたはＭｒｆを変更する必要がある。この際、本実施形態では、Ｍｒｆが上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄにできる限り一致すること、すなわち、ＭｒｆのＭｒｆｄｍｄからの修正が最小になることを優先させる。
この理由を以下に述べる。
上体姿勢角補正用摂動モデル２０３で補正できる床反力モーメントは、比較的小さく、大きなモーメントを補正しようとすると、上体姿勢角が大きく変動する。したがって、逆フルモデル２０１を用いた単純化モデル歩容の補正は、できる限り上体水平位置補正用摂動モデル２０２の動きを単純化モデル歩容に加えることによって行なうことが望ましい。そして、上体水平位置補正用摂動モデル２０２の動きで補正しきれない場合に上体姿勢角補正用摂動モデル２０３の動きを単純化モデル歩容に加えることによって該単純化モデル歩容を補正するのが望ましい。故に、上体姿勢角補正用摂動モデル２０３を復元させる（基準上体姿勢角に復元させる）ことを、上体水平位置補正用摂動モデル２０２を復元させる（上体水平位置を安定な位置に復元させる）ことよりも優先するのが望ましい。すなわち、上体姿勢角補正用摂動モデル２０３を復元させるための操作量であるＭｒｆが上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄにできる限り一致することを優先させることが望ましい。
さて、Ｍｒｆが上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致するためには、式ａ２０と式ａ２１とにおいて、ＭｌｔｄにＭｉｎ、ＦｌｔｄにＦｉｎを代入することで得られたＭｐｆとＭｒｆの内で、Ｍｒｆは変更されずに、Ｍｐｆだけが変更されなければならない。
ここで、Ｆｌｔｄの変更量をΔＦとし、Ｍｌｔｄの変更量をΔＭとする。つまり、次式のようにＦｌｔｄとＭｌｔｄを変更するものとする。
2003061917.tif 000072
ところで、前記式ａ２０および式ａ２１より、次式の関係が得られる。
2003061917.tif 000073
上記式ａ３７〜式ａ３９から明らかなように、次式が成立していれば、Ｍｒｆは変更されない。つまり、次式が成立していれば、Ｍｒｆは、Ｆｌｔｄ＝Ｆｉｎ且つＭｌｔｄ＝Ｍｉｎとした場合と同じになる。
2003061917.tif 000074
以上より、ΔＦおよびΔＭは、上記式ａ４０を満足しなければならない。
また、その上で、Ｍｐｆが上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄにできる限り近いことが望ましいので、変更量ΔＦ（またはΔＭ）の絶対値ができる限り小さくなるように決定する。さらに式ａ３７、式ａ３８を用いて、ＦｌｔｄとＭｌｔｄとを決定する。これにより、少なくとも前記補正用摂動モデル上体姿勢角θｃを適切に安定化して発散を防止することができ、またその上で前記補正用摂動モデル上体水平位置Ｘｃを可能な範囲で安定化して発散を防止することができる。
このようにして決定されたＦｌｔｄとＭｌｔｄの組を、図５３、図５４において黒丸点（Ｆｌｔｄ，Ｍｌｔｄ）で表すと、黒丸点は、白丸点（Ｆｉｎ，Ｍｉｎ）を通る、傾きが上体並進モード床反力比率ｈである直線と、式ａ３５、式ａ３６で表される許容範囲の境界との交点の内で、最も白丸点（Ｆｉｎ，Ｍｉｎ）に近い点となる。
補足すると、上記直線は、式ａ３７、式ａ３８、式ａ４０を満足する点（Ｆｌｔｄ，Ｍｌｔｄ）の集合を意味する。また、白丸点の状態から、上体水平位置補正用摂動モデル安定化モーメントＭｐｆだけを変更すると、Ｍｌｔｄの変更量ΔＭとＦｌｔｄの変更量ΔＦの比が上体並進モード床反力比率ｈとなることからも、上記直線が、（Ｆｌｔｄ、Ｍｌｔｄ）を（Ｆｉｎ，Ｍｉｎ）に一致させた状態から上体水平位置補正用摂動モデル安定化モーメントＭｐｆだけを変更した場合の（Ｆｌｔｄ，Ｍｌｔｄ）の組の集合を意味することがわかる。
ただし、図５５のように、式ａ３５、式ａ３６、式ａ３７、式ａ３８、式ａ４０を満足するＦｌｔｄとＭｌｔｄが存在しない場合には、次のようにＦｌｔｄとＭｌｔｄとが決定される。この場合には、Ｍｒを変更しないことをあきらめ、Ｍｒの変更量ができる限り小さくなるように考慮する。
そこで、式ａ４０の代わりに式４１が用いられる。
2003061917.tif 000075
ＦｌｔｄとＭｌｔｄは、式ａ３５、式ａ３６、式ａ３７、式ａ３８、式ａ４１を満足するように決定される。ただし、ΔＭｐ２が０に近いほど、Ｍｒの変更量が小さくなるので、ΔＭｐ２の絶対値が最小となるように決定する。また、これらの条件を満足した上で、Ｍｐが上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄにできる限り近いことが望ましいので、ΔＦの絶対値ができる限り小さくなるように決定する。これにより、少なくとも前記補正用摂動モデル上体姿勢角θｃを可能な範囲で安定化して発散を防止し、またその上で、前記補正用摂動モデル上体水平位置Ｘｃを可能な範囲で安定化して発散を防止することができる。
このようにして決定されたＦｌｔｄとＭｌｔｄとの組を、図５５において黒丸点（Ｆｌｔｄ，Ｍｌｔｄ）で表すと、黒丸点は、式ａ３５、式ａ３６で表される許容範囲の長方形の境界線の頂点（許容範囲の上限および下限を表す４本の線の交点）のうちで（より一般的には、許容範囲の境界線上の点のうちで）、白丸点（Ｆｉｎ，Ｍｉｎ）を通る、傾きが上体並進モード床反力比率ｈである直線に最も近い点となる。
以上のごとく、目標ＺＭＰまわりの制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄとが決定される。
尚、補足すると、床反力鉛直方向成分が０の時（走行歩容の空中期）には、式ａ３５、式ａ３６の上限値Ｍｍａｘ、Ｆｍａｘおよび下限値Ｍｍｉｎ、Ｆｍｉｎは０となるので、目標ＺＭＰまわりの制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄは共に０となる。
以上がＳ３０３８の処理の詳細である。
ここで、本実施形態の各変数を前記第２実施形態で説明した各変数に以下のように対応させることとする。すなわち、
ａ）上体姿勢角補正用摂動モデル制御則２０７の出力である上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄを、モデル上体姿勢角安定化床反力モーメント要求値に、
ｂ）上体水平位置補正用摂動モデル制御則２０６の出力である上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄを、モデル上体水平位置安定化床反力モーメント要求値に、
ｃ）上体姿勢角補正用摂動モデル安定化モーメントＭｒｆを、モデル上体姿勢角安定化床反力モーメントに、
ｄ）上体水平位置補正用モデル安定化モーメントＭｐｆを、モデル上体水平位置安定化床反力モーメントに、対応させることとする。
上記対応関係を考慮しつつ、本実施形態で生成される修正歩容が、前記式ｄ２７および前記復元条件を満足することを以下に証明する。
図４９からわかるように、修正歩容の運動は、単純化モデル歩容の運動に上体水平位置補正用摂動モデル２０２の上体運動と上体姿勢角補正用摂動モデル２０３の上体運動を加えたものになっている。また、フルモデル２０１の動力学から見ると、単純化モデル歩容の運動によって発生する目標ＺＭＰまわりに発生する床反力は、フルモデル床反力モーメントＭｆｕｌｌであり、上体水平位置補正用摂動モデル２０２の上体運動が発生する床反力は、上体水平位置補正用摂動モデルモーメントＭｐであり、上体姿勢角補正用摂動モデル２０３の上体運動が発生する床反力は上体姿勢角補正用摂動モデルモーメントＭｒである。
したがって、修正歩容の運動が発生する床反力モーメント、すなわちモデル操作床反力モーメントは、次式となる。
2003061917.tif 000076
また、図４９からわかるように、以下の式が成立する。
2003061917.tif 000077
また、上体姿勢角補正用摂動モデルモーメントＭｒは上体姿勢角補正用摂動モデル安定化モーメントＭｒｆと一致させている。すなわち、次式が成立する。
2003061917.tif 000078
したがって、式ｄ３１、式ｄ３２および式ｄ３３より次式が成立する。
2003061917.tif 000079
式ｄ３４は、第２実施例の式ｄ３０に対応する式となっている。
補足すると、図４９において、上体水平位置補正用モデル安定化モーメントＭｐｆと上体姿勢角補正用摂動モデル安定化モーメントＭｒｆを共に０とした場合に生成される歩容は、目標ＺＭＰまわりのモーメントが０である。したがって、この場合に生成された歩容が元歩容に相当する。
またさらに、図４９からわかるように、次式が成立する。
2003061917.tif 000080
したがって、式ｄ３４および式ｄ３５より次式が成立する。
2003061917.tif 000081
ところで、前記制限手段（制御処理部２１１）によって、制限補正目標床反力モーメントＭｌｔｄは、床反力モーメント許容範囲を越えないように決定される。したがって、式ｄ３６の右辺も床反力モーメント許容範囲を越えない。すなわち、前記復元条件１を満足する。
さらに、本実施形態では、制限床反力モーメントＭｌｔｄをコンプライアンス制御用目標床反力モーメントとするので、上記式ｄ３６によって、前記式ｄ２７が成立する。
一方、前述のように制限手段（制限処理部２１１）によって、制限補正目標床反力水平成分Ｆｌｔｄは、床反力水平成分許容範囲内を越えないように決定される。
また、制限補正目標床反力水平成分Ｆｌｔｄから上体水平位置補正用摂動モデルモーメントＭｐを決定するまでの演算処理によれば、修正歩容の床反力水平成分は、制限補正目標床反力水平成分Ｆｌｔｄにほぼ一致する。
したがって、修正歩容の床反力水平成分は、床反力水平成分許容範囲を越えない。すなわち、前記復元条件２を満足する。
また、制限手段（制限処理部２１１）の処理では、復元条件１および２を満足した上で可能な限り、制限床反力モーメントＭｌｔｄから上体水平位置補正用モデル安定化モーメントＭｐｆと補償全床反力モーメントＭｄｍｄとを減じたモーメントが、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致または近い値になるように決定される。
したがって、復元条件１および２を満足した上で可能な限り、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆは、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致または近い値になるように決定される。すなわち、前記復元条件３を満足する。
また、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄから上体水平位置補正用摂動モデル安定化モーメントＭｐｆを決定するまでの演算処理によれば、制限手段（制限処理部２１１）によって、復元条件１、２および３を満足した上で可能な限り、上体水平位置補正用摂動モデル安定化モーメントＭｐは、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致または近い値になるように決定される。すなわち、前記復元条件４を満足する。
また、制限手段（制限処理部２１１）は、入力が連続であれば、出力も連続となる。したがって、前記復元条件５を満足する。
以上のごとく、本実施形態では、前記復元条件すべてを満足する。したがって、前記第２実施形態と同様の作用効果を奏することができる。
さらに、第２実施形態の作用効果に加え、前記復元条件を満足するように上体水平位置、上体姿勢角を決定する際に、演算量を比較的少ないいものとすることができる。
ここで、前述した第３実施形態と本発明との対応関係を説明しておく。第３実施形態は、前記第１〜１４発明、第１６〜２３発明に対応している。この場合、第３実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、上体姿勢角補正用摂動モデルモーメントＭｒおよび上体水平位置補正用摂動モデルモーメントＭｐが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当し、上体位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄおよび上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄが第２要求操作量に相当する。また、動力学モデルとして、単純化モデル２００（図１０のモデル）と逆フルモデル２０１と上体姿勢補正用摂動モデル２０３と、上体水平位置補正量摂動モデル２０２とが用いられ、単純化モデル２００、上体姿勢補正用摂動モデル２０３および上体水平位置補正量摂動モデル２０２の運動は、上体並進モードと上体回転モードを含んでいる。また、歩容パラメータは図４８のＳ３０２８の処理で修正歩容の状態量に応じて適宜修正される。
次に本発明の第４実施形態を以下に説明する。第４実施形態に係る装置の補正手法は、前記第３実施形態のものと歩容生成装置１００の処理（図４８のＳ３０３８の処理）のみが相違するものであり、フルモデルフィードバック補正型である。また、逆動力学フルモデル（逆フルモデル）を用いる手法で、かつ単純化モデル歩容の入力を補正しない手法であり、かつ摂動モデルを用いる手法である。
図５６は、この第４実施形態に係る装置の動作、具体的には、図４８フローチャートのＳ３０３８の歩容の修正手法を説明する機能ブロック図である。ただし、図５６の単純化モデル２００は、前記第３実施形態と同様、単に動力学モデルだけでなく、図４８のＳ３０１０からＳ３０３４の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表す。したがって、図５６において、単純化モデル２００から先の部分がＳ３０３８の処理に相当する。なお、図５６の機能部分のうち、第３実施形態に係る図４９と同一の機能分については、図４９と同一の参照符号を用い、詳細な説明を省略する。
Ｓ３０３８の処理以外の構成は第３実施形態と同じであるので説明を省略し、以下に図５６を用いてＳ３０３８の処理を詳細に説明する。
Ｓ３０３８では、まず、前述のごとく図４８のＳ３０３２で求めた単純化モデル歩容の目標上体水平位置の瞬時値（現在時刻ｔの制御周期で求めた仮瞬時値）に、前回制御周期（時刻ｔ−Δｔ）で後述のごとく算出した補正用摂動モデル上体水平位置Ｘｃを演算部２０５により加えることにより、最終的な目標上体水平位置（補正目標上体水平位置）が求められる。また、図４８のＳ３０３２で求めた単純化モデル歩容の目標上体姿勢角の瞬時値（現在時刻ｔの制御周期で求めた仮瞬時値）に、前回制御周期（時刻ｔ−Δｔ）で算出した後述する補正用摂動モデル上体姿勢角θｃを演算部２０４により加えることにより、最終的な目標上体姿勢角（補正目標上体姿勢角）が求められる。そして、これらの補正目標上体水平位置および補正目標上体姿勢角が、それぞれ上体水平位置および上体姿勢角の最終的な目標瞬時値として出力される。
すなわち、前記式ａ２４、式ａ２５により、補正目標上体水平位置と補正目標上体姿勢角とが求められる。
次いで、上記の如く単純化モデル歩容を補正して得られた目標上体水平位置（補正目標上体水平位置）、目標上体姿勢角（補正目標上体姿勢角）、および前述のごとく得られた単純化モデル歩容の目標重心位置、目標足平位置姿勢、目標腕姿勢などの運動変数の瞬時値と目標ＺＭＰの瞬時値とを、前記逆動力学フルモデル２０１に入力し、その入力された運動変数で表される運動に釣り合う（すなわち運動によって逆フルモデル２０１が発生する）床反力水平成分と目標ＺＭＰまわりの床反力モーメントとが算出される。従って、本実施形態では、逆フルモデル２０１には、単純化モデル上体水平位置および上体姿勢角に加えて、補正用摂動モデル上体水平位置Ｘｃと補正用摂動モデル上体姿勢角θｃとが追加的に入力される。以降、第３実施形態と同様、逆フルモデル２０１により算出された床反力水平成分、床反力モーメントをそれぞれフルモデル床反力水平成分Ｆｆｕｌｌ、フルモデル床反力モーメントＭｆｕｌｌと呼ぶ。
フルモデル床反力水平成分Ｆｆｕｌｌとフルモデル床反力モーメントＭｆｕｌｌとのうち、フルモデル床反力水平成分Ｆｆｕｌｌは、補正目標床反力水平成分（現在時刻ｔにおける床反力水平成分の最終的な目標瞬時値）として出力される。
すなわち、以下の式により、補正目標床反力水平成分が決定されて出力される。
2003061917.tif 000082
以上の処理からわかるように、本実施形態では、フルモデル歩容は、単純化モデル歩容に上体水平位置補正用摂動モデル２０２の挙動と上体姿勢角補正用摂動モデル２０３の挙動とを加えたものになる。したがって、次式が成立する。ただし、単純化モデル床反力水平成分は、単純化モデル歩容の運動によって発生する床反力を逆フルモデル２０１を用いて算出したものの並進力水平成分である。
2003061917.tif 000083
次いで、上体水平位置補正用摂動モデル制御則２０６により、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄが決定される。本実施形態における上体水平位置補正用摂動モデル制御則２０６は、特願２００１−１３３６２１号で本願出願人が提案した如く、設定される。例えば、次式のように該制御則２０６が決定される。
2003061917.tif 000084
ただし、重心差は、次式で求められる。
2003061917.tif 000085
式ａ５２におけるＫｐｇ、Ｋｖｇはフィードバック制御のゲインであり、フルモデル重心水平位置、単純化モデル重心水平位置および補正用摂動モデル上体水平速度ｄＸｃ／ｄｔは、それぞれ、前回（時刻ｔ−Δｔ）に後述のごとく算出したフルモデル歩容瞬時値の重心水平位置、単純化モデル歩容瞬時値の重心水平位置（単純化モデル歩容の瞬間姿勢を基に単純化モデルを用いて計算される重心水平位置ＸＧｓ）、および補正用摂動モデル上体水平速度ｄＸｃ／ｄｔである。
即ち、フルモデル重心水平位置から単純化モデル重心水平位置を減算して得た重心差と摂動モデルの状態量のひとつである摂動モデル上体速度とに基づいて摂動モデル制御用フィードバック量（操作量）を演算するようにした。かかる摂動モデル制御則により、前記重心差の時間的平均値を、ほぼ０に制御することができる。
次いで、上体姿勢角補正用摂動モデル制御則２０７により、上体姿勢補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄが決定される。これは、第１実施形態と同様の制御則を用いれば良い。従って、該制御則２０７としては、例えば前記式ａ１１が用いられる。
次いで、無制限時補正目標床反力モーメントＭｉｎがＭｉｎ演算部２０９により求められる（推定される）。無制限時補正目標床反力モーメントＭｉｎは、第３実施形態と同様、前記制約（ＺＭＰ制約条件および床反力水平成分制約条件）を無視して、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致させ、かつ、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆを、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致させると共に、目標ＺＭＰまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメントを補償全床反力モーメントＭｄｍｄとＭｐｆとＭｒｆとの総和に一致させた場合に、目標ＺＭＰまわりに発生する床反力モーメントである。
無制限時補正目標床反力モーメントＭｉｎは、第３実施形態と同様、前記式ａ１３の演算によって求められる。すなわち、無制限時補正目標床反力モーメントＭｉｎは、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄと上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄと補償全床反力モーメントＭｄｍｄとを加算することにより得られる。
さらに、Ｆｉｎ演算部２１０により、無制限時補正目標床反力水平成分Ｆｉｎも求められる（推定される）。無制限時補正目標床反力水平成分Ｆｉｎは、第３実施形態と同様、前記制約（ＺＭＰ制約条件および床反力水平成分制約条件）を無視して、上体水平位置補正用摂動モデル安定化モーメントＭｐｆを上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄに一致させ、かつ、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆを、上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄに一致させると共に、コンプライアンス制御用目標床反力モーメントを補償全床反力モーメントＭｄｍｄとＭｐｆとＭｒｆとの総和に一致させたとした場合に、逆フルモデル２０１が発生する床反力水平成分（Ｆｆｕｌｌに相当）である。
無制限時補正目標床反力水平成分Ｆｉｎは、第３実施形態とは異なり、次式により求められる。
2003061917.tif 000086
ただし、上体水平位置補正用摂動モデル安定化モーメントＭｐｆは前回値（時刻ｔ−Δｔでの値）が用いられる。すなわち、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄと上体水平位置補正用摂動モデル安定化モーメントＭｐｆの差を求め、上体水平位置補正用摂動モデルの入力を前記差だけ増加させることに起因するフルモデル床反力水平成分Ｆｆｕｌｌの増加量を、前記差を上体並進モード床反力比率ｈで割ることにより推定する。さらに、これにフルモデル床反力水平成分Ｆｆｕｌｌを加えることにより、無制限時補正目標床反力水平成分Ｆｉｎが推定される。
次いで、第３実施形態と同様の制限手段（制限処理部２１１）によって、無制限時補正目標床反力モーメントＭｉｎと無制限時補正目標床反力水平成分Ｆｉｎとから、前記制約（ＺＭＰ制約条件および床反力水平成分制約条件）を満足するように、これらに制限を加えた値である（目標ＺＭＰまわりの）制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄとが決定される。この処理手法は、第３実施形態と同一である。
本実施形態においても、コンプライアンス制御用目標床反力モーメントは制限補正目標床反力モーメントＭｌｔｄに一致され、また、補正目標床反力水平成分は制限補正目標床反力水平成分Ｆｌｔｄにほぼ一致するので、上記のごとく制限補正目標床反力モーメントＭｌｔｄと制限補正目標床反力水平成分Ｆｌｔｄを決定することによって、コンプライアンス制御用目標床反力モーメントおよび補正目標床反力水平成分は、それぞれＺＭＰ制約条件および床反力水平成分制約条件をほぼ満足する。
次いで、Ｍｐｆ演算部２１５によって、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが求められる。さらに詳細には、制限補正目標床反力水平成分Ｆｌｔｄからフルモデル床反力水平成分Ｆｆｕｌｌを減じた値にゲインＫｃを乗じたものを積分器２１５ａにて積分し、さらに得られた積分値に上体並進モード床反力比率ｈを乗じることにより、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが求められる。すなわち、次式により、上体水平位置補正用摂動モデル安定化モーメントＭｐｆが得られる。
2003061917.tif 000087
次いで、Ｍｒｆ演算部２１４によって、制限補正目標床反力モーメントＭｌｔｄから上体水平位置補正用摂動モデル安定化モーメントＭｐｆと補償全床反力モーメントＭｄｍｄとを減じることにより、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆが求められる。すなわち、前記式ａ２１により、上体姿勢角補正用摂動モデル安定化モーメントＭｒｆが得られる。
さらに、前記式ａ２３により、上体姿勢角補正用摂動モデル床反力モーメントＭｒが決定される。すなわち、Ｍｒｆ演算部２１４の出力である上体姿勢角補正用摂動モデル安定化モーメントＭｒｆがそのまま上体姿勢角補正用摂動モデル床反力モーメントＭｒとして決定される。
次いで、次式で定義されるフルモデル床反力モーメント誤差ＭｅｒｒがＭｅｒｒ演算部２１６により算出される。
2003061917.tif 000088
次いで、Ｍｐ演算部２１７により、次式に従って、上体水平位置補正用摂動モデル床反力モーメントＭｐが求められる。
2003061917.tif 000089
すなわち、フルモデル床反力モーメント誤差Ｍｅｒｒに積分ゲインＫｍを乗じた値を積分器２１７ａにて積分すると共に、その積分値の符号を反転する。さらに、該積分器２１７ａの出力を上体水平位置補正用摂動モデル安定化モーメントＭｐｆに加えることにより、上体水平位置補正用摂動モデル床反力モーメントＭｐが求められる。
次いで、上体水平位置補正用摂動モデル床反力モーメントＭｐを上体位置補正用摂動モデル２０２に入力して、入力された床反力モーメントに釣り合う補正用摂動モデル上体位置Ｘｃが算出される。
また、上体姿勢角補正用摂動モデル床反力モーメントＭｒを上体姿勢角補正用摂動モデル２０３に入力して、入力された床反力モーメントに釣り合う補正用摂動モデル上体姿勢角θｃが算出される。
求めた補正用摂動モデル上体位置Ｘｃ、補正用摂動モデル上体姿勢角θｃおよび上体水平位置補正用摂動モデル安定化モーメントＭｐｆは、前述のごとく次回制御周期（時刻ｔ＋Δｔ）において、前回値として用いられる。
残余の構成および処理は、第３実施形態と同一である。この第４実施形態によれば、第３実施形態と同様の作用効果を奏することができる。
ここで、前述した第４実施形態と本発明との対応関係を説明しておく。第４実施形態は、前記第１〜１０発明、第１２〜１７発明に対応している。この場合、第４実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、上体姿勢角補正用摂動モデルモーメントＭｒおよび上体水平位置補正用摂動モデルモーメントＭｐが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当し、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄおよび上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄが第２要求操作量に相当する。また、動力学モデルとして、単純化モデル２００（図１０のモデル）と逆フルモデル２０１と上体姿勢補正用摂動モデル２０３と、上体水平位置補正量摂動モデル２０２とが用いられ、単純化モデル２００、上体姿勢補正用摂動モデル２０３および上体水平位置補正量摂動モデル２０２の運動は、上体並進モードと上体回転モードを含んでいる。また、歩容パラメータは図４８のＳ３０２８の処理で適宜修正される。
次に、本発明の第５実施形態を図５７および図５８を参照して説明する。第５実施形態は、前記第３および第４実施形態で用いた逆動力学フルモデル（逆フルモデル）２０１に代えて、順動力学モデル（より正確には擬似順動力学モデル）を用いた歩容を修正しつつ生成する手法である。
図５７は、この第５実施形態に係る装置の動作を説明する機能ブロック図である。同図５７に示すように、本実施形態では、擬似順フルモデル（擬似順動力学フルモデル）２２２を備えている。
この擬似順フルモデル２２２は、上体水平位置安定化モーメント要求値Ｍｐｆｄｍｄ、上体姿勢角安定化モーメント要求値Ｍｒｆｄｍｄ、目標ＺＭＰ、目標床反力鉛直成分および補償全床反力モーメントＭｄｍｄと、目標足平位置姿勢および目標腕姿勢など上体２４を除く部位の運動状態とを入力として、目標上体鉛直位置、目標上体水平位置、目標上体姿勢角、目標ＺＭＰまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメント、および目標床反力水平成分を出力するモデルである。なお、擬似順フルモデル２２２の入力は、前記図４７を参照して説明した如く、歩容パラメータ決定部１００ａで決定される歩容パラメータを基に、目標瞬時値発生部１００ｂにより生成される。
上記擬似順フルモデル２２２は、具体的には図５８の機能ブロック図で表される。なお、この機能ブロック図で前記第４実施形態の図５６と同一機能部分については、図５６と同一の参照符号を用いている。
ここで、同図５８の単純化モデル２００は、単に動力学モデルだけでなく、前述の図４８のＳ３０１０からＳ３０３４の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表す。また、Ｓ３０３２の今回歩容瞬時値（単純化モデル歩容瞬時値）の算出（決定）処理においては、前記第３実施形態で説明したように目標ＺＭＰまわりのモデル操作床反力モーメントを０として歩容の瞬時値を生成した上で、さらに第３実施形態において述べた上体姿勢角補正用摂動モデルモーメントＭｒに相当する上体姿勢角補正用摂動モデルモーメントＭｒ（前回値）を発生する上体回転モードの摂動運動を付加すると共に、第３実施形態において述べた上体水平位置補正用摂動モデルモーメントＭｐに相当する単純化モデル上体水平位置補正用モーメントＭｐ（前回値）を発生する上体並進モードの摂動運動を付加する。これにより、単純化モデル２００が出力する歩容の瞬時値を補正する。
より具体的に説明すると、本実施形態における図４８のＳ３０３２の処理においては、そのサブルーチン処理である図４０のＳ１０１４において、単純化モデル２００で求めた上体水平加速度に、前記式０３の右辺第２項をＭｐ（前回値）に等しいとした式、すなわちＭｐ＝ｍｂ＊（Ｚｂ−Ｚｚｍｐ）＊（ｄ２Ｘｂ／ｄｔ２）という式に基づいて求められる上体水平加速度（ｄ２Ｘｂ／ｄｔ２）を摂動分として加えてなる上体水平加速度を今回歩容の初期から現在時刻ｔまで２階積分することにより、現在時刻ｔにおける上体水平位置の瞬時値が求められる。また、図４０のＳ１０１４において、単純化モデル２００で求めた上体姿勢角加速度に、前記式０３の右辺第７項をＭｒ（前回値）に等しいとした式、すなわち、Ｍｒ＝Ｊ＊ｄ２θｂｙ／ｄｔ２という式に基づいて求められる上体姿勢角加速度（ｄ２θｂｙ／ｄｔ２）を摂動分として加えてなる上体姿勢角加速度を今回歩容の初期から現在時刻ｔまで２階積分することにより、現在時刻ｔにおける上体姿勢角の瞬時値が求められる。
図５８において、単純化モデル２００から先の部分がＳ３０３８の処理に相当する処理を実行する部分である。以下に図５８を用いてＳ３０３８の処理を詳細に説明する。
Ｓ３０３８では、まず、Ｓ３０３２において上記の如く単純化モデル上体水平位置補正用モーメントＭｐ（詳しくは時刻（ｔ−Δｔ）の制御周期での前回値）を基に補正された単純化モデル水平上体位置が目標上体水平位置（時刻ｔでの上体水平位置の最終的な目標瞬時値）として出力される。またさらに、上体姿勢角補正用摂動モデルモーメント  Ｍｒ（詳しくは時刻（ｔ−Δｔ）の制御周期での前回値）を基に補正された単純化モデル上体姿勢角が目標上体姿勢角（時刻ｔでの上体姿勢角の最終的な目標瞬時値）として出力される。
すなわち、式ａ１００、式ａ１０１により、最終的な目標上体水平位置と目標上体姿勢角とが決定される。
2003061917.tif 000090
次いで、目標上体水平位置（すなわち単純化モデル上体水平位置）、目標上体姿勢角（すなわち単純化モデル上体姿勢角）、および前述のごとく得られた単純化モデル歩容の目標全体重心位置、目標足平位置姿勢、目標腕姿勢などの運動変数の瞬時値と目標ＺＭＰの瞬時値とを、前記逆動力学フルモデル（逆フルモデル）２０１に入力し、入力された運動変数で表される運動に釣り合う（すなわち運動によって逆フルモデル２０１が発生する）床反力水平成分と目標ＺＭＰまわりの床反力モーメントとが算出される。以降、第４実施形態と同様、これらの算出された床反力水平成分及び床反力モーメントをそれぞれフルモデル床反力水平成分Ｆｆｕｌｌ、フルモデル床反力モーメントＭｆｕｌｌと呼ぶ。
第４実施形態と同様、フルモデル床反力水平成分Ｆｆｕｌｌは、目標床反力水平成分（時刻ｔでの床反力水平成分の最終的な目標瞬時値）として出力される。
本実施形態では、上体姿勢角補正用摂動モデルと上体水平位置補正用摂動モデルとを備えないので、上体水平位置補正用摂動モデル制御則と上体姿勢角補正用摂動モデル制御則に相当する処理は、後述するように、第４実施形態とは異なる。
このことを除けば、上記処理以降、単純化モデル上体姿勢角補正用モーメントＭｒと単純化モデル上体水平位置補正用モーメントＭｐとを決定するまでは、第４実施形態において、上体姿勢角補正用摂動モデルモーメントＭｒと上体水平位置補正用摂動モデルモーメントＭｐとを決定する処理と同一の処理が実行される。すなわち、Ｍｉｎ演算部２０９、Ｆｉｎ演算部２１０、制限処理部２１１（制限手段）、Ｍｐｆ演算部２１５、Ｍｅｒｒ演算部２１６、Ｍｒｆ演算部２１７（＝Ｍｒ演算部）、及びＭｐ演算部２１４の処理は、前記第４実施形態と同一である。なお、本実施形態では、Ｍｒ、Ｍｐは、それぞれ第４実施形態における上体姿勢角補正用摂動モデルモーメントＭｒと上体水平位置補正用摂動モデルモーメントＭｐとにそれぞれ対応するものであるが、第４実施形態のように摂動モデルに入力するものではなく、単純化モデル２００に入力するものである。このため、Ｍｒ、Ｍｐの名称を本実施形態では、単純化モデルを上体姿勢角補正用モーメント、単純化モデル上体水平位置補正用モーメントを称している。
上記のごとく求められた上体姿勢角補正用摂動モデルモーメントＭｒと単純化モデル上体水平位置補正用モーメントＭｐは、前述のごとく次回制御周期（時刻ｔ＋Δｔ）に、単純化モデル歩容瞬時値の決定（生成）時に前回値として用いられる。
残余の構成及び処理は、第４実施形態と同一である。
以下に、上体姿勢角安定化モーメント要求値Ｍｒｆｄｍｄと上体水平位置安定化モーメント要求値Ｍｐｆｄｍｄとの決定処理について図５７を参照して説明する。
図５７に示すように、本実施形態では、前述したように擬似順フルモデル２２２に備える単純化モデル２００とは別の単純化モデル２２３を備えている。この単純化モデル２２３は、本実施形態でのその機能は、前述した単純化モデル２００と同一であり、単に動力学モデルだけでなく、前述の図４８のＳ３０１０からＳ３０３４の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表すものである。なお、単純化モデル２２３は、実際上は、上体姿勢角の瞬時値と、上体水平位置の瞬時値とを決定できればよく、必ずしも図４３のＳ３０１０からＳ３０３４の処理の全てを行なう必要はない。
以下に図５７を用いて本実施形態におけるＭｐｆｄｍｄおよびＭｒｆｄｍｄの決定処理を詳細に説明する。
本実施形態においては、単純化モデル２２３を用いて生成される歩容と前記擬似順動力学フルモデル２２２を用いて前述の通り算出される歩容との間の上体水平位置および上体姿勢角のそれぞれの差が演算部２２４，２２５により求められる。そして、これらの差に応じて、これらの差が０に収束するようにＰＩＤなどのフィードバック制御則によって上体水平位置安定化モーメント要求値Ｍｐｆｄｍｄおよび上体姿勢角安定化モーメント要求値Ｍｒｆｄｍｄが決定される。すなわち、単純化モデル２２３による上体水平位置と擬似順フルモデル２２２による上体水平位置との差に応じて、フィードバック制御則からなる上体水平位置安定化制御則２２６によって、Ｍｐｆｄｍｄが求められる。また、単純化モデル２２３による上体姿勢角と擬似順フルモデル２２２による上体姿勢角との差に応じて、フィードバック制御則からなる上体姿勢角安定化制御則２２７によって、Ｍｒｆｄｍｄが求められる。そして、この決定されたＭｐｆｄｍｄ及びＭｒｆｄｍｄが、前記擬似順動力学フルモデルにフィードバックされて入力される。
なお、本実施形態では、歩容生成装置１００は、前記擬似順動力学フルモデル２２２への入力の一部である目標ＺＭＰ、目標床反力鉛直成分、目標足平位置姿勢および目標腕姿勢などと、前記擬似順動力学フルモデル２２２からの出力である目標上体鉛直位置、目標上体水平位置、目標上体姿勢角、目標床反力水平成分およびコンプライアンス制御用目標床反力モーメントを最終的な今回歩容の目標瞬時値として出力する。
以上説明した第５実施形態によれば、前記第４実施形態と同様の作用効果を奏することができる。
ここで、前述した第５実施形態と本発明との対応関係を説明しておく。第５実施形態は、前記第１〜１４発明、第１６〜２３発明に対応している。この場合、第５実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、単純化モデル上体姿勢角補正用モーメントＭｒおよび単純化モデル上体水平位置補正用モーメントＭｐが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当し、上体水平位置安定化床反力モーメント要求値Ｍｐｆｄｍｄおよび上体姿勢角安定化モーメント要求値Ｍｒｆｄｍｄが第２要求操作量に相当する。また、動力学モデルとして、単純化モデル２００および逆フルモデル２０１を含む擬似順フルモデル２２２と、単純化モデル２２３とが用いられ、単純化モデル２００，２２３は、上体並進モードと上体回転モードとを含んでいる。また、歩容パラメータは図４８のＳ３０２８の処理で適宜修正される。
なお、前記第５実施形態では、上体姿勢角安定モーメント要求値Ｍｒｆｄｍｄおよび上体水平位置安定化モーメントＭｐｆｄｍｄを擬似順フルモデル２２２のみに入力するようにしたが、ＭｒｆｄｍおよびＭｐｆｄｍｄを単純化モデル２２３に入力するようにしてもよく、あるいは、単純化モデル２２３と擬似順フルモデル２２２とに分配・供給するようにしてもよい。
次に、本発明の第６実施形態を図５９を参照して説明する。図５９は、この発明の第６実施形態に係る装置の動作、具体的には、図４８のフローチャートのＳ３０３８の歩容の修正手法を説明する機能ブロック図である。なお、図５９では、第３実施形態もしくは第４実施形態と同一機能部分については、図４９又は図５６と同一の参照符号を用いる。
第６実施形態においては、上体水平位置補正用摂動モデル２０２と上体姿勢角補正用摂動モデル２０３とを備える。また、２つの分配器２２０，２２１を備える。
分配器２２０，２２１を定義すると、いずれも、１入力２出力の伝達ブロックであり、入力に応じて、一方の出力を決定し（例えば、入力に周波数特性、不感帯特性、飽和特性などの信号処理を行って一方の出力を決定し）、２つの出力の和が入力に一致またはほぼ一致するように、他方の出力を決定する伝達ブロックである。
分配器２２０には、制限補正目標床反力モーメントＭｌｔｄから、上体水平位置補正用摂動モデル安定化モーメントＭｐｆと補償全床反力モーメントＭｄｍｄとを減じたもの（Ｍｒ演算部２１４の出力）である上体姿勢角補正用モーメントＭｒが入力され、上体姿勢角補正用摂動モデル２０３に入力する上体姿勢角補正用摂動モデル入力Ｍｒｉと、単純化モデル２００に入力する単純化モデル上体姿勢角補正用モーメントＭｒｓとに分配される。この時、上体姿勢角補正用摂動モデル入力Ｍｒｉと単純化モデル上体姿勢角補正用モーメントＭｒｓとの和が上体姿勢角補正用モーメントＭｒに一致するように、上体姿勢角補正用摂動モデル入力Ｍｒｉと単純化モデル上体姿勢角補正用モーメントＭｒｓとが決定される（出力される）。
より具体的には、上体姿勢角補正用モーメントＭｒに応じて、上体姿勢角補正用摂動モデル入力Ｍｒｉが決定される。例えば、上体姿勢角補正用モーメントＭｒを不感帯特性、飽和特性、あるいは周波数特性を持つ信号処理を通して、上体姿勢角補正用摂動モデル入力Ｍｒｉが決定される。また、上体姿勢角補正用モーメントＭｒから上体姿勢角補正用摂動モデル入力Ｍｒｉを差し引いたものが、単純化モデル上体姿勢角補正用モーメントＭｒｓとして決定される。さらに具体的に説明すると、本実施形態では、分配器２２０は、例えば、入力（上体姿勢角補正用モーメントＭｒ＝Ｍｌｔｄ−Ｍｐｆ−Ｍｄｍｄ）をローパスフィルタに通してなる低周波成分（直流分）を単純化モデル上体姿勢角補正用モーメントＭｒｓとして出力すると共に、その入力（上体姿勢角補正用モーメントＭｒ）からＭｒｓを差し引いた成分が上体姿勢角補正用摂動モデル入力Ｍｒｉとして出力される。この場合、低周波成分（直流分）である単純化モデル上体姿勢角補正用モーメントＭｒｓに関しては、不感帯特性を持たせておき、上記ローパスフィルタの出力が、ある所定値を中心とする所定範囲内に存在する状態では、Ｍｒｓが該所定値（例えば０）に維持されるようにしておく。
分配器２２０の出力である上体姿勢角補正用摂動モデル入力Ｍｒｉは、前記上体姿勢角補正用摂動モデル２０３に入力され、上体姿勢角補正用摂動モデル２０３により補正用摂動モデル上体姿勢角θｃが決定される。
分配器２２０のもうひとつの出力である単純化モデル上体姿勢角補正用モーメントＭｒｓは単純化モデル２００に入力される。これは、前記第５実施形態における図５８において、単純化モデル上体姿勢角補正用モーメントＭｒを単純化モデル２００に入力することに相当するものである。
分配器２２１においては、フルモデル床反力モーメント誤差ＭｅｒｒにゲインＫｍを乗じた値を積分器２１７ａによって積分し、さらに符号を反転した値を入力とする。
分配器２２１の入力は、分配器２２０と同様、単純化モデル２００に入力する単純化モデル上体水平位置補正用モーメントＭｐｓと、上体水平位置補正用摂動モデル２０２に入力する誤差補正モーメントＭｅとに分配される。より具体的には、積分器２１７ａの出力に応じて、誤差補正モーメントＭｅが決定される。例えば、積分器２１７ａの出力（分配器２２０の入力）を不感帯特性、飽和特性、あるいは周波数特性を持つ信号処理を通して、誤差補正モーメントＭｅが決定される。また、積分器２１７ａの出力から、誤差補正モーメントＭｅを差し引いたものが、単純化モデル上体姿勢角補正用モーメントＭｒｓとして決定される。さらに具体的に説明すると、本実施形態では、分配器２２１は、例えば、入力（積分器２１７ａの出力）をローパスフィルタに通してなる低周波成分（直流分）を単純化モデル上体水平位置補正用モーメントＭｐｓとして出力すると共に、その入力（積分器２１７ａの出力）から  Ｍｐｓを差し引いた成分を誤差補正モーメントＭｅとして出力する。この場合、低周波成分（直流分）である単純化モデル上体水平位置補正用モーメントＭｐｓに関しては、不感帯特性を持たせておき、上記ローパスフィルタの出力が、ある所定値を中心とする所定範囲内に存在する状態では、Ｍｐｓが該所定値（例えば０）に維持されるようにしておく。
分配器２２１の出力である誤差補正モーメントＭｅに上体水平位置補正用モデル安定化モーメントＭｐｆをＭｐ演算部２１７ｂで加算することにより上体水平位置補正用摂動モデルモーメントＭｐが求められる。そして、上体水平位置補正用摂動モデルモーメントＭｐは、上体水平位置補正用摂動モデル２０２に入力され、上体水平位置補正用摂動モデル２０２により補正用摂動モデル上体水平位置Ｘｃが決定される。
分配器２２１のもうひとつの出力である単純化モデル上体水平位置補正用モーメントＭｐｓは単純化モデル２００に入力される。これは、前記第５実施形態の図５８において、単純化モデル上体水平位置補正用モーメントＭｐを単純化モデル２００に入力することに相当するものである。
単純化モデル２００は、前記第５実施例と同様、単純化モデル歩容瞬時値の算出（決定）処理において、目標ＺＭＰまわりに床反力モーメントが発生しないように（モデル操作床反力モーメントを０として）歩容の瞬時値を生成した上で、単純化モデル上体姿勢角補正用モーメントＭｒｓ（前回値）を発生する上体回転モードの摂動運動を付加すると共に、単純化モデル上体水平位置補正用モーメントＭｐｓ（前回値）を発生する上体並進モードの摂動運動を付加して、歩容の瞬時値を補正する。
なお、本実施形態では、図４８のＳ３０２８の処理の一部である図３５のＳ８００において、前回歩容の終端状態として、前回歩容終端における単純化モデルの状態量を用いる。従って、分配器２２０，２２１から単純化モデルに出力されるＭｒｓ及びＭｐｓが０以外の値となる時があれば、単純化モデルは当初の挙動からずれるので、これに応じてＳ３０２８において、歩容パラメータが適宜修正される。また、前記不感帯の領域を大きく設定すると、Ｍｒｓ、Ｍｐｓの絶対値が小さくなるので、歩容パラメータの修正量の絶対値も小さくなる。
その他の構成及び処理は、第４実施例と同一である。より詳しくは、演算部２０４，２０５、Ｍｅｒｒ演算部２１６、上体姿勢角補正用摂動モデル制御則２０７、上体水平位置補正用摂動モデル制御則２０６、Ｍｉｎ演算部２０９、Ｆｉｎ演算部２１０、制限処理部２１１、Ｍｐｆ演算部２１５の処理は、前記第４実施形態と同一である。
なお、本実施形態では、前記第３実施形態で説明したように今回歩容の終端等で各摂動モデル２０２、２０３の状態量により単純化モデル２００の状態量を修正するような処理は行なう必要はない。これは、分配器２２０，２２１から単純化モデル上体姿勢角補正用モーメント  Ｍｒｓ及び単純化モデル上体水平位置補正用モーメントＭｐｓが追加入力されるからである。
かかる本実施形態では、前記第４実施形態あるいは第５実施形態と同様の作用効果を奏することができる。
なお、本実施形態では、各分配器２２０，２２１の２つの出力のうち一方を０とし、他方を入力に一致させるようにしても良い。
この場合、例えば分配器２２０の出力である単純化モデル上体姿勢角補正用モーメントＭｒｓを０にし、分配器２２１の出力である単純化モデル上体水平位置補正用モーメントＭｐｓを０にすると、第４実施形態と同一の作用効果を奏することとなる（事実上、第４実施形態のものと同一構成となる）。
また、分配器２２０の出力である単純化モデル上体姿勢角補正用モーメントＭｒｓを０にし、分配器２２１の出力である誤差補正モーメントＭｅを０にしても良い。この場合、上体姿勢角補正用摂動モデルは省略できる。
また、分配器２２０の出力である上体姿勢角補正用摂動モデル入力Ｍｒｉを０にし、分配器２２１の出力である単純化モデル上体水平位置補正用モーメントＭｐｓを０にしても良い。
また、さらに、誤差補正モーメントＭｅに上体水平位置補正用モデル安定化モーメントＭｐｆを加算した値を図示しない第３の分配器に入力し、その出力の一方を上体水平位置補正用摂動モデルに入力し、もう一方の出力を単純化モデル上体水平位置補正用モーメントＭｐｓに加算しても良い。この場合、分配器２２１の出力である単純化モデル上体水平位置補正用モーメントＭｐｓを０にしても良い。すなわち、分配器２２１を省略し、フルモデル床反力モーメント誤差ＭｅｒｒにゲインＫｍを乗じた値を積分して符号を反転した値を上体水平位置補正用モデル安定化モーメントＭｐｆに加算した値を第３の分配器に入力しても良い。
ここで、前述した第６実施形態と本発明との対応関係を説明しておく。第６実施形態は、前記第１〜１４発明、第１６〜２３発明に対応している。この場合、第６実施形態では、ロボット１の上体姿勢角がロボットの姿勢の状態量に相当し、床反力水平成分が制限対象量に相当し、単純化モデル上体姿勢角補正用モーメントＭｒｓ、単純化モデル上体水平位置補正用モーメントＭｐｓ、上体姿勢角補正用摂動モデル入力Ｍｒｉおよび上体水平位置補正用摂動モデルモーメントＭｐが目標床反力の修正操作量に相当し、補償全床反力モーメントＭｄｍｄが第１要求操作量に相当し、コンプアライアンス制御用床反力モーメントが実床反力操作量に相当し、上体水平位置補正用摂動モデル安定化モーメント要求値Ｍｐｆｄｍｄおよび上体姿勢角補正用摂動モデル安定化モーメント要求値Ｍｒｆｄｍｄが第２要求操作量に相当する。また、動力学モデルとしての単純化モデル２００（図１０のモデル）と逆フルモデル２０１と上体姿勢補正用摂動モデル２０３と、上体水平位置補正量摂動モデル２０２とが用いられ、単純化モデル２００、上体姿勢補正用摂動モデル２０３および上体水平位置補正量摂動モデル２０２の運動は、上体並進モードと上体回転モードを含んでいる。また、歩容パラメータは図４８のＳ３０２８の処理で適宜修正される。
以上説明した第１〜第６実施形態においては、補償全床反力モーメントＭｄｍｄは、上体姿勢角・角速度の代わりに、全体重心水平位置・速度など、ロボット１の他の姿勢に関する状態量に基づいて決定しても良い。
前記第３〜第６実施形態においては、単純化モデル２００による歩容生成（単純化モデル歩容生成）には、本願出願人が先に提案した特開平５−３３７８４９号公報の第１実施例、第２実施例の歩容生成装置を用いても良い。フルモデルによる補正量を小さくすることができるので、補正された歩容が単純化モデル歩容から大きくずれることを防止できる。
前記第１実施形態においては、補償全床反力モーメントＭｄｍｄが床反力モーメント許容範囲を越えると（第２実施形態以降においては、補償全床反力モーメントＭｄｍｄとモデル操作床反力モーメントとの和が床反力モーメント許容範囲を越えると）、コンプライアンス制御用目標床反力モーメントは、床反力モーメント許容範囲の上限値あるいは下限値になる。但し、補償全床反力モーメントＭｄｍｄ（第２実施形態以降においては、補償全床反力モーメントＭｄｍｄとモデル操作床反力モーメントとの和）が床反力モーメント許容範囲を越えていても、補償全床反力モーメントＭｄｍｄ（第２実施形態以降においては、補償全床反力モーメントＭｄｍｄとモデル操作床反力モーメントとの和）の増加もしくは減少に応じてコンプライアンス制御用目標床反力モーメントも増加もしくは減少するようにしても良い。コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲に接近すると、コンプライアンス制御によって制御される実床反力モーメントが目標よりも少なめになる傾向があり、このためコンプライアンス制御用目標床反力モーメントが少々許容範囲を越えても、すぐには、足平２２の本来の接地感を低下させたり、足平２２の底面が浮いてしまうような不具合が発生しない場合が多いからである。
同様に理由により、床反力モーメント許容範囲をいわゆる支持多角形で表されるＺＭＰ存在可能範囲（厳密な表現では、実床反力作用点の存在許容範囲）をモーメントに換算して得られる許容範囲を越えて設定しても良い。
コンプライアンス制御によって発生する床反力モーメントに過度に期待すると、上記のごとく足平２２の本来の接地感を低下させたり、足平２２の底面が浮いてしまうような不具合が発生する。よって、床反力モーメント許容範囲は、姿勢制御がコンプライアンス制御に期待する実床反力モーメントの許容範囲であると言える。
床反力モーメント許容範囲は、歩容パラメータからのみならず、検出された実床反力にも基づいて決定しても良い。また、階段でのエッジ位置検出値など、足平２２の接地領域の検出値にも基づいて決定してもよい。
前記第１実施形態の図１２のＳ０２８における今回歩容パラメータの修正（目標ＺＭＰの修正）を制御周期毎に実行するように、メインフローチャートの処理を変更しても良い。
修正歩容（目標歩容）が元歩容より大きくずれてしまう場合（発散してしまう場合）には、次回歩容の歩容パラメータ修正をする頃に既に、大きくずれて過ぎてしまい（発散し過ぎてしまい）、次回歩容の歩容パラメータ修正だけでは、継続的長期的に安定な目標歩容を生成することが困難になる。今回歩容パラメータの修正（目標ＺＭＰの修正）を制御周期毎に実行することによって、この問題をかなり解決することができる。
さらには、元歩容からのずれ具合によって、今回歩容の足平着地位置や着地時刻などを制御周期毎に変更しても良い。
具体的には、Ｓ０２０からＳ０２９までの処理を制御周期毎に実行するように処理フローを変更しておき、Ｓ０２０において、次回歩容支持脚座標系（次回歩容支持脚座標系は、次の足平着地位置姿勢に対応）、次次回歩容支持脚座標系（次次回歩容支持脚座標系は、次の次の足平着地位置姿勢に対応）、今回歩容周期（今回歩容周期は、次の足平着地時刻に対応）および次回歩容周期（次回歩容周期は、定常歩容の周期に相当）の少なくともいずれかを、Ｓ０２８における今回歩容パラメータの修正（特に目標ＺＭＰの修正）が小さくなるように（すなわち、今回歩容の安定余裕が高く維持されるように）、適宜変更すれば良い。
さらには、上記以外の歩容パラメータを変更しても良い。
また、修正歩容の元歩容からのずれは、単純化モデル上体姿勢角補正用モーメントＭｒおよび単純化モデル上体水平位置補正用モーメントＭｐなどから動力学モデルを用いて推定することができるので、Ｍｒ，Ｍｐに応じてモデルの挙動ずれを推定し、推定された挙動ずれを基に、歩容パラメータを修正してもよい。あるいは、Ｍｒ，Ｍｐと歩容パラメータの修正量の適正値との関係をあらかじめ求めてマップ化しておいて、Ｍｒ，Ｍｐに応じてマップを基に歩容パラメータの修正量を決定してもよい。
また、他の実施形態（第２〜第６実施形態）に対しても上記と同様に、処理フローを変更して良い。
復元条件には、前記した条件以外にも、関節角が許容範囲を越えていないか、脚などの干渉がないか、関節角速度、トルクが過大でないかなどのキネマティクス条件、ダイナミクス条件を加えても良い。
また、これに伴って、前述したごとく、今回歩容パラメータの修正（目標ＺＭＰあるいは着地位置、時刻などの修正）を制御周期毎に実行するようにメインフローチャートの処理を変更した場合には、今回歩容の安定余裕が高く維持されるように適宜変更される歩容パラメータの値が、適切な値になるように（ある制約条件を満足するように）することも、復元条件のひとつに加えても良い。
歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻は、上位の制御装置からの指示（歩行計画決定部やオペレータからの指示など。これを当初の要求と呼ぶ。）によって決定されるので、修正歩容の着地位置、着地時刻は、できる限り歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻に戻るべきである。そこで、歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻を記憶し、修正歩容の着地位置、着地時刻が、記憶された着地位置、着地時刻にできる限り一致または近づくことも復元条件に加えても良い。ただし、実は、前記復元条件３および復元条件４によって、修正歩容の着地位置、着地時刻は、できる限り歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻に徐々に戻る作用が働くので、あえて追加する必要はない。
また、状況の変化に対応して、当初の要求を変更し、変更した要求を満足する歩容パラメータに、上記のごとく歩容の安定余裕を高く維持するために適宜変更される歩容パラメータが、できる限り一致または近づくことを復元条件に加えても良い。この場合、前記復元条件３および復元条件４は削除するべきである。
前述した各種の復元条件を満足するモデル上体水平位置安定化床反力モーメントとモデル上体姿勢角安定化床反力モーメントの決定法には、制約条件下で最適値を求めるための線形計画法（シンプレックス法など）や探索法を用いれば良い。または、ファジー推論を用いても良い。
また、着地位置を変更する場合には、歩行環境の障害物なども考慮しなければならない状況も考えられる。このような状況にも対処できるようにするためには、環境認識や行動決定などの人工知能の分野に属する処理も加えて、修正歩容を決定すべきである。
前記第３〜第６実施形態における制限手段（制限処理部２１１）の前後のブロック図は、例えば本願出願人が２００２年１２月２６日付けで出願したＰＣＴ出願（名称：制御システム）で示したように、等価変換あるいは近似変換しても良い。
この場合、上体水平位置補正用モデル安定化モーメントＭｐｆをｈで割った値をローパスフィルタに通した値を補正目標床反力水平成分（フルモデル床反力水平成分Ｆｆｕｌｌ）から減じた値が上記ＰＣＴ出願におけるバイアス推定値、ｄｅｓｔｍに相当する。したがって、例えば、前記第４〜第６実施形態におけるゲインＫｃの積分を、時定数１／Ｋｃの１次遅れをフィードバック要素としたポジティブフィードバック系に置き換えたりして、図６０のように、近似変換をしても良い。この例では、第４〜第６実施形態における  Ｍｐｆ演算部２１５が図示の如く近似変換されている。
前記第１実施形態においては、本願出願人が提案した特開平５−３３７８４９号公報に記載の構成に加え、床反力の並進力水平成分が床反力水平成分の許容範囲を越えないように、床反力水平成分と目標ＺＭＰまわりの床反力モーメントとの発生の割合が異なる２つの運動モードの挙動、例えば、上体並進運動モードの上体並進加速度と上体回転運動モードの上体姿勢角加速度が決定されるので、実ロボット１を、修正目標歩容（最終的に歩容生成装置１００が出力する歩容）に収束させることができる。つまり、実ロボット１の姿勢を安定化させることができる。
また、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントの差がトータルの復元力になる。
また、モデル操作床反力モーメントは、ＺＭＰ存在可能範囲を無視していかなる値でも取ることが可能であるので、非常に高い姿勢復元力を発生することができる。
また、床反力の並進力水平成分が、床反力水平成分の許容範囲を越えないので、ロボット１のスリップを防止できる。
また、床反力鉛直成分が０の時期、すなわち、両脚体２，２がいずれも接地していない時期では、上体並進運動モードに依存せずに上体回転運動モードに依存した姿勢復元が行われ、床と足平２２の間の摩擦力に依存せずに効果的に姿勢復元が行われる。
また、実床反力モーメントが過大になることを防止するので、足平２２の本来の接地性が低下したり、足平２２の底面が浮いてしまうような不具合が発生することを防止または抑制することができる。
また、１歩分の修正歩容の終端状態を新たな初期状態とした新たな今回歩容が、定常歩容に漸近するように、今回歩容パラメータが決定または変更されるので、継続的に（長期的に）安定性が保証された歩容を生成し続けることができる。
前記第２実施形態においては、上記のごとく、元歩容と修正歩容が同時に生成され、修正歩容は、実ロボット１の姿勢安定化のために修正されると共に、コンプライアンス制御により姿勢復元に必要な床反力モーメントを発生してもまだ余裕がある場合には、この余裕を用いて、可能な範囲で元歩容に収束するようにしている。このため、第１実施形態の効果に加え、当初設定した元歩容に近い、すなわち、当初の要求通りの歩容に近い歩容を生成することができる。したがって、あらかじめ設定された移動経路がある場合には、移動経路から大きくずれることを防止できる。また、修正歩容の上体姿勢角が元歩容（当初決定した歩容）の上体姿勢角に収束することを、修正歩容の上体水平位置が元歩容（当初決定した歩容）の上体水平位置に収束することよりも優先したので、上体姿勢角が大きく変動することを抑制することができる。
前記第１〜第６実施形態では、床反力水平成分許容範囲を設定するようにしたが、床反力水平成分とロボット全体重心水平加速度は、比例関係にあるので、前記各実施形態における床反力水平成分およびその許容範囲の代わりに、ロボット全体重心水平加速度およびその許容範囲を用いても構わない。また、ロボット全体重心水平軌道に近い挙動をする部位の水平加速度軌道に関するパラメータを明示的に設定しても構わない。例えば、脚体２，２の質量が上体２４の質量に比べ十分に小さい場合には、上体水平加速度軌道とロボット１の全体重心水平加速度軌道は、ほぼ同じか比例関係にあるので、床反力水平成分およびその許容範囲の代わりに上体水平加速度およびその許容範囲を用いても良い。
さらに、斜面移動時（傾斜した床面でロボット１を移動させるとき）の歩容生成においては、床反力水平成分許容範囲や全体重心加速度水平成分の許容範囲の代わりに、並進床反力の床面平行成分（床面に平行な成分）、すなわち摩擦力の許容範囲、あるいは全体重心加速度の床面平行成分（これは重力成分を除けば摩擦力に比例する）の許容範囲を設定するようにしてもよい。例えば、並進床反力の床面平行成分（摩擦力）の許容範囲を設定する場合に関して説明（この説明は全体重心加速度の床面平行成分の許容範囲を設定する場合でも同様である）すると、該摩擦力は、床面の水平面に対する傾斜角度をθｆ（ロボット１の進行方向に向かって前下がり斜面の場合を正とする）とすると、次式ｃ７２の関係が成立する。従って、前記実施形態と同様のアルゴリズムに歩容を生成する場合、この式ｃ７２の関係を用いて、摩擦力許容範囲を床反力水平成分許容範囲に変換することで、該床反力水平成分許容範囲を設定するようにすればよい。なお、この場合、式ｃ７２の床反力鉛直成分には、目標床反力鉛直成分を用いればよい。
2003061917.tif 000091
床反力水平成分と目標ＺＭＰまわりの床反力モーメントとを適切な値にするために、前記の実施形態では、上体回転モードと上体並進モードとの２つの運動モードを用いたが、これら以外の運動モードを用いても良い。この場合、運動モードのひとつが床反力水平成分を発生しない運動モードである必要もない。いかなるモードの組み合わせであっても、床反力水平成分と目標ＺＭＰまわりの床反力モーメントの発生の割合が異なる２つの運動モードを用いさえすれば、上記例のように、任意の床反力水平成分と目標ＺＭＰまわりの床反力モーメントとを発生させることができるからである。
また、上体姿勢以外の運動モードを用いても良い。ただし、なるべく小さい変位で大きな床反力水平成分または目標ＺＭＰまわりの床反力モーメントが発生できる運動モードを選ぶべきである。
例えば、左右の腕を同一回転方向に振りまわす運動モード、接地していない（空中に存在する）足平の位置を摂動させる運動モードでも良い。ただし、遊脚軌道を摂動させる場合には、着地位置が変わらないように着地直前までには、摂動量を実質的に０に戻すべきである。
また、３つ以上の運動モードを用いても良い。
また、選んだモードのうちの少なくとも２つは、床反力水平成分と目標ＺＭＰまわりの床反力モーメントとの発生比率が、互いに異なるものである必要がある。そうでないと、一般的に連立方程式の解がなくなるからである。
さらにできる限り、床反力水平成分をあまり変化させないで目標ＺＭＰまわりの床反力モーメントを十分に大きく変化させることができる運動モードと、目標ＺＭＰまわりの床反力モーメントをあまり変化させないで床反力水平成分を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。
言いかえると、全体重心をあまり変化させないで角運動量を十分に大きく変化させることができる運動モードと、角運動量をあまり変化させないで全体重心を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。運動モードの変位が小さくなるからである。
前記単純化モデルとして、前記実施形態に用いた動力学モデル以外にも以下のモデルを用いても良い。
１）図１２に示したように複数のリンクに質点を設定した非線形なモデル（多質点モデル）
２）本願出願人による特願２０００−３５２０１１号に示された３質点モデル
３）上体にのみ質量がある１質点モデル
４）全体重心まわりの角運動量変化によって生じる慣性力のモーメントを無視したモデル
５）重力と慣性力との合力（または床反力）と上体並進運動との関係を表す部分モデルと、上記合力と上体回転運動の関係を表す部分モデルを分離して持つ分離型モデル。例えば、図１０に示す質点は、上記合力と上体並進運動との関係を表す部分モデルであり、図１０に示すフライホイールは、上記合力と上体回転運動との関係を表す部分モデルである。
ただし、単純化モデルに単純化モデル上体姿勢角補正用モーメントが加えられる実施形態においては、上記２）、３）、および４）のモデルを用いることはできない。
尚、フルモデルは、基本的には、単純化モデルよりもより近似精度の高い動力学モデルを用いることが好ましいが、単純化モデルと同等の近似精度の動力学モデルを用いてもよい。
また、前述した各実施形態において、ブロック線図、フローチャート、およびアルゴリズムなどは、演算処理順序を変えるなどの等価変形をしても良い。また、適宜ローパスフィルタを挿入しても良い。
また、前記各実施形態を２足移動ロボットに関して説明してきたが、１足もしくは３足以上の多脚ロボットにも本発明を適応することができる。
産業上の利用可能性
以上のように本発明は２足移動ロボット等の脚式移動ロボットを、スリップ等を生じることなく安定した姿勢で円滑に移動させることが可能なロボットの動作制御を行なうことができるものとして有用である。
【図面の簡単な説明】
図１は本発明の実施形態における脚式移動ロボットとしての２足移動ロボットの全体的構成の概略を示す概略図、図２は図１のロボットに備えた制御ユニットの構成を示すブロック図、図３は第１実施形態における制御ユニットの機能的構成を示すブロック図、図４は図３の補償全床反力モーメント分配器１１０の機能を示すブロック図、図５はロボットの走行歩容を例示する説明図、図６は目標床反力鉛直成分の設定例を示す線図、図７は目標ＺＭＰの設定例を示す線図、図８及び図９はロボットの運動モード（上体並進モード及び上体回転モード）の説明図、図１１０及び図１１はロボットの動力学モデルの説明図である。図１２は第１実施形態における歩容生成装置のメインルーチン処理を示すフローチャート、図１３はロボットの上体位置の発散状態を示す説明図、図１４は図１２のフローチャートサブルーチン処理を示すフローチャート、図１５は定常歩容足平着地位置姿勢及び座標系の関係を示す説明図、図１６は定常歩容の目標床反力鉛直成分の設定例を示す線図、図１７は定常歩容の床反力水平成分許容範囲の設定例を示す線図、図１８は定常歩容の目標ＺＭＰの設定例を示す線図、図１９は図１２のフローチャートのサブルーチン処理を示すフローチャート、図２０は図１９のフローチャートのサブルーチン処理を示すフローチャート、図２１は図２０のフローチャートのサブルーチン処理を示すフローチャート、図２２は図２１のフローチャートのサブルーチン処理を示すフローチャートである。図２３〜図３０は、図２１のフローチャートの要部の処理を説明するための線図、図３１は定常歩容における上体位置の軌道の例を示す説明図、図３２は図１２のフローチャートのサブルーチン処理を示すフローチャート、図３３は今回歩容の床反力水平成分許容範囲の設定例を示す線図、図３４は図１２のフローチャートのサブルーチン処理を示すフローチャート、図３５は図３４のフローチャートのサブルーチン処理を示すフローチャート、図３６は図３５のフローチャートのサブルーチン処理を示すフローチャート、図３７は図３６のフローチャートのサブルーチン処理を示すフローチャート、図３８は図３７のフローチャートの要部の処理を説明するための線図、図３９はＺＭＰ許容範囲の設定例を示す説明図である。図４０は図１２のフローチャートのサブルーチン処理を示すフローチャート、図４１は図４０のフローチャートのサブルーチン処理を示すフローチャート、図４２は歩行歩容における目標床反力鉛直成分の設定例を示す線図である。図４３は第２実施形態における制御ユニットの機能的構成を示すブロック図、図４４は第２実施形態における歩容生成装置のメインルーチン処理を示すフローチャート、図４５は図４４のフローチャートのサブルーチン処理を示すフローチャート、図４６は図４５のフローチャートのサブルーチン処理を示すフローチャート、図４６は図４５のフローチャートのサブルーチン処理を示すフローチャートである。図４７は第３実施形態における歩容生成装置の要部構成を機能的に示すブロック図、図４８は第３実施形態における歩容生成装置のメインルーチン処理を示すフローチャート、図４９は第３実施形態における歩容生成手法を機能的に示すブロック図、図５０および図５１は摂動モデルの説明図、図５２〜図５５は制限対象量（床反力水平成分および床反力モーメント）の制限手法の説明図である。図５６は第４実施形態における歩容生成手法を機能的に示すブロック図、図５７および図５８は第５実施形態における歩容生成手法を機能的に示すブロック図、図５９は第６実施形態における歩容生成手法を機能的に示すブロック図、図６０は第４〜第６実施形態における要部の変形態様を示すブロック図、図６１および図６２は従来技術を説明するための説明図である。
【図１】
2003061917.tif 000092
【図２】
2003061917.tif 000093
【図３】
2003061917.tif 000094
【図４】
2003061917.tif 000095
【図５】
2003061917.tif 000096
【図６】
2003061917.tif 000097
【図７】
2003061917.tif 000098
【図８】
2003061917.tif 000099
【図９】
2003061917.tif 000100
【図１０】
2003061917.tif 000101
【図１１】
2003061917.tif 000102
【図１２】
2003061917.tif 000103
【図１３】
2003061917.tif 000104
【図１４】
2003061917.tif 000105
【図１５】
2003061917.tif 000106
【図１６】
2003061917.tif 000107
【図１７】
2003061917.tif 000108
【図１８】
2003061917.tif 000109
【図１９】
2003061917.tif 000110
【図２０】
2003061917.tif 000111
【図２１】
2003061917.tif 000112
【図２２】
2003061917.tif 000113
【図２３】
2003061917.tif 000114
【図２４】
2003061917.tif 000115
【図２５】
2003061917.tif 000116
【図２６】
2003061917.tif 000117
【図２７】
2003061917.tif 000118
【図２８】
2003061917.tif 000119
【図２９】
2003061917.tif 000120
【図３０】
2003061917.tif 000121
【図３１】
2003061917.tif 000122
【図３２】
2003061917.tif 000123
【図３３】
2003061917.tif 000124
【図３４】
2003061917.tif 000125
【図３５】
2003061917.tif 000126
【図３６】
2003061917.tif 000127
【図３７】
2003061917.tif 000128
【図３８】
2003061917.tif 000129
【図３９】
2003061917.tif 000130
【図４０】
2003061917.tif 000131
【図４１】
2003061917.tif 000132
【図４２】
2003061917.tif 000133
【図４３】
2003061917.tif 000134
【図４４】
2003061917.tif 000135
【図４５】
2003061917.tif 000136
【図４６】
2003061917.tif 000137
【図４７】
2003061917.tif 000138
【図４８】
2003061917.tif 000139
【図４９】
2003061917.tif 000140
【図５０】
2003061917.tif 000141
【図５１】
2003061917.tif 000142
【図５２】
2003061917.tif 000143
【図５３】
2003061917.tif 000144
【図５４】
2003061917.tif 000145
【図５５】
2003061917.tif 000146
【図５６】
2003061917.tif 000147
【図５７】
2003061917.tif 000148
【図５８】
2003061917.tif 000149
【図５９】
2003061917.tif 000150
【図６０】
2003061917.tif 000151
【図６１】
2003061917.tif 000152
【図６２】
2003061917.tif 000153
【国際調査報告】
2003061917.tif 000154
2003061917.tif 000155