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

Description

本発明は２足移動ロボット等の移動ロボットの目標歩容を生成する装置に関する。

２足移動ロボット等の移動ロボットの目標歩容を生成する技術としては、例えば特開２００２−３２６１７３号公報（特許文献１）や、ＰＣＴ国際公開公報ＷＯ／０３／０５７４２７／Ａ１（特許文献２）に見られるものが本願出願人により提案されている。これらの文献に見られる技術は、ロボットの運動（各部位の位置、姿勢）と、床反力との関係を表す第１の動力学モデルを用いて該第１の動力学モデル上での所要の動力学的平衡条件（床反力の並進力成分が目標値になる、ある点のまわりの床反力モーメントが目標値になるなどの条件）を満足するようにロボットの目標運動の瞬時値（瞬時目標運動）と目標床反力の瞬時値（瞬時目標床反力）とからなる瞬時目標歩容が逐次作成される。そして、この瞬時目標歩容を第２の動力学モデルに入力して、該瞬時目標運動の一部（目標上体位置姿勢や目標ＺＭＰまわりの目標モーメントなど）を補正することで、最終的な瞬時目標歩容を時系列的に生成するようにしている。

この場合、第１の動力学モデルとしては、線形性の高いモデルが一般に使用される。線形性の高い動力学モデルを用いて瞬時目標歩容を作成することで、仮想的な周期的歩容である定常歩容につながり、もしくは漸近するような歩容（ロボットの安定な運動を継続的に行い得る歩容）を効率よく短時間で作成することが可能となる。ひいては実ロボットの実際の運動を行いながら、リアルタイムでロボットの瞬時目標歩容を逐次生成することが可能となる。

ところが、線形性の高い動力学モデルは、ロボットの種々様々の動作において一般に動力学的精度が比較的低くなりがちである。すなわち、その動力学モデル上でのロボットの動力学は、実ロボットの実際の動力学に対して誤差を生じやすい。このため、第１の動力学モデルを用いて作成される瞬時目標歩容を、そのまま実ロボットに適用して、該実ロボットの動作を行わせると、第１の動力学モデル上で保証された動力学的平衡条件が、実ロボット上では成立せず、実ロボットの動作が安定性に欠けるものとなりやすい。

そこで、前記特許文献１、２に見られる技術では、第１の動力学モデルを用いて作成した瞬時目標歩容の一部をさらに、第２の動力学モデルを用いて補正するようにしている。この場合、第２の動力学モデルとしては、第１の動力学モデルよりも動力学的精度の高いモデルが用いられる。これにより、第１の動力学モデルを用いて作成した歩容よりも、より動力学的精度の高い（実ロボットの動力学により近い）歩容を生成することが可能となる。

ところで、前記第１の動力学モデルは、前記したように動力学的精度が低くなりがちであるので、生成しようとする歩容によっては、動力学的な誤差が比較的大きなものとなることがある。すなわち、第１の動力学モデルで想定（考慮）されていない慣性力が発生するようなロボットの運動を行わせる歩容を生成するような場合には、上記誤差が大きなものとなりやすい。例えば、２足移動ロボットの上体、各脚体の先端部付近にそれぞれ対応する質点を１つずつもつような３質点の動力学モデル、あるいは、ロボットの上体にのみ質点をもつ１質点の動力学モデルを前記第１の動力学モヂルとして用いた場合には、特に、各脚体の膝関節を曲げるような動作を比較的すばやく行う場合には、それに伴う慣性力の変化の影響で、動力学的な誤差が比較的大きなものとなる。その結果、この第１の動力学モデルを用いて作成される瞬時目標歩容がロボットの継続的な安定性を確保する上で過剰に不適切なものとなることがある。そして、このような場合には、第２の動力学モデルで該瞬時目標歩容を補正するようにしても、その補正が適正になされず、その補正後の瞬時目標歩容が、安定余裕の低いものになったり、ロボットの継続的な安定性を確保できないものに発散してしまう恐れがあった。

このようなことから、本願発明者は、上記第１の動力学モデルを用いて作成した瞬時目標歩容の運動を動力学モデルを用いることなく（運動と力との関係を表す微分方程式や積分方程式を用いることなく）、幾何学的な演算処理によって所定の部位の位置および姿勢を補正して、瞬時目標床反力との間の動力学的精度を高める（動力学的な誤差を低減する）手法を先に特願２００４−５０２９号にて提案した。この手法は、例えば、上記第１の動力学モデルを用いて作成した瞬時目標歩容の上体位置および上体姿勢が幾何学的演算処理（瞬時目標床反力の値やその時系列値、上体位置姿勢の微分値を用いない演算処理）によって補正される。この手法は動力学的演算処理を必要としないことから、瞬時目標運動の補正を短時間で効率よく行うことが可能である。

ところで、この手法では、瞬時目標歩容の発生毎に、動力学的誤差を小さくするように瞬時目標歩容の運動を幾何学的な演算処理によって補正するものであるため、補正した部位（上体など）の姿勢が頻繁に変動する恐れがある。

特に、上体姿勢を補正する場合には、その上体姿勢が頻繁に変動すると、該上体が一般に重く、また、イナーシャが比較的大きいものであることから、股関節に過大なモーメントが発生する。その結果、股関節用アクチュエータに過大な負荷が掛かったり、股関節部分およびその近辺が撓んで振動し、ロボットの安定性が損なわれる恐れがある。また、例えば２足移動ロボットでは、その上体に視覚装置としての撮像装置が支持されることが一般的であるので、上体姿勢の頻繁な変動によって該撮像装置が揺れ、該撮像装置による環境認識が困難となる恐れがある。さらに、上体姿勢が頻繁に変動することは見栄えも悪い。

なお、上体姿勢の変動を防ぐためには、常に上体姿勢を一定に保持することも考えられる。この場合には、動力学的精度を高めるための瞬時目標運動の補正は、主に、上体位置の補正で行うことが可能である。しかし、このようにした場合には、ロボットの運動形態や床面の摩擦状態によっては、補正後の上体位置の運動（並進運動）によって発生すべき慣性力に見合う床反力を実際には発生できないことがある。そして、このような場合には、補正された瞬時目標運動は、ロボットのスリップを発生させるようなものとなってしまう。

本発明はかかる背景に鑑みてなされたものであり、動力学モデルを用いて作成された瞬時目標歩容の運動を、動力学モデルを用いることなく（運動と力の関係を表す微分方程式や積分方程式を用いることなく）、瞬時目標歩容の床反力との間の動力学的精度の向上と、ロボットの上体などの所定の部位の姿勢の変動を極力少なくすることとの両立を図るように適切に補正することができ、ロボットの運動を安定に行うことを可能とする歩容を生成できる移動ロボットの歩容生成装置を提供することを目的とする。

本発明の移動ロボットの歩容生成装置の第１発明は、前記の目的を達成するために、移動ロボットの瞬時目標運動と瞬時目標床反力とからなる瞬時目標歩容を逐次発生する瞬時歩容発生手段を備えた歩容生成装置において、前記瞬時目標運動から前記移動ロボットの所定の部位の位置および姿勢を仮補正してなる第１の仮補正瞬時目標運動を決定する第１仮補正運動決定手段と、前記瞬時目標運動から前記所定の部位の姿勢を該瞬時目標運動における姿勢と同一に維持しつつ、該所定の部位の位置を仮補正してなる第２の仮補正瞬時目標運動を決定する第２仮補正運動決定手段と、前記第１の仮補正瞬時目標運動と第２の仮補正瞬時目標運動とを基に、前記瞬時目標運動における前記所定の部位の位置および姿勢の真の補正を実行してなる補正後瞬時目標運動を決定する目標運動補正手段とを備える。そして、前記移動ロボットの全体または一部を、イナーシャをもつ剛体と質点とのうちの少なくともいずれか一方を要素として、複数の要素からなるモデルで表現し、前記瞬時歩容発生手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第１の配置とし、前記第１仮補正運動決定手段が決定した第１の仮補正瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第２の配置とし、前記第２仮補正運動決定手段が決定した第２の仮補正瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第３の配置としたとき、前記第１仮補正運動決定手段は、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記第１の仮補正瞬時目標運動を決定し、前記第２仮補正運動決定手段は、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が前記所定の点まわりに発生するモーメント成分がほぼ前記所定の値になるように前記第２の仮補正瞬時目標運動を決定し、前記目標運動補正手段は、前記第１の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における前記所定の部位の瞬時目標姿勢として決定するとともに、前記第１の仮補正瞬時目標運動における前記所定の部位の位置に前記所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の位置に所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における前記所定の部位の瞬時目標位置として決定し、前記第１仮補正運動決定手段と前記第２仮補正運動決定手段とがそれぞれ前記第１の仮補正瞬時目標運動と前記第２の仮補正瞬時目標運動とを決定するために用いる前記所定の値は、前記瞬時目標床反力のモーメント成分に対する動力学的精度が、前記瞬時目標運動よりも前記第１の仮補正瞬時目標運動及び前記第２の方が高まるようにあらかじめ設定された値であることを特徴とする。

なお、この第１発明を含めて以降に説明する本発明では、前記モデルの要素の「配置」は、該要素としての質点の「位置」と、該要素としての、イナーシャをもつ剛体（リンク）の「姿勢」（傾斜角）とを総称的に表現する呼称である。一般的には、剛体は質量とイナーシャとをもつが、便宜上、本発明では、その質量およびイナーシャをもつ剛体は、前記質量を有して該剛体の重心に位置する質点と、質量が０で前記イナーシャをもつ剛体に分解しておくものとする。このようにしても一般性は失われない。また、「第１の配置」、「第２の配置」、「第３の配置」というときには、それは、前記モデルに含まれる全ての要素の配置の組を意味する。

かかる第１発明によれば、前記第１の幾何学的拘束条件と、第２の幾何学的拘束条件とを適切に設定して、また、モデルを構成する要素を適切に設定しておくことで、前記第２の配置と、第１の配置との差（第２の配置における各要素の配置と第１の配置における各要素の配置との差）を、前記第１の仮補正瞬時目標運動（前記第１仮補正瞬時目標運動が決定したロボットの各部位の位置と姿勢とのうちの少なくともいずれか一方の瞬時目標値）と前記瞬時歩容発生手段が発生した瞬時目標床反力（ロボットに作用する床反力の並進力とモーメントとのうちの少なくともいずれか一方の瞬時目標値）との間の動力学的誤差の程度（度合い）に対応させることが可能となる。同様に、前記第３の配置と、第１の配置との差（第３の配置における各要素の配置と第１の配置における各要素の配置との差）を、前記第２の仮補正瞬時目標運動（前記第２仮補正瞬時目標運動が決定したロボットの各部位の位置と姿勢とのうちの少なくともいずれか一方の瞬時目標値）と前記瞬時歩容発生手段が発生した瞬時目標床反力との間の動力学的誤差の程度（度合い）に対応させることが可能となる。補足すると、これらの対応関係には、一般には定常的なオフセットが存在する。

そして、第１発明では、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値（ある一定のオフセット値）になるように、前記所定の部位の位置および姿勢を前記瞬時歩容発生手段が発生した瞬時目標運動から仮補正したものとしての前記第１の仮補正瞬時目標運動が決定される。これにより、第１の仮補正瞬時目標運動は、前記瞬時目標床反力の並進力成分およびモーメント成分の両者に対して動力学的精度が高まるものとなる。但し、この第１の仮補正瞬時目標運動における前記所定の部位の姿勢は、頻繁に変動する恐れがある。

補足すると、前記瞬時目標床反力の並進力成分が明示的に設定されない場合には、前記瞬時歩容発生手段が歩容発生のために用いる動力学モデルが発生する床反力の並進力成分を前記瞬時目標床反力の並進力成分とみなす。

一方、前記瞬時歩容発生手段が発生した瞬時目標運動から前記所定の部位の位置を補正したものとしての前記第２の仮補正瞬時目標運動は、前記所定の部位の姿勢を前記瞬時歩容発生手段が発生した瞬時目標運動における該所定の部位の姿勢と同一にした状態（すなわち該所定の部位に対応するモデルの要素の配置を、該瞬時目標運動における所定の部位の姿勢に合致させた状態）で、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が前記所定の点まわりに発生するモーメント成分がほぼ前記所定の値になるように決定される。このため、第２の仮補正瞬時目標運動は、前記瞬時目標床反力のモーメント成分に対して動力学的精度が高まるものとなると同時に、前記所定の部位の姿勢が瞬時歩容発生手段が発生した該所定の部位の姿勢に維持されることで、その姿勢の変動が第１の仮補正瞬時目標運動よりも制限（抑制）される。

そして、第１発明では、前記補正後瞬時目標運動における前記所定の部位の姿勢（瞬時目標姿勢）は、前記第１の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ2を乗じたものとの和して決定され、また、補正後瞬時目標運動における前記所定の部位の位置（瞬時目標位置）は、前記第１の仮補正瞬時目標運動における前記所定の部位の位置に前記所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の位置に所定の重みｗ2を乗じたものとの和として決定される。

この結果、重みｗ1，ｗ2を適切に設定しておくことで、前記所定の部位の瞬時目標姿勢の過剰な変動を抑えつつ、前記瞬時目標床反力との間の動力学的精度を瞬時歩容発生手段が発生した瞬時目標運動よりも良好に確保できる補正後瞬時目標運動を決定できることとなる。この場合、補正後瞬時目標運動の基となる前記第１の仮補正瞬時目標運動および第２の仮補正瞬時目標運動は、いずれも前記瞬時目標床反力のモーメント成分に対する動力学的精度が良好となるので、補正後瞬時目標運動と瞬時目標床反力との組からなる歩容は、ロボットの全体的な姿勢の安定性を良好に確保し得るものとなる。また、第１および第２仮補正瞬時目標運動は、前記モデルの要素の配置の時間的な変化（位置や姿勢の１階微分値や２階微分値）を用いることなく、該要素の配置に関する幾何学的な演算処理によって決定できる。さらに、補正後瞬時目標運動における所定の部位の位置姿勢は、単なる乗算、加算演算によって決定できる。

従って、第１発明によれば、動力学モデルを用いることなく（運動と力の関係を表す微分方程式や積分方程式を用いることなく）、瞬時目標歩容の床反力との間の動力学的精度の向上と、ロボットの上体などの所定の部位の姿勢の変動を極力少なくすることとの両立を図るように適切に補正することができ、ロボットの運動を安定に行うことを可能とする歩容を生成できる。

補足すると、第１発明では、結果的に上記の如く、補正後瞬時目標運動が決定されていればよく、前記第１の配置、第２の配置、第３の配置を実際に求めたり、前記各要素の慣性力の合力の並進力成分や、モーメント成分を実際に求めることは必ずしも必要ではない。

また、本発明の移動ロボットの歩容生成装置の第２発明は、移動ロボットの瞬時目標運動と瞬時目標床反力とからなる瞬時目標歩容を逐次発生する瞬時歩容発生手段を備えた歩容生成装置において、前記瞬時目標運動から前記移動ロボットの所定の部位の位置および姿勢を仮補正してなる仮補正瞬時目標運動を決定する仮補正運動決定手段と、前記瞬時目標運動における前記所定の部位の位置および姿勢の真の補正を実行してなる補正後瞬時目標運動を決定する目標運動補正手段とを備える。そして、前記移動ロボットの全体または一部を、イナーシャをもつ剛体と質点とのうちの少なくともいずれか一方を要素として、複数の要素からなるモデルで表現し、前記瞬時歩容発生手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第１の配置とし、前記仮補正運動決定手段が決定した仮補正瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第２の配置とし、前記目標運動補正手段が決定する補正後瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第３の配置としたとき、前記仮補正運動決定手段は、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記仮補正瞬時目標運動を決定し、前記目標運動補正手段は、前記仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ1を乗じたものと前記瞬時歩容発生手段が発生した前記瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における該所定の部位の瞬時目標姿勢として決定すると共に、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記補正後瞬時目標運動における前記所定の部位の瞬時目標位置を決定し、前記仮補正運動決定手段と前記目標運動補正手段とがそれぞれ前記仮補正瞬時目標運動と前記補正後瞬時目標運動とを決定するために用いる前記所定の値は、前記瞬時目標床反力のモーメント成分に対する動力学的精度が、前記瞬時目標運動よりも前記仮補正瞬時目標運動及び前記補正後瞬時目標運動の方が高まるようにあらかじめ設定された値であることを特徴とする。

かかる第２発明によれば、前記第１発明と同様、前記第１の幾何学的拘束条件と、第２の幾何学的拘束条件とを適切に設定して、また、モデルを構成する要素を適切に設定しておくことで、前記第２の配置と、第１の配置との差を、前記仮補正瞬時目標運動（前記仮補正瞬時目標運動が決定したロボットの各部位の位置と姿勢とのうちの少なくともいずれか一方の瞬時目標値）と前記瞬時歩容発生手段が発生した瞬時目標床反力（ロボットに作用する床反力の並進力とモーメントとのうちの少なくともいずれか一方の瞬時目標値）との間の動力学的誤差の程度（度合い）に対応させることが可能となる。また、前記第３の配置と、第１の配置との差を、前記補正後瞬時目標運動（前記目標運動補正手段が決定したロボットの各部位の位置と姿勢とのうちの少なくともいずれか一方の瞬時目標値）と前記瞬時歩容発生手段が発生した瞬時目標床反力との間の動力学的誤差の程度（度合い）に対応させることが可能となる。補足すると、これらの対応関係には、一般には前記第１発明と同様、定常的なオフセットが存在する。

そして、第２発明では、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値（ある一定のオフセット値）になるように、前記所定の部位の位置および姿勢を前記瞬時歩容発生手段が発生した瞬時目標運動から仮補正したものとしての前記仮補正瞬時目標運動が決定される。この仮補正瞬時目標運動は、第１発明における第１の仮補正瞬時目標運動と同等のものであり、前記瞬時目標床反力の並進力成分およびモーメント成分の両者に対して動力学的精度が高まるものとなる。但し、この仮補正瞬時目標運動における前記所定の部位の姿勢は、頻繁に変動する恐れがある。

一方、前記瞬時歩容発生手段が発生した瞬時目標運動の真の補正後の瞬時目標運動としての前記補正後瞬時目標運動は、前記所定の部位の姿勢を前記仮補正瞬時目標運動における該所定の部位の姿勢に前記所定の重みｗ1を乗じた姿勢と前記瞬時歩容発生手段が発生した瞬時目標運動における該所定の部位の姿勢に前記所定の重みｗ2を乗じた姿勢との和と同一にした状態（すなわち該所定の部位に対応するモデルの要素の配置を、該仮補正瞬時目標運動における所定の部位の姿勢に重みｗ1を乗じた姿勢と前記瞬時歩容発生手段が発生した瞬時目標運動における所定の部位の姿勢に重みｗ2を乗じた姿勢との和に合致させた状態）で、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が前記所定の点まわりに発生するモーメント成分がほぼ前記所定の値になるように決定される。ここで、前記仮補正瞬時目標運動から、前記所定の部位の姿勢のみを、該仮補正瞬時目標運動における所定の部位の姿勢に重みｗ1を乗じた姿勢と前記瞬時歩容発生手段が発生した瞬時目標運動における所定の部位の姿勢に所定の重みｗ2を乗じた姿勢との和に変更してなる瞬時運動に対応して、前記第２の幾何学的条件に従って決定される前記モデルの要素の配置を第４の配置としたとき、その第４の配置の全体重心の位置と、前記第２の配置の全体重心の位置との差は一般には比較的小さい。従って、上記の如く補正後瞬時目標運動を決定することによって、その補正後瞬時目標運動は、前記瞬時目標床反力のモーメント成分に対して動力学的精度が高まるものとなると同時に、瞬時目標床反力の並進力成分に対しても良好な動力学的精度を確保できるものとなる。さらに前記所定の部位の姿勢が仮補正瞬時目標運動における該所定の部位の姿勢に重みｗ1を乗じた姿勢と瞬時歩容発生手段が発生した瞬時目標運動における所定の部位の姿勢に重みｗ2を乗じた姿勢との和に規制されることで、重みｗ1，ｗ2を適切に設定しておくことで、その姿勢の変動が仮補正瞬時目標運動よりも制限（抑制）される。

この結果、前記所定の部位の瞬時目標姿勢の過剰な変動を抑えつつ、前記瞬時目標床反力との間の動力学的精度を瞬時歩容発生手段が発生した瞬時目標運動よりも良好に確保できる補正後瞬時目標運動を決定できることとなる。この場合、補正後瞬時目標運動は、前記瞬時目標床反力のモーメント成分のみならず、並進力成分に対しても動力学的精度が良好となるので、補正後瞬時目標運動と瞬時目標床反力との組からなる歩容は、移動ロボットの全体的な姿勢の安定性をより良好に確保し得るものとなる。また、仮補正瞬時目標運動および補正後瞬時目標運動は、前記モデルの要素の配置の時間的な変化（位置や姿勢の１階微分値や２階微分値）を用いることなく、該要素の配置に関する幾何学的な演算処理によって決定できる。

従って、第２発明によれば、動力学モデルを用いることなく（運動と力の関係を表す微分方程式や積分方程式を用いることなく）、瞬時目標歩容の床反力との間の動力学的精度の向上と、移動ロボットの上体などの所定の部位の姿勢の変動を極力少なくすることとの両立を図るように適切に補正することができ、移動ロボットの運動を安定に行うことを可能とする歩容を生成できる。

補足すると、第２発明では、結果的に上記の如く、補正後瞬時目標運動が決定されていればよく、前記第１の配置、第２の配置、第３の配置を実際に求めたり、前記各要素の慣性力の合力の並進力成分や、モーメント成分を実際に求めることは必ずしも必要ではない。

前記第１発明では、前記所定の重みｗ1，ｗ2は、基本的には０から１の範囲内の大きさ（例えば0.3、0.7など）の重みに設定しておけばよいが、その大きさが一定であると、移動ロボットの運動形態や路面状態によっては、前記補正後瞬時目標運動におけるロボットの全体重心の並進慣性力の水平成分が、移動ロボットと床との間の摩擦力によって発生させ得る床反力水平成分と釣り合わなく恐れがある。そして、このことは、前記第２発明でも同様である。

そこで、前記第１発明および第２発明では、前記所定の重みｗ1および所定の重みｗ2は、いずれも、その大きさが０から１までの範囲内の大きさであり、少なくとも前記所定の重みｗ1を、前記目標歩容に応じて前記移動ロボットの動作を行わせようとする路面状態および／または該目標歩容に応じた移動ロボットの運動形態に応じて可変的に決定する手段を備えることが好ましい（第３発明）。そして、この場合、前記所定の重みｗ1の大きさと前記所定の重みｗ2の大きさとの和は１であることが好ましい（第４発明）。

前記第３発明では、上記のように重みｗ1，ｗ2を可変的に設定することによって、移動ロボットの所定の部位の姿勢を、路面状態や移動ロボットの運動形態に適した姿勢に制限しながら、動力学的精度も良好に確保し得る補正後瞬時目標運動を決定できる。さらに、第４発明では、重みｗ1の大きさと重みｗ2の大きさとの和を１にすることによって、前記補正後瞬時目標運動における所定の部位の位置は、前記第１の仮補正瞬時目標運動における所定の部位の位置と、第２の仮補正瞬時目標運動における所定部位の位置の重み付き平均値となるので、補正後瞬時目標運動と瞬時目標床反力のモーメント成分と間の動力学的精度と、補正後瞬時目標運動と瞬時目標床反力の並進力成分との間の動力学的精度とのバランスを好適に保ちながら補正後瞬時目標運動を決定できる。

また、第４発明では、第３発明と同様に、移動ロボットの所定の部位の姿勢を、路面状態や移動ロボットの運動形態に適した姿勢に制限しながら、動力学的精度も良好に確保し得る補正後瞬時目標運動を決定できる。

なお、移動ロボットが例えば２足移動ロボットであるときには、前記運動形態としては例えば移動ロボットの歩行、走行などの運動形態が挙げられる。また、前記路面状態は、路面（床面）の摩擦係数などの摩擦状態を含むことが好ましい。

前記第１〜第４発明では、前記所定の重みｗ1は、単なる実数値であってもよいが、これを乗じる前記所定の部位の姿勢に対して周波数特性をもつ重みであってもよい（第５発明）。なお、重みｗ1が周波数特性をもつということは、所定の部位の姿勢の時系列を周波数領域で表現したときの各周波数成分に対する重みｗ1のゲイン（スペクトル）が周波数に応じて変化することを意味する。このような重みｗ1は、一般に複素数を用いた伝達関数で表現され、フィルタとして機能するものとなる。

このように重みｗ1に周波数特性をもたせることで、前記第１発明における第１の仮補正瞬時目標運動における前記所定の部位の姿勢、あるいは前記第２発明における仮補正瞬時目標運動における前記所定の部位の姿勢から所要の周波数成分を除去して補正後瞬時目標運動における前記所定の部位の姿勢を決定できる。この場合、重みｗ1の周波数特性を例えばローカット特性（低周波成分を遮断する特性）にすれば、前記第１の仮補正瞬時目標運動（第１発明並びにこれに従属する第３および第４発明）あるいは前記仮補正瞬時目標運動（第２発明ならびにこれに従属する第３および第４発明）における前記所定の部位の姿勢に、前記モデルの誤差などに起因して定常的なオフセット（誤差分）が発生するような場合に、それを除去することができる。また、重みｗ1の周波数特性を例えばハイカット特性（高周波成分を遮断する特性）にすれば、前記第１の仮補正瞬時目標運動（第１発明並びにこれに従属する第３および第４発明）あるいは前記仮補正瞬時目標運動（第２発明ならびにこれに従属する第３および第４発明）における前記所定の部位の姿勢に細かい振動が発生するような場合に、それを除去することができる。

ところで、前記第１〜第５発明において、前記モーメント成分のうち、前記モデルのイナーシャをもつ要素（剛体）の配置の差（姿勢の差）に起因する成分は、その要素の姿勢の差（傾斜角の差）とその要素のイナーシャの値との積に相当するものとなる。また、前記モデルの質量をもつ要素の配置の差（位置の差）に起因する成分は、その位置の差と該要素の前記所定の点からの距離とをそれぞれベクトルで表したときのそれらのベクトルの積（外積）に該要素の質量を乗算したものに相当する。そして、この場合、質量をもつ要素の配置の差（位置の差）に起因する成分は、その位置の差に係わる２つの位置の一方の位置と前記所定の点とを結ぶ線分と、当該２つの位置の他方の位置と前記所定の点とを結ぶ線分とがなす角度に応じたもの（より詳しくは該角度に応じて単調に増加もしくは減少するもの）となる。

