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

Description

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

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

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

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

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

特開２００２−３２６１７３号公報 ＰＣＴ国際公開公報ＷＯ／０３／０５７４２７／Ａ１

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

本発明はかかる背景に鑑みてなされたものであり、瞬時歩容発生手段が発生した瞬時目標運動では考慮されていない、各脚体の中間部の関節の屈曲動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該瞬時目標運動を補正することができる移動ロボットの歩容生成装置を提供することを目的とする。

本発明の移動ロボットの歩容生成装置は、上体から延設された複数の脚体または複数の腕体を複数の可動体として備えると共に、各可動体の上体との連結部と該可動体の先端部との間の中間部とに屈曲可能な関節を有する移動ロボットの瞬時目標運動を逐次発生する瞬時歩容発生手段を備え、その瞬時目標運動が、前記ロボットの運動と床反力との関係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可動体の中間部もしくはその近傍で発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて生成された移動ロボットの歩容生成装置において、
前記各可動体の中間部の関節の曲げ角を参照量Ａとし、前記各可動体の先端部近傍の所定の点と、該可動体の上体との連結部近傍の所定の点とを結ぶ線分と、該可動体の中間部の関節の中心または該関節に連結されたリンクの重心との前記線分に交差する方向での相対位置を参照量Ｂとしたとき、前記参照量Ａ又は参照量Ｂに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定し、その決定した補正量で該瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方を補正するように瞬時目標運動を補正することによって、当該補正後の瞬時目標運動の動力学的精度を当該補正前の瞬時目標運動よりも高めるように当該補正前の瞬時目標運動を補正する瞬時目標運動補正手段を備えたことを特徴とする。

かかる本発明では、前記各可動体の中間部の関節の曲げ角（前記参照量Ａ）に応じて、あるいは、前記線分と可動体の中間部の関節の中心または該関節に連結されたリンクの重心との前記線分に交差する方向での相対位置（前記参照量Ｂ。線分からの距離など）に応じて、フィードフォワード制御則によって直接的に前記瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量が決定される。これにより、瞬時歩容発生手段が発生した瞬時目標運動では考慮されていない、各可動体の中間部の関節の屈曲動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該瞬時目標運動を補正することが可能となる。この場合、フィードフォワード制御則によって補正量を決定するので、瞬時瞬時で直ちに上記慣性力の影響を補償するような補正量を決定できる。

そして、前記瞬時目標運動補正手段が、前記参照量Ａに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定するものである場合において、前記各可動体が脚体であるときには、前記瞬時目標運動補正手段は、前記各可動体の中間部の関節の曲げ角が増加するに伴い、前記瞬時目標運動の上体姿勢をより大きく前傾させ、且つ、前記複数の可動体を除いた移動ロボットの全体の重心位置をより後方に変位させるように前記補正量を決定することが好ましい。

同様に、前記瞬時目標運動補正手段は、前記参照量Ａに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定するものである場合において、前記各可動体が脚体であるときには、前記瞬時目標運動補正手段は、前記線分と、前記各可動体の中間部の関節の中心または該関節に連結されたリンクの重心との距離を前記相対位置として用い、その距離が増加するに伴い、前記瞬時目標運動の上体姿勢をより大きく前傾させ、且つ、前記複数の可動体を除いた移動ロボットの全体の重心位置をより後方に変位させるように前記補正量を決定することが好ましい。

このように、各可動体（各脚体）の中間部の関節の曲げ角と、上体位置と上体姿勢とのうちの少なくともいずれか一方に関する補正量とを相関付け、あるいは、前記線分と前記各可動体（脚体）の中間部の関節の中心または該関節に連結されたリンクの重心との距離と、上体位置と上体姿勢とのうちの少なくともいずれか一方に関する補正量とを相関付ける。これによって、前記第１７発明と同様に、各脚体の中間部の関節の屈曲動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該瞬時目標運動を補正できる。

本発明の実施形態を適用する移動ロボット（２足歩行ロボット）の全体構成を概略を示す図。 図１のロボットの各脚体の足平部分の構成を示す側面図。 図１のロボットに備えた制御ユニットの構成を示すブロック図。 図３の制御ユニットの機能的構成を示すブロック図。 図４に示す歩容生成装置の機能を示すブロック図。 第１参考例における単純化モデル（動力学モデル）の構造を示す図。 図７（ａ）〜（ｃ）は、第１参考例における第１変位次元補正用モデルと単純化モデルとの関係示す図。 第１参考例における第２変位次元補正用モデルの構造を示す図。 第１参考例における歩容生成装置のメインルーチン処理を示すフローチャート。 図９のフローチャートにおける変位次元歩容補正サブルーチンの処理を示すフローチャート。 第１参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第１参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第１参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 図１４（ａ），（ｂ）は第１参考例において、ロボットの直立姿勢状態から上体を下げるときの第１および第２変位次元補正用モデルの要素の配置の変化を示す図。 フルモデル補正で使用するフルモデルの例を示す図。 第２参考例における単純化モデル（動力学モデル）の構造を示す図。 第２参考例における第１変位次元補正用モデルの構造を示す図。 第２参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第２参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第３参考例における第１変位次元補正用モデルを示す図。 第３参考例における第２変位次元補正用モデルを示す図。 第３参考例における第１および第２変位次元補正用モデルの要素の配置例を示す図。 第３参考例における変位次元歩容補正サブルーチンの処理を示すフローチャート。 本発明の一実施形態における変位次元歩容補正サブルーチンの処理を示すフローチャート。 第１および第２変位次元補正用モデルの要素の配置の他の例を示す図。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

そして、本明細書で詳説する参考例及び実施形態では、上記点Ｑは例えば目標ＺＭＰに設定される。なお、点Ｑは、目標ＺＭＰに限られるものではないが、これについては後に補足する。また、必ずしも上記条件１，２の両者を満たす必要はないが、これについても後に補足する。

本明細書の参考例及び実施形態では、上記した如く、第２変位次元補正用モデルの各質点の位置（イナーシャを持つ場合には、各質点の位置とイナーシャをもつ各リンクの姿勢角）を決定することで、単純化モデル歩容の目標上体位置姿勢を補正してなる変位次元補正上体位置姿勢が求められる。前記図６のフローチャートのＳ０２４の処理は、上記の如く変位次元補正上体位置姿勢を求める処理である。

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

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

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

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

