JP4485279B2 - 脚式移動ロボットの歩容生成装置および制御装置 - Google Patents

Description

本発明は、脚式移動ロボットに床反力以外の外力が適宜作用する状態でのロボットの歩容を生成する歩容生成装置とロボットの動作制御を行なう制御装置とに関する。

２足移動ロボットなどの脚式移動ロボットによりある対象物を押すなどの作業を行なう場合には、ロボットはその脚体の先端部が接触する床から床反力を受けるだけでなく、対象物から反力を受ける。このようにロボットに外部環境から作用する、床反力以外の力を本明細書では外力という。特に対象物からロボットが受ける外力（反力）を対象物反力という。なお、外力には重力は含まれないものとする。

脚式移動ロボットに外力が作用する状態でロボットの歩容を生成したり、動作制御を行なう技術としては、例えば本願発明者による特開平１０−２３０４８５号公報（特許文献１）に見られるものが知られている。この技術は、ロボットの運動により発生する慣性力と、ロボットに作用する重力および外力との合力が目標ＺＭＰ（目標床反力中心点）まわりに発生するモーメントの鉛直成分を除く成分（水平成分）が０になる（目標床反力中心点に作用する床反力と上記合力とが釣り合う）という動力学的平衡条件を満たすように、目標歩容を生成するものである。さらに、この技術では、ロボットに作用する外力が予期しない外力になっても（目標外力と実外力との偏差がある程度大きくなっても）、ロボットの重心位置がロボットの動バランスを保ち得るような重心位置に平衡するようにロボットの目標歩容を調整するようにしている。

一方、ロボットの目標歩容をリアルタイムに生成する技術として、本出願人が先に提案したＰＣＴ国際公開公報WO／02／40224A1（特許文献２）に見られる技術も知られている。この技術は、目標歩容の上***置姿勢を、仮想的な周期歩容である定常旋回歩容の上***置姿勢に収束させるように生成することで、ロボットの継続的な安定性を確保しつつ（ロボットの運動状態の発散を防止しつつ）、リアルタイムにロボットの目標歩容を生成するようにしたものである。
特開平１０−２３０４８５号公報 ＰＣＴ国際公開公報WO/02/40224A1

前記特許文献１に見られる技術では、基本的な目標歩容は、単に、前記した動力学的平衡条件が満たされるように生成される。このため、予期しない外力変化に対して、基本的な目標歩容が、本来の動力学的平衡条件を満たす歩容に対して大きくずれる場合がある。そして、そのような場合には、重心位置を平衡させるようにロボットの上***置姿勢を調整しても、即座にはロボットの動バランスを確保することができずに、ロボットの継続的な運動を安定に行なうことができなくなる恐れがあった。また、ある対象物にロボットが接触していない状態から、ロボットがその腕体などを対象物に接触させて該対象物を移動させる作業を開始する際などのように外力変化が急激に生じる場合にも、上記と同様に、ロボットの継続的な運動を安定に行なうことができなくなる恐れがあった。

また、特許文献２に見られる技術では、外力が考慮されておらず、外力がロボットに作用する環境下では、ロボットの継続的な安定性を確保できる目標歩容を生成することができないものとなっていた。

本発明はかかる背景に鑑みてなされたものであり、ロボットによりある対象物を移動させる場合に該ロボットに対象物から作用する反力など、ロボットに外力が適宜作用する環境下で、その外力が急激に変化するような場合であってもロボットの継続的な安定性を確保できる目標歩容を生成できる歩容生成装置および制御装置を提供することを目的とする。さらには、その外力が必ずしも予想通りにならならないような場合でも、ロボットの継続的な安定性を確保できる目標歩容を生成できる歩容生成装置および制御装置を提供することを目的とする。

本発明の脚式移動ロボットの歩容生成装置は、上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための該ロボットの歩容を生成する装置において、
少なくとも前記対象物の運動と該対象物に作用する力との関係を表わす対象物動力学モデルと前記対象物の移動計画（対象物をどの時点でどの位置に移動させるかなどの計画）とを基に、前記対象物からロボットに作用する反力としての外力の目標軌道と前記対象物の目標運動軌道とを仮決定する対象物軌道仮決定手段と、
少なくとも前記仮決定された対象物の目標運動軌道と外力の目標軌道とを基に、新たに作成しようとする所定期間分のロボットの目標歩容である今回歩容における脚体の運動軌道を規定するパラメータとロボットに作用させるべき床反力の軌道を規定するパラメータとロボットに作用させるべき外力の軌道を規定するパラメータとを少なくとも含む今回歩容パラメータを仮決定する今回歩容パラメータ仮決定手段と、
少なくとも前記仮決定された対象物の目標運動軌道と外力の目標軌道とを基に、前記今回歩容に続く仮想的な周期的歩容における脚体の運動軌道を規定するパラメータとロボットに作用させるべき床反力の軌道を規定するパラメータとロボットに作用させるべき外力の軌道を規定するパラメータとを少なくとも含む周期的歩容パラメータを決定する周期的歩容パラメータ決定手段と、
前記ロボットに作用する床反力および外力とロボットの運動との関係を表すロボット動力学モデルと前記今回歩容パラメータとに基づいて求められる該ロボットの上体の運動軌道が、該ロボット動力学モデルと前記周期的歩容パラメータとに基づいて求められる該ロボットの上体の運動軌道に収束するという条件を満足するように、前記仮決定された今回歩容パラメータのうち、前記脚体の運動軌道を規定するパラメータと前記床反力の軌道を規定するパラメータと前記外力の軌道を規定するパラメータとの少なくともいずれか１つのパラメータを修正して今回歩容パラメータを決定するパラメータ修正手段と、
少なくとも前記修正された今回歩容パラメータに基づいて前記今回歩容の瞬時値を逐次決定する今回歩容瞬時値決定手段とを備えたことを特徴とする（第１発明）。

かかる第１発明によれば、今回歩容パラメータは、これと前記ロボット動力学モデルとに基づいて求められるロボットの上体の運動軌道が、該動力学モデルと前記周期的歩容パラメータとに基づいて求められるロボットの上体の運動軌道に収束するという条件を満足するように決定される。周期的歩容は、１周期分の同じ形態のロボットの運動を繰り返す歩容であるから、ロボットの運動の継続的な安定性を保証し得る歩容である。そして、この場合、周期的歩容パラメータには、その周期的歩容における外力（対象物からロボットが受ける反力）の軌道を規定するパラメータが含まれる。このため、今回歩容に続く将来の外力（対象物からロボットが受ける反力）の変化を考慮して、今回歩容パラメータを決定できる。その結果、この決定された今回歩容パラメータに基づいて今回歩容の瞬時値を決定したとき、将来の外力変化を加味した今回歩容の瞬時値の時系列（より詳しくは、将来の外力変化によってもロボットの継続的な運動を行い得るような今回歩容の瞬時値の時系列）を生成することができる。

従って、第１発明によれば、ロボットに対象物から反力が作用する場合に、その反力が急激に変化するような場合であってもロボットの継続的な安定性を確保できる目標歩容を生成できる。

かかる第１発明では、前記パラメータ修正手段が修正対象とするパラメータは、例えば前記今回歩容パラメータのうち、少なくとも前記床反力の軌道を規定するパラメータを含む。そして、この場合、該パラメータ修正手段は、前記仮決定された今回歩容パラメータのうち、前記床反力の軌道を規定するパラメータを前記条件を満足するように修正したときの該パラメータの修正量が所定量を超えるとき、該修正量を小さくしつつ前記条件を満足するように、前記仮決定された今回歩容パラメータのうちの少なくとも前記外力の軌道を規定するパラメータを修正する手段を備えることが好ましい（第３発明）。

今回歩容パラメータのうち、床反力の軌道を規定するパラメータ（例えば床反力の作用点を規定するパラメータ）を修正対象とすることで、多くの場合、前記条件を満足するように今回歩容パラメータを決定できる。但し、床反力の軌道を適切な軌道から大きくずらすと、ロボットのＺＭＰがロボットの安定性を余裕もって確保し得る領域から逸脱する場合がある。ここで、ＺＭＰは、ロボットの運動によって発生する慣性力と、ロボットに作用する床反力以外の力（すなわち、重力と前記外力）との合力が、その点まわりに発生するモーメントの水平成分が０になるような床上の点である。

そこで、第３発明では、今回歩容における床反力の軌道を規定するパタメータの修正量が所定量を超えるときには、該修正量を小さくしつつ前記条件を満足するように、前記仮決定された今回歩容パラメータのうちの少なくとも前記外力の軌道を規定するパラメータを修正する。このようにすることで、床反力の軌道を規定するパラメータの適切な修正が困難であるときでも、前記条件を満足するように今回歩容パラメータを修正できる。また、外力の軌道を規定するパラメータを修正することで、今回歩容の脚体運動を規定するパラメータのような今回歩容パラメータのうちの運動パラメータを修正することなく、外力を調整することによって、前記条件を満足するように今回歩容の瞬時値が決定されることとなる。従って、ロボットの脚体の運動などを即座には調整することができないような状況（例えば脚体の着地直前など）であっても、ロボットの継続的な安定性を確保できる今回歩容の瞬時値の時系列を生成できると共に、外力を利用して、ロボットの急加速や急減速を行なうことができる。

上記のように前記パラメータ修正手段が修正対象とするパラメータが、前記今回歩容パラメータのうち、少なくとも前記床反力の軌道を規定するパラメータを含む場合において、該パラメータ修正手段は、前記仮決定された今回歩容パラメータのうち、前記床反力の軌道を規定するパラメータを前記条件を満足するように修正したときの該パラメータの修正量が所定量を超えるとき、該修正量を小さくしつつ前記条件を満足するように、前記仮決定された今回歩容パラメータのうちの少なくとも前記脚体の運動軌道を規定するパラメータを修正する手段を備えるようにしてもよい（第４発明）。

このように脚体の運動軌道を規定するパラメータを修正するようにしても、床反力の軌道を規定するパラメータの適切な修正が困難である場合に、前記条件を満足するように今回歩容パラメータを修正できる。

上記第３および第４発明では、前記床反力の軌道を規定するパラメータは、ロボットの目標ＺＭＰの軌道を規定するパラメータであることが好適である（第５発明）。

この第５発明によれば、前記パラメータ修正手段が修正対象とするパラメータは、目標ＺＭＰを含むので、ロボットの脚体のうちの接地している個数によらずに、ロボットの安定性の余裕を確保し得る今回歩容パラメータを決定できる。

また、前記第１発明では、前記パラメータ修正手段は、少なくとも前記パラメータ修正手段により修正された今回歩容パラメータと前記周期的歩容パラメータと、前記ロボット動力学モデルとを基に前記ロボットの将来の運動を予測するロボット将来挙動予測手段と、前記予測されたロボットの将来の運動と、前記仮決定された前記対象物の目標運動軌道とを基に、該ロボットと対象物とが所定の幾何学的制約条件を満たしているか否かを判断し、満たしていない場合には前記対象物の目標運動軌道と前記修正された今回歩容パラメータのうちの脚体の運動軌道を規定するパラメータとのうちの少なくともいずれか一方を修正して今回パラメータを決定する手段とを備えることが好ましい（第６発明）。

この第６発明では、修正された今回歩容パラメータと周期的歩容パラメータとロボット動力学モデルとを基に前記ロボットの将来の運動を予測し、その予測されたロボットの将来の運動と、対象物の目標運動軌道とを基に、該ロボットと対象物とが所定の幾何学的制約条件を満たしているか否か、例えば将来のある時点でロボットと対象物とが干渉する事態が発生するか否か、ロボットと対象物とが近づき過ぎて、ロボットの対象物に係合させる部位（腕体など）の関節の回転角が過剰になるか否かなどの幾何学的制約条件が満たされているか否かが判断される。そして、幾何学的制約条件が満たされていない場合には、対象物の目標運動軌道と修正された今回歩容パラメータのうちの脚体の運動軌道を規定するパラメータとのうちの少なくともいずれか一方を修正して今回パラメータを決定する。これにより、ロボットと対象物との適正な位置関係（前記幾何学的制約条件が満たされるような位置関係）が将来的に保たれるように今回歩容パラメータを決定できると共に、適正な位置関係が保たれない恐れがある場合には、事前にそれを解消し得る適切な今回歩容パラメータを決定できるので、ロボットの継続的な運動を中断したりすることなく円滑に行なうことができる。

なお、この第６発明は、前記第３〜第５発明と複合させてもよい。

また、前記第１発明では、前記対象物に作用する力のうち、前記今回歩容瞬時値に応じて動作している前記ロボットから前記対象物に作用する力以外の外乱力を推定する対象物外乱力推定手段を備え、前記対象物仮軌道決定手段は、前記対象物動力学モデルと前記移動計画と前記推定された外乱力とを基に前記対象物の目標運動軌道と前記外力の目標軌道とを仮決定する手段であることが好適である（第７発明）。

この第７発明によれば、前記外乱力（例えば、移動計画で想定されていない、対象物に作用する摩擦力もしくは制動力や、床の傾斜面で対象物に作用する重力の傾斜面方向成分など）を推定し、その外乱力を考慮して、対象物動力学モデルを用いて対象物の目標運動軌道と外力の目標軌道とを仮決定する。このため、対象物からロボットに作用する外力（反力）が必ずしも予想通りにならないような場合であっても、実際の環境に則して対象物の目標運動軌道と外力の目標軌道とを仮決定できる。これらの目標運動軌道と外力の目標軌道とを基に、その結果、ロボットに作用する外力（反力）が必ずしも予想通りにならないような場合であってもロボットの継続的な安定性を確保できる目標歩容を生成できる。

なお、この第７発明は、前記第３〜第６発明と複合させてもよい。

かかる第７発明では、前記対象物軌道仮決定手段は、前記移動計画に対応する対象物の運動軌道と前記仮決定した対象物の目標運動軌道との間の差と、前記推定された外乱力とのうちの少なくともいずれか一方に基づき、前記移動計画を修正する手段を備えることが好適である（第８発明）。

すなわち、外乱力が変化すると、前記移動計画に対応する対象物の運動軌道と前記仮決定した対象物の目標運動軌道との間のずれが発生し、そのずれがある程度大きくなると、対象物を当初の移動計画に則して移動させるためのロボットの挙動変化が過大になる恐れがある。そこで、第８発明では、前記移動計画に対応する対象物の運動軌道と前記仮決定した対象物の目標運動軌道との間の差と、前記推定された外乱力とのうちの少なくともいずれか一方に基づき、前記移動計画を修正する。このようにすることで、ロボットの過大の挙動変化を避けつつ、ロボットの安定性の高い運動を継続することができる。また、移動計画を実際の環境に則した適切な計画に修正することができる。

次に、本発明の脚式移動ロボットの制御装置（第９発明）は、前記第１、第３〜第８発明のいずれかの脚式移動ロボットの歩容生成装置により生成された今回歩容瞬時値に応じて該ロボットの動作を制御する制御装置において、
前記ロボットに作用する実床反力以外の実外力と、前記今回歩容瞬時値のうちの前記外力の瞬時値との偏差に応じて、該偏差を０に近づけるように、前記ロボットの今回歩容瞬時値のうちの前記ロボットの目標運動と目標床反力とのうちの少なくともいずれか一方を修正する歩容瞬時値修正手段と、その修正された歩容瞬時値に追従するように該ロボットの動作を制御する制御手段とを備えたことを特徴とするものである。