そこで、本発明の第６発明は、前記第１〜第５発明において、前記第２の配置と第１の配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの質量をもつ各要素の、前記第１の配置における位置Ａと前記第２の配置における位置Ｂの差に起因する成分は、前記所定の点と前記位置Ａとを結ぶ線分と、前記所定の点と前記位置Ｂとを結ぶ線分とがなす角度から、該角度に関する実質的に単調な関数を用いて算出され、前記第３の配置と第１の配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの質量をもつ各要素の、前記第１の配置における位置Ａと前記第３の配置における位置Ｃの差に起因する成分は、前記所定の点と前記位置Ａとを結ぶ線分と、前記所定の点と前記位置Ｃとを結ぶ線分とがなす角度から前記単調な関数を用いて算出されることを特徴とする。

このようにすることによって、前記モーメント成分を算出するとき、ベクトル演算が不要になって、その算出が容易になる。

前記第１〜第６発明は、前記瞬時歩容発生手段が発生する瞬時目標運動が、前記移動ロボットの運動と床反力との関係を表す動力学モデルであって、少なくとも該移動ロボットの１つ以上の特定部位の特定の運動成分によって発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて決定されており、前記モデルが、前記特定部位のうちの少なくとも１つの部位に対応する要素を含む場合に好適である（第７発明）。

すなわち、前記瞬時目標運動が、移動ロボットの１つ以上の特定部位の特定の運動成分（ある方向の並進運動、回転運動など）によって発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて決定されているときには、その特定部位が比較的大きな慣性力を発生するような目標歩容を生成するときに、瞬時歩容発生手段が発生する瞬時目標運動と瞬時目標床反力との間の動力学的精度が低下しやすい。この場合、第７発明では、その特定部位のうちの少なくとも１つの部位に対応する要素を前記モデルに含ませているので、前記補正後瞬時目標運動と瞬時目標床反力との間の動力学的精度を的確に高めることができる。

また、前記第１〜６発明において、前記瞬時歩容発生手段が発生する瞬時目標運動は、前記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標床反力または目標ＺＭＰを満足するように決定されており、前記瞬時目標運動から、前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の時間的変化によって発生する各要素の慣性力の合力に釣り合う床反力と、前記瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の時間的変化によって発生する各要素の慣性力の合力に釣り合う床反力との差に所定の定常オフセットを加えたものが、前記瞬時目標運動によって前記動力学モデルで発生する床反力の誤差に略一致するように前記第１および第２の幾何学的拘束条件が設定されていることが好適である（第８発明）。

この第８発明によれば、前記瞬時歩容発生手段が発生する瞬時目標運動と瞬時目標床反力との間の動力学的誤差が、該瞬時目標運動から前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置と、該瞬時目標運動から前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置との差（モデルの質点の位置の差、あるいはモデルの剛体の姿勢の差）に対応するものとなる。従って、前記したように第１発明に係る第１の仮補正瞬時目標運動および第２の仮補正瞬時目標運動を決定し、さらに、これらの仮補正瞬時目標運動から補正後瞬時目標運動を決定することで、前記瞬時歩容発生手段が発生した瞬時目標床反力との間の動力学的精度を、該瞬時歩容発生手段が発生した瞬時目標運動よりも高め得る補正後瞬時目標運動を適切に決定できると同時に、前記所定の部位の姿勢の変動を抑制できる。同様に、前記したように第２発明に係る仮補正瞬時目標運動を決定し、さらにこの仮補正瞬時目標運動における前記所定の部位の姿勢と同じ姿勢とした補正後瞬時目標運動を決定することで、前記瞬時歩容発生手段が発生した瞬時目標床反力との間の動力学的精度を、該瞬時歩容発生手段が発生した瞬時目標運動よりも高め得る補正後瞬時目標運動を適切に決定できると共に、前記所定の部位の姿勢の変動を抑制できる。

また、前記第１〜第６発明において、前記瞬時歩容発生手段が発生する瞬時目標運動は、前記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標床反力または目標ＺＭＰを満足するように決定されており、前記瞬時目標運動から、前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の全体重心と、前記瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の全体重心との差に該要素の総質量を乗じたものが、前記瞬時目標運動における前記動力学モデルの全体重心の誤差に該動力学モデルの総質量を乗じたものに略一致するように前記第１および第２の幾何学的拘束条件を設定するようにしてもよい（第９発明）。

これによって、前記第１発明に係る第１仮補正瞬時目標運動と前記瞬時目標床反力との間の動力学的精度や、前記第２仮補正瞬時目標運動と前記瞬時目標床反力との間の動力学的精度を低下させる要因の１つである前記動力学モデルの全体重心位置の誤差の影響を打ち消すことができる。同様に、前記第２発明に係る仮補正瞬時目標運動と前記瞬時目標床反力との間の動力学的精度や、前記補正後瞬時目標運動と前記瞬時目標床反力との間の動力学的精度についても、前記動力学モデルの全体重心位置の誤差の影響を打ち消すことができる。その結果、第１または第２発明に係る前記補正後瞬時目標運動と瞬時目標床反力との間の動力学的精度を良好に高めることができる。

なお、前記第７〜第９発明は、その２つ以上を組み合わせてもよいことはもちろんである。

また、前記第１〜第９発明において、前記移動ロボットが、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えるロボットであるときには、前記第１の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と、該可動体の前記上体との連結部近傍の所定の点を結ぶ線分に平行な直線上に前記モデルの要素のうちのいずれかが存在するという条件を含むことが好ましい（第１０発明）。あるいは、前記第１の幾何学的拘束条件は、前記モデル上での前記上体と各可動体とが所定の一定姿勢状態に保持されるという条件を含むことが好ましい（第１１発明）。そして、この第１１発明では、前記所定の一定姿勢は、前記移動ロボットの上体と複数の可動体とをほぼ鉛直方向に向けた姿勢であることが好ましい（第１２発明）。

また、第１〜第１２発明において、前記第２の幾何学的拘束条件は、前記移動ロボットの任意の瞬時目標運動から、その条件に従って決定される前記モデルの各要素の配置が、前記瞬時目標運動に従う前記ロボットにおける該要素に対応する部位の配置に略一致するように設定されていることが好ましい（第１３発明）。

このように第１および第２の幾何学的拘束条件を定めることにより、前記瞬時目標運動から、前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置と、該瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置との差を、該瞬時目標運動と前記瞬時歩容発生手段が発生した瞬時目標床反力との間の動力学的誤差に好適に対応させることが可能となる。

また、前記第１〜第６発明において、前記移動ロボットが、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えると共に、各可動体の上体との連結部と該可動体の先端部との間の中間部とに屈曲可能な関節を有し、さらに前記瞬時歩容発生手段が発生する瞬時目標運動が、前記ロボットの運動と床反力との関係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可動体の中間部もしくはその近傍で発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて決定されているときには、前記モデルは少なくとも前記各可動体の中間部もしくはその近傍部位に対応させた質点を要素として含むモデルであることが好ましい（第１４発明）。

すなわち、前記瞬時目標運動が、各可動体の屈伸運動に起因して該可動体の中間部もしくはその近傍で発生する慣性力がほぼ０であるとして（すなわち、該慣性力を無視して）構築された動力学モデルを用いて決定されている場合には、各可動体の屈伸運動が比較的すばやく行われるような目標歩容を生成するときに、前記瞬時歩容発生手段が発生する瞬時目標運動と瞬時目標床反力との間の動力学的な精度が低下しやすい。そこで、第１５発明の如く、前記モデルにおいて、各可動体の中間部もしくはその近傍部位に対応させた質点を要素として含ませることによって、前記第１発明に係る第１および第２の仮補正瞬時目標運動、あるいは、前記第２発明に係る仮補正瞬時目標運動および補正後瞬時目標運動を前記第１または第２発明に関して説明した如く決定するに当たって、各可動体の中間部の関節の屈曲動作による該可動体の屈伸運動に伴う慣性力の影響を補償するようにして、それらの瞬時目標運動を決定できることとなる。このため、それらの決定される瞬時目標運動と瞬時歩容発生手段が決定する瞬時目標床反力との間の動力学的精度を高めることができる。その結果、前記第１または第２発明に係る補正後瞬時目標運動と前記瞬時目標床反力とから構成される瞬時歩容は、前記所定の部位の姿勢の変動を抑えながら、瞬時歩容発生手段が発生する瞬時歩容よりも動力学的精度の高いものとすることができる。

この第１４発明において、前記第１幾何学拘束条件は、例えば前記第１０発明または第１１発明と同様に設定すればよく、また、第２幾何学的拘束条件は、前記第１３発明の如く設定すればよい。そして、特に、第１および第２幾何学的拘束条件を、それぞれ、第１０発明、第１３発明の如く設定することが好適である。

すなわち、前記第１の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と、該可動体の前記上体との連結部近傍の所定の点を結ぶ線分に、前記モデルの要素のうちの、該可動体の中間部もしくはその近傍部位に対応させた質点が存在するという条件を含み、前記第２の幾何学的拘束条件は、前記移動ロボットの任意の瞬時目標運動から、その条件に従って決定される前記モデルの各要素の配置が、前記瞬時目標運動に従う前記ロボットにおける該要素に対応する部位の配置に略一致するように設定されていることが好適である（第１５発明）。

このようにしたとき、前記瞬時歩容発生手段が発生する瞬時目標運動から前記第２の幾何学的拘束条件に従って前記モデルの各要素の配置を決定したとき、その配置における各可動体の中間部もしくはその近傍部位に対応する前記モデルの質点（以下、ここでは可動体中間質点という）と、前記第１の配置における可動体中間質点（これは前記線分上に在る）との位置の差が、各可動体の中間部の関節の屈曲動作に伴う慣性力に相当するものとなる。そして、その慣性力が、瞬時歩容発生手段が発生する瞬時目標床反力の誤差分に対応するものとなる。このため、前記第１発明に係る第１および第２の仮補正瞬時目標運動、あるいは、前記第２発明に係る仮補正瞬時目標運動および補正後瞬時目標運動を前記第１または第２発明に関して説明した如く決定することで、各可動体の中間部の関節の屈曲動作による該可動体の屈伸運動に伴う慣性力の影響を補償して、それらの瞬時目標運動と瞬時目標床反力との間の動力学的精度を高めることができる。ひいては、第１発明あるいは第２発明に係る補正後瞬時目標運動と瞬時目標床反力との間の動力学的精度を効果的に高めつつ、前記所定の部位の姿勢の変動を抑えることができる。

以下、添付図面を参照して本発明の実施形態を説明する。尚、本明細書の実施形態では、移動ロボットとしては２足移動ロボットを例にとる。

図１は、本発明の実施形態を適用する２足移動ロボットの全体的構成の概略を示す概略図である。

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

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

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

なお、本明細書においては、ロボット１の「リンク」はロボット１の剛体とみなせる部位の意味で使用する。例えば上体３も１つのリンク（剛体）であり、その意味で上体３を上体リンクと言うこともある。

上体３の上部の両側部には左右一対の腕体５，５が取り付けられると共に、上体３の上端部には頭部４が配置される。各腕体５は、３つの関節３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）から構成された肩関節と、関節３６Ｒ（Ｌ）から構成された肘関節と、関節３８Ｒ（Ｌ）から構成された手首関節と、この手首関節に連結された手先部４０Ｒ（Ｌ）とを備えている。肩関節と肘関節との間、および肘関節と手首関節との間はそれぞれリンクで構成されている。

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

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

さらに、図１では図示を省略するが、ロボット１の外部には、ロボット１を操縦するためのジョイスティック（操作器）７３（図３参照）が設けられ、そのジョイスティック７３を操作することで、直進移動しているロボット１を旋回させる、ロボット１の移動方向を指定する、ロボット１の歩行、走行などの運動形態および床面の摩擦状態（路面状態）を規定する動作のモードを指定するなど、ロボット１の歩容に対する要求もしくは制約を必要に応じて制御ユニット６０に入力できるように構成されている。ジョイスティック７３は有線もしくは無線により制御ユニット６０との通信が可能とされている。

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

図２に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示している。各脚体２が床反力を受けると、コンプライアンス機構７２のばね機構７０と足底弾性体７１とがたわみ、足平２２Ｒ（Ｌ）は図中に点線で例示したような位置姿勢に移る。このコンプラインアス機構７２の構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。その詳細は、例えば本出願人が先に提案した特開平５−３０５５８４号公報に詳細に説明されているので、本明細書でのさらなる説明は省略する。

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

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

図４は、本明細書の実施形態におけるロボット１の制御ユニット６０の主な機能的構成を示すブロック図である。この図４中の「実ロボット」の部分以外の部分が制御ユニット６０が実行する処理機能（主として第１の演算装置９０及び第２の演算装置９２の機能）によって構成されるものである。その処理機能は、制御ユニット６０に実装されたプログラム等によって実現されている。尚、以下の説明では、ロボット１の各部（脚体２、腕体５など）の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。

以下説明すると、制御ユニット６０は、後述の如く目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置１００を備えている。この歩容生成装置１００は、その機能によって本発明の実施形態を実現するものである。この歩容生成装置１００が出力する目標歩容は、補正目標上体姿勢軌道（上体３の目標姿勢の軌道）、補正目標上体位置軌道（上体３の目標位置の軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体の目標姿勢の軌道）、目標ＺＭＰ（目標全床反力中心点）軌道、目標ＺＭＰまわりの補正目標床反力モーメント軌道および目標全床反力軌道から構成される。尚、脚体２や腕体５以外に、上体３に対して可動な部位（頭部など）を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。

ここで、本明細書での歩容に関する基本的な用語の定義などについて説明しておく。歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、「軌道」の代わりに「パターン」と称することもある。また、「姿勢」は空間的な向きを意味する。例えば上体姿勢はＺ軸（鉛直軸）に対するロール方向（Ｘ軸まわり）の上体３の傾斜角（姿勢角）とピッチ方向（Ｙ軸まわり）の上体３の傾斜角（姿勢角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。なお、腕体５に関する目標腕姿勢は、本明細書の実施形態では上体３に対する相対姿勢で表される。

上体位置は、上体３のあらかじめ定めた代表点（上体３に対して任意に固定設定したローカル座標系でのある固定点）の位置を意味する。同様に、足平位置は、各足平２２のあらかじめ定めた代表点（各足平２２に対して任意に固定設定したローカル座標系での固定点）の位置を意味する。例えば各足平２２の代表点は、各足平２２の底面上（より具体的には各脚体２の足首関節の中心から各足平２２の底面への垂線が該底面と交わる点等）に設定される。

上体３に関する前記補正目標上体姿勢および補正目標上体位置は、ある基本となる目標上体姿勢（仮目標上体姿勢）および目標上体位置（仮目標上体位置）を補正したものである。本明細書の実施形態では、基本となる目標上体位置姿勢は、後述の変位次元補正上体位置姿勢が相当する。

なお、以降の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。

歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット１の各部位の位置姿勢に関する構成要素を総称的に「運動」という。また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点（全床反力の中心点の目標位置）を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Ｚ軸）まわりのモーメント成分）を除いて零になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（Ｘ軸及びＹ軸）まわりのモーメント）は零になる。

なお、動力学的平衡条件を満足する歩容では、ロボット１の目標運動軌道から算出されるＺＭＰ（目標運動軌道から算出される慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直成分を除いて零になる点）と目標床反力中心点とは一致することから、目標床反力中心点軌道の代わりに目標ＺＭＰ軌道を与えると言っても同じことである。

ここで、ロボット１の歩行を行う場合には、例えば本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法によってロボット１の上体３の鉛直位置（上体高さ）が決定されると、並進床反力鉛直成分は従属的に決定される。さらに、目標歩容の運動による慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になるようにロボット１の上体水平位置軌道（あるいは全体重心の位置軌道）を決定することで、並進床反力水平成分も従属的に決定される。このため、ロボット１の歩行を行う場合には、目標歩容の床反力に関して明示的に設定すべき物理量としては、目標ＺＭＰだけでもよい。

一方、床反力が０もしくはほぼ０になるような時期を伴う歩容でのロボット１の移動、例えばロボット１の走行を行う場合には、並進床反力鉛直成分もロボット１の動作制御上重要である。このため、並進床反力鉛直成分の目標軌道を明示的に設定した上で、ロボット１の目標上体鉛直位置等の軌道を決定することが望ましい。また、ロボット１の歩行においても、摩擦係数が低い床面上（低ミュー路上）でロボット１を移動させるような場合には、並進床反力鉛直成分（より厳密には並進床反力の床面に垂直な成分）が摩擦力に影響を及ぼすことから、ロボット１のスリップなどを防止する上で、並進床反力鉛直成分の目標軌道を明示的に設定することが望ましい。さらに、本発明の実施形態では、最終的に歩容生成装置１００が出力する目標歩容では、目標ＺＭＰまわりに補正目標床反力モーメント（水平成分が０とは限らないモーメント）を発生させる。

このようなことから、本明細書の実施形態では、歩容生成装置１００が出力する目標歩容の床反力に関する構成要素として、目標ＺＭＰ軌道のほか、目標ＺＭＰまわりの補正目標床反力モーメントと、目標並進床反力鉛直成分とを含ませている。

そして、本明細書では、歩容生成装置１００が出力する目標歩容は、広義には、「１歩ないしは複数歩の期間の目標運動軌道と目標床反力軌道との組」の意味で使用され、狭義には、「１歩の期間の目標運動軌道と、目標ＺＭＰ、補正目標床反力モーメント及び目標並進床反力鉛直成分を含む目標床反力軌道との組」の意味で使用される。

但し、本明細書の実施形態においては、最終的な目標歩容（歩容生成装置１００が出力する目標歩容）を決定するまでの過程で作成する目標歩容（仮目標歩容）では、目標ＺＭＰまわりの目標床反力モーメントの水平成分は、本来の目標ＺＭＰの定義どおりに０とされる。従って、最終的に決定する目標歩容以外の仮目標歩容（後述の単純化モデル歩容や、第１仮補正歩容、第２仮補正歩容、変位次元補正歩容）では、上記狭義の目標歩容から、補正目標床反力モーメントを除いたものが目標歩容の意味で使用される。補足すると、本明細書の実施形態では、最終的な目標歩容（歩容生成装置１００が出力する目標歩容）を決定するまでの過程で作成する目標歩容（仮目標歩容）が本発明に密接に関連するものとなっている。このため、以降の説明で現れる目標歩容の大部分は、前記狭義の目標歩容から、補正目標床反力モーメントを除いたもの（目標ＺＭＰを満足する歩容）の意味で使用される。

なお、以降の説明では、「床反力鉛直成分」は「並進床反力鉛直成分」を意味するものとし、床反力のうちのモーメントの鉛直成分（鉛直軸回り成分）は、「モーメント」という用語を用いて「床反力鉛直成分」と区別をする。同様に、「床反力水平成分」は「並進床反力水平成分」を意味するものとする。

また、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してからもう一方の脚体２が着地するまでの意味で使用する。

また、歩容における両脚支持期とは、ロボット１がその自重を両脚体２，２で支持する期間、片脚支持期とはいずれか一方のみの脚体２でロボット１の自重を支持する期間、空中期とは両脚体２，２が床から離れている（空中に浮いている）期間を言う。片脚支持期においてロボット１の自重を支持しない側の脚体２を遊脚と呼ぶ。なお、片脚支持期と空中期とが交互に繰り返されるロボット１の走行歩容では両脚支持期は無い。この場合、空中期では両脚２，２とも、ロボット１の自重を支持しないこととなるが、便宜上、該空中期の直前の片脚支持期において遊脚であった脚体２、支持脚であった脚体２をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。

また、目標歩容の軌道は、グローバル座標系（床に固定された座標系）で記述される。グローバル座標系としては、例えば支持脚足平２２の着地位置姿勢に対応して定まる支持脚座標系が用いられる。この支持脚座標系は、例えば支持脚足平２２の底面のほぼ全面を床に接地した状態で、その足平２２が連結された足首関節の中心から床面に延ばした垂線が床と交わる点を原点とし、その原点を通る水平面に支持脚足平２２を投影したときの該足平２２の前後方向をＸ軸方向、左右方向をＹ軸方向とする座標系（Ｚ軸方向は鉛直方向）である。

図５は、歩容生成装置１００の詳細を示すブロック図である。この図５を参照して、歩容生成装置１００の処理のより具体的な概要を以下に説明する。

図示の如く、歩容生成装置１００は歩容パラメータ決定部１００ａを備える。歩容パラメータ決定部１００ａは、目標歩容を規定する歩容パラメータの値あるいは時系列テーブルを決定する。

本明細書の実施形態では、歩容パラメータ決定部１００ａが決定する歩容パラメータには、目標歩容のうちの、目標足平位置姿勢軌道、目標腕姿勢軌道、目標ＺＭＰ軌道、および目標床反力鉛直成分軌道をそれぞれ規定するパラメータが含まれる。

ここで、歩容生成装置１００が目標歩容を生成するとき、遊脚足平２２の着地予定位置姿勢、着地予定時刻、あるいは歩幅、移動速度等の歩容生成用の基本的な要求値（要求パラメータ）が、前記ジョイスティック７３、もしくは図示しない行動計画部（ロボット１の行動計画を作成する装置）などの装置から歩容生成装置１００に与えられる。あるいは、上記要求パラメータをあらかじめ記憶保持した記憶媒体から該要求パラメータを歩容生成装置１００が読み込む。そして、歩容生成装置１００の歩容パラメータ決定部１００ａは、その要求パラメータに応じて歩容パラメータを決定する。

また、本明細書の実施形態では、歩容パラメータ決定部１００ａが決定する歩容パラメータには、基準上体姿勢軌道、ＺＭＰ許容範囲、床反力水平成分許容範囲をそれぞれ規定するパラメータも含まれる。

ここで、前記基準上体姿勢軌道は、最終的に歩容生成装置１００が出力するものではないが、目標歩容を決定するときに参酌されるものである。この基準上体姿勢軌道は、ロボット１の上体姿勢に関して、前記ジョイスティック７３あるいは行動計画部から与えられ、もしくはあらかじめ定められた要求（上体姿勢を鉛直姿勢に保つなどの要求）にそのまま従って生成される上体姿勢軌道である。目標上体姿勢（以降、「基準」が付いていない「上体姿勢」は、目標上体姿勢を表す）は、基準上体姿勢に長期的に追従するか、または一致するように生成される。

また、前記ＺＭＰ許容範囲に関して補足すると、本明細書の実施形態では、目標歩容は、目標ＺＭＰのまわりに補正目標床反力モーメント（これは一般には０ではない）を発生するように修正される。したがって、目標ＺＭＰは、本来の定義（床反力モーメント水平成分が０である点という定義）とは異なる点となり、本来の定義を満足するＺＭＰ（以下、真のＺＭＰという）は、補正目標床反力モーメントを目標床反力鉛直成分で割った値だけ目標ＺＭＰからずれた位置に移る。

修正された歩容（歩容生成装置１００が最終的に出力する目標歩容）の真のＺＭＰは、少なくともＺＭＰ存在可能範囲（いわゆる支持多角形のこと。床と足平２２の底面との間に粘着力が作用しないと仮定した場合における床反力作用点（ＺＭＰ）の存在可能範囲）内になければならない。さらにロボット１の安定余裕を十分にとるためには、修正された歩容の真のＺＭＰは、ＺＭＰ存在可能範囲のなかの中心付近の範囲にあることが望ましい。そこで、本明細書の実施形態では修正された歩容の真のＺＭＰが存在できる許容範囲を設定する。この範囲をＺＭＰ許容範囲と呼ぶ。ＺＭＰ許容範囲は、ＺＭＰ存在可能範囲と一致あるいはＺＭＰ存在可能範囲内に包含されるように設定される。

なお、前記したように、目標ＺＭＰまわりの補正目標床反力モーメントを目標床反力鉛直成分で除算したものが、目標ＺＭＰに対する真のＺＭＰの位置のずれ量を表すので、目標ＺＭＰまわりの補正目標床反力モーメントを設定する代わりに、目標ＺＭＰに対する真のＺＭＰの位置のずれ量（補正目標床反力モーメントのＺＭＰ換算値）を設定してもよい。また、ＺＭＰ許容範囲は、その境界の位置と目標床反力鉛直成分とを用いて、補正目標床反力モーメントの許容範囲に変換することができ、その補正目標床反力モーメントの許容範囲をＺＭＰ許容範囲の代わりに設定するようにしてもよい。

また、前記床反力水平成分許容範囲は、ロボット１の足平２２の床との接地面に、足平２２が滑らないような大きさの摩擦力を発生させ得る床反力水平成分の許容範囲である。本明細書の実施形態では、少なくとも最終的に歩容生成装置１００が出力する目標歩容の運動（目標運動）は、それによって発生するロボット１の慣性力の水平成分に釣り合う床反力水平成分が床反力水平成分許容範囲内に収まるように生成される。

歩容パラメータ決定部１００ａで決定された歩容パラメータは目標瞬時値発生部１００ｂに入力される。目標瞬時値発生部１００ｂは入力された歩容パラメータに基づき、基準上体姿勢、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分等、目標歩容の一部の構成要素の瞬時値（前記制御ユニット６０の所定の制御処理周期毎の値）を逐次算出（発生）する。なお、図５では一部の目標瞬時値のみを代表的に記載している。

目標瞬時値発生部１００ｂで算出された目標瞬時値は、単純化モデル歩容生成部１００ｃに入力される。単純化モデル歩容生成部１００ｃは、入力された目標瞬時値を基に、ロボット１の運動と床反力との関係を近似表現する後述の動力学モデル（以下、単純化モデルという）を用いて目標上体位置姿勢（仮目標上体位置姿勢）の瞬時値を算出する。単純化モデル歩容生成部１００ｃは、単純化モデル上での動力学的平衡条件が満たされるように、すなわち単純化モデル上でのロボット１の目標運動によって発生する慣性力とロボット１に作用する重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になるように目標上体位置姿勢の瞬時値を算出する。補足すると、本明細書の実施形態では、目標床反力鉛直成分軌道も明示的に設定するので、目標上体位置姿勢の瞬時値は、目標運動によって発生する慣性力とロボット１に作用する重力との合力が目標ＺＭＰまわりに発生するモーメント水平成分が０になるほか、その合力の並進力鉛直成分（換言すれば、ロボット１の全体重心の鉛直方向の並進運動に伴う慣性力と重力との合力）が目標床反力鉛直成分に釣り合うように決定される。