Ｐb2_s＝Ｐb＋（Ｐb2_p−Ｐb_p） …式０４
θb2_s＝θb＋（θb2_p−θb_p） …式０５

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

具体的には、足平質点Ａ３，Ａ４の位置は、目標足平位置姿勢から決定される。また、上体質点Ａ１の位置は、候補（Ｐb2_s，θb2_s）から決定され、上体３（上体リンク）の姿勢角はθb2_sと同一とされる。そして、各大腿質点Ａ２，Ａ３の位置は、目標両足平位置姿勢と候補（Ｐb2_s，θb2_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に変位次元補正上体姿勢の候補の現在値θb2_sを代入することで、モデル間角運動量積ずれＬc_errが算出される。

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

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

以上のようにして、Ｓ２０６〜Ｓ２１６のループ処理によって、Gc_errとL_errとが０近傍の所定の範囲内に収まるような変位次元補正上体位置姿勢、換言すれば、前記条件１，２を満たすような変位次元補正上体位置姿勢が探索的に求められる。

そして、Ｓ２１０の判断結果がＹＥＳになると、Ｓ２１２を経てＳ２１８に進み、現在の（Ｐb2_s，θb2_s）が今回時刻ｔにおける変位次元補正上体位置姿勢（Ｐb2，θb2）として決定される。これにより、単純化モデル歩容の上体位置姿勢を補正してなる目標歩容（以下、変位次元補正歩容ということがある）が得られることとなる。この変位次元補正歩容は、単純化モデル歩容のうちの、目標上体位置姿勢のみを補正したものであり、目標足平位置姿勢、目標ＺＭＰ、目標床反力鉛直成分など、目標歩容の他の構成要素は単純化モデル歩容と同一である。

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

また、図１３は上記した変位次元歩容補正サブルーチンによって決定される変位次元補正歩容に対応する第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。

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

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

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

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

これにより、変位次元補正歩容は、単純化モデル歩容では考慮されていない各脚体の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力と同じような床反力が発生するようにロボット１の目標運動を補正したものとなる。

また、図１４は、単純化モデル歩容に対する変位次元補正歩容の変化の形態の典型的な例を示している。これは、より詳しくは、ロボット１の直立姿勢状態から目標ＺＭＰを動かさずに両脚体２，２の膝関節を曲げて、上体３を下げる場合の例であり、図１４（ａ）は、ロボット１の直立姿勢状態、図１４（ｂ）は膝関節を曲げて上体３を下げた状態（膝曲げ状態）を示している。ロボット１の直立姿勢状態では、単純化モデル歩容と変位次元補正歩容とはほぼ一致し、ひいては、図１４（ａ）に示す如く、第１および第２変位次元補正用モデルの各質点Ａｉ（ｉ＝１，２，…，５）の位置Ｐi1，Ｐi2および上体３の姿勢は、両変位次元補正用モデルでほぼ一致する。

この状態から、目標ＺＭＰを維持したまま上体３を下げるような歩容を生成するとき、単純化モデル歩容は、両足平２２の位置姿勢を維持すると共に、上体３の姿勢を直立姿勢（鉛直姿勢）に維持したまま、上体３を下げる歩容となる。従って、第１変位次元補正用モデルでの各質点Ａｉの位置Ｐi1は、それらの位置関係を維持したまま図１４（ａ）の位置から下がる。そして、このとき、両脚体２，２の膝関節がロボット１の前方側に突き出ることに対応して、第２変位次元補正用モデルでは、その大腿質点Ａ２，Ａ３の位置Ｐ22，Ｐ32が、第１変位次元補正用モデルの大腿質点Ａ２，Ａ３の位置Ｐ21，Ｐ31よりも図１４（ｂ）に示す如くロボット１の前方側の位置になる。このため、両変位次元補正用モデルで全体重心を一致させるべく（前記条件１を満たすべく）、第２変位次元補正用モデルの上体質点Ａ１の位置Ｐ12は、第１変位次元補正用モデルの上体質点Ａ１の位置Ｐ11よりも後方側の位置になる。別の言い方をすれば、両大腿質点Ａ２，Ａ３のモデル間擬似並進加速度に伴う慣性力の並進力成分を、上体質点Ａ１のモデル間擬似並進加速度に伴う慣性力の並進力成分によって打ち消すようにして、第２変位次元補正用モデルの上体質点Ａ１の位置Ｐ12は、第１変位次元補正用モデルの上体質点Ａ１の位置Ｐ11よりも後方側の位置になる（脚体２，２の上体３への連結部（股関節）が後方側に移る）。

また、このとき、仮に上体３の姿勢を維持すると、第１変位次元補正用モデルと第２変位次元補正用モデルとの間で前記モデル間角運動量積ずれＬ_errが発生する。この場合、このモデル間角運動量積ずれは、図１４（ｂ）に斜線を付した２つの三角形の面積の差分の２倍に相当するものとなるので、このずれを解消すべく（前記条件２を満たすべく）、第２変位次元補正モデルでの上体姿勢（図１４（ｂ）に実線で示す上体３の姿勢）、すなわち変位次元補正歩容の上体姿勢は、第１変位次元補正モデルでの上体姿勢（図１４（ｂ）に破線で示す上体３）、すなわち、単純化モデル歩容の上体姿勢に対して前傾側に傾く。別の言い方をすれば、両大腿質点Ａ２，Ａ３および上体質点Ａ１のモデル間擬似並進加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントを、上体リンクのモデル間擬似角加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントを打ち消すようにして、第２変位次元補正歩容の上体姿勢は、単純化モデル歩容の上体姿勢に対して前傾側に傾く。

一般的にいえば、各脚体２の膝関節が、該脚体２の股関節の中心と足首関節の中心とを結ぶ線分に対してロボット１の前方側に突き出るほど、第２変位次元補正用モデルの上体質点Ａ１の位置は、ロボット１の後方側に移動し、また、上体３（上体リンク）は前傾側に傾く。

このように単純化モデル歩容に対して運動（上体位置姿勢）を修正した変位次元補正歩容が決定されることで、目標ＺＭＰまわりの床反力モーメントの水平成分が０になること等の動力学的平衡条件を単純化モデル歩容と同じように満たしつつ、単純化モデル歩容よりも動力学的精度の高い変位次元補正歩容が決定されることとなる。

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

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

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

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

以上が、本参考例における歩容生成装置１００の歩容生成処理である。

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

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

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

