JP4808626B2 - 脚式移動ロボットの歩容生成装置 - Google Patents

Description

本発明は脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を、その目標歩容に追従するようにロボットの動作を制御しながら生成する装置に関する。

２足移動ロボットなどの脚式移動ロボットに、ある対象物を押して移動させるなどの作業を行わせる場合には、ロボットはその脚体の先端部が接触する床から床反力を受けるだけでなく、対象物から反力を受ける。このように対象物からロボットが受ける反力を本明細書では対象物反力という。

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

また、２足移動ロボット（ヒューマノイドロボット）の挙動をシミュレーションするものとして、日本ロボット学会誌／Vol.19／No.1, pp.28〜36, 2001／「仮想ロボットプラットフォーム」（以下、非特許文献１という）に見られるものが知られている。

ところで、対象物を移動させる作業を行なうためのロボットの目標歩容を生成する場合、一般に、該対象物を移動させる環境条件（床の形状、摩擦力状態など）を想定した上で、対象物の移動計画（対象物をどのようなタイミングでどのように移動させるというような計画）を基に、対象物反力の軌道（時系列パターン）や、対象物の運動軌道（対象物の移動位置および姿勢の時系列パターン）、ロボットの各脚体の先端部の運動軌道などを規定する基本的な歩容生成用のパラメータが決定される。そして、その決定されたパラメータを用いて、前記想定した環境条件下で動力学的な制約条件（例えば前記動力学的平衡条件を満たす、ＺＭＰがロボットの接地面内に存するなどの条件）を満たすようにロボットの目標歩容が生成される。

この場合、移動計画や歩容生成用のパラメータが不適切であると、ロボットの上体が対象物に衝突しない、対象物に係合させるロボットの腕体が可動範囲を超えない、などの幾何学的な制約条件を満たさない目標歩容が生成されてしまうことがある。

そこで、例えば前記特許文献１に見られる技術に前記非特許文献１に見られるような技術を適用し、ロボットの挙動のシミュレーションを行って、幾何学的な制約条件を満たしているか否かをチェックしながら、目標歩容を生成することが考えられる。

しかしながら、前記非特許文献１に見られるような技術は、ある環境条件を想定した上で、ロボットの挙動をシミュレートする（予測する）ものであるため、実際の環境条件と想定した環境条件とが異なると（このような状況はしばしば発生する）、生成した目標歩容では実際には前記幾何学的制約条件が満たされないという状況が発生することが多々ある。

本発明はかかる背景に鑑みてなされたものであり、ロボットの動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、ロボットと対象物との間の干渉などに係わる所要の幾何学的制約条件を満たす目標歩容を生成することができる脚式移動ロボットの歩容生成装置を提供することを目的とする。

本発明の脚式移動ロボットの歩容生成装置の第１発明は、かかる目的を達成するために、
上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を、該目標歩容に追従するようにロボットを動作させつつ生成する歩容生成装置において、
対象物に作用する力と該対象物の運動との関係を表す対象物動力学モデルと、
前記目標歩容に追従するようにロボットを動作させているときの実際の対象物の挙動を観測しつつ、前記対象物動力学モデル上での対象物の挙動を実際の対象物の挙動に近づけるために前記対象物動力学モデル与えるべきモデル操作量であって、該対象物動力学モデル上での対象物に、前記ロボットから作用させる力以外で作用させるべき力を表すモデル操作量を決定するモデル操作量決定手段と、
前記ロボットを追従させるべき新たな目標歩容を生成するとき、前記決定されたモデル操作量を前記対象物動力学モデルに与えつつ、少なくとも前記対象物の移動計画と前記対象物動力学モデル上での対象物の運動状態量とに基づいて、該運動状態量を前記移動計画に基づく対象物の運動状態量に追従させるように現在以降の所定期間における対象物とロボットとの間の作用力の目標軌道であるロボット・対象物間目標作用力軌道を仮決定するロボット・対象物間作用力軌道仮決定手段と、
前記決定されたモデル操作量を前記対象物動力学モデルに与えつつ、前記仮決定されたロボット・対象物間目標作用力軌道を該対象物動力学モデルに入力して、該対象物動力学モデルにより、前記所定期間における対象物の目標運動軌道を仮決定する対象物運動軌道仮決定手段と、
少なくとも前記仮決定された対象物の目標運動軌道と前記仮決定されたロボット・対象物間目標作用力軌道とを基に、ロボットの目標歩容を生成するために用いる歩容パラメータを仮決定し、その仮決定した歩容パラメータを用いて前記所定期間におけるロボットの目標歩容を仮生成するロボット歩容仮生成手段と、
前記仮決定した対象物の目標運動軌道と前記仮生成したロボットの目標歩容の運動軌道とを基に、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断する制約条件判断手段と、
前記幾何学的制約条件が満たされていないとき、前記移動計画と前記仮決定した歩容パラメータと前記仮決定した対象物の目標運動軌道とのうちの少なくとも一つを修正対象として、該修正対象を前記幾何学的制約条件が満たされるように修正する修正手段とを備え、
前記修正された修正対象を用いて前記新たな目標歩容を生成するようにしたことを特徴とするものである。