これにより、目標上体位置姿勢を含む目標歩容（仮目標歩容）の瞬時値が逐次決定されていくこととなる。以降、単純化モデル歩容生成部１００ｃで求められた目標上体位置姿勢を構成要素とする目標歩容を単純化モデル歩容という。なお、単純化モデル歩容生成部１００ｃに入力される目標瞬時値は、目標瞬時値発生部１００ｂで算出されたすべての目標瞬時値である必要はない。単純化モデル歩容生成部１００ｃに必要な入力は、単純化モデルの構造、あるいはそれに適宜付加される制約条件に依存する。例えば図５では、目標足平位置姿勢を単純化モデル歩容生成部１００ｃに入力しているが、後述する第１実施形態における単純化モデルでは、目標足平位置姿勢を入力する必要はない。

なお、単純化モデル歩容生成部１００ｃは、目標瞬時値発生部１００ｂと合わせて、本発明における瞬時歩容発生手段を構成する。

単純化モデル歩容生成部１００ｃで算出された目標上体位置姿勢は、変位次元歩容補正部１００ｄに入力される。変位次元歩容補正部１００ｄには、目標上体位置姿勢のほか、目標足平位置姿勢の瞬時値、目標ＺＭＰの瞬時値も入力される。但し、変位次元歩容補正部１００ｄには、目標ＺＭＰを入力することは必須ではなく、より一般的には、後述する角運動量積に関する中心点が入力される。図５では、その中心点の一例として目標ＺＭＰを変位次元歩容補正部１００ｄに入力するようにしている。

この変位次元歩容補正部１００ｄは、入力された目標上体位置姿勢の瞬時値などを基に、後述の第１および第２変位次元補正用モデルを用いて、単純化モデル歩容生成部１００ｃで求めた目標上体位置姿勢を補正してなる変位次元補正上体位置姿勢の瞬時値を求める。詳細は後述するが、第１および第２変位次元補正用モデルは、一般的には、質点とイナーシャをもつリンクとのうちの少なくともいずれか一方を要素として構成されるモデル（幾何学モデル）であり、その各要素の配置（質点の位置、リンクの姿勢）が、ロボット１の瞬時運動における１つ以上の部位の位置姿勢に対応づけられる。この場合、これらの第１および第２変位次元補正用モデルは、共に同じ要素で構成される。但し、これらの第１および第２変位次元補正用モデルでは、その要素の配置に関して互いに異なる幾何学的拘束条件が定められており、ロボット１の任意の瞬時目標運動（ロボット１の各部位の位置姿勢の瞬時値）が与えられたとき、それに対応する各変位次元補正用モデルの要素の配置が各別の幾何学的拘束条件に基づいて定まるようになっている。従って、ある目標瞬時運動が与えられたとき、それに対応する各変位次元補正用モデルの要素の配置は一般には互いに異なる。そして、変位次元歩容補正部１００ｄは、これらの第１および第２変位次元補正用モデルにおける要素の配置の差（質点の位置の差、あるいはリンクの姿勢角の差）を基に、単純化モデル歩容の目標上体位置姿勢を補正して変位次元補正上体位置姿勢の瞬時値を逐次求める。

また、図５では図示を省略しているが、変位次元歩容補正部１００ｄには、前記ジョイスティック７３あるいはロボット１の行動計画部（図示しない）から、目標歩容において要求されるロボット１の運動形態（ロボット１の歩行、走行など）や、床面の摩擦状態（摩擦係数の大きさなど）を示すロボット１の動作モードも入力される。そして、変位次元歩容補正部１００ｄは、入力された動作モードに応じて変位次元補正上体位置姿勢の瞬時値を可変的に求める。本明細書の実施形態では、上記動作モードは、ロボット１の走行を行う走行モードと、比較的摩擦係数の小さな床面上（低ミュー路上）でロボット１の歩行を行う低摩擦床面歩行モードと、これらのモード以外の動作モードである通常モードとがある。通常モードには、比較的摩擦係数の大きな床面（通常的な床面）上でロボット１の歩行を行う動作モードが含まれる。

なお、変位次元歩容補正部１００ｄは、その機能によって、第１発明における第１仮補正運動決定手段、第２仮補正運動決定手段および目標運動補正手段を構成し、あるいは、第２発明における仮補正運動決定手段および目標運動補正手段を構成するものである。

変位次元歩容補正部１００ｄで求めた変位次元補正上体位置姿勢の瞬時値は、フルモデル補正部１００ｅに入力される。フルモデル補正部１００ｅには、変位次元補正上体位置姿勢の瞬時値のほか、目標瞬時値発生部１００ｂで算出された各目標瞬時値（基準上体位置姿勢の瞬時値を除く）が入力される。このフルモデル補正部１００ｅは、単純化モデルよりも動力学的精度の高い動力学モデルとしてのフルモデルを用いて変位次元上体位置姿勢を補正してなる補正目標上体位置姿勢を算出すると共に、目標ＺＭＰまわりの床反力モーメント水平成分の目標値である補正目標床反力モーメントを算出する。

フルモデル補正部１００ｅは、より一般的には、次のD1〜D3の条件を満足するように、E1あるいはE2の処理を実行する。すなわち、フルモデル補正部１００ｅは、
D1）単純化モデルを用いて生成した歩容（単純化モデル歩容）を変位次元補正用モデルを用いて修正してなる歩容（以降、変位次元補正歩容と呼ぶ）よりも高い精度で動力学的平衡条件を満足する。
D2）真のＺＭＰ（目標ＺＭＰのまわりに補正目標床反力モーメントを発生させることによって修正された本来の定義を満足するＺＭＰ）は、ＺＭＰ許容範囲（安定余裕が十分維持できる許容範囲）に存在する。
D3）床反力水平成分は床反力水平成分許容範囲内になる。
という条件を満足するように、
E1）前記変位次元補正歩容の上体位置姿勢を補正する。
あるいは
E2）前記変位次元補正歩容の上体位置姿勢を補正すると共に、目標ＺＭＰまわりの補正目標床反力モーメントを出力する（目標床反力を補正する）。

本明細書の実施形態では、D1〜D3の条件を満足するように、E2の処理が実行される。なお、本明細書の実施形態におけるフルモデル補正部１００ｅの処理は、例えば本願出願人が先に提案したＰＣＴ国際公開公報ＷＯ／０３／０５７４２７／Ａ１にて詳細に説明されているもの（具体的には、同公報の図１３のＳ０３８の処理）と同じである。従って、本明細書でのフルモデル補正部１００ｅの処理の詳細な説明は省略する。

図４に戻って、上述のように決定される補正目標上体位置姿勢、目標ＺＭＰまわりの補正目標床反力モーメント、目標足平位置姿勢の瞬時値を含む目標歩容の瞬時値は、複合コンプライアンス制御装置１０１（図４で破線で囲んだ部分）に入力される。この複合コンプライアンス制御装置１０１は、ロボット１のバランスを保ちつつ、目標歩容に追従するように関節アクチュエータ（電動モータ６４）を制御する。なお、複合コンプライアンス制御装置１０１のより具体的な処理は後述する。

以上が歩容生成装置１００の概要である。なお、以上説明した歩容生成装置１００の概要は、本明細書のいずれの実施形態においても同じである。

［第１実施形態］
次に、本発明の第１実施形態を具体的に説明する。まず、第１実施形態における前記単純化モデル（動力学モデル）、第１変位次元補正用モデル、第２変位次元補正用モデルについて説明する。なお、第１実施形態は、前記第１、３、４、７〜９、１１〜１４発明の一実施形態である。

図６は、第１実施形態における単純化モデルの構造を示している。図示の如く、この単純化モデルは、ロボット１の上体３に対応する１つの質点（上体質点）３ｍを備える１質点モデルである。なお、図６に示すロボット１は、側面から見たロボット１を模式化して示し、腕体５，５や頭部６の図示を省略している。この図６以降の図面（第１実施形態以外の実施形態の図面を含む）では、ロボット１を図示するときに、特に上体３と区別する必要がある場合を除いて、図６と同様に、腕体５，５や頭部６の記載を省略する。また、図６を含む以降の図面で記載されるＸ軸、Ｚ軸は、グローバル座標系を示している。

図６の単純化モデルの上体質点３ｍは、上体３の位置姿勢に対応して一義的に定まる点、すなわち上体３に任意に固定設定されたローカル座標系でのある固定点（ローカル座標系で上体３の代表点と所定の位置関係を有する点）に設定されている。また、上体質点３ｍの質量は、ロボット１の総質量ｍtotalと同一とされている。なお、上体質点３ｍは、上体３の代表点と一致していてもよいが、一般には異なる。

この単純化モデルの動力学は、上体質点３ｍと、これを目標ＺＭＰを支点として揺動自在に支持する可変長リンク３ｂとから構成される倒立振子の動力学により表現される。より具体的には、単純化モデルでのロボット１の運動と床反力との関係を表す運動方程式は、以下に示す式０１、式０２、式０３で表される。ただし、本明細書の理解を容易にするために、ここではサジタルプレーン（前後軸（Ｘ軸）と鉛直軸（Ｚ軸）を含む平面で、いわゆる矢状面）での運動方程式のみを記述し、ラテラルプレーン（左右軸（Ｙ軸）と鉛直軸（Ｚ軸）を含む平面で、いわゆる前額面）での運動方程式は省略する。

なお、本明細書では、任意の変数Ｘに対してd2X/dt2は変数Ｘの２階微分値を意味するものとする。また、図６の単純化モデルの動力学に関する変数は以下のように定義する。

ｇ：重力加速度、Zb：上体質点の鉛直位置、Xb：上体質点の水平位置、mtotal：ロボット１の総質量、Fx：床反力水平成分（詳しくは並進床反力の前後方向（Ｘ軸）成分）、Fz：床反力鉛直成分（詳しくは並進床反力の鉛直方向（Ｚ軸）成分）、My：目標ＺＭＰまわりの床反力モーメント（詳しくは床反力モーメントの左右軸（Ｙ軸）まわり成分）、Xzmp：目標ＺＭＰの水平位置、Ｚzmp：目標ＺＭＰの鉛直位置。

Fz＝mtotal＊(g＋d2Zb/dt2) …式０１
Fx＝mb＊d2Xb/dt2 …式０２
My＝−mtotal＊(Xb−Xzmp)＊(g＋d2Zb/dt2)＋mtotal＊(Zb−Zzmp)＊(d2Xb/dt2)
…式０３

これらの式０１〜０３により記述される単純化モデルでは、例えば目標ＺＭＰと目標床反力鉛直成分とを決めたとき、式０１に従って、上体質点３ｍの鉛直位置Ｚｂを決定できることとなる。さらに、ロボット１の動力学的平衡状態では、式０３の左辺のＭｙが０になる（目標ＺＭＰまわりの床反力モーメントの水平成分が０になる）から、上体質点３ｍの鉛直位置Ｚｂと式０３とから上体質点３ｍの水平位置Ｘｂを決定できることとなる。

以上説明した第１実施形態の単純化モデルは、上体３にのみ質点３ｍを持つ１質点モデルであるため、各脚体２の運動によって発生する慣性力や、上体３のイナーシャ（慣性モーメント）は無視されている。つまり、第１実施形態の単純化モデルは、各脚体２の運動（並進あるいは姿勢変化運動）や上体３の姿勢変化運動によって発生する慣性力を０と見なして構築された動力学モデルであると言える。補足すると、第１実施形態を含む本明細書の各実施形態での単純化モデルは、ロボット１の少なくとも１つ以上の特定部位の、特定の運動（並進運動、姿勢変化運動など）によって発生する慣性力がほぼ０であるとみなして（該慣性力を無視して）構築されている。

なお、第１実施形態の単純化モデルは１質点モデルとしたが、例えば各脚体２の足平２２の近傍にも質点を持たせた３質点モデルとしてもよい。また、例えば上体３が上体質点３ｍのまわりにイナーシャ（慣性モーメント）をもつようなモデルであってもよい。

次に第１実施形態における第１変位次元補正用モデルを説明する。図７（ａ），（ｂ），（ｃ）の右側の図は、第１実施形態における第１変位次元補正用モデルの構造を示し、左側の図は、右側の図にそれぞれ対応するロボット１の目標とする全体的な姿勢状態（単純化モデル歩容の姿勢状態）と前記単純化モデルとを示している。なお、図７（ａ），（ｂ），（ｃ）の右側に示すロボット１は、両脚体２，２を左右方向（Ｙ軸方向）に並べて直立姿勢で起立している状態のロボット１を側面視（サジタルプレーン）で表したものである。このため、両脚体２，２は図面上、重なっている。

第１実施形態の第１変位次元補正用モデルは、ロボット１の上体３に対応する１つの上体質点Ａ１、各脚体２の膝関節近傍の大腿リンク部分に対応する大腿質点Ａ２，Ａ３、および各脚体２の先端部（足平２２）にそれぞれ対応する足平質点Ａ４，Ａ５からなる５質点モデルである。また、第１変位次元補正用モデルにおける上体３（上体リンク）は、上体質点Ａ１のまわりにイナーシャ（慣性モーメント）Ｉｂを持つものとされている。つまり、第１変位次元補正用モデルは、質点Ａ１〜Ａ５とイナーシャＩｂをもつ上体リンクとを要素として構成されている。この場合、第１変位次元補正用モデルの質点Ａ２〜Ａ５とイナーシャＩｂをもつ上体リンクは、前記した図６の単純化モデルが持たない要素であり、それぞれに対応する部位の運動（上体３についてはその姿勢変化運動）によって慣性力を発生するものとなっている。

この第１変位次元補正用モデルの上体質点Ａ１と、足平質点Ａ４，Ａ５とは、それぞれに対応する部位（上体３、各足平２２）の位置姿勢に対応して一義的に定まる点、すなわち、対応する部位に任意に固定設定されたローカル座標系上でのある固定点（その対応する部位のローカル座標系上で、該部位の代表点と所定の位置関係を有する点）に設定されている。但し、上体３のローカル座標系上での上体質点Ａ１の位置は、図６に示した前記単純化モデルの上体質点３ｍとは一般には異なる。また、大腿質点Ａ２，Ａ３は、各脚体２の大腿リンク２４に任意に固定設定されたローカル座標系でのある固定点（膝関節近傍の固定点）に設定されている。なお、上体質点Ａ１、各足平質点Ａ４，Ａ５および各大腿質点Ａ２，Ａ３の質量の総和は、ロボット１の総質量ｍtotalと一致する。また、上体質点Ａ１の質量は、上体３の質量のほか、両腕体５，５および頭部４の質量を含んでいる。

そして、第１変位次元補正用モデルの要素の配置には、ある幾何学的拘束条件が設定されている。具体的には、第１変位次元補正用モデルでは、ロボット１の姿勢状態は、上体３を鉛直姿勢にすると共に、両脚体２，２をロボット１の左右方向（Ｙ軸方向）に所定間隔で並べて起立している姿勢状態（直立姿勢状態）に定常的に拘束されている（このため、図７（ａ），（ｂ），（ｃ）の右側の第１変位次元補正用モデルの図では一方の脚体２に対応する各質点Ａ２，Ａ４はそれぞれ他方の脚体２に対応する各質点Ａ３，Ａ５と重なっている）。

従って、上体質点Ａ１、各足平質点Ａ４，Ａ５および大腿質点Ａ２，Ａ３の相互の相対的位置関係が、ロボット１の直立姿勢状態に対応する所定の位置関係に拘束されている。また、第１変位次元補正用モデルにおいてイナーシャをもつリンク（剛体）である上体３の姿勢は、鉛直姿勢（鉛直軸に対する姿勢角が０となる姿勢）に拘束されている。

また、第１変位次元補正用モデルの各質点Ａ１〜Ａ５のグローバル座標系（床に固定された座標系）上での位置は、単純化モデル歩容の運動の瞬時値に対応して定まるものとされている。すなわち、第１実施形態の第１変位次元補正用モデルでは、その質点Ａ１〜Ａ５の全体重心が単純化モデル上でのロボット１の全体重心の位置、すなわち、単純化モデルの上体質点３ｍの位置（グローバル座標系での位置）と一致するように、質点Ａ１〜Ａ５のグローバル座標系での位置が決定される。この場合、前記したように第１変位次元補正用モデルの質点Ａ１〜Ａ５の相互の相対的位置関係は一定であるので、それらの質点Ａ１〜Ａ５の全体重心の位置（グローバル座標系での位置）が決まれば、各質点Ａ１〜Ａ５のグローバル座標系での位置も一義的に定まる。

このようにグローバル座標系での質点Ａ１〜Ａ５の位置が単純化モデル歩容に対応して定められる第１変位次元補正用モデルでは、その全体重心の運動が単純化モデル上での全体重心の運動と一致するため、単純化モデル上でロボット１に作用する床反力と、第１変位次元補正用モデル上でロボット１に作用する床反力とが同等になる。

ここで、第１実施形態で上記のように、第１変位次元補正用モデルの要素の配置を定めるということは、第１変位次元補正用モデルの要素の配置（グローバル座標系での質点Ａ１〜Ａ５の位置および上体リンクの姿勢）を定めるための幾何学的拘束条件（１）を次のように定義したとき、単純化モデル歩容の瞬時運動から、その幾何学的拘束条件（１）に従って第１変位次元補正用モデルの要素の配置を定めることと同じである。

幾何学的拘束条件（１）：与えられた任意の瞬時目標運動に対して、第１変位次元補正用モデルの要素の配置に対応するロボット１の姿勢状態が定常的に直立姿勢状態に維持され、且つ、第１変位次元補正用モデルの要素の全体重心が、与えられた瞬時目標運動におけるロボット１の全体重心に一致する。

第１実施形態では、この幾何学的拘束条件（１）が本発明における第１の幾何学的拘束条件に相当するものである。

次に第１実施形態における第２変位次元補正用モデルを説明する。図８は、その第２変位次元補正用モデルの構造を示している。この第２変位次元補正用モデルは、その構成要素は、第１変位次元補正用モデルと同じであり、第１変位次元補正用モデルと同様に５個の質点Ａ１〜Ａ５を有すると共に、上体３（上体リンク）が質点Ａ１のまわりにイナーシャＩｂを持つモデルである。各質点Ａ１〜Ａ５の質量と、各質点Ａ１〜Ａ５の、対応する部位に固定設定されたローカル座標系での位置とは、第１変位次元補正用モデルと同一である。また、上体３のイナーシャＩｂも第１変位次元補正用モデルと同一である。

この第２変位次元補正用モデルでは、第１変位次元補正用モデルのようにロボット１の姿勢は直立姿勢状態には拘束されておらず、各質点Ａ１〜Ａ５および上体３（上体リンク）はロボット１が採り得る任意の姿勢状態に対応する位置姿勢に移動可能とされている。

別の言い方をすれば、第２変位次元補正用モデルの要素の配置（グローバル座標系での質点Ａ１〜Ａ５の位置および上体リンクの姿勢）と、これに対応するロボット１の瞬時目標運動（各部位の目標位置姿勢の瞬時値）との間には、次のような幾何学的拘束条件（２）が設定されていることとなる。この幾何学的拘束条件（２）は、本発明における第２の幾何学的拘束条件に相当するものである。

幾何学的拘束条件（２）：第２変位次元補正用モデルの各要素の配置により定まる、ロボット１の各要素に対応する部位の位置姿勢と、当該配置に対応する瞬時目標運動におけるロボット１の各要素に対応する部位の位置姿勢とは一致する。

従って、第２変位次元補正用モデルでは、その質点Ａ１〜Ａ５の全体重心が、該第２変位次元補正用モデルの各要素の配置（質点Ａ１〜Ａ５の位置および上体リンクの姿勢）に対応する姿勢状態での実際のロボット１の真の全体重心の位置にほぼ一致する。

補足すると、任意の瞬時目標運動から、上記幾何学的拘束条件（２）に従って第２変位次元補正用モデルの要素の配置を決定するということは、第２変位次元補正用モデルの各要素の配置が、与えられた瞬時目標運動に従うロボット１における該要素に対応する部位の配置（位置姿勢）に一致するように第２変位次元補正用モデルの各要素の配置を決定するということと等価である。また、第２変位次元補正用モデルの要素の任意の配置から、幾何学的拘束条件（２）に従って瞬時目標運動を決定するということは、瞬時目標運動に従うロボット１の各要素に対応する部位の配置（位置姿勢）が、与えられた第２変位次元補正用モデルの要素の配置に一致するように瞬時目標運動を決定するということと等価である。

第２変位次元補正用モデルは、第１変位次元補正用モデルと協働して前記変位次元補正上体位置姿勢を決定するためのモデルであり、その変位次元補正上体位置姿勢を決定するに際して、第２変位次元補正用モデルの要素の２種類の配置が仮決定される。その１つの配置では、第２変位次元補正用モデルの各足平質点Ａ４，Ａ５の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、上体質点Ａ１および各大腿質点Ａ２，Ａ３の位置、並びに上体３（上体リンク）の姿勢角は、第１変位次元補正用モデルおよび第２変位次元補正用モデルの全体重心と、それらのモデル間の後述する角運動量積とに関する所定の条件を満たすように決定される。これについては、詳細を後述する。

補足すると、本実施形態のロボット１の各脚体２はそれぞれ６自由度をもつので、両足平２２，２２の位置姿勢および上体３の位置姿勢が決まれば、ロボット１の脚体２，２の全体の姿勢（ロボット１の各脚体２，２の各部位（各リンク）の位置姿勢）は一義的に定まる。従って、第２変位次元補正用モデル上での両足平質点Ａ４，Ａ５並びに上体質点Ａ１の位置および上体３（上体リンク）の姿勢を決めれば、それに対応して大腿質点Ａ２，Ａ３の位置は従属的に定まる。

また、変位次元補正上体位置姿勢を決定するに際して仮決定される第２変位次元補正用モデルの要素の他の１つの配置では、第２変位次元補正用モデルの各足平質点Ａ４，Ａ５の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。さらに、上体リンクの姿勢は、単純化モデル歩容の上体姿勢と同一に決定される。また、上体質点Ａ１および各大腿質点Ａ２，Ａ３の位置は、第１変位次元補正用モデルおよび第２変位次元補正用モデルの間の後述する角運動量積に関する所定の条件を満たすように決定される。これについても詳細は後述する。

そして、第２変位次元補正用モデルの要素の上記の２種類の配置を基に、最終的に変位次元補正上体位置姿勢が決定される。

なお、以降の説明では、単純化モデル、第１および第２変位次元補正用モデルに係わる各質点の「位置」、あるいはイナーシャをもつリンクの「姿勢」は特にことわらない限り、グローバル座標系での位置、姿勢を意味するものとする。

次に、第１実施形態での歩容生成装置１００の処理の詳細をより具体的に説明する。歩容生成装置１００は、以下に説明するフローチャートの処理によって、ロボット１の片方の脚体２が着地してから他方の脚体２が着地するまでの１歩分の目標歩容（前記狭義の目標歩容）を単位として、その１歩分の目標歩容を順番に生成する。このとき、新たに生成しようとしている目標歩容を「今回歩容」と呼ぶ。

図９は、歩容生成装置１００のメインルーチン処理を示す構造化フローチャートである。以下、詳説すると、まずＳ０１０において時刻ｔを０に初期化するなど種々の初期化作業が行なわれる。この処理は歩容生成装置１００の起動時等に行なわれる。次いで、Ｓ０１２を経てＳ０１４に進み、歩容生成装置１００は、制御周期（図９のフローチャートの演算処理周期）毎のタイマ割り込みを待つ。制御周期はΔｔである。以降、その制御周期Δｔ毎に、Ｓ０１４からＳ０３２までの処理が繰り返される。

Ｓ０１４の次にＳ０１６に進み、歩容の切り替わり目であるか否かが判断され、歩容の切り替わり目であるときはＳ０１８に進むと共に、切り替わり目でないときはＳ０２２に進む。ここで、「歩容の切り替わり目」は、今回歩容の生成を開始するタイミングを意味し、例えば今回歩容の１つ前の目標歩容の生成が完了した制御周期の次の制御周期が歩容の切り替わり目になる。

Ｓ０１８に進むときには、現在時刻ｔを０に初期化した後、Ｓ０２０に進んで今回歩容の歩容パラメータが決定される。このＳ０２０の処理は、前記図５の歩容パラメータ決定部１００ａの処理に相当し、目標足平位置姿勢軌道、目標腕姿勢軌道、目標ＺＭＰ軌道、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、基準上体姿勢軌道、床反力水平成分許容範囲、およびＺＭＰ許容範囲を規定するパラメータが決定される。

このＳ０２０の処理は、例えば本願出願人が先に提案したＰＣＴ国際公開公報ＷＯ／０３／０５７４２７／Ａ１（以下、公報文献１という）の図１３のＳ０２２〜Ｓ０３０に対応する処理であり、同公報文献１と同様に行われる。それを要約的に説明すれば、まず、今回歩容がつながり、もしくは漸近すべき仮想的な周期的歩容としての定常歩容（ロボット１の２歩分の歩容を１周期とする歩容）が決定される。定常歩容は、今回歩容を含む２歩先までの遊脚足平２２の着地予定位置姿勢、着地予定時刻などを基に、周期性の条件（定常歩容の１周期の初期の状態（ロボット１の各部の位置姿勢やその変化速度など）と終端の状態とが一致するという条件）を満たすように決定される。そして、今回歩容がその定常歩容につながり、もしくは漸近するように目標足平位置姿勢軌道、目標腕姿勢軌道、目標ＺＭＰ軌道、および目標床反力鉛直成分軌道を規定する歩容パラメータが決定される。ここで、目標足平位置姿勢軌道を規定する歩容パラメータは、例えば本願出願人が先に特許３２３３４５０号で提案した有限時間整定フィルタを用いて目標足平位置姿勢軌道を生成する場合、今回歩容の遊脚足平２２の着地予定位置姿勢および着地予定時刻、今回歩容の支持脚足平２２の次の着地予定位置姿勢および着地予定時刻などから構成される。また、例えば目標ＺＭＰ軌道、目標床反力鉛直成分軌道を規定する歩容パラメータは、それらの軌道が折れ線状の軌道であるとした場合、その折れ点での値やその折れ点の時刻などから構成される。なお、本実施形態では、基準上体姿勢は、例えば鉛直姿勢（鉛直軸に対する上体３の傾斜角が０である姿勢）とされる。また、本実施形態のＳ０２０で決定する歩容パラメータのうち、床反力水平成分許容範囲を規定する歩容パラメータは、前記公報文献１の図１３のＳ０３０で決定するフルモデル補正用の床反力水平成分許容範囲のパラメータに相当するものである。