ロボット１に生じた床反力（詳しくは実各足床反力）は６軸力センサ５０によって検出される。その検出値は前記複合コンプライアンス動作決定部１０４に送られる。また、ロボット１に生じた姿勢傾斜偏差θ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の鉛直位置に対応して決定される。そして、目標床反力鉛直成分が比較的大きい時期では、主に上体３の水平加速度を調整して、目標ＺＭＰまわりのモーメント水平成分が０になるようにしつつ、目標上体姿勢の瞬時値が基準上体姿勢（例えば鉛直姿勢）に近づくように目標上体姿勢および目標上体位置水平成分が決定される。また、目標床反力鉛直成分が比較的小さいか、もしくは０となる時期では、上体３の水平加速度をほぼ０にしつつ、主に上体３の姿勢角の角加速度を調整して、目標ＺＭＰまわりのモーメント水平成分が０になるように、目標上体姿勢および目標上体位置水平成分の瞬時値が決定される。

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

次いで、Ｓ０２４に進んで変位次元補正サブルーチンが実行される。このサブルーチン処理は、基本的処理手順は、前記第１参考例と同一であり、前記図１０のフローチャートに従って実行される。すなわち、まず、Ｓ２００において、現在時刻ｔの単純化モデル歩容の瞬時値を基に、第１変位次元補正用モデルの各質点Ａ１〜Ａ５の位置とイナーシャをもつ上体リンクの姿勢角とを求める。この場合、前記したように、第１変位次元補正用モデルの上体質点Ａ１の位置は、単純化モデル歩容の上体位置姿勢の瞬時値に対応する位置に決定され、各足平質点Ａ４，Ａ５の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、各大腿質点Ａ２，Ａ３の位置は、単純化モデル歩容の上体位置姿勢と各足平位置姿勢を基に定まるロボット１の各脚体２の股関節の中心点と足首関節の中心点とを結ぶ線分を、所定の比率で内分してなる内分点の位置に決定される。また、第１変位次元補正用モデルの上体リンクの姿勢角は、単純化モデル歩容の上体姿勢角と同一とされる。

これにより単純化モデル歩容の瞬時目標運動（今回時刻ｔの瞬時値）から、本参考例での第１変位次元補正用モデルに係る前記幾何学的拘束条件（３）に従って、第１変位次元補正用モデルの各要素の配置が決定されることとなる。

次いで、Ｓ２０２からＳ２１８までの処理が実行される。これらの処理は、第１参考例と同じである。すなわち、前記条件１，２を満たすような第２変位次元補正用モデルの要素の配置、ひいては変位次元補正上体位置姿勢が探索的に求められ、それが、今回時刻ｔにおける変位次元補正上体位置姿勢（Ｐb2，θb2）として決定される。これにより、単純化モデル歩容の上体位置姿勢を修正してなる変位次元補正歩容が得られる。補足すると、本参考例では、各変位次元補正用モデルの両足平質点Ａ５，Ａ６の位置は、両変位次元補正用モデルで同じである。従って、Ｓ２０８でモデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errとを算出するに当たっては、両足平質点Ａ５，Ａ６に係わる項は省略してもよい。

図９の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、Ｓ０２６〜Ｓ０３２の処理が第１参考例と同様に実行される。これらの処理は、第１参考例と同一である。

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

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

また、図１９は上記した変位次元歩容補正サブルーチンによって決定される第２変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角と、第１変位次元補正用モデルの各質点Ａｉの位置および上体３（上体リンク）の姿勢角との関係を例示している。

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

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

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

これにより、変位次元補正歩容は、単純化モデル歩容では考慮されていない各脚体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力と同じような床反力が発生するようにロボット１の目標運動を運動を補正したものとなる。

なお、本参考例によるときには、ロボット１の直立姿勢状態から目標ＺＭＰを一定に維持して、両脚体２，２の膝関節を曲げつつ、上体３を下げるような歩容を生成したときには、前記第１参考例と同様に（前記図１４に示したものと同様に）、変位次元補正上体位置姿勢は、上体３と両脚体２，２との連結部である股関節（上体３の下端部）が単純化モデル歩容のよりもロボット１の後方側（つまり膝関節が突き出る向きとほぼ逆向き）に変位し、且つ、上体３が単純化モデル歩容の上体姿勢よりも前傾側に傾くような位置姿勢となる。

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

本参考例では、単純化モデルは、例えば前記図８の第２変位次元補正用モデルと同じ構造のモデルとされている。すなわち、本参考例の単純化モデルは、前記図８に示した如く、上体３、両脚体２，２の大腿リンク、両足平２２，２２にそれぞれ対応する上体質点Ａ１、大腿質点Ａ２，Ａ３、足平質点Ａ４，Ａ５からなる５質点を有し、また、上体３（上体リンク）が上体質点Ａ１のまわりにイナーシャＩｂを持つ動力学モデルである。この単純化モデルの動力学は、その式の記載は省略するが、前記した第２参考例の単純化モデルなどと同様に、各質点Ａ１〜Ａ５の並進運動および上体リンクの回転運動と、床反力（並進床反力および目標ＺＭＰまわりの床反力モーメント）との関係式として記述される。例えば、各質点Ａ１〜Ａ５および上体リンクの動力学は相互に非干渉に構成され、ロボット１の全体の動力学は、それらの線形結合で表される。

図２０は、本参考例における第１変位次元補正用モデルの構造を示している。なお、図２０では、ロボット１の姿勢状態が両脚体２，２を左右方向（Ｙ軸方向）に並列させて起立している状態であるため、両脚体２，２が図面上、重なっている。

この第１変位次元補正用モデルは、ロボット１の上体３に対応する１つの上体質点Ｂ１、各脚体２の膝関節近傍の大腿リンク部分に対応する大腿質点Ｂ２，Ｂ３、各脚体２の先端部（足平２２）にそれぞれ対応する足平質点Ｂ４，Ｂ５、並びに、ロボット１の各腕体５にそれぞれ対応する腕質点Ｂ６，Ｂ７からなる７質点モデルである。また、第１変位次元補正用モデルにおける上体３（上体リンク）は、上体質点Ａ１のまわりにイナーシャ（慣性モーメント）Ｉｔを持つと共に、各腕体５は、各腕質点Ｂ６，Ｂ７のまわりにイナーシャＩａを持つものとされている。つまり、本参考例の第１変位次元補正用モデルは、質点Ｂ１〜Ｂ７と、イナーシャＩｔをもつ上体３と、イナーシャＩａをそれぞれつ腕体５，５とを要素として構成されている。