かかる第９発明によれば、実外力と今回歩容における外力の瞬時値（すなわち目標外力の瞬時値）との偏差を０に近づけるように今回歩容瞬時値のうちの前記ロボットの目標運動と目標床反力とのうちの少なくともいずれか一方を修正するので、今回歩容瞬時値を生成する上で想定した外力（目標外力）と実外力との偏差が大きくなったような場合であっても、ロボットの安定性を保つように、ロボットの動作制御を行なうことができる。従って、ロボットの実際の動作に際しての継続的な安定性を高めることができる。

以下、添付図面を参照して本発明の実施形態に係る脚式移動ロボットの歩容生成装置および制御装置を説明する。尚、脚式移動ロボットとしては２足移動ロボットを例にとる。

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

図示の如く、２足移動ロボット（以下、ロボットという）１は上体(ロボット１の基体）３から下方に延設された左右一対の脚体（脚部リンク）２，２を備える。両脚体２，２は同一構造であり、それぞれ６個の関節を備える。その６個の関節は上体３側から順に、股（腰部）の回旋（回転）用（上体３に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌ（符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、股（腰部）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌと、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。

各脚体２の足首の２つの関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には、各脚体２の先端部を構成する足平（足部）２２Ｒ（Ｌ）が取着されると共に、両脚体２，２の最上位には、各脚体２の股の３つの関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して前記上体３が取り付けられている。上体３の内部には、詳細を後述する制御ユニット６０などが格納される。なお、図１では図示の便宜上、制御ユニット６０を上体３の外部に記載している。

上記構成の各脚体２においては、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、膝関節は関節１６Ｒ（Ｌ）から構成され、足首関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２４Ｒ（Ｌ）で連結され、膝関節と足首関節とは下腿リンク２６Ｒ（Ｌ）で連結される。

上体３の上部の両側部には左右一対の腕体（腕リンク）５，５が取り付けられると共に、上体３の上端部には頭部４が配置される。両腕体５，５は同一構造であり、それぞれ７個の関節を備える。すなわち、各腕体５は、３つの関節３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）から構成された肩関節と、１つの関節３６Ｒ（Ｌ）から構成された肘関節と、３つの関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）から構成された手首関節と、この手首関節に連結された手先部（ハンド）４０Ｒ（Ｌ）とを備えている。なお、頭部４は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。

上記の構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体３に対して６つの自由度を与えられている。そして、ロボット１の歩行等の移動中に、両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行うことができる。これにより、ロボット１は任意に３次元空間を移動することができる。また、各腕体５は、上体３に対して７つの自由度を与えられ、両腕体５，５を合わせて７＊２＝１４個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行なうことができる。

図１に示す如く、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下方には足平２２Ｒ（Ｌ）との間に公知の６軸力センサ５０が介装されている。該６軸力センサ５０は、各脚体２の足平２２Ｒ（Ｌ）の着地の有無、および各脚体２に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の３方向成分Ｆｘ，Ｆｙ，Ｆｚ並びにモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚの検出信号を制御ユニット６０に出力する。この６軸力センサ５０と同様の６軸力センサ５２が、各腕体５の手先部（ハンド）４０Ｒ（Ｌ）と手首関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）との間に介装され、手先部４０Ｒ（Ｌ）に作用する外力の並進力の３方向成分並びにモーメントの３方向成分の検出信号が該６軸力センサ５２から制御ユニット６０に出力される。また、上体３には、Ｚ軸（鉛直方向（重力方向））に対する上体３の傾斜角およびその角速度、並びにＺ軸回りの上体３の回転角（ヨー角）およびその角速度を検出するための姿勢センサ５４が備えられ、その検出信号が該姿勢センサ５４から制御ユニット６０に出力される。この姿勢センサ５４は、図示を省略する３軸方向の加速度センサおよび３軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体３の姿勢角（傾斜角およびヨー角）およびその角速度を検出するために用いられると共に、ロボット１の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット１の各関節には、それを駆動するための電動モータ６４（図３参照）と、その電動モータ６４の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ）６５（図３参照）とが設けられ、該エンコーダ６５の検出信号が該エンコーダ６５から制御ユニット６０に出力される。

なお、図示は省略するが、各足平２２Ｒ（Ｌ）と６軸力センサ５０との間には、ばね等の弾性体が介装され、また、足平２２Ｒ（Ｌ）の底面には、ゴム等の弾性体が貼り付けられている。これらの弾性体は、コンプライアンス機構を構成するものであり、各脚体２が床反力を受けたときに弾性変形するようになっている。

図２は制御ユニット６０の構成を示すブロック図である。該制御ユニット６０はマイクロコンピュータにより構成されており、ＣＰＵからなる第１の演算装置９０及び第２の演算装置９２、Ａ／Ｄ変換器８０、カウンタ８６、Ｄ／Ａ変換器９６、ＲＡＭ８４、ＲＯＭ９４、並びにこれらの間のデータ授受を行うバスライン８２を備えている。この制御ユニット６０では、各脚体２の６軸力センサ５０、各腕体５の６軸力センサ５２、姿勢センサ５４（加速度センサおよびレートジャイロセンサ）等の出力信号はＡ／Ｄ変換器８０でデジタル値に変換された後、バスライン８２を介してＲＡＭ８４に送られる。またロボット１の各関節のエンコーダ６５（ロータリーエンコーダ）の出力は、カウンタ８６を介してＲＡＭ８４に入力される。

前記第１の演算装置９０は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ６４の回転角の指令値）を算出し、ＲＡＭ８４に送出する。また、第２の演算装置９２はＲＡＭ８４から関節角変位指令と、前記エンコーダ６５の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器９６とサーボアンプ６４ａとを介して各関節を駆動する電動モータ６４に出力する。

図３は、本明細書の実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図３中の破線で囲んだ部分が制御ユニット６０が実行する処理機能（主として第１の演算装置９０及び第２の演算装置９２の機能）によって構成されるものである。なお、以下の説明では、脚体２および腕体５の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。

以下説明すると、制御ユニット６０の機能的構成は、歩容生成装置１００、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６から構成されている。この明細書の実施形態では、歩容生成装置１００の処理を除き、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の処理は、本願出願人が先に特開平１０−２３０４８５号公報にて提案したものと同じである。従って、以下の本実施形態の説明では、歩容生成装置１００を中心に説明し、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の説明は概略的な説明に留める。

歩容生成装置１００は、ロボット１の目標歩容を自在かつリアルタイムに生成して出力するものであり、本願発明の歩容生成装置に相当するものである。この歩容生成装置１００が出力する目標歩容は、目標上***置姿勢軌道（上体３の目標位置及び目標姿勢軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢軌道）、目標手先位置姿勢軌道（各腕体５の手先部４４の目標位置および目標姿勢軌道）、目標全床反力中心点軌道（全床反力中心点の目標位置の軌道））、目標全床反力軌道、および目標対象物反力軌道から構成される。なお、脚体２や腕体５以外に上体３に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。

ここで、歩容生成装置１００が生成する目標歩容に係る用語の意味および定義について説明しておく。歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味する。

足平位置、上***置など、ロボット１の各部位の「位置」は、その部位に固定的に設定されたある代表点の位置を意味する。例えば、本実施形態では、各足平２２の代表点は、その足平２２を備える脚体２の足首関節の中心から、該足平２２の底面に延ばした垂線が該底面と交わる点であり、その代表点の位置が足平位置である。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はＺ軸（鉛直軸）に対するロール方向（Ｘ軸まわり）の上体３の傾斜角とピッチ方向（Ｙ軸まわり）の上体３の傾斜角とヨー方向（Ｚ軸まわり）の上体３の回転角（ヨー角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。

なお、歩容のうちの床反力および対象物反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上***置姿勢等、ロボット１の運動に係わる歩容を総称的に「運動」という。また、運動の軌道は、一定（時間的に変化しない）あるいはほぼ一定のものであってもよい。例えばロボット１がその両足平２２，２２を着地させた状態に維持して、移動しない状態では、足平位置姿勢軌道は一定になる。

歩容のうちの対象物反力は、ロボット１が環境から受ける、床反力以外の外力を意味する。従って、目標対象物反力軌道は、本発明における外力の目標軌道に相当する。本実施形態では、例えば図４に示すように、ロボット１がある対象物１２０（図示の例では台車）の所定の部位に両腕体５，５の手先部４４Ｒ，４４Ｌを係合させた状態で歩行動作を行いながら該対象物１２０を押す作業を行なう場合を例にとって説明する。この場合、ロボット１が環境から受ける床反力以外の外力は、ロボット１が対象物１２０から受ける反力を意味する。このために本実施形態では、その外力を対象物反力という。対象物反力は、一般的には、並進力成分とモーメント成分とから構成されるが、本実施形態では、対象物反力はロボット１に対象物１２０から作用する並進力を意味するものとし、その並進力によってロボット１に作用する、ある点回りのモーメントを対象物反力モーメントという。

なお、本明細書では、対象物１２０の運動に関し、対象物１２０の位置はロボット１の足平位置などと同様に、対象物１２０に固定的に設定された代表点の位置を意味する。また、対象物１２０の姿勢はロボット１の足平姿勢などと同様に、対象物１２０の空間的な向きを意味する。そして、対象物１２０の位置姿勢を総称的に対象物運動ということがある。

また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点（全床反力の中心点の目標位置）を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Ｚ軸）まわりのモーメント）を除いて０になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（Ｘ軸及びＹ軸）回りのモーメント）は０になる。そこで、本実施形態では、目標床反力中心点を目標全床反力の作用点として、歩容生成装置１００で生成するものとした。なお、目標床反力中心点は、目標全床反力の作用点としての意味を持つものであるから、本質的には目標全床反力の１つの構成要素であると言える。但し、本実施形態では、目標床反力のうちの目標床反力中心点が本発明と密接に関連するものである。そこで、図４では、目標全床反力のうちの作用点である目標床反力中心点を目標全床反力とは別に記載するものとした。

また、本明細書では、ＺＭＰ(zero moment point)は、ロボットの運動によって生じる慣性力と該ロボットに作用する重力と対象物反力との合力（あるいはその合力と釣り合う床反力）がその点まわりに作用するモーメントが、鉛直成分を除いて０になる点の意味で使用する。動力学的平衡条件を満足する歩容では、ロボットの目標運動軌道と重力と目標対象物反力とから算出されるＺＭＰと目標床反力中心点とは一致する。本明細書では、多くの場合、目標床反力中心点の代わりに、目標ＺＭＰという。

また、本明細書では、目標歩容は、所定期間分の目標運動と目標床反力（目標ＺＭＰを含む）と目標対象物反力との組を意味するものとする。特に、本実施形態では、目標歩容（単に歩容と称した場合を含む）は、ことわらない限り、ロボット１の１歩の期間分の目標運動と目標床反力（目標ＺＭＰを含む）と目標対象物反力との組を意味するものとする。この場合、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してから、もう一方の脚体２が着地するまでの意味で使用する。なお、一連の歩容は、いくつかの歩容（所定期間分の歩容）がつながったものとする。また、以降の説明では、１歩の期間分の目標歩容を単位歩容ということがある。また、ロボット１が移動しない状態（両足平２２，２２の接地を維持する状態）での１歩の期間は、ある所定の期間（ロボット１が移動する場合の１歩の期間に相当する期間）を意味する。

また、歩容における両脚支持期とは、ロボット１がその自重を両脚体２，２で支持する期間（両脚体２，２が支持脚となる期間）、片脚支持期とはいずれか一方のみの脚体２でロボット１の自重を支持する期間（一方のみの脚体２が支持脚となる期間）を言う。本実施形態での目標歩容（単位歩容）は、別の言い方をすれば、両脚支持期の開始時から次の片脚支持期の終了時までの期間分の目標歩容である。ロボット１の自重を支持する脚体２を支持脚と呼ぶ。両脚支持期では、両脚体２，２が支持脚となり、片脚支持期では１つの脚体２が支持脚となる。また、片脚支持期においてロボット１の自重を支持しない側の脚体２（支持脚でない脚体２）を遊脚と呼ぶ。なお、以降の説明では、特にことわならい限り、目標歩容における支持脚は、両脚支持期の開始時に着地する脚体２（該両脚支持期に続く方脚支持期で支持脚となる脚体２）を意味するものとする。また、支持脚側の脚体２の足平２２、遊脚側の脚体２の足平２２をそれぞれ支持脚足平２２、遊脚足平２２という。また、ロボット１が移動しない状態（両足平２２，２２の接地を維持する状態）であっても、一方の脚体２が支持脚、他方の脚体２が遊脚であるとする。

また、歩容生成装置１００により新たに生成しようとしている目標歩容（単位歩容）、あるいは生成しつつある目標歩容（単位歩容）を今回歩容と呼び、この今回歩容の１歩前の目標歩容を前回歩容、今回歩容の次の１歩の目標歩容を次回歩容、さらにその次の１歩の目標歩容を次次回歩容などと呼ぶ。

なお、目標歩容は、グローバル座標系としての支持脚座標系で記述される。本実施形態では、支持脚座標系は、支持脚足平２２を水平姿勢（より一般的には床面に平行な姿勢）にして該支持脚足平２２の底面のほぼ全面を床面に接触（密着）させた状態において、該支持脚の足首関節１８，２０の中心から床面に延ばした垂線が該床面と交わる点（この点は、本実施形態の例では支持脚足平２２の底面のほぼ全面を床面に接触させた状態では、該足平２２の代表点と合致する）を原点とし、その原点を通る水平面をＸＹ平面とするグローバル座標系（床に固定された座標系）である。この場合、Ｘ軸方向、Ｙ軸方向は、それぞれ支持脚足平２２の前後方向、左右方向である。以降の説明では、Ｘ軸、Ｙ軸、Ｚ軸は、特にことわらない限り、この支持脚座標系の３軸を意味するものとする。尚、支持脚座標系の原点は、必ずしも支持脚足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の代表点に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。

以上説明した事項は、以下に説明する第１実施形態に限らず、本明細書で説明する各実施形態において共通の事項である。

以下に本発明の第１実施形態に係る歩容生成装置１００の詳細を説明する。図５〜図７は歩容生成装置１００の処理を示すフローチャートである。なお、この第１実施形態は、本発明の第１〜第３発明、第５〜第９発明の実施形態である。

歩容生成装置１００は、図５〜図７のフローチャートに示す処理を所定の演算処理周期で逐次実行する。

まず、Ｓ０１において、対象物１２０の移動計画が決定される。ここで決定される移動計画は、少なくとも現在時刻から将来の所定期間分（ロボット１の複数歩分）の対象物１２０の移動計画を含んでいる。この移動計画は、基本的には対象物１２０の移動要求（対象物１２０をどのようなタイミングでどのように動かすかという設計的な要求）に応じて決定され、該対象物１２０の位置姿勢軌道（位置および姿勢の時系列）、あるいは、その軌道を規定するパラメータもしくは関数式等から構成される。例えば対象物１２０の移動要求が、ある時刻ｔ0からＸ軸方向に一定速度で対象物１２０を移動させるという要求である場合には、移動計画は、図８のグラフg1で示す如く決定される。グラフg1は、移動計画における対象物位置（Ｘ軸方向位置）の経時変化を示している。この場合、例えば現在時刻以降の各時刻（ある刻み時間毎の時刻）における対象物１２０の位置の時系列を移動計画として決定してもよいが、時刻ｔ0とグラフg1の傾き（対象物１２０の移動速度）とを移動計画を規定する要素（パラメータ）として決定したり、あるいはグラフg1の関数式を移動計画を規定する要素（パラメータ）として決定してもよい。なお、移動要求は、歩容生成装置１００に外部から適宜与えられ、あるいは、あらかじめ制御ユニット６０の図示しない記憶手段に記憶保持される。

補足すると、Ｓ０１で決定する移動計画は、必ずしも移動要求通りに決定されるわけではなく、必要に応じて適宜修正されるが、これについては後述する。

次にＳ０３に進み、上記の如くＳ０１で決定された移動計画を基に、対象物動力学モデルを用いて目標対象物運動軌道（目標対象物位置姿勢軌道）と、目標対象物反力軌道とが仮決定される。

ここで、このＳ０３の処理と、この処理で使用する対象物動力学モデルとを図９のブロック図を参照して説明する。図９は、Ｓ０３で目標対象物運動軌道のうちの目標対象物位置軌道と、目標対象物反力軌道とを求める演算処理を示すブロック図である。このブロック図中の破線で囲んだ部分が対象物１２０に作用する力と対象物１２０の運動との関係を表す対象物動力学モデルとなっている。なお、本実施形態では、理解の便宜上、対象物１２０をほぼ水平な床上で移動させる場合を例に採って説明する。

図９に示す対象物動力学モデルは、対象物１２０に作用させる力（より詳しくは水平方向の並進力）を入力として、対象物１２０の位置を出力する動力学モデルである。より詳しくは、この対象物動力学モデルは、対象物１２０に作用させる水平方向の並進力の入力値（後述の加算部２０４で求められる値）に、対象物１２０の質量Ｍの逆数１／Ｍを乗算部２０６で乗算することにより対象物１２０の運動加速度を求め、これを積分器２０８，２１０で順次積分する（２重積分する）ことにより対象物１２０の位置（対象物動力学モデル上での位置）を出力するものとなっている。つまり、本実施形態では、図９に示す対象物動力学モデルは、対象物１２０に作用する並進力が、対象物１２０の加速度と質量との積に等しいという運動方程式を基に構成されている。なお、積分器２０８の出力は、対象物動力学モデル上での対象物１２０の移動速度を意味しており、以下、これを対象物モデル速度という。

かかる対象物動力学モデルを用いるＳ０３の演算処理を図９を参照して具体的に説明する。まず、前記Ｓ０１で決定された移動計画に基づく対象物１２０の瞬時瞬時の（各時刻の）目標速度である目標対象物速度と、積分器２０８で先に求められた対象物モデル速度とが減算部２００に入力され、それらの偏差（＝目標対象物速度−対象物モデル速度）が求められる。ここで、目標対象物速度は、移動計画における対象物１２０の位置軌道の１階微分値として得られる値、あるいは移動計画の構成要素である。なお、前記図８に示した移動計画の例では、グラフg1の傾きをそのまま目標対象物速度とすればよく、この場合には、目標対象物速度は継続的に一定値となる。そして、現在時刻から将来の所定期間分（ロボット１の複数歩分）の目標対象物速度の時系列が減算部２００に順次入力される。また、減算部２００に入力される対象物モデル速度は、新たに入力する目標対象物速度の１つ前の目標対象物速度を減算部２００に入力したときに積分器２０８から出力された値である。

次いで、上記偏差に所定のゲインＫｖを乗算部２０２で乗算することにより、対象物１２０にロボット１から作用させるべき並進力の要求値が求められる。つまり、本実施形態では、該並進力の要求値は、対象物目標速度と対象物モデル速度との偏差が０に収束するように（対象物モデル速度が対象物目標速度に追従するように）、フィードバック制御則により決定される。そのフィードバック制御則としては、この例では比例制御則が使用される。そして、求められた並進力の要求値の符号を反転させたものの時系列が目標対象物反力軌道として出力される。

また、求められた並進力の要求値と、対象物１２０にロボット１以外から作用する外乱力（床から対象物１２０に作用する摩擦力など）の推定値である推定外乱力とが加算部２０４に入力され、該並進力の要求値と推定外乱力との和が、前記対象物動力学モデルに対する並進力の入力値として加算部２０４により求められる。ここで、推定外乱力は、歩容生成装置１００の演算処理周期毎に後述のＳ３５の処理で求められるものであり、加算部１２０に入力する推定外乱力としては、歩容生成装置１００の前回の演算処理周期で求められた値（前回値）が用いられる。そして、このように求めた並進力の入力値を対象物動力学モデルに入力することで、前記した如く対象物１２０の位置が求められ、この求めた位置の時系列が積分器２１０から目標対象物位置軌道として出力される。

なお、対象物動力学モデルの積分器２０８の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた対象物モデル速度の時系列のうちの、前回の演算処理周期に対応する時刻における値に設定される。また、積分器２１０の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻における値に設定される。

また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。

補足すると、移動計画および推定外乱が一定に維持されている限り、目標対象物運動軌道や目標対象物反力軌道は一定になるので、Ｓ０３の処理は、必ずしも歩容生成装置１００の演算処理周期毎に行なう必要はない。従って、例えば、Ｓ０３の処理は、ロボット１の１歩毎あるいは歩容生成装置１００の演算処理周期の複数周期毎に行なったり、Ｓ０１で移動計画を変更した場合や、推定外乱力が比較的大きく変化した場合に行なうものとしてもよい。

以上説明したＳ０３の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。

補足すると、Ｓ０３の処理は、本発明における対象物軌道仮決定手段に相当する。また、図９に示した対象物動力学モデルが本発明の第７発明における対象物動力学モデルに相当する。

次いで、Ｓ０５に進んで、上記の如く仮決定された目標対象物運動軌道を基に、ロボット１の遊脚足平２２の着地予定位置姿勢および着地予定時刻が仮決定される。ここで仮決定する着地予定位置姿勢および着地予定時刻は、現在、作成しようとしている目標歩容である今回歩容での遊脚足平２２の着地予定位置姿勢および着地予定時刻を含む、ロボット１の複数歩分（少なくとも２歩分）の遊脚足平２２の着地予定位置姿勢および着地予定時刻である。すなわち、仮決定する着地予定位置姿勢および着地予定時刻は、少なくとも今回歩容の遊脚足平２２の着地予定位置姿勢および着地予定時刻と、次回歩容の遊脚足平２２の着地予定位置姿勢および着地予定時刻とを含む。この場合、遊脚足平２２の着地予定位置姿勢および着地予定時刻は、Ｓ０３で決定された目標対象物運動軌道に対して、着地予定時刻での遊脚足平２２の着地位置姿勢がその時刻での対象物位置姿勢と所定の相対的位置姿勢関係になるように決定される。

但し、遊脚足平２２の着地予定位置姿勢および着地予定時刻は、必ずしも目標対象物位置姿勢に追従するような形態で決定する必要はない。例えば、ロボット１の移動を行なわない状態（両足平２２，２２の接地を維持する状態）で、対象物１２０を押して移動させ、その移動開始後は、対象物１２０から腕体５，５の手先部４４を対象物１２０からはずすような場合には、各歩容における遊脚足平２２の着地予定位置姿勢は経時的に一定に維持されることとなる。

なお、遊脚足平２２の着地予定位置姿勢は、より詳しくは、遊脚足平２２の踵を着地してから、該遊脚足平２２を床に接触させたまま、すべらないようにピッチ方向に回転させて該遊脚足平２２の底面のほぼ全面を床面に接触させた状態での該遊脚足平２２の位置姿勢である。従って、本実施形態では、遊脚足平２２Ｒまたは２２Ｌの着地予定位置姿勢は、その着地時から次の遊脚足平２２Ｌまたは２２Ｒの着地までの単位歩容における支持脚座標系の位置姿勢を規定するものとなり、遊脚足平２２の着地予定位置姿勢を決定することで、それに対応して、前記した支持脚座標系の設定手法に従って各単位歩容における支持脚座標系の位置姿勢が決定されることとなる。具体的には、各単位歩容における支持脚座標系は、該単位歩容の一つ前の単位歩容の遊脚足平２２をその着地予定位置姿勢に合致させた状態において、該足平２２を有する脚体２の足首関節の中心から床面に延ばした垂線が該床面と交わる点を原点とする座標系となる。

次いで、Ｓ０７に進んで、今回歩容の目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータが仮決定される。ここで仮決定されるＺＭＰ軌道パラメータは、それにより規定される今回歩容の目標ＺＭＰ（以下、仮目標ＺＭＰという）が、Ｓ０５で仮決定された着地予定位置姿勢および着地予定時刻により定まる、今回歩容での支持脚足平２２の接地面（両脚支持期では、両足平２２の接地面を含む、いわゆる支持多角形）のほぼ中央付近に存在し、且つ、急激な変化をしないように決定される。すなわち、今回歩容の仮目標ＺＭＰが、ロボット１の接地面内（あるいは支持多角形内）の、ロボット１の安定余裕ができるだけ高くなるような位置に存在し、且つ、急激な変化をしないようにＺＭＰ軌道パラメータが決定される。このような指針で決定される仮目標ＺＭＰは、例えば図１０（ａ）で示すようなパターンのものとなる。なお、図１０（ａ）は仮目標ＺＭＰのＸ軸方向位置のパターンを示している。この例の場合には、仮目標ＺＭＰパターン（軌道）の折れ点の位置および時刻がＺＭＰ軌道パラメータとして仮決定される。

次いで、Ｓ０９に進んで、現在仮決定されている目標対象物運動軌道と目標対象物反力軌道とを基に、現在仮決定されているＺＭＰ軌道パラメータにより定まる仮目標ＺＭＰまわりの対象物反力モーメント軌道（対象物反力モーメントの瞬時値の時系列）が算出される。より具体的には、まず、目標対象物運動軌道を基に、ロボット１２０の両腕体５，５の手先部４４Ｒ，４４Ｌを係合させるべき対象物１２０の部位の運動軌道（位置姿勢軌道）が決定される。該部位の位置姿勢は、対象物１２０の位置姿勢に対して所定の位置姿勢関係を有するものとされる。そして、その部位の運動軌道（位置姿勢軌道）に合致するように、ロボット１の手先位置姿勢軌道（これはロボット１に対する対象物反力の作用点の軌道を規定する）が求められる。次いで、この求めた手先位置姿勢軌道と目標対象物反力軌道と仮目標ＺＭＰ軌道とから各時刻（ある刻み時間毎の時刻）の仮目標ＺＭＰまわりの対象物反力モーメントが算出される。そして、その算出された対象物反力モーメントの時系列が対象物反力モーメント軌道として得られる。なお、対象物１２０から離れた箇所からロボット１を対象物１２０の近くに移動させ、その後、手先部４４Ｒ，４４Ｌを対象物１２０に係合させて該対象物１２０を押す作業を開始する場合には、対象物１２０を手先部４４Ｒ，４４Ｌを係合させる時刻（この時刻は適宜決定される）までの対象物反力軌道、ひいては、対象物反力モーメント軌道は０とされる。

次いで、Ｓ１１に進んで、今回歩容に続く周期的歩容としての定常旋回歩容の歩容パラメータが決定される。該歩容パラメータは、定常旋回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。

これらの歩容パラメータを具体的に説明する前に、定常旋回歩容の概要を説明しておく。なお、以降の説明では、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。

定常旋回歩容は、その歩容を繰り返したときに歩容の境界（本実施形態では１歩毎の歩容の境界）においてロボット１の運動状態（足平位置姿勢、上***置姿勢等の状態）に不連続が生じないような周期的歩容を意味する。

周期的歩容である定常旋回歩容は、本実施形態では、ロボット１の２歩分の歩容、すなわち今回歩容に続く第１旋回歩容と該第１旋回歩容に続く第２旋回歩容とからなる歩容を該定常旋回歩容の１周期分の歩容として、その１周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。以降、定常旋回歩容を定常歩容と略す場合もある。

定常歩容について補足すると、ロボット１としての２足移動ロボットでは、定常歩容の１周期分は、少なくとも２歩分の歩容（連続する２つの単位歩容）から構成される。なお、３歩以上の歩容を１周期分の歩容とする複雑な定常歩容を設定することも可能である。但し、定常歩容は、後述の如く、今回歩容の終端における発散成分（詳細は後述する）を決定するためだけに用いられる。このため、３歩以上の歩容を１周期とする定常歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本実施形態での定常歩容の１周期分の歩容を２歩分の歩容（第１及び第２旋回歩容）により構成するようにしている。以下の定常歩容の説明では、説明の便宜上、２歩分の歩容から成る定常歩容を１歩の歩容とみなす。定常歩容は、歩容生成装置１００で今回歩容の終端における発散成分を決定するために暫定的に（歩容生成装置１００の演算処理上で）想定される仮想歩容であり、歩容生成装置１００からそのまま出力されるものではない。

尚、「発散」とは、２足移動ロボット１の上体３の水平位置が両足平２２，２２の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、２足移動ロボット１の上体３の水平位置が両足平２２，２２の位置（より具体的には、支持脚足平２２の接地面に設定された支持脚座標系の原点）からかけ離れていく具合を表す数値である。

本実施形態では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成する。すなわち、今回歩容の後に続く定常歩容の初期発散成分を決定してから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる（より一般的には、今回歩容の上***置姿勢を定常歩容の上***置姿勢に収束させる）ように、今回歩容を生成する（今回歩容を規定する歩容パラメータを決定する）。そして、定常歩容の初期発散成分は、Ｓ１１で決定した歩容パラメータを基に、該定常歩容の初期（第１旋回歩容の初期）と終端（第２旋回歩容の終端）とでロボット１の運動状態が一致するという定常歩容の条件（以下、これを定常歩容の境界条件という）をロボット１の動力学モデル上で満たすように決定される。このような歩容生成の基本的な指針は本出願人が先に提案したＰＣＴ国際公開公報WO/02/40224A1のものと同様である。従って、以下の定常歩容に関する説明では、ＰＣＴ国際公開公報WO/02/40224A1の記載事項と異なる技術事項を主体に説明し、同様の技術事項については詳細な説明を省略することがある。

Ｓ１１の説明に戻って、定常歩容の歩容パラメータのうちの足平軌道パラメータは、第１旋回歩容および第２旋回歩容の初期及び終端のそれぞれにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成され、今回歩容、第１旋回歩容、第２旋回歩容の順に、足平位置姿勢軌道がつながるように決定される。以下に具体的な設定方法を図１１を参照して説明する。

第１旋回歩容初期遊脚足平位置姿勢は、次回歩容の支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とする。この今回歩容終端支持脚足平位置姿勢は、今回歩容初期の支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（これは前回歩容の遊脚足平２２の着地予定位置姿勢に一致する）である。なお、次回歩容支持脚座標系の位置姿勢は、図１１に示す如く、今回歩容の遊脚足平２２の着地予定位置姿勢に対応して決定される。

第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とする。この今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢（あるいはこれに対応して定まる次回歩容支持脚座標系）に対応した位置姿勢に決定される。具体的には、今回歩容の遊脚足平２２を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の遊脚足平２２の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。

第１旋回歩容終端遊脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩容終端遊脚足平位置姿勢とする。この次回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容の遊脚足平２２の着地予定位置姿勢（あるいはこれに対応して定まる次次回歩容支持脚座標系）に対応した位置姿勢に決定される。

第１旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に位置姿勢を合致させた支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（この位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢に一致する）とする。

第２旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端支持脚足平位置姿勢とする。第２旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端遊脚足平位置姿勢とする。

第２旋回歩容終端遊脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢に一致するように決定される。第２旋回歩容終端支持脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢に一致するように決定される。

第１旋回歩容および第２旋回歩容の歩容周期は、次回歩容周期（これは今回歩容の遊脚足平２２の着地予定時刻と次回歩容の遊脚足平２２の着地予定時刻との差である）と同一に設定される。これらの第１旋回歩容及び第２旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第１旋回歩容および第２旋回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む）は、上記決定されたパラメータに応じて、歩容の条件（アクチュエータの速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど）を満足するように適宜決定する。

定常歩容の歩容パラメータのうちのＺＭＰ軌道パラメータは、前記Ｓ０７で目標ＺＭＰ軌道パラメータを仮決定した場合と同様に、該ＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道が、ロボット１の安定余裕が高くなり、且つ、急激な変化をしないように決定される。

また、定常歩容の対象物反力モーメント軌道パラメータは、Ｓ０９で算出された対象物反力モーメント軌道を基に、周期性の条件を満足するように決定される。例えば、今回歩容、次回歩容および次次回歩容に対応する対象物反力モーメント軌道が図１２（ａ）に示すようになっているとする。ここで、今回歩容の対象物反力モーメント軌道はＳ０９で求められた軌道である。次回歩容の対象物反力モーメント軌道は、Ｓ０３で求めた次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基にＳ０９と同様に算出される軌道である。次次回歩容の対象物反力モーメント軌道は、Ｓ０３で求めた次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基にＳ０９と同様に算出される軌道である。

このとき、定常旋回歩容の対象物反力モーメント軌道は、例えば図１２（ｂ）で示す軌道になるように、定常旋回歩容の対象物反力モーメント軌道パラメータが決定される。この例では、定常旋回歩容の第１旋回歩容の初期から終端までの対象物反力モーメント軌道は、今回歩容の対象物反力モーメント軌道に連続して、次回歩容の対象物反力モーメント軌道と一致するように決定されている。また、定常旋回歩容の第２旋回歩容の対象物反力モーメント軌道は、第１旋回歩容の対象物反力モーメント軌道に連続すると共に、第２旋回歩容の初期から終端の直前のある時刻ｔxまでは、次次回歩容の対象物反力モーメント軌道と一致し、時刻ｔxから終端までは、時刻ｔxの値から、第１旋回歩容の初期（定常旋回歩容の初期）の値と同じ値まで連続的に変化するように決定されている。時刻ｔxは、例えば定常旋回歩容のＺＭＰ軌道の折れ点の時刻に対応する。

なお、次回歩容の初期の対象物反力モーメントの値と、次次回歩容の対象物反力モーメントの値との差がさほど大きくないときには、必ずしも、定常旋回歩容の初期の対象物反力モーメントと終端の対象物反力モーメントの値とを同じにしなくてもよい。

補足すると、Ｓ０５、Ｓ０９、Ｓ１１の処理は、本発明における周期的歩容パラメータ決定手段に相当する。この場合、Ｓ１１で決定される足平軌道パラメータ、ＺＭＰ軌道パラメータ、対象物反力モーメント軌道パラメータがそれぞれ、周期的歩容である定常歩容における脚体２の運動軌道を規定するパラメータ、該定常歩容における床反力を規定するパラメータ、該定常歩容における外力の軌道を規定するパラメータに相当する。なお、Ｓ０３で決定される目標対象物運動軌道を基に、足平軌道パラメータに係る着地予定位置姿勢および着地予定時刻が決定されるので、該目標対象物運動軌道は、「脚体の運動に関する要求」に相当する。

次いで、Ｓ１３に進み、定常旋回歩容の初期発散成分が決定される。この場合、定常旋回歩容は周期的歩容であるから、ロボット１のあらかじめ定めた動力学モデル上で、第１旋回歩容の運動の初期状態（初期発散成分を含む）と第２旋回歩容の運動の終端状態（終端発散成分を含む）とが一致するように定常旋回歩容の初期発散成分が決定される。

このＳ１３の処理を詳細に説明する前に、まず、このＳ１３での処理に用いるロボット１の動力学モデルと、この動力学モデルを使用して目標上***置を決定するための動力学演算とを説明する。この動力学モデルは、Ｓ１３の処理に用いられるほか、後述のＳ３７までの処理で今回歩容を決定するために用いられ、ロボット１の動力学的挙動（ロボット１に作用する力と運動との関係）を単純化して（近似的に）表現した動力学モデルである。以下、この動力学モデルをロボット単純化モデルという。なお、このロボット単純化モデルの基本構造は、ＰＣＴ国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが加味されている点で同出願のものと相違している。

図１３はロボット単純化モデルの構造を示している。図示の如く、このロボット単純化モデルは、３質点モデルであり、脚体２の動力学と上体３の動力学とが相互に非干渉に構成されると共に、ロボット１の全体の動力学は、それらの線形結合で表される。

このロボット単純化モデルは、倒立振子Ａと２つの脚質点２ｍ，２ｍ（支持脚質点および遊脚質点）とから構成される。各脚質点２ｍは、各脚体２に対応する質点である。この各脚質点２ｍは、それぞれに対応する脚体２の足平２２に任意に固定的に設定されたローカル座標系での固定点であり、その位置は、各足平位置姿勢に対応して一義的に定まるものとされている。例えば、各足平質点２ｍは、その足平２２の底面の代表点から足首関節の中心に向かって所定の距離だけ該足首関節側にずれた点に設定される。

倒立振子Ａは、水平に移動するフリーの支点ａと、一つの質点ｂと、支点ａと質点ｂとを結ぶ質量の無い可変長のリンクｃとから構成される。この場合、リンクｃが傾いてもリンクｃが伸縮し、支点ａから見た質点ｂの高さｈが一定値に保たれるものとする。

この倒立振子Ａの質点ｂ（以下、単に倒立振子質点ｂという）は、ロボット３の上体３の質点に相当するものであり、以降、倒立振子質点ｂを上体質点ｂということもある。倒立振子質点ｂの位置を以降、略して倒立振子位置という。なお、上体質点ｂの質量は、上体３の質量と腕体５，５および頭部４の質量を含んでいる。

ロボット１の上体３の水平位置は、倒立振子質点ｂの水平位置から幾何学的に決定される。具体的には、例えば、上体３の代表点の水平位置（支持脚座標系で見たＸＹ座標）、すなわち上***置の水平成分が、倒立振子質点ｂの水平位置に一致するものとされる。換言すれば、図１３に示すように、上体３の代表点と倒立振子質点ｂとは、同一鉛直線上にあるものとされる。

なお、倒立振子ＡのＺＭＰは、フリーの支点ａの位置にあるので（支点ａのまわりにはモーメントが発生しない）、以降、倒立振子Ａの支点ａの位置を、倒立振子ＺＭＰと呼び、ZMPpendと記述する。

かかるロボット単純化モデルを数式表現するために、該モデルに関する変数およびパラメータを以下のように定義する。

msup：支持脚質点２ｍの質量、 mswg：遊脚質点２ｍの質量、 mb：倒立振子質点ｂの質量、 mtotal：ロボット１の全体質量（＝ｍsup＋ｍswg＋ｍb）、 mfeet：両脚体２，２の総質量（＝ｍsup＋ｍswg）、 xsup：支持脚質点２ｍの位置、 xswg：遊脚質点２ｍの位置、 xb：倒立振子位置（上体質点ｂの位置）、 ｈ：倒立振子高さ（倒立振子Ａの支点ａから倒立振子質点ｂまでの高さ）

以降、ことわらない限り、xb，xsup，xswgは３次元ベクトル（ＸＹＺ座標ベクトル）で表す。また、Ｘを任意の変数としたとき、dX/dtはＸの１階微分を表し、d2X/dt2はＸの２階微分を表す。ｇは重力加速度定数を示す。Ｇは、そのＸ成分、Ｙ成分、Ｚ成分がそれぞれ０、０、−ｇである重力加速度ベクトルとする。

図１３のロボット単純化モデルにおいて、両脚質点２ｍ，２ｍの総慣性力（両脚質点２ｍ，２ｍの運動による慣性力と重力との合力）がある作用点Ｐまわりに作用するモーメントを点Ｐまわりの脚総慣性力モーメントと定義し、その作用点Ｐの座標（位置）をxpとする。

下記の式０１は、点Ｐまわりの脚総慣性力モーメントの動力学的定義式である。

点Ｐまわりの脚総慣性力モーメント
＝msup(xsup−xp)＊Ｇ−msup(xsup−xp)＊d2xsup/dt2
＋mswg(xswg−xp)＊Ｇ−mswg(xswg−xp)＊d2xswg/dt2 ……式０１

脚ＺＭＰをZMPfeetと記述し、式０２で定義する。ただし、脚ＺＭＰの高さ（ZMPfeetのＺ成分）は、前記点Ｐの高さと同一とする。この脚ＺＭＰは、両脚体２，２の運動によって発生する慣性力と重力との合力に擬似的に対応させた値である。

点Ｐまわりの脚総慣性力モーメント＝mfeet＊(ZMPfeet−xp)＊Ｇ ……式０２

なお、前記作用点Ｐは、ロボット単純化モデルの近似精度が高くなるように設定される。例えば、今回歩容に係る作用点Ｐは、前回歩容の支持脚座標系の原点から、今回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動し、該両脚支持期に続く片脚支持期では今回歩容支持脚座標系の原点に維持されるように設定される。このことは、定常旋回歩容の第１旋回歩容、第２旋回歩容に係る作用点Ｐについても同様である。

また、目標ＺＭＰまわりの対象物反力モーメントをロボット１の全体質量mtotalで除算したものを、対象物反力ＺＭＰと定義し、これをZMPbojと記述する。すなわち、ZMPobjを次式０３で定義する。

ZMPobj＝目標ＺＭＰまわりの対象物反力モーメント／mtotal ……式０３

このとき、目標ＺＭＰと脚ＺＭＰ（ZMPfeet）と倒立振子ＺＭＰ(ZMPpend）と対象物反力ＺＭＰとの間には、近似的に次式０４の線形関係が成り立つ。

ZMPpend＝mtotal／mb＊目標ＺＭＰ−mfeet／mb＊ZMPfeet
−mtotal／mb＊ZMPobj ……式０４

なお、式０４は、サジタルプレーン（ＸＺ平面）上での関係式であり、ラテラルプレーン（ＹＺ平面）上では、式０３の右辺第３項の符号が「−」から「＋」に反転する。

また、倒立振子Ａの挙動を表す微分方程式は、次式０５で表される。

d2xb/dt2の水平成分＝g／h＊(xbの水平成分−ZMPpendの水平成分) ……式０５

以上の式０１〜０５が図１３のロボット単純化モデルの動力学を記述する式である。

このようなロボット単純化モデルを用いることで、次のような動力学的演算により、目標足平位置姿勢と目標ＺＭＰまわりの対象物反力モーメントと目標ＺＭＰと目標上体姿勢とから目標上***置を決定できる。

その動力学演算を図１４のブロック図を参照して説明しておく。図１４はこの動力学演算を示すブロック図である。目標両足平位置姿勢（支持脚足平２２および遊脚足平２２の目標位置姿勢）の軌道と、前記の如く設定される作用点Ｐとから、脚ＺＭＰ算出器２２０により、前記式０１および０２を用いて脚ＺＭＰ(ZMPfeet)が算出される。

また、目標ＺＭＰまわりの対象物反力モーメントに、乗算部２２２で１／mtotalを乗算することで、対象物反力ＺＭＰ(ZMPobj)が算出される。そして、算出したZMPfeetに乗算部２２４でmfeet／mtotalを乗算したものとZMPobjとを減算部２２６で目標ＺＭＰから減算し、さらにその減算結果に乗算部２２８でmtotal／mbを乗算することによって、前記式０４の右辺の演算が行なわれる。これにより倒立振子ＺＭＰ(ZMPpend）が算出される。なお、上記の演算処理により求められるZMPpendは、サジタルプレーン上のものであり、ラテラルプレーン上のZMPpendを算出する場合には、乗算部２２２の演算結果の符号を反転させたものを減算部２２６に入力すればよい。

このようにして算出されたZMPpendを倒立振子Ａに入力することで、前記式０５に基づいて、倒立振子水平位置xbが算出される。さらに、この倒立振子水平位置xbと目標上体姿勢とを基に、上***置決定器２３０により目標上***置が決定される。この場合、上***置決定器２３０は、倒立振子水平位置xbを目標上体水平位置とする。また、目標上体鉛直位置は、例えば本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法によって、目標上体姿勢などを基に決定される。

図５の説明に戻って、以下に、Ｓ１３の処理を詳細に説明する。

Ｓ１３の処理では、前記したロボット単純化モデルを基に、定常歩容の初期発散成分を決定する。歩容における発散成分をｑとおき、前記倒立振子Ａの質点ｂの水平速度（あるいは上体水平速度）vbとおくと、ｑは、本実施形態では次式０６により定義される。

ｑ＝xb＋vb／ω0 ……式０６

ただし、ω0は、倒立振子Ａの固有周波数、すなわち、ｇ／ｈの平方根である。

なお、このように定義される発散成分の技術的意味については、ＰＣＴ国際公開公報WO/02/40224A1に詳細に説明されているので、ここでの説明は省略する。

このように発散成分ｑを定義したとき、倒立振子Ａの運動方程式（前記式０５）を離散化して、ｑに関して解き、それに前記式０４を適用すると、次式０７が得られる。なお、目標ＺＭＰをZMPtotalと記述する。

ｑ[k]＝exp(ω0kΔt)＊ｑ[0]
＋exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPtotal[i])
−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mfeet／mb＊ZMPfeet[i])
−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPobj[i])
…式０７