補足すると、前記公報文献１で今回歩容の歩容パラメータを決定するための処理では、定常歩容の作成などのために動力学モデルを使用しているが、その動力学モデルとして、本実施形態では、前記単純化モデルが用いられる。この場合、本実施形態の単純化モデルは、公報文献１の図１１で例示した動力学モデルとは同じではないものの、同公報文献１の動力学モデルの両脚質点の質量を０とし、且つ上体に関するイナーシャ（フライホールのイナーシャ）を０としたものと等価である。従って、同公報文献１の図１１の動力学モデルの両脚質点の質量を０とし、且つ上体に関するイナーシャを０とすれば、同公報文献１の図１３のＳ０２２〜Ｓ０３０の処理をそのまま適用して、本実施形態におけるＳ０２０の処理を実行できる。また、同公報文献１の図１３のＳ０２２〜Ｓ０２８の処理では、定常歩容の作成などのために、単純化モデル歩容用の床反力水平成分許容範囲（この許容範囲は歩容生成装置から出力されるものではない）を設定して使用しているが、本実施形態では、その単純化モデル歩容用の床反力水平成分許容範囲は、例えば無限大の範囲とするか、もしくは、単純化モデル歩容（あるいは定常歩容）の床反力水平成分が常にその床反力水平成分許容範囲内に収まるような広い範囲に定めておけばよい。このようにすることで、同公報文献１に示したアルゴリズムを本実施形態のＳ０２０の処理で支障なく適用できる。

次いで、Ｓ０２０の処理の後、あるいはＳ０１６の判断結果がＮＯであった場合には、Ｓ０２２に進んで、今回歩容の瞬時値を決定する。この処理は、前記図５の目標瞬時値発生部１００ｂおよび単純化モデル歩容生成部１００ｃで実行される処理であり、Ｓ０２０で決定された歩容パラメータに基づいて今回歩容の瞬時値（単純化モデル歩容の瞬時値）が決定される。

この処理は、より具体的には前記公報文献１の図１３のＳ０３２の処理に相当するものであり、同公報文献１と同様に行われる。それを要約的に説明すれば、前記Ｓ０２０で決定した歩容パラメータを基に、目標足平位置姿勢、目標ＺＭＰ、目標腕姿勢、目標床反力鉛直成分、基準上体姿勢の瞬時値が決定され、さらに、それらの瞬時値を基に、前記単純化モデル上で、目標ＺＭＰと目標床反力鉛直成分とを満足するように（ロボット１の運動が発生する慣性力と重力との合力が目標ＺＭＰまわりに作用するモーメント水平成分が０になり、また、その合力の並進力鉛直成分が目標床反力鉛直成分に釣り合うように）目標上体位置姿勢の瞬時値が決定される。ここで、目標上体位置姿勢の瞬時値に関して補足すると、目標上体姿勢の瞬時値は本実施形態では基準上体姿勢の瞬時値と同じとされる。また、目標上体位置鉛直成分は、目標床反力鉛直成分と前記式０１とから求められる単純化モデルの上体質点３ｍの鉛直位置に対応して決定される。そして、単純化モデルの上体質点３ｍの水平位置が前記式０３の左辺を０とした式を満たすように（目標ＺＭＰまわりの床反力モーメント水平成分が０になるように）決定され、この上体質点３ｍの水平位置に対応して目標上体位置水平成分が決定される。

なお、前記公報文献１の図１３のＳ０３２の処理では、単純化モデル歩容用の床反力水平成分許容範囲を使用しているが、本実施形態では、前記Ｓ０２０の処理に関して説明した場合と同様、単純化モデル歩容用の床反力水平成分許容範囲は、例えば無限大の範囲とするか、もしくは、単純化モデル歩容の床反力水平成分が常にその床反力水平成分許容範囲内に収まるように定めておけばよい。

Ｓ０２２の処理で、逐次（制御周期Δｔ毎に）、瞬時値が決定される目標歩容（今回歩容）は、それを簡潔にいえば、前記単純化モデル上において、その運動が発生する慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になり、また、その合力の並進力鉛直成分が目標床反力鉛直成分に釣り合うような歩容である。

次いで、Ｓ０２４に進んで、変位次元歩容補正サブルーチンを実行する。この変位次元歩容補正サブルーチンは、本発明の中核に係わるものであり、以下に詳細に説明する。

前記単純化モデルを用いた目標歩容の生成処理では、今回歩容（発散しない今回歩容）を安定にリアルタイムで決定できるという利点がある反面、生成される歩容の動力学的な近似精度が低い。このため、本発明の実施形態では、単純化モデルよりも動力学的精度の高いフルモデルを使用して、歩容の一部（目標上体位置姿勢、目標ＺＭＰまわりのモーメント）を補正する。この場合、単純化モデル歩容の動力学的近似精度が低いこと、フルモデルの非線形性が強いことなどのために、単純化モデル歩容をフルモデルに入力するようにすると、単純化モデル歩容の補正が適正に行われずに、ロボット１の継続的な運動を行い得ない歩容が生成されてしまうなどの不具合が生じる場合がある。特に、ロボット１の走行のように脚体２の運動が短い時間内で大きく変化するような歩容を生成する場合には、単純化モデルでは考慮されていない各脚体２の膝関節の曲げ動作などに伴う慣性力の変化の影響が大きくなって、単純化モデル歩容の動力学的な近似精度が低下し、上記の不具合が生じやすくなる。そして、このような不具合を解消するために、例えば各脚体２に複数の質点をもつ動力学モデルを構築したり、さらには上体などの、ロボットの１つ以上のリンクにイナーシャ（慣性モーメント）をもつ動力学モデルを構築し、それを単純化モデルとして用いることで、単純化モデル歩容の動力学的近似精度を高めることが考えられる。しかし、このようにした場合には、単純化モデルの非線形性が高まって、定常歩容につながる今回歩容の歩容パラメータ（ロボット１の運動の継続性を確保し得る歩容パラメータ）を安定して適正に見出すことが困難となる場合があると共に、その演算処理に時間がかかり、ひいては、適正な歩容をリアルタイムに生成することが困難となる。

そこで、本実施形態（第１実施形態）を含む本発明の各実施形態では、第１および第２変位次元補正用モデルを用いて、目標ＺＭＰや床反力を含む動力学方程式を用いることなく、第１および第２変位次元補正用モデルの要素の配置（質点とイナーシャをもつリンクの位置、姿勢）に関する幾何学的な処理（位置と姿勢との変位の次元での処理）によって、単純化モデル歩容の一部の運動（具体的には上体位置姿勢）のみを補正するようにした。そして、それによって、単純化モデル歩容よりも動力学的精度の高い歩容、詳しくは、その歩容の運動によってロボット１が発生する実際の慣性力と重力との合力の並進力成分がより高精度に目標床反力の並進力成分に釣り合い、且つ、該合力が目標ＺＭＰまわりに作用するモーメントの水平成分がより高精度に０になるような歩容を生成するようにした。

但し、動力学的な精度を高めることだけに着目して、単純化モデル歩容の上体位置および上体姿勢の両者を補正した場合、上体姿勢が変動し過ぎる恐れがある。ここで、ロボット１の上体３は一般に他の部位に比して重いと共にイナーシャが大きいので、上体姿勢が頻繁に変動するような歩容でロボット１の運動を行うと、股関節に過大なモーメントが発生する。その結果、股関節用アクチュエータに過大な負荷が掛かったり、股関節部分およびその近辺が撓んで振動し、ロボット１の姿勢が急速に不安定になりやすい。また、ロボット１の頭部４などに視覚装置として撮像装置を搭載した場合には、その揺れが生じやすくなって、該撮像装置による環境認識を正確に行うことが困難となる。

一方、例えば比較的摩擦係数の大きい床上でロボット１の歩行を行う目標歩容を生成する場合には、主に上体３の並進運動の調整によって、ロボット１のスリップを生じることなく目標ＺＭＰを満足する歩容を生成することが可能である。従って、単純化モデル歩容の上体姿勢および上体位置のうち、主に上体位置を補正することで、目標ＺＭＰを満足しつつ動力学的な精度を高めることが可能である。他方、比較的摩擦係数の小さい床上でロボット１の歩行を行う場合や、ロボット１の走行のように、目標床反力鉛直成分が０もしくは微小となる時期をもたざるを得ない目標歩容を生成する場合には、目標ＺＭＰを満足するために上体姿勢のある程度の変動は避けられない。これは、ロボット１の上体３の並進運動によって発生し得る慣性力の水平成分（より正確には床面に平行な成分）が制限を受け、その制限内での上体３の並進運動の調整だけでは、目標ＺＭＰを満足することが困難となるからである。

このようなことから、第１実施形態を含む本発明の各実施形態では、ロボット１の目標歩容における運動形態や床の摩擦状態を考慮しつつ、前記第１変位次元補正用モデルと第２変位次元補正用モデルとを用いて、できるだけ上体姿勢の変動を抑えながら動力学的な精度を高められるように単純化モデル歩容の上体位置姿勢を補正することとした。

このように単純化モデル歩容の上体位置姿勢を補正するために用いる第１および第２変位次元補正用モデルは、通常、ロボット１のいくつかの部位に対応する質点を同じように備え、あるいは、質点とイナーシャを持つリンク（上体３など）とを同じように備える。そして、両変位次元補正用モデルは、単純化モデルよりも多くの質点を有し、あるいは、単純化モデルが持たないイナーシャを持つ。なお、本明細書の各実施形態における両変位次元補正用モデルでは、単純化モデル歩容の上体位置姿勢を補正するために、いずれも上体３に対応する質点とイナーシャをもつ上体リンクとが含まれる。

この場合、第１変位次元補正用モデルでは、各質点の相互の位置関係あるいはイナーシャを持つリンク（上体３など）の姿勢にある適当な拘束条件が付加される。

より詳しく説明すると（ここでは第１実施形態に限らない一般的な説明を行う）、生成した単純化モデル歩容の各部位の位置姿勢の瞬時値（瞬時運動）に応じて、第１変位次元補正用モデルの各質点の位置が決定される。このとき、第１変位次元補正用モデルが、ロボット１の１つ以上のリンク（上体３など）にイナーシャをもつモデルである場合には、そのリンクの姿勢角も決定される。ただし、第１変位次元補正用モデルでは、各質点の位置関係、あるいはイナーシャを持つリンク（上体３など）の姿勢にある適当な幾何学的拘束条件が付加されており、これによって、単純化モデル歩容の各部位の位置姿勢の瞬時値（瞬時運動）に応じて、第１変位次元補正用モデルの各質点の位置や、イナーシャをもつリンクの姿勢角を決定したとき、単純化モデル歩容の床反力と同じような床反力が第１変位次元補正用モデルでも発生するようにする。第１実施形態では、このような幾何学的拘束条件として、前記幾何学的拘束条件（１）が第１変位次元補正用モデルに付加されている。

さらに、第１変位次元補正用モデルで設定された幾何学的拘束条件を持たない第２変位次元補正用モデルに対し、第１変位次元補正用モデルとの間で、次の条件１，２を満足するような第２変位次元補正用モデルの各質点の位置（あるいは各質点の位置とイナーシャをもつリンクの姿勢角）が、第２変位次元補正用モデルの第１要素配置として仮決定される。

条件１）第１変位次元補正用モデルの全体重心の位置と第２変位次元補正用モデルの全体重心の位置とはほぼ一致する。
条件２）ある点Ｑを定めたとき、第１変位次元補正用モデルに対する第２変位次元補正用モデルの、点Ｑのまわりの角運動量積の総和がある一定値（所定値）になる。

ここで、条件１は、両変位次元補正用モデルで並進床反力もしくは全体重心の運動によって発生する慣性力がほぼ同じになるための条件である。条件１は別の言い方をすれば、第１変位次元補正用モデルの各質点の位置とこれに対応する第２変位次元補正用モデルの質点の位置との差（位置ベクトルの差）のベクトルを該質点の並進加速度とみなしたときに、各質点が発生する慣性力の並進力成分（質点の質量＊並進加速度）の、全ての質点についての総和がほぼ０になるという条件と等価である。

また、条件２に関する前記角運動量積は、各変位次元補正用モデルの各質点に対する基準位置をそれぞれ任意に定め、また、前記点Ｑの位置を任意に定めたとき、その各質点毎に以下の如く定義されるものである。さらに、各変位次元補正用モデルがイナーシャを持つ場合（ある１つ以上のリンクにイナーシャが設定されている場合）には、そのイナーシャをもつ各リンクに対する基準姿勢角をそれぞれ任意に定めたとき、その各リンク毎に以下の如く定義されるものである。

すなわち、各変位次元補正用モデルの質点に係る角運動量積は、点Qとその質点に対応する基準位置の点とを結ぶ線分（その線分のベクトル）と、該質点の該基準点からの位置のずれ（その位置ずれのベクトル）との外積に、該質点の質量を乗算したものに相当するものである。なお、この場合、上記外積と質量との積に比例関係を有するもの、あるいは該外積と質量との積に近似的に等しいものを当該質点に係る角運動量積と定義してもよい。また、各変位次元補正用モデルの、イナーシャを持つリンクに係る角運動量積は、そのリンクの姿勢角の、該リンクに対応する基準姿勢角からのずれと該リンクのイナーシャとの積に相当するものである。なお、この場合、リンクの姿勢角の基準姿勢角からのずれとイナーシャとの積に比例関係を有するもの、あるいは該積に近似的に等しいものを当該リンクに係る角運動量積と定義してもよい。

また、各変位次元補正用モデルの質点に係る角運動量積について補足すると、任意の質点に係る角運動量積は、質点と前記所定の点とを結ぶ線分と、その質点に対する基準点と前記所定の点とを結ぶ線分とのなす角度に対して単調に変化する関数（単調増加関数または単調減少関数）となる。

このように角運動量積を定義したとき、条件２は、より詳しく言えば、第１変位次元補正用モデルの各質点の位置を、第２変位次元補正用モデルの各質点に対応する基準位置とし、また、第１変位次元補正用モデルのイナーシャをもつ各リンクの姿勢角を、第２変位次元補正用モデルのイナーシャを持つ各リンクの基準姿勢角としたときの、第２変位次元補正用モデルの角運動量積の総和がある一定値になる、という条件である。

また、別の言い方をすれば、条件２は、第１変位次元補正用モデルの各質点の位置とこれに対応する第２変位次元補正用モデルの質点の位置との差（位置ベクトルの差）のベクトルを該質点の並進加速度とみなし、また、両変位次元補正用モデルのイナーシャをもつ各リンクの姿勢角の差を該リンクの角加速度とみなしたときに、各質点が発生する慣性力の並進力成分が点Ｑのまわりに作用するモーメントとイナーシャをもつ各リンクの慣性力（回転運動の慣性力）が点Ｑまわりに作用するモーメントとの総和がある一定値（所定値）になる、という条件と等価である。

そして、本明細書で詳説する実施形態では、上記点Ｑは例えば目標ＺＭＰに設定される。なお、点Ｑは、目標ＺＭＰに限られるものではないが、これについては後に補足する。

さらに、本明細書の第１および第２実施形態では、第２変位次元補正用モデルに対して、ロボット１の上体姿勢を単純化モデル歩容での姿勢に規制した上で、第１変位次元補正用モデルとの間で、前記条件２を満足するような第２変位次元補正用モデルの各質点の位置（あるいは各質点の位置とイナーシャをもつリンクの姿勢角）が第２変位次元補正用モデルの第２要素配置として仮決定される。そして、ロボット１の目標歩容における運動形態や床の摩擦状態に応じた重みを用いて、前記第１要素配置に対応する上体姿勢と前記第２要素配置に対応する上体姿勢との重み付け平均が、変位次元補正上体姿勢として決定されると共に、第１要素配置に対応する上体位置と第２要素配置に対応する上体位置との重み付け平均が変位次元補正上体位置として決定される。あるいは、本明細書の第３実施形態では、第２変位次元補正用モデルに対して、ロボット１の目標歩容における運動形態や床の摩擦状態とに応じた重みを用いて、ロボット１の上体姿勢を、前記第１要素配置に対応する上体姿勢と単純化モデル歩容の上体姿勢との重み付け平均に規制した上で、前記条件２を満足するような第２変位次元補正用モデルの各質点の位置（あるいは各質点の位置とイナーシャをもつリンクの姿勢角）が第２変位次元補正用モデルの第２要素配置として決定される。そして、この第２要素配置に対応する上体位置姿勢が、そのまま変位次元補正上体位置姿勢として決定される。

本明細書の各実施形態では、上記した如く、第１変位次元補正用モデルおよび第２変位次元補正用モデルを用いて、単純化モデル歩容の目標上体位置姿勢を補正してなる変位次元補正上体位置姿勢が求められる。前記図６のフローチャートのＳ０２４の処理は、上記の如く変位次元補正上体位置姿勢を求める処理である。

以下、第１実施形態の説明に戻って、該第１実施形態でのＳ０２４のサブルーチン処理を図１０を参照して具体的に説明する。なお、ここでは、本明細書の実施形態の理解の便宜上、サジタルプレーン（Ｘ軸、Ｚ軸を含む平面）上での上体位置姿勢の補正（変位次元補正上体位置姿勢の算出）について説明し、ラテラルプレーン（Ｙ軸、Ｚ軸を含む平面）上での上体位置姿勢の補正に関しては省略する。

まず、Ｓ１００において、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の重心に関する前記条件１と、角運動量積に関する前記条件２とを満足するように第１仮補正上体位置姿勢（Pb21，θb21）（第１仮補正上体位置Ｐb21と第１仮補正上体姿勢θb21との組）が決定される。より正確に言えば、両モデル間での前記条件１，２を満足するように第２変位次元補正用モデルの前記第１要素配置が決定され、その第１要素配置における上体質点Ａ１の位置および上体リンクの姿勢に対応するロボット１の上体位置姿勢が前記第１仮補正上体位置姿勢(Pb21，θb21)として決定される。

このＳ１００の処理は、図１１のサブルーチン処理によって実行される。以下説明すると、まず、Ｓ２００において、今回時刻（現在時刻）ｔでの単純化モデル歩容の瞬時値（目標上体位置姿勢などの目標運動の瞬時値）を基に、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角とを求める。具体的には、単純化モデル歩容でのロボット１の全体重心の位置と、第１変位次元補正用モデル上でのロボット１の全体重心の位置とが等しくなるように第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置が決定される。この場合、本実施形態では、単純化モデル歩容でのロボット１の全体重心の位置は、単純化モデルの上体質点３ｍの位置に一致するので、その位置は、単純化モデル歩容の目標上体位置姿勢から一義的に定まる。そして、第１変位次元補正用モデルでは、前記した通り各質点Ａ１〜Ａ５の相対的位置関係が拘束されているので、それらの質点Ａ１〜Ａ５の全体重心の位置（第１変位次元補正用モデルでのロボット１の全体重心の位置）を単純化モデルの上体質点３ｍの位置に一致させることで、各質点Ａ１〜Ａ５の位置が一義的に決定されることとなる。また、第１変位次元補正用モデルの上体リンクの姿勢角は、単純化モデル歩容の上体姿勢角（本実施形態では鉛直姿勢）と同一とされる。これにより単純化モデル歩容の瞬時運動（今回時刻ｔの瞬時値）から、第１変位次元補正用モデルに係る前記幾何学的拘束条件（１）に従って、第１変位次元補正用モデルの各要素の配置が決定されることとなる。なお、この第１変位次元補正用モデルの要素の配置は、本発明の第１発明における「第１の配置」に相当するものである。

次いで、Ｓ２０２からの処理が実行され、第１変位次元補正用モデルに対して前記条件１、２を満たすような第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角との組、すなわち、第２変位次元補正用モデルの前記第１要素配置が探索的に決定され、その第１要素配置における上体質点Ａ１および上体リンクの姿勢に対応するロボット１の上体位置姿勢が第１仮補正上体位置姿勢（Ｐb21，θb21）として決定される。

さらに詳細には、まず、Ｓ２０２において、第１仮補正上体位置姿勢の初期候補（Ｐb21_s，θb21_s）を決定する。初期候補（Ｐb21_s，θb21_s）は、今回時刻ｔ（現在時刻ｔ）での第１仮補正上体位置Ｐb21および第１仮補正上体姿勢θb21の概略的な予想値に相当するものであり、例えば次のように決定される。すなわち、今回時刻ｔでの第１仮補正上体位置Ｐb21と単純化モデル歩容の上体位置Ｐbとの差分（位置のずれ量）は、前回時刻（前回の制御周期の時刻）ｔ−ΔｔでのＰb21とＰbとの差分に近いと考えられる。同様に、今回時刻ｔでの第１仮補正上体姿勢θb21と単純化モデル歩容の上体姿勢θbとの差分（姿勢角のずれ量）は、前回時刻ｔ−Δｔでのθb21とθbとの差分に近いと考えられる。そこで、初期候補（Ｐb21_s，θb21_s）を、今回時刻ｔでのＰb，θbと、前回時刻ｔ−ΔｔでのＰb，θbの値Ｐb_p，θb_pと、前回時刻ｔ−ΔｔでのＰb21，θb21の値Ｐb21_p，θb21_pとから次式により決定する。

Ｐb21_s＝Ｐb＋（Ｐb21_p−Ｐb_p） …式０４ａ
θb21_s＝θb＋（θb21_p−θb_p） …式０５ａ

次いで、Ｓ２０４を経て、Ｓ２０６〜Ｓ２１６のループ処理を実行する。Ｓ２０６では、第１仮補正上体位置姿勢の現在の候補（Ｐb21_s，θb21_s）と、今回時刻ｔでの単純化モデル歩容の目標両足平位置姿勢とを基に、第２変位次元補正モデルでの各質点Ａ１〜Ａ５の位置を求める。この場合、第２変位次元補正用モデルでのロボット１の上体３の位置姿勢が現在の候補（Ｐb21_s，θb21_s）に一致し、且つ、第２変位次元補正用モデルでのロボット１の各足平２２の位置姿勢が単純化モデル歩容の目標足平位置姿勢に一致しているとして、各質点Ａ１〜Ａ５の位置が求められる。別の言い方をすれば、単純化モデル歩容の瞬時運動のうちの、上体位置姿勢の瞬時値のみを候補（Ｐb21_s，θb21_s）で置き換えた瞬時運動から、前記幾何学的拘束条件（２）に従って、第２変位次元補正用モデルにおける各質点Ａ１〜Ａ５の位置が求められる。

具体的には、足平質点Ａ３，Ａ４の位置は、目標足平位置姿勢から決定される。また、上体質点Ａ１の位置は、候補（Ｐb21_s，θb21_s）から決定され、上体３（上体リンク）の姿勢角はθb21_sと同一とされる。そして、各大腿質点Ａ２，Ａ３の位置は、目標両足平位置姿勢と候補（Ｐb21_s，θb21_s）とから定まるロボット１の各脚体２の姿勢から決定される。補足すると、前記したように本明細書の実施形態のロボット１では、各脚体２はそれぞれ６自由度を有するので、両足平２２，２２および上体３の位置姿勢が定まれば、各脚体２の各部の位置姿勢も一義的に定まる。従って、第２変位次元補正用モデルの上体質点Ａ１の位置、上体リンクの姿勢角、両足平質点Ａ４，Ａ５の位置が定まれば、各大腿質点Ａ２，Ａ３の位置が一義的に定まる。

次いで、Ｓ２０８に進み、第１変位次元補正用モデルと第２変位次元補正用モデルとの間での全体重心の位置のずれGc_err（以下、モデル間全体重心ずれGc_errという）と、第１変位次元補正用モデルに対する第２変位次元補正用モデルの角運動量積の総和のずれ量L_err（以下、モデル間角運動量積ずれL_errという）とを求める。この処理を以下により具体的に説明する。なお、以下の説明では、第１および第２変位次元補正用モデルの各質点Ａ１〜Ａ５の質量をそれぞれｍi（ｉ＝１，２，…，５）、位置（位置ベクトル）を一般的にＰi1もしくはＰi2（ｉ＝１，２，…，５）で表す。Ｐi1は第１変位次元補正用モデルの質点Ａiの位置、Ｐi2は第２変位次元補正用モデルの質点Ａiの位置を一般的に示す符号である。また、第１および第２変位次元補正用モデルでの上体３（上体リンク）の姿勢角をそれぞれ一般的にθb1、θb2で表す。本実施形態では、θb1は、単純化モデル歩容の目標上体姿勢θｂ（鉛直姿勢）と同一である。

モデル間全体重心ずれGc_errおよびモデル間角運動量積ずれL_errは、それぞれ例えば次式０６，０７により算出される。

Gc_err＝Σ(mi＊(Ｐi2−Ｐi1)) …式０６
L_err＝Σ(mi＊(Ｐi1−Ｑ)＊(Ｐi2−Ｐi1))＋Ｉb＊(θb2−θb1)＋Const …式０７

ここで、これらの式のΣはそれに続く括弧内の部分の、全ての質点Ａi（ｉ＝１，２，…，５）についての総和を意味する。また、式０７の「Const」は、あらかじめ定めた所定値であり、前記条件２における「一定値」（所定値）に相当するものである。また、式０７のＱは、本実施形態では、単純化モデル歩容の目標ＺＭＰの位置と同一である。

これらの式０６，０７において、式０６の右辺は、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置Ｐi1（ｉ＝１，２，…，５）により定まる全体重心の位置と第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置Ｐi2（ｉ＝１，２，…，５）により定まる全体重心の位置とのずれを意味している。従って、モデル間全体重心ずれGc_errの値が０（０ベクトル）もしくはほぼ０であれば、前記条件１が満たされることとなる。

また、式０７の右辺から「Const」を除いた項は、第１変位次元補正用モデルに対する第２変位次元補正用モデルの角運動量積の総和を意味する。別の言い方をすれば、式０７の右辺から「Const」を除いた項は、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置Ｐi1（ｉ＝１，２，…，５）を第２変位次元補正用モデルの各質点Ａ１〜Ａ５の基準位置とし、且つ、第１変位次元補正用モデルの上体３（上体リンク）の姿勢角を、第２変位次元補正用モデルの上体３（上体リンク）の基準姿勢角としたときの、第２変位次元補正用モデルの角運動量積の総和を意味する。

従って、モデル間角運動量積ずれL_errの値が常に０もしくはほぼ０であれば前記条件２が満たされることとなる。

ここで式０７の右辺のΣに続く括弧内の項について補足すると、(Ｐi1−Ｑ)＊(Ｐi2−Ｐi1)は、点Ｑと質点Ａiとを結ぶ線分のベクトルと、第１変位次元補正用モデルの質点Ａiに対する第２変位次元補正用モデルの質点Ａiの位置ずれのベクトルとの外積である。それを視覚的に表現すれば、(Ｐi1−Ｑ)＊(Ｐi2−Ｐi1)は、図１３に示す如く、斜線もしくは網掛けを付した各三角形の面積の２倍の大きさの量に相当する。なお、図１３では、第１および第２変位次元補正用モデルのそれぞれにおける質点Ａi（ｉ＝１，２，…，５）の位置Ｐi1，Ｐi2をＰi1(Ai),Ｐi2(Ai)というように記載している。