この場合、第１変位次元補正用モデルでの上体質点Ｂ１と、大腿質点Ｂ２，Ｂ３と、足平質点Ｂ４，Ｂ５とは、前記第１参考例の第２変位次元補正用モデルの各質点Ａ１〜Ａ５と同様に、それぞれに対応する部位（上体３、各大腿リンク、各足平２２）に任意に固定されたローカル座標系上でのある固定点に設定されている。また、腕質点Ｂ６，Ｂ７は、各腕体５の１つのリンク、例えば腕体５の肘関節と手首関節との間のリンクに任意に固定されたローカル座標系でのある固定点（腕体５の先端部寄りの点）に設定されている。

なお、第１変位次元補正用モデルの各質点Ｂ１〜Ｂ７の質量の総和は、ロボット１の総質量と同一であると共に、上体質点Ｂ１および腕質点Ｂ６，Ｂ７の質量の総和は、本参考例における単純化モデル（図８）の上体質点Ａ１と同一の質量とされている。

そして、本参考例においても第１変位次元補正用モデルの要素の配置には、ある幾何学的拘束条件が設定されている。具体的には、本参考例の第１変位次元補正用モデルでは、両腕体５，５は、いずれも上体３に対して図２０に示すような姿勢状態に固定された状態に拘束されている。すなわち、第１変位次元補正用モデルでの両腕体５，５の上体３に対する相対姿勢、ひいては、上体質点Ｂ１と腕質点Ｂ６，Ｂ７との間の相対的位置関係は一定である。

補足すると、第１変位次元補正用モデルの上体質点Ｂ１および腕体質点Ｂ６，Ｂ７の重心点は、本参考例における単純化モデル（図８）の上体質点Ａ１と一致するものとされている。また、上体リンクのイナーシャＩｔと、各腕体５のイナーシャＩａとは、本参考例の単純化モデルの上体リンクのイナーシャＩｂと次の関係式１４を満たすように設定されている。

Ｉｂ＝Ｉｔ＋Ｉｂ
＋上体質点Ｂ１の質量＊(上体質点Ｂ１と上体質点Ａ１との距離)^２
＋腕体質点Ｂ６の質量＊(腕体質点Ｂ６と上体質点Ａ１との距離)^２
＋腕体質点Ｂ７の質量＊(腕体質点Ｂ７と上体質点Ａ１との距離)^２
…式１４

また、この第１変位次元補正用モデルの各質点Ｂ１〜Ｂ７のグローバル座標系上での位置、並びに上体３の姿勢は、単純化モデル歩容の運動の瞬時値に対応して幾何学的に定まるものとされている。より具体的には、本参考例の第１変位次元補正用モデルの上体質点Ｂ１のグローバル座標系での位置は、単純化モデル歩容の上体位置姿勢に対応する位置に決定され、各足平質点Ｂ４，Ｂ５のグローバル座標系での位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、各大腿質点Ｂ２，Ｂ３の位置は、単純化モデル歩容の上体位置姿勢および両足平位置姿勢から定まる各脚体２の大腿リンクの位置姿勢に対応する位置に決定される。なお、第１変位次元補正用モデルでは、上体３に対する腕体５，５の相対姿勢は前記した如く一定であるので、腕質点５，５のグローバル座標系での位置は、単純化モデル歩容の上体位置姿勢に対応して一義的に定まることとなる。

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

幾何学的拘束条件（４）：与えられた任意の瞬時目標運動に対して、第１変位次元補正用モデルの要素のうちの上体質点Ｂ１および上体リンクの配置が、与えられた瞬時目標運動におけるロボット１の上体３の位置姿勢に対応して定まる配置に一致し、且つ、各足平質点Ｂ４，Ｂ５の位置が、与えられた瞬時目標運動におけるロボット１の各足平位置姿勢に対応して定まる位置に一致し、且つ、各大腿質点Ｂ２，Ｂ３の位置が、与えられた瞬時目標運動における各脚体２の大腿リンクの位置姿勢に対応して定まる位置に一致し、且つ、各腕質点Ｂ６，Ｂ７および各腕体５（イナーシャを持つリンク）の配置が、与えられた瞬時目標運動におけるロボット１の上体３の位置姿勢に対して所定の相対関係を持つ配置になる。

図２１は本参考例における第２変位次元補正用モデルの構造を示している。この第２変位次元補正用モデルは、基本的構造は、図２０の第１変位次元補正用モデルと同じであり、第１変位次元補正用モデルと同様に７個の質点Ｂ１〜Ｂ７を有すると共に、上体３（上体リンク）が質点Ｂ１のまわりにイナーシャＩｔを持ち、また、各腕体５が腕質点Ｂ６，Ｂ７のまわりにイナーシャＩａを持つモデルである。各質点Ｂ１〜Ｂ７の質量と、各質点Ｂ１〜Ｂ７の、対応する部位に固定設定されたローカル座標系での位置とは、図２０の第１変位次元補正用モデルと同一である。また、各イナーシャＩｔ，Ｉａも図２０の第１変位次元補正用モデルと同一である。そして、この第２変位次元補正用モデルにおいては、各質点Ｂ１〜Ｂ７、並びに上体３（上体リンク）および各腕体５はロボット１が採り得る任意の姿勢状態に対応する位置姿勢に移動可能とされている。すなわち、ロボット１の任意の瞬時目標運動と第２変位次元補正用モデルの各要素の配置との間には、前記第１参考例で説明した前記幾何学的拘束条件（２）が設定されている。

この第２変位次元補正用モデルの各質点Ｂ１〜Ｂ７のグローバル座標系での位置に関しては、各足平質点Ｂ４，Ｂ５の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、上体質点Ｂ１、各大腿質点Ｂ２，Ｂ３の位置、および各腕質点５の位置並びに上体３（上体リンク）および腕体５の姿勢は、前記条件１，２を満たし、且つ、上体３に対する腕体５の相対姿勢が単純化モデル歩容の相対姿勢に一致するように決定される。

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

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