かかる第１発明によれば、対象物動力学モデル（これは順動力学モデルである）に前記モデル操作量を与えつつ、前記ロボット・対象物間作用力仮決定手段と対象物運動軌道仮決定手段とによりそれぞれロボット・対象物間目標作用力軌道と対象物の目標運動軌道とが仮決定されるため、これらのロボット・対象物間目標作用力軌道と目標運動軌道とは、現在以降の所定期間における実際のロボット・対象物間の作用力軌道と対象物の運動軌道との予測軌道として確度（信頼性）の高いものとなる。なお、前記モデル操作量としては、実際の対象物に作用する力のうち、ロボット以外から作用する外乱力（対象物に床から作用する摩擦力など）、もしくはそれを規定するものなどが挙げられる。また、ロボット・対象物間目標作用力軌道を仮決定するときに追従させる運動状態量としては、例えば対象物の移動速度が挙げられる。

さらに、このように仮決定されたロボット・対象物間目標作用力軌道と対象物の目標運動軌道とを基に、ロボットの歩容パラメータを決定して、前記所定期間における目標歩容を仮生成するので、その仮生成した目標歩容も、現在以降の所定期間における実際のロボットの予測歩容として確度（信頼性）の高いものとなる。なお、歩容パラメータとしては、ロボットの各脚体の運動軌道を規定するパラメータ（例えば各脚体の着地予定位置、着地予定時刻など）、目標ＺＭＰもしくは目標床反力中心点の軌道を規定するパラメータなどが挙げられる。この場合、例えばロボットの各部の運動が発生する慣性力とロボットに作用する重力とロボット・対象物間目標作用力により定まる、対象物からロボットへの反力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になる、というような所定の動力学的平衡条件が、ロボットの動力学モデル（ロボットの運動とロボットに作用する力との関係を表すモデル）上で満たされるように歩容パラメータを基に目標歩容を仮生成すればよい。

そして、前記仮決定された対象物の目標運動軌道と前記仮生成したロボットの目標歩容の運動軌道とを基に、前記所定期間において、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断することで、その判断を適正に行なうことができる（信頼性の高い判断を行なうことができる）。ここで、前記幾何学的制約条件としては、例えば対象物とロボットの上体とが干渉しない、ロボットの腕体など対象物に係合させるべき部分の関節が可動範囲を超えるような変位をしないなど、対象物とロボットとの間の位置・姿勢関係に依存する幾何学的制約条件である。

さらに、本発明では、上記の判断で前記幾何学的制約条件が満たされないときに、前記移動計画と歩容パラメータと前記仮決定した対象物の目標運動軌道とのうちの少なくとも一方を修正対象として、前記幾何学的制約条件が満たされるように該修正対象を修正し、その修正した修正対象を用いて新たな目標歩容を生成する。これにより、前記幾何学的制約条件を満たすような新たな目標歩容を生成できる。

このように第１発明では、ロボットの動作制御を行ないながら、ロボットの目標歩容を生成するとき、対象物の実際の挙動を前記モデル操作量によって対象物動力学モデルに反映させつつ、将来の（前記所定期間における）ロボット・対象物間目標作用力軌道と対象物の目標運動軌道とを仮決定し、さらにこれらを基に、ロボットの将来の目標歩容を仮決定するので、対象物とロボットとの挙動の信頼性の高い予測を行なうことができる。そして、その将来の対象物の目標運動軌道と目標歩容の運動軌道とを基に前記幾何学的制約条件が満たされているか否かを判断し、満たされていない場合に、移動計画と歩容パラメータと前記仮決定した対象物の目標運動軌道とのうちの少なくともいずれか一つ（修正対象）を修正した上で、その修正対象を用いて新たな目標歩容を生成する。

このため、第１発明によれば、ロボットの動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、ロボットと対象物との間の干渉などに係わる所要の幾何学的制約条件を満たす目標歩容を生成することができる。