また、角運動量積に関する式０７に関しては、これの代わりに、次式０８〜１０のいずれかを用いてもよい。

L_err＝Σ(Ci＊mi＊角度(Pi1_Q_Pi2))＋Ｉb＊(θb2−θb1)＋Const …式０８

L_err＝Σ(mi＊(質点Ａiの水平成分変位＊高さ))
＋Ｉb＊(θb2−θb1)＋Const …式０９

L_err＝Σ(mi＊(質点Ａiの水平成分変位＊高さ)＊C(質点Ａiの高さ))
＋Ｉb＊(θb2−θb1)＋Const …式１０

ここで、式０８中の「角度(Pi1_Q_Pi2)」は、第１変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分と、第２変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分とのなす角度を意味する。また、式０８中の「Ｃi」は所定の係数であり、その値は、Ci＊mi＊角度(Pi1_Q_Pi2)が、両変位次元補正用モデルの質点Ａiと点Ｑとで形成される三角形の面積の２倍にほぼ等しくなるように決定される。また、式０９および１０における「質点Ａiの水平成分変位」は、第１変位次元補正用モデルの質点Ａiと第２変位次元補正用モデルの質点Ａiとの位置ずれ（Ｐi2−Ｐi1）の水平成分を意味し、「高さ」は第１または第２変位次元補正用モデルの質点Ａiの点Ｑに対する相対高さ、すなわち、Ｐi1−ＱまたはＰi2−Ｑの鉛直成分を意味する。また、式１０の「Ｃ（質点Ａiの高さ)」は、第１または第２変位次元補正用モデルの質点Ａiの点Ｑに対する相対高さ（Ｐi1−ＱまたはＰi2−Ｑの鉛直成分）のある関数値を意味する。この場合、関数値Ｃ（質点Ａiの高さ)は、基本的には、質点Ａiの高さが高いほど、値が小さくなるような単調関数であることが好適である。

角運動量積に関する上記式０７〜１０のいずれを用いても、その式の右辺の「Const」を除いた項は、角運動量積の総和にほぼ比例するかもしくは概ね等しいものとなる。なお、式０７〜１０の「Const」は一般には互いに相違する。

補足すると、前記式０７〜１０の右辺のΣに続く各項は、第１変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分と、第２変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分とのなす角度(Pi1_Q_Pi2)に対してほぼ単調に変化する関数となる。

本実施形態では、Ｓ２０８において、前記式（６）のＰi1にＳ２００で求めた第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置を代入すると共に、同式（６）のＰi2に、Ｓ２０６で求めた各質点Ａ１〜Ａ５の位置を代入することで、モデル間重心位置ずれＧc_errが算出される。また、前記式（７）のＰi1，Ｐi2を式（６）と同じにすると共に、θb1にＳ２００で求めた上体姿勢（本実施形態では鉛直姿勢）を代入し、さらに、θb2に第１仮補正上体姿勢の候補の現在値θb21を代入することで、モデル間角運動量積ずれＬc_errが算出される。

上述のようにＳ２０８において、モデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errを求めた後、Ｓ２１０に進んで、Gc_errとL_errとが０近傍の所定の範囲内にあるか否かが判断される。そして、この判断結果がＹＥＳである場合には、Ｓ２１２を経て、後述のＳ２１８に進む。一方、その判断結果がＮＯである場合には、Ｓ２１４に進んで、第１仮補正上体位置姿勢の現在の候補（Ｐb21_s，θb21_s）の近辺に複数の仮候補（Ｐb21_s＋ΔPb21x，θb21_s）、（Ｐb21_s＋ΔPb21z，θb21_s）、（Ｐb21_s，θb21_s＋Δθb21）を決める。ΔPb21x、ΔPb21yはそれぞれ第１仮補正上体位置の候補Ｐb21_sを現在値からＸ軸方向、Ｙ軸方向に微小量変化させるための所定値であり、Δθb21は第１仮補正上体姿勢の候補θb21をＹ軸まわりに微小量変化させるための所定値である。そして、これらの各仮候補に対して、前記Ｓ２０６、Ｓ２０８と同じ処理を実行して、モデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errとを求める。このＳ２１４の処理は、第１仮補正上体位置姿勢の候補（Ｐb21_s，θb21_s）を現在値から変化させたときのGc_errとL_errとの変化の度合いを観測するための処理である。

次いで、Ｓ２１６に進んで、Ｓ２０８およびＳ２１４で求めたGc_err、L_errを基に、それらの値が０に近づくように第１仮補正上体位置姿勢の新たな候補を決定し、それを（Ｐb21_s，θb21_s）に代入する。新たな候補は、例えばヤコビアン（感度マトリクス）を用いて決定される。そして、Ｓ２０６からの処理が再び実行される。

以上のようにして、Ｓ２０６〜Ｓ２１６のループ処理によって、Gc_errとL_errとが０近傍の所定の範囲内に収まるような第１仮補正上体位置姿勢、換言すれば、前記条件１，２を満たすような第１仮補正上体位置姿勢が探索的に求められる。補足すると、Ｓ２１０の条件が満たされたときの第１仮補正上体位置姿勢と、このＳ２１０の直前のＳ２０６で求められた各質点の位置との組が、前記第１要素配置に相当するものとなる。また、この第１要素配置は、本発明の第１発明における「第２の配置」に相当するものである。

そして、Ｓ２１０の判断結果がＹＥＳになると、Ｓ２１２を経てＳ２１８に進み、現在の（Ｐb21_s，θb21_s）が今回時刻ｔにおける第１仮補正上体位置姿勢（Ｐb21，θb21）として決定される。これにより、単純化モデル歩容の上体位置姿勢を前記条件１，２を満たすように補正してなる歩容（以下、第１仮補正歩容ということがある）が得られることとなる。この第１仮補正歩容は、単純化モデル歩容のうちの、目標上体位置姿勢のみを補正したものであり、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分など、目標歩容の他の構成要素は単純化モデル歩容と同一である。また、この第１仮補正歩容の運動は、Ｓ２１０の条件が満たされたときの第２変位次元補正用モデルの配置（前記第１要素配置）から、前記幾何学的拘束条件（２）に従って決定される瞬時目標運動と同じである。また、この第１仮補正歩容の運動は、本発明の第１発明における第１の仮補正瞬時目標運動に相当する。従って、Ｓ１００の処理によって、本発明の第１発明おける第１仮補正運動決定手段が構成されていることとなる。

以上がＳ１００のサブルーチン処理である。

図１０のフローチャートの説明に戻って、上述したようにＳ１００の処理を実行した後、Ｓ１０２の処理が実行される。このＳ１０２の処理では、第２変位次元補正用モデルでの上体姿勢を単純化モデル歩容の瞬時値（今回時刻ｔでの瞬時値）における上体姿勢と同一にして、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の角運動量積に関する前記条件２を満足するように第２仮補正上体位置姿勢（Pb22，θb22）（第２仮補正上体位置Pb22と第２仮補正上体姿勢θb22との組）が決定される。より正確に言えば、第２変位次元補正用モデルでの上体姿勢を単純化モデル歩容の上体姿勢と同一にした上で、両モデル間での前記条件２を満足するように第２変位次元補正用モデルの前記第２要素配置が決定され、その第２要素配置における上体質点Ａ１の位置および上体リンクの姿勢に対応するロボット１の上体位置姿勢が前記第２仮補正上体位置姿勢(Pb22，θb22)として決定される。なお、このＳ１０２の処理では、第２仮補正上体姿勢θb22は、単純化モデル歩容の上体姿勢と同一にされるので、該Ｓ１０２の処理は、実質的には、条件２を満足するように第２仮補正上体位置Ｐb22を決定する処理であると言える。

このＳ１０２の処理は、図１２のサブルーチン処理によって実行される。以下説明すると、まず、Ｓ３００において、今回時刻（現在時刻）ｔでの単純化モデル歩容の瞬時値（目標上体位置姿勢などの目標運動の瞬時値）を基に、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角とを求める。この処理は、図１１のＳ２００の処理と同じであり、求められる第１変位次元補正用モデルの各要素の配置もＳ２００の処理で求められるものと同一である。従って、Ｓ２００で求めた第１変位次元補正用モデルの各要素の配置を、そのまま図１２のサブルーチン処理で用いるようにすれば、Ｓ３００の処理は省略してもよい。

次いで、Ｓ３０２からの処理が実行され、第１変位次元補正用モデルに対して前記条件２を満たすような第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角との組、すなわち、第２変位次元補正用モデルの前記第２要素配置が探索的に決定され、その第２要素配置における上体質点Ａ１および上体リンクの姿勢に対応するロボット１の上体位置姿勢が第２仮補正上体位置姿勢（Ｐb22，θb22）として決定される。但し、この場合、第２仮補正上体姿勢θb22は、単純化モデル歩容の瞬時値（今回時刻ｔの瞬時値）と同一とされる。従って、Ｓ３０２からの処理は、実質的には、第２仮補正上体位置Ｐb22を前記条件２を満たすように決定する処理である。

さらに詳細には、まず、Ｓ３０２において、第２仮補正上体位置姿勢の初期候補（Ｐb22_s，θb22_s）を決定する。すなわち、初期候補（Ｐb22_s，θb22_s）のうちの、Ｐb22_sを、今回時刻ｔでの単純化モデル歩容の上体位置Ｐbと、前回時刻ｔ−Δｔでの単純化モデル歩容の上体位置の値Ｐb_pと、前回時刻ｔ−Δｔでの第２仮補正上体位置の値Ｐb22_pとから次式０４ｂにより決定すると共に、今回時刻ｔでの単純化モデル歩容の上体姿勢θｂから次式０５ｂにより決定する。

Ｐb22_s＝Ｐb＋（Ｐb22_p−Ｐb_p） …式０４ｂ
θb21_s＝θb …式０５ｂ

従って、第２仮補正上体位置の初期候補Ｐb22_sは、前記Ｓ２０２で先に説明したように決定される第１仮補正上体位置の初期候補Ｐb21_sと同様に決定される一方、第２仮補正上体姿勢の初期候補θb22_sは、単純化モデル歩容の上体姿勢と同一とされる。

次いで、Ｓ３０４を経て、Ｓ３０６〜Ｓ３１６のループ処理を実行する。Ｓ３０６では、第２仮補正上体位置姿勢の現在の候補（Ｐb22_s，θb22_s）と、今回時刻ｔでの単純化モデル歩容の目標両足平位置姿勢とを基に、第２変位次元補正モデルでの各質点Ａ１〜Ａ５の位置を求める。この処理は、候補（Ｐb22_s，θb22_s）の値以外は、前記Ｓ２０６と同じであり、今回時刻ｔでの単純化モデル歩容の瞬時運動のうちの、上体位置姿勢のみを候補（Ｐb22_s，θb22_s）で置き換えた瞬時運動から、前記幾何学的拘束条件（２）に従って、第２変位次元補正用モデルにおける各質点Ａ１〜Ａ５が求められる。

次いで、Ｓ３０８に進み、第１変位次元補正用モデルと第２変位次元補正用モデルとの間でのモデル間角運動量積ずれL_errを求める。この求め方は、前記Ｓ２０８でモデル間角運動量積ずれを求める場合と同様である。すなわち、前記式（７）のＰi1にＳ３００で求めた第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置を代入すると共に、同式（７）のＰi2に、Ｓ３０６で求めた各質点Ａ１〜Ａ５の位置を代入し、さらに、式（７）のθb2に第２仮補正上体姿勢の候補の現在値θb22（＝今回時刻ｔのθｂ）を代入することで、モデル間角運動量積ずれＬc_errが算出される。なお、式（７）の代わりに前記式（８）〜（１０）を用いてもよい。

次いで、Ｓ３１０に進んで、L_errが０近傍の所定の範囲内にあるか否かが判断される。そして、この判断結果がＹＥＳである場合には、Ｓ３１２を経て、後述のＳ３１８に進む。一方、その判断結果がＮＯである場合には、Ｓ３１４に進んで、第２仮補正上体位置姿勢の現在の候補（Ｐb22_s，θb22_s）の近辺に、該候補の上体位置Ｐb22_sのみを微小量変化させた複数の仮候補（Ｐb22_s＋ΔPb22x，θb22_s）、（Ｐb22_s＋ΔPb22y，θb22_s）を決める。ΔPb22x、ΔPb22yはそれぞれ第１仮補正上体位置の候補Ｐb22_sを現在値からＸ軸方向、Ｙ軸方向に微小量変化させるための所定値である。そして、これらの各仮候補に対して、前記Ｓ３０６、Ｓ３０８と同じ処理を実行して、モデル間角運動量積ずれL_errとを求める。このＳ３１４の処理は、第２仮補正上体位置姿勢の候補（Ｐb22_s，θb22_s）のうちの上体位置の候補だけを現在値から変化させたときのL_errとの変化の度合いを観測するための処理である。

次いで、Ｓ３１６に進んで、Ｓ３０８およびＳ３１４で求めたL_errを基に、その値が０に近づくように第２仮補正上体位置姿勢の新たな候補を決定し、それを（Ｐb22_s，θb22_s）に代入する。新たな候補は、例えばヤコビアン（感度マトリクス）を用いて決定される。そして、Ｓ３０６からの処理が再び実行される。

以上のようにして、Ｓ３０６〜Ｓ３１６のループ処理によって、第２仮補正上体姿勢を単純化モデル歩容の上体姿勢と同じにした状態で、L_errが０近傍の所定の範囲内に収まるような第２仮補正上体位置、換言すれば、前記条件２を満たすような第２仮補正上体位置が探索的に求められる。補足すると、Ｓ３１０の条件が満たされたときの第２仮補正上体位置姿勢と、このＳ３１０の直前のＳ３０６で求められた各質点の位置との組が、前記第２要素配置に相当するものとなる。また、この第２要素配置は、本発明の第１発明における「第３の配置」に相当するものである。

そして、Ｓ３１０の判断結果がＹＥＳになると、Ｓ３１２を経てＳ３１８に進み、現在の（Ｐb22_s，θb22_s）が今回時刻ｔにおける第２仮補正上体位置姿勢（Ｐb22，θb22）として決定される。これにより、単純化モデル歩容の上体位置を前記条件２を満たすように補正してなる歩容（以下、第２仮補正歩容ということがある）が得られることとなる。この第２仮補正歩容は、単純化モデル歩容のうちの、目標上体位置のみを補正したものであり、目標上体姿勢、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分など、目標歩容の他の構成要素は単純化モデル歩容と同一である。また、この第２仮補正歩容は、Ｓ３１０の条件が満たされたときの第２変位次元補正用モデルの配置（前記第２要素配置）から、前記幾何学的拘束条件（２）に従って決定される瞬時目標歩容と同じである。また、この第２仮補正歩容の運動は、本発明の第１発明における第２の仮補正瞬時目標運動に相当する。従って、Ｓ１０２の処理によって、本発明の第１発明における第２仮補正運動決定手段が構成されていることとなる。

以上がＳ１０２のサブルーチン処理である。

図１０のフローチャートの説明に戻って、上述したようにＳ１０２の処理を実行した後、Ｓ１０４の処理が実行される。この処理では、前記第１仮補正上体位置姿勢および第２仮補正上体位置姿勢から最終的に変位次元補正上体位置姿勢を決定するための重みw1に関する基本値w1_aim（以下、重み基本値w1_aimという）が決定される。具体的には、今回歩容に係るロボット１の動作モードが前記走行モードである場合には、w1_aim＝１、前記低摩擦床面歩行モードである場合には、w1_aim＝0.5、これら以外の動作モード（すなわち前記通常モード）である場合には、w1_aim＝０とされる。

次いで、Ｓ１０６において、重みw1の値を現在値（前回時刻ｔ−Δｔに決定された値）から、今回時刻ｔのＳ１０４で上記の如く決定した重み基本値w1_aimに徐々に近づけるように決定する。具体的には、例えば、重みw1の現在値と重み基本値w1_aimとの差に所定の係数値（０＜係数値＜１）を乗算したものを、重みw1の現在値に加えることで、今回時刻ｔでの重みw1を決定する。このようにすることで、重みw1は、重み基本値w1_aimに応答遅れを伴いつつ徐々に追従するように決定される。これは、ロボット１の動作モードが変わって、重み基本値w1_aimが変わったときに、重みw1の値の急変（不連続な変化）を避けるための処理である。なお、重みw1は、重み基本値w1_aimが定常的に同じ値に維持されれば、最終的にその重み基本値w1_aimと同じ値に維持されることとなる。

次いで、Ｓ１０８において、他のもう一つの重みw2が決定される。この重みw2は、先に決定した重みw1との和が１になるように決定される。すなわち、重みw2は、次式１１により決定される。

w2＝１−w1 …式１１

次いで、Ｓ１１０において、今回時刻ｔの変位次元補正上体位置姿勢(Pb2，θb2)が最終的に、次式により決定される。

Ｐb2＝w1＊Ｐb21＋w2＊Ｐb22 …式１２
θb2＝w1＊θb21＋ｗ2＊θb22 …式１３
すなわち、変位次元補正上体位置Ｐb2は、今回時刻ｔで、前述の如く求めた第１仮補正上体位置Ｐb21、第２仮補正上体位置Ｐb22にそれぞれ重みw1，w2を乗じたものの総和、換言すれば、第１仮補正上体位置Ｐb21、第２仮補正上体位置Ｐb22の重み付き平均値として求められる。また、変位次元補正上体姿勢θb2は、第１仮補正上体姿勢θb21、第２仮補正上体姿勢θb22にそれぞれ重みｗ1，ｗ2を乗じたものの総和、換言すれば、第１仮補正上体姿勢θb21および第２仮補正上体姿勢θb22の重み付き平均値として求められる。

以上のようにして、変位次元歩容補正サブルーチンの処理が実行されて、変位次元補正上体位置姿勢が求められる。これにより、単純化モデル歩容の上体位置姿勢を補正してなる目標歩容（以下、変位次元補正歩容ということがある）が得られることとなる。この変位次元補正歩容は、単純化モデル歩容のうちの、目標上体位置姿勢のみを補正したものであり、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分など、目標歩容の他の構成要素は単純化モデル歩容と同一である。

補足すると、第１実施形態における上記変位次元補正歩容の運動は、本発明の第１発明における補正後瞬時目標運動に相当する。従って、Ｓ０２４の変位次元歩容補正サブルーチンの処理によって、該第１発明における目標運動補正手段が構成されることとなる。

ここで、単純化モデル歩容と、この単純化モデル歩容の上体位置姿勢を前記第１仮補正上体位置姿勢に補正してなる歩容（前記第１仮補正歩容）と、単純化モデル歩容の上体位置姿勢を前記第２仮補正上体位置姿勢に補正してなる歩容（前記第２仮補正歩容）と、変位次元補正歩容との関係について図１４〜図１６を参照して補足説明をしておく。図１４は第２変位次元補正用モデルの各質点Ａi（ｉ＝１，２，…，５）の位置および上体３（上体リンク）の姿勢角を単純化モデル歩容通りに決定した場合（すなわち単純化モデル歩容を前記Ｓ０２４で補正しない場合）における第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、該単純化モデル歩容に対応して定まる第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この場合の第２変位次元補正用モデルの各質点Ａiの位置および上体３の姿勢角、すなわち、第２変位次元補正用モデルの要素の配置は、別の言い方をすれば、単純化モデル歩容の瞬時運動から、前記幾何学的拘束条件（２）に従って決定されたものと言える。なお、前記図１４では、単純化モデル歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi2'(Ai)で表している。

また、図１５は前記第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この図１５の第２変位次元補正用モデルの各要素の配置は、図１４で想定している単純化モデル歩容の瞬時値に対応して、前記図１０のＳ１００で最終的に決定される第２変位次元補正用モデルの前記第１要素配置である。この配置は、前記第１仮補正歩容から、前記幾何学的拘束条件（２）に従って定まるものと同一である。なお、図１５では、第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi21(Ai)で表している。また、図１５に示す第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角は、図１４と同じである。

また、図１６は前記第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この図１６の第２変位次元補正用モデルの各要素の配置は、図１４で想定している単純化モデル歩容の瞬時値に対応して、前記図１０のＳ１０２で最終的に決定される第２変位次元補正用モデルの前記第２要素配置である。この配置は、前記第２仮補正歩容から、前記幾何学的拘束条件（２）に従って定まるものと同一である。なお、図１６では、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi22(Ai)で表している。また、図１６に示す第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角は、図１４と同じである。

以降の説明では、一般に、第１変位次元補正用モデルのある任意の質点と、これに対応する第２変位次元補正用モデルの質点との位置ずれを並進加速度とみなしたときのその並進加速度を、その質点のモデル間擬似並進加速度という。また、第１変位次元補正用モデルのイナーシャをもつあるリンクと、これに対応する第２変位次元補正用モデルのリンクの姿勢角のずれを角加速度とみなしたときのその角加速度を、そのリンクのモデル間擬似角加速度という。

図１４に示す例において、単純化モデル歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置Ｐi2'では、両脚体２，２の質点Ａ２〜Ａ５のうち、支持脚側の質点Ａ２，Ａ４は、第１変位次元補正用モデルに対して若干、ロボット１の前方側に位置ずれする。また、第２変位次元補正用モデルの遊脚側の質点Ａ３，Ａ５は、第１変位次元補正用モデルに対して比較的大きくロボット１の後方側に位置ずれする。このため、第２変位次元補正用モデルでのロボット１の全体重心が第１変位次元補正用モデルでのロボット１の全体重心（これは本実施形態では単純化モデル歩容でのロボット１の全体重心に一致する）よりもロボット１の後方側（Ｘ軸の負方向）に偏る。別の言い方をすれば、両脚体２，２の各質点Ａ２〜Ａ５のモデル間擬似並進加速度によって各質点Ａ２〜Ａ５が発生する慣性力の並進力成分（＝各質点Ａ２〜Ａ５の質量＊モデル間擬似並進加速度）の総和がロボット１の前方側に比較的大きなものとなる。また、各質点Ａ２〜Ａ５のモデル間擬似並進加速度によって各質点Ａ２〜Ａ５が発生する慣性力が目標ＺＭＰまわりに作用するモーメントが、ロボット１の前傾側に比較的大きなものとなる。なお、本実施形態では、単純化モデル歩容の上体姿勢と第１変位次元補正用モデルの上体姿勢とは同一であるので、図１４の例では、上体質点Ａ１のモデル間擬似並進加速度は０であり、また、上体リンクのモデル間擬似角加速度も０である。

これに対して、前記第１仮補正歩容に対応する第２変位次元補正用モデルの要素の配置（第１要素配置）、すなわち、前記図１０のＳ１００で最終的に決定される第２変位次元補正用モデルの各質点Ａｉの位置および上体リンクの姿勢角により定まる第２変位次元補正用モデルの要素の配置にあっては、図１５に見られるように、両脚体２，２の質点Ａ２〜Ａ５の重心の上記の偏りを補償するようにして、第２変位次元補正用モデルの上体質点Ａ１の位置が、第１変位次元補正用モデルの上体質点Ａ１よりも前方側に決定され、換言すれば、第１仮補正上体位置が単純化モデル歩容よりも前方側に補正される。同時に、第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置並びにイナーシャをもつ上体３の姿勢は、前記した角運動量積の総和がある一定値になるように決定される。図示の例では、第２変位次元補正用モデルでの上体姿勢（実線で示す上体３の姿勢）は、単純化モデル歩容の上体姿勢（破線で示す上体３の姿勢）に対して、角度θb21−θbだけ後傾している。

このため、第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置および上体リンクの姿勢角では、各質点Ａiのモデル間擬似並進加速度に伴う慣性力の並進力成分の総和が図１４に示した場合よりも小さくなって、ほぼ０になると共に、該慣性力が目標ＺＭＰまわりに作用するモーメントの総和も図１４に示した場合よりも所定の値（前記条件２の「一定値」に相当する値）に近づく。

これにより、第１仮補正歩容は、単純化モデル歩容では考慮されていない各脚体の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力（並進床反力および床反力モーメント、より厳密には、単純化モデル歩容によって単純化モデルが発生する床反力）と同じような床反力が発生するようにロボット１の目標運動（より詳しくは目標上体位置および目標上体姿勢）を補正したものとなる。このため、第１仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力が、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力よりも、目標床反力（＝単純化モデル歩容の床反力）により近いものとなる。つまり、第１仮補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的精度が単純化モデル歩容の運動と床反力の間の動力学的精度よりも高まることとなる。

一方、前記第２仮補正歩容に対応する第２変位次元補正用モデルの要素の配置（第２要素配置）、すなわち、前記図１０のＳ１０２で最終的に決定される第２変位次元補正用モデルの各質点Ａｉの位置および上体リンクの姿勢角により定まる第２変位次元補正用モデルの要素の配置にあっては、図１６に見られるように、上体姿勢を単純化モデル歩容の瞬時上体姿勢と同じに維持したまま、第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置が、前記した角運動量積の総和がある一定値になるように決定される。

このため、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置および上体リンクの姿勢角では、各質点Ａiのモデル間擬似並進加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントの総和が図１４に示した場合よりも所定の値（前記条件２の「一定値」に相当する値）に近づく。

これにより、第２仮補正歩容は、ロボット１の目標上体姿勢を単純化モデル歩容の上体姿勢と同じに維持しながら、単純化モデル歩容では考慮されていない各脚体の運動に伴う慣性力の影響を補償して、単純化モデル歩容の床反力モーメントと同じような床反力モーメントが発生するように、ロボット１の目標上体位置を補正したものとなる。このような第２仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントが、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントよりも、目標床反力モーメント（＝単純化モデル歩容の床反力モーメント）により近いものとなる。つまり、第２仮補正歩容の運動と床反力モーメントとの間の動力学的精度が単純化モデル歩容の運動と床反力モーメントとの間の動力学的精度よりも高まることとなる。但し、第２仮補正歩容を決定する（第２変位次元補正用モデルの第２要素配置を決定する）ときには、第１および第２変位次元補正用モデルの間の全体重心のずれは考慮していないので、第２仮補正歩容の運動と並進床反力との間の動力学的な精度は、必ずしも単純化モデル歩容の運動と並進床反力との間の動力学的な精度よりも高まるとは限らない。