そして、Ｓ０１８の次にＳ０２０の処理が実行され、今回歩容の歩容パラメータが決定される。すなわち、今回歩容の目標足平位置姿勢軌道、目標腕姿勢軌道、目標ＺＭＰ軌道、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、基準上体姿勢軌道、床反力水平成分許容範囲、およびＺＭＰ許容範囲を規定するパラメータが決定される。この場合、Ｓ０２０の処理は、例えば前記第２参考例と同様に行えばよい。但し、本参考例では、Ｓ０２０の処理を行うための動力学モデル（単純化モデル）として、前記図１６の動力学モデルに代えて、図８の単純化モデルが用いられる。

次いで、図９のＳ０２０の処理の後、あるいはＳ０１６の判断結果がＮＯであった場合には、Ｓ０２２に進んで、歩容パラメータ（Ｓ０２０で決定した歩容パラメータ）を基に、今回歩容（単純化モデル歩容）の瞬時値が決定される。この処理は、例えば前記第２参考例と同様に行われる。但し、本参考例では、今回歩容の瞬時値（詳しくは上体位置姿勢の瞬時値）を決定するために、図８の単純化モデルが用いられる。補足すると、本参考例における図８の単純化モデルでは、上体３に対する両腕体５，５の相対姿勢は前記図２０に示した姿勢状態に固定されているものとしている。従って、Ｓ０２２で求められる今回歩容の瞬時値のうち、上体位置姿勢の瞬時値は、上体３に対する両腕体５，５の相対姿勢を図２０の姿勢状態に固定していると見なして、図８の単純化モデル上で目標ＺＭＰまわりの床反力モーメントの水平成分が０になるように決定されたものとなる。

次いで、Ｓ０２４に進んで変位次元補正サブルーチンが実行される。このサブルーチン処理は、基本的処理手順は、前記第１参考例と同様であるが、一部の処理が第１参考例と相違している。その処理は図２３のフローチャートに従って実行される。すなわち、まず、Ｓ４００において、現在時刻ｔの単純化モデル歩容の瞬時値を基に、第１変位次元補正用モデルの各質点Ｂ１〜Ｂ７の位置と、イナーシャをもつ上体リンクおよび各腕体５，５の姿勢を求める。この場合、前記したように、本参考例の第１変位次元補正用モデルの上体質点Ｂ１および両腕質点Ｂ６，Ｂ７の位置は、単純化モデル歩容の上体位置姿勢に対応する位置に決定され、各足平質点Ｂ４，Ｂ５の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置に決定される。また、各大腿質点Ｂ２，Ｂ３の位置は、単純化モデル歩容の上体位置姿勢および両足平位置姿勢から定まる各脚体２の大腿リンクの位置姿勢に対応する位置に決定される。また、第１変位次元補正用モデルの上体リンクの姿勢は、単純化モデル歩容の上体姿勢と同一とされる。なお、第１変位次元補正用モデルの各腕体５，５の姿勢は、前記した如く、上体３に対して図２０の姿勢状態に固定されているので、第１変位次元補正用モデルの上体３（上体リンク）の姿勢が定まれば、一義的に各腕体５，５の姿勢も定まる。従って、第１変位次元補正用モデルでの各腕体５，５の姿勢は、一般には単純化モデル歩容の腕姿勢の瞬時値とは異なる。

次いで、Ｓ４０２に進んで、前記図１０のＳ２０２と全く同様に、変位次元補正上体位置姿勢の初期候補が決定される。

次いで、Ｓ４０４を経て、Ｓ４０６〜Ｓ４１６のループ処理が図１０のＳ２０６〜Ｓ２１６と同様に実行される。この場合、Ｓ４０６、Ｓ４０８、Ｓ４１４の処理の一部が第１参考例と相違する。具体的には、Ｓ４０６では、変位次元補正上体位置姿勢の現在の候補（Ｐb2_s，θb2_s）と、今回時刻ｔでの単純化モデル歩容の目標両足平位置姿勢および目標腕姿勢とを基に、第２変位次元補正モデルでの各質点Ｂ１〜Ｂ７の位置を求める。この場合、第２変位次元補正用モデルでのロボット１の上体３の位置姿勢が現在の候補（Ｐb2_s，θb2_s）に一致し、且つ、第２変位次元補正用モデルでのロボット１の各足平２２の位置姿勢が単純化モデル歩容の目標足平位置姿勢に一致し、且つ、第２変位次元補正用モデルでのロボット１の両腕体５，５の上体３に対する相対姿勢が単純化モデル歩容の腕姿勢に一致しているとして、各質点Ｂ１〜Ｂ７の位置と各腕体５，５の姿勢とが求められる。

具体的には、足平質点Ｂ３，Ｂ４の位置は、目標足平位置姿勢から決定される。また、上体質点Ｂ１の位置は、候補（Ｐb2_s，θb2_s）から決定され、上体３（上体リンク）の姿勢角はθb2_sと同一とされる。そして、各大腿質点Ｂ２，Ｂ３の位置は、目標両足平位置姿勢と候補（Ｐb2_s，θb2_s）とから定まるロボット１の各脚体２の姿勢から決定される。さらに、第２変位次元補正用モデルの両腕体５，５の姿勢（上体３に対する相対姿勢）は、目標腕姿勢と同一とされる。さらに、両腕質点Ｂ６，Ｂ７の位置は、上体３に対する目標腕姿勢に対応する位置に決定される。補足すると、上体位置姿勢と、上体３に対する両腕体５，５の相対姿勢が決まれば、両腕質点Ｂ６，Ｂ７の位置（グローバル座標系のでの位置）は一義的に定まる。

また、Ｓ４０８の処理では、モデル間全体重心ずれGc_errは、第１参考例と同様に、前記式０６で求められる。但し、この場合の式０６の右辺のΣの演算は本参考例での変位次元補正用モデルの全ての質点Ｂ１〜Ｂ７についての総和である。また、モデル間角運動量積ずれＬ_errは、例えば前記式０７の右辺の「Ｉb＊(θb2−θb1)」を「Ｉt＊(θb2−θb1)＋Ｉa＊（θa2R−θa1R）＋Ｉa＊（θa2L−θa1L）」で置き換えた式によって算出される。ここで、θa2Rは、第２変位次元補正用モデルの右側腕体５Ｒの上体３に対する姿勢を代表する姿勢角（例えば腕体５の肩関節と肘関節とを連結するリンクの上体３に対する相対傾斜角）である。同様に、θa1Rは、第１変位次元補正用モデルの右側腕体５Ｒの上体３に対する姿勢角、θa2Lは、第２変位次元補正用モデルの左側腕体５Ｌの上体３に対する姿勢角、θa1Lは、第１変位次元補正用モデルの左側腕体５Ｌの上体３に対する姿勢角である。なお、本参考例では、両腕体５，５の目標腕姿勢は、肘関節の曲げ角を一定としている。補足すると、本参考例では、式０７の右辺のΣの演算は、モデル間全体重心ずれGc_errの場合と同様、全ての質点Ｂ１〜Ｂ７についての総和である。