なお、この第１発明において、修正後の修正対象を用いて目標歩容を生成する場合、その修正対象が移動計画であるときには、新たな目標歩容を生成するとき、例えば前記ロボット・対象物間作用力軌道仮決定手段、対象物運動軌道仮決定手段、ロボット歩容仮生成手段と同じ処理を順次実行して歩容パラメータを決定し、その決定した歩容パラメータを用いてロボットの新たな目標歩容を生成するようにすればよい。また、修正対象が歩容パラメータであるときには、その歩容パラメータを用いてロボットの新たな目標歩容を生成するようにすればよい。この場合、ロボット・対象物間目標作用力軌道と対象物の目標運動軌道とは、前記仮決定されたものと同じでよい。また、修正対象が対象物の目標運動軌道であるときには、前記ロボット歩容仮生成手段と同じ処理を実行して歩容パラメータを決定し、その決定した歩容パラメータを用いてロボットの新たな目標歩容を生成するようにすればよい。この場合、対象物の目標運動軌道は、修正後のものを用いる。

なお、第１発明では、好ましくは、前記モデル操作量決定手段は、実際の対象物に作用する力のうち、前記ロボットから対象物に作用する力以外の外乱力を前記モデル操作量として推定する手段である（第２発明）。この場合には、前記対象物運動軌道仮決定手段では、前記推定された外乱力（モデル操作量）と、前記仮決定されたロボット・対象物間目標作用力軌道により定まるロボットから対象物への目標作用力との合力を前記対象物動力学モデルに入力して、その合力と、該対象物動力学モデルの動力学的関係を満たす対象物の運動軌道を、前記対象物の目標運動軌道として決定するようにすればよい。

補足すると、前記外乱力は、例えば次のように推定することができる。すなわち、対象物の実際の運動速度あるいは加速度を推定または検出して、その推定値または検出値から対象物に実際に作用している力を把握する。また、ロボットから対象物に実際に作用している力を推定または検出し、その力の推定値または検出値を前記把握した力から差し引くことにより、前記外乱力を推定する。

次に、本発明の脚式移動ロボットの歩容生成装置の第３発明は、前記の目的を達成するために、
上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を、該目標歩容に追従するようにロボットを動作させつつ生成する歩容生成装置において、
対象物の運動と該対象物に作用する力との関係を表す対象物動力学モデルと、
前記目標歩容に追従するようにロボットを動作させているときの実際の対象物の挙動を観測しつつ、実際の対象物に作用する力のうち、ロボットから対象物に作用する力以外の外乱力を推定する外乱力推定手段と、
前記ロボットを追従させるべき新たな目標歩容を生成するとき、少なくとも前記対象物の移動計画に基づいて、現在以降の所定期間における対象物の目標運動軌道を規定する目標運動状態量を仮決定する運動状態量仮決定手段と、
前記仮決定された目標運動状態量を前記対象物動力学モデルに入力して、該対象物動力学モデルにより、現在以降の所定期間において前記対象物に作用させるべき力の目標軌道である対象物作用力目標軌道を仮決定する対象物作用力軌道仮決定手段と、
前記仮決定された対象物作用力目標軌道と前記推定された外乱力とを基に前記所定期間におけるロボットと対象物との間の作用力の目標軌道であるロボット・対象物間目標作用力軌道を仮決定するロボット・対象物間作用力軌道仮決定手段と、
少なくとも前記仮決定された目標運動状態量により規定される対象物の目標運動軌道と前記仮決定されたロボット・対象物間目標作用力軌道とを基に、ロボットの目標歩容を生成するために用いる歩容パラメータを仮決定し、その仮決定した歩容パラメータと前記推定された外乱力と前記仮決定されたロボット・対象物間目標作用力とを用いて前記所定期間におけるロボットの目標歩容を仮生成するロボット歩容仮生成手段と、
前記仮決定された目標運動状態量により規定される対象物の目標運動軌道と前記仮生成したロボットの目標歩容の運動軌道とを基に、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断する制約条件判断手段と、
前記幾何学的制約条件が満たされていないとき、前記移動計画と前記仮決定した歩容パラメータと前記仮決定された目標運動状態量により規定される対象物の目標運動軌道とのうちの少なくとも一つを修正対象として、該修正対象を前記幾何学的制約条件が満たされるように修正する修正手段とを備え、
前記修正された修正対象を用いて前記新たな目標歩容を生成するようにしたことを特徴とするものである。

かかる第３発明によれば、対象物の移動計画に基づいて前記運動状態量仮決定手段により仮決定した対象物の目標運動状態量を対象物動力学モデル（これは逆動力学モデルである）に入力することで、前記対象物作用力軌道仮決定手段により現在以降の所定期間における対象物作用力目標軌道を仮決定し、この対象物作用力目標軌道と前記推定外乱力とを基に、前記ロボット・対象物間作用力軌道仮決定手段によりロボット・対象物間目標作用力軌道を仮決定する（具体的には、例えば対象物作用力から外乱力の推定値を差し引いたものをロボットから対象物への目標作用力として、ロボット・対象物間目標作用力軌道を仮決定する）ので、そのロボット・対象物間目標作用力軌道は、現在以降の所定期間において実際の対象物を移動計画に従って移動させる上で、実際のロボットと対象物との間で発生させるべきロボット・対象物作用力軌道の予測軌道として確度（信頼性）の高いものとなる。なお、前記運動状態量としては、例えば対象物の移動速度が挙げられる。また、前記外乱力は、前記第２発明に関して説明した如く、推定すればよい。