上述したような第１仮補正歩容の上体位置姿勢と第２仮補正歩容の上体位置姿勢とから、Ｓ０２４の変位次元歩容補正サブルーチンで最終的に決定される変位次元補正歩容の上体位置姿勢、すなわち変位次元補正上体位置姿勢は、前記式１２および式１３によって決定される。この場合、ロボット１の動作モードが定常的に前記通常モード（走行モードおよび低摩擦床面歩行モード以外の動作モードで、摩擦係数の高い通常的な床面をロボット１の歩行などを行う動作モード）であるときには（このときw1＝０，w2＝１）、変位次元補正上体姿勢が単純化モデルで想定している姿勢（基準姿勢）、具体的には本実施形態では鉛直姿勢に維持されると共に、変位次元補正上体位置は、第２仮補正歩容の上体位置（第２仮補正上体位置）と同一になる。このため、変位次元補正歩容は、上体姿勢を変動させない（上体姿勢を補正しない）ようにしつつ、単純化モデル歩容の上体位置を補正して、変位次元補正歩容の運動と床反力モーメントとの間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。つまり、変位次元補正歩容の運動は、これに従って実際のロボット１を動作させたときに、単純化モデル歩容の運動に従って実際のロボット１を動作させたときよりも精度よく目標ＺＭＰを満足するものとなる。

また、ロボット１の動作モードが定常的に走行モードであるときには（このときw1＝１，w2＝０）、変位次元補正上体姿勢および変位次元補正上体位置が、それぞれ第１仮補正歩容の上体姿勢（第１仮補正上体姿勢）と同一になる。このため、変位次元補正歩容は、単純化モデル歩容の上体位置および上体姿勢の両者を補正して、変位次元補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。補足すると、走行モードでは、目標床反力鉛直成分が０に近い時期（ロボット１と床面との間の摩擦力が微小となる時期）が必然的に存在することとなる。そして、そのような時期では、一般には、上体３の並進加速度をほとんど操作できないことから、上体姿勢を一定に維持したままでは目標ＺＭＰを満足することはできず、上体姿勢の変化が必要となる。このため、本実施形態では、ロボット１の動作モードが定常的に走行モードであるときには、変位次元補正歩容の動力学的な精度を高めることを重視し、変位次元補正上体を姿勢が第１仮補正上体姿勢と同一になるようにしている。

また、ロボット１の動作モードが定常的に低摩擦床面歩行モードであるときには（このときw1＝w2＝0.5）、変位次元補正上体姿勢は、第１仮補正歩容の上体姿勢角の１／２の姿勢角（第１仮補正歩容の上体姿勢角よりも鉛直方向に対する傾き角が小さい上体姿勢角、より厳密には、第１仮補正歩容の上体姿勢角の１／２と第２仮補正歩容の上体姿勢角の１／２との和）とされ、変位次元補正上体位置は、第１仮補正歩容の上体位置と第２仮補正歩容の上体位置との中間位置とされる。このため、変位次元補正歩容は、上体姿勢の変動を抑制しつつ、変位次元補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的な精度を単純化モデル歩容よりも高めるように単純化モデル歩容の上体位置および上体姿勢の両者を補正したものとなる。この場合、変位次元補正上体位置を、第１仮補正歩容の上体位置と第２仮補正歩容の上体位置との中間位置とすることで、変位次元補正歩容の運動によってロボット１に発生する並進慣性力水平成分が過大になるのを防止することができる。同時に、変位次元補正上体姿勢を、第１仮補正歩容の上体姿勢角よりも小さい傾き角にすることで、上体姿勢の変動を抑制しつつ、目標ＺＭＰを満足することもできる。従って、ロボット１の動作モードが定常的に低摩擦床面歩行モードであるときには、変位次元補正歩容は、単純化モデル歩容よりも動力学的精度を高めつつ、且つ、上体姿勢の変動を極力抑えつつ、且つ、ロボット１のスリップを防止しつつ、ロボット１の安定な動作を可能とする歩容となる。

なお、ロボット１の動作モードが切り替る際には、前記図１０のＳ１０６の処理によって、重みw1の値を（ひいては、重みw2も）徐々に変化させるので、変位次元補正上体姿勢や変位次元補正上体位置が急変することはない。

図９の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、Ｓ０２６に進んで、ロボット１のスピン（鉛直軸まわりの回転）をキャンセルするための腕体の動作が決定される。この処理は、ロボット１の腕を振らずに目標歩容通りにロボット１を運動させた場合に目標ＺＭＰまわりに発生する床反力モーメントの鉛直成分と逆向きの床反力モーメントを腕体の腕振り（両腕を前後逆方向に振る運動）によって発生させるように腕体の姿勢を決定するものであり、前記公報文献１の図１３のＳ０３４と同様に行われる。その詳細は、同公報文献１に記載されているので、ここではさらなる説明を省略する。

次いで、Ｓ０２８に進んで、フルモデル補正用（前記フルモデル補正部１００ｅの処理用）のＺＭＰ許容範囲の瞬時値と、床反力水平成分許容範囲の瞬時値とが決定される。これは、前記目標瞬時値発生部１００ｂで実行される処理であり、前記Ｓ０２０で決定した今回歩容パラメータのうちのＺＭＰ許容範囲および床反力水平成分許容範囲を規定する歩容パラメータに基づいて決定される。

次いで、Ｓ０３０に進んで、フルモデルを用いた補正歩容を発生する。この処理は、前記フルモデル補正部１００ｅにより実行される処理である。この場合、この処理は、前記公報文献１の図１３のＳ０３８の処理と同一であり、同公報文献１に記載されたとおりに実行される。従って、本明細書では詳細な説明は省略する。この処理により、目標上体位置姿勢（前記変位次元補正歩容の上体位置姿勢）をさらに修正してなる補正目標上体位置姿勢と補正目標床反力モーメントとが決定される。

なお、前記フルモデル補正部１００ｅで用いるフルモデルは、例えば図１７に示す如く、ロボット１の上体３、各脚体２の股関節、大腿リンク、下腿リンク、足首関節、足平２２にそれぞれ質点をもち、また、上体３（上体リンク）にイナーシャＩｂを持つような多質点モデルである。この場合、上体３以外のリンクにもイナーシャを設定するようにしてもよい。

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

次に図４を参照して複合コンプアライアンス制御装置１０１の動作を説明しておく。なお、複合コンプライアンス制御装置１０１の動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されているので、本明細書では概略的な説明にとどめる。歩容生成装置１００において、上記したように生成された目標歩容のうち、補正目標上体位置姿勢（軌道）、目標腕姿勢（軌道）が、ロボット幾何学モデル（逆キネマティクス演算部）１０２に送出される。

また、目標足平位置姿勢（軌道）、目標ＺＭＰ軌道（目標全床反力中心点軌道）、および目標全床反力（軌道）（補正目標床反力モーメントと目標床反力鉛直成分）は、複合コンプライアンス動作決定部１０４に送られると共に、目標床反力分配器１０６にも送られる。そして、目標床反力分配器１０６で、床反力は各足平２２に分配され、目標各足平床反力中心点および目標各足平床反力が決定される。この決定された目標各足平床反力中心点および目標各足平床反力は複合コンプライアンス動作決定部１０４に送られる。

複合コンプライアンス動作決定部１０４から、機構変形補償付き修正目標足平位置姿勢（軌道）がロボット幾何学モデル１０２に送られる。なお、機構変形補償付き修正目標足平位置姿勢は、各脚体２のコンプライアンス機構７２の変形を考慮しつつ、６軸力センサ５０により検出される実際の床反力を目標床反力に近づけるように各足平２２の目標足平位置姿勢を補正したものを意味する。ロボット幾何学モデル１０２は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）を入力されると、それらを満足する脚体２，２の１２個の関節の関節変位指令（値）を算出して変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。また、ロボット幾何学モデル１０２は、目標腕姿勢を満足する腕関節の変位指令（値）を算出して変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令（値）を目標値としてロボット１の腕体の１２個の関節の変位を追従制御する。

ロボット１に生じた床反力（詳しくは実各足床反力）は６軸力センサ５０によって検出される。その検出値は前記複合コンプライアンス動作決定部１０４に送られる。また、ロボット１に生じた姿勢傾斜偏差θerrx，θerry（詳しくは目標上体姿勢角に対する実上体姿勢角の偏差で、ロール方向（Ｘ軸回り）の姿勢角偏差がθerrxであり、ピッチ方向（Ｙ軸回り）の姿勢角偏差がθerryである）が姿勢センサ５４を介して検出され、その検出値は姿勢安定化制御演算部１１２に送られる。この姿勢安定化制御演算部１１２で、ロボット１の上体姿勢角を目標上体姿勢角に復元するための目標全床反力中心点（目標ＺＭＰ）まわり補償全床反力モーメントが算出されて複合コンプライアンス動作決定部１０４に送られる。複合コンプライアンス動作決定部１０４は、入力値に基づいて目標床反力を修正する。具体的には、目標全床反力中心点（目標ＺＭＰ）回りに補償全床反力モーメント、あるいは、補償全床反力モーメントと補正目標床反力モーメントとの和が作用するように目標床反力を修正する。

複合コンプライアンス動作決定部１０４は、修正された目標床反力に、センサ検出値などから算出される実ロボットの状態および床反力を一致させようと上記機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすべての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の２乗）の重み付き平均が最小になるように制御する。これにより、実際の足平位置姿勢と全床反力とが目標足平位置姿勢と目標全床反力とに概ね従うように制御される。

［第２実施形態］
次に、本発明の第２実施形態を図１８〜図２２を参照しつつ説明する。本実施形態は、ロボット１および制御ユニット６０の構成は第１実施形態と同一で、単純化モデルおよび第１変位次元補正用モデルと、歩容生成装置１００の処理の一部とが第１実施形態と相違するものである。従って、本実施形態の説明では、第１実施形態と同一部分については第１実施形態と同じ符号および図面を用い、詳細な説明を省略する。なお、第２実施形態は、本発明の第１、３、４、７〜１０、１３〜１５発明の一実施形態である。

図１８は、本実施形態での単純化モデル（動力学モデル）の構造を示し、図１９は本実施形態での第１変位次元補正用モデルの構造を示している。

図１８に示す本実施形態の単純化モデルは、ロボット１の各脚体２（詳しくは各脚体２の足平２２）にそれぞれ対応する２つの足平質点２m2，２m2、及び上体３に対応する上体質点３m2からなる３質点と、イナーシャＪがあって質量のないフライホイールＦＨとから構成されるモデルであり、前記公報文献１の図１１に示したモデルと同一である。従って、本明細書での詳細な説明は省略するが、その概要は次の通りである。

すなわち、この単純化モデルでは、各足平質点２m2，２m2の動力学（運動と床反力との関係）、並びに上体質点３m2及びフライホイールＦＨの動力学が相互に非干渉に構成されると共に、ロボット１全体の動力学は、それらの線形結合で表される。また、フライホイールＦＨの回転運動によって発生する床反力は、上体３の姿勢角の回転運動（並進床反力を変化させずに、床反力モーメントだけを変化させる回転運動）によって発生する床反力に対応する。なお、上体質点３m2は、上体３の位置姿勢に対応して一義的に定まる点（上体３に任意に固定されたローカル座標系上でのある固定点）に設定されており、各脚質点２m2は、各脚体２の足平２２の位置姿勢に対応して一義的に定まる点（足平２２に任意に固定されたローカル座標系上でのある固定点）に設定されている。また、各質点２m2，２m2，３m2の質量の総和は、ロボット１の総質量と同一である。上体質点３m2の質量は、上体３の質量のほか、両腕体の質量を含んでいる。

この単純化モデルの動力学を記述する式（運動方程式）は、以下の式１４〜１６により表される。但し、本明細書の理解の便宜上、ここではサジタルプレーン（前後軸（Ｘ軸）と鉛直軸（Ｚ軸）を含む平面）での運動方程式のみを記述し、ラテラルプレーン（左右軸（Ｙ軸）と鉛直軸（Ｚ軸）を含む平面）での運動方程式は省略する。また、式１４〜１６の変数は以下のように定義する。
Ｚsup：支持脚足平質点鉛直位置、Ｚswg：遊脚足平質点鉛直位置、Ｚb：上体質点鉛直位置、Ｘsup：支持脚足平質点水平位置、Ｘswg：遊脚足平質点水平位置、Ｘb：上体質点水平位置、θby：鉛直方向に対するＹ軸回りの上体姿勢角、mb：上体質点質量、msup：支持脚足平質点質量、mswg：遊脚足平質点質量、Ｊ：フライホイールの慣性モーメント、Fx：床反力水平成分、Fz：床反力鉛直成分、My：目標ＺＭＰまわりの床反力モーメント（詳しくは床反力モーメントの左右軸（Ｙ軸）まわり成分）

Ｆz＝mb＊(g＋d2Zb/dt2)＋msup＊(g＋d2Zsup/dt2)
＋mswg＊(g＋d2Zswg/dt2) ……式１４
Ｆx＝mb＊d2Xb/dt2＋msup＊d2Xsup/dt2＋mswg＊d2Xswg/dt2 ……式１５
Ｍy＝−mb＊(Xb−Xzmp)＊(g＋d2Zb/dt2)＋mb＊(Zb−Zzmp)＊(d2Xb/dt2)
−msup＊(Xsup−Xzmp)＊(g＋d2Zsup/dt2)
＋msup＊(Zsup−Zzmp)＊(d2Xsup/dt2)
−mswg＊(Xswg−Xzmp)＊(g＋d2Zswg/dt2)
−mswg＊(Zswg−Zzmp)＊(d2Xswg/dt2)＋Ｊ＊d2θby/dt2
……式１６

かかる単純化モデルを用いる第２実施形態では、後述する如く、前記公報文献１と全く同様に、目標ＺＭＰを満足する単純化モデル歩容が生成される。

補足すると、この第２実施形態の単純化モデルは、各脚体２の膝関節の曲げ動作に伴う該膝関節の近傍部位の運動によって発生する慣性力は無視されている。つまり、第２実施形態の単純化モデルは、各脚体２の膝関節の曲げ動作に伴う該膝関節の近傍部位の運動によって発生する慣性力を０と見なして構築された動力学モデルであると言える。

次に、図１９を参照して本実施形態の第１変位次元補正用モデルを説明すると、このモデルは、上体３、各脚体２の大腿リンク、足平２２にそれぞれ対応する上体質点Ａ１、大腿質点Ａ２，Ａ３、足平質点Ａ４，Ａ５からなる５質点モデルである。また、ロボット１の上体３（上体リンク）が、上体質点Ａ１のまわりにイナーシャ（慣性モーメント）Ｉｂをもつものとされている。つまり、本実施形態の第１変位次元補正用モデルは、第１実施形態の第１および第２変位次元補正用モデルと同様、質点Ａ１〜Ａ５とイナーシャをもつ上体リンクとを要素として構成されている。

この場合、上体質点Ａ１、各足平質点Ａ４，Ａ５は、前記第１実施形態の第１もしくは第２変位次元補正用モデルのものと同様、対応する部位（上体３、足平２２）の位置姿勢に対応して一義的に定まる点（その対応する部位に任意に固定設定したローカル座標系でのある固定点）に設定されている。なお、上体質点Ａ１、各足平質点Ａ４，Ａ５および各大腿質点Ａ２，Ａ３の質量の総和は、ロボット１の総質量ｍtotalと一致する。また、上体質点Ａ１の質量は、上体３の質量のほか、両腕体５，５および頭部４の質量を含んでいる。

そして、本実施形態においても第１変位次元補正用モデルの要素の配置には、ある幾何学的な拘束条件が設定されている。具体的には、この第１変位次元補正用モデルでは、ロボット１の各脚体２の膝関節を、該脚体２の足首関節の中心と股関節の中心とを結ぶ方向にのみ伸縮する直動型（テレスコピック型）の関節とみなしており、各大腿質点Ａ２，Ａ３は、対応する脚体２の足首関節の中心と股関節の中心とを結ぶ線分の内分点に設定されている。その内分点は、該内分点から足首関節の中心までの距離と股関節の中心までの距離との比率が所定の比率となるような点であり、各脚体２を直線状に延ばしたときの膝関節の近傍の点（例えば、該膝関節の中心よりも若干、大腿リンク２４寄りの点）である。従って、本実施形態での第１変位次元補正用モデルでは、大腿質点Ａ２，Ａ３が対応する脚体２の足首関節の中心と股関節の中心とを結ぶ線分の内分点に拘束されている。

なお、大腿質点Ａ２，Ａ３は、上記内分点から上記線分と直角な方向に所定の距離だけオフセットした点に設定してもよい。言い換えると、上記線分から所定の距離だけ離れた、該線分に平行な直線上に各大腿質点Ａ２，Ａ３を設定してもよい。

また、この第１変位次元補正用モデルの各質点Ａ１〜Ａ５のグローバル座標系上での位置、並びに上体３（上体リンク）の姿勢角は、単純化モデル歩容の運動の瞬時値に対応して幾何学的に定まるものとされている。より具体的には、本実施形態の第１変位次元補正用モデルの上体質点Ａ１のグローバル座標系での位置は、単純化モデル歩容の上体位置姿勢に対応する位置に決定され、各足平質点Ａ４，Ａ５のグローバル座標系での位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。さらに、上体リンクの姿勢角は、単純化モデル歩容の上体姿勢と同一とされる。そして、各大腿質点Ａ２，Ａ３のグローバル座標系上での位置は、単純化モデル歩容の上体位置姿勢と各足平位置姿勢を基に定まる前記の内分点の位置に決定される。すなわち、ロボット１の上体位置姿勢および各足平位置姿勢に対応して、各脚体２の各股関節および各足首関節の中心点のグローバル座標系での位置が一義的に定まるので、各脚体２の股関節の中心点および足首関節の中心点を結ぶ線分の内分点としての各大腿質点Ａ２，Ａ３のグローバル座標系での位置が定まる。

また、第２実施形態の第１変位次元補正用モデルでは、その質点Ａ１〜Ａ５の全体重心が単純化モデル上でのロボット１の全体重心の位置、すなわち、単純化モデルの全ての質点２m2，２m2，３m2の重心の位置に一致するように前記内分点に係る所定の比率と、質点Ａ１〜Ａ５の質量比が決定されている。

ここで、第２実施形態で上記のように第１変位次元補正用モデルの要素の配置を定めるということは、第１変位次元補正用モデルの要素の配置（グローバル座標系での質点Ａ１〜Ａ５の位置および上体リンクの姿勢）を定めるための幾何学的拘束条件（３）を次のように定義したとき、単純化モデル歩容の瞬時運動から、その幾何学的拘束条件（３）に従って第１変位次元補正用モデルの要素の配置を定めることと同じである。

幾何学的拘束条件（３）：与えられた任意の瞬時目標運動に対して、第１変位次元補正用モデルの要素のうちの上体質点Ａ１および上体リンクの配置が、与えられた瞬時目標運動におけるロボット１の上体３の位置姿勢に対応して定まる配置に一致し、且つ、各足平質点Ａ４，Ａ５の位置が、与えられた瞬時目標運動におけるロボット１の各足平位置姿勢に対応して定まる位置に一致し、且つ、各大腿質点Ａ２，Ａ３の位置が、与えられた瞬時目標運動における各脚体２の股関節の中心と足首関節の中心とを結ぶ線分上の所定の内分点の位置に一致する。

第２実施形態では、この幾何学的拘束条件（３）が本発明における第１の幾何学的拘束条件に相当するものである。

なお、本実施形態では、第２変位次元補正用モデルの構造は、前記図８に示した第１実施形態のものと同じ構造であり、本実施形態（第２実施形態）における第１変位次元補正用モデルと同様に、上体質点Ａ１、大腿質点Ａ２，Ａ３、および足平質点Ａ４，Ａ５を有し、また、上体リンクにイナーシャＩｂをもっている。この場合、上体質点Ａ１，各足平質点Ａ４，Ａ５の、対応する部位（上体３、各足平２２）に固定されたローカル座標系での位置は、図１９の第１変位次元補正用モデルと同じである。また、各質点Ａ１〜Ａ５の質量は、図１９の第１変位次元補正用モデルのものと同じである。そして、第２変位次元補正用モデルにおいては、各質点Ａ１〜Ａ５および上体３（上体リンク）はロボット１が採り得る任意の姿勢状態に対応する位置姿勢に移動可能とされている。すなわち、ロボット１の任意の瞬時目標運動と第２変位次元補正用モデルの各要素の配置との間には、前記第１実施形態で説明した前記幾何学的拘束条件（２）が設定されている。

次に、本実施形態（第２実施形態）における歩容生成装置１００の処理を詳細に説明する。本実施形態における歩容生成装置１００の基本的処理手順は、第１実施形態と同じであり、前記図９のフローチャートに従って歩容が生成される。

具体的には、Ｓ０１０からＳ０１８までの処理が第１実施形態と同様に実行される。これらの処理は、第１実施形態と同一である。

そして、Ｓ０１８の次にＳ０２０の処理が実行され、今回歩容の歩容パラメータが決定される。すなわち、今回歩容の目標足平位置姿勢軌道、目標腕姿勢軌道、目標ＺＭＰ軌道、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、基準上体姿勢軌道、床反力水平成分許容範囲、およびＺＭＰ許容範囲を規定するパラメータが決定される。この場合、本実施形態での単純化モデルは、前記したように前記公報文献１で用いている動力学モデルと同一であるので、同公報文献１の図１３のＳ０２２〜Ｓ０３０の処理と同一の処理を本実施形態のＳ０２０で実行することで、今回歩容の歩容パラメータが決定される。

なお、同公報文献１の図１３のＳ０２２〜Ｓ０２８の処理では、定常歩容の作成などのために、単純化モデル歩容用の床反力水平成分許容範囲を設定して使用しているが、本実施形態では、その単純化モデル歩容用の床反力水平成分許容範囲は、例えば同公報文献１の図１３のＳ３０で設定するフルモデル補正用の床反力水平成分許容範囲と同一にするか、もしくは、それよりも広めの範囲に設定すればよい。あるいは、本明細書の第１実施形態と同様に、単純化モデル歩容用の床反力水平成分許容範囲を無限大の範囲とするか、もしくは、単純化モデル歩容（あるいは定常歩容）の床反力水平成分が常にその床反力水平成分許容範囲内に収まるような広い範囲に定めてもよい。

次いで、図９のＳ０２０の処理の後、あるいはＳ０１６の判断結果がＮＯであった場合には、Ｓ０２２に進んで、歩容パラメータ（Ｓ０２０で決定した歩容パラメータ）を基に、今回歩容（単純化モデル歩容）の瞬時値が決定される。この場合、本実施形態での単純化モデルは、前記したように前記公報文献１で用いている動力学モデルと同一であるので、同公報文献１の図１３のＳ０３２の処理と同一の処理を本実施形態のＳ０２２で実行することで、単純化モデル歩容の瞬時値が決定される。

より具体的には、前記Ｓ０２０で決定した歩容パラメータを基に、目標足平位置姿勢、目標ＺＭＰ、目標腕姿勢、目標床反力鉛直成分、基準上体姿勢の瞬時値が決定され、さらに、前記図１６の単純化モデル上で、ロボット１の運動が発生する慣性力と重力との合力が目標ＺＭＰまわりに作用するモーメント水平成分が０になり、且つ、床反力水平成分が単純化モデル歩容用の床反力水平成分許容範囲を超えないように目標上体位置姿勢の瞬時値が決定される。ここで、目標上体位置姿勢の瞬時値に関して補足すると、目標上体位置鉛直成分は、目標床反力鉛直成分と前記式１４とから求められる単純化モデルの上体質点３m2の鉛直位置に対応して決定される。そして、目標床反力鉛直成分が比較的大きい時期では、主に上体３の水平加速度を調整して、目標ＺＭＰまわりのモーメント水平成分が０になるようにしつつ、目標上体姿勢の瞬時値が基準上体姿勢（例えば鉛直姿勢）に近づくように目標上体姿勢および目標上体位置水平成分が決定される。また、目標床反力鉛直成分が比較的小さいか、もしくは０となる時期では、上体３の水平加速度をほぼ０にしつつ（厳密には、全体重心の水平加速度をほぼ０にしつつ）、主に上体３の姿勢角の角加速度を調整して、目標ＺＭＰまわりのモーメント水平成分が０になるように、目標上体姿勢および目標上体位置水平成分の瞬時値が決定される。

なお、Ｓ０２２の処理で用いる単純化モデル歩容用の床反力水平成分許容範囲は、前記Ｓ０２０の処理で用いるものと同一でよい。

次いで、Ｓ０２４に進んで変位次元補正サブルーチンが実行される。このサブルーチン処理は、基本的処理手順は、前記第１実施形態と同一であり、前記図１０のフローチャートに従って実行される。すなわち、まず、Ｓ１００において、前記第１実施形態と同様に、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の重心に関する前記条件１と、角運動量積に関する前記条件２とを満足するように第１仮補正上体位置姿勢（Pb21，θb21）が決定される。この処理は、第１実施形態と同様に、図１１のサブルーチン処理によって実行される。

具体的には、Ｓ２００において、今回時刻（現在時刻）ｔでの単純化モデル歩容の瞬時値（目標上体位置姿勢などの目標運動の瞬時値）を基に、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角とを求める。この場合、前記したように、第１変位次元補正用モデルの上体質点Ａ１の位置は、単純化モデル歩容の上体位置姿勢の瞬時値に対応する位置に決定され、各足平質点Ａ４，Ａ５のグローバル座標系での位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、各大腿質点Ａ２，Ａ３の位置は、単純化モデル歩容の上体位置姿勢と各足平位置姿勢を基に定まるロボット１の各脚体２の股関節の中心点と足首関節の中心点とを結ぶ線分を、所定の比率で内分してなる内分点の位置に決定される。また、第１変位次元補正用モデルの上体リンクの姿勢角は、単純化モデル歩容の上体姿勢角と同一とされる。

これにより単純化モデル歩容の瞬時運動（今回時刻ｔの瞬時値）から、本実施形態での第１変位次元補正用モデルに係る前記幾何学的拘束条件（３）に従って、第１変位次元補正用モデルの各要素の配置が決定されることとなる。なお、この第１変位次元補正用モデルの要素の配置は、本発明の第１発明における「第１の配置」に相当するものである。