また、Ｓ４１４で、モデル間全体重心ずれおよびモデル間角運動量積ずれを求める場合も、Ｓ４０８と同様に行われる。

Ｓ４０６〜Ｓ４１６のループ処理に続いて、Ｓ４１８の処理が実行され、今回時刻ｔの変位次元補正上体位置姿勢(Pb2，θb2)が決定される。この処理は、図１０のＳ２１８と全く同様である。

上述した変位次元歩容補正サブルーチンによって、前記条件１，２を満たすような変位次元補正上体位置姿勢が探索的に求められ、それが、今回時刻ｔにおける変位次元補正上体位置姿勢（Ｐb2，θb2）として決定される。これにより、単純化モデル歩容の上体位置姿勢を修正してなる変位次元補正歩容が得られる。補足すると、本参考例では、各変位次元補正用モデルの両足平質点Ｂ５，Ｂ６の位置は、両変位次元補正用モデルで同じである。従って、Ｓ２０８でモデル間全体重心ずれGc_errとモデル間角運動量積ずれL_errとを算出するに当たっては、両足平質点Ｂ５，Ｂ６に係わる項は省略してもよい。

図９の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、Ｓ０２６の処理を省略して、Ｓ０２８〜Ｓ０３２の処理が第１参考例と同様に実行される。Ｓ０２８〜Ｓ０３２の処理は、第１参考例と同一である。

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

ここで、本参考例での前記変位次元補正歩容について図２２を参照して補足説明をしておく。図２２は、例えば図２０のようにロボット１を直立姿勢で起立させた状態から、目標ＺＭＰを維持したまま、両腕体５，５の先端部を上体３の前方側に突き出すような歩容を生成する場合において、上記した変位次元歩容補正サブルーチンによって決定される第２変位次元補正用モデルの各質点Ｂｉ（ｉ＝１，２，…，７）の位置、上体３（上体リンク）の姿勢角、および両腕体５，５の姿勢と、第１変位次元補正用モデルの各質点Ｂｉの位置、上体３（上体リンク）の姿勢角、および両腕体５，５の姿勢との関係を例示している。なお、この場合、先に示した図２１のロボット１の姿勢状態は、第２変位次元補正用モデルの各質点Ｂｉと、上体２の姿勢角、および両腕体５，５の姿勢（上体３に対する相対姿勢）を単純化モデル歩容どおりに決定した場合に相当するものとなっている。換言すれば、図２１における第２変位次元補正用モデルの各質点Ｂiの位置、並びに上体３の姿勢角および各腕体３の姿勢、すなわち、第２変位次元補正用モデルの要素の配置は、単純化モデル歩容の瞬時運動から、前記幾何学的拘束条件（２）に従って決定されたものとなっている。

ロボット１の図２０の姿勢状態から、両腕体５，５を図２２のように前方側に突き出す歩容を生成する場合、単純化モデル歩容では、図２１に示すように、上体位置姿勢を維持したまま、両腕体５，５を前方に突き出すような歩容となる。このため、単純化モデル歩容通りに第２変位次元補正用モデルの各質点Ｂｉの位置、上体３の姿勢、および両腕体５，５の姿勢を決定すると、第２変位次元補正用モデルの両腕質点Ｂ６，Ｂ７が第１変位次元補正用モデル（図２０参照）に対して前方側に突き出ることとなる。このため、図２２に示す如く、両変位次元補正用モデルで全体重心を一致させるべく（前記条件１を満たすべく）、第２変位次元補正用モデルの上体質点Ｂ１の位置Ｐ12は、第１変位次元補正用モデルの上体質点Ｂ１の位置Ｐ11よりも後方側の位置になる。別の言い方をすれば、両腕質点Ｂ６，Ｂ７のモデル間擬似並進加速度に伴う慣性力の並進力成分を、上体質点Ａ１のモデル間擬似並進加速度に伴う慣性力の並進力成分によって打ち消すようにして、第２変位次元補正用モデルの上体質点Ｂ１の位置Ｐ12は、第１変位次元補正用モデルの上体質点Ｂ１の位置Ｐ11よりも後方側の位置になる。なお、このとき、第１変位次元補正用モデルの上体質点Ｂ１の位置Ｐ11よりも後方側の位置になることに付随して、第２変位次元補正用モデルの大腿質点Ｂ２，Ｂ３の位置Ｐ22，Ｐ32も、第１変位次元補正用モデルの大腿質点Ｂ２，Ｂ３の位置Ｐ21，Ｐ31に対して若干ずれる。

また、このとき、仮に上体３の姿勢を維持すると、第１変位次元補正用モデルと第２変位次元補正用モデルとの間で前記モデル間角運動量積ずれＬ_errが発生する。そして、このずれＬ_errを解消すべく（前記条件２を満たすべく）、第２変位次元補正モデルでの上体姿勢（図２２に示す上体３の姿勢）、すなわち変位次元補正歩容の上体姿勢は、第１変位次元補正モデルでの上体姿勢（図２０に示す上体３の姿勢）、すなわち、単純化モデル歩容の上体姿勢に対して前傾側に傾く。より詳しく言えば、上体質点Ｂ１、大腿質点Ｂ２，Ｂ３および腕質点Ｂ６，Ｂ７のモデル間擬似並進加速度に伴う慣性力、並びに、腕体５，５のモデル間擬似角加速度に伴う慣性力が目標ＺＭＰまわりに作用するモーメントを打ち消すようにして、変位次元補正歩容の上体姿勢は、単純化モデル歩容の上体姿勢に対して前傾側に傾く。

このように単純化モデル歩容に対して運動（上体位置姿勢）を修正した変位次元補正歩容が決定されることで、目標ＺＭＰまわりの床反力モーメントの水平成分が０になること等の動力学的平衡条件を単純化モデル歩容と同じように満たしつつ、単純化モデル歩容よりも動力学的精度の高い変位次元補正歩容が決定されることとなる。