ただし、exp( )は、自然対数の底ｅの指数関数を意味する。また、Δｔは離散系の刻み時間、ｋはステップ数（ｋΔｔが時刻を意味する）である。また、式０７のΣ( )は、ｉ＝０からｉ＝ｋ−１までの累積加算演算を意味している。

なお、式０７はサジタルプレーン上での発散成分ｑを記述する式であり、ラテラルプレーン上での発散成分ｑを記述する式は、式０７の右辺の第４項の全体の符号を「−」から「＋」に反転させた式となる。

ここで、定常旋回歩容の初期（第１旋回歩容の初期）における発散成分（以下、初期発散成分という）と終端（第２旋回歩容の終端）における発散成分（以下、終端発散成分という）に着目し、ｑ[0]（時刻０における発散成分）を初期発散成分、ｑ[k]（時刻ｋΔｔにおける発散成分）を終端発散成分とする。

このとき、式０７の右辺第１項は、初期発散成分によって発生する終端発散成分を表す。右辺第２項は、目標ＺＭＰパターンによって発生する終端発散成分を表す。右辺第３項は、両脚体２，２の運動（両脚質点２ｍ，２ｍの運動）によって発生する終端発散成分を表す。右辺第４項は、対象物反力モーメントパターンによって発生する終端発散成分を表す。以降、右辺第２項をWzmptotal、右辺第３項をWfeet、右辺第４項をWobjと記述する。すなわち、Wzmptotal、Wfeet、Wobjを次式０８ａ，０８ｂ，０８ｃで定義する。