さらに、前記仮決定された目標運動状態量により規定される対象物の目標運動軌道と前記仮決定されたロボット・対象物間目標作用力軌道とを基に、ロボットの歩容パラメータを決定して、前記所定期間における目標歩容を仮生成するので、その仮生成した目標歩容も、現在以降の所定期間における実際のロボットの予測歩容として確度（信頼性）の高いものとなる。なお、歩容パラメータとしては、前記第１発明と同様のパラメータが挙げられる。この場合、例えば第１発明の場合と同様、所定の動力学的平衡条件が、ロボットの動力学モデル（ロボットの運動とロボットに作用する力との関係を表すモデル）上で満たされるように歩容パラメータを基に目標歩容を仮生成すればよい。

そして、前記第１発明と同様に、前記仮決定された目標対象物運動軌道と前記仮生成したロボットの目標歩容とを基に、前記所定期間において、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断することで、その判断を適正に行なうことができる（信頼性の高い判断を行なうことができる）。該幾何学的制約条件は、前記第１発明に関して説明したものと同様の条件である。さらに、上記の判断で前記幾何学的制約条件が満たされないときに、前記移動計画と歩容パラメータと前記仮決定された目標運動状態量により規定される対象物の目標運動軌道とのうちの少なくとも一つを修正対象として、前記幾何学的条件が満たされるように該修正対象を修正し、その修正した歩容パラメータを用いて新たな目標歩容を生成する。これにより、前記幾何学的制約条件を満たすような新たな目標歩容を生成できる。

このように第３発明は、ロボットの動作制御を行ないながら、ロボットの目標歩容を生成するとき、移動計画に従って対象物を運動させるために要求させる、対象物への作用力（対象物にロボットから作用させる力と、該ロボット以外から対象物に作用する外乱力との合力）として対象物動力学モデルにより求められる対象物作用力目標軌道と、実際の対象物に作用する外乱力の推定値とを基にロボット・対象物間目標作用力軌道を仮決定する。そして、このロボット・対象物間目標作用力軌道と、前記対象物動力学モデルに入力する目標運動状態量により規定される対象物の目標運動軌道とを基に、ロボットの将来の目標歩容を仮決定するので、対象物とロボットとの挙動の信頼性の高い予測を行なうことができる。さらに、その将来の対象物の目標運動軌道と目標歩容とを基に前記幾何学的制約条件が満たされているか否かを判断し、満たされていない場合に、移動計画と歩容パラメータと前記仮決定された目標運動状態量により規定される対象物の目標運動軌道とのうちの少なくともいずれか一つ（修正対象）を修正した上で、その修正対象を用いて新たな目標歩容を生成する。

このため、第３発明によっても、第１発明と同様に、ロボットの動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、ロボットと対象物との間の干渉などに係わる所要の幾何学的制約条件を満たす目標歩容を生成することができる。

なお、この第３発明において、修正後の修正対象を用いて目標歩容を生成する場合、その修正対象が移動計画であるときには、新たな目標歩容を生成するとき、例えば前記運動状態量仮決定手段、対象物作用力軌道仮決定手段、ロボット・対象物間作用力軌道仮決定手段、ロボット歩容仮生成手段と同じ処理を順次実行して歩容パラメータを決定し、その決定した歩容パラメータを用いてロボットの目標歩容を生成するようにすればよい。また、修正対象が歩容パラメータであるときには、その歩容パラメータを用いてロボットの目標歩容を生成するようにすればよい。この場合、ロボット・対象物間目標作用力軌道と目標運動状態量（ひいては対象物の目標運動軌道）とは、前記仮決定されたものと同じでよい。また、修正対象が対象物の目標運動軌道であるときには、前記ロボット歩容仮生成手段と同じ処理を実行して歩容パラメータを決定し、その決定した歩容パラメータを用いてロボットの目標歩容を生成するようにすればよい。この場合、対象物の目標運動軌道は、修正後のものを用いる。

前記第１〜第３発明では、前記修正手段が前記歩容パラメータを修正対象として修正するとき、その修正対象の歩容パラメータは、前記ロボットの各脚体の着地予定位置を含むことが好ましい（第４発明）。

これによれば、歩容パラメータの修正に伴う前記幾何学的制約条件への影響の予測がつきやすいので、繰り返し歩容パラメータを修正しなければならないような事態を防止でき、修正手段の修正処理が容易になる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