補足すると、以上説明した第１〜第３参考例では、前記したように第１の幾何学的拘束条件と第２の幾何学的拘束条件とを設定しているため、単純化モデル歩容の瞬時運動から、第１の幾何学的拘束条件に従って決定される第１変位次元補正用モデルの各要素の配置の時間的変化（歩容生成装置１００の演算処理周期毎の変化）によって発生する各要素の慣性力の合力に釣り合う床反力（目標ＺＭＰまわりの床反力モーメント）と、該単純化モデル歩容の瞬時運動から、前記第２の幾何学的拘束条件に従って第２変位次元補正用モデルの各要素の配置を決定した場合における該第２変位次元補正用モデルの各要素の配置の時間的変化（歩容生成装置１００の演算処理周期毎の変化）によって発生する、その各要素の慣性力の合力に釣り合う床反力（目標ＺＭＰまわりの床反力モーメント）との差に所定の定常オフセット（前記式０７〜１０の「Const」に相当するもの）を加えたものが、単純化モデル歩容の瞬時運動が単純化モデル上で発生する床反力（目標ＺＭＰまわりの床反力モーメント）の誤差にほぼ一致するものとなっている。

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

前記第１〜第３参考例では、角運動量積に係る点Ｑとして、目標ＺＭＰを用いたが、点Ｑは目標ＺＭＰ以外の点でもよく、例えば、次のような点でもよい。
ａ）歩容を記述する座標系（グローバル座標系）の原点
ｂ）ロボット１とともに連続的に移動する適当に設定した点
ｃ）フルモデルにおけるロボット１の全体重心
ｄ）単純化モデルにおけるロボット１の全体重心
ｅ）第１および第２変位次元補正用モデルに係るある所定の質点の集合の重心（具体的には、第１および第２変位次元補正用モデルの間で位置の差を生じる可能性がある質点の集合の重心。例えば、第１参考例では、全ての質点Ａ１〜Ａ５の集合の重心が相当し、第２参考例では、上体質点Ａ１，大腿質点Ａ２，Ａ３の集合の重心が相当する）
また、前記第１〜第３参考例の説明では、図９のＳ０２４の変位次元歩容補正サブルーチンの処理において、サジタルプレーン上での単純化モデル歩容の上体位置姿勢を補正する処理について説明したが、サジタルプレーンに直交するラテラルプレーン上での上体位置姿勢を合わせて補正するようにしてもよい。この場合には、例えば図１０のＳ２００〜Ｓ２１８の処理を３次元に拡張して行えばよい。あるいは、サジタルプレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での上体位置姿勢の補正処理とを図１０と同様のアルゴリズムによって、それぞれ独立に行うようにしてもよい。補足すると、上体位置の鉛直成分を含めて上体位置姿勢を補正する場合において、サジタルプレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での上体位置の補正処理とを独立して行う場合には、上体位置の鉛直成分の補正は、サジタルプレーン上、あるいはラテラルプレーン上のいずれか一方のプレーン上での補正処理によって行い、他方のプレーン上での補正処理では、上体位置の鉛直成分を除いて、その他方のプレーン上での上体位置姿勢の補正を行うようにすればよい。

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

また、第１〜第３参考例の図９のＳ０２４の変位次元歩容補正サブルーチンの処理では、変位次元補正上体位置姿勢の初期候補（Ｐb2_s，θb2_s）を、前回制御周期の時刻で求めた変位次元補正上体位置姿勢を用いて決定したが、例えば該初期候補（Ｐb2_s，θb2_s）を単純化モデル歩容の上体位置姿勢と同一にしてもよい。但し、前記条件１，２を満たす変位次元補正上体位置姿勢を短時間で探索する上では、前記第１〜第３参考例で説明した如く、初期候補（Ｐb2_s，θb2_s）を決定することが望ましい。

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

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

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

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

また、前記第２参考例において、第１および第２変位次元補正用モデルの両足平質点Ａ４，Ａ５の位置は、いずれも単純化モデル歩容の両足平位置姿勢に対応する位置に決定される。このため、変位次元補正上体位置姿勢は、結果的に、第１および第２変位次元補正用モデルの間での、各大腿質点Ａ２，Ａ３の位置のずれに応じて定まることとなる。従って、前記したように第２変位次元補正用モデルの要素の配置を前記幾何学的拘束条件（２）に従って（単純化モデル歩容に対応させて）決定したときのその配置と、第１変位次元補正用モデルの要素の配置とのずれから、あらかじめ作成した関数式あるいはマップなどを用いて、単純化モデル歩容の上体位置姿勢から変位次元補正上体位置姿勢への補正量を決定するようにしたときには、単純化モデル歩容に対応する第２変位次元補正用モデルの各大腿質点Ａ２，Ａ３の位置と、第１変位次元補正用モデルの各大腿質点Ａ２，Ａ３の位置との間の位置ずれに応じてフィードフォワード制御則によって直接的に（微積分演算を用いることなく）、単純化モデル歩容に対する変位次元補正上体位置姿勢の補正量を決定できることとなる。そして、この場合に、各大腿質点Ａ２，Ａ３の位置のずれの代わりに、前記した擬似位置ずれ距離を用いたり、あるいは、その擬似位置ずれに対応する各脚体２の膝関節の曲げ角θを用いることによって、その擬似位置ずれ距離または膝関節の曲げ角θから、フィードフォワード制御則によって直接的に変位次元補正上体位置姿勢（単純化モデル歩容の上体位置姿勢から変位次元補正上体位置姿勢の補正量）を決定できることとなる。

［実施形態］
以上説明したことを基礎として、以下に本発明の一実施形態を図２４を参照して説明する。この実施形態は、前記図９のＳ０２４の変位次元歩容補正サブルーチンのみが前記第２参考例と相違するものであり、該サブルーチン処理を示すフローチャートが図２４である。

以下説明すると、Ｓ６００において、現在時刻ｔの単純化モデル歩容の瞬時値（図９のＳ０２２で求められたもの）を基に、各脚体２の膝関節の曲げ角θが求められる。具体的には、単純化モデル歩容の上体位置姿勢と両足平位置姿勢とから各脚体２の姿勢状態が一義的に定まるので、それによって、各脚体２の膝関節の曲げ角θが求められる。この曲げ角θは、単純化モデル歩容そのものに対応する膝関節の曲げ角である。