次いで、Ｓ２０２からＳ２１８までの処理が実行される。これらの処理は、第１実施形態と同じである。すなわち、前記条件１，２を満たすような第１仮補正上体位置姿勢が探索的に求められ、それが、今回時刻ｔにおける第１仮補正上体位置姿勢(Ｐb21，θb21）として決定される。これにより、単純化モデル歩容の上体位置姿勢を前記条件１，２を満たすように補正してなる歩容（第１仮補正歩容）が得られる。この第１仮補正歩容の運動は、Ｓ２１０の条件が満たされたときの第２変位次元補正用モデルの配置（前記第１要素配置）から、前記幾何学的拘束条件（２）に従って決定される瞬時目標運動と同じである。

補足すると、本実施形態では、各変位次元補正用モデルの両足平質点Ａ４，Ａ５の位置は、両変位次元補正用モデルで同じである。従って、Ｓ２０８でモデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errとを算出するに当たっては、両足平質点Ａ４，Ａ５に係わる項は省略してもよい。

上述したようにＳ１００の処理を実行した後、Ｓ１０２の処理が前記第１実施形態と同様に実行され、第２変位次元補正用モデルでの上体姿勢を単純化モデル歩容の瞬時値（今回時刻ｔでの瞬時値）における上体姿勢と同一にして、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の角運動量積に関する前記条件２を満足するように第２仮補正上体位置姿勢（Pb22，θb22）が決定される。なお、このＳ１０２の処理では、第２仮補正上体姿勢θb22は、単純化モデル歩容の上体姿勢と同一にされるので、該Ｓ１０２の処理は、実質的には、条件２を満足するように第２仮補正上体位置Ｐb22を決定する処理であると言える。

この処理は、第１実施形態と同様に、図１２のサブルーチン処理によって実行される。

具体的には、Ｓ３００において、今回時刻（現在時刻）ｔでの単純化モデル歩容の瞬時値を基に、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置と、イナーシャをもつ上体３（上体リンク）の姿勢角とを求める。この処理およびそれにより求められる第１変位次元補正用モデルの要素の配置は、本実施形態での図１１のＳ２００の処理と同じである。従って、本実施形態においても、Ｓ２００で求めた第１変位次元補正用モデルの各要素の配置を、そのまま図１２のサブルーチン処理で用いるようにすれば、Ｓ３００の処理は省略してもよい。

次いで、Ｓ３０２〜Ｓ３１８までの処理が第１実施形態と同様に実行される。これらの処理は、第１実施形態と同じである。すなわち、上体姿勢が単純化モデル歩容の上体姿勢と同一で、且つ、前記条件２を満たすような第２仮補正上体位置姿勢が探索的に求められ、それが、今回時刻ｔにおける第２仮補正上体位置姿勢(Ｐb22，θb22）として決定される。これにより、単純化モデル歩容の上体位置のみを前記条件２を満たすように補正してなる歩容（第２仮補正歩容）が得られる。この第２仮補正歩容は、Ｓ３１０の条件が満たされたときの第２変位次元補正用モデルの配置（前記第２要素配置）から、前記幾何学的拘束条件（２）に従って決定される瞬時目標歩容と同じである。

補足すると、本実施形態では、各変位次元補正用モデルの両足平質点Ａ４，Ａ５の位置は、両変位次元補正用モデルで同じであので、前記Ｓ２０８の処理の場合と同様、Ｓ３０８でモデル間角運動量積ずれL_errとを算出するに当たっては、両足平質点Ａ４，Ａ５に係わる項は省略してもよい。

上述したようにＳ１０２の処理を実行した後、Ｓ１０４〜Ｓ１１０の処理が第１実施形態と同様に実行される。これらの処理は、第１実施形態と全く同一である。これにより、今回時刻ｔにおける変位次元補正上体位置姿勢(Pb2，θb2)が決定され、単純化モデル歩容の上体位置姿勢を補正してなる変位次元補正歩容が得られる。

本実施形態（第２実施形態）では、以上のように図９のＳ０２４の処理（変位次元歩容補正サブルーチン）を実行した後、Ｓ０２６〜Ｓ０３２の処理が第１実施形態と同様に実行される。これらの処理は、第１実施形態と同一である。

なお、歩容生成装置１００で以上説明したように生成される目標歩容を入力する複合コンプライアンス制御装置１０１の動作は前記第１実施形態と同一である。

また、補足すると、第２実施形態における変位次元歩容補正サブルーチンの処理は、本発明の第１発明における目標運動補正手段を構成しており、この処理で決定される変位次元補正歩容の運動が該第１発明における補正後瞬時目標運動に相当する。また、第２実施形態におけるＳ１００の処理、Ｓ１０２の処理が、それぞれ第１発明における第１仮補正運動決定手段、第２仮補正運動決定手段に相当し、それぞれの処理で決定される第１仮補正歩容、第２仮補正歩容がそれぞれ第１発明における第１の仮補正瞬時目標運動、第２の仮補正瞬時目標運動に相当する。

ここで、本実施形態での単純化モデル歩容と、前記第１仮補正歩容と、前記第２仮補正歩容と、変位次元補正歩容との関係について図２０〜図２２を参照して補足説明をしておく。図２０は第２変位次元補正用モデルの各質点Ａi（ｉ＝１，２，…，５）の位置および上体３（上体リンク）の姿勢角を単純化モデル歩容通りに決定した場合（すなわち単純化モデル歩容を前記Ｓ０２４で補正しない場合）における第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、該単純化モデル歩容に対応して定まる第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この場合の第２変位次元補正用モデルの要素の配置は、単純化モデル歩容の瞬時運動から、前記幾何学的拘束条件（２）に従って決定されたものと言える。なお、前記図２０では、単純化モデル歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi2'(Ai)で表している。

また、図２１は前記第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この図２１の第２変位次元補正用モデルの各要素の配置は、図２０で想定している単純化モデル歩容の瞬時値に対応して、前記図１０のＳ１００で最終的に決定される第２変位次元補正用モデルの前記第１要素配置である。この配置は、前記第１仮補正歩容から、前記幾何学的拘束条件（２）に従って定まるものと同一である。なお、図２１では、第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi21(Ai)で表している。また、図２１に示す第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角は、図２０と同じである。

また、図２２は前記第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。この図２２の第２変位次元補正用モデルの各要素の配置は、図２０で想定している単純化モデル歩容の瞬時値に対応して、前記図１０のＳ１０２で最終的に決定される第２変位次元補正用モデルの前記第２要素配置である。この配置は、前記第２仮補正歩容から、前記幾何学的拘束条件（２）に従って定まるものと同一である。なお、図２２では、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi22(Ai)で表している。また、図２２に示す第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角は、図２０と同じである。

図２０に示す例において、単純化モデル歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置Ｐi2'では、大腿質点Ｐ22'，Ｐ32'が第１変位次元補正用モデルに対してロボット１の前方側に在る。すなわち、第２変位次元補正用モデルでのロボット１の全体重心が単純化モデル歩容でのロボット１の全体重心よりもロボット１の前方側（Ｘ軸の正方向）に偏る。別の言い方をすれば、両脚体２，２の各大腿質点Ａ２，Ａ３のモデル間擬似並進加速度によって各大腿質点Ａ２，Ａ３が発生する慣性力の並進力成分（＝各質点Ａ２，Ａ３の質量＊モデル間擬似並進加速度）の総和がロボット１の後方側に発生するものとなる。また、各大腿質点Ａ２，Ａ３のモデル間擬似並進加速度によって各大腿質点Ａ２，Ａ３が発生する慣性力が目標ＺＭＰまわりに作用するモーメントが、ロボット１の後傾側に発生するものとなる。なお、本実施形態では、単純化モデル歩容と第１変位次元補正用モデルとで、両足平位置姿勢および上体位置姿勢は同一であるので、図２０の例では、上体質点Ａ１、各足平質点Ａ４，Ａ５のそれぞれのモデル間擬似並進加速度は０であり、また、上体リンクのモデル間擬似角加速度も０である。

これに対して、前記第１仮補正歩容に対応する第２変位次元補正用モデルの要素の配置（第１要素配置）、すなわち、本実施形態で前記図１０のＳ１００で最終的に決定される第２変位次元補正用モデルの各質点Ａｉの位置および上体リンクの姿勢角により定まる第２変位次元補正用モデルの要素の配置にあっては、図２１に見られるように、大腿質点Ａ２，Ａ３の前方側への偏りを補償するようにして、第２変位次元補正用モデルの上体質点Ａ１の位置が、第１変位次元補正用モデルの上体質点Ａ１よりも後方側に決定され、換言すれば、上体位置が単純化モデル歩容よりも後方側に補正される。同時に、第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置並びにイナーシャをもつ上体３の姿勢は、前記した角運動量積がある一定値（所定値）になるように決定される。図示の例では、第２変位次元補正用モデルでの上体姿勢（実線で示す上体３の姿勢）は、単純化モデル歩容の上体姿勢（破線で示す上体３の姿勢）に対して、角度θb21−θbだけ前傾している。補足すると、図２１の斜線もしくは横線を付した三角形の面積の２倍が、それぞれ、上体質点Ａ１、大腿質点Ａ２，Ａ３に係る角運動量積に相当するものである。

このため、第１仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置および上体リンクの姿勢角では、各質点Ａiのモデル間擬似並進加速度に伴う慣性力の並進力成分の総和が図２０に示した場合よりも小さくなって、ほぼ０になると共に、該慣性力が目標ＺＭＰまわりに発生するモーメントの総和も図２０に示した場合よりも所定の値（前記条件２に係る「一定値」に相当する値）に近づく。

これにより、第１仮補正歩容は、単純化モデル歩容では考慮されていない各脚体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力（並進床反力および床反力モーメント）と同じような床反力が発生するようにロボット１の目標運動（より詳しくは目標上体位置および目標上体姿勢）を補正したものとなる。このため、第１仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力が、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力よりも、目標床反力（＝単純化モデル歩容の床反力）により近いものとなる。つまり、第１仮補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的精度が単純化モデル歩容の運動と床反力の間の動力学的精度よりも高まることとなる。

一方、前記第２仮補正歩容に対応する第２変位次元補正用モデルの要素の配置（第２要素配置）、すなわち、本実施形態で前記図１０のＳ１０２で最終的に決定される第２変位次元補正用モデルの各質点Ａｉの位置および上体リンクの姿勢角により定まる第２変位次元補正用モデルの要素の配置にあっては、図２２に見られるように、上体姿勢を単純化モデル歩容の瞬時上体姿勢と同じに維持したまま、第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置が、前記した角運動量積の総和がある一定値（所定値）になるように決定される。

このため、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置および上体リンクの姿勢角では、各質点Ａi（より詳しくは上体質点Ａ１および大腿質点Ａ２，Ａ３）のモデル間擬似並進加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントの総和が図２０に示した場合よりも所定の値（前記条件２の「一定値」に相当する値）に近づく。

これにより、第２仮補正歩容は、ロボット１の目標上体姿勢を単純化モデル歩容の上体姿勢と同じに維持しながら、単純化モデル歩容では考慮されていない各脚体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容の床反力モーメントと同じような床反力モーメントが発生するように、ロボット１の目標上体位置を補正したものとなる。このような第２仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントが、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントよりも、目標床反力モーメント（＝単純化モデル歩容の床反力モーメント）により近いものとなる。つまり、第２仮補正歩容の運動と床反力モーメントとの間の動力学的精度が単純化モデル歩容の運動と床反力モーメントとの間の動力学的精度よりも高まることとなる。但し、第２仮補正歩容を決定する（第２変位次元補正用モデルの第２要素配置を決定する）ときには、第１および第２変位次元補正用モデルの間の全体重心のずれは考慮していないので、第２仮補正歩容の運動と並進床反力との間の動力学的な精度は、必ずしも単純化モデル歩容の運動と並進床反力との間の動力学的な精度よりも高まるとは限らない。

そして、本実施形態では、上述したような第１仮補正歩容の上体位置姿勢と第２仮補正歩容の上体位置姿勢とから、第１実施形態と同様に前記式１２および式１３によって変位次元補正上体位置姿勢が決定される。このため、第１実施形態と同様、ロボット１の動作モードが定常的に通常モードであるときには、変位次元補正歩容は、上体姿勢を変動させない（上体姿勢を補正しない）ようにしつつ、単純化モデル歩容の上体位置を補正して、変位次元補正歩容の運動と床反力モーメントとの間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。

また、ロボット１の動作モードが定常的に走行モードであるときには、第１実施形態と同様に、変位次元補正歩容は、単純化モデル歩容の上体位置および上体姿勢の両者を補正して、変位次元補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。

また、ロボット１の動作モードが定常的に低摩擦床面歩行モードであるときには、第１実施形態と同様に、変位次元補正歩容は、単純化モデル歩容よりも動力学的精度を高めつつ、且つ、上体姿勢の変動を極力抑えつつ、且つ、ロボット１のスリップを防止しつつ、ロボット１の安定な動作を可能とする歩容となる。

さらに、ロボット１の動作モードが切り替る際には、前記図１０のＳ１０６の処理によって、重みw1が（ひいては、重みw2も）の値を徐々に変化させるので、変位次元補正上体姿勢や変位次元補正上体位置が急変することはない。

［第３実施形態］
次に、本発明の第３実施形態を図２３〜図２５を参照しつつ説明する。本実施形態は、ロボット１の構成は第１および第２実施形態と同一で、また、単純化モデル、第１変位次元補正用モデルおよび第２変位次元補正用モデルの構造は第２実施形態と同一である。そして、本実施形態は、歩容生成装置１００の処理の一部のみが第２実施形態と相違するものである。従って、本実施形態の説明では、第２実施形態と同一部分については第２実施形態と同じ符号および図面を用い、詳細な説明を省略する。以下、前記第２実施形態と相違する部分を主体として、本実施形態を説明する。なお、本実施形態は、本発明の第２〜４、第７〜１０、第１３〜１５発明の一実施形態である。

本実施形態では歩容生成装置１００の処理は、図９のＳ０２４の変位次元歩容補正サブルーチンのみが前記第２実施形態と相違している。従って、この変位次元歩容補正サブルーチン以外の処理については説明を省略する。

本実施形態における変位次元歩容補正サブルーチンは、図２３のフローチャートで示す如く行われる。以下説明すると、まず、Ｓ５００において、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の重心に関する前記条件１と、角運動量積に関する前記条件２とを満足するように第１仮補正上体位置姿勢（Pb21，θb21）が決定される。

この処理は、第２実施形態でのＳ１００（図１０）の処理と同一であり、図１１のサブルーチン処理によって第２実施形態と全く同様に実行される。補足すると、本実施形態（第３実施形態）では、図１１のＳ２００の処理で求められる第１変位次元補正用モデルの各要素の配置が、本発明の第２発明における「第１の配置」に相当し、また、Ｓ２１０の条件が成立したときの第２変位次元補正用モデルの各要素の配置が前記第１要素配置に相当すると共に、本発明の第２発明における「第２の配置」に相当する。

次いで、Ｓ５０２、Ｓ５０４およびＳ５０６の処理が、それぞれ第２実施形態におけるＳ１０４、Ｓ１０６、Ｓ１０８と全く同じように実行され、今回時刻ｔでの重みw1，w2の値が決定される。

次いで、Ｓ５０８において、第２変位次元補正用モデルでの上体姿勢を、Ｓ５００で決定した第１仮補正上体姿勢θb21にＳ５０４で決定した重みw1を乗算してなる姿勢と前記単純化モデル歩容の瞬時値における上体姿勢θbにＳ５０６で決定した重みw2を乗算してなる姿勢との和（w1＊θb21＋w2＊θb）と同一にして、第１変位次元補正用モデルと第２変位次元補正用モデルとの間の角運動量積に関する前記条件２を満足するように第２仮補正上体位置姿勢（Pb22，θb22）が決定される。なお、このＳ５０８の処理では、第２仮補正上体姿勢θb22は、w1＊θb21＋w2＊θbと同一にされるので、該Ｓ５０８の処理は、実質的には、条件２を満足するように第２仮補正上体位置Ｐb22を決定する処理であると言える。

このＳ５０８の処理は、図２４のサブルーチン処理によって実行される。このサブルーチン処理は、前記第２実施形態における図１２のサブルーチン処理に対して第２仮補正上体姿勢の候補θb22_sの値のみが相違するものである。すなわち、本実施形態（第３実施形態）の図２４のサブルーチン処理では、第２仮補正上体姿勢の候補θb22_sがw1＊θb21＋w2＊θb（今回時刻ｔの重みw1を今回時刻ｔの第１仮補正上体姿勢θb21に乗じてなる姿勢角と今回時刻ｔの重みw2を今回時刻ｔの単純化モデル歩容の瞬時値における状態姿勢θbに乗じてなる姿勢角との和）に固定され、この点でのみ、図１２のサブルーチン処理と相違する（図１２のサブルーチン処理では、候補θb22_sは単純化モデル歩容の瞬時値における上体姿勢に固定されている）。より具体的には、図２４のサブルーチン処理は、Ｓ６０２の処理で、初期候補θb22_sがw1＊θb21＋w2＊θbに設定され、これ以外は、図１２のサブルーチン処理と同一である。この場合、図２４のＳ６００、Ｓ６０４〜Ｓ６１８がそれぞれ図１２のＳ３００、Ｓ３０４〜Ｓ３１８と同じである。

かかるＳ５０８の処理により、単純化モデル歩容の上体姿勢をw1＊θb21＋w2＊θbに補正し、且つ、単純化モデル歩容の上体位置を前記条件２を満たすように補正してなる歩容としての第２仮補正歩容が得られることとなる。

上記のようにＳ５０８の処理を実行した後、Ｓ５１０の処理が実行され、今回時刻ｔにおける変位次元補正上体位置姿勢(Pb2，θb2)が決定される。すなわち、第２仮補正歩容の上体位置姿勢(Pb22，θb22)（＝(Pb22，w1＊θb21＋w2＊θb)）がそのまま、変位次元補正上体位置姿勢(Pb2，θb2)として決定される。補足すると、本実施形態（第３実施形態）では、Ｓ６１０の条件が成立したときの第２変位次元補正用モデルの要素の配置が前記第２要素配置に相当すると共に、本発明の第２発明における「第３の配置」に相当する。

以上が本実施形態（第３実施形態）での変位次元歩容補正サブルーチンの詳細である。

補足すると、この第３実施形態では、変位次元歩容補正サブルーチンの処理が、本発明の第２発明における目標運動補正手段に相当し、この処理で決定される変位次元補正歩容の運動（第３実施形態ではこれは第２仮補正歩容の運動に等しい）が、該第２発明における補正後瞬時目標運動に相当する。さらに、Ｓ５００の処理が第２発明における仮補正運動決定手段に相当し、この処理で決定される第１仮補正歩容が第２発明における仮補正瞬時目標運動に相当する。

以上説明した本実施形態では、第１仮補正歩容は、第２実施形態と同一である。従って、前記図２０および図２１を参照して説明した如く、第１仮補正歩容は、単純化モデル歩容では考慮されていない各脚体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力（並進床反力および床反力モーメント）と同じような床反力が発生するようにロボット１の目標運動（より詳しくは目標上体位置および目標上体姿勢）を補正したものとなる。このため、第１仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力が、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力よりも、目標床反力（＝単純化モデル歩容の床反力）により近いものとなる。つまり、第１仮補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的精度が単純化モデル歩容の運動と床反力の間の動力学的精度よりも高まることとなる。

一方、図２５は、本実施形態（第３実施形態）での第２仮補正歩容に対応する第２変位次元補正用モデルの要素の配置（第２要素配置）と第１変位次元補正用モデルの要素の配置との関係を例示している。この図２５の第２変位次元補正用モデルの各要素の配置は、前記図２０で想定している単純化モデル歩容の瞬時値に対応して、前記図２３のＳ５０６で最終的に決定される第２変位次元補正用モデルの前記第２要素配置である。この配置は、前記第２仮補正歩容から、前記幾何学的拘束条件（２）に従って定まるものと同一である。なお、図２５では、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置をＰi22(Ai)で表している。また、図２５に示す第１変位次元補正用モデルの各質点Ａｉの位置および上体リンクの姿勢角は、前記図２０と同じである。

この図２５に見られるように、第２仮補正歩容に対応する第２変位次元補正用モデルの要素の配置では、上体姿勢を、第１仮補正上体姿勢θb21に重みw1を乗じてなる姿勢角と前記単純化モデル歩容の瞬時値における上体姿勢θbに重みw2を乗算してなる姿勢角との和に規制した状態で、第２変位次元補正用モデルの各質点Ａ１〜Ａ５の位置が、前記した角運動量積の総和がある一定値（所定値）になるように決定される。補足すると、図２５の斜線もしくは横線を付した三角形の面積の２倍が、それぞれ、上体質点Ａ１、大腿質点Ａ２，Ａ３に係る角運動量積に相当するものである。

このため、第２仮補正歩容に対応する第２変位次元補正用モデルの各質点Ａiの位置および上体リンクの姿勢角では、各質点Ａi（より詳しくは上体質点Ａ１および大腿質点Ａ２，Ａ３）のモデル間擬似並進加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントの総和が図２０に示した場合よりも所定の値（前記条件２の「一定値」に相当する値）に近づく。

これにより、第２仮補正歩容は、ロボット１の目標上体姿勢を単純化モデル歩容の瞬時値における上体姿勢（本実施形態では鉛直姿勢）と第１仮補正上体姿勢との間の姿勢に規制しながら、単純化モデル歩容では考慮されていない各脚体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容の床反力モーメントと同じような床反力モーメントが発生するように、ロボット１の目標上体位置を補正したものとなる。このような第２仮補正歩容は、その歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントが、単純化モデル歩容の運動どおりに実際のロボット１を動作させたときに発生する実床反力モーメントよりも、目標床反力モーメント（＝単純化モデル歩容の床反力モーメント）により近いものとなる。つまり、第２仮補正歩容の運動と床反力モーメントとの間の動力学的精度が単純化モデル歩容の運動と床反力モーメントとの間の動力学的精度よりも高まることとなる。

そして、本実施形態では、第２仮補正上体位置姿勢が変位次元補正上体位置姿勢として決定される。この場合、ロボット１の動作モードが定常的に通常モードである場合（w1＝０である場合）には、変位次元補正歩容（＝第２仮補正歩容）は、第２実施形態と同様に、上体姿勢を変動させない（一定に維持する）ようにしつつ、単純化モデル歩容の上体位置を補正して、変位次元補正歩容の運動と床反力モーメントとの間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。この場合、単純化モデル歩容が前記第２実施形態と同一であれば、本実施形態（第３実施形態）における変位次元補正歩容も第２実施形態と同じになる。

また、ロボット１の動作モードが定常的に走行モードである場合（w1＝１である場合）には、前記図２３のＳ５０６で決定される第２仮補正上体位置姿勢は第１仮補正上体位置姿勢と同一か、もしくはほぼ同一となる（両者の上体位置は、完全に同一になるとは限らない）。このため、この場合の変位次元補正歩容（＝第２仮補正歩容）は、単純化モデル歩容の上体位置および上体姿勢の両者を補正して、変位次元補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的な精度を単純化モデル歩容よりも高めることができる歩容となる。この場合も、単純化モデル歩容が前記第２実施形態と同一であれば、本実施形態（第３実施形態）における変位次元補正歩容も第２実施形態と同じになる。

また、ロボット１の動作モードが定常的に低摩擦床面歩行モードである場合（w1＝0.5である場合）には、変位次元補正上体姿勢は、第１仮補正歩容の上体姿勢角の１／２の姿勢角と単純化モデル歩容の上体姿勢角の１／２の姿勢角との和とされ、変位次元補正上体位置は、上体姿勢角が第１仮補正歩容の上体姿勢角の１／２の姿勢角と単純化モデル歩容の上体姿勢角の１／２の姿勢角との和である場合に対応する第２仮補正歩容の上体位置とされる。このため、この場合の変位次元補正歩容は、前記第２実施形態と同様、上体姿勢の変動を抑制しつつ、変位次元補正歩容の運動と床反力（並進床反力および床反力モーメント）との間の動力学的な精度を単純化モデル歩容よりも高めるように単純化モデル歩容の上体位置および上体姿勢の両者を補正したものとなる。ひいては、該変位次元補正歩容は、単純化モデル歩容よりも動力学的精度を高めつつ、且つ、上体姿勢の変動を極力抑えつつ、且つ、ロボット１のスリップを防止しつつ、ロボット１の安定な動作を可能とする歩容となる。また、この場合、本実施形態では、変位次元補正上体位置は、補正後の上体姿勢としての変位次元補正上体姿勢に対応して、前記条件２を満たすように決定されるものであるため、その変位次元補正上体位置姿勢を含む変位次元補正歩容の運動どおりに実際のロボット１の動作させたときに発生する実床反力モーメントと目標床反力モーメントとの誤差を効果的に低減できる。

なお、ロボット１の動作モードが切り替る際には、前記図２３のＳ５０４の処理によって、重みw1の値を徐々に変化させるので、前記第１および第２実施形態と同様、変位次元補正上体姿勢や変位次元補正上体位置が急変することはない。

次に、以上説明した第１〜第３実施形態に関連した変形態様をいくつか説明する。

前記第１〜第３実施形態では、変位次元補正上体姿勢は、前記第１仮補正上体姿勢に、重みw1を乗じたものに応じて決定される。この場合、第１仮補正上体姿勢（第１仮補正歩容の上体の傾斜角）に対して、重みw1が周波数特性を持つようにしてもよい。例えば、第１仮補正上体姿勢の周波数成分に対して、図２６（ａ）に示す如く、重みw1にローカット特性を持たせる。このようにした場合には、第１仮補正上体姿勢が定常的にほぼ一定に維持されるような状態、例えば、ロボット１を直立姿勢状態で継続的に静止させるような目標歩容を生成する場合には、第１仮補正上体姿勢に重みw1を乗じてなる変位次元補正上体姿勢を、鉛直方向に対するオフセットが発生することなく、確実に定常的に鉛直姿勢に維持することができる。このため、ロボット１の全体姿勢の見栄えがよくなる。あるいは、必要に応じて、図２６（ｂ）に示す如く、重みw1にハイカット特性を持たせるようにしてもよい。このようにした場合には、第１仮補正上体姿勢の高周波成分、すなわち、第１仮補正上体姿勢を高速で振動させる成分を除去して、変位次元補正上体姿勢を決定できることとなる。この結果、変位次元補正上体姿勢の細かな振動が生じるのを防止し、ロボット１の頭部等に搭載される撮像装置の揺れを防止できる。

なお、図２６（ａ）あるいは図２６（ｂ）のように重みw1に周波数特性をもたせることで、本発明の第５発明の一実施形態が構築されることとなる。

また、前記第１〜第３実施形態では、角運動量積に係る点Ｑとして、目標ＺＭＰを用いたが、点Ｑは目標ＺＭＰ以外の点でもよく、例えば、次のような点でもよい。
ａ）歩容を記述する座標系（グローバル座標系）の原点
ｂ）ロボット１とともに連続的に移動する適当に設定した点
ｃ）フルモデルにおけるロボット１の全体重心
ｄ）単純化モデルにおけるロボット１の全体重心
ｅ）第１および第２変位次元補正用モデルに係るある所定の質点の集合の重心（具体的には、第１および第２変位次元補正用モデルの間で位置の差を生じる可能性がある質点の集合の重心。例えば、第１実施形態では、全ての質点Ａ１〜Ａ５の集合の重心が相当し、第２および第３実施形態では、上体質点Ａ１，大腿質点Ａ２，Ａ３の集合の重心が相当する）
また、前記第１実施形態では、図９のＳ０２４の変位次元歩容補正サブルーチンを図１０の処理により行うようにしたが、前記第３実施形態と同様に、図２３の処理により行うようにしてもよい。これにより、本発明の第２発明に係る他の実施形態が構成されることとなる。

また、前記第１〜第３実施形態の説明では、図９のＳ０２４の変位次元歩容補正サブルーチンの処理において、サジタルプレーン上での単純化モデル歩容の上体位置姿勢を補正する処理について説明したが、サジタルプレーンに直交するラテラルプレーン上での上体位置姿勢を合わせて補正するようにしてもよい。この場合には、例えば図１１のＳ２００〜Ｓ２１８の処理、図１２のＳ３００〜Ｓ３１８の処理、および図２４のＳ６００〜Ｓ６１８の処理を３次元に拡張して行えばよい。あるいは、サジタルプレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での上体位置姿勢の補正処理とを図１１、図１２、図２４と同様のアルゴリズムによって、それぞれ独立に行うようにしてもよい。補足すると、上体位置の鉛直成分を含めて上体位置姿勢を補正する場合において、サジタルプレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での上体位置の補正処理とを独立して行う場合には、上体位置の鉛直成分の補正は、サジタルプレーン上、あるいはラテラルプレーン上のいずれか一方のプレーン上での補正処理によって行い、他方のプレーン上での補正処理では、上体位置の鉛直成分を除いて、その他方のプレーン上での上体位置姿勢の補正を行うようにすればよい。

また、ホリゾンタルプレーン（水平面）での上体位置姿勢を合わせて補正するようにしてもよい。あるいは、サジタルプレーン、ラテラルプレーン、ホリゾンタルプレーンのいずれか一つまたは二つに対して上体位置姿勢を補正するようにしてもよい。

また、第１〜第３実施形態では、図１０のＳ１００あるいは図２３のＳ５００において、第１仮補正上体位置姿勢の初期候補（Ｐb21_s，θb21_s）を、前回制御周期の時刻で求めた第１仮補正上体位置姿勢を用いて決定したが、例えば該初期候補（Ｐb21_s，θb21_s）を単純化モデル歩容の上体位置姿勢と同一にしてもよい。同様に、図１０のＳ１０２あるいは図２３のＳ５０６において、第２仮補正上体位置の初期候補Pb22_sを、前回制御周期の時刻で求めた第２仮補正上体位置を用いて決定したが、例えば該初期候補Pb22_sを単純化モデル歩容の上体位置と同一にしてもよい。但し、前記条件１，２を満たす第１仮補正上体位置姿勢、あるいは前記条件２を満たす第２仮補正上体位置を短時間で探索する上では、前記第１〜第３実施形態で説明した如く、初期候補（Ｐb2_s，θb2_s）を決定することが望ましい。

また、第１〜第３実施形態の変位次元歩容補正サブルーチンの処理で、前記条件１，２を満たす第１仮補正上体位置姿勢を探索的に求めようにしたが、例えば第２変位次元補正用モデルの要素の配置（各質点の位置とイナーシャを持つ各リンクの姿勢）を前記幾何学的拘束条件（２）に従って単純化モデル歩容に対応させて決定したときのその配置と、第１変位次元補正用モデルの要素の配置との差（両モデル間の各質点の位置の差と、イナーシャをもつ各リンクの姿勢角の差）から、あらかじめ作成した関数式あるいはマップなどを用いて、単純化モデル歩容の上体位置姿勢から第１仮補正上体位置姿勢への補正量を決定し、その補正量で単純化モデル歩容の上体位置姿勢を補正することで、第１仮補正上体位置姿勢を決定することも可能である。

また、第１および第２実施形態の変位次元歩容補正サブルーチンの処理では、上体姿勢を単純化モデル歩容の上体姿勢と同一にしたときに前記条件２を満たす第２仮補正上体位置を探索的に求めるようにしたが、第２変位次元補正用モデルの要素の配置（各質点の位置とイナーシャを持つ各リンクの姿勢）を前記幾何学的拘束条件（２）に従って単純化モデル歩容に対応させて決定したときのその配置と、第１変位次元補正用モデルの要素の配置との差（両モデル間の各質点の位置の差と、イナーシャをもつ各リンクの姿勢角の差）から、あらかじめ作成した関数式あるいはマップなどを用いて、単純化モデル歩容の上体位置から第２仮補正上体位置への補正量を決定し、その補正量で単純化モデル歩容の上体位置を補正することで、第２仮補正上体位置を決定することも可能である。同様に、第３実施形態にあっては、単純化モデル歩容の上体姿勢を第１仮補正上体姿勢に前記重みw1を乗じたものと単純化モデル歩容の上体姿勢に前記重みw2を乗じたものとの和で置き換えてなる歩容（以下、ここでの説明では置き換え歩容という）の瞬時値から前記幾何学的拘束条件（２）に従って第２変位次元補正用モデルの要素の配置を決定したときのその配置と、第１変位次元補正用モデルの要素の配置との差から、あらかじめ作成した関数式あるいはマップなどを用いて、上記置き換え歩容の上体位置から第２仮補正上体位置への補正量を決定し、その補正量で該置き換え歩容の上体位置を補正することで、第２仮補正上体位置を決定することも可能である。

また、第１〜第３実施形態の変位次元歩容補正サブルーチンの処理で、第１仮補正上体位置姿勢を決定するときに、モデル間全体重心ずれGc_errとモデル間角運動量積ずれLc_errとがそれぞれ許容範囲内であるか否かの判定（図１１のＳ２１０の処理）を行わず、探索回数（候補(Pb21_s，θb21_s)の更新回数）が所定回数に達したら探索完了とし、そのときの候補(Pb21_s，θb21_s)を第１仮補正上体位置姿勢として決定するようにしてもよい。あるいは、モデル間全体重心ずれGc_errとモデル間角運動量積ずれLc_errとがそれぞれ許容範囲内に収まるか、もしくは、探索回数が所定回数に達したら探索完了として、そのときの候補(Pb21_s，θb21_s)を第１仮補正上体位置姿勢として決定するようにしてもよい。

同様に、第１〜第３実施形態の変位次元歩容補正サブルーチンの処理で、第２仮補正上体位置姿勢を決定するときに、モデル間角運動量積ずれLc_errとがそれぞれ許容範囲内であるか否かの判定（図１２のＳ３１０の処理、あるいは図２４のＳ６１０の処理）を行わず、探索回数（候補(Pb22_s，θb22_s)の更新回数）が所定回数に達したら探索完了とし、そのときの候補(Pb22_s，θb22_s)を第２仮補正上体位置姿勢として決定するようにしてもよい。あるいは、モデル間角運動量積ずれLc_errとがそれぞれ許容範囲内に収まるか、もしくは、探索回数が所定回数に達したら探索完了として、そのときの候補(Pb22_s，θb22_s)を第２仮補正上体位置姿勢として決定するようにしてもよい。

また、前記第１〜第３実施形態では、モデル間角運動量積ずれLc_errを算出するときに、先にも述べたように、前記式０７の代わりに例えば前記式０８を用いるようにしてもよい。この場合、式０８の右辺のΣに続く各項は、第１変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分と、第２変位次元補正用モデルの質点Ａiと点Ｑとを結ぶ線分とのなす角度(Pi1_Q_Pi2)に対してほぼ単調に変化する関数となる。従って、前記第１〜第３実施形態では、モデル間角運動量積ずれLc_errを算出するために、式０８を使用することで、本発明の第６発明の一実施形態が構築されることとなる。

また、前記第２および第３実施形態に関して、第１および第２変位次元補正用モデルの間での各大腿質点Ａ２，Ａ３のそれぞれの位置ずれは、各脚体２の足首関節の中心点と股関節の中心点とを結ぶ線分に対する第２変位次元補正モデルの大腿質点Ａ２，Ａ３の位置ずれ（該線分にほぼ直交する面内での位置のずれ）、あるいは、該線分に対する膝関節の中心の位置ずれにほぼ等しい。従って、モデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errとを求めるときに、例えば前記式０６，０７における各大腿質点Ａ２，Ａ３に係る位置ずれ(Ｐ22−Ｐ21)，(Ｐ32−Ｐ31)の代わりに、上記線分と、大腿質点Ａ２，Ａ３もしくは各膝関節の中心との距離（以下、大腿質点Ａ２，Ａ３の擬似位置ずれ距離という）を用いてもよい。

さらには、この大腿質点Ａ２，Ａ３の擬似位置ずれ距離は、各脚体２の膝関節の曲げ角と密接な関係があり、この大腿質点Ａ２，Ａ３の擬似位置ずれを膝関節の曲げ角から求めるようにしてもよい。より具体的には、前記した図２０に示すように、各大腿リンク２４の長さ（大腿リンク２４の両端の股関節および膝関節のそれぞれの中心点の間の距離）をＬ、膝関節の曲げ角（大腿リンクの軸心（股関節の中心と膝関節の中心とを通る直線）に対する下腿リンクの軸心（膝関節の中心と足首関節の中心とを通る直線の傾斜角）をθとしたとき、各大腿質点Ａ２，Ａ３の擬似位置ずれ距離は、ほぼ、Ｌ＊sin(θ／２)に等しいものとなる。なお、長さＬは、両大腿リンク２４，２４について同一である。従って、例えば前記式０６，０７における各大腿質点Ａ２，Ａ３に係る位置ずれ(Ｐ22−Ｐ21)，(Ｐ32−Ｐ31)の代わりに、各脚体２の膝関節の曲げ角θに応じて定まるＬ＊sin（θ／２)を用いるようにしてもよい。補足すると、上体位置姿勢と両足平位置姿勢を定めれば、各脚体２の膝関節の曲げ角はロボット１の幾何学モデル（リンクモデル）によって一義的に決定できる。

また、前記第１〜第３実施形態では、第１および第２変位次元補正用モデルにおける各脚体２の質点を２つとしたが、例えば各脚体２の足平２２の近傍、下腿リンク、および大腿リンクにそれぞれ質点をもつような（各脚体２に３質点をもつような）変位次元補正用モデルを構築してもよい。この場合、第２あるいは第３実施形態のように、第１変位次元補正用モデルの質点の位置を拘束するときには、各足平質点以外の２つの質点を、例えば足首関節の中心と股関節の中心とを結ぶ線分上の所定の内分比で定まる２つの点に設定するようにすればよい。また、下腿リンクおよび／または上体リンクに相当するイナーシャをもつ剛体（リンク）を両変位次元補正用モデルの要素として加えてもよい。

また、第２および第３実施形態における第１および第２変位次元補正用モデルの足平質点Ａ４，Ａ５のように、両変位次元補正用モデルでの配置が同一位置となるような質点は、それを両変位次元補正用モデルから除いてもよい。

また、前記第１〜第３実施形態において、例えばロボット１の動作モードが前記通常モード（走行モードおよび低摩擦床面歩行モード）でないときに、ロボット１を立ち止まらせて両腕体５，５を共に前方に突き出したりするような運動を行う目標歩容を生成するような場合には、第１および第２変位次元補正用モデルで、各腕体５に対応する部位に質点やイナーシャをもたせるようにしてもよい。

さらには、両腕体５，５の肘関節の屈伸を行うような場合には、前記第２および第３実施形態での第１および第２変位次元補正用モデルで大腿質点を備えた場合と同様に、肘関節もしくはその近傍に対応する質点を備えるようにしてもよい。より具体的には、例えば図２７に示すように、第１および第２変位次元補正用モデルで、上体質点Ｂ１、大腿質点Ｂ２，Ｂ３および足平質点Ｂ４，Ｂ５に加えて、各腕体５の肘関節近傍にそれぞれ対応する肘質点Ｂ８，Ｂ９と、各腕体５の先端部近傍にそれぞれ対応する手先質点Ｂ６，Ｂ７を備えるようにして、第１変位次元補正用モデルでは、各腕体５の肩関節の中心と手首関節の中心とを結ぶ線分上の、所定の内分比で定まる点に肘質点Ｂ８，Ｂ９が拘束されるようにする。そして、第１変位次元補正用モデルと第２変位次元補正用モデルとの間での肘関節Ｂ８，Ｂ９の位置の差を含めて、前記第２あるいは第３実施形態と同様に、モデル間全体重心ずれGc_errおよびモデル間角運動量積ずれL_errが０に近づくように（前記条件１、２を満たすように）第１仮補正上体位置姿勢を求めると共に、上体姿勢を単純化モデル歩容の上体姿勢と同一にし、もしくは第１仮補正上体姿勢に前記重みw1を乗じたものと単純化モデル歩容の上体姿勢に前記重みw2を乗じたものとの和と同一にした状態で、モデル間角運動量積ずれL_errが０に近づくように（前記条件２を満たすように）第２仮補正上体位置姿勢を決定する。なお、第１変位次元補正用モデルに関しては、第１実施形態で、各脚体２の姿勢を拘束した場合と同様に、第１変位次元補正用モデルの腕姿勢を、ロボット１の直立姿勢状態における腕姿勢（上下方向に伸ばした姿勢）に拘束するようにしてもよい。

また、第１〜第３実施形態に関して補足すると、第１実施形態では、本発明における第１の幾何学的拘束条件に相当する前記幾何学的拘束条件（１）と、第２の幾何学的拘束条件に相当する前記幾何学的拘束条件（２）とが前記した如く設定されていることによって、それらの幾何学的拘束条件（１）、（２）は本発明の第９発明の如く設定されていることとなっている。同様に、第２および第３実施形態では、本発明における第１の幾何学的拘束条件に相当する前記幾何学的拘束条件（３）と、第２の幾何学的拘束条件に相当する前記幾何学的拘束条件（２）とが前記した如く設定されていることによって、それらの幾何学的拘束条件（３）、（２）は本発明の第８発明の如く設定されていることとなっている。

さらに、第１〜第３実施形態では、第１変位次元補正用モデルの全ての要素の質量の総和がロボット１の総質量と一致し、ロボット１の瞬時目標運動に対する第１変位次元補正用モデルの全体重心位置G1は、該瞬時目標運動に対する単純化モデルの全体重心位置Gsに一致あるいはほぼ一致するようにしている。また、第２変位次元補正用モデルの全ての要素の質量の総和もロボット１の総質量と一致し、ロボット１の瞬時目標運動に対する第２変位次元補正用モデルの全体重心位置G2は、該瞬時目標運動に対する実際のロボット１の真の全体重心位置Gfにほぼ一致するようにしている。従って、第１〜第３実施形態では、G1とG2との差（G1−G2）は、単純化モデルの全体重心位置Gsとロボット１の真の全体重心位置Gfとの差（Gs−Gf）、すなわち単純化モデルの全体重心位置の誤差にほぼ一致するものとなっている。従って、第１〜第３実施形態は、本発明における第１の幾何学的拘束条件としての前記幾何学的拘束条件（１）または（３）と、第２の幾何学的拘束条件としての前記幾何学的拘束条件（２）とが前記した第９発明の如く設定されていることとなる。なお、この場合において、先に述べたように、足平の近傍の質点（足平質点）のように、両変位次元補正用モデルでの配置が同一位置となるような質点は、それを両変位次元補正用モデルから除いてもよいことはもちろんである。

以上説明した如く、本発明は、動力学モデルを用いて作成された瞬時目標歩容の運動を、微分方程式や積分方程式を含まない幾何学的な演算によって、適切に補正し、その補正後の運動を含む瞬時目標歩容の動力学的精度を高めることと、ロボットの上体などの所定の部位の姿勢の変動を極力少なくすることとを両立できる移動ロボットの歩容生成装置を提供できる点で有用である。

本発明の実施形態を適用する移動ロボット（２足歩行ロボット）の全体構成を概略を示す図。 図１のロボットの各脚体の足平部分の構成を示す側面図。 図１のロボットに備えた制御ユニットの構成を示すブロック図。 図３の制御ユニットの機能的構成を示すブロック図。 図４に示す歩容生成装置の機能を示すブロック図。 第１実施形態における単純化モデル（動力学モデル）の構造を示す図。 （ａ）〜（ｃ）は、第１実施形態における第１変位次元補正用モデルと単純化モデルとの関係示す図。 第１実施形態における第２変位次元補正用モデルの構造を示す図。 第１実施形態における歩容生成装置のメインルーチン処理を示すフローチャート。 第１実施形態における図９のフローチャートの変位次元歩容補正サブルーチンの処理を示すフローチャート。 第１実施形態における図１０のＳ１００のサブルーチン処理を示すフローチャート。 第１実施形態における図１０のＳ１０２のサブルーチン処理を示すフローチャート。 第１実施形態における角運動量積の算出を説明するための図。 第１実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第１実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第１実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 フルモデル補正で使用するフルモデルの例を示す図。 第２実施形態における単純化モデル（動力学モデル）の構造を示す図。 第２実施形態における第１変位次元補正用モデルの構造を示す図。 第２実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第２実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第２実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第３実施形態における図９のフローチャートの変位次元歩容補正サブルーチンの処理を示すフローチャート。 図２３のＳ５０６のサブルーチン処理を示すフローチャート。 第３実施形態における第１および第２変位次元補正用モデルの要素の配置例を示す図。 （ａ），（ｂ）は重みw1に周波数特性を持たせた例を示すグラフ。 第１および第２変位次元補正用モデルの要素の配置の他の例を示す図。

Claims (15)

1. 移動ロボットの瞬時目標運動と瞬時目標床反力とからなる瞬時目標歩容を逐次発生する瞬時歩容発生手段を備えた歩容生成装置において、
   前記瞬時目標運動から前記移動ロボットの所定の部位の位置および姿勢を仮補正してなる第１の仮補正瞬時目標運動を決定する第１仮補正運動決定手段と、
   前記瞬時目標運動から前記所定の部位の姿勢を該瞬時目標運動における姿勢と同一に維持しつつ、該所定の部位の位置を仮補正してなる第２の仮補正瞬時目標運動を決定する第２仮補正運動決定手段と、
   前記第１の仮補正瞬時目標運動と第２の仮補正瞬時目標運動とを基に、前記瞬時目標運動における前記所定の部位の位置および姿勢の真の補正を実行してなる補正後瞬時目標運動を決定する目標運動補正手段とを備え、
   前記移動ロボットの全体または一部を、イナーシャをもつ剛体と質点とのうちの少なくともいずれか一方を要素として、複数の要素からなるモデルで表現し、前記瞬時歩容発生手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第１の配置とし、前記第１仮補正運動決定手段が決定した第１の仮補正瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第２の配置とし、前記第２仮補正運動決定手段が決定した第２の仮補正瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第３の配置としたとき、
   前記第１仮補正運動決定手段は、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記第１の仮補正瞬時目標運動を決定し、
   前記第２仮補正運動決定手段は、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が前記所定の点まわりに発生するモーメント成分がほぼ前記所定の値になるように前記第２の仮補正瞬時目標運動を決定し、
   前記目標運動補正手段は、前記第１の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における前記所定の部位の瞬時目標姿勢として決定するとともに、前記第１の仮補正瞬時目標運動における前記所定の部位の位置に前記所定の重みｗ1を乗じたものと前記第２の仮補正瞬時目標運動における前記所定の部位の位置に前記所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における前記所定の部位の瞬時目標位置として決定し、
   前記第１仮補正運動決定手段と前記第２仮補正運動決定手段とがそれぞれ前記第１の仮補正瞬時目標運動と前記第２の仮補正瞬時目標運動とを決定するために用いる前記所定の値は、前記瞬時目標床反力のモーメント成分に対する動力学的精度が、前記瞬時目標運動よりも前記第１の仮補正瞬時目標運動及び前記第２の方が高まるようにあらかじめ設定された値であることを特徴とする移動ロボットの歩容生成装置。
2. 移動ロボットの瞬時目標運動と瞬時目標床反力とからなる瞬時目標歩容を逐次発生する瞬時歩容発生手段を備えた歩容生成装置において、
   前記瞬時目標運動から前記移動ロボットの所定の部位の位置および姿勢を仮補正してなる仮補正瞬時目標運動を決定する仮補正運動決定手段と、
   前記瞬時目標運動における前記所定の部位の位置および姿勢の真の補正を実行してなる補正後瞬時目標運動を決定する目標運動補正手段とを備え、
   前記移動ロボットの全体または一部を、イナーシャをもつ剛体と質点とのうちの少なくともいずれか一方を要素として、複数の要素からなるモデルで表現し、前記瞬時歩容発生手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第１の配置とし、前記仮補正運動決定手段が決定した仮補正瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素の配置との関係を規定する所定の第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第２の配置とし、前記目標運動補正手段が決定する補正後瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置を第３の配置としたとき、
   前記仮補正運動決定手段は、前記第２の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力の並進力成分がほぼ０になり、且つ、該合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記仮補正瞬時目標運動を決定し、
   前記目標運動補正手段は、前記仮補正瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ1を乗じたものと前記瞬時歩容発生手段が発生した前記瞬時目標運動における前記所定の部位の姿勢に所定の重みｗ2を乗じたものとの和を前記補正後瞬時目標運動における該所定の部位の瞬時目標姿勢として決定すると共に、前記第３の配置と第１の配置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が所定の点まわりに発生するモーメント成分がほぼ所定の値になるように前記補正後瞬時目標運動における前記所定の部位の瞬時目標位置を決定し、
   前記仮補正運動決定手段と前記目標運動補正手段とがそれぞれ前記仮補正瞬時目標運動と前記補正後瞬時目標運動とを決定するために用いる前記所定の値は、前記瞬時目標床反力のモーメント成分に対する動力学的精度が、前記瞬時目標運動よりも前記仮補正瞬時目標運動及び前記補正後瞬時目標運動の方が高まるようにあらかじめ設定された値であることを特徴とする移動ロボットの歩容生成装置。
3. 前記所定の重みｗ1および所定の重みｗ2は、いずれも、その大きさが０から１までの範囲内の大きさであり、少なくとも前記所定の重みｗ1を、前記目標歩容に応じて前記移動ロボットの動作を行わせようとする路面状態と該目標歩容に応じた移動ロボットの運動形態とのうちの少なくともいずれか一方に応じて可変的に決定する手段を備えることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
4. 前記所定の重みｗ1の大きさと前記所定の重みｗ2の大きさとの和は１であることを特徴とする請求項３記載の移動ロボットの歩容生成装置。
5. 前記所定の重みｗ1は、これを乗じる前記所定の部位の姿勢に対して周波数特性をもつ重みであることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
6. 前記第２の配置と第１の配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの質量をもつ各要素の、前記第１の配置における位置Ａと前記第２の配置における位置Ｂの差に起因する成分は、前記所定の点と前記位置Ａとを結ぶ線分と、前記所定の点と前記位置Ｂとを結ぶ線分とがなす角度から、該角度に関する実質的に単調な関数を用いて算出され、
   前記第３の配置と第１の配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの質量をもつ各要素の、前記第１の配置における位置Ａと前記第３の配置における位置Ｃの差に起因する成分は、前記所定の点と前記位置Ａとを結ぶ線分と、前記所定の点と前記位置Ｃとを結ぶ線分とがなす角度から前記単調な関数を用いて算出されることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
7. 前記瞬時歩容発生手段が発生する瞬時目標運動は、前記移動ロボットの運動と床反力との関係を表す動力学モデルであって、少なくとも該移動ロボットの１つ以上の特定部位の特定の運動成分によって発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて決定されており、前記モデルは、前記特定部位のうちの少なくとも１つの部位に対応する要素を含むことを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
8. 前記瞬時歩容発生手段が発生する瞬時目標運動は、前記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標床反力または目標ＺＭＰを満足するように決定されており、
   前記瞬時目標運動から、前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の時間的変化によって発生する各要素の慣性力の合力に釣り合う床反力と、前記瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の時間的変化によって発生する各要素の慣性力の合力に釣り合う床反力との差に所定の定常オフセットを加えたものが、前記瞬時目標運動によって前記動力学モデルで発生する床反力の誤差に略一致するように前記第１および第２の幾何学的拘束条件が設定されていることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
9. 前記瞬時歩容発生手段が発生する瞬時目標運動は、前記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標床反力または目標ＺＭＰを満足するように決定されており、
   前記瞬時目標運動から、前記第１の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の全体重心と、前記瞬時目標運動から、前記第２の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の全体重心との差に該要素の総質量を乗じたものが、前記瞬時目標運動における前記動力学モデルの全体重心の誤差に該動力学モデルの総質量を乗じたものに略一致するように前記第１および第２の幾何学的拘束条件が設定されていることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
10. 前記移動ロボットは、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えるロボットであり、前記第１の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と、該可動体の前記上体との連結部近傍の所定の点を結ぶ線分に平行な直線上に前記モデルの要素のうちのいずれかが存在するという条件を含むことを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
11. 前記移動ロボットは、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えるロボットであり、前記第１の幾何学的拘束条件は、前記モデル上での前記上体と各可動体とが所定の一定姿勢状態に保持されるという条件を含むことを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
12. 前記所定の一定姿勢は、前記移動ロボットの上体と複数の可動体とをほぼ鉛直方向に向けた姿勢であることを特徴とする請求項１１記載の移動ロボットの歩容生成装置。
13. 前記第２の幾何学的拘束条件は、前記移動ロボットの任意の瞬時目標運動から、その条件に従って決定される前記モデルの各要素の配置が、前記瞬時目標運動に従う前記ロボットにおける該要素に対応する部位の配置に略一致するように設定されていることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
14. 前記移動ロボットは、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えると共に、各可動体の上体との連結部と該可動体の先端部との間の中間部とに屈曲可能な関節を有し、前記瞬時歩容発生手段が発生する瞬時目標運動は、前記ロボットの運動と床反力との関係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可動体の中間部もしくはその近傍で発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて決定されており、前記モデルは少なくとも前記各可動体の中間部もしくはその近傍部位に対応させた質点を要素として含むモデルであることを特徴とする請求項１または２記載の移動ロボットの歩容生成装置。
15. 前記第１の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と、該可動体の前記上体との連結部近傍の所定の点を結ぶ線分に、前記モデルの要素のうちの、該可動体の中間部もしくはその近傍部位に対応させた質点が存在するという条件を含み、前記第２の幾何学的拘束条件は、前記移動ロボットの任意の瞬時目標運動から、その条件に従って決定される前記モデルの各要素の配置が、前記瞬時目標運動に従う前記ロボットにおける該要素に対応する部位の配置に略一致するように設定されていることを特徴とする請求項１４記載の移動ロボットの歩容生成装置。

Images