また、本明細書では、ＺＭＰ(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で除算したものを、対象物反力ＺＭＰと定義し、これをZMPobjと記述する。すなわち、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は、解析的あるいは探索的に求めることが可能である。

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

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

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

この場合、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、各歩容の歩容パラメータを基に、前記図１４のブロック図を参照して説明した如く、目標上***置姿勢軌道を算出する。より具体的には、各歩容の足平軌道パラメータを基に、目標両足平位置姿勢軌道が算出されると共に、ＺＭＰ軌道パラメータおよび対象物反力モーメント軌道パラメータを基に、それぞれ目標ＺＭＰ軌道、対象物反力モーメント軌道が算出される。なお、目標両足平位置姿勢軌道の算出処理は、前記Ｓ１３の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。そして、これらの目標両足平位置姿勢軌道、目標ＺＭＰ軌道、および対象物反力モーメント軌道を図１４のブロック図の演算処理に用いることで、目標上***置姿勢が決定される。なお、本実施形態では、目標状態姿勢は鉛直姿勢とされる。このように、目標上***置姿勢を算出することで、ロボット単純化モデル上で、ロボット１の運動が発生する慣性力と、ロボット１に作用する目標対象物反力と重力との合力が目標ＺＭＰまわりの発生するモーメントの水平成分が０になるという動力学的平衡条件を満たすようにロボット１の目標運動のうちの目標上***置姿勢軌道が算出される。

さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、Ｓ０９で求めた場合と同様に、目標対象物運動軌道を基に算出する。なお、定常旋回歩容の第１旋回歩容、第２旋回歩容に対応する目標対象物運動軌道は、Ｓ０３で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容に対応する目標対象物運動軌道である。

以上説明したＳ０５〜Ｓ２３の処理は、本発明の第１発明におけるロボット歩容仮生成手段の処理に相当している。この場合、Ｓ０５〜Ｓ２１の処理により最終的に得られる今回歩容パラメータ、定常歩容パラメータが第１発明におけるロボット歩容仮生成手段の処理が仮決定する歩容パラメータに相当する。

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

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

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

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

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

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

補足すると、以上説明したＳ２５〜Ｓ２９の処理は、本発明の第１発明における修正手段に相当する。

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

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

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

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

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

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

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

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

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

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

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

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

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

そして、上記の如く求められた並進力の要求値の符号を反転させたものが目標対象物反力の瞬時値として求められる。また、該並進力の要求値が対象物動力学モデル２の乗算部２４４に入力されると共に、前記モデル速度操作量が加算部２４８に入力され、これにより、該対象物動力学モデル２の積分器２５０から目標対象物位置の瞬時値が出力される。換言すれば、対象物動力学モデル２上での対象物１２０の運動状態量としての対象物モデル速度を、目標対象物位置と実対象物位置との偏差を０に近づけるためのモデル速度操作量で修正しつつ、対象物動力学モデル２の動力学演算により目標対象物位置が逐次決定される。

また、図１７中の参照符号２６４を付した部分は、推定外乱力を求める処理を行なう推定外乱力決定部であり、この推定外乱力決定部２６４には、対象物モデル速度と、前記並進力の要求値とが入力される。対象物モデル速度は、M・s／(Tc・s＋1)という形の伝達関数で表される変換部２６６に入力され、この変換部２６６により、対象物１２０に作用している並進力の推定値が算出される。この変換部２６６の伝達関数中のＭは対象物１２０の質量、Ｔcは所定の時定数である。従って、変換部２６６は、対象物モデル速度の微分値（１階微分値）に対象物１２０の質量を乗算したもの（これは対象物１２０に作用する全ての力の合力の瞬時値に相当する）に、時定数Ｔcを有するフィルタリング処理を施したものを対象物１２０に作用している並進力の推定値として求める。換言すれば、変換部２６６により算出される推定値は、対象物動力学モデル２上での対象物１２０の運動加速度（対象物モデル速度の微分値）を発生させる並進力に、時定数Ｔ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における移動計画に基づくＸ軸方向の目標対象物運動軌道（位置軌道）が図中のグラフg10で示す軌道（以下、修正前移動計画という）、すなわち、対象物１２０の移動速度が現在時刻以降、急激に大きくなるような軌道であるとする。この場合、現在時刻以降のロボット１の目標上***置の軌道が例えば図中のグラフg11で示すような軌道で算出され、将来のある時刻t2で目標上***置が目標対象物位置から離れすぎて、腕体５，５が伸び過ぎる状態となり、Ｓ２５の判断結果がＮＧになることがある。修正前移動計画に基づく対象物１２０の移動速度が急激に変化すると、このような状況が生じることがある。このとき、対象物１２０の修正前移動計画をグラフg12で示すような軌道となる移動計画（以下、修正後移動計画という）に修正する。すなわち、該修正後移動計画に基づく対象物１２０の移動速度が修正前移動計画に基づく移動速度よりも遅くなるように修正する。このようにすることで、目標上***置が目標対象物位置により近づくような目標歩容を生成することができるようになる。

なお、前記第１実施形態で説明した図１６の場合の如く、将来のある時刻でロボット１の上体３と対象物１２０との干渉が生じるような目標歩容がＳ２３で生成された場合にも、Ｓ２９’において、移動計画を修正するようにしてもよい。この場合には、図１６の元軌道に対応する移動計画を図１６の修正軌道を持つような移動計画に修正すればよい。

かかる第２実施形態においても、第１実施形態と同様の効果を奏することができる。補足すると、第２実施形態では、Ｓ２９’での移動計画の修正後、Ｓ０３からの処理を実行するので、結果的に、目標対象物運動軌道、目標対象物反力軌道、歩容パラメータの足平軌道パラメータ（遊脚足平２２の着地予定位置姿勢および着地予定時刻）も修正されることとなる。

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

前記第１実施形態では、Ｓ０３（図５を参照）の演算処理で用いる対象物動力学モデルとして、図９に示した順動力学モデルを用いた。本実施形態は、これに代えて、逆動力学モデルを用いるものであり、Ｓ０３の演算処理のみが第１実施形態と相違するものである。

図２２は、本実施形態でのＳ０３の演算処理を示すブロック図であり、このブロック図中の破線で囲んだ部分が第２発明における対象物動力学モデルに相当するものとなっている。この対象物動力学モデルでは、対象物１２０の移動計画に基づく対象物１２０の目標運動状態量としての目標対象物移動速度を入力として、対象物１２０に作用する力（並進力）と対象物１２０の位置とを出力する動力学モデルである。より詳しくは、この対象物動力学モデルは、目標対象物移動速度（入力値）を微分器３００で微分（１階微分）することにより、対象物１２０の運動加速度を求め、さらに、この運動加速度に対象物１２０の質量Ｍを乗算部３０２で乗算することにより、対象物１２０に作用する力（並進力）の要求値を出力するものとなっている。ここで、対象物動力学モデルから出力される並進力の要求値は、対象物１２０の作用する全ての並進力（対象物１２０の移動方向の並進力）の合力の要求値である。さらに、対象物動力学モデルは、目標対象物移動速度を積分器３０４で積分することにより、対象物１２０の位置の目標値である目標対象物位置を出力するものとなっている。

なお、目標対象物速度は、第１実施形態と同様、歩容生成装置１００において、移動計画における対象物１２０の位置軌道の１階微分値として算出され、あるいは、移動計画の構成要素としての移動速度がそのまま、目標対象物速度として決定される。

かかる対象物動力学モデルを用いるＳ０３の演算処理を図２２を参照して具体的に説明すると、まず、前記図５のＳ０１で決定された移動計画に基づく対象物１２０の瞬時瞬時の（各時刻の）目標対象物速度が対象物動力学モデルに入力され、前記した如く、対象物１２０に作用する並進力の要求値と、目標対象物位置とが時系列的に算出される。

次いで、上記並進力の要求値から、歩容生成装置１００の前回の演算処理周期において前記Ｓ３５（図７参照）で求められた推定外乱力を減算部３０６で減算することにより、ロボット１から対象物１２０に作用させるべき力（並進力）の目標値が求められる。そして、この目標値の符号を反転させたものの時系列が目標対象物反力軌道として求められる。

なお、対象物動力学モデルの積分器３０４の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻（前回の演算処理周期での現在時刻）における値に設定される。また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。

以上説明したＳ０３の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。このＳ０３の処理以外は、前記第１実施形態と同一である。

なお、このＳ０３の処理は、本発明の第２発明における目標運動状態量仮決定手段、対象物作用力軌道仮決定手段、および目標作用力軌道仮決定手段に相当している。より詳しくは、移動計画に基づき目標対象物速度を決定する処理が目標運動状態量仮決定手段に相当しており、対象物動力学モデルの微分器３００及び乗算部３０２の処理が対象物作用力仮決定手段の処理に相当している。この場合、乗算部３０２の出力が対象物作用力目標軌道に相当する。また、減算部３０６の処理がロボット・対象物間作用力軌道仮決定手段の処理に相当している。

以上説明した第３実施形態の作動、特に歩容生成装置１００の処理によって、第１実施形態と同様に、ロボット１の継続的な安定性を確保しながら、ロボット１により対象物１２０を押すなどの作業を円滑に行なうことができる。

また、Ｓ０３では、対象物１２０の移動計画に基づく目標対象物速度を基に目標対象物運動軌道を仮決定する一方、対象物１２０の実際の挙動（本実施形態では実対象物位置の挙動）を観測しながら前記図１７のブロック図で求めた推定外乱力を図２２のブロック図の処理で反映させ、現在時刻から所定期間分（複数歩分）の目標対象物反力軌道が仮決定される。そして、前記第１実施形態と同様に、これらの仮決定した目標対象物運動軌道と目標対象物反力軌道とを基に、今回歩容および定常旋回歩容の歩容パラメータを決定して、現在時刻以降の将来のロボットの目標歩容がＳ２３で仮生成される。さらに、この仮生成された目標歩容の運動軌道と目標対象物運動軌道とを基に、Ｓ２５で幾何学的制約条件を満たしているか否かが判断され、該条件が満たされない場合には、Ｓ２９でロボット１の遊脚足平２２の着地予定位置姿勢や目標対象物運動軌道が修正され、ひいては、歩容パラメータのうちの足平軌道パラメータが幾何学的制約条件を満たすように修正される。そして、最終的にその修正された歩容パラメータを用いて今回歩容の瞬時値が歩容生成装置１００の演算処理周期毎にリアルタイムで逐次生成される。このため、第１実施形態と同様に、ロボット１の動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、前記幾何学的制約条件を満たす目標歩容を生成することができ、ひいては、対象物１２０とロボット１との干渉などを生じることなく、ロボット１による対象物１２０の移動を円滑に行なうことができる。

なお、本実施形態においても、第２実施形態と同様に、図６のＳ２９の処理で移動計画を修正するようにしてもよい。

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

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

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

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

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

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

補足すると、第４実施形態では、推定外乱力の変化に応じて移動計画を修正するようにしたが、両腕体５，５の６軸力センサ５２の検出値から把握される実対象物反力と、Ｓ３５で決定される目標対象物反力との偏差が所定値よりも大きくなったときに移動計画を修正するようにしてもよい。また、Ｓ０３では、第３実施形態で説明した如く、前記図２２の対象物動力学モデルを用いて目標対象物位置軌道及び目標対象物反力軌道を求めるようにしてもよい。また、Ｓ２９では、第２実施形態で説明した如く、移動計画を修正するようにしてもよい。

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

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

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

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

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

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

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

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

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

以上が本発明の第５実施形態である。かかる第５実施形態においても、第１実施形態と同様の作用効果を奏することができる。なお、第５実施形態では、Ｓ１９の判断結果がＮＯである場合に遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正するようにしたが、それと併せて、第１実施形態の如く、目標対象物反力軌道を修正するようにしてもよい。また、Ｓ０１では、第４実施形態で説明した如く、推定外乱力に応じて移動計画を推定するようにしてもよい。また、Ｓ０３では、第３実施形態で説明した如く、前記図２２の対象物動力学モデルを用いて目標対象物位置軌道及び目標対象物反力軌道を求めるようにしてもよい。また、Ｓ２９では、第２実施形態で説明した如く、移動計画を修正するようにしてもよい。

なお、以上説明した第１〜第３実施形態では、ロボット１により対象物１２０を押して移動させる場合を例に採って説明したが、対象物１２０を引いて移動させる場合や、対象物１２０を持ち上げて移動させる場合などにも本発明を適用することができる。

また、前記各実施形態では、今回歩容パラメータを決定するときに、周期的な歩容である定常歩容の歩容パラメータを決定した上で、該定常歩容に今回歩容を収束させるように（定常歩容の初期発散成分に今回歩容の終端発散成分を一致させるように）今回歩容パラメータを決定するようにしたが、本発明では、それを必須とするものではない。本発明は、基本的には、最終的に決定されたロボット・対象物間目標作用力（前記各実施形態では目標対象物反力）と対象物の目標移動位置（前記各実施形態では目標対象物運動軌道）とを満足し得るようにロボットの動作を制御できるものであれば、前記各実施形態で説明した手法とは異なる手法でロボットの動作を制御するようにしてもよい。

以上のように、本発明は、２足移動ロボットなどのロボットに対象物を移動させる作業を行わせる歩容を生成する場合に、ロボットと対象物との間の干渉などに関する幾何学的制約条件を満たす目標歩容を生成できるものとして有用である。

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

Claims (4)

1. 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を、該目標歩容に追従するようにロボットを動作させつつ生成する歩容生成装置において、
   対象物に作用する力と該対象物の運動との関係を表す対象物動力学モデルと、
   前記目標歩容に追従するようにロボットを動作させているときの実際の対象物の挙動を観測しつつ、前記対象物動力学モデル上での対象物の挙動を実際の対象物の挙動に近づけるために前記対象物動力学モデル与えるべきモデル操作量であって、該対象物動力学モデル上での対象物に、前記ロボットから作用させる力以外で作用させるべき力を表すモデル操作量を決定するモデル操作量決定手段と、
   前記ロボットを追従させるべき新たな目標歩容を生成するとき、前記決定されたモデル操作量を前記対象物動力学モデルに与えつつ、少なくとも前記対象物の移動計画と前記対象物動力学モデル上での対象物の運動状態量とに基づいて、該運動状態量を前記移動計画に基づく対象物の運動状態量に追従させるように現在以降の所定期間における対象物とロボットとの間の作用力の目標軌道であるロボット・対象物間目標作用力軌道を仮決定するロボット・対象物間作用力軌道仮決定手段と、
   前記決定されたモデル操作量を前記対象物動力学モデルに与えつつ、前記仮決定されたロボット・対象物間目標作用力軌道を該対象物動力学モデルに入力して、該対象物動力学モデルにより、前記所定期間における対象物の目標運動軌道を仮決定する対象物運動軌道仮決定手段と、
   少なくとも前記仮決定された対象物の目標運動軌道と前記仮決定されたロボット・対象物間目標作用力軌道とを基に、ロボットの目標歩容を生成するために用いる歩容パラメータを仮決定し、その仮決定した歩容パラメータを用いて前記所定期間におけるロボットの目標歩容を仮生成するロボット歩容仮生成手段と、
   前記仮決定した対象物の目標運動軌道と前記仮生成したロボットの目標歩容の運動軌道とを基に、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断する制約条件判断手段と、
   前記幾何学的制約条件が満たされていないとき、前記移動計画と前記仮決定した歩容パラメータと前記仮決定した対象物の目標運動軌道とのうちの少なくとも一つを修正対象として、該修正対象を前記幾何学的制約条件が満たされるように修正する修正手段とを備え、
   前記修正された修正対象を用いて前記新たな目標歩容を生成するようにしたことを特徴とする脚式移動ロボットの歩容生成装置。
2. 前記モデル操作量決定手段は、実際の対象物に作用する力のうち、前記ロボットから対象物に作用する力以外の外乱力を前記モデル操作量として推定する手段であることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
3. 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を、該目標歩容に追従するようにロボットを動作させつつ生成する歩容生成装置において、
   対象物の運動と該対象物に作用する力との関係を表す対象物動力学モデルと、
   前記目標歩容に追従するようにロボットを動作させているときの実際の対象物の挙動を観測しつつ、実際の対象物に作用する力のうち、ロボットから対象物に作用する力以外の外乱力を推定する外乱力推定手段と、
   前記ロボットを追従させるべき新たな目標歩容を生成するとき、少なくとも前記対象物の移動計画に基づいて、現在以降の所定期間における対象物の目標運動軌道を規定する目標運動状態量を仮決定する運動状態量仮決定手段と、
   前記仮決定された目標運動状態量を前記対象物動力学モデルに入力して、該対象物動力学モデルにより、現在以降の所定期間において前記対象物に作用させるべき力の目標軌道である対象物作用力目標軌道を仮決定する対象物作用力軌道仮決定手段と、
   前記仮決定された対象物作用力目標軌道と前記推定された外乱力とを基に前記所定期間におけるロボットと対象物との間の作用力の目標軌道であるロボット・対象物間目標作用力軌道を仮決定するロボット・対象物間作用力軌道仮決定手段と、
   少なくとも前記仮決定された目標運動状態量により規定される対象物の目標運動軌道と前記仮決定されたロボット・対象物間目標作用力軌道とを基に、ロボットの目標歩容を生成するために用いる歩容パラメータを仮決定し、その仮決定した歩容パラメータと前記推定された外乱力と前記仮決定されたロボット・対象物間目標作用力とを用いて前記所定期間におけるロボットの目標歩容を仮生成するロボット歩容仮生成手段と、
   前記仮決定された目標運動状態量により規定される対象物の目標運動軌道と前記仮生成したロボットの目標歩容の運動軌道とを基に、対象物とロボットとのうちの少なくともいずれか一方に係る所定の幾何学的制約条件が満たされているか否かを判断する制約条件判断手段と、
   前記幾何学的制約条件が満たされていないとき、前記移動計画と前記仮決定した歩容パラメータと前記仮決定された目標運動状態量により規定される対象物の目標運動軌道とのうちの少なくとも一つを修正対象として、該修正対象を前記幾何学的制約条件が満たされるように修正する修正手段とを備え、
   前記修正された修正対象を用いて前記新たな目標歩容を生成するようにしたことを特徴とする脚式移動ロボットの歩容生成装置。
4. 前記修正手段が修正対象とする歩容パラメータは、前記ロボットの脚体の着地予定位置を含むことを特徴とする請求項１または２記載の脚式移動ロボットの歩容生成装置。

Images