次いで、Ｓ６０２に進んで、各脚体２の膝関節の曲げ角θから、例えばあらかじめ定められた所定の関数により、上体位置姿勢の補正量を決定する。この場合、各膝関節の曲げ角θに対して、上体位置の補正量と上体姿勢の補正量とが単調に増加又は減少するように前記所定の関数が設定される。より具体的には、膝関節の曲げ角θが大きくなるに伴い、上体位置の補正量が、ロボット１の後方側に大きくなり、また、上体姿勢の補正量が上体３の前傾側に大きくなるように前記所定の関数が設定される。

次いで、Ｓ６０４に進んで、単純化モデル歩容の上体位置姿勢の瞬時値をＳ６０２で上記の如く求めた補正量で補正することにより、現在時刻ｔでの変位次元補正上体位置姿勢を決定する。

以上が、本実施形態における変位次元歩容補正サブルーチンの処理である。本実施形態によるときには、第１および第２変位次元補正用モデルを使用せずに、単純化モデル歩容の上体位置姿勢が、各脚体２，２の膝関節の曲げ角に応じてフィードフォワード制御則によって決定されることとなる。そして、上体位置姿勢の補正量を決定する関数を上記のように設定しておくことで、結果的に、第２参考例と同様に第１および第２変位次元補正用モデルを用いて前記条件１，２を満たすように変位次元補正上体位置姿勢を決定した場合と同じように、単純化モデル歩容の上体位置姿勢を補正できることとなる。

なお、本実施形態では、上体位置姿勢の補正量を決定するために関数を用いたが、マップを用いてもよいことはもちろんである。また、膝関節の曲げ角の代わりに、各脚体２の前記擬似位置ずれ距離を用いてもよい。この場合、各脚体の擬似位置ずれ距離から上体位置姿勢の補正量を決定するための関数もしくはマップは、擬似位置ずれ距離が大きくなるに伴い、上体位置の補正量が、ロボット１の後方側に大きくなり、また、上体姿勢の補正量が上体３の前傾側に大きくなるように設定しておけばよい。このようにすることで、本発明の他の実施形態が構成されることとなる。

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

あるいは、前記実施形態で説明した場合と同様に、各腕体５の肩関節の中心と手首関節の中心とを結ぶ線分からの肘関節の中心までの距離、あるいは、肘関節の曲げ角に応じて所定の関数やマップなどにより、単純化モデル歩容の上体位置姿勢の補正量を決定し、その補正量で単純化モデル歩容の上体位置姿勢を補正することで、変位次元補正上体位置姿勢を決定するようにすることも可能である。

また、第１〜第３参考例及び実施形態に関して補足すると、これらの第１〜第３参考例及び実施形態では、第１変位次元補正用モデルの全ての要素の質量の総和がロボット１の総質量と一致し、ロボット１の瞬時目標運動に対する第１変位次元補正用モデルの全体重心位置G1は、該瞬時目標運動に対する単純化モデルの全体重心位置Gsに一致あるいはほぼ一致するようにしている。また、第２変位次元補正用モデルの全ての要素の質量の総和もロボット１の総質量と一致し、ロボット１の瞬時目標運動に対する第２変位次元補正用モデルの全体重心位置G2は、該瞬時目標運動に対する実際のロボット１の真の全体重心位置Gfにほぼ一致するようにしている。

従って、第１〜第３参考例及び実施形態では、G1とG2との差（G1−G2）は、単純化モデルの全体重心位置Gsとロボット１の真の全体重心位置Gfとの差（Gs−Gf）、すなわち単純化モデルの全体重心位置の誤差にほぼ一致するものとなっている。

なお、この場合において、先に述べたように、足平の近傍の質点（足平質点）のように、両変位次元補正用モデルでの配置が同一位置となるような質点は、それを両変位次元補正用モデルから除いてもよいことはもちろんである。

また、前記各参考例及び実施形態では、上体位置姿勢を補正するようにしたが、上体以外の他の部位の位置姿勢、あるいは、上体を含めた複数の部位の位置姿勢を補正するようにしてもよい。

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

１…２足移動ロボット、２…脚体（可動体）、３…上体、５…腕体（可動体）、１００…歩容生成装置。

Claims (3)

1. 上体から延設された複数の脚体または複数の腕体を複数の可動体として備えると共に、各可動体の上体との連結部と該可動体の先端部との間の中間部とに屈曲可能な関節を有する移動ロボットの瞬時目標運動を逐次発生する瞬時歩容発生手段を備え、その瞬時目標運動が、前記ロボットの運動と床反力との関係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可動体の中間部もしくはその近傍で発生する慣性力がほぼ０であるとして構築された動力学モデルを用いて生成された移動ロボットの歩容生成装置において、
   前記各可動体の中間部の関節の曲げ角を参照量Ａとし、前記各可動体の先端部近傍の所定の点と、該可動体の上体との連結部近傍の所定の点とを結ぶ線分と、該可動体の中間部の関節の中心または該関節に連結されたリンクの重心との前記線分に交差する方向での相対位置を参照量Ｂとしたとき、前記参照量Ａ又は参照量Ｂに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定し、その決定した補正量で該瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方を補正するように瞬時目標運動を補正することによって、当該補正後の瞬時目標運動の動力学的精度を当該補正前の瞬時目標運動よりも高めるように当該補正前の瞬時目標運動を補正する瞬時目標運動補正手段を備えたことを特徴とする移動ロボットの歩容生成装置。
2. 前記各可動体は脚体であり、前記瞬時目標運動補正手段は、前記参照量Ａに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定するものであって、前記各可動体の中間部の関節の曲げ角が増加するに伴い、前記瞬時目標運動の上体姿勢をより大きく前傾させ、且つ、前記複数の可動体を除いた移動ロボットの全体の重心位置をより後方に変位させるように前記補正量を決定することを特徴とする請求項１記載の移動ロボットの歩容生成装置。
3. 前記各可動体は脚体であり、前記瞬時目標運動補正手段は、前記参照量Ｂに応じて、フィードフォワード制御則により前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいずれか一方の補正量を決定するものであって、前記線分と、前記各可動体の中間部の関節の中心または該関節に連結されたリンクの重心との距離を前記相対位置として用い、その距離が増加するに伴い、前記瞬時目標運動の上体姿勢をより大きく前傾させ、且つ、前記複数の可動体を除いた移動ロボットの全体の重心位置をより後方に変位させるように前記補正量を決定することを特徴とする請求項１記載の移動ロボットの歩容生成装置。

Images