Wzmptotal
＝exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPtotal[i])
……式０８ａ
Wfeet
＝−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mfeet／mb＊ZMPfeet[i])
……式０８ｂ
Wobj
＝−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPobj[i])
……式０８ｃ

なお、式０８ｃは、サジタルプレーン上でのWobjを定義する式であり、ラテラルプレーン上でのWobjは、式０８ｃの右辺の符号を反転させたものとなる。

これらのWzmptotal、Wfeet、Wobjを用いて前記０７を書き換えると、次式０９となる。

ｑ[k]＝exp(ω0kΔt)＊ｑ[0]＋Wzmptotal＋Wfeet＋Wobj ……式０９

本実施形態では、定常旋回歩容の境界条件を満足する初期発散成分は、今回歩容に続く定常旋回歩容の支持脚座標系（図１１の次回歩容支持脚座標系（Ｘ'Ｙ'座標系））で見た初期発散成分が、該定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））から見た定常旋回歩容（今回歩容に続く定常旋回歩容）の終端発散成分に一致するように、上記式０８ａ，０８ｂ，０８ｃ，０９に基づいて解析的に求められる。

ここで、定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））から見た該定常旋回歩容の終端発散成分をｑ'[k]とおくと、定常旋回歩容の境界条件を満足するためには、次式１０が成立しなければならない。

ｑ[0]＝ｑ'[k] …式１０

また、定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））で記述される値を、該定常旋回歩容の支持脚座標系（図１１の次回歩容支持脚座標系（Ｘ'Ｙ'座標系））で記述される値に座標変換するための回転行列をＭ、座標原点の平行移動ベクトルをＢとおくと、次式１１が得られる。

ｑ[k]＝Ｍｑ'[k]＋Ｂ ……式１１

従って、これらの式１０、１１から次式１２が得られる。

ｑ[k]＝Ｍｑ[0]＋Ｂ ……式１２

この式１２は、定常歩容の境界条件を満足するために、初期発散成分ｑ[0]と終端発散成分ｑ[k]とが満たすべき条件である。

そして、この式１２と前記式０８ａ〜０８ｃおよび０９とから、次式１３が得られる。

ｑ[0]＝inv(Ｍ−exp(ω0kΔt)Ｉ)(Wzmptotal＋Wfeet＋Wobj−Ｂ） ……式１３

なお、式１３において、inv( )は、括弧内の行列の逆行列であり、Ｉは単位行列である。

本実施形態では、この式１３に基づいて、定常旋回歩容の境界条件を満足する初期発散成分ｑ[0]を決定する。

具体的には、前記Ｓ１１で決定した定常旋回歩容のＺＭＰ軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における目標ＺＭＰの瞬時値であるZMPtotal[i]（ｉ＝０，１，……，ｋ−１）を求め、これを用いて前記式０８ａの右辺の演算を行なうことで、Wzmptotalを算出する。なお、Wzmptotalの算出は、ＰＣＴ国際公開公報WO/02/40224A1に説明されている如く、目標ＺＭＰ軌道が折れ線軌道であることを利用したアルゴリズムによって算出するようにしてもよい。

また、前記Ｓ１１で決定した定常旋回歩容の足平軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における各足平位置姿勢の瞬時値を求め、それを、前記式０１，０２を離散系で表現した式に適用することで、ZMPfeet[i]を求める。なお、足平位置姿勢は、例えば本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。この場合、算出される足平位置姿勢の軌道は、定常旋回歩容の第１旋回歩容および第２旋回歩容のそれぞれにおける遊脚足平２２が片脚支持期の開始時刻から上昇して、該遊脚足平２２の着地予定位置に向かって移動し、該遊脚足平２２の着地予定時刻にて該遊脚足平２２がその踵で着地予定位置に対応する位置に着地するような軌道である。そして、上記の如く求めたZMPfeetl[i]を用いて前記式０８ｂの右辺の演算を行なうことで、Wfeetを算出する。補足すると、遊脚足平２２の着地予定位置姿勢が、１つ前の歩容における支持脚足平２２の着地位置姿勢と同じである場合には、足平位置姿勢軌道は、一定の着地位置姿勢に維持される軌道とされる。但し、遊脚足平２２を一旦上昇させ、その後、元の着地位置姿勢に戻すような足平位置姿勢軌道を生成するようにしてもよい。

さらに、前記Ｓ１１で決定した定常旋回歩容の対象物反力モーメント軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における、目標ＺＭＰまわりの対象物反力モーメントを求めると共に、それをmtotalで除算することにより、ZMPobj[i]を求め、その求めたZMPobj[i]を用いて前記式０８ｃの右辺の演算を行なうことで、Wobjを算出する。

なお、Wzmptotal、Wfeet、Wobjの算出で用いる刻み時間Δｔは、歩容生成装置１００の演算処理周期と同一であることが好ましいが、それよりも長い時間に設定して、演算処理時間を短縮するようにしてもよい。

そして、上記の如く求めたWzmptotal、Wfeet、Wobjから、前記式１２の右辺の演算を行なうことで、定常旋回歩容の境界条件を満足する初期発散成分ｑ[0]が算出される。

以上が、本実施形態におけるＳ１３の処理の詳細である。

なお、本実施形態では、初期発散成分ｑ[0]を解析的に求めるようにしたが、例えばＰＣＴ国際公開公報WO/02/40224A1の第２実施形態で説明されている如く、探索的な手法により、今回歩容に続く定常旋回歩容の次の歩容の支持脚座標系から見た該定常旋回歩容の終端発散成分にほぼ一致するような該定常旋回歩容の初期発散成分を決定するようにしてもよい。

図５のフローチャートの説明に戻って、上記の如くＳ１３の処理を実行した後、Ｓ１５に進んで、今回歩容の歩容パラメータが仮決定される。該歩容パラメータは、今回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。

今回歩容の歩容パラメータのうちの足平軌道パラメータは、今回歩容の初期及び終端のそれぞれにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、今回歩容の歩容周期等から構成される。この場合、今回歩容初期遊脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の遊脚足平位置姿勢（前回歩容の支持脚足平位置姿勢）とする。今回歩容初期支持脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の支持脚足平位置姿勢（前回歩容の遊脚足平位置姿勢）とする。今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢に対応させて決定される。具体的には、今回歩容の遊脚足平２２を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。今回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系に位置姿勢を合致させた支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（この位置姿勢は、前回歩容の遊脚足平２２の着地予定位置姿勢に一致する）とする。

なお、本実施形態では、今回歩容終端における支持脚足平２２は、床面にほぼ平行な姿勢になるが、必ずしもこのようにする必要はない。例えば歩容終端において、支持脚足平２２の踵側が床面から離れるようにしてもよい。

今回歩容の歩容パラメータのうちのＺＭＰ軌道パラメータは、Ｓ０７で仮決定されたＺＭＰ軌道パラメータと同一に設定される。従って、このＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道は、例えば前記図１０（ａ）に示したようなパターンのものである。

また、今回歩容の歩容パラメータのうちの対象物反力モーメント軌道パラメータは、それにより規定される対象物反力モーメント軌道が、前記Ｓ０９で算出した対象物反力モーメント軌道のうちの今回歩容に対応する軌道に一致するように決定される。

補足すると、Ｓ０５、Ｓ０７、Ｓ０９、Ｓ１５の処理は、本発明における今回歩容パラメータ仮決定手段に相当する。この場合、Ｓ１５で決定される足平軌道パラメータ、ＺＭＰ軌道パラメータ、対象物反力モーメント軌道パラメータが、それぞれ、今回歩容における脚体の運動軌道を規定するパラメータ、今回歩容における床反力を規定するパラメータ、外力の軌道を規定するパラメータに相当する。

次いで、Ｓ１７に進んで、定常旋回歩容に今回歩容が収束するように、Ｓ１５で仮決定した歩容パラメータのうちのＺＭＰ軌道パラメータを修正する。より詳しくは、定常旋回歩容の初期発散成分に今回歩容の終端発散成分がほぼ一致するようにＺＭＰ軌道パラメータが修正される。

本実施形態では、定常旋回歩容の初期発散成分に今回歩容の終端発散成分をほぼ一致させるようにするために、前記仮目標ＺＭＰに図１０（ｂ）に示すようなＺＭＰ修正量を加えることにより、該仮目標ＺＭＰを修正し、それにより、図１０（ｃ）に示すような目標ＺＭＰを得る。

この場合、仮目標ＺＭＰの修正処理の基本的指針は、ＰＣＴ国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが考慮される点で同出願のものと相違している。以下にその相違する点を中心にＳ１７の処理を具体的に説明する。なお、この処理で使用する変数、記号のうち、定常歩容の初期発散成分を決定する処理（Ｓ１３）に関して説明したものと同様の意味を持つ変数については、定常歩容に関して先に定義したものと同じ変数、記号を用いてＳ１７の処理を説明する。また、以下の説明では、理解の便宜上、ＺＭＰのＸ軸方向成分の修正に関して説明を行なう。

本実施形態では、ＺＭＰ修正量は、図１０（ｂ）に示す如く台形状とされ、その高さをａとおく。なお、台形の折れ点の時刻は、図示の如く、仮目標ＺＭＰ軌道の折れ点の時刻に合わせて設定される。本実施形態では、ＺＭＰ修正量は、今回歩容の片脚支持期で発生させる。また、前記Ｓ１３で決定した初期発散成分を今回歩容支持脚座標系から見た値に座標変換したものをｑ''とおく。すなわち、今回歩容支持脚座標系で見た定常旋回歩容の初期発散成分をｑ''とおく。また、仮目標ＺＭＰ軌道によって発生する今回歩容の終端発散成分をWzmptmp、ａ＝１である場合のＺＭＰ修正量パターンによって発生する今回歩容の終端発散成分をWtrimとおく。

このとき、修正後の目標ＺＭＰ軌道によって発生する終端発散成分をWzmptotalとおくと、近似的に次式１４が得られる。

Wzmptotal＝Wzmptmp＋ａ＊Wtrim ……式１４

従って、目標ＺＭＰを修正した後の今回歩容の終端発散成分ｑ''は、定常歩容に関して説明した前記式０９と同様の次式１５により表される。

ｑ''＝exp(ω0kΔt)＊ｑ[0]＋Wzmptmp＋ａ＊Wtrim＋Wfeet＋Wobj ……式１５

なお、式１５において、ｋΔｔは今回歩容の終端時刻、ｑ[0]は今回歩容の初期発散成分、Wfeetは今回歩容の両脚体２，２の運動によって発生する終端発散成分、Wobjは今回歩容の対象物反力モーメント軌道によって発生する終端発散成分である。

この式１５から、ＺＭＰ修正量の台形高さａを決定するための次式１６が得られる。

ａ＝(ｑ''−exp(ω0kΔt)＊ｑ[0]−Wzmptmp−Wfeet−Wobj)／Wtrim ……式１６

そこで、本実施形態では、式１６により、ＺＭＰ修正量の台形高さａを求める。この場合、初期発散成分ｑ[0]は今回歩容初期の倒立振子位置（あるいは上***置）とその変化速度とから、前記式０６により算出される。また、Wzmptmpは、Ｓ１５で仮決定されたＺＭＰ軌道パラメータにより規定される仮目標ＺＭＰ軌道から、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wtrimは、台形高さａを１として定まるＺＭＰ修正量パターンから、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wfeetは、Ｓ１５で仮決定された足平軌道パラメータから、定常旋回歩容に係るWfeetを求めた場合と同様に前記式０８ｂに基づいて求められる。Wobjは、Ｓ１５で仮決定された対象物反力モーメント軌道パラメータから、定常旋回歩容に係るWobjを求めた場合と同様に前記式０８ｃに基づいて求められる。

Ｓ１７では、このようにして求めた台形高さａにより定まるＺＭＰ修正量を仮目標ＺＭＰ軌道に加えることで、ＺＭＰ軌道パラメータが修正される。

なお、本実施形態では、ＺＭＰ修正量を解析的に求めるようにしたが、探索的に求めるようにしてもよい。この場合には、台形高さａの値を適当な値に設定したＺＭＰ修正量により仮目標ＺＭＰを修正してなる目標ＺＭＰ軌道を用いて、今回歩容を仮に作成し、その仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分との差などに応じて、その差が小さくなるように台形高さａの値を修正する、という処理を繰り返す。そして、最終的に仮作成した今回歩容の終端発散成分を定常歩容の初期発散成分とがほぼ一致するときの台形高さａの値によってＺＭＰ修正量を決定すればよい。

補足すると、Ｓ１７の処理は、本発明におけるパラメータ修正手段に相当する。この場合、ＺＭＰ軌道パラメータが修正対象のパラメータである。

Ｓ１７の処理の後、Ｓ１９に進んで修正後のＺＭＰ軌道パラメータにより定まる目標ＺＭＰが適正か否かが判断される。具体的には、修正後の目標ＺＭＰが、今回歩容の支持脚足平２２の接地面から逸脱したり、接地面の境界付近に偏ったりしていない場合、すなわち、ロボット１の安定余裕が確保できる場合には、目標ＺＭＰが適正であると判断され、そうでない場合には、目標ＺＭＰが不適正であると判断される。

Ｓ１９の判断結果がＮＯである場合には、Ｓ２１に進んで、今回歩容パラメータのうちのＺＭＰ軌道パラメータ以外のパラメータに係わる要素、例えば対象物反力モーメント軌道パラメータに係わる目標対象物反力軌道を修正する。この場合、目標対象物反力軌道は、その修正後にＳ０９〜Ｓ１７の処理を再度実行したときに、ＺＭＰ修正量ができるだけ小さくなるように（少なくとも目標対象物反力軌道の修正前に求めたＺＭＰ修正量よりも小さくなるように）修正される。

この目標対象物反力軌道の修正例を以下に説明する。例えば図１５（ａ）のグラフg2で示すように移動計画に基づく目標対象物運動軌道（位置軌道）が時刻ｔ1からＸ軸方向で対象物１２０の減速を行なって、時刻ｔ3で停止させるような軌道であるとする。なお、図１５（ａ）のグラフg3は、目標対象物運動軌道に対応して決定される目標上***置軌道の例である。このとき、時刻t1からの対象物１２０の減速加速度が比較的大きい場合には、その時刻t1以後の、ある現在時刻ｔ2における今回歩容に係るＺＭＰ修正量がＸ軸の正方向に過大になって、支持脚足平２２の接地面を逸脱する場合がある（この場合、Ｓ１９の判断結果がＮＯになる）。そして、仮に、そのまま目標歩容を生成し続けた場合には、図示のようにロボット１の上体３がある時刻ｔ4で対象物１２０に衝突するような目標歩容が生成されてしまう。

そこで、このような場合には、目標対象物反力を、前記したようにＳ０３で仮決定された目標対象物反力軌道に対して、図１５（ｂ）に示す如く、現在時刻ｔ2以降のある期間ΔＴ1内で、Ｘ軸の負方向に増加させるように該目標対象物反力軌道を修正する。換言すれば、ロボット１から対象物１２０に加える作用力（目標対象物反力の符号を反転させた力）をＸ軸の正方向に増加させるように目標対象物反力を修正する。この場合、目標対象物反力を増加させる期間ΔＴ1あるいはその増加分ΔＦ1の大きさは、修正後の目標対象物反力を基に前記Ｓ０９〜Ｓ１７の処理により決定されるＺＭＰ修正量ができるだけ小さくなって、Ｓ１９の判断結果がＹＥＳになるように決定される。このようなΔＴ1あるいはΔＦ1は、解析的あるいは探索的に求めることが可能である。

このように目標対象物反力軌道を修正することにより、ロボット１が積極的に対象物１２０を押すような上体３の運動が行なわれるように今回歩容パラメータのうちの対象物反力モーメント軌道パラメータがＳ１５で決定されることとなる。その結果、この今回歩容パラメータを基に生成される目標歩容が、安定余裕を確保しつつ、対象物１２０にロボット１の上体３が衝突することが無いようなような歩容になる。

補足するとＳ１９およびＳ２１の処理は、本発明の第３発明におけるパラメータ修正手段の処理に相当する。

Ｓ２１の処理を行なった後、Ｓ０９〜Ｓ１７の処理が前記したように再度実行される。このとき、Ｓ１７の次のＳ１９の判断結果はＹＥＳになり、この場合には、次に図６のＳ２３に進む。

Ｓ２３では、現在の今回歩容パラメータと定常歩容パラメータとを基に、現在時刻から定常歩容の第２旋回歩容終端までの（今回歩容の１歩を含めて３歩目までの）目標上***置姿勢軌道を算出すると共に、目標対象物運動軌道を基に目標手先位置姿勢軌道を算出する。

この場合、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、各歩容の歩容パラメータを基に、前記図１４のブロック図を参照して説明した如く、目標上***置姿勢軌道を算出する。さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、Ｓ０９で求めた場合と同様に、目標対象物運動軌道を基に算出する。この場合、定常旋回歩容の第１旋回歩容、第２旋回歩容に対応する目標対象物運動軌道は、Ｓ０３で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容に対応する目標対象物運動軌道である。

補足すると、Ｓ２３の処理は、本発明の第６発明におけるロボット将来挙動予測手段に相当する。

次いで、Ｓ２５に進んで、ロボット１と対象物１２０との幾何学的制約条件をチェックする。ここで、この幾何学的制約条件は、対象物１２０とロボット１との干渉（上体３が対象物１２０に当たるなど）の有無、並びに、各腕体３の関節の回転角が機構的な制限内に収まっているかなどの条件である。目標上***置姿勢と目標対象物運動軌道とからロボット１の上体３と対象物１２０との干渉の有無を判断できる。また、目標上***置姿勢と目標手先位置姿勢とから、各腕体３の関節の回転角を決定できるので、その回転角が機構的な制限内に収まっているか否かが判る。

Ｓ２５の判断処理は、現在時刻から第２旋回歩容の終端まで、所定の刻み時間間隔の目標上体姿勢等の瞬時値に対して逐次行なわれる。すなわち、各瞬時値に対して、Ｓ２５の判断結果がＯＫである場合には、Ｓ２７において、現在時刻から第２旋回歩容の終端までの全時刻分のチェックが完了したか否かを判断する。そして、このＳ２７の判断結果がＮＯである場合には、Ｓ２５の判断処理を繰り返す。

Ｓ２５の判断結果がＮＧである場合には、Ｓ２９に進んで、ＮＧとなった該当時刻における幾何学的制約条件を満たすように着地予定位置姿勢および／または目標対象物運動軌道を修正する。その修正例を図１６を参照して説明する。

同図を参照して、現在時刻t1における移動計画に基づくＸ軸方向の目標対象物運動軌道（位置軌道）が図中のグラフg4で示す軌道（以下、元軌道という）であるとし、将来のある時刻t2で目標上***置と目標対象物位置とが近すぎて、上体３と対象物１２０との干渉が発生し、Ｓ２５の判断結果がＮＧになるとする。このとき、その干渉が発生する時刻t2の目標対象物位置を、Ｘ軸の正方向、すなわち、ロボット１から遠ざける方向に図中の破線矢印で示す如くずらすと共に、時刻t2の前後に亘る期間ΔT2での目標対象物運動軌道（位置軌道）をグラフg5で示す修正軌道に変更する。この修正軌道g5は、ロボット１の挙動の急変を避けるために、その期間ΔT2の両端で元軌道g4に滑らかにつながるように決定される。また、時刻t2でのずらし量ΔＸは、それによって、ロボット１の上体３と対象物１２０との干渉が無くなるように（Ｓ２５の判断結果がＯＫとなるように）決定される。

なお、上記の例では、目標対象物運動軌道を修正する場合を例に採って説明したが、目標対象物運動軌道を修正する代わりに、Ｓ２５の判断結果がＮＧとなる該当時刻を含む歩容での遊脚足平２２の着地予定位置姿勢を修正するようにしてもよい。例えば、図１６の場合のように時刻t2で干渉が発生する場合には、その時刻g2を含む歩容での着地予定位置姿勢を対象物１２０からＸ軸方向で遠ざけるような位置姿勢に修正すればよい。あるいは、目標対象物運動軌道と着地予定位置姿勢の両者を修正するようにしてもよい。

Ｓ２９の修正処理が終了すると、Ｓ０５に戻って、このＳ０５からの処理が前述したように実行される。これにより、前記幾何学的制約条件が満たされない場合に、目標対象物運動軌道あるいは着地予定位置姿勢が修正され、ひいては、今回歩容パラメータのうちの足平軌道パラメータが修正されることとなる。なお、Ｓ２９で目標対象物運動軌道を修正せずに、遊脚足平２２の着地予定位置を修正した場合には、Ｓ０５の処理を省略してＳ０７からの処理を実行するようにしてもよい。

補足すると、Ｓ２５〜Ｓ２９の処理、並びに、Ｓ２９に続くＳ０５〜Ｓ１７の処理は、本発明の第６発明におけるパラメータ修正手段が、ロボット将来挙動予測手段の処理以外に行なう処理に相当する。

一方、Ｓ２７の判断結果がＹＥＳになると、図７のＳ３１に進む。このＳ３１では、今回歩容パラメータを基に、目標ＺＭＰを満足するように、手先位置姿勢瞬時値を除く、現在時刻の今回歩容瞬時値を算出する。

この算出処理は、次のように行なわれる。すなわち、今回歩容パラメータのうちの足平軌道パラメータを基に、目標足平位置姿勢の瞬時値が算出される。この算出処理は、前記Ｓ１３の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。なお、定常歩容に関して説明した場合と同様、今回歩容の遊脚足平２２の着地予定位置姿勢が、前回歩容の支持脚足平２２の着地位置姿勢と同じである場合には、今回歩容の各目標足平位置姿勢の瞬時値は、今回歩容初期の状態に維持される。但し、今回歩容の遊脚足平２２を一旦上昇させて、元の着地位置姿勢に戻すように今回歩容の遊脚足平２２の足平位置姿勢軌道の瞬時値を生成するようにしてもよい。

また、今回歩容パラメータのうちの、ＺＭＰ軌道パラメータを基に、目標ＺＭＰの瞬時値が算出されると共に、対象物反力モーメント軌道パラメータを基に、目標対象物反力モーメントの瞬時値が算出される。

さらに、上記算出された目標足平位置姿勢の瞬時値と、目標ＺＭＰの瞬時値と、目標対象物反力モーメントの瞬時値とから、前記図１４の動力学的演算によって、目標上***置姿勢の瞬時値が算出される。なお、本実施形態では、目標上体姿勢は、例えば鉛直姿勢とされる。但し、目標上体姿勢を鉛直姿勢に維持する必要はなく、変化させるようにしてもよい。目標上体姿勢を変化させる場合には、ロボット単純化モデルは、上体３の姿勢変化に伴うロボット３の角運動量変化を考慮した動力学モデルにすることが望ましく、例えば本願出願人はＰＣＴ国際公開公報WO/03/057422A1にて提案した、同公報の図１０に示すような動力学モデルを使用すればよい。

以上のＳ３１の処理によりロボット１の目標歩容の運動のうちの、目標足平位置姿勢および目標上***置姿勢の瞬時値が決定される。本実施形態のロボット１では、各脚体２は６自由度を有するので、目標上***置姿勢と目標足平位置姿勢とが決まれば、各脚体２の各関節の回転角は一義的に定まる。補足すると、本実施形態では、Ｓ３１で求められる目標上***置姿勢は後述するＳ３９の処理によってさらに補正される。

次いで、Ｓ３３に進んで、最終修正目標手先位置の前回値（前回の演算処理周期での値）を基に、前回の演算処理周期（歩容生成装置１００の演算処理周期）における対象物１２０の実際の位置である実対象物位置を推定する。ここで、最終修正目標手先位置は、後述する腕メイン制御装置１０６で求められるものであり、ロボット１の実際の手先位置の指令値に相当するものである。そこで、例えば最終修正目標手先位置の前回値をロボット１の実際の手先位置の前回値とし、これと所定の相対位置関係を有する位置を実対象物位置として推定する。なお、対象物１２０にジャイロセンサや加速度センサを搭載しておき、それらの検出値から実対象物位置を推定するようにすることも可能である。

次いで、Ｓ３５に進み、上記の如く推定した実対象物位置と、目標対象物位置との偏差（以下、対象物位置偏差という）と、現在の目標対象物運動軌道とを基に、対象物動力学モデルを用いて目標対象物運動の瞬時値（今回値）と、推定外乱力の瞬時値（今回値）と、目標対象物反力の瞬時値（今回値）とが算出される。なお、対象物位置偏差を規定する実対象物位置と目標対象物位置とのうちの目標対象物位置は、前回の演算処理周期で求めた値（前回値）が用いられる。

ここで、このＳ３５の処理で使用する対象物動力学モデルとＳ３５の処理とを図１７のブロック図を参照して説明する。図１７は、Ｓ０３で目標対象物運動のうちの目標対象物位置と目標対象物反力と推定外乱力とを求める演算処理を示すブロック図であり、図中の参照符号２３８を付した部分が対象物１２０に作用する力と対象物１２０の運動との関係を表す対象物動力学モデルとなっている。この対象物動力学モデルは、前記図９に示したものと一部の構造が相違するので、以下の説明では、図１７中の対象物動力学モデルを図９の対象物動力学モデルと区別するため、対象物動力学モデル２という。

この対象物動力学モデル２の基本構造は、図９の対象物動力学モデルと同様であり、対象物１２０に作用する力（より詳しくは水平方向の並進力）を入力として、その入力値（後述の乗算部２４２で求められる値）に、対象物１２０の質量Ｍの逆数１／Ｍを乗算部２４４で乗算することにより対象物１２０の運動加速度を求め、これを積分器２４６，２５０で順次積分する（２重積分する）ものとなっている。但し、対象物動力学モデル２では、図９の対象物動力学モデルと異なり、積分器２５０には、積分器２４６の出力（運動加速度の積分値）に加えて、モデル速度操作量が追加的に入力されるようになっている。このモデル速度操作量は、モデル速度操作量決定部２５２によって、実対象物位置と目標対象物位置との偏差を０に近づけるようにフィードバック制御則により決定される速度操作量であり、本実施形態では次式１７により決定される。

モデル速度操作量＝Ke1＊対象物位置偏差＋∫(Ke2＊対象物位置偏差)dt ……式１７

すなわち、モデル速度操作量は、対象物位置偏差からＰＩ制御則（比例積分制御則）により決定される。なお、式１７のKe1、Ke2は所定のゲインである。図１７のモデル速度操作量決定部２５２は、式１７の右辺の演算を行なう演算処理部である。すなわち、モデル速度操作量決定部２５２は、実対象物位置（現在の演算処理周期でのＳ３３で求めた今回値）と目標対象物位置（前回の演算処理周期でのＳ３５で求めた前回値）とから、それらの差である対象物位置偏差を減算部２５４で求める。そして、求めた対象物位置偏差に乗算部２５６でゲインKe1を乗算したもの（比例項）と、該対象物位置偏差に乗算部２５８でゲインKe2を乗算したものをさらに積分器２６０で積分したもの（積分項）とを、加算部２６２で加算することにより、モデル速度操作量を算出する。なお、算出されたモデル速度操作量は、対象物動力学モデル２の積分器２５０の入力側に備えた加算部２４８で、積分器２４６の出力に加算された後、積分器２５０に入力される。補足すると、図１７のブロック図では、モデル速度操作量を算出した後に、これを対象物動力学モデル２に追加的に入力するようになっているが、モデル速度操作量決定部２５２の積分器２６０を省略し、前記乗算部２５８の出力を追加的に積分器２４６に入力しつつ、このときの積分器２４６の出力と、乗算部２５６の出力との和を積分器２５０に入力するようにしてもよい。このようにしても、対象物動力学モデル２の出力（積分器２５０の出力）は図１７のものと同じになる。

かかる対象物動力学モデル２では、加算部２４８の出力を積分器２５０で積分したものが目標対象物位置の瞬時値として得られる。また、加算部２４８の出力が、該対象物動力学モデル２上での対象物１２０の移動速度である対象物モデル速度となる。

対象物動力学モデル２の入力である、対象物１２０への作用力（並進力）の要求値は、現在の目標対象物運動軌道（Ｓ２７の判断結果がＹＥＳになったときの目標対象物運動軌道）に基づく目標対象物速度と、対象物モデル速度とから減算部２４０で求められるそれらの偏差（目標対象物速度−対象物モデル速度）に乗算部２４２で所定のゲインKvを乗算することで求められる。すなわち、対象物動力学モデル２に入力する並進力の要求値は、図９のものと同様、対象物目標速度と対象物モデル速度との偏差が０に収束するように、フィードバック制御則（この例では比例制御則）により決定される。但し、この場合の対象物モデル速度は、前記したように加算部２４８の出力であるから、図９のものと異なり、モデル速度操作量が加味されたものである。また、目標対象物速度は、現在の目標対象物運動軌道のうちの位置軌道の１階微分値として得られる。

そして、上記の如く求められた並進力の要求値の符号を反転させたものが目標対象物反力の瞬時値として求められる。また、該並進力の要求値が対象物動力学モデル２の乗算部２４４に入力されると共に、前記モデル速度操作量が加算部２４８に入力され、これにより、該対象物動力学モデル２の積分器２５０から目標対象物位置の瞬時値が出力される。

また、図１７中の参照符号２６４を付した部分は、推定外乱力を求める処理を行なう推定外乱力決定部であり、この推定外乱力決定部２６４には、対象物モデル速度と、前記並進力の要求値とが入力される。対象物モデル速度は、M・s／(Tc・s＋1)という形の伝達関数で表される変換部２６６に入力され、この変換部２６６により、対象物１２０に作用している並進力の推定値が算出される。なお、変換部２６６の伝達関数中のＭは対象物１２０の質量であり、Ｔcは所定の時定数である。さらに、この並進力の推定値から前記乗算部２４２の出力である並進力の要求値を減算部２６８で差し引くことにより、推定外乱力の瞬時値が算出される。ここで求められた推定外乱力の瞬時値は、前記したようにＳ０２の演算処理（図９を参照）で使用される。

補足すると、前記推定外乱力決定部２６４は、本発明の第７発明における対象物外乱力推定手段に相当する。

なお、目標対象物運動のうちの目標対象物姿勢の瞬時値は、例えば目標対象物速度の向きにほぼ一致するように決定される。

以上が、Ｓ３５の演算処理である。

次いでＳ３７に進んで、Ｓ３５で求めた目標対象物運動の瞬時値を基に、目標手先位置姿勢の瞬時値（今回値）を決定する。目標手先位置姿勢の瞬時値は、Ｓ０９で目標手先位置姿勢軌道を求めた場合と同様に求められる。

以上のＳ３７までの処理によって求められる目標歩容の瞬時値のうち、目標上***置姿勢は、前記ロボット単純化モデルを用いて、該ロボット単純化モデル上で目標ＺＭＰを満足するように（ロボット１の運動による慣性力と重力と対象物反力モーメントとの合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になるように）求められている。従って、Ｓ３７までの処理によって求められる目標歩容は、目標ＺＭＰまわりの床反力モーメント水平成分がロボット単純化モデル上で０となる歩容である。

ここで、ロボット単純化モデルは、歩容生成装置１００の演算負荷を効果的に軽減できる点での利点は大きいものもの、動力学的精度は必ずしも高精度であるとはいえない。そこで、本実施形態では、さらに、動力学的精度の高いロボット動力学モデル（以下、これをフルモデルという）を用いて、目標歩容のうちの一部の構成要素（具体的には、目標上***置姿勢瞬時値と目標ＺＭＰまわりの床反力モーメント瞬時値）とを補正する。この補正処理をフルモデル補正と呼び、このフルモデル補正がＳ３７の処理の次にＳ３９で実行される。

このフルモデル補正は、例えば本願出願人が先に提案した特開２００２−３２６１７３号公報にて説明したものと全く同様に行なわれる。従って、本明細書での説明は省略する。なお、フルモデル補正は、例えば、本願出願人によるＰＣＴ国際公開公報WO 03/057427 A1にて説明したものと同様に行なうようにしてもよい。

このＳ３９のフルモデル補正により、目標上***置姿勢と、目標ＺＭＰまわりの床反力モーメントとが補正され、これにより、最終的に歩容生成装置１００が出力する目標歩容の全ての構成要素の瞬時値が得られる。

補足すると、Ｓ３１〜Ｓ３９の処理は、本発明における今回歩容瞬時値決定手段に相当する。

以上説明したＳ０１〜Ｓ３９までの処理が、歩容生成装置１００の演算処理周期毎に実行される処理である。

次に、説明を後回しにした、Ｓ０１での移動計画の修正処理について説明する。

ロボット１が対象物１２０を押しながら移動しているときに、現在の移動計画（前記移動要求通りの移動計画など）で想定していない段差（床の凸部）があったり、対象物１２０にロボット１以外から予期しない外力が作用したような場合には、移動計画に基づく目標対象物運動軌道と実際の対象物運動軌道（以下、実対象物運動軌道という）とのずれが大きくなる。一方、歩容生成装置１００は、目標対象物運動軌道を基に歩容パラメータ等を決定して、歩容を生成するので、目標対象物運動軌道と実対象物運動軌道とのずれが過大になると、ロボット１の継続的安定性を確保し得る歩容を生成することが困難となる。そこで、本実施形態におけるＳ０１では、そのずれがある程度大きくなった場合（あるいは大きくなると予測される場合）には、Ｓ０１で決定する移動計画をリアルタイムで修正する。

以下、この処理の具体例を図１８（ａ），（ｂ）を参照して説明する。例えば現在（修正前の）移動計画が、対象物１２０を一定速度でＸ軸の正方向に移動させるという計画であるとする。このとき、この修正前の移動計画（以下、修正前移動計画という）を基に、前記した如くＳ３５で算出される目標対象物運動軌道（瞬時値の時系列）のうちの目標対象物位置軌道は、ほぼ、図１８（ａ）のグラフg6で示すような軌道となる。ここで、時刻t1において、実際の対象物１２０が、修正前移動計画で予期していない床の段差（凸部）に引っかかって、対象物１２０が止まってしまったとする。このとき、実対象物位置の軌道（Ｓ３３で推定される実対象物位置の時系列）は、図１８（ａ）のグラフg7で示すような軌道となる。この場合、時刻t1以後、修正前移動計画に基づく目標対象物位置と、実対象物位置との偏差が時間の経過に伴い大きくなっていく。

そこで、Ｓ０１では、例えば前回の演算処理周期のＳ３５で求めた対象物位置偏差（実対象物位置と目標対象物位置との差）、すなわち、図１７の減算部２５４で求めた対象物位置偏差を所定値と比較するようにしている。そして、その対象物位置偏差が所定値よりも大きくなったとき（図１８（ａ）の時刻t2）には、図１８（ａ）のグラフg8で示す如く、対象物位置偏差の増加を抑制するように修正前移動計画を修正する。修正した移動計画を以下、修正後移動計画という。図１８（ａ）の例では、修正後移動計画は、時刻t2以後、対象物１２０の移動速度ほぼ０になるまで徐々に減速するような移動計画となる。なお、修正後移動計画は、それにより規定される目標対象物運動が修正前移動計画に対して急激な変化を生じることなく滑らかに連続するように決定することが望ましい。

このように移動計画を修正することによって、目標対象物反力が過剰に大きくなったりすることがなく、ロボット１の継続的な安定性を確保し得る歩容を生成することができる。

さらに、本実施形態でのＳ０１の処理では、上記のように移動計画を修正した後、実対象物位置の、前記修正後移動計画に対応する目標対象物位置軌道への追従性が良くなり、目標対象物運動軌道と実対象物運動軌道とのずれがある程度小さくなったら、修正後移動計画を修正前移動計画に近づけるように移動計画を修正する。この修正後の移動計画を以下、再修正後移動計画という。

図１８（ａ）に示したように修正後移動計画を決定した場合を例に採って説明すると、対象物１２０が段差を乗り越えると（図１８（ｂ）の時刻t3）、実対象物位置軌道は、図１８（ｂ）のグラフg7で示すように修正後移動計画に基づく目標対象物位置軌道g8に近づいていく。

このとき、Ｓ０１では、対象物位置偏差が所定値よりも小さくなると（図１８（ｂ）の時刻t4）、図１８（ｂ）のグラフg9で示す如く、対象物１２０の移動速度が当初の修正前移動計画に基づく対象物１２０の移動速度に近づくように、修正後移動計画を再修正して、再修正後移動計画を決定する。図１８（ｂ）の例では、再修正後移動計画は、時刻t4以後、対象物１２０の移動速度が当初の修正前移動計画に基づく移動速度にほぼ一致するまで、該移動速度を徐々に増加させるような移動計画となる。なお、再修正後移動計画は、それにより規定される目標対象物運動が修正後移動計画に対して急減な変化を生じることなく滑らかに連続するように決定することが望ましい。

上記の例では、再修正後移動計画に基づく対象物１２０の移動速度を修正前移動計画に基づく移動速度に近づけるようにしたが、再修正後移動計画に基づく目標対象物位置軌道を修正前移動計画に基づく対象物位置軌道に近づけるようにすることも可能である。

補足すると、上記のようにＳ０１で移動計画を修正する処理が、本発明の第８発明で移動計画を修正する処理に相当するものである。

以上が、本実施形態における歩容生成装置１００の処理の詳細である。

次に、制御ユニット６０の歩容生成装置１００以外の処理を以下に説明する。なお、この処理は、先に述べたように、特開平１０−２３０４８５号公報の第１実施例にて提案したものと同じであるので、概略的な説明に留める。

歩容生成装置１００で生成された目標歩容のうち、目標上***置姿勢軌道と、目標ＺＭＰ軌道と、目標対象物反力軌道とが対象物反力平衡制御装置１０２に入力される。対象物反力平衡制御装置１０２は、目標対象物反力と実際のロボット１が対象物１２０から受ける反力である実対象物反力との偏差を解消する（０に近づける）ように、目標ＺＭＰまわりの目標床反力モーメントを修正するための対象物反力平衡制御用補償全床反力を算出すると共に、目標運動のうちの目標上***置姿勢を修正してなる修正目標上***置姿勢を求めるものである。この対象物反力平衡制御装置１０２の処理については後述する。

補足すると、この対象物反力平衡制御装置１０２は、本発明の第９発明における歩容瞬時値修正手段に相当するものとである。

また、目標歩容のうちの目標足平位置姿勢軌道、目標ＺＭＰ軌道および目標全床反力軌道は、脚メイン制御装置１０４に入力される。さらに、脚メイン制御装置１０４には、対象物反力平衡制御装置１０２から修正目標上***置姿勢と対象物反力平衡制御用補償全床反力が入力される。脚メイン制御装置１０４は、コンプライアンス制御処理によって、目標歩容の運動（腕体５，５の運動を除く）と床反力とに追従させるように脚体２，２の関節アクチュエータ（電動モータ）を制御する。より具体的には、姿勢センサ５４の検出値（実上体姿勢）を修正目標上体姿勢に復元させるために、目標ＺＭＰに発生させるべき復元全床反力を算出し、目標ＺＭＰに作用する実全床反力モーメント成分（これは脚体２，２の６軸力センサ５０の検出値から求められる）が、この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力との合力のモーメント成分に一致するように、目標足平位置姿勢を修正する。修正された目標足平位置姿勢を修正目標足平位置姿勢という。そして、脚メイン制御装置１０４は、この修正目標足平位置姿勢と修正目標上***置姿勢とから決定される両脚体２，２の目標関節変位に実関節変位が追従するように両脚体２，２の関節アクチェエータを制御する（各脚体２のモータ駆動指令を各関節アクチュエータに出力する）。

また、目標歩容のうちの目標手先位置姿勢軌道と目標対象物反力軌道とは、腕メイン制御装置１０６に入力される。さらに、腕メイン制御装置１０６には、対象物反力平衡制御装置１０２から修正目標上***置姿勢が入力される。腕メイン制御装置１０６は、コンプライアンス制御処理によって、目標手先位置姿勢軌道と目標対象物反力軌道に追従させるように腕体５，５の関節アクチュエータ（電動モータ）を制御する。より具体的には、６軸力センサ５２の検出値（実対象物反力）と目標対象物反力との差に応じて目標手先位置姿勢を修正する。修正された目標手先位置姿勢を最終修正目標手先位置姿勢という。そして腕メイン制御装置１０６は、この最終修正目標手先位置姿勢と修正目標上***置姿勢とから決定される両腕体５，５の目標関節変位に実関節変位が追従するように両腕体５，５の関節アクチュエータを制御する（各腕体５のモータ駆動指令を各関節アクチュエータに出力する）。

補足すると前記脚メイン制御装置１０４および腕メイン制御装置１０６は、本発明の第９発明における制御手段に相当するものである。

対象物反力平衡制御装置１０２の処理をより具体的に以下に説明する。図１９は、対象物反力平衡制御装置１０２の処理を機能的に示すブロック図である。

対象物反力平衡制御装置１０２では、まず、目標対象物反力による目標ＺＭＰまわりの目標対象物反力モーメントと実対象物反力による目標ＺＭＰまわりの実対象物反力モーメントとの偏差である対象物反力モーメント偏差がモーメント偏差算出部２７０により算出される。この場合、目標対象物反力モーメントは、歩容生成装置１００から出力された目標対象物反力および目標ＺＭＰと、腕メイン制御装置１０６で求められた最終修正目標手先位置姿勢（より詳しくは前回の制御処理周期で値）とから算出される。また、実対象物反力モーメントは、６軸力センサ５２の検出値（実対象物反力）と、目標ＺＭＰと、最終修正目標手先位置姿勢（より詳しくは前回の制御処理周期で値）とから算出される。

この対象物反力モーメント偏差に応じて、重心位置摂動量算出部２７２により、ロボット１の全体重心位置の目標摂動量である目標重心位置摂動量が算出される。目標重心位置摂動量は、対象物反力モーメント偏差を、ロボット１に作用する重力によって長期的に解消するためのロボット１の全体重心の摂動量としての意味を持ち、例えば対象物反力モーメントに比例した値に決定される。次いで、後述する摂動動力学モデルで算出される、該摂動動力学モデル上でのロボット１の全体重心の摂動量である全体重心位置モデル摂動量と、前記目標重心位置摂動量との偏差が減算部２７４により算出され、この偏差からフィードバック制御則２７６、例えばＰＤ制御則によって、該偏差を０に収束させるための、目標ＺＭＰまわりのモーメント操作量である対象物反力平衡制御用補償全床反力モーメントが算出される。さらに、この対象物反力平衡制御用補償全床反力モーメントと前記対象物反力モーメント偏差との和が加算部２７８により算出される。そして、この加算部２７８の出力がロボット１の全体重心の摂動と、目標ＺＭＰまわりのモーメントの摂動との関係、並びに、全体重心の摂動と上***置姿勢の摂動との関係を表す摂動動力学モデル２８０に入力され、この摂動動力学モデルで、上***置姿勢摂動量が算出される。

この場合、摂動動力学モデルは、ロボット１の全体重心の摂動と、目標ＺＭＰまわりのモーメントの摂動との関係（動力学的関係）を次式１８により記述するモデルである。

mtotal＊hG＊d2ΔxG/dt2＝ΔxG＊mtotal＊g＋ΔMx ……式１８

ここで、hGは、目標ＺＭＰから全体重心までの高さ、ΔxGは全体重心の水平方向摂動量、ΔMxは目標ＺＭＰまわりのモーメント水平成分である。その他の変数は、前記ロボット単純化モデルに関して定義したものと同じである。なお、式１８は、サジタルプレーン上での式であり、ラテラルプレーン上での関係式は、式１８の右辺の第２項の符号を反転させればよい。

この式１８は、ロボット１の全体質量mtotalの質点を持ち、また、その質点の支点である目標ＺＭＰまわりに発生するモーメント水平成分がΔMxとなるような倒立振子の動力学的挙動を示す式である。

また、ロボット１の全体重心位置の摂動量ΔxGと上***置の摂動量（以下、これをΔxbと記述する）との関係は、次式１９により表される。

Δxb＝ｋ＊ΔxG ……式１９

ここで、ｋはある比例定数である。従って、Δxｂは、ΔxGに比例するものとされる。摂動運動に対しては、式１９は近似的に成立すると考えてよい。

従って、摂動動力学モデル２８０では、式１８の右辺のΔMxとして、加算部２７８の出力を用いることで、全体重心の摂動量ΔxGが算出され、さらに、このΔxGから式１９によって、上***置摂動量が求められる。なお、本実施形態では、修正目標上***置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿勢の摂動量は０とする。

対象物反力平衡制御装置１０２では、このようにして摂動動力学モデル２８０から出力される上***置摂動量を目標上***置姿勢（歩容生成装置１００の出力）に加算部２８２で加算することにより、修正目標上***置姿勢を算出する。

以上が対象物反力平衡制御装置１０２の具体的な演算処理である。

以上説明した第１実施形態の作動、特に歩容生成装置１００の処理によって、対象物反力を考慮しつつ、今回歩容が定常歩容に収束するように今回歩容が生成される。このため、ロボット１の継続的な安定性を確保しながら、ロボット１により対象物１２０を押すなどの作業を円滑に行なうことができる。また、定常歩容で、将来の対象物反力（２歩目の対象物反力）が考慮されることから、例えばロボット１により対象物１２０を押す作業を今回歩容に続く次回歩容で開始する場合において、ロボット１は今回歩容において、上体３を対象物１２０に向かって加速する動作を行う。そして、その上で、次回歩容から対象物１２０を押す動作を開始する。つまり、押す動作を開始する前に、ロボット１の重心を対象物１２０に向かわせる慣性力を発生した状態で、対象物１２０を押し始める。このため、対象物１２０を押す動作を開始するために、ロボット１の足平位置姿勢を今回歩容から大きくずらしたりすることなく、安定余裕を確保することができる（ＺＭＰ修正量を小さくできる）。

次に、本発明の第２実施形態を図２０を参照して説明する。なお、この第２実施形態の説明では、第１実施形態と同一構成部分あるいは同一機能部分については第１実施形態と同じ参照符号を用い、説明を省略する。この第２実施形態は、本発明の第１〜第３発明、第５〜第９発明の実施形態である。

第２実施形態は、図５のＳ０１において、移動計画を修正する処理（本発明の第８発明に対応する処理）のみが第１実施形態と相違するものである。以下説明すると、実対象物運動軌道と、移動計画に基づく目標対象物運動軌道とのずれが大きくなるような状況は、実際の対象物１２０に作用する力のうち、ロボット１から作用する力以外の外乱力が比較的大きく変化した場合に発生する。

そこで、第２実施形態では、第１実施形態で説明した如く対象物位置偏差に応じて移動計画を修正する代わりに、例えば前記推定外乱力の変化に応じて移動計画を修正する。これ以外は、第１実施形態と同じである。

以下、具体例を説明すると、例えば、前記図１８（ａ）に示した例の場合には、前記Ｓ３５で算出される推定外乱力（Ｘ軸方向成分）は、図２０（ａ）に示すように、対象物１２０が段差に引っかかる時刻t1以後に負方向に急激に大きくなる。そこで、第２実施形態では、この推定外乱力が所定値以上変化したとき（図２０（ａ）の時刻t5）に、図１８（ａ）に関して第１実施形態で説明した如く、修正前移動計画を修正して、修正後移動計画を決定する。

さらに、図１８（ｂ）に示した例の場合には、前記Ｓ３５で算出される推定外乱力（Ｘ軸方向成分）は、図２０（ｂ）に示すように、対象物１２０が段差を乗り越えた時刻t3以後に、時刻t1以前の元の値とほぼ同じ値に戻っていく。そこで、この推定外乱力が、所定値以上変化した後に、元の値にほぼ近い値に戻ったとき（図２０（ｂ）の時刻t6）に、図１８（ｂ）に関して第１実施形態で説明した如く、修正後移動計画を修正して、再修正後移動計画を決定する。

以上説明した以外の処理は、前記第１実施形態と同じである。

かかる第２実施形態においても、第１実施形態と同様の作用効果が得られる。

補足すると、第２実施形態では、推定外乱力の変化に応じて移動計画を修正するようにしたが、両腕体５，５の６軸力センサ５２の検出値から把握される実対象物反力と、Ｓ３５で決定される目標対象物反力との偏差が所定値よりも大きくなったときに移動計画を修正しするようにしてもよい。

次に本発明の第３実施形態を図２１および図２２を参照して説明する。なお、この第３実施形態の説明では、第１実施形態と同一構成部分あるいは同一機能部分については第１実施形態と同じ参照符号を用い、説明を省略する。この第３実施形態は、本発明の第１発明、第４〜第９発明の実施形態である。

図２１は第３実施形態での歩容生成装置１００の処理のうち、第１実施形態に係る図５の処理に対応する部分の処理を示すフローチャートである。同図２１に示すように、第３実施形態では、Ｓ１９の判断結果がＮＯである場合の処理のみが第１実施形態と相違している。

すなわち、第１実施形態では、Ｓ１９の判断結果がＮＯであるときに、目標対象物反力軌道を修正するようにしたが、第３実施形態では、これに代えて、Ｓ２１’において、遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正する。これは今回歩容パラメータのうちの足平軌道パラメータを修正することを意味する。この場合、着地予定位置姿勢または着地予定時刻は、その修正後にＳ０７〜Ｓ１７の処理を再度実行したときに、ＺＭＰ修正量ができるだけ小さくなるように（少なくとも着地予定位置姿勢または着地予定時刻の修正前に求めたＺＭＰ修正量よりも小さくなるように）修正される。そして、その修正後に、Ｓ０７からの処理を再度実行する。これ以外は、第１実施形態と同一である。

Ｓ２１’での具体的な修正例を以下に説明する。例えば今現在、今回歩容の遊脚足平２２の着地予定位置姿勢と次回歩容の遊脚足平２２の着地予定位置姿勢が図２２の実線で示す如く決定されているとする。なお、図示の例では、ロボット１をほぼ一定の歩幅で、今回歩容支持脚座標系のＸ軸方向に直進歩行させるものとなっている。

そして、今回歩容まではロボット１が対象物１２０に近づき、次回歩容の途中から、ロボット１が対象物１２０をＸ軸の正方向に押す作業を開始するものとする。この場合、図２３（ａ）に示す如く、目標対象物反力軌道は、例えば次回歩容の途中から立ち上がるようなステップ状の軌道となる。

このとき、図２２の実線で示すような着地予定位置姿勢を維持したまま、Ｓ１７までの処理を実行すると、Ｓ１７の処理で決定されるＺＭＰ修正量（Ｘ軸方向成分）は、例えば図２３（ｂ）に実線で示す如く、比較的大きなものとなって、Ｓ１９の判断結果がＮＯとなる場合がある。

この場合に、Ｓ２１’の処理では、例えば次回歩容の遊脚足平２２の着地予定位置姿勢を図２２の破線で示す如く、今回歩容の支持脚足平２２ＬにＸ軸方向で近づけるように修正する。すなわち、次回歩容の歩幅をより小さくするように遊脚足平２２Ｌの着地予定位置姿勢を修正する。このように次回歩容の着地予定位置姿勢を修正した後に、Ｓ０５からの処理を再び実行すると、Ｓ１７で決定されるＺＭＰ修正量が、図２３（ｂ）に破線で示す如く小さくなる。その結果、Ｓ１９の判断結果がＹＥＳになる。補足すると、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を図２２の如く修正したとき、前記した定常歩容の足平軌道パラメータの決定手法によって、定常歩容の第２旋回歩容の終端遊脚足平位置も、第２旋回歩容の初期遊脚足平位置（今回歩容の遊脚足平２２Ｒの着地予定位置）に近づくこととなる。

なお、上記の例では、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を修正するようにしたが、今回歩容の遊脚足平２２Ｒの着地予定位置姿勢を修正する余裕がある場合には、それを修正するようにしてもよい。あるいは、今回歩容および次回歩容の両者の着地予定位置姿勢を修正するようにしてもよい。

また、上記の例では、着地予定位置姿勢を修正するようにしたが、今回歩容および／または次回歩容の遊脚足平２２の着地予定時刻を修正するようにしてもよい。図２３に示した状況では、例えば次回歩容の着地予定時刻を遅らせるようにすればよい。

以上が本発明の第３実施形態である。かかる第３実施形態においても、第１実施形態と同様の作用効果を奏することができる。なお、第３実施形態では、Ｓ１９の判断結果がＮＯである場合に遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正するようにしたが、それと併せて、第１実施形態の如く、目標対象物反力軌道を修正するようにしてもよい。

なお、以上説明した第１〜第３実施形態では、ロボット１により対象物１２０を押して移動させる場合を例に採って説明したが、対象物１２０を引いて移動させる場合や、対象物１２０を持ち上げて移動させる場合などにも本発明を適用することができる。さらには、電磁力や、空気流などの流体によって、ロボット１が直接的にある対象物に接触せずに、該ロボット１が外力を受ける環境下でロボット１を移動させるような場合にも本発明を適用することができる。

本発明の実施形態における脚式移動ロボットのとしての２足移動ロボットの概略構成を示す図。 図１のロボットに備えた制御ユニットの構成を示すブロック図。 図２の制御ユニットの要部の機能的構成を示すブロック図。 実施形態におけるロボットと対象物との関係を示す図。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 図５のＳ０２で決定する移動計画の例を示す図。 図５のＳ０２の処理を示すブロック図。 図１０（ａ）は図５のＳ０７に係る仮目標ＺＭＰ軌道の例を示す図、図１０（ｂ）は図５のＳ１７で決定されるＺＭＰ修正量の例を示す図、図１０（ｃ）はＳ１７で修正された目標ＺＭＰ軌道の例を示す図。 図５のＳ１１で決定する足平軌道パラメータの例を示す図。 図１２（ａ）は今回歩容の対象物床反力モーメント軌道の例を示す図、図１２（ｂ）は図５のＳ１１で決定する定常歩容の対象物反力モーメント軌道の例を示す図。 実施形態で用いるロボット動力学モデルの例を示す図。 図１３のロボット動力学モデルを使用して上***置を求める処理を示すブロック図。 図１５（ａ），（ｂ）は、図５のＳ２１の処理を説明するための図。 図６のＳ２９の処理を説明するための図。 図７のＳ３５の処理を示すブロック図。 図１８（ａ），（ｂ）は、図５のＳ０１の移動計画の修正処理を説明するための図。 図３に示す対象物反力平衡制御装置の処理を示すブロック図。 本発明の第２実施形態におけるＳ０１（図５）の移動計画の修正処理を説明するための図。 本発明の第３実施形態における歩容生成装置の要部の処理を示すフローチャート。 図２１のＳ２１’の処理を説明するための図。 図２３（ａ），（ｂ）は図２１のＳ２１’の処理を説明するための図。

符号の説明

１…脚式移動ロボット（２足移動ロボット）、２…脚体、３…上体、１００…歩容生成装置（外力軌道仮決定手段、今回歩容パラメータ決定手段、周期的歩容パラメータ決定手段、パラメータ修正手段、今回歩容瞬時値決定手段、対象物軌道仮決定手段、ロボット将来挙動予測手段）、１２０…推定外乱力決定部（対象物外乱力推定手段）。

Claims (8)

1. 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための該ロボットの歩容を生成する装置において、
   少なくとも前記対象物の運動と該対象物に作用する力との関係を表わす対象物動力学モデルと前記対象物の移動計画とを基に、前記対象物からロボットに作用する反力としての外力の目標軌道と前記対象物の目標運動軌道とを仮決定する対象物軌道仮決定手段と、
   少なくとも前記仮決定された対象物の目標運動軌道と外力の目標軌道とを基に、新たに作成しようとする所定期間分のロボットの目標歩容である今回歩容における脚体の運動軌道を規定するパラメータとロボットに作用させるべき床反力の軌道を規定するパラメータとロボットに作用させるべき外力の軌道を規定するパラメータとを少なくとも含む今回歩容パラメータを仮決定する今回歩容パラメータ仮決定手段と、
   少なくとも前記仮決定された対象物の目標運動軌道と外力の目標軌道とを基に、前記今回歩容に続く仮想的な周期的歩容における脚体の運動軌道を規定するパラメータとロボットに作用させるべき床反力の軌道を規定するパラメータとロボットに作用させるべき外力の軌道を規定するパラメータとを少なくとも含む周期的歩容パラメータを決定する周期的歩容パラメータ決定手段と、
   前記ロボットに作用する床反力および外力とロボットの運動との関係を表すロボット動力学モデルと前記今回歩容パラメータとに基づいて求められる該ロボットの上体の運動軌道が、該ロボット動力学モデルと前記周期的歩容パラメータとに基づいて求められる該ロボットの上体の運動軌道に収束するという条件を満足するように、前記仮決定された今回歩容パラメータのうち、前記脚体の運動軌道を規定するパラメータと前記床反力の軌道を規定するパラメータと前記外力の軌道を規定するパラメータとの少なくともいずれか１つのパラメータを修正して今回歩容パラメータを決定するパラメータ修正手段と、
   少なくとも前記修正された今回歩容パラメータに基づいて前記今回歩容の瞬時値を逐次決定する今回歩容瞬時値決定手段とを備えたことを特徴とする脚式移動ロボットの歩容生成装置。
2. 前記パラメータ修正手段が修正対象とするパラメータは、前記今回歩容パラメータのうち、少なくとも前記床反力の軌道を規定するパラメータを含み、
   該パラメータ修正手段は、前記仮決定された今回歩容パラメータのうち、前記床反力の軌道を規定するパラメータを前記条件を満足するように修正したときの該パラメータの修正量が所定量を超えるとき、該修正量を小さくしつつ前記条件を満足するように、前記仮決定された今回歩容パラメータのうちの少なくとも前記外力の軌道を規定するパラメータを修正する手段を備えることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
3. 前記パラメータ修正手段が修正対象とするパラメータは、前記今回歩容パラメータのうち、少なくとも前記床反力の軌道を規定するパラメータを含み、
   該パラメータ修正手段は、前記仮決定された今回歩容パラメータのうち、前記床反力の軌道を規定するパラメータを前記条件を満足するように修正したときの該パラメータの修正量が所定量を超えるとき、該修正量を小さくしつつ前記条件を満足するように、前記仮決定された今回歩容パラメータのうちの少なくとも前記脚体の運動軌道を規定するパラメータを修正する手段を備えることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
4. 前記床反力の軌道を規定するパラメータは、ロボットの目標ＺＭＰの軌道を規定するパラメータであることを特徴とする請求項２または３記載の脚式移動ロボットの歩容生成装置。
5. 前記パラメータ修正手段は、少なくとも前記パラメータ修正手段により修正された今回歩容パラメータと前記周期的歩容パラメータと、前記ロボット動力学モデルとを基に前記ロボットの将来の運動を予測するロボット将来挙動予測手段と、
   前記予測されたロボットの将来の運動と、前記仮決定された前記対象物の目標運動軌道とを基に、該ロボットと対象物とが所定の幾何学的制約条件を満たしているか否かを判断し、満たしていない場合には前記対象物の目標運動軌道と前記修正された今回歩容パラメータのうちの脚体の運動軌道を規定するパラメータとのうちの少なくともいずれか一方を修正して今回パラメータを決定する手段とを備えることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
6. 前記対象物に作用する力のうち、前記今回歩容瞬時値に応じて動作している前記ロボットから前記対象物に作用する力以外の外乱力を推定する対象物外乱力推定手段を備え、
   前記対象物仮軌道決定手段は、前記対象物動力学モデルと前記移動計画と前記推定された外乱力とを基に前記対象物の目標運動軌道と前記外力の目標軌道とを仮決定する手段であることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
7. 前記対象物軌道仮決定手段は、前記移動計画に対応する対象物の運動軌道と前記仮決定した対象物の目標運動軌道との間の差と、前記推定された外乱力とのうちの少なくともいずれか一方に基づき、前記移動計画を修正する手段を備えることを特徴とする請求項６記載の脚式移動ロボットの歩容生成装置。
8. 請求項１〜７のいずれか１項に記載の脚式移動ロボットの歩容生成装置により生成された今回歩容瞬時値に応じて該ロボットの動作を制御する制御装置において、
   前記ロボットに作用する実床反力以外の実外力と、前記今回歩容瞬時値のうちの前記外力の瞬時値との偏差に応じて、該偏差を０に近づけるように、前記ロボットの今回歩容瞬時値のうちの前記ロボットの目標運動と目標床反力とのうちの少なくともいずれか一方を修正する歩容瞬時値修正手段と、その修正された歩容瞬時値に追従するように該ロボットの動作を制御する制御手段とを備えたことを特徴とする脚式移動ロボットの制御装置。

Images