JPWO2005051608A1 - 移動体の制御装置 - Google Patents

Abstract

３つ以上の接地部位１０を備える移動体１（移動ロボット）の接地部位１０のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように接地部位１０をツリー構造状に分類し、子ノードを持つ各ノード（第Ｃノード）に対し、少なくとも基体３６などの所定部位の実姿勢傾斜と目標姿勢傾斜との偏差（姿勢傾斜偏差）を０に近づけるように、第Ｃノードの接地部位１０の目標相対高さの修正量を決定し、それらの修正量を合成してなる目標相対高さを満足するように移動体１の関節を動作させる。

Description

本発明は、脚式移動ロボット、アクティブサスペンションを持つ車輪型移動体などの移動体の床反力制御に関する。より具体的には脚式移動ロボット、アクティブサスペンションを持つ車輪型移動体などの移動体が接地する床（地面）に未知の傾斜や凹凸がある場合にも、その影響を受けにくく、適切に床反力を制御する装置に関する。さらに床形状を推定しながら、適切に床反力を制御する装置に関する。

本願出願人による特開平５−３０５５８５号公報（特許文献１）には２足移動ロボットの両脚のコンプライアンス制御技術が開示されている。この技術は、４脚等のロボットにも適用可能なものである。この場合、同公報の技術のコンプラインス制御では、全脚を仮想平面上に接触させた状態で、仮想平面を傾ける動作を行なうので、床の大域的なうねりや傾斜には対応できる。しかし、同公報の技術では、局所的な床の傾斜や凹凸があると、荷重（床反力）の配分が適切に行われない場合があった。その結果、接地すべき面が浮いて接地部位（足平）が揺れたり、着地衝撃が大きく、足平の接地性の低下することがあった。さらにその結果、ロボットの滑りやスピンが発生し易くなる恐れがあった。また、ロボットの姿勢を所望の姿勢に復元させる床反力モーメントが適切に各脚先端部に分配されるように制御できず、ロボットの姿勢が揺れることがあった。また、ロボットのある部位が振動し始めると、その振動が減衰し難くいという不具合が生じることもあった。また、床反力の各脚先端部への分配制御が適切に行なわれないために、一部のアクチュエータの負荷が過大になる場合もあった。
さらに、本願出願人による特開平１０−２７７９６９号公報（特許文献２）、特開２００１−３２２０７６号公報（特許文献３）には、複合コンプライアンス制御の技術、および床形状の推定技術が開示されている。これらの技術では、２足移動ロボットにおいて、各脚先端部への床反力の適切な分配ができ、かつ、床形状を複合的に、具体的には各脚先端部の接地面傾斜と各接地面間の高低差とを同時に複合的に、精度良く推定することができた。これらの公報の技術は、原理的には４脚ロボット等の多脚ロボットに対応可能なものではあるが、それを具体的に適用するための技術については、記述されていない。
また、４脚ロボットの脚先端に備えた触覚（荷重センサまたは接触センサ）を用いて着地時に床の高さを推定しようとする手法も知られている。しかし、この手法は、シーケンス的な手法であり、軟弱地盤のように床が着地後に変形するような場合には対応できなかった。特に、ロボットの高速移動時等、ロボットの本体が激しく振動しているときには、床の高さを正しく推定できず、各脚への荷重の分配が適切に行われない恐れがあった。
また、ロボットの各脚のコンプライアンス制御を脚毎に独立に行う技術も知られている。しかし、この技術では、ロボットの上体の揺れを通して脚同士の干渉が生じるにもかかわらず、このことが考慮されていない。すなわち、各脚のコンプライアンス制御が個別に行なわれる。このため、各脚のコンプライアンス制御が互いに協調し合うことがないので、適切に各脚に床反力を分配することができなかった。
本発明はかかる背景に鑑みてなされたものであり、脚式移動ロボットや、アクティブサスペンションを持つ車輪型移動体などの移動体が接地する床に未知の傾斜や凹凸がある場合にも、その影響を受けにくく適切に床反力を制御する装置を提供することを目的とする。さらに床形状を精度よく推定しながら、床反力を適切に制御できる装置を提供することを目的とする。

本発明の移動体の制御装置の第１発明は、互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類し、
複数の接地部位を子孫ノードとして持つノードである第Ｃノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量を該第Ｃノード毎に決定し、その決定した修正量をすべての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とするものである。
なお、本発明においては、子ノードを持つ各ノードに対し、その「ノードに属する接地部位」とは、より正確に言えば、その「ノードに子孫ノードとして属する」ということを意味するものである。このことは、第１発明に限らず、本願のいずれの発明についても同じである。
かかる第１発明によれば、移動体が備える３つ以上の接地部位をツリー構造状に分類し、子ノードを持つ第Ｃノードのそれぞれに対し、姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量（換言すれば操作量）を該第Ｃノード毎に決定する。そして、それらの修正量を全ての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を付加した修正目標運動を決定する。そして、その決定した修正目標運動を満足するように移動体の関節を動作させる。これにより、個々の接地部位の高さをそれぞれ独立に（各別）の操作する場合に較べて、複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を０に近づけるように操作することができる。その結果、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、ロボットの姿勢の安定性を高めることができる。
かかる第１発明では、好ましくは、前記ツリー構造における各ノードに重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、
前記ノード動作制御手段は、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノードの目標高さは前記目標運動における目標高さに維持される（第２発明）。
この第２発明によれば、各第Ｃノードの目標高さは、この第Ｃノードの子ノードの目標高さ（これは、第Ｃノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標歩容の目標運動に付加して前記修正目標運動を決定したときには、前記一つのノードの子ノードの上記重み付き平均値が、修正目標運動と目標運動とで同じになるように前記第１の修正が目標運動に付加される。
この結果、各第Ｃノード毎に、それに属する複数の接地部位の床反力の相互のバランスを採りながら、各Ｃノードに属する複数の接地部位の間の目標相対高さを修正することができることとなる。ひいては、ロボットの姿勢の安定性をさらに高めることができる。
また、本発明の移動体の制御装置の第３発明は、互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ツリー構造における各ノードに重みが設定されており、
少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、前記目標運動に、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、
前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、前記ノード動作制御手段が、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記目標運動を決定したとき、該修正目標運動における該第Ｃノードの目標高さは前記目標運動における目標高さに維持されることを特徴とするものである。
この第３発明によれば、前記所定の第Ｃノードの目標高さは、この第Ｃノードの子ノードの目標高さ（これは、第Ｃノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したときには、該第Ｃノードの子ノードの上記重み付き平均値が修正目標運動と目標運動と同じになるように前記第１の修正が目標運動に付加される。
この結果、第３発明によれば、第Ｃノードに属する複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を０に近づけるように操作することができる。このため、第１発明や第２発明と同様に、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、ロボットの姿勢の安定性を高めることができる。
なお、前記第２発明、第３発明においては、前記ツリー構造において子ノードを持つ各ノードに対し、該各ノードの全ての子ノードの重みは、その全ての子ノードの重みの総和が１となるように設定されていることが好ましい（第４発明）。
また、前記第２発明、第３発明では、前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第Ｄノードとしたとき、前記重みを設定する手段は、各第Ｄノードの重み、または該第Ｄノードの祖先ノードのうちの少なくともいずれか１つのノードの重みを０に設定することが好ましい（第５発明）。
この第５発明によれば、前記姿勢傾斜偏差を０に近づけるために寄与できない非接地状態の接地部位の高さが無駄に修正されるのを防止できる。なお、第５発明は、第４発明と複合してもよい。
また、前記第１発明、第２発明では、前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されている場合に、前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい（第６発明）。そして、このことは、前記第３発明においても同じである（第７発明）。
ここで、移動体の基体に連結されている接地部位が多数あると、移動体の関節の自由度が不足して、それらの接地部位の間の相対高さの修正が機構的な制限を受け、決定した目標相対高さを満足するように強制的に関節を動作させると、いくつかの接地部位でこじりなどの滑りが発生する場合がある。そして、そのような滑りが生じる状況では、各接地部位の床反力が不適切なものとなると共にロボットの姿勢の安定性が却って損なわれる恐れがある。このような場合には、第６発明、第７発明の如く、第Ｃノードに属する接地部位の間の目標相対高さを修正するだけでなく、基体の目標姿勢の修正を目標運動にさらに付加して、前記修正目標運動を決定することことにより、上記こじりなどの滑りを抑制することができる。従って、接地部位に無理な床反力が作用したりすることなく、それらの床反力を適正なものとしつつ、移動体の姿勢の安定性を確保することができる。
前記第６発明、第７発明では、より好ましくは、前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定する（第８発明）。この第８発明によれば、基体の目標姿勢の修正を目標運動に付加して修正目標運動を決定したときに、移動体の全体重心が、目標歩容における適正な位置に対して水平方向に大きくずれるようなことを防止できるので、移動体の安定性を適正に保ちつつ、第Ｃノードの接地部位の間の相対高さと基体の姿勢とを修正できる。
また、前記第６発明は、前記移動体が、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっている場合に適している（第９発明）。
また、第７発明は、前記移動体が、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第Ｃノードに属する接地部位となっている場合に適している（第１０発明）。
すなわち、複数のリンク機構が基体から延設されたロボットでは、それらのリンク機構の先端部で接地することが通常的であるので、該リンク機構の中間部位で接地させようとした場合（例えば人間型移動ロボットをその脚の中間部（膝）で接地させようとした場合）には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第６発明、第７発明は、それぞれ第９発明、第１０発明の如く移動体が構成されている場合に適している。
これらの第９発明、第１０発明では、前記中間部位には、その接地時に弾性変形する弾性体が設けられていることが好ましい（第１１発明）。
これによれば、前記中間部位の間の相対高さの修正による床反力の調整能力が高まって、移動体（ロボット）の姿勢の安定性の確保が容易になる。
また、前記第６発明は、前記移動体が、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられている場合にも適している（第１２発明）。
また、第７発明は、前記移動体が、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられており、前記第Ｃノードに属する接地部位には、少なくとも前記基体の接地部位が含まれる場合に適している（第１３発明）。
例えば１つ以上の脚（リンク機構）が基体（上体など）から延設されたロボットに腰掛動作を行なわせる場合、その基体で接地することとなる。そしてこのような場合には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第６発明、第７発明は、それぞれ第１２発明、第１３発明の如く移動体が構成されている場合にも適している。
これらの第１２発明、第１３発明では、前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることが好ましい（第１４発明）。
これによれば、前記基体における接地部位の間の相対高さの修正による床反力の調整能力が高まって、移動体の姿勢の安定性の確保が容易になる。
なお、前記第６〜第１４発明は、前記第２発明、第４発明、第５発明と複合してもよい。
また、前記第１発明、第３発明では（あるいは第１〜第１４発明では）、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されている場合に、前記修正目標運動を満足するように前記関節を動作させたときに前記第Ｃノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、
前記ノード動作制御手段は、少なくとも前記第Ｃノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第Ｃノードに属する各接地部位に作用したときに前記基体と該第Ｃノードに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第Ｃノードに属する複数の接地部位の目標高さの第２の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい（第１５発明）。
この第１５発明によれば、前記連結機構や接地部位が該接地部位の接地時に受ける床反力によってたわみなどの変形が生じることを考慮し、その変形量を目標床反力の並進力成分を基に推定する。そして、その推定した変形量を打ち消すための、前記第Ｃノードに属する各接地部位の目標高さの第２の修正を目標運動にさらに付加して前記修正目標運動を決定する。このため、第１５発明によれば、連結機構や接地部位の変形の影響を補償しつつ、適正に前記姿勢傾斜偏差を０に近づける床反力を移動体に作用させることが可能となり、移動体の姿勢の安定性をより一層高めることができる。
また、前記第１発明、第３発明（あるいは第１〜第１５発明）では、少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメータを床形状を表す床形状パラメータとして推定する床形状推定手段を備え、
前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメータの推定値に応じた、前記第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正をさらに付加して前記修正目標運動を決定することが好ましい（第１６発明）。
この第１６発明によれば、少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定する床形状パラメータの推定値に応じた、第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正を目標運動にさらに付加するので、実際の床形状が前記目標運動で想定したものと相違しても、接地部位の接地面を、それが接地すべきときに実際の床面に適切に接地させ、ひいては、前記姿勢傾斜偏差を０に近づけるような床反力を適切に移動体に作用させることができる。従って、移動体の姿勢の安定性をより一層高めることができる。
この第１６発明では、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位の実床反力とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１７発明）。
あるいは、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、
前記ツリー構造における各ノードである第Ａノードに対し、該第Ａノードが前記葉ノードである場合には、該葉ノードである接地部位に作用する床反力を第Ａノードのノード床反力と定義し、且つ、第Ａノードが子ノードを持つ場合には、該第Ａノードのすべての子ノードのノード床反力の合力を第Ａノードのノード床反力と定義したとき、少なくとも前記第Ｃノードの各子ノードのノード床反力の実際値である実ノード床反力を該第Ｃノードに属する各接地部位の実床反力から逐次決定する手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１８発明）。
あるいは、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されているときには、
該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１９発明）。
これらの第１７発明、第１８発明、第１９発明によれば、前記床形状パラメータをリアルタイムで逐次更新しながら、適切に推定できる。補足すると、第１９発明では、前記連結機構は、その全てが必ずしも関節を備えている必要はない。
なお、第１９発明では、例えば前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定する（第２０発明）。
このように実床反力を検出または推定し、それを用いることで、前記連結機構や接地部位の変形量を適切に推定できる。
前記第１６発明（あるいは前記第１６〜第２０発明）においては、前記ツリー構造における各ノードの重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、前記床形状推定手段が推定する前記床形状パラメータは、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることが好ましい（第２１発明）。
この第２１発明によれば、上記のように、子ノードを持つ第Ｂノードの接地面の高さをその子ノードの重み付き平均値として定義することによって、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係によって、該第Ｃノードに属する複数の接地部位が接地する床の形状に係わる前記床形状パラメータを決定できる。従って、床形状パラメータを各接地部位の絶対高さを把握したりすることなく推定できる。そのため、床形状の推定処理が容易になる。
この第２１発明では、前記床形状推定手段は、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定することが好ましい（第２２発明）。
すなわち、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるということは、その子ノードの接地面が実際の床面から離れたか、もしくは離れそうな状態であるので、その子ノードの接地面の高さは実際の床面の高さを反映していない。そこで、第２２発明では、このような状態では、当該浮きそうな状態の子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定する。これにより、床形状パラメータが実際の床面形状を反映していない値に発散してしまうような事態を防止でき、実際の床面形状を反映した床形状パラメータを推定できる。ひいては、不適切な床形状パラメータを使用して、接地部位の相対高さを修正し、移動体の姿勢の安定性が損なわれるような事態を防止できる。
また、前記第１６発明（あるいは第１６〜第２２発明）では、前記床形状推定手段は、前記床形状パラメータの変動を緩和するようにローパスフィルタを用いて該床形状パラメータを推定することが好ましい（第２３発明）。このようにローパスフィルタを使用することで、移動体の移動時の振動などによって床形状パラメータが頻繁に変動したりするのを防止できる。
また、前記第１６発明（あるいは第１６〜第２３発明）では、前記ツリー構造の各ノードには床形状の推定用の重みが設定されており、
前記ツリー構造における各葉ノードに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に０を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードについて階層相対化する処理として定義し、
前記葉ノードである接地部位のそれぞれに作用する床反力Ｆｎ（ｎ＝１，２，…）を基に、各ノードについて階層相対化したノード相対床反力Ｆｎ＿ｒｅｌ（ｎ＝１，２，…）を求め、さらに子ノードを持つ任意のノードである第ｎノードのすべての子ノードａｊ（ｊ＝１，２，…，ｒ_ｏｒは第ｎノードの子ノードの総数）のノード相対床反力を要素とするベクトル（Ｆａ１＿ｒｅｌ，Ｆａ２＿ｒｅｌ，…，Ｆａｒ＿ｒｅｌ）を、第ｎノードのすべての子ノードの重みを要素とするベクトル（Ｗａ１，Ｗａ２，…，Ｗａｒ）にそれぞれ直交する所定の互いに独立な複数のベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張床反力モーメントＭｎ ｅｘｐと定義し、
また、前記葉ノードである接地部位のそれぞれの接地面の高さＺｎ（ｎ＝１，２，…）を基に、各ノードについて階層相対化したノード相対高さＺｎ＿ｒｅｌ（ｎ＝１，２，…）を求め、さらに前記第ｎノードのすべての子ノードａｊ（ｊ＝１，２，…，ｒ_ｏｒは第ｎノードの子ノードの総数）のノード相対高さを要素とするベクトル（Ｚａ１＿ｒｅｌ，Ｚａ２＿ｒｅｌ，…，Ｚａｒ＿ｒｅｌ）を、前記所定の互いに独立な複数のベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張傾斜角θｎと定義したとき、
前記床形状推定手段は、少なくとも前記第Ｃノードの前記ノード拡張床反力モーメントＭｎ＿ｅｘｐ（ｎ＝Ｃ）またはノード拡張傾斜角θｎ（ｎ＝Ｃ）を用いて前記床形状パラメータを推定するようにしてもよい（第２４発明）。
上記のように定義されるノード拡張床反力モーメントや、ノード拡張傾斜角を用いることで、第Ｃノードに属する接地部位が４つ以上であるような場合であっても、床形状を表す床形状パラメータを適切に推定できる。
この第２４発明では、前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
前記床形状パラメータに、前記第Ｃノードの各子ノードの接地面の相対高さを前記ノード拡張傾斜角を用いて表したパラメータが含まれることが好ましい（第２５発明）。
これによれば、第Ｃノードに属する接地部位が４つ以上であっても、前記ノード拡張傾斜角を用いて、床形状パラメータを適切に推定できる。
また、第２４発明では、より具体的には、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを推定する（第２５発明）。
この第２５発明によれば、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定すると共に、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定するので、その決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを逐次更新しながら求めることができる。
なお、前記第１〜第２５発明においては、「床反力」は、本来の意味での床（もしくは地面）から移動体に作用する反力だけを意味するものではなくでなく、移動体が接触する床以外の対象物から受ける反力をも含むものとする。そして、接地部位は、床（もしくは地面）への接触箇所だけでなく、移動体の対象物との接触箇所をも含むものとする。

図１は、本発明の第１実施形態および第２実施形態に係るロボットの全体外観図、図２は第１実施形態の制御装置の機能的構成を示すブロック図、図３（ａ）〜（ｃ）および図４（ａ）〜（ｃ）は、第１実施形態のロボット（４脚ロボット）の動作を説明するための図、図５（ａ）〜（ｃ）は第１実施形態における重みの設定例を示すグラフ、図５（ｄ）は第１実施形態におけるＺＭＰ（目標全床反力中心点）の設定例を示すグラフである。図６は第１実施形態のノードの階層構造を示す図、図７は目標ノード床反力中心点と重みとの関係を示す図、図８は目標ノード床反力と重みとの関係を示す図、図９は第１実施形態の制御装置のメインルーチン処理を示すフローチャートである。図１０〜図１４はそれぞれ第１実施形態のロボット（４脚ロボット）の目標ノード床反力並進力成分、実ノード床反力並進力成分、実ノード床反力モーメント、補償全床反力モーメント、ノード補償床反力モーメントを例示する図である。図１５および図１６は第１実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図、図１７〜図２１はそれぞれ第１実施形態における階層型コンプライアンス動作決定部、補償全床反力モーメント分配器、θ１４２３決定部（補償角決定部）、θ１４決定部（補償角決定部）、機構変形補償量算出部の機能的構成を示すブロック図、図２２は補償角を決定する処理を示すフローチャート図２３は第１実施形態のロボットに係る階層構造の他の例を説明するための図である。図２４は第２実施形態のロボット（６脚ロボット）の階層構造を説明するための図、図２５は第２実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図、図２６〜図２８はそれぞれ第２実施形態のロボット（６脚ロボット）の目標ノード床反力の並進力成分、実ノード床反力の並進力成分、実ノード床反力モーメントを例示する図、図２９（ａ），（ｂ）は第２実施形態におけるノード補償床反力モーメントの設定手法を説明するための図、図３０および図３１は第２実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図である。図３２および図３３はそれぞれ第２実施形態におけるθ１４５２３６決定部（補償角決定部）、θ１４５決定部（補償角決定部）の機能を示すブロック図、図３４は第２実施形態のロボットに係る階層構造の他の例を説明するための図である。図３５は第３実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図、図３６は第３実施形態における制御装置のメインルーチン処理を示すフローチャート、図３７〜図３９は第３実施形態における概念および用語を説明するための図、図４０は第３実施形態における床形状推定器の機能を示すブロック図、図４１は図４０における床高さ偏差推定処理サブルーチンの処理を示すフローチャートである。図４２および図４３はそれぞれ４脚ロボット、６脚ロボットにおける階層相対化の処理を説明するための図である。図４４は床形状推定に係る各ノードの要求モードの設定例を示す図、図４５〜図５５は床形状推定処理を示すフローチャートである。図５６は第４実施形態における床形状推定器の機能を示すブロック図、図５７は第５実施形態における床形状推定器の機能を示すブロック図である。図５８および図５９は第６実施形態におけるロボット（２足移動ロボット）が膝を付いた状態を示す図、図６０は第６実施形態におけるロボットの膝の床反力センサの構成を示す図、図６１は第６実施形態におけるノードの階層構造を示す図、図６２は第６実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図である。図６３（ａ）〜（ｃ）は第６実施形態におけるロボットの上体の姿勢の修正手法を説明するための図、図６４は第６実施形態におけるロボットの上体の位置姿勢の修正手法を説明するための図、図６５は第６実施形態における逆キネマティクス演算部の機能を示すブロック図である。図６６は第７実施形態における逆キネマティクス演算部の機能を示すブロック図、図６７および図６８は第７実施形態におけるロボットの姿勢の修正手法を説明するための図である。図６９は第８実施形態におけるロボットの構成を示す図、図７０は第８実施形態におけるノードの階層構造を示す図である。

まず、本発明の実施形態の説明で用いる主な用語について説明しておく。
本願出願人が先に提案した特開平１０−２７７９６９号公報の複合コンプライアンス制御の技術においては、ロボットの脚先端部（足平）から床反力を受けることを前提として、脚毎に床反力を分別した。これに対して、本明細書の実施形態では、例えば人間型ロボットがひざまづいた状況や腰掛けた状況、腕を接地させた状況など、脚先端部以外からも床反力を受ける状況までも考慮する。このため、脚式移動ロボットの所要の動作において床と接して反力を受ける該ロボットの部位を「接地部位」と呼ぶ。この場合、「床」は、通常の意味での床（あるいは地面）だけではなく、該床（あるいは地面）に固定された椅子（ロボットが腰掛ける椅子）等のように、ロボットがその動作において接触して反力を受ける対象物も、「床」に含まれるものとする。従って、例えば２足移動ロボットが椅子などに腰掛けるときには、該ロボットの腰部は接地部位に相当するものとなる。なお、脚式移動ロボットの通常的な移動時（２足移動ロボットの歩行時など）には、各脚の先端部（足平など）が接地部位に相当するものとなることはもちろんである。
接地部位を分類（区別）するに当たっては、ロボットの同一のリンク（単一の剛体に相当する部分）上で接地部位が複数箇所に分離されて分布している場合（同一のリンクの、互いに分離した複数の部分が接地している場合）でも、すなわち同一リンク上で複数の接地面がつながっていなくても、これらをまとめてひとつの接地部位と定義する。例えば、スパイクピンが４本装着された接地部位がそれらのスパイクピンを介して接地する場合には、４本のスパイクピンのそれぞれの接地面をまとめてひとつの接地部位とみなす。ただし、それらの接地面を必ずしも一つにまとめる必要はない。
第ｎ接地部位とは、このようなルールの基で分類（区別）されて識別番号ｎ（ｎ＝１，２，３，…）を割り振られた接地部位の識別名称である。
接地部位床反力とは、接地部位に作用する床反力とし、特に、第ｎ接地部位に作用する床反力を第ｎ接地部位床反力という。また、全床反力とは、すべての接地部位に作用する床反力の合力とする。更に、床反力中心点とは、床反力がその点に発生するモーメントの水平成分が０となるような作用点とする。
なお、接地部位床反力、全床反力などの床反力は、一般的には、力の作用点と、その作用点にかかる並進力およびモーメントとの組によって表現され、同一の床反力に対して、それを表現する作用点と、並進力およびモーメントとの組は無限にある。その中には、鉛直軸まわりの成分（鉛直成分）を除くモーメント成分（モーメントの水平成分）が０となるような点を作用点として用いる床反力の表現が存在する。この表現における作用点を、本明細書では床反力中心点という。また、床反力中心点を、床反力がその点に発生するモーメントの床面平行成分（床面に平行な成分）が０となるような点として定義してもよい。
尚、この明細書で「床面」は、本願出願人が先に提案した特開平５−３１８３４０号公報に記載される仮想的な床面（目標歩容上で想定される床面であって、実際の床面とは必ずしも一致しない）であっても良い。
補足すると、接地部位の床反力中心点は、その接地部位が接地している状態では、通常、その接地面（床との接触面）上に設定される。一方、接地部位が空中を移動しているときの接地部位床反力は常に０であるので、どの点を作用点としても該接地部位床反力のモーメント水平成分は０である。つまり、床反力中心点は任意に設定できる。ただし、ロボットの動作制御を円滑に行なう上では、目標とする床反力中心点は、連続的に移動することが望ましい。そこで、本明細書の実施形態では、接地部位床反力の床反力中心点は、接地部位が空中を移動するときに、接地部位と共に移動する作用点であるとする。
本明細書の実施形態においては、少なくとも実床反力（ロボットに実際に作用する床反力）に応じて各接地部位の位置姿勢の修正を行なうコンプライアンス制御動作において、単に、各接地部位毎に、互いに無関係に（独立して）コンプライアンス制御動作を行うのではない。本明細書の実施形態は、各接地部位をツリー構造で分類し、少なくとも分類された各グループに作用する実床反力（各グループに含まれる全ての接地部位に作用する実床反力もしくはその合力）に基づき、接地部位の位置姿勢の修正動作を決定することに特徴がある。尚、以降、ツリー構造で分類することを「階層化」と呼ぶこともある。
本明細書の実施形態における脚式移動ロボットの歩容生成装置が生成する変数の目標値には、変数名の頭に「目標」を付ける。実際の脚式移動ロボットにおける当該変数の値（真の値はわからないので、実際には、検出値または推定値）には、変数名の頭に「実」を付ける。前記「実床反力」はその例である。
後述するコンプライアンス制御（床反力制御）の全床反力（ロボットのすべての接地部位に作用する実床反力の合力）の目標を目標全床反力と呼ぶ。目標全床反力のモーメント水平成分が０となる点を目標全床反力中心点と呼ぶ。
移動ロボットの目標歩容の運動（目標歩容におけるロボットの各部位の運動）が発生する慣性力と該ロボットに作用する重力との合力のモーメント水平成分が０となる床面上の点を目標ＺＭＰと呼ぶ。なお、目標歩容の運動は、目標歩容におけるロボットの各部位の目標位置姿勢の時系列によって定まるので、その各部位の目標位置姿勢の時系列を総称的に目標歩容の運動もしくは目標運動という。補足すると、これらの時系列を規定できるならば、上記の如く定義した目標運動と異なる表現で目標運動を記述してもよい。例えば、ロボットの各関節の目標変位の時系列と基体などの特定の部位の目標位置姿勢の時系列との組を目標運動としてもよい。
目標全床反力は、通常、目標歩容の運動パターン（目標運動の時系列パターン）に対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標ＺＭＰに一致する。そこで、以下の説明では、多くの場合、目標全床反力中心点と目標ＺＭＰとを区別せずに使用する。ただし、例外的に、本願出願人が先に提案した特許第３２６９８５２号などにて説明しているロボットの制御の場合には、目標全床反力中心点と目標ＺＭＰとは一致するとは限らない。以下の説明では、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所もある。
尚、移動ロボットの目標歩容で該ロボットが床反力以外の反力（外力）を環境から受けるように想定してもよい。そして、その床反力以外の反力（外力）を、例えば目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、ロボットの目標歩容の運動パターンによって発生する慣性力と該ロボットに作用する重力と目標対象物反力との合力を動力学的に求め、その合力が床面上のある点に発生するモーメントが、鉛直軸まわりの成分を除いて０であるならば、その点を改めて目標ＺＭＰとするようにしても良い。ただし、目標対象物反力も、床反力の一形態としてとらえるならば、目標ＺＭＰの定義は、先に述べた定義のままでよい。
以上を前提として、本発明の実施形態を以下に詳細に説明する。
［第１実施形態］
図１は、第１および第２実施形態に係る一般的な多脚の移動ロボット（脚式移動ロボット）の外観図である。ただし、図１では、ロボット１は、第１脚＃１〜第６脚＃６までの６個の脚（脚体）を有するものとして図示しているが、第１実施形態においては、第５脚＃５および第６脚＃６は無いものとする。すなわち、第１実施形態では、ロボット１は、第１脚＃１〜第４脚＃４の４個の脚（脚体）を有する４脚ロボットであるとする。図１では、第２実施形態に係るロボット１の構成要素については、括弧付きの参照符号を付している。
図１に示すように、第１実施形態のロボット１（４脚ロボット）では、該ロボット１の基体である上体２４の右側部から２個の脚（第１脚＃１、第３脚＃３）が前後方向に並んで延設され、これと同様に上体２４の左側部から２個の脚（第２脚＃２、第４脚＃４）が前後方向に並んで延設されている。各脚＃１〜＃４の先端部には、フリージョイントである球関節１２を介して円盤状の接地部位１０が取り付けられている。以降、接地部位１０を各脚＃１〜＃４毎に区別する必要があるときは、それぞれを第１接地部位、第２接地部位、第３接地部位、第４接地部位という。
各接地部位１０は、その中心点を球関節１２の中心点に一致させて該球関節１２に係合されており、該接地部位１０の中心点（厳密には球関節１２）には、床反力モーメント（床反力のモーメント成分）が作用しないようになっている。すなわち接地部位１０の中心点の床反力モーメント（実床反力モーメント）は常に０となる。
図示のロボット１では、各脚＃１〜＃４は、ロボット１の上体２４寄りの部分と中間部とにそれぞれ関節１４，１５を備えると共に、各脚＃１〜＃４の先端部近傍（球関節１２と関節１４とを連結するリンクの球関節１２寄りの部分）に、バネなどの弾性部材から構成されたコンプライアンス機構４２と、接地部位１０に作用する実床反力を検出する床反力検出手段としての６軸力センサ３４とが介装されている。図示の例では、関節１４は２軸回りの回転が可能とされ、関節１５は１軸回りの回転が可能とされている。なお、コンプライアンス機構とし、接地部位の底面にスポンジやゴムなどの弾性体を設けてもよい。
各関節１４，１５の変位動作（各軸回りの回転動作）は、図示を省略する電動モータなどのアクチュエータにより行なわれる。そして、各関節１４，１５の実際の変位量（各軸回りの回転角）である実関節変位が、図示しないロータリエンコーダ等のセンサにより検出される。なお、６軸力センサ３４は、３軸方向の並進力と３軸回りのモーメントとを検出可能なものであるが、第１実施形態のロボット１では、前記したように接地部位１０の中心点に実床反力モーメントが作用しない。従って、６軸力センサ３４の代わりに、３軸方向の並進力を検出する３軸力センサ、あるいは、実床反力の並進力の鉛直成分だけを検出する力センサを用いてもよい。
上体２４には、マイクロコンピュータやアクチュエータ駆動回路等を含む電子回路ユニットから構成された制御装置５０と、上体２４の姿勢を検出する姿勢センサ３６と、図示しない電源（２次電池、キャパシタなど）とが内蔵されている。姿勢センサ３６は、例えば加速度センサとジャイロセンサとから構成されている。なお、本明細書では、「姿勢」は一般的には空間的な向きを意味する（但し、ロボットの全体の「姿勢」は、ロボットの運動の瞬時値を意味する）。そして、本実施形態では、姿勢センサ３６は、上体２４の姿勢のうち、例えば鉛直方向に対するピッチ方向およびロール方向の姿勢傾斜（傾き角）を検出する。すなわち、姿勢センサ３６は上体２４の実姿勢傾斜を検出する実姿勢傾斜検出手段として機能する。
なお、ロボット１の上体２４のヨー方向の姿勢（回転角）も制御するような場合には、上体２４のヨー方向の回転角も（換言すれば上体２４の３軸方向の姿勢を）姿勢センサ３６で検出するようにしてもよい。
図２は、前記制御装置５０の機能的構成および動作を示すブロック図である。なお、図２では、図１のロボット１から、制御装置５０、姿勢センサ３６および６軸力センサ３４を除いたものを実ロボット１と称している。ここで、前記図１に示したように、ロボット１のおおよそ前方向をＸ軸、おおよそ左横方向をＹ軸、上方向をＺ軸とした、床に固定された所定の座標系（ＸＹＺ座標系）を「支持脚座標系」または「グローバル座標系」と呼ぶ。以降、特に断らない限り、位置、姿勢、並進力およびモーメントはこの支持脚座標系（グローバル座標系）の座標成分の組によって表現されるものとする。攴持脚座標系（グローバル座標系）の原点は、定常的に単一の点に固定されている必要はなく、ロボット１の移動途中で床に対する原点位置を変更してもよい。例えば、ロボット１の所定の脚が着地する毎に支持脚座標系（グローバル座標系）の原点位置を変更してもよい。
図２に示すように、制御装置５０は、その機能的構成要素（機能的手段）として、歩容生成装置１００、目標床反力分配器１０２、ロボット幾何学モデル（逆キネマティクス演算部）１１０、階層型コンプライアンス動作決定部１１４、変位コントローラ１１２、実床反力検出器１０８、姿勢偏差演算部１０３、および姿勢安定化制御演算部１０４を備えている。以下に、制御装置５０のこれらの要素の概要を説明する。
歩容生成装置１００は、本発明における目標歩容決定手段あるいは目標運動決定手段としての機能を持ち、ロボット１の動作を規定する目標歩容を生成（決定）して出力する。目標歩容は、本実施形態では、ロボットの目標運動の軌道（ロボットの各部位の目標位置姿勢の時系列）と目標床反力の軌道（ロボットが床から受ける反力の作用点の目標位置とその作用点にかかる並進力およびモーメントの目標値との組の時系列）とから構成される。なお、本明細書では、「軌道」は、時系列パターン（時間的変化のパターン）を意味する。
歩容生成装置１００が出力する目標運動の軌道は、ロボット１の各接地部位１０の位置および姿勢の目標値の軌道である目標接地部位軌道と、ロボット１の上体２４の位置および姿勢の目標値の軌道である目標上体位置姿勢軌道とから構成される。なお、後述する第６実施形態のように、腕や頭部に係る関節を備えるロボットにおける歩容生成装置は、腕や頭部の目標位置姿勢軌道も目標運動の構成要素として決定して出力する。
また、歩容生成装置１００が出力する目標床反力の軌道は、本実施形態では、ロボット１の全床反力中心点の目標位置の軌道である目標全床反力中心点軌道と、この目標全床反力中心点を作用点とする全床反力の目標値の軌道である目標全床反力軌道とから構成される。本実施形態では、目標全床反力中心点軌道は、ＺＭＰの目標位置の軌道である目標ＺＭＰ軌道と同一とされている。
各接地部位１０の位置は、該接地部位１０のある代表点の位置であり、その代表点は、例えば、各接地部位１０の中心点（球関節１２の中心点）の、該接地部位１０の接地面（底面）への鉛直方向の投影点あるいは該接地部位１０の中心点（球関節１２の中心点）とされている。以降、各接地部位１０の代表点の位置を単に接地部位位置と呼ぶ。この接地部位位置の目標値の軌道（目標接地部位位置軌道）が歩容生成装置１００により決定される前記目標接地部位軌道に含まれる。
ここで、本実施形態のロボット１では、各接地部位１０はフリージョイントである球関節１２に係合されているので、各接地部位１０の姿勢を制御することはできない。このため、本実施形態では、歩容生成装置１００は、実際には各接地部位１０の目標姿勢の軌道を生成しない（生成する必要がない）。従って、本実施形態では、前記目標接地部位軌道は、目標接地部位位置軌道と同じものを意味する。
但し、後述する第６実施形態のロボットのように、各接地部位が各脚の先端部にその姿勢を制御可能に取り付けられている場合（各脚の先端部にアクチュエータにより動作可能な関節を介して接地部位が取り付けられている場合）には、各接地部位の目標姿勢の軌道も、前記目標接地部位軌道に含まれるべきものである。本明細書では、このような場合も一般的に考慮するために、しばしば「接地部位位置姿勢」という用語を用いるが、それは、本実施形態では、実質的には「接地部位位置」のことを意味する。
図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）を参照して、目標接地部位軌道（目標接地部位位置軌道）および目標全床反力中心点軌道をより具体的に説明する。第１実施形態では、脚＃１〜＃４のうち、第１脚＃１および第４脚＃４の組を支持脚、第２脚＃２および第３脚＃３の組を遊脚とする時期と、全ての脚＃１〜＃４を支持脚とする時期と、第１脚＃１および第４脚＃４の組を遊脚、第２脚＃２および第３脚＃３の組を支持脚とする時期とを順番に繰り返しつつ、遊脚となる脚の組を離床させて空中で移動させた後、所望の位置に着地させるという脚＃１〜＃４の運動を行なうことによりロボット１の移動を行なうようにしている。支持脚は、接地してロボット１の自重を支える脚（０でない床反力を作用させるべき脚）であり、遊脚は支持脚でない脚である。
図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）は、上記の如くロボット１を移動させるときの各脚＃１〜＃４の先端の目標接地部位位置（詳しくは水平面（ＸＹ平面）上での位置）を時系列的に順番に（時刻ｔ１〜ｔ６の順に）示したものである。これらの図中の参照符号Ｑ１〜Ｑ４に対応する三角形が、それぞれ第１〜第４接地部位１０の目標接地部位位置（第１〜第４接地部位１０の前記代表点の水平面（ＸＹ平面）上での位置）を示している。
補足すると、図３および図４中の参照符号Ｑ１〜Ｑ４を付した三角形は、正確には、各接地部位１０に係る後述の目標ノード床反力中心点（目標接地部位床反力中心点）の位置を示すものである。ただし、本実施形態では、各接地部位１０の代表点を前記した如く設定しているので、第１〜第４接地部位１０のそれぞれの目標接地部位位置（代表点の目標位置）は、後述する目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）のそれぞれの位置に一致するか、もしくは、該目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）のそれぞれの位置と一定の位置関係を有する（水平面（ＸＹ平面）上での位置は一致する）。このため、図３および図４中の参照符号Ｑ１，Ｑ２，Ｑ３，Ｑ４に対応する三角形は、第１〜第４接地部位１０のそれぞれの後述の目標ノード床反力中心点を表すと同時に、第１〜第４接地部位１０のそれぞれの位置を表している。
図３（ａ）は、第２脚＃２および第３脚＃３の組を支持脚とし、遊脚としての第１脚＃１および第４脚＃４の組を着地させる瞬間（時刻ｔ１）での各接地部位１０の目標接地部位位置を示し、図３（ｂ）は、全ての脚＃１〜＃４が攴持脚となっている状態の時刻ｔ２での目標接地部位位置を示し、図３（ｃ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を離床させる（空中に持ち上げる）直前の時刻ｔ３での目標接地部位位置を示している。また、図４（ａ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を空中に持ち上げた状態の時刻ｔ４での目標接地部位位置を示し、図４（ｂ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を着地させた瞬間（時刻ｔ５）での目標接地部位位置を示し、図４（ｃ）は、第２脚＃２および第３脚＃３の組を支持脚とし、遊脚としての第１脚＃１および第４脚＃４の組を離床させる直前の時刻ｔ６での目標接地部位位置を示している。なお、図４（ａ）では遊脚である第２脚＃２および第３脚＃３の目標接地部位位置を破線の三角形で示している。補足すると、遊脚の接地部位１０の鉛直方向（Ｚ軸方向）の位置の軌道は、床面から所定の高さまで上昇した後、下降して、再び着地するように決定される。
これらの図３および図４における点Ｐは、目標全床反力中心点（目標ＺＭＰ）を示している。目標全床反力中心点軌道は、ＺＭＰの存在可能範囲（いわゆる支持多角形に相当する床面上の領域）内で該範囲の境界に近づきすぎない位置（例えばＺＭＰの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。第１実施形態では、２つの脚＃１，＃４または＃２，＃３が支持脚となるときには（図３（ａ），（ｃ）、図４（ａ）〜（ｃ）を参照）、それらの脚の接地部位１０，１０の代表点を結ぶ線分上で、その線分の端点に近づき過ぎないように目標全床反力中心点が設定される。また、全ての脚＃１〜＃４が支持脚となるときには（図３（ｂ）を参照）、全ての接地部位１０の代表点を頂角とする多角形内で、該多角形の境界に近づき過ぎないように目標全床反力中心点が設定される。図５（ｄ）は、このように決定された目標全床反力中心点（目標ＺＭＰ）の位置のＸ軸方向（ロボット１の進行方向）の成分ＺＭＰｘの軌道を例示したグラフである。なお、図５（ａ）〜（ｃ）は後述の重みの設定例を示すグラフである。
また、歩容生成装置１００が決定する目標上体位置姿勢軌道は、少なくとも、ロボット１の目標運動によって発生する慣性力とロボット１に作用する重力との合力が目標ＺＭＰまわりに作用するモーメントの水平成分が０になるようにロボット１の動力学モデルなどを使用して決定される。なお、「上体位置」は、上体２４のある代表点の位置である。
また、歩容生成装置１００が決定する目標全床反力は、目標全床反力中心点に作用する並進力およびモーメントの目標値から構成され、本実施形態では、ロボット１の目標運動によって発生する慣性力とロボット１に作用する重力との合力に目標全床反力中心点で釣り合うように決定される。なお、目標全床反力中心点（目標ＺＭＰ）まわりの目標全床反力のモーメント水平成分は０である。補足すると、目標全床反力中心点に作用する並進力およびモーメントの全ての成分を目標値として決定する必要はない。例えばロボット１の鉛直軸まわりの姿勢もしくは床反力を制御しない場合には、目標全床反力のモーメントの鉛直軸まわりの成分（Ｚ軸方向成分）を決定する必要はない。
なお、歩容生成装置１００が決定する目標接地部位軌道（目標接地部位位置軌道）は、後述する階層型コンプライアンス動作決定部１１４にて修正される。
前記目標床反力分配器１０２は、ロボット１の第１〜第４接地部位１０をツリー構造状にグループ分けし（すなわち階層化し）、階層化された各グループに、ツリー構造のノードを対応させる。したがって、以下の説明において、ノードをグループに置き換えて表現しても構わない。各ノードは、１つ以上の接地部位１０から構成されるグループである。
第１実施形態においては、図６に示す如く接地部位１０が階層化される。すなわち、第ｎ接地部位１０（ｎ＝１，２，３，４）を第ｎノードに対応させ、第１ノードと第４ノードとを子ノードとして持つノードを第１４ノードとし、第２ノードと第３ノードとを子ノードとして持つノードを第２３ノードとし、第１４ノードと第２３ノードとを子ノードとして持つノードを第１４２３ノードとする。従って、第１〜第４ノードは、それぞれ第１、第２、第３、第４接地部位１０から成るノード、第１４ノードは、第１接地部位１０と第４接地部位１０とから成るノード、第２３ノードは、第２接地部位１０と第３接地部位１０とから成るノード、第１４２３ノードは全ての接地部位１０から成るノードである。
ツリー構造における一般的な呼び方に従い、子ノードを持たないノードを葉ノードと呼び、親ノードを持たないノードを根ノードと呼ぶ。したがって、第ｎノード（ｎ＝１，２，３，４）は葉ノード、第１４２３ノードは根ノードである。なお、本明細書では、ノードを区別するために、葉ノードには、それに対応する接地部位（もしくは脚）と同じ番号（１，２，３，…）を割り付け、葉ノード以外のノードには、葉ノードより大きい番号を割り付けることとする。また、葉ノードおよび根ノード以外のノードを中間ノードと呼ぶ。第１実施形態では、中間ノードは第１４ノード、第２３ノードである。
目標床反力分配器１０２には、歩容生成装置１００で決定された目標歩容のうち、目標全床反力中心点軌道、目標全床反力軌道および目標接地部位軌道とが入力される。なお、歩容生成装置１００が目標歩容を決定するために使用した歩容パラメータ（ロボット１の遊脚の接地部位１０の着地予定位置、着地予定時刻など）も目標床反力分配器１０２に入力してもよい。そして、目標床反力分配器１０２は、これらの入力を基に、上記のように階層化された各ノードの床反力中心点の目標位置である目標ノード床反力中心点（目標第ｎノード床反力中心点）Ｑｎ（ｎ＝１，２，３，４，１４，２３，１４２３）と、根ノードを除く各ノードの重みＷｎ（ｎ＝１，２，３，４，１４，２３）とを決定する。ただし、各重みＷｎは１以下の非負の値とする。
目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）および重みＷｎ（ｎ＝１，２，３，４，１４，２３）の決定手法を図３（ｂ）、図５および図７などを参照しつつ以下に詳説する。図３（ｂ）には、同図示の状態におけるＱｎとＷｎとの関係が示され、図５（ａ）〜（ｃ）にはＷｎの設定例が示され、図７には重みと目標ノード床反力中心点との関係が示されている。
根ノードである第１４２３ノードの目標ノード床反力中心点（目標第１４２３ノード床反力中心点）Ｑ１４２３は、目標全床反力中心点（図３および図４中の点Ｐ）と同一に決定される（Ｐ＝Ｑ１４２３）。従って、以降の説明では、目標第１４２３ノード床反力中心点を示す符号として、多くの場合、目標全床反力中心点と同じ参照符号Ｐを用いる。
また、根ノードの重みＷ１４２３は、表現の統一性を保つために、便宜上「１」に設定される。
第ｎ接地部位１０（ｎ＝１，２，３，４）の目標床反力中心点（目標第ｎ接地部位床反力中心点とも言う）は、本実施形態においては、常に第ｎ接地部位１０の中心点（球関節１２の中心点）に設定される。そして、第ｎ接地部位１０に対応する葉ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）は、第ｎ接地部位１０の目標床反力中心点と同一に決定される。従って、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）も常に第ｎ接地部位１０の中心点（この点の位置は該第ｎ接地部位１０の目標接地部位位置から一義的に定まる）に設定される。このように決定した葉ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）の位置は、第ｎ接地部位１０の目標接地部位位置から一義的に定まる。逆に言えば、葉ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）は、第ｎ接地部位１０の目標位置を規定するものとなる。なお、以降の説明では、葉ノードの目標ノード床反力中心点を目標接地部位床反力中心点ということもある。また、各目標接地部位軌道は、連続的に変化するように設定されるので、Ｑｎ（ｎ＝１，２，３，４）も連続的に変化するものとなる。
補足すると、本実施形態では、第ｎ葉ノード（第ｎ接地部位１０）の目標床反力中心点Ｑｎ（ｎ＝１，２，３，４）を第ｎ接地部位１０の中心点に設定したが、接地部位の姿勢を制御できるロボットでは、各葉ノード（各接地部位）の目標床反力中心点は、例えば、その葉ノードに対応する接地部位の目標位置姿勢により定まる該接地部位の接地面（目標歩容で想定される床面との接触面）内に設定すればよい。
以降、一般に、任意の２つの点Ａ，Ｂを結ぶ線分またはその長さをＡＢというように表す。また、演算子「＊」は、スカラーとスカラーとの組もしくはスカラーとベクトルとの組に対しては、乗算を意味し、ベクトルとベクトルとの組に対しては、外積（ベクトル積）を意味する。
根ノード以外の各ノードの重みＷｎ（ｎ＝１，２，３，４，１４，２３）と各中間ノードの目標ノード床反力中心点Ｑ１４，Ｑ２３とは、目標第１４ノード床反力中心点Ｑ１４が、Ｑ１Ｑ１４：Ｑ１４Ｑ４＝Ｗ４：Ｗ１＝（１−Ｗ１）：Ｗ１という関係式を満たす線分Ｑ１Ｑ４の内分点、目標第２３ノード床反力中心点Ｑ２３が、Ｑ２Ｑ２３：Ｑ２３Ｑ３＝Ｗ３：Ｗ２＝（１−Ｗ２）：Ｗ２という関係を満たす線分Ｑ２Ｑ３の内分点、目標第１４２３ノード床反力中心点Ｐ（＝目標全床反力中心点）が、Ｑ１４Ｐ：ＰＱ２３＝Ｗ２３：Ｗ１４＝（１−Ｗ１４）：Ｗ１４を満たす線分Ｑ１４Ｑ２３の内分点となるように決定される。
言い換えると、少なくとも次式１，２，３を満足するように、Ｑ１４，Ｑ２３，Ｗｎ（ｎ＝１，２，３，４，１４，２３）が決定される。なお、これらの式１〜３中のＱｎ（ｎ＝１，２，３，４，１４，２３）は、その点の位置（位置ベクトル）を意味している。

Ｗｎ（ｎ＝１，２，３，４，１４，２３）が１以下の非負の値であることから、上式１，２，３の右辺におけるＱｎ（ｎ＝１，２，３，４，１４，２３）の係数（重み）は、すべて非負の値となる。
上式１〜３は、子ノードを持つ各ノード（すなわち葉ノードでない各ノード）の目標ノード床反力中心点の位置が、所定の非負の重みを用いて、そのノードの子ノードの目標ノード床反力中心点の位置の重み付き平均の位置に設定されることを意味する。言い換えると、前記図３（ｂ）および図７に示す如く、子ノードを持つ各ノードの目標床ノード反力中心点Ｑｎ（ｎ＝１４，２３，１４２３）は、そのノードのすべての子ノードの目標ノード床反力中心点の内分点に設定される。図７は、各ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３，１４２３）と重みＷｎ（ｎ＝１，２，３，４，１４，２３）との関係を示す図である。なお、図３（ｂ）中のＬ２３，Ｌ１４，Ｌ１４２３はそれぞれ線分Ｑ２Ｑ３、Ｑ１Ｑ４、Ｑ２３Ｑ１４の長さである。
補足すると、Ｑ１〜Ｑ４およびＰ（＝Ｑ１４２３）は前記した如く決定されるので、Ｗｎ（ｎ＝１，２，３，４，１４，２３）を決定すると、式１〜３を満たすＱ１４，Ｑ２３は一義的に定まり、逆に、Ｑ１４，Ｑ２３を決定すると、式１〜３を満たすＷｎ（ｎ＝１，２，３，４，１４，２３）は一義的に定まる。従って、重みＷｎ（ｎ＝１，２，３，４，１４，２３）を決定した上で、前記式１〜３に基づいて中間ノードの目標ノード床反力中心点Ｑ１４，Ｑ２３を決定しても、中間ノードの目標ノード床反力中心点Ｑ１４，Ｑ２３を決定した上で、前記式１〜３に基づいて重みＷｎ（ｎ＝１，２，３，４，１４，２３）を決定しても、どちらでもよい。
中間ノードの目標ノード床反力中心点Ｑ１４，Ｑ２３は、図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）に時系列的に示したように、各接地部位１０の移動に伴って移動する。この時の重みＷ１４（＝１−Ｗ２３），Ｗ１（＝１−Ｗ４），Ｗ３（＝１−Ｗ２）は、それぞれ例えば前記図５（ａ）〜（ｃ）のグラフで示す如く連続的に変化するように決定される。第ｎノード（ｎ＝１，２，３，４）のＱｎの軌道および重みＷｎが連続的に変化するように設定されるので、目標ノード床反力中心点Ｑ１４，Ｑ２３も連続的に移動するように決定される。つまり、すべての目標ノード床反力中心点（目標第ｎノード床反力中心点（ｎ＝１，２，３，４，１４，２３，１４２３））は、連続的に移動するように設定される。
目標床反力分配器１０２は、上記のように決定した各ノードの目標ノード床反力中心点を出力する。なお、根ノードの目標床反力中心点は、歩容生成装置１００が決定する目標全床反力中心点と同じであるので、目標床反力分配器１０２から出力する必要はない。
目標床反力分配器１０２は、さらに、各ノードの目標床反力中心点に作用する床反力の目標値である目標ノード床反力を決定して出力する。出力する目標ノード床反力には、少なくとも葉ノードである第ｎノード（ｎ＝１，２，３，４）の目標ノード床反力中心点Ｑｎに作用する目標ノード床反力（目標第ｎノード床反力）、すなわち、各接地部位１０の目標床反力が含まれる。この目標ノード床反力は、後述する階層型コンプラインアス動作決定部１１４の処理で実行される、各脚＃１〜＃４のコンプライアンス機構４２など（図１参照）のたわみ補償（より詳しくはコンプライアンス機構４２および各脚のリンク機構のたわみによる各接地部位１０の位置ずれの補償）等のために必要である。補足すると、ロボット１が、各脚＃１〜＃４の先端部以外にもコンプライアンス機構を備える場合には、コンプライアンス機構の変形を求めるために、目標第１４ノード床反力、目標第２３ノード床反力（中間ノードの目標ノード床反力）も決定して出力することが望ましい。
一般的に、各ノードの目標ノード床反力中心点に作用する目標床反力（目標ノード床反力）は、目標全床反力と各ノードの重みとから決定すればよい。具体的には、任意のノードの目標床反力は、そのノードの重みとそのノードのすべての先祖ノードの重みとの積に目標全床反力を乗じることによって決定すればよい。すなわち、次式４ａ（または式４ｂ）によって目標第ｎノード床反力が算出される。

あるいは、葉ノードでない任意の第ｎノードの目標床反力が、第ｎノードのすべての子ノードの目標床反力の和（合力）に一致し、かつ、根ノードの目標床反力が目標全床反力に一致するように、目標各ノード床反力が決定される。この関係を示したものが図８である。
すなわち、第ｎノードの目標床反力である目標第ｎノード床反力をＦｎ（ｎ＝１，２，３，４，１４，２３，１４２３）、目標全床反力をＦｔｏｔａｌｒｅｆで表すと、Ｆｎは、図８中に示す式によって、Ｆｔｏｔａｌｒｅｆと、重みＷｎ（ｎ＝１，２，３，４，１４，２３）とから決定される。図８中の式は前記式４ａまたは４ｂと等価である。
以上のごとく各ノードの目標床反力中心点（目標ノード床反力中心点）と各ノードの目標床反力（目標ノード床反力）とを決定することは、第ｎノードのすべての子ノードの目標床反力の合力が目標第ｎノード床反力中心点まわりに発生するモーメントの水平成分が０となるように各ノードの目標床反力中心点と目標床反力とを決定していることになる。従って、目標ノード床反力のモーメント水平成分は、いずれのノードについても０である。
なお、接地していない（非接地状態の）接地部位１０の目標床反力（接地していない接地部位１０に対応する葉ノードの目標ノード床反力）が０となるように、各目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）、各重みＷｎ（ｎ＝１，２，３，４，１４，２３）および各目標ノード床反力Ｆｎ（ｎ＝１，２，３，４，１４，２３）の組が決定される。従って、第１実施形態では、図５（ａ）に示したように、第１接地部位１０および第４接地部位１０が遊脚となって離床する時期（該遊脚の接地部位１０が空中を移動する時期）では、重みＷ１４が０（Ｗ２３＝１）とされ、また、第２接地部位１０および第３接地部位１０が遊脚となって離床する時期では、重みＷ２３が０（Ｗ１４＝１）とされる。このように、子ノードを持つ中間ノードの重みは、その中間ノードに葉ノードとして属する全ての接地部位が接地していない時期では０に設定される。逆に言えば、中間ノードの重みは、その中間ノードに属するいずれかの接地部位が接地している時期（厳密にはいずれかの接地部位に０でない床反力が作用している時期）では０には設定されない。
補足すると、第１実施形態では、中間ノードＱ１４に属する第１接地部位１０および第４接地部位１０のいずれか一方のみを非接地状態とする時期を有しないため、これらの接地部位１０，１０に対応する重みＷ１，Ｗ４は０または１となる時期を持たないものとなっている。ただし、第１接地部位１０および第４接地部位１０のいずれか一方のみが非接地状態となる時期を有するように、目標接地部位軌道を決定するようにした場合には、その時期で非接地状態となる接地部位１０に対応する重みを０に設定し、且つ、接地状態となる接地部位１０に対応する重みを１に設定するようにすればよい。この場合、その時期における中間ノードＱ１４の重みは、０で無い値に設定される。中間ノードＱ２３、並びにこれに属する葉ノードである第２接地部位１０および第３接地部位１０に係る重みについても同様である。一般的に言えば、子ノードを持つノードの重みは、そのノードに属する接地部位のいずれかが接地状態であるときには、０で無い値に設定され、そのノードに属する接地部位の全てが非接地状態となるときに、０に設定される。
上記のように求めた各目標ノード床反力は連続的に変化するので、ロボット１の衝撃の少ない移動（歩行）を実現するために適している。
以上の如く、第１実施形態では、各目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３，１４２３）、各重みＷｎ（ｎ＝１，２，３，４，１４，２３）および各目標各床反力Ｆｎ（ｎ＝１，２，３，４，１４，２３，１４２３）は、目標床反力分配器１０２によって次の条件Ａ）〜Ｇ）を満たすように決定される。
Ａ）各葉ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）は、該葉ノードに対応する接地部位１０の中心点に一致する。より一般的には、Ｑｎ（ｎ＝１，２，３，４）は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。補足すると、例えば各接地部位１０の目標床反力中心点を歩容生成装置１００で決定するようにしてもよく、その場合には、歩容生成装置１００が決定する目標床反力を基に、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４）を決定すればよい。
Ｂ）根ノードの目標ノード床反力中心点は、目標全床反力中心点Ｐに一致する。
Ｃ）根ノードを除く各ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）と重みＷｎ（ｎ＝１，２，３，４，１４，２３）とは、前記式１〜３の関係式を満たす。換言すれば、子ノードを持つ任意の第ｎノード（ｎ＝１４，２３，１４２３）の目標ノード床反力中心点Ｑｎは、その第ｎノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。
Ｄ）子ノードを持つ任意の第ｎノード（ｎ＝１４，２３，１４２３）の目標ノード床反力Ｆｎは、該第ｎノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノード（第１４２３ノード）の目標ノード床反力Ｆ１４２３は目標全床反力Ｆｔｏｔａｌｒｅｆに一致する。換言すれば、第ｎノード（ｎ＝１，２，３，４，１４，２３，１４２３）の目標床反力Ｆｎおよび重みＷｎが図８中の関係式を満たす。
Ｅ）接地していない接地部位１０に対応する葉ノードの目標ノード床反力は０となる。
Ｆ）Ｑｎ，Ｗｎ，Ｆｎ（ｎ＝１，２，３，４，１４，２３，１４２３）は連続的に変化する。
Ｇ）非接地状態の接地部位１０に対応する葉ノードの重み、または、その葉ノードの祖先ノードのいずれか１つの重みは０とされる。
なお、目標ノード床反力を決定する際に、前記のごとく重みを基に決定する代わりに、各目標ノード床反力中心点を基に決定しても良い。すなわち、前記Ａ）〜Ｃ）の条件を満たすように各目標ノード床反力中心点を決定した上で、その目標ノード床反力中心点と、前記式１〜３とを基に、各重みを決定し、その決定した重みを用いて前記式４により目標各ノード床反力を決定するようにしても良い。
図２の説明に戻って、前記姿勢偏差演算部１０３は、ロボット１に目標上体姿勢に対する実上体姿勢の偏差を算出して出力するものである。本実施形態では、該姿勢偏差演算部１０３には、前記姿勢センサ３６で検出された上体２４の鉛直方向に対する傾き角（以下、実上体姿勢傾斜という）と、歩容生成装置１００で決定された目標上体位置姿勢（詳しくは、目標上体姿勢のうちの鉛直方向に対する傾き角。以下、目標上体姿勢傾斜という）とが入力され、これらの偏差θｂｅｒｒ（実上体姿勢傾斜−目標上体姿勢傾斜。以下、上体姿勢傾斜偏差θｂｅｒｒという）が算出される。算出するθｂｅｒｒは、Ｘ軸まわり成分（ロール方向成分）θｂｅｒｒｘと、Ｙ軸まわり成分（ピッチ方向成分）θｂｅｒｒｙとから構成される。なお、目標上体姿勢傾斜が例えば定常的に０であるような場合には、θｂｅｒｒ＝実上体姿勢傾斜であるので、姿勢センサ３６の検出値（実上体姿勢傾斜）をそのまま、上体姿勢傾斜偏差θｂｅｒｒとして出力すればよい。
前記姿勢安定化制御演算部１０４は、一般的に言えば、前記上体姿勢傾斜偏差など、ロボット１に備えられたセンサの情報に基づいて検出または推定されるロボット１の状態に応じてロボット１の姿勢を安定化するための全床反力の補償量（目標全床反力の補正量）である補償全床反力を算出するものである。
ロボット１の姿勢を長期的に安定化させるためには、ロボット１の上体２４などの所定の部位の実際の位置姿勢を望ましい位置姿勢に復元させるために必要な並進力とモーメントとを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な並進力とモーメントとを補償全床反力という。また、補償全床反力のモーメント成分を「補償全床反力モーメントＭｄｍｄ」（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）と呼ぶ。
本実施形態では、姿勢安定化制御演算部１０４は、実上体姿勢傾斜を目標上体姿勢傾斜に復元する（近づける）ように補償全床反力モーメントＭｄｍｄを算出する。このために、姿勢安定化制御演算部１０４には、前記姿勢偏差演算部１０３で求められた上体姿勢傾斜偏差θｂｅｒｒ（θｂｅｒｒｘ，θｂｅｒｒｙ）が入力される。
姿勢安定化制御演算部１０４は、入力された上体姿勢傾斜偏差θｂｅｒｒを基に、前記補償全床反力モーメントＭｄｍｄを算出する。算出するＭｄｍｄは、Ｘ軸まわり成分Ｍｄｍｄｘと、Ｙ軸まわり成分Ｍｄｍｄｙとから構成される。
ＭｄｍｄｘおよびＭｄｍｄｙは、具体的には、例えば次式５，６のフィードバック制御則（ここではＰＤ制御則）によって決定される。すなわち、ＭｄｍｄｘおよびＭｄｍｄｙは、上体姿勢傾斜偏差θｂｅｒｒｘ，θｂｅｒｒｙを０に近づけるように決定される。

ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、所定のゲインである。また、（ｄθｂｅｒｒｘ／ｄｔ），（ｄθｂｅｒｒｙ／ｄｔ）は、それぞれ上体姿勢傾斜偏差θｂｅｒｒｘ、θｂｅｒｒｙの時間微分値である。
本実施形態においては、補償全床反力モーメントＭｄｍｄのＺ軸まわり成分（ヨー方向成分）Ｍｄｍｄｚを用いないので、Ｍｄｍｄｚの決定を行なわないが、ロボット１のスピン（鉛直軸回りの滑り）を防止したりするためにＭｄｍｄｚを決定するようにしても良い。Ｍｄｍｄｚの決定法に関しては、本願出願人が先に提案した特願２００３−１８５６１３号や特願２００３−１８５９３０号にて詳細に説明されている。また、例えばロボット１の実際の重心の位置を目標歩容での重心の位置に復元するために、その重心の位置偏差に応じて補償全床反力の並進力を決定するようにすることも可能である。
前記床反力検出器１０８は、各脚＃１〜＃４の６軸力センサ３４の出力を基に、実際のロボット１の各接地部位１０に作用する床反力の実際値である実床反力（すなわち葉ノードの実床反力（実ノード床反力））を検出する。さらに、床反力検出器１０８は、ロボット１の各関節１４，１５に備えたエンコーダ等のセンサ（図示しない）によって検出される各脚＃１〜＃４の各関節１４，１５の実関節変位（各関節１４，１５の各回転軸まわりの実回転角）に基づき、上体２４に固定された座標系に対する各接地部位１０の相対位置姿勢（第１実施形態では相対位置）を算出する。なお、このとき、実関節変位の代わりに、各関節１４，１５の変位指令値（回転角指令値）である関節変位指令を用いたり、あるいは、実関節変位と関節変位指令との両者を使用してもよい。そして、算出した各接地部位１０の相対位置姿勢を基に、各脚＃１〜＃４毎の６軸力センサ３４の検出値（これは６軸力センサ３４などに固定されたローカル座標系での値である）を座標変換して、上体２４に固定された座標系で表現された実床反力を算出した後、これを攴持脚座標系（グローバル座標系）で表現された実床反力に変換する。支持脚座標系への座標変換には、姿勢センサ３６の検出値、あるいは目標上体姿勢傾斜を用いればよい。補足すると、第１実施形態のロボット１では、前記したように接地部位１０の中心点に床反力モーメントが作用しないので、各接地部位１０の実床反力のうちのモーメント成分を検出する必要はない。この場合、前記したように６軸力センサ３４の代わりに、３軸力センサを用いて、実床反力の３軸方向の並進力成分を検出したり、１軸床反力センサを用いて実床反力の並進力鉛直成分だけを検出するようにしてもよい。
前記ロボット幾何学モデル（逆キネマティクス演算部）１１０は、各接地部位位置姿勢の最終的な目標軌道（これは後述の階層型コンプライアンス動作決定部１１４により決定される）と目標上体位置姿勢などを基に、逆キネマティクスの演算によって、それらを満足するロボット１の各関節１４，１５の変位（回転角）の指令値である各関節変位指令を算出する。本実施形態では、逆キネマティクスの演算の解の式をあらかじめ求めておき、その式に目標上体位置姿勢と各接地部位の最終的な目標位置を代入するだけで各関節変位指令を算出するようにした。すなわち、ロボット幾何学モデル１１０は、歩容生成装置１００で決定された目標上体位置姿勢軌道と階層型コンプライアンス動作決定部１１４で後述する如く修正された修正目標接地部位軌道（機構変形補償入り修正目標接地部位軌道）とが入力され、それらの入力値から逆キネマティクスの演算によって、各脚＃１〜＃４の各関節１４，１５の関節変位指令を算出する。
なお、後述する第６の実施形態のように腕関節や首関節など脚関節以外の関節があるロボットの場合には、手や頭部などの上体に対する相対位置姿勢を基に、逆キネマティクスの演算によって、脚関節以外の関節の変位が決定される。
前記変位コントローラ１１２は、ロボット１の各関節１４，１５の実関節変位（検出値）と、ロボット幾何学モデル（逆キネマティクス演算部）１１０で算出された関節変位指令とが入力され、該関節変位指令を目標値として、この目標値に実関節変位を追従させるように各関節１４，１５のアクチュエータ（図示しない）を制御（フィードバック制御）する。
階層型コンプライアンス動作決定部１１４は、目標全床反力と補償全床反力との合力に実全床反力を近づけるように目標接地部位軌道を修正し、その修正後の目標接地部位軌道である機構変形補償付き修正目標接地部位位置姿勢軌道を出力するものである。なお、本実施形態では、各接地部位１０の姿勢は制御できないので、機構変形補償付き修正目標接地部位位置姿勢軌道は、実際には、機構変形補償付き修正目標接地部位位置軌道である。
階層型コンプライアンス動作決定部１１４は、一般的には、以下の３つの要求をできるだけ満足させるように各接地部位１０の目標接地部位軌道を修正するものである。
要求１）ロボット１の位置姿勢を安定化するために、実全床反力を姿勢安定化制御演算部１０４が出力する補償全床反力（モーメントＭｄｍｄ）と目標全床反力との合力に追従させる。なお、第１実施形態ではロボット１の上体２４の姿勢傾斜（鉛直方向に対する傾き）を安定化するため、目標全床反力中心点まわりの実全床反力モーメントの水平成分を補償全床反力モーメントＭｄｍｄの水平成分Ｍｄｍｄｘ，Ｍｄｍｄｙに追従させる。補足すると、目標全床反力中心点まわりの目標全床反力モーメントの水平成分は０であるので、これとＭｄｍｄとの合力は、Ｍｄｍｄに一致する。
要求２）接地しているべき複数の接地部位１０に対し、その一部の接地部位１０に実床反力が集中し、それ以外のある一部の接地部位１０の実床反力が減少し過ぎ、その実床反力が減少した接地部位１０の接地性が極端に低下しないように、できる限り葉ノードでない各ノードの目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。なお、第１実施形態では、第１４ノード、第２３ノード、第１４２３ノードの目標床反力中心点まわりの実床反力モーメントの絶対値をできる限り小さくする。
要求３）接地しているべき各接地部位１０の接地性を確保するように、すなわち各接地部位１０におけるローカルな接地圧分布（実床反力の分布）が偏って、該接地部位１０の局所的な接地性が低下しないように、できるかぎり各接地部位１０（各葉ノード）の目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。但し、第１実施形態のロボット１では、各接地部位１０の目標床反力中心点まわりの実床反力モーメントは常に０となるので、この要求３）は考慮する必要はない。
尚、一般に、要求１）〜３）の全て、あるいは要求１）、２）を完全に満足することはできない。例えば実全床反力を補償全床反力と目標全床反力との合力に一致させながら各接地部位１０の目標床反力中心点まわりの実床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、階層型コンプライアンス動作決定部１１４は、一般には、要求１）〜３）、あるいは要求１）、２）をできるだけ満たしつつ、ある妥協点で、各接地部位１０の目標接地部位軌道を修正することとなる。
以上が制御装置５０の各機能的手段（機能的構成要素）の概要である。
補足すると、前記階層型コンプライアンス動作決定部１１４、姿勢安定化制御演算部１０４およびロボット幾何学モデル（逆キネマティクス演算部）１１０は、本発明におけるノード動作制御手段に相当するものである。
次に、図９のフローチャートを参照しつつ、制御装置５０の全体的な動作（演算処理）をより詳細に説明する。図９は制御装置５０のメインルーチン処理を示すフローチャート（構造化フローチャート）である。尚、図９の左端には、該当する処理を行う制御装置５０の構成要素を示す。
先ず、Ｓ１０において制御装置５０の初期化が行なわれた後、Ｓ１２を経てＳ１４に進み、制御装置５０の演算処理は、該制御装置５０の制御周期毎のタイマ割り込みを待つ状態となる。制御周期は例えば５０ｍｓである。
続いてＳ１６に進んで歩容の切り替わり目か否か判断し、その判断結果がＮＯであるときは後述のＳ２２に進む。また、Ｓ１６の判断結果がＹＥＳであるときはＳ１８に進んで時刻ｔを０に初期化した後、Ｓ２０に進んで歩容パラメータを設定する。本実施形態では、例えばロボット１のある所定の脚（例えば＃１）が離床してから次に離床するまで（もしくは着地してから次に着地するまで）の所定期間分の目標歩容を単位として、その所定期間分の目標歩容を規定するパラメータ（目標歩容を決定するためのアルゴリズムで使用するパラメータ）である歩容パラメータがＳ２０で設定される。そして、Ｓ１６でいうところの「歩容の切り替り目」は、上記所定期間分の目標歩容の切り替り目のことである。なお、目標歩容の切り替り目の判断は、時刻や、上記所定の脚の６軸力センサ３４の検出値などを基に行えばよい。
Ｓ２０で設定される歩容パラメータは、ロボット１の目標運動軌道（詳しくは目標接地部位軌道および目標上体位置姿勢軌道）を規定する運動パラメータと、目標床反力軌道（詳しくは目標全床反力軌道および目標全床反力中心点軌道）を規定する床反力パラメータとから構成される。補足すると、ロボット１の目標運動が決まれば、その目標運動によって発生する慣性力とロボット１に作用する重力との合力の符号を反転させたものを目標全床反力とすればよいので、床反力パラメータは、目標全床反力中心点軌道のみを規定するものでもよい。また、目標接地部位軌道（より一般的には目標上体位置姿勢以外の目標運動のパラメータ）と目標全床反力中心点とを決定した上で、ロボット１の動力学モデルを使用し、ロボット１の目標上体位置姿勢を含む目標運動が発生する慣性力とロボット１に作用する重力との合力が目標全床反力中心点（目標ＺＭＰ）まわりに発生するモーメントの水平成分が０になるように、目標上体位置姿勢を決定するような場合には、運動パラメータには、目標上体位置姿勢軌道を規定するパラメータが含まれていなくてもよい。
続いてＳ２２に進み、上記歩容パラメータを基に、目標歩容の瞬時値を決定する。ここで「瞬時値」は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置姿勢、目標接地部位位置（目標接地部位軌道の瞬時値）、目標全床反力、および目標全床反力中心点位置（目標ＺＭＰ位置）の瞬時値から構成される。なお、本実施形態では、接地部位１０の姿勢を制御できないので、各接地部位１０の目標姿勢の瞬時値は決定しない。各接地部位の姿勢を制御できる場合には、前記歩容パラメータに各接地部位の目標姿勢を規定するパラメータを含めておき、そのパラメータを基に各接地部位の目標姿勢の瞬時値を決定すればよい。
上記Ｓ１４〜Ｓ２２の処理は、前記歩容生成装置１００により実行される処理である。
続いてＳ２４に進んで、各ノードの重みＷｎ（ｎ＝１，２，３，４，１４，２３）と各ノードの目標床反力中心点（目標ノード床反力中心点）Ｑｎ（ｎ＝１，２，３，４，１４，２３）とが求められる。この処理は目標床反力分配器１０２によって前記した如く実行される処理である。
続いてＳ２６に進んで、目標ノード床反力（少なくとも各接地部位１０（各葉ノード）の目標床反力を含む）が求められる。第１実施形態では、各葉ノードの目標ノード床反力（各目標接地部位床反力）が求められる。このＳ２６処理も、目標床反力分配器１０２によって前記した如く実行される処理である。なお、前述の通り、各脚＃１〜＃４の先端部以外にもコンプライアンス機構を設けた場合には、さらに葉ノードでない各中間ノードの目標床反力も求めた方が良い。補足すると、目標ノード床反力のモーメント水平成分は０である。
続いてＳ２８に進み、姿勢センサ３６などの出力から前記実上体姿勢傾斜などのロボット１の状態を検出する。第１実施形態では、前記姿勢偏差演算部１０３によって、姿勢センサ３６による実上体姿勢傾斜の検出値が取り込まれ、その検出値と目標上体位置姿勢のうちの目標上体姿勢傾斜（現在時刻の瞬時値）とから、上体姿勢傾斜偏差θｂｅｒｒが算出される。
続いてＳ３０に進み、Ｓ２８で検出したロボット１の状態などからロボット１の姿勢を安定化するための補償全床反力を求める。第１実施形態では、前記姿勢安定化制御演算部１０４によって、前記上体姿勢傾斜偏差θｂｅｒｒから、目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄの水平成分Ｍｄｍｄｘ，Ｍｄｍｄｙが前記式５および式６に従って算出される。
続いてＳ３２に進んで各接地部位１０の実床反力を検出する。これは前記実床反力検出器１０８によって実行される処理であり、前記の如く、６軸力センサ３４によって検出された各接地部位１０毎の実床反力を支持脚座標系（グローバル座標系）に変換したものが求められる。以降、各接地部位１０の実床反力を実接地部位床反力ということがある。
続いてＳ３４からＳ３８まで、前記階層型コンプライアンス動作決定部１１４の処理が実行される。
以下、しばらくの間、これらのＳ３４〜Ｓ３８の処理を具体的に説明する前に、階層型コンプライアンス動作決定部１１４の全体的な処理を説明する。なお、この説明は、理解の便宜上、主に前記図３（ｂ）に示す状態（ロボット１の全ての脚＃１〜＃４が支持脚となっている状態）を例にとって行なう。
階層型コンプライアンス動作決定部１１４は、目標床反力分配器１０２で決定された各葉ノードの目標ノード床反力などを基に、葉ノードを除く各ノードの目標ノード床反力の並進力成分とモーメント成分（より詳しくは目標ノード床反力中心点を作用点とする並進力成分とモーメント成分）とを決定する。
図３（ｂ）の状態における各ノードの目標床反力（各目標ノード床反力）の並進力成分を、例示的に図１０に示す。この図において、ベクトルＦｎ＿ｒｅｆ（ｎ＝１，２，３，４，１４，２３）が目標第ｎノード床反力の並進力成分を表している。また、Ｆｔｏｔａｌｒｅｆは目標全床反力（＝目標第１４２３ノード床反力）の並進力成分を表す。前記図８に示したように、葉ノードでない任意の第ｎノード（ｎ＝１４，２３，１４２３）の目標第ｎノード床反力は、第ｎノードのすべての子ノードの目標ノード床反力の合力に決定される。従って、Ｆ１４ｒｅｆ＝Ｆ１ｒｅｆ＋Ｆ４ｒｅｆ、Ｆ２３ｒｅｆ＝Ｆ２ｒｅｆ＋Ｆ３ｒｅｆ、Ｆｔｏｔａｌｒｅｆ（＝Ｆ１４２３ｒｅｆ）＝Ｆ１４ｒｅｆ＋Ｆ２３ｒｅｆである。階層型コンプライアンス動作決定部１１４は、目標床反力分配器１０２で決定された各葉ノード（各接地部位１０）の目標ノード床反力の並進力成分Ｆｎ＿ｒｅｆ（ｎ＝１，２，３，４）から、上記の如く、各中間ノードの目標ノード床反力の並進力成分Ｆ１４ｒｅｆ（＝Ｆ１ｒｅｆ＋Ｆ４ｒｅｆ）、Ｆ２３ｒｅｆ（＝Ｆ２ｒｅｆ＋Ｆ３ｒｅｆ）を求める。なお、Ｆｔｏｔａｌｒｅｆ（＝Ｆ１４２３ｒｅｆ）は、前記歩容生成装置１００で決定された目標全床反力の並進力成分に設定される。
さらに、階層型コンプライアンス動作決定部１１４は、目標ノード床反力の並進力成分の場合と同様に、葉ノードを除く各ノードの目標ノード床反力の、目標ノード床反力中心点を作用点とするモーメント成分を決定する。ただし、この場合、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）の定義より、目標第ｎノード床反力のモーメント水平成分は、常に０に設定される。目標第１４２３ノード床反力（＝目標全床反力）のモーメント水平成分も０に設定される。
本実施形態のロボット１においては、各接地部位１０は各脚＃１〜＃４の先端部の球関節１２（フリージョイント）に係合しているので、各接地部位１０（各葉ノード）に床反力モーメント（水平成分および鉛直成分）を発生させることができない。このため、階層型コンプライアンス動作決定部１１４では、各接地部位１０（各葉ノード）の目標ノード床反力のモーメント鉛直成分も０に設定される。
葉ノードでないノードの目標ノード床反力モーメント鉛直成分は、ロボット１の目標運動に動力学的に釣り合うように決定すると、一般的には０ではない値も採り得るが、本実施形態では、ロボット１の姿勢の鉛直軸まわりの回転（ヨー方向の回転）に関する制御を行なわない。このため、本実施形態では、葉ノードでないノードの目標ノード床反力のモーメントの鉛直成分の設定は省略する。このようなことから、図３（ｂ）の状態における各目標ノード床反力のモーメント成分の図示は省略する。尚、ロボット１の姿勢の鉛直軸まわりの回転に関する制御を行なう場合には、各ノードの目標床反力モーメント鉛直成分も設定すべきである。
補足すると、葉ノードを含めた各ノードの目標ノード床反力の並進力成分およびモーメント成分の決定を、前記目標床反力分配器１０２で行なうようにした場合には、その決定を階層型コンプラインアンス動作決定部１１４で行なう必要はない。
また、階層型コンプライアンス動作決定部１１４は、各ノードの実床反力である実ノード床反力の並進力成分およびモーメント成分も決定する。
図３（ｂ）の状態における各ノードの実床反力（実ノード床反力）の並進力成分を、例示的に図１１に示す。この図において、ベクトルＦｎ＿ａｃｔ（ｎ＝１，２，３，４，１４，２３）が実第ｎノード床反力の並進力成分を表している。また、Ｆｔｏｔａｌａｃｔは実全床反力（＝実第１４２３ノード床反力）の並進力成分を表す。一般に、葉ノードでない各ノードの実床反力の並進力成分は、そのノードのすべての子ノードの実床反力の並進力成分の合力とする。従って、第１４ノード、第２３ノードおよび第１４２３ノードの実床反力の並進力成分はＦ１４ａｃｔ＝Ｆ１ａｃｔ＋Ｆ４ａｃｔ、Ｆ２３ａｃｔ＝Ｆ２ａｃｔ＋Ｆ３ａｃｔ、Ｆｔｏｔａｌａｃｔ（＝Ｆ１４２３ａｃｔ）＝Ｆ１４ａｃｔ＋Ｆ２３ａｃｔである。また、各葉ノードの実床反力の並進力成分Ｆ１ａｃｔ，Ｆ２ａｃｔ，Ｆ３ａｃｔ，Ｆ４ａｃｔは、前記実床反力検出器１０８で得られる各接地部位１０の実床反力（実接地部位床反力）の並進力成分である。なお、図１１の点線で示したベクトルは、前記図１０に示した目標ノード床反力の並進力成分である。階層型コンプライアンス動作決定部１１４は、上記のように、実床反力検出器１０８で得られた各接地部位１０の実床反力から各ノードの実ノード床反力の並進力成分を決定する。
また、図３（ｂ）の状態における各ノードの実床反力のモーメント成分を、例示的に図１２に示す。この図において、ベクトルＭｎ＿ａｃｔ（ｎ＝１４，２３，１４２３）は実第ｎノード床反力のモーメント成分を表す。この場合、各ノードの実床反力の並進力成分と同様に、一般的には、葉ノードでないノードの実床反力のモーメント成分Ｍ１４ａｃｔ、Ｍ２３ａｃｔ、Ｍ１４２３ａｃｔ（＝Ｍｔｏｔａｌａｃｔ）は、そのノードのすべての子ノードの実床反力の合力のモーメント成分（そのノードの目標床反力中心点Ｑ１４，Ｑ２３，Ｑ１４２３を作用点とするモーメント成分）とする。
本実施形態のロボット１においては、前記したように各接地部位１０（各葉ノード）に床反力モーメントを発生させることができないので、各葉ノードの実床反力（実接地部位床反力）のモーメント成分は、常に０になる。よって、各葉ノードの実ノード床反力のモーメント成分の図示は省略する。
葉ノードでないノードの実床反力モーメント成分（本実施形態では、Ｍ１４ａｃｔ，Ｍ２３ａｃｔ，Ｍ１４２３ａｃｔ）は、一般的に、０にならない。例えば目標第１４ノード床反力中心点Ｑ１４のまわりには、一般には、第１接地部位１０（第１ノード）および第４接地部位１０（第４ノード）の実床反力の並進力成分の水平成分によってモーメントが発生する。ただし、本実施形態のロボット１においては、脚の先端部にフリージョイントである球関節１２を備えているので、Ｍ１４ａｃｔの線分Ｑ１Ｑ４と同方向の成分、Ｍ２３ａｃｔの線分Ｑ２Ｑ３と同方向の成分は０となる。
尚、各脚の先端部の関節にアクチュエータを備えたロボット（各脚の先端部の接地部位の姿勢を制御可能なロボット）においては、該ロボットの姿勢の鉛直軸まわり回転に関する制御を行なわない場合でも、本願出願人が先に提案した特開平１０−２７７９６９号公報の実施例に示すごとく、各接地部位の目標床反力モーメント水平成分も設定し、また、各接地部位の実床反力モーメントも検出する。そして、各接地部位の実床反力モーメント水平成分を目標床反力モーメント水平成分に近づけるように、あるいは、各接地部位の実床反力モーメントを、目標床反力モーメント水平成分とその接地部位補償床反力モーメントとの和（ベクトル和）に近づけるように、各接地部位の姿勢の補正動作を行うべきである。
階層型コンプライアンス動作決定部１１４の処理をより詳細に説明する。この場合、図３（ｂ）に示す状態で、ロボット１の上体２４の姿勢が左後ろ側に倒れそうな状態となり、姿勢安定化制御演算部１０４が決定した補償全床反力モーメントＭｄｍｄが、図１３のようになっている状況を想定する。
ロボット１の上体２４の姿勢（鉛直方向に対する傾き）を復元させる（目標上体姿勢における傾きに復元させる）ためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｔｏｔａｌｒｅｆ（＝Ｍ１４２３ｒｅｆ）と補償全床反力モーメントＭｄｍｄとの和の水平成分に追従させれば良い。
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｔｏｔａｌｒｅｆの水平成分は０である。従って、ロボット１の上体２４の前後左右の姿勢（傾き）を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分（Ｍｄｍｄｘ，Ｍｄｍｄｙ）に追従させれば良い。また、本実施形態では、各接地部位１０の目標床反力中心点まわりの実床反力モーメントは０である。
そこで、第１実施形態における階層型コンプライアンス動作決定部１１４は、前記した要求１）、２）の要求をできる限り満足するよう歩容生成装置１００で決定した各接地部位１０の目標接地部位位置（特に高さ方向の位置）を修正する。
この修正を行なうために、階層型コンプラインアス動作決定部１１４は、補償角というものを決定する。この補償角は、接地部位１０の位置の間の相対関係をある点まわりの回転操作によって修正する（本実施形態では上下方向で修正する）ための操作量（回転量）である。本実施形態においては、その補償角としては、第１４ノード補償角θ１４、第２３ノード補償角θ２３、第１４２３ノード補償角θ１４２３がある。すなわち、葉ノード以外のノードの補償角である。そして、階層型コンプライアンス動作決定部１１４は、これらの補償角θ１４，θ２３，θ１４２３を基に、目標第ｎノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）を修正してなる目標第ｎノード床反力中心点Ｑｎ’を決定する。
図１５に示すように、第１４ノード補償角θ１４は、線分Ｑ１Ｑ４と線分Ｑ１’Ｑ４’とが成す角度、第２３ノード補償角θ２３は、線分Ｑ２Ｑ３と線分Ｑ２’Ｑ３’とが成す角度である。また、図１６に示すように、第１４２３ノード補償角θ１４２３は、線分Ｑ１４Ｑ２３と線分Ｑ１４’Ｑ２３’とが成す角度である。これらのノード補償角θ１４，θ２３，θ１４２３の決定手法は後述する。
図１５および図１６を参照して、補償角θ１４，θ２３，θ１４２３を基にした目標第ｎノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）の修正は次のように行なわれる。
図１５を参照して、目標第１ノード床反力中心点Ｑ１（第１接地部位１０の目標床反力中心点）と目標第４ノード床反力中心点Ｑ４（第２接地部位１０の目標床反力中心点）とを含みかつ水平面と垂直な平面の法線ベクトルＶ１４を求める。Ｖ１４の大きさは１とする。目標第１ノード床反力中心点Ｑ１の座標（位置）を、目標第１４ノード床反力中心点Ｑ１４を回転中心として法線ベクトルＶ１４まわり（Ｑ１４を通ってＶ１４に平行な軸まわり）に、前記第１４ノード補償角θ１４だけ回転移動する。この回転移動によるＱ１の移動後の点をＱ１’とする。同様に、目標第４ノード床反力中心点Ｑ４の座標（位置）を、目標第１４ノード床反力中心点を回転中心として法線ベクトルＶ１４まわりに第１４ノード補償角θ１４だけ回転移動する。この回転移動によるＱ４の移動後の点をＱ４’とする。換言すれば、線分Ｑ１Ｑ４をその内分点であるＱ１４を回転中心としてＶ１４まわりにθ１４だけ回転させて得られる線分の端点をＱ１’，Ｑ４’とする。従って、第１４ノード補償角θ１４は、第１４ノードの目標床反力中心点Ｑ１４の位置を動かさずに、第１４ノードの子ノードである第１ノードおよび第４ノードのそれぞれの目標床反力中心点Ｑ１，Ｑ４の位置の相対関係を動かすための操作量である。
また、目標第２ノード床反力中心点Ｑ２と目標第３ノード床反力中心点Ｑ３を含みかつ水平面と垂直な平面の法線ベクトルＶ２３を求める。Ｖ２３の大きさは１とする。目標第２ノード床反力中心点Ｑ２の座標（位置）を、目標第２３ノード床反力中心点Ｑ２３を回転中心として法線ベクトルＶ２３まわり（Ｑ２３を通ってＶ２３に平行な軸まわり）に、第２３ノード補償角θ２３だけ回転移動する。図１５に示すように、この回転移動によるＱ２の移動後の点をＱ２’とする。同様に、目標第３接地部位床反力中心点Ｑ３の座標（位置）を、目標第２３ノード床反力中心点を回転中心として法線ベクトルＶ２３まわりに第２３ノード補償角θ２３だけ回転移動する。この回転移動によるＱ３の移動後の点をＱ３’とする。換言すれば、線分Ｑ２Ｑ３をその内分点であるＱ２３を回転中心としてＶ２３まわりにθ２３だけ回転させて得られる線分の端点をＱ２’，Ｑ３’とする。従って、第２３ノード補償角θ２３は、第２３ノードの目標床反力中心点Ｑ２３の位置を動かさずに、第２３ノードの子ノードである第２ノードおよび第３ノードのそれぞれの目標床反力中心点Ｑ２，Ｑ３の位置の相対関係を動かすための操作量である。
次に、図１６を参照して、目標第１４ノード床反力中心点Ｑ１４と目標第２３ノード床反力中心点Ｑ２３を含みかつ水平面と垂直な平面の法線ベクトルＶ１４２３を求める。Ｖ１４２３の大きさは１とする。目標第１４ノード床反力中心点Ｑ１４の座標（位置）を、目標全床反力中心点Ｐ（＝Ｑ１４２３）を回転中心として法線ベクトルＶ１４２３まわり（Ｐを通ってＶ１４２３に平行な軸まわり）に、第１４２３補償角θ１４２３だけ回転移動する。この回転移動によるＱ１４の移動後の点をＱ１４’とする。同様に、目標第２３接地部位床反力中心点Ｑ２３の座標（位置）を、目標全床反力中心点Ｐを回転中心として法線ベクトルＶ１４２３まわりに第１４２３補償角θ１４２３だけ回転移動する。この回転移動によるＱ２３の移動後の点をＱ２３’とする。換言すれば、線分Ｑ１４Ｑ２３をその内分点であるＰ（＝Ｑ１４２３）を回転中心としてＶ１４２３まわりにθ１４２３だけ回転させて得られる線分の端点をＱ１４’，Ｑ２３’とする。従って、第１４２３ノード補償角θ１４２３は、第１４２３ノードの目標床反力中心点Ｐの位置を動かさずに、第１４２３ノードの子ノードである第１４ノードおよび第２３ノードのそれぞれの目標床反力中心点Ｑ１４，Ｑ２３の位置の相対関係を動かすための操作量である。
なお、以降、一般に、始点がＡ、終点がＢであるベクトルをベクトルＡＢと表記する。
次に、Ｑ１’をベクトルＱ１４＿Ｑ１４’だけ移動した点Ｑ１”を求める。同様に、Ｑ４’をベクトルＱ１４＿Ｑ１４’だけ移動した点Ｑ４”を求める。また、Ｑ２’をベクトルＱ２３＿Ｑ２３’だけ移動した点Ｑ２”を求める。同様に、Ｑ３’をベクトルＱ２３＿Ｑ２３’だけ移動した点Ｑ３”を求める。
次いで、第ｎ接地部位（ｎ＝１，２，３，４）の目標接地部位位置をベクトルＱｎ＿Ｑｎ”だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位１０の目標接地部位位置が修正されることとなる。補足すると、各ノードの目標床反力中心点と同様に、各葉ノードの位置を該葉ノードに対応する接地部位１０の位置とし、子ノードを持つ各ノードの位置を、そのノードの全ての子ノードの位置の、前記の如く設定された重みを用いた重み付け平均の位置として定義したとき、上記のように、第１４ノード、第２３ノード、第１４２３ノードのそれぞれの各子ノードの目標床反力中心点を補償角θ１４、θ２３、θ１４２３に応じて動かす処理は、第１４ノード、第２３ノード、第１４２３ノードのそれぞれの各子ノードの位置を補償角θ１４、θ２３、θ１４２３に応じて動かすことと同じことである。
尚、各脚の先端部にフリージョイントがないロボット（接地部位の姿勢を制御可能なロボット）においては、上記平行移動を各接地部位の姿勢（目標姿勢）を変えずに行なった後に、さらに、第ｎ接地部位をＱｎ”を中心に、前後方向軸（Ｘ軸）まわりに、ある回転角θｎ＿ｘだけ回転させると共に、左右方向軸（Ｙ軸）まわりに、ある回転角θｎ＿ｙだけ回転させる。これにより各接地部位の目標姿勢を修正する。この場合の回転角θｎ（θｎ＿ｘ、θｎ＿ｙからなる２次元量）を第ｎ接地部位補償角と呼び、θｎ＿ｘを第ｎ接地部位補償角Ｘ成分、θｎ＿ｙを第ｎ接地部位補償角Ｙ成分と呼ぶ。第ｎ接地部位補償角θｎは、本願出願人が先に提案した特開平１０−２７７９６９号公報に記載された手法に従って決定すればよい。
補足すると、前記各ノード補償角は、そのノードの目標ノード床反力中心点を作用点として、そこに本来発生すべき目標床反力モーメント（その水平成分は０）とノード補償床反力モーメントＭｎ＿ｄｍｄとの合力が発生するように決定される。この場合、ノード補償床反力モーメントＭｎ＿ｄｍｄの基となる補償全床反力モーメントＭｄｍｄは、実姿勢傾斜偏差を０に近づけるように決定されたものである。従って、各ノード補償角は、目標全床反力中心点まわりの床反力モーメントを所望のモーメント（この場合は補償全床反力モーメント）に近づけつつ、実姿勢傾斜偏差を０に近づけるよう各接地部位１０の相対位置関係を操作するための操作量として機能することとなる。
以上のごとく、各接地部位１０の目標接地部位位置（詳しくはその位置の相互の相対関係）を修正する処理が第１実施形態における階層型のコンプライアンス動作である。このコンプライアンス動作では、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位１０の目標接地部位位置の間の相対関係（相対的位置関係）の操作量（修正量）が各補償角θ１４，θ２３，θ１４２３に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位１０の目標接地部位位置の相互の相対関係が修正されることとなる。上記のように修正された目標接地部位位置姿勢を修正目標接地部位位置姿勢という。
このようなコンプライアンス動作では、一般に、前記補償角（補償動作量）が過大でなければ、各接地部位１０の接地圧力分布は変わっても、接地領域（接地面の圧力が正の領域）は変わらない。このような場合には、補償角に比例して各接地部位１０に装着されたコンプライアンス機構４２などが変形し、その変形量に応じた各接地部位１０の実床反力が発生する。この結果、補償角と補償角によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性、すなわち線形特性を持つ。
特性１）葉ノードでないノードの目標床反力中心点まわりの補償角だけを操作して各接地部位１０の目標接地部位位置を移動させると、下がった接地部位１０の実床反力の並進力成分が増加し、上がった接地部位１０の実床反力の並進力成分が減少する。このとき、各接地部位１０（各葉ノード）の修正された目標床反力中心点まわりの実床反力モーメントは、ほとんど変化しない。
特性２）第ｎ接地部位補償角だけを操作して第ｎ接地部位の目標姿勢を回転させると、第ｎ接地部位の目標床反力中心点に作用する実床反力のモーメント成分が変化し、並進力成分はあまり変化しない。
特性３）葉ノードでないノードの目標床反力中心点まわりの補償角と第ｎ接地部位補償角を同時に操作すると、各接地部位１０の実床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
図１７は、本実施形態における階層型コンプライアンス動作決定部１１４の機能を示すブロック図であり、同図を参照して階層型コンプライアンス動作決定部１１４の処理をさらに詳細に説明する。
階層型コンプライアンス動作決定部１１４は、その機能的手段として、補償全床反力モーメント分配器１１４ａ、補償角決定部１１４ｂ，１１４ｃ，１１４ｄ、修正目標接地部位位置姿勢算出部１１４ｇ、機構変形補償量算出部１１４ｎ、および機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈを備えている。
補償全床反力モーメント分配器１１４ａは、前記補償全床反力モーメントＭｄｍｄ（Ｍｄｍｄｘ，Ｍｄｍｄｙ）を、第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄに分配するものである。
第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄは、第１４２３補償角θ１４２３を操作することによって発生する各接地部位１０（詳しくは第１〜第４接地部位）の床反力の並進力成分が、目標第１４２３ノード床反力中心点（＝目標全床反力中心点（目標ＺＭＰ））まわりに発生すべきモーメントの目標値である。
第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄの前記ベクトルＶ１４２３方向の成分（Ｖ１４２３方向の軸まわりの成分）をＭ１４２３ｄｍｄｖと記述する。尚、ベクトルＶ１４２３は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１６参照）。Ｖ１４２３に直交し、且つ鉛直方向にも直交するベクトルをＵ１４２３とすると、本実施形態では、第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄのＵ１４２３方向の成分（Ｕ１４２３方向の軸回りの成分）Ｍ１４２３ｄｍｄｕは０に設定される。これは、本実施形態のロボット１では、第１４２３ノード補償角θ１４２３を操作しても、床反力モーメントのＵ１４２３方向成分を発生することはできないからである。また、本実施形態では鉛直軸まわりのロボット１の姿勢の回転を制御しないので、Ｍ１４２３ｄｍｄの鉛直方向成分Ｍ１４２３ｄｍｄｚも０に設定される。
第１４ノード補償床反力モーメントＭ１４ｄｍｄは、第１４補償角θ１４を操作することによって発生する各接地部位１０（詳しくは第１および第４接地部位）の床反力の並進力成分が、目標第１４ノード床反力中心点まわりに発生すべきモーメントの目標値である。
第１４ノード補償床反力モーメントＭ１４ｄｍｄのベクトルＶ１４方向成分をＭ１４ｄｍｄｖと記述する。尚、ベクトルＶ１４は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１５を参照）。Ｖ１４に直交し、且つ鉛直方向にも直交するベクトルをＵ１４とすると、本実施形態では、第１４ノード補償床反力モーメントＭ１４ｄｍｄのＵ１４方向成分Ｍ１４ｄｍｄｕは０に設定される。これは、本実施形態のロボット１では、第１４ノード補償角θ１４を操作しても、床反力モーメントのＵ１４方向成分を発生することはできないからである。なお、本実施形態では、Ｍ１４ｄｍｄの鉛直方向成分も０に設定される。
第２３ノード補償床反力モーメントＭ２３ｄｍｄは、第２３補償角θ２３を操作することによって発生する各接地部位１０（詳しくは第２および第３接地部位）の床反力の並進力成分が、目標第２３ノード床反力中心点まわりに発生すべきモーメントの目標値である。
第２３ノード補償床反力モーメントＭ２３ｄｍｄのベクトルＶ２３方向成分をＭ２３ｄｍｄｖと記述する。尚、ベクトルＶ２３は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１５を参照）。Ｖ２３に直交し、且つ鉛直方向にも直交するベクトルをＵ２３とすると、本実施形態では、第２３ノード補償床反力モーメントＭ２３ｄｍｄのＵ２３方向成分Ｍ２３ｄｍｄｕは０に設定される。これは、本実施形態のロボット１では、第２３ノード補償角θ２３を操作しても、床反力モーメントのＵ２３方向成分を発生することはできないからである。なお、本実施形態では、Ｍ２３ｄｍｄの鉛直方向成分も０に設定される。
第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄは、例えば次のように決定される。
任意の第ｎノードに対して、目標第ｎノード床反力中心点に第ｎノード補償床反力モーメントを加えることと等価になるように、第ｎノード補償床反力モーメントの水平成分を０にしたまま、目標第ｎノード床反力中心点を修正した場合における修正された目標第ｎノード床反力中心点位置を、修正目標第ｎノード床反力中心点位置と呼ぶ。
修正目標第１４２３ノード床反力中心点Ｐｍｄｆｄ（＝Ｑ１４２３ｍｄｆｄ）、修正目標第１４ノード床反力中心点Ｑ１４ｍｄｆｄ、修正目標第２３ノード床反力中心点Ｑ２３ｍｄｆｄと各ノード補償床反力モーメントとの関係を次式７〜９に示す。なお、目標第１４２３ノード床反力中心点Ｑ１４２３は全床反力中心点Ｐと一致するので、修正目標第１４２３ノード床反力中心点Ｐｍｄｆｄは、修正目標全床反力中心点ということもある。

なお、式７中の（Ｐｍｄｆｄ−Ｐ）など、２つの点の差は、それらの点の位置ベクトルの差を意味する。また、Ｆｔｏｔａｌｒｅｆ、Ｆ１４ｒｅｆ、Ｆ２３ｒｅｆは図１０に示したように、それぞれ第１４２３ノード、第１４ノード、第２３ノードの目標ノード床反力の並進力成分である。
ロボット１の接地性を高く保つためには、接地すべき接地部位１０の床反力が０に近づき過ぎたり、負の値になろうとするように制御すべきではない。したがって、以下の条件１）〜３）を満足すべきである。
修正ノード存在位置条件１）
Ｐｍｄｆｄは、全床反力中心点Ｐから線分Ｑ１４Ｑ２３の端点に近づき過ぎず、かつ、線分Ｑ１４Ｑ２３上にあること。このように、修正目標第１４２３ノード床反力中心点（修正目標全床反力中心点）Ｐｍｄｆｄが存在すべき範囲を、修正目標第１４２３ノード床反力中心点（修正目標全床反力中心点）の存在許容範囲と呼ぶ。
修正ノード存在位置条件２）
Ｑ１４ｍｄｆｄは、目標第１４ノード床反力中心点Ｑ１４から線分Ｑ１Ｑ４の端点に近づき過ぎず、かつ、線分Ｑ１Ｑ４上にあること。このように、修正目標第１４ノード床反力中心点Ｑ１４ｍｄｆｄが存在すべき範囲を、修正目標第１４ノード床反力中心点の存在許容範囲と呼ぶ。
修正ノード存在位置条件３）
Ｑ２３ｍｄｆｄは、目標第２３ノード床反力中心点Ｑ２３から線分Ｑ２Ｑ３の端点に近づき過ぎず、かつ、線分Ｑ２Ｑ３上にあること。このように、修正目標第２３ノード床反力中心点Ｑ２３ｍｄｆｄが存在すべき範囲を、修正目標第２３ノード床反力中心点の存在許容範囲と呼ぶ。
一方、ロボット１に実際に発生する姿勢復元力（上体２４の傾きの、目標上体姿勢の傾きへの復元力）を適切にするためには、第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄの合力が、補償全床反力モーメントＭｄｍｄに略一致すべきである。すなわち、以下の式１０を略満足すべきである。

そこで、本実施形態では、第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄは、それらと前記式７、式８、式９とによって定まる修正目標ノード床反力中心点Ｐｍｄｆｄ，Ｑ１４ｍｄｆｄＱ２３ｍｄｆｄの位置が前記修正ノード存在位置条件１）、２）および３）を満足している限りは、次式１１〜１３によって決定される。

ただし、Ｍａｔ１４２３、Ｍａｔ１４、Ｍａｔ２３はゲイン行列（第３要素が０である１行３列の行列）であり、これらは、式１１、式１２および式１３よって求められるＭ１４２３ｄｍｄ、Ｍ１４ｄｍｄ、Ｍ２３ｄｍｄが、式１０を満足するように設定される。
より厳密には、式１０、式１１、式１２および式１３からなる連立方程式がＭｄｍｄの値にかかわらず恒等的に成立するように、ゲイン行列Ｍａｔ１４２３、Ｍａｔ１４、Ｍａｔ２３が決定される。上記連立方程式が恒等的に成立するためのゲイン行列は一義的に決定されるわけではないので、例えば、修正ノード存在位置条件１）、２）および３）のいずれを特に重視するかによって、それに適切なゲイン行列を決定すれば良い。補足すると、各補償角θ１４２３，θ１４，θ２３を連続的に変化させるために、ゲイン行列は連続的に変化させることが望ましい。また、ロボット１の直立時であるか否かや、移動形態の違いなどによって、ゲイン行列の設定方針を変えても良い。
式１１、式１２および式１３にしたがって決定される第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄが修正ノード存在位置条件１）、２）および３）のいずれかを満足していない場合には、該条件１）、２）、３）を満足するように、これらを修正する。より具体的には、修正目標第ｎノード床反力中心点（ｎ＝１４２３，１４，２３）のうち、前記存在許容範囲を越えた修正目標ノード床反力中心点をその存在許容範囲の境界の点に設定し、その他の修正目標ノード床反力中心点を、式１１、式１２および式１３を可能な限り満足するように（左辺と右辺の差の絶対値がなるべく小さくなるように）決定する。ただし、一般的にフィードバック制御においてはフィードバック量が少々変っても、制御対象の挙動が大きく変わることはないので、式１１、式１２および式１３を厳密あるいは強引に満足させる必要はない。
以上のごとく、補償全床反力モーメント分配器１１４ａは、各ノード補償床反力モーメントＭ１４２３ｄｍｄ、Ｍ１４ｄｍｄ、Ｍ２３ｄｍｄを決定する。図１４には、このようにして決定された各ノード補償床反力モーメントＭ１４２３ｄｍｄ、Ｍ１４ｄｍｄ、Ｍ２３ｄｍｄの例を示した。図中のＭｄｍｄは、前記図１３に示したものと同じである。
また、補償全床反力モーメント分配器１１４ａは、各接地部位１０の目標床反力（各葉ノードの目標ノード床反力）である目標接地部位床反力、各ノード補償床反力モーメントＭ１４２３ｄｍｄ、Ｍ１４ｄｍｄ、Ｍ２３ｄｍｄを基に、目標接地部位床反力に各ノード補償床反力モーメントを付加することによって修正される各接地部位１０の目標床反力である修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４）を求める。
この際、各接地部位１０の目標床反力中心点（葉ノードの目標ノード床反力中心点）Ｑｎ（ｎ＝１，２，３，４）、目標全床反力中心点Ｐ、各目標ノード床反力中心点Ｑ１４、Ｑ２３と目標全床反力Ｆｔｏｔａｌｒｅｆとから、各目標ノード床反力を求める決定する手法と同様の手法によって、各接地部位１０の目標床反力中心点Ｑｎ（ｎ＝１，２，３，４）、修正全床反力中心点Ｐｍｄｆｄ、各修正目標ノード床反力中心点Ｑ１４ｍｄｆｄ、Ｑ２３ｍｄｆｄと目標全床反力Ｆｔｏｔａｌｒｅｆとから、修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４）を求めればよい。すなわち、各接地部位１０の目標床反力中心点Ｑｎ（ｎ＝１，２，３，４）、修正全床反力中心点Ｐｍｄｆｄ、各修正目標ノード床反力中心点Ｑ１４ｍｄｆｄ、Ｑ２３ｍｄｆｄから、前記式１〜３に基づいて各ノードの重みを決定し、その重みを用いて前記式４に基づいて修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４）を求めればよい。
このことから明らかなように、子ノードを持つ第ｎノードの目標第ｎノード床反力中心点にノード補償床反力モーメントを発生させる（目標第ｎノード床反力中心点に作用する目標床反力のモーメント成分を修正する）ということは、その第ｎノードの子ノードの重みを修正することと同等である。
以上説明した補償全床反力モーメント分配器１１４ａの処理は、図１８に示す機能ブロック図により表される。すなわち、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３）を基に、第１４ノード、第２３ノードおよび修正目標ノード床反力中心点Ｑｎ＿ｍｄｆｄ（ｎ＝１４，２３，１４２３）の存在許容範囲が前記修正ノード存在位置条件１）、２）、３）に従って決定される。また、補償全床反力モーメントＭｄｍｄと、各ノードの目標床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３，１４２３）および目標床反力Ｆｎ＿ｒｅｆ（ｎ＝１，２，３，４，１４，２３，１４２３）と、上記存在許容範囲とを基に、修正目標ノード床反力中心点Ｑｎ＿ｍｄｆｄ（ｎ＝１４，２３，１４２３）およびノード補償床反力モーメントＭｎ＿ｄｍｄ（ｎ＝１４，２３，１４２３）が決定される。また、修正目標ノード床反力中心点Ｑｎ＿ｍｄｆｄ（ｎ＝１４，２３，１４２３）と、各接地部位１０の目標床反力中心点Ｑｎ（ｎ＝１，２，３，４）と、目標全床反力Ｆｔｏｔａｌｒｅｆとから、修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４）が求められる。
次に、前記補償角決定部１１４ｂ〜１１４ｄのうちの補償角決定部（θ１４２３決定部）１１４ｂの処理を図１９のブロック図を参照して説明する。目標第１４ノード床反力中心点Ｑ１４に実第１ノード床反力と実第４ノード床反力との合力の並進力成分（Ｆ１ａｃｔ＋Ｆ４ａｃｔ）が作用すると共に、目標第２３ノード床反力中心点Ｑ２３に実第２ノード床反力と実第３ノード床反力との合力の並進力成分（Ｆ２ａｃｔ＋Ｆ３ａｃｔ）が作用した場合に、それらの並進力成分が、目標全床反力中心点Ｐ（目標第１４２３ノード床反力中心点）のまわりに発生するモーメントＭ１４２３ａｃｔを、次式１４により求める。

ここで、Ｐ＿Ｑ１４は始点がＰ、終点がＱ１４のベクトル、Ｐ＿Ｑ２３は始点がＰ、終点がＱ２３のベクトルである。
なお、式１４の代わりに、次式１５を用いてＭ１４２３ａｃｔを算出しても実際上はほとんど問題がない。

式１５の右辺は、第１〜第４ノード（各葉ノード）の実床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１，２，３，４）から目標全床反力中心点Ｐまわりに作用する実全床反力モーメントＭｔｏｔａｌａｃｔを算出する式になっている。補足すると、式１４は、目標全床反力中心点Ｐまわりに作用する実全床反力モーメントＭｔｏｔａｌａｃｔから、目標第１４ノード床反力中心点Ｑ１４まわりに作用する実床反力モーメントと目標第２３ノード床反力中心点Ｑ２３まわりに作用する実床反力モーメントとを減じたものになっている。
一般的に、葉ノードである任意の第ｎノードの実第ｎノード床反力モーメントＭｎ＿ａｃｔは、第ｎ接地部位の実床反力モーメントとする。また、葉ノードでない任意の第ｎノードに対し、そのすべての子ノードの床反力（ここでの子ノードの床反力は、厳密には、該子ノードの目標床反力中心点に作用する床反力）が目標第ｎノード床反力中心点に作用するモーメントを、実第ｎノード床反力モーメントＭｎ＿ａｃｔと呼ぶ。式１４および式１５のように、実第ｎノード床反力モーメントは、子ノードの実床反力モーメントを含む定義と含まない定義とがあるがいずれを用いても構わない。特に、子ノードの実床反力モーメントの水平成分を制御するフィードバック制御の応答性が高ければ、子ノードの実床反力モーメントの水平成分はただちに０に収束するので、いずれの定義を用いても、第ｎノードの実床反力の制御の挙動はあまり変らない。
以下に示す式１６は、式１４に対応した実第ｎノード床反力モーメントの算出の一般式、式１７は、式１５に対応した実第ｎノード床反力モーメントの算出の一般式である。ただし、式１６および式１７において、Ｑｎ＿Ｑｍは始点がＱｎ、終点がＱｍのベクトルである。また、葉ノードである第ｍノードの実床反力モーメントＭｍ＿ａｃｔは、実床反力検出器で検出された各接地部位の実床反力モーメントとする。
ｍ∈｛第ｎノードの子ノードの番号の集合｝とすると

ｍ∈｛第ｎノードの子孫である葉ノードの番号の集合｝であるとすると

なお、式１６、１７中のΣは、ｍについての総和を意味する。補足すると、第１実施形態では、葉ノード（各接地部位）の目標床反力中心点まわりの実床反力モーメントが０となることから、前記式１５の右辺では、式１７のＭｍ＿ａｃｔの成分が含まれていない。接地部位の姿勢を制御可能なロボットでは、一般には、式１７のＭｍ＿ａｃｔは０にならない。
第１４２３ノード補償角θ１４２３は、一般的には、上記のように求められるＭ１４２３ａｃｔと先に補償全床反力モーメント分配器１１４ａで求められた第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄとの偏差（Ｍ１４２３ａｃｔ−Ｍ１４２３ｄｍｄ）を０に近づけるようにフィードバック制御則などにより決定すればよい。例えば該偏差に所定のゲイン行列（３次の対角行列）を乗算することでθ１４２３を求めればよい。
但し、本実施形態では、前記ベクトルＶ１４２３の方向の軸回りの第１４２３ノード補償角θ１４２３を決定すればよいので、Ｍ１４２３ａｃｔのベクトルＶ１４２３方向の成分Ｍ１４２３ａｃｔｖとＭ１４２３ｄｍｄのベクトルＶ１４２３方向の成分Ｍ１４２３ｄｍｄｖとの偏差に応じてθ１４２３を決定すればよい。そして、この際、本実施形態では、床反力の制御の応答性や安定性を高めるために、Ｍ１４２３ａｃｔｖとＭ１４２３ｄｍｄｖとをフィルタに通した上で、それらの偏差に応じてθ１４２３を決定することとした。
具体的には、上記の如く求めたＭ１４２３ａｃｔのベクトルＶ１４２３方向の成分Ｍ１４２３ａｃｔｖを抽出する。これは、ベクトルの内積演算「・」を用いた次式１８によって得られる。

以上のようにＭ１４２３ａｃｔｖを算出する処理が図１９の参照符号１１４ｋを付した演算部により実行される。
次に、上記Ｍ１４２３ａｃｔｖをローパスフィルタ１１４ｉに通し、Ｍ１４２３ａｃｔｖｆｉｌｔを得る。また、前記第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄのＶ１４２３方向の成分Ｍ１４２３ｄｍｄｖを補償フィルタ１１４ｊに通し、Ｍ１４２３ｄｍｄｖｆｉｌｔを得る。なお、Ｍ１４２３ｄｍｄｖは、前記式１８と同様に、Ｍ１４２３ｄｍｄとＶ１４２３との内積演算により求められる。そして、Ｍ１４２３ｄｍｄｖｆｉｌｔをＭ１４２３ａｃｔｖｆｉｌｔから減じたものを、偏差モーメントＶ１４２３方向成分Ｍ１４２３ｅｒｒｖとして得る。
尚、前記補償フィルタ１１４ｊは、制御系におけるＭ１４２３ｄｍｄｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
最後に次式１９のフィードバック制御則（ここでは比例制御則）の演算によって前記第１４２３ノード補償角θ１４２３を得る。ここでＫ１４２３は制御ゲインであり、通常、これは正の値に設定する。

すなわち、偏差モーメントＶ１４２３方向成分Ｍ１４２３ｅｒｒｖに制御ゲインＫ１４２３を乗じることにより、第１４２３ノード補償角θ１４２３を得る。
次に、図１７の補償角決定部（θ１４決定部）１１４ｃの処理を、図２０を参照して説明すると、目標第１ノード床反力中心点Ｑ１に実第１ノード床反力（第１接地部位１０の実床反力）の並進力成分Ｆ１ａｃｔが作用すると共に、目標第４ノード床反力中心点Ｑ４に実第４ノード床反力（第４接地部位１０の実床反力）の並進力成分Ｆ４ａｃｔと作用した場合にそれらの並進力成分が、目標第１４ノード床反力中心点Ｑ１４のまわりに発生するモーメントＭ１４ａｃｔを、一般式である前記式１６を用いて求める。
より具体的には、次式２０により求める。

ここで、Ｑ１４＿Ｑ１は始点がＱ１４、終点がＱ１のベクトル、Ｑ１４＿Ｑ４は始点がＱ１４、終点がＱ４のベクトルである。
また、式２０の代わりに、次式２１（前記式１７を具体化した式）を用いても、前述の理由により、実際上はほとんど問題がない。

だたし、Ｍ１ａｃｔは、実第１ノード床反力モーメント、Ｍ４ａｃｔは、実第４ノード床反力モーメントである。なお、第１実施形態では各脚＃１〜＃４の先端部にフリージョイント（球関節１２）を備えているので、Ｍ１ａｃｔ，Ｍ４ａｃｔは０である。
式２１は、第１４ノードのすべての葉ノードの実床反力の合力が目標第１４ノード床反力中心点まわりに作用するモーメントを算出する式になっている。尚、式２０は、第１４ノードのすべての葉ノードの実床反力の合力が目標第１４床反力中心点まわりに作用するモーメントから、実第１ノード床反力モーメントと実第４ノード床反力モーメントとを減じたものになっている。
第１４ノード補償角θ１４は、一般的には、上記のように求められるＭ１４ａｃｔと先に補償全床反力モーメント分配器１１４ａで求められた第１４ノード補償床反力モーメントＭ１４２３ｄｍｄとの偏差（Ｍ１４ａｃｔ−Ｍ１４ｄｍｄ）を０に近づけるようにフィードバック制御則などにより求めればよい。
但し、本実施形態では、θ１４２３の場合と同様の理由によって、Ｍ１４ａｃｔの前記ベクトルＶ１４方向の成分Ｍ１４ａｃｔｖとＭ１４ｄｍｄのベクトルＶ１４方向の成分Ｍ１４ｄｍｄｖとをそれぞれフィルタに通したものの偏差に応じてθ１４を決定することとした。
すなわち、上記の如くもとめたＭ１４ａｃｔのベクトルＶ１４方向成分Ｍ１４ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式２２によって得られる。

以上のようにＭ１４ａｃｔｖを算出する処理が図２０の参照符号１１４ｋ’を付した演算部により実行される。
次に、上記Ｍ１４ａｃｔｖをローパスフィルタ１１４ｉ’に通し、Ｍ１４ａｃｔｖｆｉｌｔを得る。また、前記第１４ノード補償床反力モーメントＭ１４ｄｍｄのＶ１４方向の成分Ｍ１４ｄｍｄｖを補償フィルタ１１４ｊ’に通し、Ｍ１４ｄｍｄｖｆｉｌｔを得る。なお、Ｍ１４ｄｍｄｖは、Ｍ１４ｄｍｄとＶ１４との内積演算により求められる。そして、Ｍ１４ｄｍｄｖｆｉｌｔを、Ｍ１４ａｃｔｖｆｉｌｔから減じたものを、偏差モーメントＶ１４方向成分Ｍ１４ｅｒｒｖとして得る。尚、補償フィルタ１１４ｊ’は、制御系におけるＭ１４ｄｍｄｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
最後に次式２３のフィードバック制御則（ここでは比例制御則）の演算によって前記第１４ノード補償角θ１４を得る。ここでＫ１４は制御ゲインであり、通常、これは正の値に設定する。

図１７の補償角決定部（θ２３決定部）１１４ｄの処理は、第１４ノード補償角θ１４決定部１１４ｃの処理と同様であるので、ここでは詳細な説明を省略する。その処理の概要は、次の通りである。第２３ノードの子ノードの実床反力によって目標第２３ノード床反力中心点Ｑ２３のまわりに発生するモーメントＭ２３ａｃｔのベクトルＶ２３方向の成分Ｍ２３ａｃｔｖが前記式１６または１７に基づいて算出される。そして、そのＭ２３ａｃｔｖをローパスフィルタに通したものＭ２３ａｃｔｖｆｉｌｔを、前記第２３ノード補償床反力モーメントＭ２３ｄｍｄのＶ２３方向の成分Ｍ２３ｄｍｄｖを補償フィルタに通したものＭ２３ｄｍｄｖｆｉｌｔから減じてなる偏差モーメントＶ２３方向成分Ｍ２３ｅｒｒｖからフィードバック制御則（比例制御則）の演算によって、前記第２３ノード補償角θ２３が算出される。
以上の補償角決定部１１４ｂ〜１１４ｄの処理によって、目標全床反力中心点Ｐに作用する実モーメントＭａｃｔの水平成分を補償全床反力モーメントＭｄｍｄに近づけるようにノード補償角θ１４２３，θ１４，θ２３の組が決定されることとなる。なお、θ１４２３は、本実施形態では、前記式１９により求めたが、式１９の右辺のＭ１４２３ｅｒｒの代わりに、Ｍ１４２３ａｃｔ＋Ｍ１４ａｃｔ＋Ｍ２３ａｃｔ（実ノード床反力モーメントの総和）と、Ｍ１４２３ｄｍｄとの偏差、あるいは、実ノード床反力モーメントの総和とＭ１４２３ｄｍｄとをそれぞれフィルタに通したものの偏差に応じて決定するようにしてもよい。
補足すると、本実施形態では、各ノードの目標ノード床反力中心点Ｑｎを変更せずに、そのＱｎを作用点として、ノード補償床反力モーメントを決定した。そして、このノード床反力補償モーメント（より正確には、該ノード補償床反力モーメントとＱｎを作用点とする目標ノード床反力のモーメント成分との合力）とＱｎを作用点とする実ノード床反力モーメントとの偏差を制御量とし、この制御量を０に近づけるようにノード補償角を決定するようにした。このようにノード補償角を決定する代わりに次のようにノード補償角θ１４２３，θ１４，θ２３を決定するようにしてもよい。すなわち、子ノードを持つ各第ｎノード（ｎ＝１４，２３，１４２３）に対し、その第ｎノードの実ノード床反力（第ｎノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。あるいは、第ｎノード（ｎ＝１４，２３，１４２３）の実ノード床反力のモーメントから、その各子ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。例えば第１４ノードに関する実第１４ノード床反力中心点は、前記式１６または式１７により求めたＭ１４ａｃｔの水平成分を、Ｆ１ａｃｔとＦ４ａｃｔとの合力（実第１４ノード床反力の並進力成分）の鉛直成分で割った値だけ、目標第１４ノード床反力中心点を線分Ｑ１Ｑ４上でずらした点として求められる。第２３ノード、第１４２３ノードのノード床反力中心点についても同様である。そして、第ｎノード（ｎ＝１４，２３，１４２３）の目標ノード床反力中心点と、上記の如く求めた実第ｎノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィルタに通した値の偏差を制御量とし、その制御量を０に近づけるように、該制御量に応じてノード補償角θ１４２３，θ１４，θ２３を決定する（例えば制御量にあるゲインを乗算することで、θ１４２３，θ１４，θ２３を決定する）。
図１７の修正目標接地部位位置姿勢算出部１１４ｇは、第１４２３ノード補償角θ１４２３、第１４ノード補償角θ１４、第２３ノード補償角θ２３に基づき、前述した階層型コンプライアンス動作の目標接地部位位置姿勢の修正手法（図１５および図１６を参照して説明した手法）に従って各接地部位１０の目標位置および目標姿勢である目標接地部位位置姿勢を修正し、修正目標接地部位位置姿勢を得る。ただし、本実施形態においては、各脚＃１〜＃４の先端部にフリージョイント（球関節１２）があり、意図的に各接地部位１０の姿勢を変化させることはできないので、修正目標接地部位位置姿勢は、実際には、修正目標接地部位位置を意味する。
図２１は、図１７の前記機構変形補償量算出部１１４ｎの処理を示す機能ブロック図である。図２１に示すように、機構変形補償量算出部１１４ｎは、前記補償全床反力モーメント分配器１１４ａから出力される各接地部位１０の修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４）（目標接地部位床反力に、第１４２３ノード補償床反力モーメントＭ１４２３ｄｍｄ、第１４ノード補償床反力モーメントＭ１４ｄｍｄおよび第２３ノード補償床反力モーメントＭ２３ｄｍｄを付加することによって修正された目標接地部位床反力）によって発生が予想される各脚＃１〜＃４の変形量（各脚のリンク機構およびコンプライアンス機構４２の変形量）である機構変形量Ｅｎ＿ｍｄｆｄ（ｎ＝１，２，３，４）を求める。機構変形量Ｅｎ＿ｍｄｆｄ（ｎ＝１，２，３，４）は、各脚に作用する力（あるいは床反力）と該脚の変形との関係を表す機構コンプライアンスモデルを用いて求められる。そして、機構変形補償量算出部１１４ｎは、機構変形量Ｅｎ＿ｍｄｆｄ（ｎ＝１，２，３，４）を打ち消すための機構変形補償量Ｅｎ＿ｃｍｐｎ（ｎ＝１，２，３，４）を求める。機構変形補償量Ｅｎ＿ｃｍｐｎは、各機構変形量にＥｎ＿ｍｄｆｄに（−１）を乗じることにより求められる。
図１７の機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈは、算出された機構変形量Ｅｎ＿ｍｄｆｄを打ち消すように、各接地部位１０の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められ位置姿勢）をさらに修正し、各接地部位１０の機構変形補償入り修正目標接地部位位置姿勢を得る。機構変形補償入り修正目標接地部位位置姿勢は、各接地部位１０の修正目標接地部位位置姿勢に、それぞれに対応する機構変形補償量Ｅｎ＿ｃｍｐｎを付加することにより決定される。
例えば、修正目標接地部位床反力によって、ある接地部位１０に対応する脚のコンプライアンス機構４２などが鉛直方向にｚだけ縮むと算出された場合には、その接地部位１０の目標位置をｚだけ下げるように修正目標接地部位床反力が修正される。即ち、機構変形補償後の接地部位１０の接地面（底面）が目標接地部位床反力を受けて変形したときの位置姿勢が、機構変形補償前の接地部位の接地面の目標位置姿勢に一致するように、機構変形補償入り修正目標接地部位位置姿勢を算出する。なお、これについての詳しい説明は、特開平１０−２７７９６９号公報に詳細に説明されている。また、本実施形態のロボット１では、接地部位１０の姿勢を制御できないので、機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈで実際に修正されるものは、各接地部位１０の修正目標接地部位位置である。
上記のような機構変形補償は、コンプライアンス機構４２などの変形によって生じる実接地部位位置姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い形態でのロボット１の移動を実現することができる。
以上が、階層型コンプライアンス動作決定部１１４の処理の詳細である。
上記を前提として図９のフローチャートの説明に戻ると、Ｓ３４において、前記の如く補償角を決定する。図２２は、その補償角の決定処理のサブルーチンを一般化して示したフローチャートである。
同図を参照して説明すると、先ずＳ１００において、各接地部位１０の実床反力（前記実床反力検出器１０８で得られる実床反力）を基に、実第ｎノード床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１，２，３，４，１４，２３，１４２３）を算出する。この場合、各葉ノードの実ノード床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１，２，３，４）は、前記実床反力検出器１０８による各接地部位１０の実床反力の検出値の並進力成分である。また、葉ノードでない各ノードの実ノード床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１４，２３，１４２３）は、前記した如く、そのノードの子ノードの実ノード床反力の合力の並進力成分である。
次いでＳ１０２に進み、実第ｎノード床反力モーメントＭｎ＿ａｃｔ（ｎ＝１，２，３，４，１４，２３，１４２３）を算出する。第１実施形態のロボット１では、各葉ノードの実ノード床反力モーメントＭｎ＿ａｃｔ（ｎ＝１，２，３，４）は０である。また、葉ノードでない各ノードの実ノード床反力モーメントＭｎ＿ａｃｔ（ｎ＝１４，２３，１４２３）は、Ｓ１００で得られたＦｎ＿ａｃｔ（ｎ＝１，２，３，４）と、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１４，２３，１４２３）とから前記式１６または式１７に基づき算出される。
次いでＳ１０４に進み、前記姿勢安定化制御演算部１０４で求められた補償全床反力モーメントＭｄｍｄを基に、第ｎノード補償床反力モーメントＭｎ＿ｄｍｄ（ｎ＝１４，２３，１４２３）を決定する。この処理は、前記補償全床反力モーメント分配器１１４ａにより前記した通り行なわれる。
次いでＳ１０６に進み、階層型コンプライアンス動作の説明で定義したベクトルＶ１４２３，Ｖ１４，Ｖ２３およびこれに直交したベクトルＵ１４２３，Ｕ１４，Ｕ２３を求める。ただし、第１実施形態では、ベクトルＵ１４２３，Ｕ１４，Ｕ２３の方向の床反力モーメントを発生することはできないので、Ｕ１４２３，Ｕ１４，Ｕ２３を求める必要はない。
補足すると、後述する第２実施形態の如く、第ｎノードの子ノードの数が３以上の場合には、Ｖｎは、時間的に急激に変化しなければ、どの向きにとっても良いので、Ｖｎの向きは、支持脚座標系のＸ軸の向きや、ロボットの上体の向きに合わせるなど、適当に決定すれば良い。また、ＵｎはＶｎに直交していればよい。
次いでＳ１０８に進み、実第ｎノード床反力モーメントＭｎ＿ａｃｔ（ｎ＝１４，２３，１４２３）のＶｎ方向成分Ｍｎ＿ａｃｔｖとＵｎ方向成分Ｍｎ＿ａｃｔｕとを抽出する。これは、Ｍｎ＿ａｃｔとＶｎ、Ｕｎとの内積演算を行なえばよい。なお、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、Ｕｎ方向成分Ｍｎ＿ａｃｔｕは０である。従って、Ｍｎ＿ａｃｔｕは求める必要はない。
次いでＳ１１０に進み、第ｎノード補償床反力モーメントＭｎ＿ｄｍｄ（ｎ＝１４，２３，１４２３）のＶｎ方向成分Ｍｎ＿ｄｍｄｖとＵｎ方向成分Ｍｎ＿ｄｍｄｕを抽出する。これは、Ｍｎ＿ｄｍｄとＶｎ、Ｕｎとの内積演算を行なえばよい。なお、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、第ｎノード補償床反力モーメントＭｎ＿ｄｍｄのＵｎ方向成分Ｍｎ＿ｄｍｄｕは０にする。あるいは、Ｍｎ＿ｄｍｄｕを求める必要はない。
次いでＳ１１２に進み、Ｍｎ＿ａｃｔｖをフィルタに通した値とＭｎ＿ｄｍｄｖをフィルタに通した値の差にゲインＫｎを乗じることにより（より一般的には、当該差からフィードバック制御則により）第ｎノード補償角θｎのＶｎ成分を決定する。第１実施形態では、この処理は、第１４ノード、第２３ノード、第１４２３ノードについて、前記各補償角決定部１１４ｂ，１１４ｃ，１１４ｄにより前記した如く実行される。
次いでＳ１１４に進み、Ｍｎ＿ａｃｔｕをフィルタに通した値とＭｎ＿ｄｍｄｕをフィルタに通した値の差にゲインＫｎを乗じることにより（より一般的には当該差からフィードバック制御則により）第ｎノード補償角θｎのＵｎ成分を決定する。ただし、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、第ｎノード補償角Ｕ成分は０にする。あるいは、Ｓ１１４の処理を実行する必要はない。
以上が、図９のＳ３４のサブルーチン処理である。補足すると、Ｓ１０６〜Ｓ１１４の処理は、目標第ｎノード床反力中心点に作用する実第ｎノード床反力モーメントが第ｎノード補償床反力モーメント（より正確には第ｎノード補償床反力モーメントと目標第ｎノード床反力中心点に作用する目標ノード床反力モーメントとの合力）に収束するように第ｎノード補償角を決定する処理であると言える。
次いで、図９のフローチャートのＳ３６に進み、前記機構変形補償量を算出する。この処理は、前記機構変形補償量算出部１１４ｎにより前記した如く行なわれる。
次いでＳ３８に進んで目標接地部位位置姿勢をＳ３４で求めた補償角に応じて修正し、さらにこれをＳ３６で求めた機構変形補償量に応じて修正することによって、各接地部位１０の機構変形補償入り修正目標接地部位位置姿勢を得る。第１実施形態では、前記修正目標接地部位位置姿勢算出部１１４ｇによって、前記した如く（前記１５および図１６を参照して説明した如く）補償角θ１４２３、θ１４、θ２３に基づき各接地部位１０の修正目標接地部位位置を求める。そして、この求めた修正目標接地部位位置が前記機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈによって、前記した如く前記機構変形補償量Ｅｎ＿ｃｍｐｎ（ｎ＝１，２，３，４）に応じてさらに修正され、それによって、各接地部位１０の機構変形補償入り修正目標接地部位位置が得られる。
以上説明した図９のＳ３２〜Ｓ３８の処理が、階層型コンプライアンス動作決定部１１４の処理である。
次いでＳ４０に進み、目標上体位置姿勢と機構変形補償入り修正接地部位位置姿勢（第１実施形態では機構変形補償入り修正接地部位位置）からロボット１の関節変位指令を算出する。この処理は、前記ロボット幾何学モデル１１０により前記した如く実行される。
次いでＳ４２に進んで、実関節変位を関節変位指令に追従させる。この追従制御（サーボ制御）は、前記変位コントローラ１１２により実行される。
次いでＳ４４に進んで、時刻をΔｔ（制御装置５０の演算処理周期）だけ更新し、Ｓ１４に戻って上記の処理を繰り返す。
以上説明した第１実施形態の如き階層型のコンプアライアンス動作によって、接地部位が３つ以上ある移動ロボットにおいても、各ノード床反力の制御が互いに殆ど干渉しないようになり、各ノード床反力を容易かつ適切に制御することができる。したがって、制御の干渉がなく、各ノードの実床反力が望ましい値からずれたり発振することがない。このため、床面の大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。また、移動ロボットの姿勢を安定化する制御を容易に実現できると共に、移動ロボットが受ける着地衝撃を低減することができ、移動ロボットの接地性を高め、移動時のスリップやスピンを防止することができる。さらに、移動ロボットのアクチュエータの負荷を低減することができる。したがって、各接地部位の床反力を適切に制御し、高い姿勢安定性を得ることができる。
ロボットの姿勢の安定化に対する効果を補足すると、ロボットが想定した床面上を想定した通りに歩行している状態から、ロボット全体が傾いた場合に、傾き角偏差θｂｅｒｒ（θｂｅｒｒｘ，θｂｅｒｒｙ）とこれに応じて発生する目標全床反力中心点まわりのモーメント水平成分の増加量ΔＭとの関係が、比例関係にあることが望ましい。そうでなくても、ある回転行列Ｔ、ある対角行列ｄｉａｇ（ａ，ｂ）に対し、次式２４が成立することが望ましい。なお、Ｔ，ｄｉａｇ（ａ，ｂ）は２次の正方行列である。

これらの関係を満足しない場合には、ロボットの姿勢が傾いた状態から戻る時に、直線的に傾き角偏差θｂｅｒｒが０に収束せずに、味噌擂り運動を起こすおそれがある。例えば、ロボットの上体が前に傾いた状態では、後ろに戻す復元力以外に、横方向への転倒力（復元力）が余計に作用し、傾き角偏差θｂｅｒｒが直線的に０に戻らずに、後ろ横方向に復元力が働いて、傾き角偏差θｂｅｒｒが渦巻き状に０に収束していく。
また、上記と同じ理由から、コンプライアンス制御においては、ロボット全体の傾き角偏差θｂｅｒｒの変化速度ｄθｂｅｒｒ／ｄｔとこれに応じて発生するモーメントの増加量ΔＭｄとの関係も、比例関係にあることが望ましい。そうでなくても、ある回転行列をＴ、ある対角行列ｄｉａｇ（ｅ，ｆ）に対し、次式２５が成立することが望ましい。なお、Ｔ，ｄｉａｇ（ａ，ｂ）は２次の正方行列である。

一般的に、ロボットの各脚に独立にコンプライアンス制御を掛けると、これらの関係を満たさず、味噌擂り運動を起こすおそれがある。第１実施形態に示した階層型コンプライアンス制御においては、前記式２４，２５の関係を満足することができるので、ロボットの姿勢制御の収束性が高く、また、発振や振動を防止することができる。
前記第１実施形態においては、目標接地部位位置を、各接地部位の姿勢を変えずに（姿勢を制御せずに）、各ノード床反力中心点まわりに回転移動させるように補正（修正）動作を行うことで、各接地部位１０の間の相対高さを目標接地部位位置の間の相対高さから修正した。但し、目標接地部位位置を鉛直方向にだけ移動させるようにして、各接地部位１０の高さだけを修正するようにしても良い。具体的には、以下の手順によって目標接地部位位置を修正する。
まず、以下の式２６，２７により、目標第１４ノード床反力中心点の鉛直位置修正量Ｚ１４と目標第２３ノード床反力中心点の鉛直位置修正量Ｚ２３を求める。

但し、ここで、θ１４２３には前記式１９で求められる値を代入する。
次いで、各接地部位の目標床反力中心点（各葉ノードの目標ノード床反力中心点）の鉛直位置修正量Ｚｎ（ｎ＝１，２，３，４）を次式により求める。

但し、ここで、θ１４には、前記式２３で求められる値を代入し、θ２３には、θ１４と同様にして求められる値を代入する。
以上のように求めたＺ１、Ｚ２、Ｚ３、Ｚ４を目標接地部位位置に鉛直方向に加えることにより、修正目標接地部位位置を得る。
また、前記第１実施形態では、接地部位１０を前記図６に示した如く階層化したが、その階層構造は、必ずしも１つの階層構造にあらかじめ決めておく必要はない。例えばトロット、ギャロップなど、ロボット１の移動形態（移動時の脚の運動形態）などに応じて、階層構造を変更しても良い。例えば、図２３に示す如く接地部位１０を階層化してもよい。図２３は、例えばロボット１の全ての脚＃１〜＃４を攴持脚とする時期において、第１接地部位１０と第２接地部位１０との組を第１２ノードとし、第３接地部位１０と第４接地部位１０との組を第３４ノードとした場合の例を前記図３（ｂ）と同様に図示したものである。この例では、各ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，１２，３４，１２３４）は図示の如く設定される。このようにすることで、場合によっては、前記コンプライアンス動作と、後述の床形状推定をより的確に行なうことが可能となる。
［第２実施形態］
以下に前記図１並びに図２４〜図３４を参照しつつ、本発明の第２実施形態を説明する。図１を参照して、第２実施形態のロボット１が第１実施形態のロボットと異なる点を中心に説明すると、第２実施形態のロボット１は、第１〜第４脚＃１〜＃４に加えて、これらの脚＃１〜＃４と同じ構造の第５脚＃５および第６脚＃６を備える。すなわち、第２実施形態のロボット１は６脚ロボットである。第５脚＃５は、第３脚＃３の後側でロボット１の上体２４の右側部から延設され、第６脚＃６は、第４脚＃４の後側でロボット１の上体２４の左側部から延設されている。これ以外のロボット１の機構的構造は、第１実施形態と同一であるので、第１実施形態と同一の参照符号を付して説明を省略する。
この第２実施形態のロボット１（６脚ロボット）の階層型コンプライアンス制御の手法の基本的な考え方は第１実施形態のものと同じである。但し、第２実施形態の階層型コンプライアンス制御では、３つの子ノードを持つノードがあることと、それに対応して制御処理が拡張されていることとが第１実施形態のものと相違している。
そこで、以降、その相違点を中心に第２実施形態のロボット１の制御処理について説明する。なお、第２実施形態の説明で用いる参照符号および用語は、第１実施形態と同等の意味を持つものは、第１実施形態と同じ参照符号および用語を用い、詳細な説明を省略する。
図２４は、第２実施形態における階層構造を説明するための図であり、第１実施形態における図３（ｂ）に対応する図である。図２４に示す如く、第２実施形態では、６個の接地部位１０のそれぞれを葉ノード（第１〜第６ノード）とし、その６個の全ての接地部位１０の組を根ノード（第１４５２３６ノード）としていると共に、第１脚＃１、第４脚＃４、第５脚＃５のそれぞれの接地部位である第１、第４、第５接地部位１０の組を第１４５ノード、第２脚＃２、第３脚＃３、第６脚＃６のそれぞれの接地部位である第２、第３、第６接地部位１０の組を第２３６ノードとしている。つまり、第１４５ノードは第１、第４、第５ノード（３個の葉ノード）を子ノードとして持つ中間ノード、第２３６ノードは第２、第３、第６ノード（３個の葉ノード）を子ノードとして持つ中間ノードである。
第２実施形態では、例えば第１４５ノードの３個の接地部位１０の組と、第２３６ノードの３個の接地部位１０の組とを交互に持ち上げて着地させることにより、ロボット１の移動を行なうようにしている。図２４は、両者の組を着地させた状態（両者の組を支持脚とした状態）を示しており、図中の参照符号Ｑｎ（ｎ＝１，２，３，４，５，６）を付した三角形が各接地部位１０の位置に対応している。
なお、Ｑｎ（ｎ＝１，２，３，４，５，６）は、葉ノードである第１〜第６の各接地部位１０の目標床反力中心点（目標ノード床反力中心点）、Ｑ１４５、Ｑ２３６は、それぞれ第１４５ノード、第２３６ノードの目標床反力中心点（目標ノード床反力中心点）、Ｐは根ノード（第１４５２３６ノード）の目標床反力中心点（目標ノード床反力中心点）Ｑ１４５２３６としての目標全床反力中心点（＝目標ＺＭＰ）を示すものである。第１実施形態と同様に、Ｑｎ（ｎ＝１，２，３，４，５，６）は、それぞれに対応する接地部位１０の中心点に一致している。
第２実施形態における制御装置５０の全体的な機能的構成は、前記第１実施形態に関して説明した図２のものと同じである。
この場合、第２実施形態における歩容生成装置１００は、第１実施形態のものと同様に、ロボット１の目標運動軌道（目標接地部位軌道、目標上体位置姿勢軌道）と目標床反力軌道（目標全床反力中心点軌道、目標全床反力軌道）とを決定して出力する。但し、第２実施形態では、目標接地部位軌道は、６個の接地部位１０のそれぞれの目標接地部位位置の軌道である。なお、各接地部位をその姿勢を制御可能に設けた場合には、目標接地部位姿勢の軌道も目標接地部位軌道に含まれる。また、目標全床反力中心点軌道は、第２実施形態における脚＃１〜＃６の運動形態（特に支持脚の着地予定位置）に合わせて、ＺＭＰの存在可能範囲内で該範囲の境界に近づきすぎない位置（例えばＺＭＰの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。
また、第２実施形態における目標床反力分配器１０２では、第１実施形態と同様に、次の条件Ａ’）〜Ｆ’）を満たすように、目標ノード床反力中心点と各ノードの重みと目標ノード床反力とを決定する。
Ａ’）各葉ノードの目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，５，６）は、該葉ノードに対応する接地部位１０の中心点に一致する。より一般的には、Ｑｎ（ｎ＝１，２，３，４，５，６）は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。なお、例えば各接地部位１０の目標床反力中心点を歩容生成装置１００で決定した場合には、歩容生成装置１００が決定する目標床反力を基に、目標ノード床反力中心点Ｑｎ（ｎ＝１，２，３，４，５，６）を決定すればよい。
Ｂ’）根ノードの目標床ノード反力中心点は、目標全床反力中心点Ｐに一致する。
Ｃ’）子ノードを持つ任意の第ｎノード（ｎ＝１４５，２３６，１４５２３６）の目標ノード床反力中心点Ｑｎは、その第ｎノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。より具体的には、第１４５ノードの目標ノード床反力中心点Ｑ１４５は、その子ノード（葉ノード）である第１、４、５ノードの目標ノード床反力中心点Ｑ１，Ｑ４，Ｑ５を頂点とする三角形の内分点（三角形の境界の点を含む）となり、第２３６ノードの目標ノード床反力中心点Ｑ２３６は、その子ノード（葉ノード）である第２、３、６ノードの目標ノード床反力中心点Ｑ２，Ｑ３，Ｑ６を頂点とする三角形の内分点（三角形の境界の点を含む）となる。また、第１４５２３６ノード（根ノード）の目標ノード床反力中心点Ｑ１４５２３６（＝目標全床反力中心点Ｐ）は、その子ノード（中間ノード）である第１４５、２３６ノードの目標ノード床反力中心点Ｑ１４５，Ｑ２３６を結ぶ線分の内分点となる。
Ｄ’）子ノードを持つ任意の第ｎノード（ｎ＝１４５，２３６，１４５２３６）の目標ノード床反力Ｆｎは、該第ｎノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノード（第１４５２３６ノード）の目標ノード床反力Ｆ１４５２３６は目標全床反力Ｆｔｏｔａｌｒｅｆに一致する。あるいは、各ノードの目標ノード床反力Ｆｎ（ｎ＝１，２，３，４，５，６，１４５，２３６，１４５２３６）は、各ノードの重みと前記式４ａまたは４ｂの関係を有する。
Ｅ’）接地していない接地部位１０に対応する葉ノードの目標ノード床反力は０となる。
Ｆ’）各ノード（第ｎノード（ｎ＝１，２，３，４，１４５，２３６，１４５２３６））の目標ノード床反力中心点、重み、目標ノード床反力は連続的に変化する。
Ｇ’）非接地状態の接地部位１０に対応する葉ノードの重み、または、その葉ノードの祖先ノードのいずれか１つの重みは０とされる。
なお、根ノードの重みは、前記第１実施形態と同様、便宜的に「１」に設定した。
第２実施形態における姿勢偏差演算部１０３および姿勢安定化制御演算部１０４の処理は、第１実施形態と同じであり、補償全床反力モーメントＭｄｍｄ（Ｍｄｍｄｘ，Ｍｄｍｄｙ）が第１実施形態で説明した如く決定される。
また、第２実施形態における実床反力検出器１０８は、第１実施形態のものと同様に、各接地部位１０（本実施形態では６個の接地部位）に作用する実床反力を各脚＃１〜＃６に備えた６軸力センサ３４の出力から検出し、それを支持脚座標系（床に固定された図１に示すグローバル座標系）で表現してなる実床反力に変換する。
また、第２実施形態におけるロボット幾何学モデル（逆キネマティクス演算部）１１０は、第１実施形態のものと同様に、各接地部位位置の最終的な目標軌道と目標上体位置姿勢などを基に、逆キネマティクスの演算によって、それらを満足するロボット１の各関節変位指令を算出する。
また、第２実施形態における変位コントローラ１１２は、第１実施形態のものと同様に、前記各関節変位指令に実関節変位を追従させるようにロボット１の各関節１４，１５のアクチュエータ（図示しない）を制御する。
また、第２実施形態における階層型コンプライアンス動作決定部１１４は、第１実施形態のものと同様に前記要求１）、２）をできるだけ満足するように各接地部位１０の目標接地部位軌道を修正する。但し、本実施形態では、３つの子ノードを持つ中間ノードを有するため、階層型コンプライアンス動作決定部１１４の具体的な処理は、第１実施形態のものより多少複雑になる。なお、各接地部位１０の姿勢を制御可能にロボットを構成した場合には、階層型コンプライアンス動作決定部１１４は、前記要求３）もできるだけ満足するように各接地部位１０の目標接地部位軌道を修正すべきである。
図２５は、第２実施形態の階層型コンプライアンス動作決定部１１４の機能を示すブロック図であり、第１実施形態における図１７に対応している。同図２５を参照して、第２実施形態の階層型コンプライアンス動作決定部１１４も、第１実施形態と同様に、補償全床反力モーメント分配器１１４ａ、補償角決定部１１４ｂ，１１４ｃ，１１４ｄと、修正目標接地部位位置姿勢算出部１１４ｇと、機構変形補償量算出部１１４ｎと、機構変形補償入り修正目標接地部位位置姿勢決定部１１４ｈとを構成要素（機能的手段）として備える。なお、第２実施形態では、補償角は、第１４５２３６ノード、第１４５ノード、第２３６ノードにそれぞれ係わる第１４５２３６補償角θ１４５２３６、第１４５補償角θ１４５および第２３６補償角θ２３６があり、補償角決定部１１４ｂ，１１４ｃ，１１４ｄは、それぞれθ１４５２３６、θ１４５、θ２３６を決定するものである。
以下に、第２実施形態における階層型コンプライアンス動作決定部１１４の処理を、第１実施形態のものと異なる点を中心に説明する。
第２実施形態における階層型コンプライアンス動作決定部１１４は、第１実施形態のものと同様に、目標床反力分配器１０２の出力などを基に、各ノードの目標ノード床反力の並進力成分およびモーメント成分を決定すると共に、実床反力検出器１０８の出力を基に、各ノードの実ノード床反力の並進力成分およびモーメント成分を決定する。
すなわち、図２６を参照して、各葉ノードの目標ノード床反力の並進力成分Ｆｎ＿ｒｅｆ（ｎ＝１，２，３，４，５，６）は、目標床反力分配器１０２で決定された目標ノード床反力の並進力成分と同一とされ、根ノードの目標ノード床反力の並進力成分Ｆ１４５２３６ｒｅｆは、歩容生成装置１００で決定された目標全床反力の並進力成分Ｆｔｏｔａｌｒｅｆと同一とされる。また、葉ノードおよび根ノード以外の中間ノードの目標ノード床反力の並進力成分Ｆｎ＿ｒｅｆ（ｎ＝１４５，２３６）は、その中間ノードの各子ノードの目標ノード床反力の合力の並進力成分に決定される（Ｆ１４５ｒｅｆ＝Ｆ１ｒｅｆ＋Ｆ４ｒｅｆ＋Ｆ５ｒｅｆ、Ｆ２３６ｒｅｆ＝Ｆ２ｒｅｆ＋Ｆ３ｒｅｆ＋Ｆ６ｒｅｆ）。この場合、中間ノードの親ノードは根ノードであるので、Ｆ１４５ｒｅｆ＋Ｆ２３６ｒｅｆ＝Ｆｔｏｔａｌｒｅｆである。なお、図２６は、第２実施形態のロボット１の全ての接地部位１０が接地している状態での各ノードの目標ノード床反力の並進力成分Ｆｎ＿ｒｅｆを例示している。
また、各ノードの目標ノード床反力のモーメント成分（詳しくは該ノードの目標ノード床反力中心点を作用点とする目標ノード床反力のモーメント成分）は、いずれも０とされる。
また、図３０を参照して、各葉ノードの実ノード床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１，２，３，４，５，６）は、実床反力検出器１０８で検出された各接地部位１０の実床反力の並進力成分と同一とされる。そして、子ノードをもつ各ノードの実ノード床反力の並進力成分Ｆｎ＿ａｃｔ（ｎ＝１４５，２３６，１４５２３６）は、そのノードの各子ノードの実ノード床反力の合力の並進力成分に決定される。従って、Ｆ１４５ａｃｔ＝Ｆ１ａｃｔ＋Ｆ４ａｃｔ＋Ｆ５ａｃｔ、Ｆ２３６ａｃｔ＝Ｆ２ａｃｔ＋Ｆ３ａｃｔ＋Ｆ６ａｃｔ、Ｆ１４５２３６ａｃｔ（＝Ｆｔｏｔａｌａｃｔ）＝Ｆ１４５ａｃｔ＋Ｆ２３６ａｃｔである。なお、図３０は、第２実施形態のロボット１の全ての接地部位１０が接地している状態での各ノードの実ノード床反力の並進力成分Ｆｎ＿ａｃｔを例示している。
また、図３１を参照して、各葉ノードの実ノード床反力のモーメント成分（該葉ノードの目標ノード床反力中心点を作用点とするモーメント成分）Ｍｎ＿ａｃｔ（ｎ＝１，２，３，４，５，６）は、基本的には、実床反力検出器１０８で検出された各接地部位１０の実床反力のモーメント成分と同一とされる。ただし、本実施形態では、各接地部位１０の中心点、すなわち該接地部位１０の目標床反力中心点に実モーメントは発生しないので、各葉ノードの実ノード床反力のモーメント成分は０に設定される。そして、子ノードをもつ各第ｎノード（ｎ＝１４５，２３６，１４５２３６）の実ノード床反力のモーメント成分Ｍｎ＿ａｃｔは、基本的にはその第ｎノードの各子ノードの実ノード床反力の合力が、該第ｎノードの目標ノード床反力中心点に作用するモーメント成分（これは一般に０にならない）に決定される。
第２実施形態における前記補償全床反力モーメント分配器１１４ａは、前記補償全床反力モーメントＭｄｍｄを前記補償全床反力モーメントＭｄｍｄ（Ｍｄｍｄｘ，Ｍｄｍｄｙ）を、第１４５２３６ノード補償床反力モーメントＭ１４５２３６ｄｍｄ、第１４５ノード補償床反力モーメントＭ１４５ｄｍｄおよび第２３６ノード補償床反力モーメントＭ２３６ｄｍｄに分配するものである。
第１４５２３６ノード補償床反力モーメントＭ１４２３ｄｍｄは、第１４５２３６補償角θ１４５２３６を操作することによって（第１、第４、第５接地部位の組と、第２、第３、第５接地部位の組とを目標全床反力中心点Ｐ（＝Ｑ１４５２３６）まわりにθ１４５２３６だけ回転させることによって）発生する各接地部位１０の床反力の並進力成分が、目標目標全床反力中心点Ｐ（目標ＺＭＰ）まわりに発生すべきモーメントの目標値である。
第１４５ノード補償床反力モーメントＭ１４５ｄｍｄは、第１４５補償角θ１４５を操作することによって（第１４５ノードに属する第１、第４および第５接地部位１０を目標第１４５ノード床反力中心点Ｑ１４５まわりにθ１４５だけ回転させることによって）発生する各接地部位１０（詳しくは第１、第４、および第５接地部位）の床反力の並進力成分が、目標第１４５ノード床反力中心点まわりに発生すべきモーメントの目標値である。
第２３６ノード補償床反力モーメントＭ２３６ｄｍｄは、第２３６補償角θ２３６を操作することによって（第２３６ノードに属する第２、第３および第６接地部位１０を目標第２３６ノード床反力中心点Ｑ２３６まわりにθ２３６だけ回転させることによって）発生する各接地部位１０（詳しくは第２、第３、および第６接地部位）の床反力の並進力成分が、目標第２３６ノード床反力中心点まわりに発生すべきモーメントの目標値である。
なお、本実施形態では、第１実施形態と同様、ロボット１の鉛直軸まわりの姿勢制御を行なわないので（補償全床反力モーメントＭｄｍｄの鉛直軸回りの成分が０であるので）、Ｍ１４５２３６ｄｍｄ，Ｍ１４５ｄｍｄ、Ｍ２３６ｄｍｄは、いずれも鉛直軸回りの成分が０であるモーメント（水平ベクトル）であり、補償角θ１４５２３６、θ１４５、θ２３６は、水平な軸回りの回転角である。また、特に、Ｍ１４５２３６ｄｍｄは、線分Ｑ１４５Ｑ２３６に垂直な水平軸回りのモーメントである。
これらのノード補償床反力モーメントＭ１４５２３６ｄｍｄ，Ｍ１４５ｄｍｄ，Ｍ２３６ｄｍｄは、基本的には、次の条件１１）、１２）を満たすように決定される。
１１）第１４５ノード、第２３６ノード、第１４５２３６ノードに対して、前記第１実施形態で定義した如く、次式７ａ〜９ａの関係式を満たす修正目標ノード床反力中心点Ｑ１４５ｍｄｆｄ、Ｑ２３６ｍｄｆｄ、Ｐｍｄｆｄ（＝Ｑ１４５２３６ｍｄｆｄ）を導入したとき、これらのＱ１４５ｍｄｆｄ、Ｑ２３６ｍｄｆｄ、Ｐｍｄｆｄは、それぞれの所定の存在許容範囲内に在る。

Ｑ１４５ｍｄｆｄ、Ｑ２３６ｍｄｆｄ、Ｐｍｄｆｄの存在可能範囲は、例えば第２実施形態のロボット１の全ての接地部位１０が接地している状態では、図２９（ａ）に示す如く設定される。すなわち、Ｑ１４５ｍｄｆｄの存在許容範囲は、図中の太線の三角形上の領域（その三角形の辺および内部の領域）であり、これは、第１４５ノードの子ノードの目標ノード床反力中心点Ｑ１，Ｑ４，Ｑ５を頂点する三角形の内部で、該三角形Ｑ１Ｑ４Ｑ４の境界に近づき過ぎないように設定された領域である。Ｑ２３６ｍｄｆｄの存在許容範囲も同様である。また、Ｐｍｄｆｄの存在許容範囲は、図中の太線の線分上の領域であり、これは、第１４５２３６ノード（根ノード）の子ノードの目標床反力中心点Ｑ１４５，Ｑ２３６を結ぶ線分Ｑ１４５Ｑ２３６上で、該線分Ｑ１４５Ｑ２３６の端点に近づきすぎないように設定された領域である。
１２）Ｍｄｍｄ＝Ｍ１４５２３６ｄｍｄ＋Ｍ１４５ｄｍｄ＋Ｍ２３６ｄｍｄをほぼ満足する。
また、本実施形態では、根ノード（第１４５２３６ノード）の子ノードが２つであることから、Ｍ１４５２３６ｄｍｄは、前記第１実施形態における各ノード補償床反力モーメントと同様に、線分Ｑ１４５Ｑ２３６に直交する水平な単位ベクトル（これをＶ１４５２３６で表す）と同方向のベクトルに制限される。そこで、本実施形態では、次の条件１３）を満たすようにＭ１４５２３６ｄｍｄ，Ｍ１４５ｄｍｄ，Ｍ２３６ｄｍｄを決定する。
１３）Ｍ１４５２３６ｄｍｄ＋Ｍ１４５ｄｍｄ＋Ｍ２３６ｄｍｄの、ベクトルＶ１４５２３６方向の成分は、可能な限り、ＭｄｍｄのベクトルＶ１４５２３６方向の成分に近い値とする。
これらの条件１１）〜１３）を満たすＭ１４５２３６ｄｍｄ、Ｍ１４５ｄｍｄ、Ｍ２３６ｄｍｄは、例えば次のように決定される。まず、Ｍ１４５２３６ｄｍｄをＭｄｍｄのＶ１４５２３６方向の成分に決定する。ただし、前記式７ａにより定まる修正目標ノード床反力中心点Ｐｍｄｆｄがその存在許容範囲に収まらない場合には、Ｐｍｄｆｄがその存在許容範囲の境界の点になるように、Ｍ１４５２３６ｄｍｄを修正する。次いで、Ｍｄｍｄから上記の如く決定したＭ１４５２３６ｄｍｄを差し引いたベクトルが、Ｍｄｍｄ１４５とＭｄｍｄ２３６との和にほぼ一致し、且つ、Ｍｄｍｄ１４５＋Ｍｄｍｄ２３のＶ１４５２３６方向の成分がＭｄｍｄ−Ｍｄｍｄ１４５２３６のＶ１４５２３６成分にできるだけ近いものとなり、且つ前記条件１１）を満たすように、Ｍｄｍｄ１４５，Ｍｄｍｄ２３６を決定する。なお、この場合、Ｍｄｍｄ１４５，Ｍｄｍｄ２３６は、互いに平行なベクトルとする。
このように決定されたＭ１４５２３６ｄｍｄ、Ｍ１４５ｄｍｄ、Ｍ２３６ｄｍｄの例を図２９（ｂ）に示す。補足すると、Ｍ１４５２３６ｄｍｄは、前記したように線分Ｑ１４５Ｑ２３６に垂直な水平ベクトルである。
なお、ロボット１の鉛直軸回りの姿勢も制御するような場合には、Ｍ１４５２３６ｄｍｄ、Ｍ１４５ｄｍｄ、Ｍ２３６ｄｍｄの鉛直成分も決定するようにしてもよい。
第２実施形態における各補償角決定部１１４ｂ〜１１４ｄは、基本的には、それぞれ第ｎノード（ｎ＝１４５２３６，１４５，２３６）の、ノード補償床反力モーメントと、実ノード床反力モーメント（目標ノード床反力中心点を作用点とするモーメント）との偏差に応じて、その偏差を０に近づけるようにノード補償角θ１４５２３６、θ１４５、θ２３６を決定する。図３０および図３１にその例を示す。この場合、本実施形態では、θ１４５は、図３０に示す如く、偏差Ｍ１４５ａｃｔ−Ｍ１４５ｄｍｄと同方向で、目標第１４５ノード床反力中心点Ｑ１４５を通る軸回りの回転角、θ２３６は、図３０に示す如く、偏差Ｍ２３６ａｃｔ−Ｍ２３６ｄｍｄと同方向で、目標第２３６ノード床反力中心点Ｑ２３６を通る軸回りの回転角である。また、θ１４５２３６は、図３１に示す如く、偏差Ｍ１４５２３６ａｃｔ−Ｍ１４５２３６ｄｍｄと同方向（線分Ｑ１４５Ｑ２３６に垂直な水平方向）で、目標全床反力中心点Ｐを通る軸回りの回転角である。
但し、本実施形態においても、第１実施形態と同様に、第ｎノード（ｎ＝１４５２３６，１４５，２３６）の、ノード補償床反力モーメントをフィルタに通したものと、実ノード床反力モーメントをフィルタに通したものとの偏差に応じて、ノード補償角θ１４５２３６、θ１４５、θ２３６を決定するようにした。
図３２は上記の如くノード補償角θ１４５２３６を決定する補償角決定部１１４ｂ（θ１４５２３６決定部）の機能を示すブロック図、図３３は上記の如くノード補償角θ１４５を決定する補償角決定部１１４ｃ（θ１４５決定部）の機能を示すブロック図である。なお、補償角決定部１１４ｄ（θ２３６決定部）の処理は、θ１４５決定部１１４ｂの処理と同様であるので詳細な説明および図示を省略する。
補償角決定部（θ１４５２３６決定部）１１４ｂでは、まず、目標第１４５ノード床反力中心点Ｑ１４５に実第１、第４、第５ノード床反力の合力の並進力成分（Ｆ１ａｃｔ＋Ｆ４ａｃｔ＋Ｆ５ａｃｔ）が作用すると共に、目標第２３６ノード床反力中心点Ｑ２３６に実第２、第３、第６ノード床反力の合力の並進力成分（Ｆ２ａｃｔ＋Ｆ３ａｃｔ＋Ｆ６ａｃｔ）が作用した場合に、目標全床反力中心点Ｐ（目標第１４５２３６ノード床反力中心点）のまわりに発生するモーメントＭ１４５２３６ａｃｔを前記式１６または１７に基づいて求める。そして、その求めたＭ１４５２３６ａｃｔの、ベクトルＶ１４５２３６方向の成分Ｍ１４５２３６ａｃｔｖを内積演算によって求める。ここでベクトルＶ１４５２３６は、線分Ｑ１４５Ｑ２３６に垂直で且つ水平な単位ベクトルである。なお、第１４５２３６ノードに関しては、Ｖ１４５２３６に垂直で且つ水平な単位ベクトルＵ１４５２３６の方向の軸回りに子ノード（第１４５ノードおよび第２３６ノード）の目標床反力中心点の位置を操作できないので、Ｍ１４５２３６ａｃｔのベクトルＵ１４５２３６方向の成分は求める必要はない。
次いで、前記第１実施形態と同様に、このＭ１４５２３６ａｃｔｖをローパスフィルタに通したものＭ１４５２３６ａｃｔｖｆｉｌｔと、前記第１４５２３６ノード補償床反力モーメントＭ１４５２３６ｄｍｄのベクトルＶ方向の成分Ｍ１４５２３６ｄｍｄｖを補償フィルタに通したものＭ１４５２３６ｄｍｄｖｆｉｌｔとの偏差Ｍ１４５２３６ｅｒｒｖ（＝Ｍ１４５２３６ａｃｔｖｆｉｌｔ−Ｍ１４５２３６ｄｍｄｖｆｉｌｔ）に所定のゲインＫ１４５２３６を乗算することで、第１４５２３６ノード補償角θ１４５２３６が決定される。
補償角決定部（θ１４５決定部）１１４ｃでは、まず、目標第１、第４、第５ノード床反力中心点Ｑ１，Ｑ４，Ｑ５にそれぞれ実第１、第４、第５ノード床反力の並進力成分Ｆ１ａｃｔ，Ｆ４ａｃｔ，Ｆ５ａｃｔが作用した場合に、第１４５ノード床反力中心点Ｑ１４５のまわりに発生するモーメントＭ１４５ａｃｔを算出する。この場合、算出するＭ１４５ａｃｔは、互いに直交する水平な単位ベクトルであるベクトルＶ１４５およびＵ１４５のそれぞれの方向の成分から構成される。ベクトルＶ１４５またはＵ１４５の向きは、任意でよい。
次いで、このＭ１４５ａｃｔをローパスフィルタに通したものＭ１４５ａｃｔｆｉｌｔと、前記第１４５ノード補償床反力モーメントＭ１４５ｄｍｄ（ベクトルＶ方向の成分およびベクトルＵ方向の成分）を補償フィルタに通したものＭ１４５ｄｍｄｆｉｌｔとの偏差Ｍ１４５ｅｒｒ（＝Ｍ１４５ａｃｔｆｉｌｔ−Ｍ１４５ｄｍｄｆｉｌｔ）に所定のゲイン行列Ｋ１４５（対角行列）を乗算することで、第１４５ノード補償角θ１４５が決定される。θ１４５は、ベクトルＶの軸回り角度成分とベクトルＵの軸回りの角度成分とから構成される。
補償角決定部（θ２３６決定部）１１４ｄによる第２３６ノード補償角θ２３６の決定処理も、上記したθ１４５決定部１１４ｃの処理と同様に行なわれる。
以上説明した補償角決定部１１４ｂ〜１１４ｄの処理によって、目標全床反力中心点Ｐに作用する実モーメントＭａｃｔの水平成分を補償全床反力モーメントＭｄｍｄに近づけるようにノード補償角θ１４５２３６，θ１４５，θ２３６の組が決定されることとなる。
なお、第１実施形態に関して補足説明したように、次のようにノード補償角θ１４５２３６，θ１４５，θ２３６を決定するようにしてもよい。すなわち、子ノードを持つ各第ｎノード（ｎ＝１４５，２３６，１４５２３６）に対し、その第ｎノードの実ノード床反力（第ｎノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。あるいは、第ｎノード（ｎ＝１４５，２３６，１４５２３６）の実ノード床反力のモーメントから、その各子ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点求める。そして、第ｎノード（ｎ＝１４５，２３６，１４５２３６）の目標ノード床反力中心点と、上記の如く求めた実第ｎノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィルタに通した値の偏差を制御量とし、その制御量を０に近づけるように、該制御量に応じてノード補償角θ１４５２３６，θ１４５，θ２３６を決定する（例えば制御量にあるゲインを乗算することで、θ１４５２３６，θ１４５，θ２３６を決定する）。
図２５に示した第２実施形態における修正目標接地部位位置姿勢算出部１１４ｇは、次のように、各接地部位１０の目標接地部位位置姿勢（図１のロボットでは実際には目標接地部位位置）を修正し、修正目標接地部位位置姿勢を得る。すなわち、図３０および図３１を参照して、第１４５ノードの子ノードである第１、第４および第５ノードの目標床反力中心点Ｑ１，Ｑ４，Ｑ５をそれぞれ、第１４５ノードの目標床反力中心点Ｑ１４５を回転中心として、第１４５ノード補償角θ１４５（水平ベクトル）だけ回転移動する。この回転移動後のＱ１，Ｑ４，Ｑ５をそれぞれＱ１’，Ｑ４’，Ｑ５’とする。従って、第１４５ノード補償角θ１４５は、第１４５ノードの目標床反力中心点Ｑ１４５の位置を動かさずに、第１４５ノードの子ノードである第１、第４、第５ノードのそれぞれの目標床反力中心点Ｑ１，Ｑ４，Ｑ４の位置の相対関係を動かすための操作量である。
同様に、第２３６ノードの子ノードである第２、第３および第６ノードの目標床反力中心点Ｑ２，Ｑ３，Ｑ６をそれぞれ、第２３６ノードの目標床反力中心点Ｑ２３６を回転中心として、第２３６ノード補償角θ２３６（水平ベクトル）だけ回転移動する。この回転移動後のＱ２，Ｑ３，Ｑ６をそれぞれＱ２’，Ｑ３’，Ｑ６’とする。従って、第２３６ノード補償角θ２３６は、第２３６ノードの目標床反力中心点Ｑ２３６の位置を動かさずに、第２３６ノードの子ノードである第２、第３、第６ノードのそれぞれの目標床反力中心点Ｑ２，Ｑ３，Ｑ６の位置の相対関係を動かすための操作量である。
これらの回転移動を視覚的に示したものが図３０である。
次いで、第１４５２３６ノードの子ノードである第１４５および第２３６ノードの目標床反力中心点Ｑ１４５，Ｑ２３６をそれぞれ、第１４５２３６ノードの目標床反力中心点Ｐ（目標全床反力中心点）を回転中心として、前記偏差Ｍ１４５２３６ｅｒｒｖのベクトル（水平ベクトル）と同方向（線分Ｑ１４５Ｑ２３６と直交する水平方向）の軸心まわりに第１４５２３６ノード補償角θ１４５２３６だけ回転移動する。この回転移動後のＱ１４５，Ｑ２３６を図３１に示す如くそれぞれＱ１４５’，Ｑ２３６’とする。従って、第１４５２３６ノード補償角θ１４５２３６は、第１４５２３６ノード（根ノード）の目標床反力中心点Ｐの位置を動かさずに、第１４５２３６ノードの子ノードである第１４５、第２３６ノードのそれぞれの目標床反力中心点Ｑ１４５，Ｑ２３６の位置の相対関係を動かすための操作量である。
次いで、図３１を参照して、先の回転移動後の目標ノード床反力中心点Ｑ１’，Ｑ４’，Ｑ５’をベクトルＱ１４５＿Ｑ１４５’だけ平行移動し、これにより、第１、第４および第５ノードの最終的な修正後の目標ノード床反力中心点Ｑ１”，Ｑ４”，Ｑ５”を得る。同様に、先の回転移動後の目標ノード床反力中心点Ｑ２’，Ｑ３’，Ｑ６’をベクトルＱ２３６＿Ｑ２３６’だけ平行移動し、これにより、第２、第３および第６ノードの最終的な修正後の目標ノード床反力中心点Ｑ２”，Ｑ３”，Ｑ６”を得る。
最後に、第ｎ接地部位（ｎ＝１，２，３，４，５，６）の目標接地部位位置を、ベクトルＱｎ＿Ｑｎ”だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位１０の目標接地部位位置（より正確には各接地部位１０の目標接地部位位置の相対関係）が修正されることとなる。すなわち、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位１０の目標接地部位位置の間の相対関係（相対的位置関係）の操作量（修正量）が各補償角θ１４５，θ２３６，θ１４５２３６に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位１０の目標接地部位位置の相互の相対関係が修正されることとなる。
なお、接地部位の姿勢を制御可能として、接地部位の目標床反力中心点まわりに床反力モーメントを発生させることができるロボットでは、各接地部位の目標接地部位床反力中心点（目標ノード床反力中心点）まわりの足平姿勢回転動作を、特開平１０−２７７９６９号公報（複合コンプライアンス制御）に示した手法で行なえば良い。すなわち、第１実施形態に関して補足説明した如く、第ｎ接地部位を上記の平行移動後のＱｎ”を中心として、該第ｎ接地部位の目標姿勢を修正するようにすればよい。
図２５に示した第２実施形態における機構変形補償量算出部１１４ｎは、前記第１実施形態のものと同様に、各各脚＃１〜＃６のコンプライアンス機構４２などの変形による影響を補償するための機構変形補償量Ｅｎ＿ｃｍｐｎ（ｎ＝１，２，３，４，５，６）を求める。すなわち、第２実施形態における補償全床反力モーメント分配器１１４ａから出力される各接地部位１０の修正目標接地部位床反力Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４，５，６，）（目標接地部位床反力に、各ノード補償床反力モーメントＭ１４５２３６ｄｍｄ、Ｍ１４５ｄｍｄ、Ｍ２３６ｄｍｄを付加することによって修正された目標接地部位床反力）によって発生が予想される各脚＃１〜＃６のコンプライアンス機構４２などの機構変形量Ｅｎ＿ｍｄｆｄ（ｎ＝１，２，３，４，５，６）を機構コンプライアンスモデルを用いて求める。このＥｎ＿ｍｄｆｄに（−１）を乗算したものを機構変形補償量Ｅｎ＿ｃｍｐｎとして求める。
図２５に示した第２実施形態における機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈは、第１実施形態のものと同様に、機構変形補償入り修正目標接地部位位置姿勢は、各接地部位１０の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められ位置姿勢）に、それぞれに対応する機構変形補償量Ｅｎ＿ｃｍｐｎを付加することにより決定される。
以上が、本実施形態（第２実施形態）における階層型コンプライアンス動作決定部１１４の処理の詳細である。
以上説明した以外の制御装置５０の演算処理は、第１実施形態と同じである。
なお、第２実施形態における階層構造は、第１実施形態の場合と同様に、ロボット１の動作形態（移動形態）などに応じて変更しても良い。例えば、図３４に示す如く、階層構造を設定してもよい。この図示の例では、第１接地部位および第２接地部位を子ノード（葉ノード）としてもつ第１２ノードと、第３接地部位および第４接地部位を子ノード（葉ノード）としてもつ第３４ノードと、第５接地部位および第６接地部位を子ノード（葉ノード）としてもつ第５６ノードと、第５６ノードおよび第３４ノードを子ノードとしてもつ第３４５６ノードとを中間ノードとして備え、根ノードは、第３４５６ノードと、第１２ノードとを子ノードとしてもつものとされている。このようにすることで、コンプライアンス動作および後述の床形状推定をより的確に行なうことが可能となる場合がある。なお、図中の符号の意味は、前記図３（ｂ）や図２４などに示したものと同様である。
［第３実施形態］
次に、第２実施形態のロボット１（６脚ロボット）に床形状推定機能とその推定結果に応じたロボット１の動作補正機能とを付加した第３実施形態について説明する。なお、本実施形態では、主に、第２実施形態で示した６脚ロボットを中心として説明するが、４脚ロボットについても補足的に説明を付加する場合がある。また、本実施形態の理解の便宜上、２脚ロボットについても言及する場合がある。
本実施形態におけるロボット１の機構的構成は、第１または第２実施形態で説明した図１に示したもの（ただし、６個の脚＃１〜＃６を有するロボット１）と同じである。従って、ロボット１の機構的構成の説明は省略する。また、本実施形態におけるロボット１に備えた制御装置５０の機能的構成も、前記図２に示したものと同じである。但し、本実施形態では、図２中の階層型コンプライアンス動作決定部１１４には、新たな機能が付加されており、その点が第２実施形態のものと相違している。そして、階層型コンプライアンス動作決定部１１４以外の制御装置５０の構成要素の処理は、第２実施形態と同一である。そこで、本実施形態での説明では、階層型コンプライアンス動作決定部１１４の処理を中心に説明し、これ以外の制御装置５０の処理については、詳細な説明を省略する。
図３５は、本実施形態における階層型コンプライアンス動作決定部１１４の処理機能を示すブロック図である。このうち、第２実施形態のものと異なる機能を説明すると、本実施形態における階層型コンプライアンス動作決定部１１４には、新たに、床形状推定手段としての床形状推定器１３０と、床形状推定器１３０が出力（推定）する推定床形状偏差（より詳しくは各接地部位１０に係る後述の床高さ偏差の推定値）を修正目標接地部位位置姿勢に加算する加算器１３２とが付加され、加算器１３２の出力を、修正目標接地部位位置姿勢の代わりに機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈに入力するようにしている。これ以外の階層型コンプライアンス動作決定部１１４の構成要素の処理は第２実施形態のものと同一である。
また、これに伴い、本実施形態での制御装置５０のメインルーチンの制御処理は、その一部が前記図９のフローチャートに示した制御処理と相違している。図３６は本実施形態における制御装置５０のメインルーチンの制御処理を示すフローチャートである。図示の如く、本実施形態では、Ｓ３６の後に、新たにＳ３７で床形状偏差を推定する処理（床形状推定器１３０の処理）が追加される。さらに、Ｓ３８’では、前記第２実施形態で説明した各補償角θ１４５２３６，θ１４５，θ２３６とＳ３７で推定された床形状偏差とに応じて各目標接地部位位置姿勢が修正され、その修正後の目標接地部位位置姿勢をさらに機構変形補償量に応じて修正することにより、最終的な目標接地部位位置姿勢たる機構変形補償入り修正目標接地部位位置姿勢が得られる。この場合、より詳しくは、各補償角θ１４５２３６，θ１４５，θ２３６に応じて第２実施形態と同様に修正目標接地部位位置姿勢を求めた後、その修正目標接地部位位置姿勢が床形状偏差に応じて修正され、さらに、これが機構変形補償量に応じて修正されることで、機構変形補償入り修正目標接地部位位置姿勢が得られる。上記した事項以外は、図９の処理と同じである。
以降、本実施形態の第２実施形態と相違する点を具体的に説明する。
まず、本実施形態の詳細な説明に入る前に、床形状推定器１３０が推定する上で使用する概念と用語を以下のように定義する。なお、ここでの説明は、便宜上、本実施形態の６脚ロボット１に限らずに、一般的なロボットの簡略的な図を使用して行なう。
図３７、図３８および図３９に示すように、目標歩容において想定された床（あるいは床面）を「想定床」と呼ぶ。ロボットが移動する実際の床を「実床」と呼ぶ。なお、説明の便宜上、図３７では第１実施形態で説明した４脚ロボットを図示し、図３８および図３９では、２脚ロボットを図示しているが、以下に説明する用語の意味は、本実施形態における６脚ロボット１を含む任意の多脚ロボットにおいても同様である。
前記第１および第２実施形態で説明した階層型コンプライアンス制御において定義された目標第ｎ接地部位床反力中心点Ｑｎは、第ｎ接地部位の中心点に設定された点であったが、その床反力中心点Ｑｎは、該第な接地部位の接地面（底面）に設定されていてもよい。この場合、目標歩容において、目標第ｎ接地部位床反力中心点Ｑｎと接することが想定された想定床面上の点を「想定第ｎ床接点Ｄｎ」と呼ぶ。
この定義から明らかなように、ロボットの目標歩容において第ｎ接地部位が接地している時期では、目標第ｎ接地部位床反力中心点Ｑｎと想定第ｎ床接点Ｄｎとは、支持脚座標系（グローバル座標系）から見て同一座標になる。これに対し、実際にロボット１が移動しているときに、実第ｎ接地部位の底面上における目標第ｎ接地部位床反力中心点Ｑｎに対応する点が、実床に接触する点を「実第ｎ床接点Ｄｎａｃｔ」と呼ぶ。
これらの点の関係を表す例を図３７、図３８および図３９に示す。尚、図３７は４脚ロボットを、目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図３８は移動（歩行）している２脚ロボットを目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図３９はほぼ直立姿勢の２脚ロボットを目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ背面から）見た図である。
これらの図３７〜図３９には、前記垂直面における想定床の断面が細線で示され、前記垂直面における実床の断面が太線で示されている。なお、図３７では、ロボットの目標姿勢（目標歩容の瞬時値におけるロボットの全体的な姿勢）と、実姿勢とがそれぞれ破線、実線で示されている。また、図３８および図３９では、ロボットの目標姿勢（目標歩容の瞬時値におけるロボットの全体的な姿勢）と、実接地部位位置姿勢とがそれぞれ細線、太線で示されている。これらの状況における実第ｎ床接点は、実床面上の点であり、図３７、図３８および図３９に示す位置になる。
想定床面に対する実床面の形状偏差を床形状偏差と呼ぶ。床形状偏差を定量的に表現する指標として、第ｎノード床高さ偏差、第ｎノード床傾斜偏差を以下のように定義する。
第ｎ床接点における床面の高さを「第ｎ接地部位床高さ」と呼ぶ。葉である第ｎノードに対して、実第ｎ接地部位床高さと想定第ｎ接地部位床高さとの差を「第ｎ接地部位床高さ偏差」あるいは、「第ｎノード床高さ偏差」と呼ぶ。第ｎ床接点における床面の傾斜を「第ｎ接地部位床傾斜」と呼ぶ。葉である第ｎノードに対して、実第ｎ接地部位床傾斜と想定第ｎ接地部位床傾斜との差を「第ｎ接地部位床傾斜偏差」あるいは、「第ｎノード床傾斜偏差」と呼ぶ。接地部位床傾斜偏差の例を図３９に示す。
葉ノードである、あらゆる第ｊノードに対して、目標第ｊ接地部位位置姿勢と想定第ｊ床面の高さと傾斜（詳しくは、想定第ｊ床接点における想定床面の高さと傾斜）の関係と、ノード補償角の組を用いたコンプライアンス動作によって修正された修正目標第ｊ接地部位位置姿勢と実第ｊ床面の高さと傾斜（詳しくは、実第ｊ床接点における実床面の高さと傾斜）の関係とが一致するためのノード補償角の組を「ノード床傾斜偏差（の組）」と呼び、このうちの第ｎノード補償角に対応するノード床傾斜偏差の成分を「第ｎノード床傾斜偏差」と呼ぶ。このように定義した「第ｎノード床傾斜偏差」は、第ｎノードが葉ノードである場合、先に上記で葉ノードに関して定義した「第ｎノード床傾斜偏差」（＝実第ｎ接地部位床傾斜と想定第ｎ接地部位床傾斜との差）に一致するものである。
結局、ノード床傾斜偏差の組は、目標歩容通りに移動しているロボットの全ての接地部位を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な補償量に相当する。
従って、歩行中に床形状偏差を推定し、推定した床形状偏差を目標接地部位位置姿勢に加えれば、床形状偏差があっても、各ノードの実床反力モーメントは想定床を歩行しているときと同一になる。当然、葉ノードの実床反力モーメントである接地部位の実床反力モーメントも目標接地部位床反力モーメントに一致する。
通常は、上記のごとく定義される「第ｎノード床傾斜偏差」を用いて床形状を表現して構わないが、あるノードが３つの子ノードを持ち、３つの子ノードの目標床反力中心点が同一直線上に並ぶ場合や、あるノードが４つ以上の子ノードを持つ場合などでは、床形状を表現することが困難になる。
そこで、以降の説明においては、葉でない第ｎノードに対しては、より一般的に適用できる以下の表現を用いることとする。以下の定義における所定の重み付き平均を求めるために用いる重みは、前記目標床反力分配器１０２が前記した如く決定した前述の重みと同一のものとする。
定義：
すべての目標接地部位の高さと傾斜を、それぞれ対応する実床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点の高さ（鉛直方向位置）から第ｎノードの親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「実第ｎノード相対床高さ」と呼ぶ。
定義：
すべての目標接地部位の高さと傾斜を、それぞれ対応する想定床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点の高さ（鉛直方向位置）から第ｎノードの親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「想定第ｎノード相対床高さ」と呼ぶ。
実第ｎノード相対床高さから想定第ｎノード相対床高さを減じた高さを「第ｎノード相対床高さ偏差」と呼ぶ。第ｎノード相対床高さ偏差は、以下のように定義しても、同一値となる。
定義：
すべての目標接地部位の高さと姿勢を、それぞれ対応する接地部位床高さ偏差と接地部位床傾斜偏差に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点高さから第ｎノードの親ノードの目標床反力中心点高さを減じた高さを「第ｎノード相対床高さ偏差」と呼ぶ。
上記した第ｎノード相対床高さ偏差の定義から明らかなように、子ノードを持つ各ノードに対し、その全ての子ノードの第ｎノード相対床高さ偏差は、床高さ偏差の相対関係を表すものとなる。また、子ノードを持つ各ノードに対し、その全ての子ノードの重み付き平均値は０となる。
本実施形態（第３実施形態）では、実床面と目標歩容上での想定床面とのずれ（すなわち床形状偏差）を補償して、接地すべき接地部位１０を適正に実床面に接地させるように各接地部位１０の目標位置姿勢の修正を行なう。そして、本実施形態では、上記のように定義される第ｎノード相対床高さ偏差を床形状偏差を表す床形状パラメータとして用い、これを基に、各第ｎ接地部位床高さ偏差（床形状偏差）を推定する。そして、推定した各第ｎ接地部位床高さ偏差（以下、単に第ｎ床高さ偏差ということがある）に応じて第ｎ接地部位１０の目標位置を修正するようにした。
以上を前提として、以下に本実施形態の要部の詳細を説明する。
図３５に示した階層型コンプライアンス動作決定部１１４の床形状推定器１３０への入力は、一般的には大きく分けて以下のようになる。
１）各目標床反力中心点（目標全床反力中心点Ｐ、目標第ｎ接地部位床反力中心点Ｑｎ（ｎ＝１，２，３，４，５，６））。
２）想定床面形状（想定第ｎ床接点の座標、想定第ｎ接地部位床傾斜）。
３）最終的に実ロボットが追従すべき目標姿勢（機構変形入り修正目標接地部位位置姿勢）または実関節変位またはこれらの周波数重み付き平均（周波数特性を持つ重みによる重み付き平均）。
４）上体姿勢傾斜偏差。
５）実床反力（実第ｎ接地部位床反力の並進力成分とモーメント成分）。
なお、本実施形態におけるロボット１では、各接地部位１０の姿勢を制御できないので、想定床面形状のうちの、想定第ｎ接地部位傾斜は不要である。
また、本実施形態におけるロボット１では、実床反力のうちの実第ｎ接地部位床反力のモーメント成分は０になるので、これも床形状推定器１３０に入力する必要は無い。ただし、以降の本実施形態の説明では、各接地部位１０の姿勢を制御できる場合も考慮し、第ｎ接地部位床反力には、一般に、実第ｎ接地部位床反力のモーメント成分も含まれるものとする。また、以降の説明では、接地部位の個数が４個や６個以外の場合にも容易に本実施形態を拡張的に適用できるように、接地部位の総数をしばしば「最終葉ノード番号」と表す。例えば図１の６脚ロボット１では、最終葉ノード番号は、「６」である。
図４０は、床形状推定器１３０の処理機能を示すブロック図である。図４０に示す床形状推定器１３０を構成する各機能要素について説明すると、床形状推定器１３０は機構コンプライアンスモデル１３４を備える。
機構コンプライアンスモデル１３４は、各接地部位１０が実床反力Ｆｎ＿ａｃｔ（ｎ＝１，２，…，最終葉ノード番号）を受けたときの各脚＃１〜＃６のコンプライアンス機構４２など（各脚のコンプライアンス機構４２およびリンク機構）の変形量を求め、求めた変形量を、前記ロボット幾何学モデル１１０（図２参照）への入力である各機構変形補償入り修正目標接地部位位置姿勢に加えることにより、機構変形後の第ｎ接地部位（各接地部位）の位置姿勢の推定値（より詳しくは、実上体姿勢が目標上体姿勢に一致していると仮定した場合の第ｎ接地部位位置姿勢の推定値）である推定第ｎ接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）を求める。なお、このときに用いる各機構変形補償入り修正目標接地部位位置姿勢は、前回制御周期で求めた値などの過去値である。
機構変形後の各推定接地部位位置姿勢を求める際に、ロボット１の関節変位の制御の追従遅れが無視できない場合には、機構変形補償入り修正目標接地部位位置姿勢をそのまま用いる代わりに、機構変形補償入り修正目標接地部位位置姿勢を、追従遅れに相当するローパスフィルタに通してなる位置姿勢を用いれば良い。
あるいは、ロボット１の実関節変位の検出値からロボット幾何学モデル（図２のロボット幾何学モデル１１０と同様のモデル）を介して機構変形が無い場合の実接地部位位置姿勢である機構変形なし実接地部位位置姿勢を求め、求めた値を機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。
あるいは、周波数重み（周波数特性を持つ重み）を用い、前記機構変形なし実接地部位位置姿勢と機構変形補償入り修正目標接地部位位置姿勢の重み付き平均を求め、それを機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。
厳密には、前記した機構変形なし実接地部位位置姿勢を用いるべきであるが、関節変位制御の能力が高ければ、上記のいずれを用いても大差はない。
なお、機構コンプライアンスモデル１３４については、本出願人が先に提案した特開平１０−２７７９６９号公報において詳細が説明してあるので、ここではこれ以上の説明を省略する。
補足すると、図２においてロボット幾何学モデル１１０への入力に機構変形補償が含まれない場合、すなわち、機構変形補償のない修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められる位置姿勢）をロボット幾何学モデル１１０に入力する場合には、その機構変形補償のない修正目標接地部位位置姿勢を床形状推定器１３０の機構コンプライアンスモデル１３４に入力すれば良い。
図４０の説明に戻ると、次いで、機構変形後の推定各接地部位位置姿勢（ｎ＝１，２，…，６）を、目標全床反力中心点Ｐを回転中心として前記上体姿勢傾斜偏差θｂｅｒｒだけ回転移動させることにより、グローバル座標系（支持脚座標系）から見た機構変形後の第ｎ推定接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）を求める。次いで、グローバル座標系から見た機構変形後の第ｎ推定接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）と第ｎ接地部位の目標接地部位位置（第ｎ接地部位の代表点の目標位置）から見た目標第ｎ接地部位床反力中心点の位置とを基に、グローバル座標系から見た機構変形後の目標第ｎ接地部位床反力中心点に対応する点の位置を求める。そして、この求めた点の位置を瞬間推定床接点位置（瞬間推定第ｎ床接点位置）Ｑｎ＿ｅｓｔｍ’とする。このＱｎ＿ｅｓｔｍ’は、実第ｎ床接点の瞬時位置の推定値に相当するものである。尚、瞬間推定床接点位置Ｑｎ＿ｅｓｔｍ’の求め方の詳細は、特開平１０−２７７９６９号公報中の式２１のＱ”を求めることに相当するので、ここではこれ以上の説明を省略する。
次いで、瞬間推定第ｎ床接点位置Ｑ＿ｅｓｔｍ’から想定第ｎ床接点位置Ｄｎを減じたものを、バイアス含有瞬間第ｎ接地部位床高さ偏差Ｚｆｎ＿ｗｉｔｈ＿ｂｉａｓ’として得る。このＺｆｎ＿ｗｉｔｈ＿ｂｉａｓ’は、第ｎ接地部位床高さ偏差の瞬時推定値に相当するものであるが、本実施形態のように上体の実高さを検出しない場合には、一般にバイアス誤差を含んでいる。そのため、Ｚｆｎ＿ｗｉｔｈ＿ｂｉａｓ’をバイアス含有瞬間第ｎ接地部位床高さ偏差と呼ぶ。
ここで、想定第ｎ床接点Ｄｎは、先に定義したように、第ｎ接地部位が接地している時期においては、目標第ｎ接地部位床反力中心点Ｑｎと同一位置とする。目標歩容において第ｎ接地部位が接地する直前の時期では、次に接地する時に想定されている目標第ｎ接地部位床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。目標歩容において第ｎ接地部位が離床した直後の時期では、離床時に想定されていた目標第ｎ接地部位床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。
次いで、バイアス含有瞬間第ｎ接地部位床高さ偏差Ｚｆｎ＿ｗｉｔｈ＿ｂｉａｓ’（ｎ＝１，２，…，最終葉ノード番号）と第ｎ接地部位床反力Ｆｎ＿ａｃｔ（ｎ＝１，２，…，最終葉ノード番号）とを基に、図４１のフローチャートで示す床高さ偏差推定処理サブルーチンによって、推定第ｎノード床高さ偏差Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，…，最終葉ノード番号）を求める。
ここで、床高さ偏差推定処理サブルーチン等に用いられる「階層相対化処理」（または「階層相対化」）について説明する。
階層相対化処理とは、一般的に定義すると、すべての葉ノードに対する入力値（ある所定種類の状態量の値）の組に対して、すべてのノードの出力値を決定する処理である。より詳しくは、階層化相対処理は、葉ノードでない任意のノードのすべての子ノードに対応する出力値の重み付き平均が０であり、かつ、任意の葉ノードの入力値（状態量）が、そのノードの出力値とそのノードのすべての先祖ノードの出力値との和に一致するように各ノード出力値を決定する処理のことである。
以降、葉ノードに対するある種類の入力（状態量）ＡＡから、階層相対化処理によってある種類の出力ＢＢの値を求めることを、「ＡＡを階層相対化してＢＢを求める」と言う。
以下に階層相対化処理のアルゴリズムを説明する。一般的に、階層相対化処理の入力を第ｎ接地部位高さＺｆｎ（ｎは葉ノード番号）、出力を第ｎノード相対高さＺｎ＿ｒｅｌ（ｎ＝１，２，…，最終ノード番号）とする。なお、「最終ノード番号」は、全てのノード番号のうちの最大番号を意味し、例えば第２実施形態で説明した階層構造では、最終ノード番号＝１４５２３６である。また、このアルゴリズムの説明では、「第ｎ接地部位高さ」は、前記第ｎ接地部位床高さ、あるいは第ｎ接地部位床高さ偏差など、階層化相対化処理の入力を総称的に表した名称であり、「第ｎノード相対高さ」は、後述する第ｎ接地部位相対床高さ偏差など、階層相対化処理の出力を総称的に表した名称である。
まず、バイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓを以下のように決定する。すなわち、第ｎノードが葉ノード（すなわち接地部位）であるならば、バイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓに第ｎ接地部位高さＺｆｎの値を代入する。第ｎノードが葉ノード（すなわち接地部位）でないならば、第ｎノードのすべての子ノードのバイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓの重み付き平均を求め、これをもってバイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓとする。ただし、各子ノードに対する重みには、前記目標床反力分配器１０２が決定した重みＷｊ（ｊ＝１，２，…）を用いる。
すなわち、以下の式３２によって、バイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓを得る。

以上のルールに従って、すべてのノードに対してバイアス含有ノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓ（ｎ＝１，２，…，最終ノード番号）を求める。
最後に、第ｎノードの親ノード（これは第ｈノードであるとする）のバイアス含有高さＺｈ＿ｗｉｔｈ＿ｂｉａｓをバイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓから減じることにより、第ｎノード相対高さＺｎ＿ｒｅｌを求める。
すなわち、次式３３により第ｎノード相対高さＺｎ＿ｒｅｌを求める。ただし、第ｈノードは、第ｎノードの親ノードであるとする。

以上のルールに従って、すべてのノードに対してノード相対床高さＺｎ＿ｒｅｌ（ｎは各ノードの番号）を求める。ただし、根ノードに対するのード相対高さＺｋ＿ｒｅｌ（ｋは根ノードの番号）は０とする。図４２に第１実施形態で示した４脚ロボットのノード相対高さの算出例を示し、図４３に本実施形態における６脚ロボットのノード相対高さの算出例を示す。
バイアス含有第ｎノード高さＺｎ＿ｒｅｌ＿ｗｉｔｈ＿ｂｉａｓは、同一の結果が得られる以下の方法によって求めても良い。
第ｎノードが葉ノード（すなわち接地部位）であるならば、バイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓに第ｎ接地部位高さＺｆｎの値を代入する。第ｎノードが葉ノード（すなわち接地部位）でないならば、第ｎノードのすべての子孫の葉ノードのバイアス含有高さの重み付き平均を求め、これをもってバイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓとする。ただし、ここでの各葉ノードｊに対する重みＷｊ’は、そのノードｊに対して前記目標床反力分配器１０２が決定した重みＷｊとそのノードｊの祖先ノードかつ第ｎノードの子孫ノードであるすべてのノードに対して前記目標床反力分配器１０２が決定した重みとの積とする。
すなわち、以下の式３４によって、バイアス含有第ｎノード高さＺｎ＿ｗｉｔｈ＿ｂｉａｓを得る。

補足すると、この場合の重みＷｊ’は、目標第ｊノード床反力が０でなければ、各葉ノードの目標床反力を目標第ｊノード床反力で割った値に一致する。
以上が階層相対化処理である。
次いで、床高さ偏差の推定処理のサブルーチン処理を、それを示す図４１のフローチャート等を用いて以下に説明する。なお、この処理では、階層相対化処理の入力として、第ｎ接地部位床高さ偏差を用い、出力を第ｎノード相対床高さ偏差とする。そして、これらの入力および出力の参照符号として、前記式３２〜３４などに示したものと同様の符号を使用する。
図４１を参照して、まず、Ｓ５０において、前記バイアス含有瞬間第ｎ接地部位床高さ偏差Ｚｆｎ＿ｗｉｔｈ＿ｂｉａｓ’を階層相対化して瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’（ｎ＝１，２，…，最終ノード番号）
を求める。
次いで、Ｓ５２において、制御装置５０の前回制御周期で求めた、第ｎ接地部位床高さ偏差の推定値である推定第ｎ接地部位床高さ偏差（以降、これを前回推定第ｎ接地部位床高さ偏差Ｚｆｎ＿ｅｓｔｍ＿ｐと呼ぶ）を階層相対化して得られる出力を前回推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍ＿ｐ（ｎ＝１，２，…，最終ノード番号）として求める。ただし、この階層相対化処理で使用する各ノードの重みには、制御装置５０の今回（現在）の制御周期で前記目標床反力分配器１０２が決定した重みＷｎ（ｎ＝１，２，…，最終ノード番号）を用いる。
補足すると、一般的に、今回の制御周期で決定された重み（以下、今回重みという）と前回の制御周期で決定された重み（以下、前回重みという）とが異なる場合には、実際の床の形状に変化がなくても、前回重みを用いて求めた各ノードの前回推定相対床高さ偏差の値と今回重みを用いて求めた各ノードの前回推定相対床高さ偏差の値とは異なる値となってしまう。そこで、前回の制御周期で求めた各ノードの推定相対床高さ偏差をそのまま各ノードの前回推定ノード相対床高さ偏差として用いず、上記のごとく、今回重みを用いて各ノードの前回推定ノード相対床高さ偏差を改めて計算するようにした。
次いで、Ｓ５４において、次式３５により、第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄ（ｎ＝１，２，…，最終ノード番号）を求める。このＺｎ＿ｉｎｃ＿ｃａｎｄは、第ｎノード相対床高さ偏差を今回の制御周期で更新するときの該偏差の補正量の仮値を意味するものである。
この場合、第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄは、次式３５に示す如く、瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’と前回推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍ＿ｐの差と所定の係数（ΔＴ／（Ｔｅｓｔｍ＋ΔＴ））との積に設定される。

ここで、式３５中のＴｅｓｔｍは第ｎノード床高さ偏差の推定（更新）における時定数（一次遅れの時定数）であり、ΔＴは制御装置５０の制御周期である。
このように、Ｚｎ＿ｉｎｃ＿ｃａｎｄは設定したとき、制御装置５０の制御周期毎に、Ｚｎ＿ｉｎｃ＿ｃａｎｄをＺｎ＿ｒｅｌ＿ｅｓｔｍ＿ｐに加算していくことで、その加算結果の値は、徐々にＺｎ＿ｒｅｌ’に近づいていくように変化する。
次いで、Ｓ５６において、目標歩容の時期に応じて各ノードのモードの要求値であるノード要求モード（第ｎ要求モードｍｄｎ＿ｄｍｄ）を決定する。
具体的には、前記６脚ロボット１においては、第ｎノード要求モード（ｎ＝１，２，３，４，５，６）、第１４５ノード要求モードｍｄ１４５ｄｍｄ、第２３６ノード要求モードｍｄ２３６ｄｍｄおよび第１４５２３６ノード要求モードｍｄ１４５２３６ｄｍｄを、目標歩容の時間経過に対して図４４のタイミングチャートで示すように設定し、これにしたがって現在の要求モードを決定する。ただし、歩容が異なれば、それに応じてノード要求モードのパターンも変えるべきである。なお、図４４の第１段（最上段）および第２段のタイミングチャートにおけるＯＮ／ＯＦＦは、接地部位が接地している状態がＯＮに対応しており、接地していない状態がＯＦＦに対応している。
補足すると、本願出願人が、特開平１０−２７７９６９号公報に示したように、接地部位として足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、接地部位の床傾斜偏差推定に対応する要求モードも、同公報に説明されているように設定すべきである。
図４４に示すように、各ノードのモード（以下、単にノードモードという）には、準備完了モード、ホールドモード、およびリセットモードがある。
準備完了モードは、床形状の推定を行なうモードである。ホールドモードは、床形状の推定値（推定第ｎ接地部位床高さ偏差）をホールドする（直前の値を維持する）モードである。ホールドモードは、床形状推定値が発散する恐れがある時期と、床形状推定値の精度が低下する恐れがある時期に存在させる。また、リセットモードは、床形状推定値を、次の床形状の推定を開始する（次の準備完了モードの開始）までに所定の初期値に移行させるモードである。
葉ノードである第ｎノード（ｎ＝１，２，３，４，５，６）に対しては、目標歩容上で第ｎ接地部位が接地しているならば（すなわち目標第ｎ接地部位床反力が０でないならば）、第ｎノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第ｎ接地部位が離床した（すなわち目標第ｎ接地部位床反力が０になった）ならば、その直後は第ｎノード要求モードがホールドモードに設定される。そして、その後しばらくしたら第ｎノード要求モードがリセットモードに設定される。さらに、目標歩容上で第ｎ接地部位が接地する直前から、第ｎノード要求モードが準備完了モードに設定される。
また、葉ノードでない第ｎノード（ｎ＝１４５，２３６，１４５２３６）、すなわち、子ノードをもつ第ｎノードに対しては、その第ｎノードに属する少なくとも１つの接地部位が目標歩容上で接地している場合には、第ｎノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第ｎノードに属するすべての接地部位が離床したならば、その直後までは、第ｎノード要求モードがホールドに設定される。なお、第ｎノードに属するすべての接地部位が離床する直前から第ｎノード要求モードをホールドモードに設定するようにしてもよい。そして、その後しばらくしたら第ｎノード要求モードがリセットモードに設定される。さらに、目標歩容上で第ｎノードに属する少なくとも１つの接地部位が接地する直前から、第ｎノード要求モードが準備完了モードに設定される。
次いで、Ｓ５８において、推定許可条件を満足するか否かを判定し、その判定結果とノード要求モードに基づき各ノードのモードを最終的に決定する。決定される各ノードのモードは、前記準備完了モード、ホールドモード、およびリセットノードのいずれかである。前記ノード要求モードは、目標歩容上での各接地部位の接地、非接地に応じて決定したものであったが、Ｓ５８では、各接地部位の実際の接地、非接地などを考慮して、各ノードのモードが決定される。
推定許可条件とは、以下のいずれかの式３６、３７を満足することである。この場合、式３６、３７のいずれも満足しないときには、推定許可条件が成立しないとする。

ここで、Ｆｎ＿ａｃｔ＿ｚは、実第ｎ接地部位床反力（ｎ＝１，２，…，最終葉ノード番号）の並進力鉛直成分である。
推定許可条件とは、第ｎノード相対床高さ偏差を推定しても（床形状偏差を推定しても）、その推定値が発散しない条件である。ここで発散とは、後述するごとく推定された推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍ（床形状偏差の推定値）を用いて、実際の第ｎ床高さ偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正する補正動作を行った場合に、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍが増加し続け、第ｎ接地部位が床から離れて行く（床からさらに浮いていく）状況を意味する。
Ｚｆｎ＿ｅｓｔｍの推定が理想的に実行されるならば、所定の許容値Ｆｎ＿ｍｉｎは、０で良いが、実際にはＦｎ＿ａｃｔの検出誤差が生じ、それにより推定値が発散する場合がある。それを防ぐために、Ｆｎ＿ｍｉｎは、Ｆｎ＿ａｃｔの検出誤差よりも十分に大きい値に設定する。
本実施形態では、推定許可条件とは、以上に述べた発散が生じない条件である。ただし、推定許可条件は、床形状の所要の推定精度を確保できる条件であってもよい。従って、実第ｎ接地部位床反力Ｆｎ＿ａｃｔの並進力鉛直成分の代わりに、実第ｎ接地部位床反力Ｆｎ＿ａｃｔの並進力成分の内の想定床面（あるいは推定床面（推定した実床面））に垂直な成分を用いても良い。
なお、床形状偏差（実床面と想定床面との形状偏差）を推定するだけで歩行動作に反映しない場合、すなわち、床形状偏差の推定値を用いて床形状偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正しない場合には、床形状偏差の推定値が発散することはない。
推定許可条件を満足するか否かの判定結果とノード要求モードに基づき各ノードのモードを決定する。
第ｎノード要求モードが準備完了モードであり、かつ、推定許可条件を満足するならば、第ｎノードモードを準備完了モードとする。
第ｎノード要求モードが準備完了モードであり、かつ、推定許可条件を満足しないならば、第ｎノードモードをホールドモードとする。
第ｎノード要求モードがホールドモードであるならば、第ｎノードモードをホールドモードとする。
第ｎノード要求モードがリセットモードであるならば、第ｎノードモードをリセットモードとする。
従って、第ｎノードのモードは、要求モードが準備完了モードであり、且つ、推定許可条件を満足する場合に限って、最終的に準備完了モードの決定される。
次いで、Ｓ６０〜Ｓ７０において、すべてのノードに対し、ノードに対応する第ｎノード相対床高さ偏差（ｎ＝１，２，…，最終ノード番号）を推定する。
補足すると、特開平１０−２７７９６９号公報に示したように、姿勢を制御可能な接地部位（足平）を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、接地部位に対応した床傾斜偏差も推定することが望ましい。
このため、Ｓ６０〜Ｓ７０の処理は、接地部位の姿勢を制御可能な場合でも、床形状偏差の推定を行なうことができるように構成されている。
以下にＳ６０〜Ｓ７０の処理を具体的に説明すると、Ｓ６２において、第ｎノードの子ノードの個数を判定する。このとき、子ノード数（子ノードの個数）が２個の場合には、Ｓ６４において、その個数に対応した床形状偏差の推定処理である２子ノード用床形状推定処理が行なわれる。また、子ノード数が３個の場合には、Ｓ６６において、その個数に対応した床形状偏差の推定処理である３子ノード用床形状推定処理が行なわれる。また、子ノード数が０個の場合には、Ｓ６８において、該第ｎノードの接地部位が床反力モーメントを制御できるか否かを判定する。
子ノード数が０の場合の処理についてさらに説明すると、第ｎノードの子ノード数が０の場合、第ｎノードは葉ノードであり、第ｎノードは接地部位に対応している。この場合、上記の如くＳ６８において、まず、第ｎ接地部位が床反力モーメントを制御できるか否かを判定する。ここで、例えば、通常の２足歩行ロボットのように、接地部位として姿勢を制御可能な足平を持ち、脚にフリージョイントを含まないロボットの場合には、接地部位で床反力モーメントを発生することができる。ただし、本実施形態のロボット１（６脚ロボット）においては、各接地部位１０がフリージョイントである球関節１２に係合しているため、いずれの接地部位１０も床反力モーメントを発生することができない。この場合には、子ノード数が０である第ｎノードに対する処理は実行されない。
一方、Ｓ６８において、床反力モーメントを制御できると判定された場合には、Ｓ７０において、接地部位床傾斜偏差推定処理が行なわれる。この処理は、各接地部位における床傾斜偏差を推定する処理である。この処理は、本出願人が先に提案した特開平１０−２７７９６９号公報の中の床傾斜推定処理における足平床反力中心点を本実施形態における第ｎ接地部位の目標床反力中心点に置き換えた処理である。したがって、本明細書では、これ以上の詳細説明を省略する。
次に、前記２子ノード用床形状推定処理を図４５〜図４８を参照して説明する。図４５は２子ノード用床形状推定処理のサブルーチン処理を示すフローチャート、図４６、図４７はそれぞれ図４５のＳ６４０４、Ｓ６４０８のサブルーチン処理を示すフローチャート、図４８は図４５のＳ６４０６およびＳ６４１０のサブルーチン処理を示すフローチャートである。
２子ノード用床形状推定処理では、図４５のフローチャートに示されるように、Ｓ６４００において、２個の子ノードをもつ第ｎノードのすべての子ノードのモード（図４１のＳ５８で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」および、「その他（ｅｌｓｅ）」の３つに分けられる。なお、以降の説明では、第ｎノードの２個の子ノードを第ｉノード、第ｊノードとする。
ここで、子ノードのモードが「すべて準備完了」と判断すると、Ｓ６４０２において、第ｎノードの子ノードの実ノード床反力の並進力鉛直成分Ｆｉ＿ａｃｔ＿ｚ，Ｆｊ＿ａｃｔ＿ｚの合力Ｆｎ＿ｚ（＝Ｆｉ＿ａｃｔ＿ｚ＋Ｆｊ＿ａｃｔ＿ｚ）が所定の値Ｆｎ＿ｍｉｎ２より大きいか否かを判断する。Ｆｎ＿ｚは、換言すれば、第ｎノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。
Ｓ６４０２の判断結果がＹＥＳである場合には、Ｓ６４０４において、２つの子ノードを持つノード用のグループ内全推定処理（第ｎノードが持つ２つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図４６のフローチャート中に示す式に従って第ｎノードの２つの子ノードである第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍが求められる（更新される）。すなわち、Ｚｉ＿ｒｅｌ＿ｅｓｔｍについて代表的に説明すると、Ｚｉ＿ｒｅｌ＿ｅｓｔｍの前回制御周期での値Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐに、前記Ｓ５４で求めた第ｊノード相対床高さ偏差補正量候補値Ｚｉ＿ｉｎｃ＿ｃａｎｄを加えることで、新たな推定第ｊノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍが求められる。第ｊノードについても同様である。
また、第ｎノードの２個の子ノードの床反力の合力Ｆｎ＿ｚが所定の値Ｆｎ＿ｍｉｎ２より小さい場合（Ｓ６４０２の判断結果がＮＯである場合）には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６４０６において、２つの子ノードを持つノード用のグループ内全ホールド処理（第ｎノードが持つ２つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図４８のフローチャート中の式で示す如く、第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｉ＿ｒｅｌ＿ｅｓｔｍの値が、前回制御周期での値Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐに維持される。
次に、図４５のＳ６４００で、２個の子ノードのモードが「すべてリセット」と判断すると、Ｓ６４０８において、２つの子ノードを持つノード用グループ内全リセット処理（第ｎノードが持つ２つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行なわれる。そのリセット処理では、図４７のフローチャート中の式に従って第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｉ＿ｒｅｌ＿ｅｓｔｍが０に徐々に近づいていくように更新される。なお、同式中のΔＴ、Ｔｅｓｔｍの意味は、前記式３５のものと同じである。
このリセット処理は、より一般的に言えば、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍを、それらの重み付き平均値が０という条件、すなわちＷｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＝０を満足しつつ、それぞれＺｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐよりも０に近い値に決定するという処理である。なお、このリセット処理は、一次遅れ要素を含むため、ノード相対床高さ偏差が完全に０に戻るためには無限時間を要する。そこで、本出願人が先に提案した有限整定関数発生器（特開平５−３２４１１５号公報）を用いて、ノード相対床高さ偏差を徐々に０にリセットするようにしてもよい。
また、図４５のＳ６４００で、２個の子ノードのモードが「その他」である場合、すなわち、「すべて準備完了」、「すべてリセット」のいずれでもない場合（例えば２個の子ノードのモードがすべてホールドモードである場合）には、Ｓ６４０６と同じ処理（図４８を参照）がＳ６４１０で実行される。
以上のように、第ｎノードの２個の子ノードのモードがすべて準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その２個の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、子ノードのモードがすべて準備完了である場合であっても、推定処理の発散が生じる恐れがある場合には、２個の子ノードの推定ノード相対床高さ偏差の値が、ホールドされる。また、２個の子ノードの実ノード床反力がいずれも０となる状況、すなわち、第ｎノードに属するすべての接地部位１０に実床反力が作用しない状況では、２個の子ノードのノード相対床高さ偏差は、徐々に０になるようにリセットされる。
次に、図４１のＳ６６の３子ノード床形状推定処理を図４９〜図５５を参照して説明する。図４９は３子ノード用床形状推定処理のサブルーチン処理を示すフローチャート、図５０、図５１はそれぞれ図４９のＳ６６０４、Ｓ６６０８のサブルーチン処理を示すフローチャート、図５２は図４９のＳ６６０６、Ｓ６６１４およびＳ６６１８のサブルーチン処理を示すフローチャート、図５３、図５４はそれぞれ図４９のＳ６６１２、Ｓ６６１６のサブルーチン処理を示すフローチャートである。
３子ノード用床形状推定処理では、図４９のフローチャートに示されるように、Ｓ６６００において、３個の子ノードをもつ第ｎノードのすべての子ノードのモード（図４１のＳ５８で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」、「２つの子ノードのみ準備完了」、「１つの子ノードのみホールドで、残りがリセット」、「２つの子ノードのみホールドで、残りがリセット」、および「その他（ｅｌｓｅ）」の６つに分けられる。
ここで、図４９のフローチャートでは、３子ノード用床形状推定処理をより一般化して説明するために、前記第２実施形態で説明した如く、各中間ノード（第１４５ノード、第２３６ノード）に属する接地部位１０のすべてが同時に接地したり離床する場合だけでなく、各中間ノードの接地部位のうちのいずれかが接地し、その他の接地部位が離床するようにロボットの目標歩容を生成する場合についても考慮している。この場合、子ノードが２つの時と違い、３つの子ノードのモードが、「２つの子ノードのみ準備完了」、「１つの子ノードのみホールドで、残りがリセット」、もしくは「２つの子ノードのみホールドで、残りがリセット」の場合も考慮する必要がある。なお、以降の説明では、第ｎノードの３個の子ノードを第ｉノード、第ｊノード、第ｋノードとする。
ここで、子ノードのモードが「すべて準備完了」と判断すると、Ｓ６６０２において、第ｎノードの子ノードの実ノード床反力の並進力鉛直成分Ｆｉ＿ａｃｔ＿ｚ，Ｆｊ＿ａｃｔ＿ｚ，Ｆｋ＿ａｃｔ＿ｚの合力Ｆｎ＿ｚ（＝Ｆｉ＿ａｃｔ＿ｚ＋Ｆｊ＿ａｃｔ＿ｚ＋Ｆｋ＿ａｃｔ＿ｚ）合力が所定の値Ｆｎ＿ｍｉｎ２より大きいか否かを判断する。Ｆｎ＿ｚは、換言すれば、第ｎノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。
Ｓ６６０２の判断結果がＹＥＳである場合には、Ｓ６６０４において、３つの子ノードを持つノード用グループ内全推定処理（第ｎノードが持つ３つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図５０のフローチャート中に示す式に従って第ｎノードの３つの子ノードである第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ、Ｚｋ＿ｒｅｌ＿ｅｓｔｍが求められる（更新される）。すなわち、Ｚｉ＿ｒｅｌ＿ｅｓｔｍについて代表的に説明すると、Ｚｉ＿ｒｅｌ＿ｅｓｔｍの前回制御周期での値Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐに、前記Ｓ５４で求めた第ｉノード相対床高さ偏差補正量候補値Ｚｉ＿ｉｎｃ＿ｃａｎｄを加えることで、新たな推定第ｉノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍが求められる。第ｊノード、第ｋノードについても同様である。
また、Ｓ６６０２の判断結果がＮＯである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６６０６において、３つの子ノードを持つノード用のグループ内全ホールド処理（第ｎノードが持つ３つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図５２のフローチャート中の式で示す如く、第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ、Ｚｋ＿ｒｅｌ＿ｅｓｔｍの値が、前回制御周期での値Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐ、Ｚｋ＿ｒｅｌ＿ｅｓｔｍに維持される。
また図４９のＳ６６００で、３個の子ノードのモードが「すべてリセット」と判断すると、Ｓ６６０８において、３つの子ノードを持つノード用グループ内全リセット処理（第ｎノードが持つ３つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行われる。そのリセット処理では、図５１のフローチャート中の式に従って第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ、Ｚｋ＿ｒｅｌ＿ｅｓｔｍが０に徐々に近づいていくように更新される。なお、同式中のΔＴ、Ｔｅｓｔｍの意味は、前記式３５のものと同じである。
このリセット処理はより一般的に言えば、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍを、それらの重みつき平均値が０という条件、すなわちＷｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｋ＊Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＝０を満足しつつ、それぞれＺｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐよりも０に近い値に決定するという処理である。なお、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐを０にリセットするために、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
また、図４９のＳ６６００で、３個の子ノードのモードが「２つの子ノードのみ準備完了」と判断すると、第ｎノードの子ノードの実ノード床反力の合力（第ｎノードに属するすべての接地部位１０の実床反力の合力）の並進力鉛直成分Ｆｎ＿ｚが所定の値Ｆｎ＿ｍｉｎ２より大きいか否かをＳ６６１０で判断する。
この判断結果がＹＥＳである場合には、Ｓ６６１２において、３つの子ノードを持つノード用グループ内部分推定処理（第ｎノードが持つ３つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）が行われる。この処理は、図５３のフローチャートに示す如く実行される。なお、ここでは、第ｉノードが準備完了モードでなく、第ｊノードおよび第ｋノードが準備完了モードであるとする。
まず、Ｓ６６１２０において第ｉノードのモードが判断される。この判断結果がホールドモードである場合には、Ｓ６６１２２において、新たな第ｉノード相対床高さ偏差補正量候補値Ｚｉ＿ｉｎｃ＿ｃａｎｄ’を０に決定し、リセットモードである場合には、Ｓ６６１２４において、図中の式により、新たな第ｉノード相対床高さ偏差補正量候補値Ｚｉ＿ｉｎｃ＿ｃａｎｄ’を決定する。Ｓ６６１２４で決定されるＺｉ＿ｉｎｃ＿ｃａｎｄ’は、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐを徐々に０に近づけるための第ｉノード相対床高さ偏差補正量候補値である。なお、Ｓ６６１２４の式中のΔＴ、Ｔｅｓｔｍの意味は、前記式３５のものと同じである。補足すると、Ｓ６６１２４でＺｉ＿ｉｎｃ＿ｃａｎｄ’を決定するとき、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
次いで、Ｓ６６１２６において、図中の式に従って、新たな第ｊノード相対床高さ偏差補正量候補値Ｚｊ＿ｉｎｃ＿ｃａｎｄ’および新たな第ｋノード相対床高さ偏差補正量候補値Ｚｋ＿ｉｎｃ＿ｃａｎｄ’を決定する。
すなわち、Ｗｉ＊Ｚｉ＿ｉｎｃ＿ｃａｎｄ’＋Ｗｊ＊Ｚｊ＿ｉｎｃ＿ｃａｎｄ’＋Ｗｋ＊Ｚｋ＿ｉｎｃ＿ｃａｎｄ’＝０という条件（Ｚｉ＿ｉｎｃ＿ｃａｎｄ’，Ｚｊ＿ｉｎｃ＿ｃａｎｄ’，Ｚｋ＿ｉｎｃ＿ｃａｎｄ’の重み付き平均値が０という条件）と、Ｚｊ＿ｉｎｃ＿ｃａｎｄ’−Ｚｊ＿ｉｎｃ＿ｃａｎｄ＝Ｚｋ＿ｉｎｃ＿ｃａｎｄ’−Ｚｋ＿ｉｎｃ＿ｃａｎｄという条件を満足するようにＺｊ＿ｉｎｃ＿ｃａｎｄ’，Ｚｋ＿ｉｎｃ＿ｃａｎｄ’を決定する。
次いで、Ｓ６６１２８において、上記の如く決定されたＺｉ＿ｉｎｃ＿ｃａｎｄ，Ｚｊ＿ｉｎｃ＿ｃａｎｄ，Ｚｋ＿ｉｎｃ＿ｃａｎｄ’をそれぞれ第ｉノード、第ｊノード、第ｋノードの相対床高さ偏差の前回制御周期での値Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐに加えることで、新たなＺｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍが決定される。
このようにＺｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍを決定することで、Ｗｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｋ＋Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＝０を満足しつつ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿Ｚｋ＿ｒｅｌ＿ｅｓｔｍがＺｊ＿ｉｎｃ＿ｃａｎｄ−Ｚｋ＿ｉｎｃ＿ｃａｎｄに近づくようにＺｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍが決定されることとなる。
また、図４９のＳ６６１０の判断結果がＮＯである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６６１４において、前記Ｓ６６０６と同じ処理（図５２を参照）が実行される。
また図４９のＳ６６００で、３個の子ノードのモードが「１つの子ノードのみホールドで、残りがリセット」と判断すると、Ｓ６６１６において、１つの子モードのみホールドで、残りがリセットの場合に対応する処理が実行される。その処理で、図５４のフローチャート中に示す式によって、新たなノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍが決定される。なお、ここでは、第ｉノードのモードがホールドモードで、第ｊノードおよび第ｋノードのモードがリセットモードであるとする。また、同式中のは、ΔＴ、Ｔｅｓｔｍの意味は、前記式３５のものと同じである。
図５４の処理は、より一般的には、Ｚｉ＿ｒｅｌ＿ｅｓｔｍを０に決定し、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍを、Ｗｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＋＋Ｗｋ＊Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＝０（Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍの重み付け平均値が０であるという条件）を満足しつつ、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐよりも０に近い値に決定する処理である。補足すると、第ｊノードおよび第ｋノードのモードが共にリセットモードになる時点までに、それらのノードの重みＷｊ，Ｗｋは０となっているはずである。従って、重みＷｉ，Ｗｊ，Ｗｋの和が１であるという条件から、この時点までにＷｉが１となっており、また、Ｗｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｋ^＊Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＝０であるから、この時点までに、Ｚｉ＿ｒｅｌ＿ｅｓｔｍは０になっている。このため、第ｉノードのＺｉ＿ｒｅｌ＿ｅｓｔｍの値をホールドするということは、その値を０に維持するということである。そこで、図５４の処理では、Ｚｉ＿ｒｅｌ＿ｅｓｔｍの値を０に決定するようにしている。
なお、Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍを徐々に０にリセットする処理では、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
更に図４９のＳ６６００で、子ノードのモードが「２つの子ノードのみホールドで、残りがリセット」と判断すると、Ｓ６６１８において、２つの子モードのみホールドで、残りがリセットの場合の処理が行われる。その処理では、図５５のフローチャート中の式に従って、新たなノード相対床高さ偏差Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍが決定される。なお、ここでは、第ｉノードおよび第ｊモードのモードが共にホールドモードで、第ｋノードのモードがリセットモードであるとする。また、同式中のΔＴ、Ｔｅｓｔｍの意味は、前記式３５のものと同じである。
図５５の処理は、より一般的には、Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍをそれぞれ前回制御周期での値にホールドし、Ｚｋ＿ｒｅｌ＿ｅｓｔｍを、Ｗｉ＊Ｚｉ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｊ＊Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＋Ｗｋ＊Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＝０（Ｚｉ＿ｒｅｌ＿ｅｓｔｍ，Ｚｊ＿ｒｅｌ＿ｅｓｔｍ，Ｚｋ＿ｒｅｌ＿ｅｓｔｍの重み付け平均値が０であるという条件）を満足しつつ、Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐよりも０に近い値に決定する処理である。補足すると、第ｋモードがリセットモードになる時点までに、Ｗｋは０となっている。
なお、Ｚｋ＿ｒｅｌ＿ｅｓｔｍを徐々に０にリセットする処理では、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
更に図４９のＳ６６２０で「その他」と判断した場合（例えば３個の子ノードのモードがすべてホールドモードである場合）には、Ｓ６６２０において、前記Ｓ６６０６と同じ処理（図５２を参照）が実行される。
以上のように、第ｎノードの３個の子ノードのモードのうち、２つ以上の子ノードのモードが準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その２個以上の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、２つ以上の子ノードのモードが準備完了である場合であっても、推定処理の発散が生じる恐れがある場合には、３個の子ノードの推定ノード相対床高さ偏差の値が、ホールドされる。
以上のごとく、図４１のＳ６０〜Ｓ６６では、推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍ（ｎ＝１，２，…，最終ノード番号）が決定される。なお、根ノードの相対床高さ偏差は０である。
図４１のフローチャートの処理では、最後に、Ｓ７２において、第ｎノード（ｎ＝１，２，…，最終葉ノード番号）のすべての先祖ノードの推定相対床高さ偏差と推定第ｎノード相対床高さ偏差との和を求め、求めた和を推定第ｎ接地部位床高さ偏差（推定第ｎ床高さ偏差）Ｚｆｎ＿ｅｓｔｍとする。
以上が、図４０における床高さ偏差の推定処理のサブルーチン処理である。
次に図３５の説明に戻って、上述のように床形状推定器１３０で逐次求められた推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，…，最終葉ノード番号）は、加算器１３２により、各接地部位１０の修正目標接地部位位置姿勢に加算され、これにより、床形状偏差補償入り修正目標接地部位位置姿勢が求められる。そして、この床形状偏差補償入り修正目標接地部位位置姿勢が修正目標接地部位位置姿勢の代わりに、前記機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈへ入力される。
以上の説明した床形状推定器１３０および加算器１３２を含む制御装置５０の処理を該制御装置５０の制御周期毎に繰り返し実行することにより、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍが実際の第ｎ床高さ偏差に収束する。また、各目標接地部位位置姿勢は、その接地部位に対応する推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍに応じて修正され、実際の第ｎ床高さ偏差が実床反力に及ぼす影響を吸収する。
尚、すべての第ｎ接地部位（ｎ＝１，２，…，最終葉ノード番号）が接地しているならば、第ｎ接地部位の実際の接地面上における目標第ｎ接地部位床反力中心点Ｑｎに相当する点は、実第ｎ床接点Ｄｎ＿ａｃｔに一致するはずである。従って、前記瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’は、変動しないはずである。しかし、実際にはロボット１の機械系や制御系の振動および電気的なノイズにより、前記床形状推定器１３０の前記した計算アルゴリズムによって得られた瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’は、激しく変動する。従って、瞬間第ｎノード相対床高さ偏差をそのまま推定第ｎノード相対床高さ偏差として用いて、これを基に推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを求め（すなわち第ｎノード（ｎ＝１，２，…，最終葉ノード番号）のすべての先祖ノードの瞬間相対床高さ偏差と瞬間第ｎノード相対床高さ偏差との和をもって推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍとし）、さらに図３５に示すように、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを修正目標接地部位位置姿勢に加算したものを機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈへ入力させると、ロボット１の接地部位１０は発振するかあるいは激しく振動する。
ところで、前記２つの子ノードを持つノード用グループ内全推定処理、３つの子ノードを持つノード用グループ内全推定処理、３つの子ノードを持つノード用グループ内部分推定処理では、瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’から推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍまでの伝達関数は、１次遅れのローパスフィルタになる。即ち、推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍは瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’にローパスフィルタを通したものとなる。従って、推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍを基に求めた推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを図３５に示すように修正目標接地部位位置姿勢に加算しても、各接地部位１０の発振や振動を生じ難くなる。これにより、各接地部位１０の発振や振動を防止することができる。
ちなみに、本実施形態では、上記ローパスフィルタの時定数はＴｅｓｔｍである。なお、本実施形態で説明した床形状推定器１３０の処理以外にも、瞬間第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ’から推定第ｎノード相対床高さ偏差Ｚｎ＿ｒｅｌ＿ｅｓｔｍまでの伝達関数がローパスフィルタになるように構成することは可能である。
次いで、この実施の形態における床形状推定器１３０の特徴を説明する。
ロボット１の姿勢安定化制御のために必要な各ノード補償モーメントを発生させようと、各ノード補償角を変化させ、最終的な目標接地部位位置姿勢である機構変形補償入り修正目標接地部位位置姿勢を変更させても、各接地部位１０が実際に接地している限り、第ｎ接地部位１０の実際の接地面上における目標第ｎ接地部位床反力中心点Ｑｎに相当する点は、実第ｎ床接点Ｄｎａｃｔに一致したまま動かない。その代わりに、各脚のコンプライアンス機構４２などが変形したり、上体２４の位置姿勢が変化する。即ち、機構変形補償入り修正目標接地部位位置姿勢を変更させた分を打ち消すように、コンプライアンス機構４２などが変形したり、上体２４の位置姿勢が変化する。
本実施形態における床形状推定器１３０は、機構変形補償入り修正目標接地部位位置姿勢から推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，…，最終葉ノード番号）を算出するまでに、実床反力を基に機構コンプライアンスモデル１３４によって算出されたコンプライアンス機構４２などの変形量と上体姿勢傾斜偏差θｂｅｒｒとによる推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍへの影響を打ち消しているので、上体姿勢傾斜偏差が生じたり、機構変形補償入り修正目標接地部位位置姿勢が変動しても、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍはその影響を受けない。
これは、床形状の推定処理が、ロボット１のコンプライアンス制御や姿勢制御から干渉を受けないことを意味する。従って、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを図３５に示すように修正目標接地部位位置姿勢に加算しても、ロボット１の制御系の安定余裕（発振のし難さ）はほとんど低下しない。すなわち、コンプライアンス制御、姿勢制御、および床形状の推定処理と、床形状偏差の推定値を用いたロボット１の目標歩容の補正動作とを同時に行なっても、各制御・処理が干渉し合って発振することはほとんどない。すなわち、簡単に言えば、床形状偏差の複数のパラメータを同時に推定しながら、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。さらには、姿勢制御のための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。
また、各接地部位１０の推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを図３５に示すように修正目標接地部位位置姿勢に加算すると、実際に第ｎ床高さ偏差が存在していても、その影響を打ち消すことができるので、実全床反力は、定常的にはロボット１が想定床を移動している場合と同一になる。しかも、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍの推定と、推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍを用いた修正目標接地部位位置姿勢の修正とは制御周期毎に逐次実行されるので、床形状が途中で変化しても、変化後の第ｎ床高さ偏差を推定し、その推定値に基づいて床形状の変化の影響を打ち消すことができる。ただし、床形状推定器１３０には前述したようにローパスフィルタが含まれるので、各接地部位１０が接地した直後や床形状が途中で変化したときには、過渡的には、実全床反力は、床形状偏差の影響を受ける。しかし、その後、時定数Ｔｅｓｔｍでその影響が減衰する。
また、以下の特徴もある。
１）床形状偏差が大きくても制御が破綻し難い。これは、階層型コンプライアンス制御は微小な補償角に限定した近似を行っていないからである。
２）床形状偏差の推定精度が高い。
３）ロボットの姿勢安定化制御との干渉による発振が生じにくいので、推定処理の時定数を短くすることができる。それによって、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作の応答性を高めることができるので、床形状偏差が床反力に及ぼす過渡的な影響が短時間で解消される。
ロボットの移動時の１歩前の着地期（ある接地部位１０が接地状態となる時期）での推定第ｎ床高さ偏差と、当該１歩前の着地期の次の着地期での推定第ｎ床高さ偏差との関係に相関がない場合には、本実施形態に示したように推定第ｎ床高さ偏差を強制的に徐々に０に収束させるのが良い。しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることが分かっているならば、前記関係にある程度の相関があると考えられる。この場合には、現在の着地期における推定第ｎ床高さ偏差を、強制的に徐々に１歩前の着地期における推定第ｎ床高さ偏差に１より小さい正の定数を乗じた値に収束させるようにしても良い。
さらに、１歩前の着地期での推定第ｎ床高さ偏差だけでなく、複数歩前の着地期での推定第ｎ床高さ偏差も用いて現在の着地期における推定第ｎ床高さ偏差の収束目標値を決定しても良い。また、現在や１歩前や複数歩前の着地期における他のノードの推定第ｎノード床高さ偏差および推定第ｎノード床傾斜偏差も用いて収束目標値を決定しても良い。
本実施形態（第３実施形態）は上記の如く構成したので、従来技術では困難であった多くの接地部位を持つロボットにおいても、床形状の推定、具体的には、各接地部位に対応する床高さ偏差を同時に、換言すれば複合的に、精度良く推定することができる。さらに、ロボットの接地部位をその姿勢を制御可能に設け、床形状の推定処理において床高さ偏差だけでなく、床傾斜偏差も推定する場合には、床傾斜偏差も含めて同時に、精度良く推定することができる。
さらには、床形状の推定結果を基に目標接地部位位置姿勢軌道を修正することにより、床形状が想定していた形状と異なっていても、その影響を吸収して望み通りの床反力を発生させることができる。
特に、床形状偏差の推定値による補正がない階層型コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づけることができる。換言すれば床形状の偏差に起因する床反力の定常偏差を解消することができる。
また、床形状の推定精度が低下する恐れがある状況あるいはその推定値が発散する恐れがある状況では床形状の推定を中断するように構成したので、前記した作用効果に加え、推定値が不適切なものとなることがない。
尚、本実施形態に係るロボットの床形状推定装置１３０は前記した手法で床形状を推定すれば足り、推定値に基づいて運動を補正することは必須ではない。
［第４実施形態］
次に、本発明の第４実施形態に係る移動ロボットの制御装置（特い床形状推定装置）を説明する。第４実施形態においては、それを簡単に説明すれば、前記第３実施形態の図４０に示した床形状推定器１３０のブロック線図を等価変換したものである。すなわち、直前に推定した床形状の推定値（前回制御周期での推定値などの該推定値の過去値）と、最終的に修正され決定された目標歩容（詳しくはコンプライアンス動作を含む機構変形補償入り修正目標接地部位位置姿勢）（もしくは実関節変位）と、実上体姿勢傾斜偏差θｂｅｒｒとを基に、各ノードの床反力を推定し、その推定値と各ノードの実床反力との差を基に、この差を０に近づけるように床形状の推定値の補正量候補値を決定し、床形状の推定が発散しないと判断されるならば、床形状の推定値の補正量候補値を基に、直前に推定した床形状の推定値を補正するようにした。この処理は、本出願人が先に提案した特開平１０−２７７９６９号公報において、２足移動ロボットの各脚の足平の床反力中心点まわりの想定されるモーメントと実モーメントとの差が０に近づくように各足平に係る床傾斜を推定する手段、ならびに、全床反力中心点まわりの想定されるモーメントと実モーメントとの差が０に近づくように両脚間の干渉角を推定する手段を発展させた階層型の床形状推定手段に相当するものである。
このように本実施形態は、床形状推定器１３０の処理のみが、第３実施形態と相違するものであり、床形状推定器１３０以外の処理については説明を省略する。
図５７は、本実施形態における床形状推定器１３０の機能的手段を示すブロック図である。この図５７を参照して本実施形態における床形状推定器１３０を以下に説明する。
先ず、各接地部位１０の機構変形補償入り修正目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）と、各接地部位１０の目標接地部位位置から見た該接地部位１０の目標第ｎ接地部位床反力中心点と、前記姿勢傾斜偏差θｂｅｒｒとを基に、機構変形補償入り修正目標第ｎ接地部位位置姿勢を、目標全床反力中心点を回転中心としてθｂｅｒｒだけ回転移動させてなる、回転後機構変形補償入り修正目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）を求める。求められた回転後機構変形補償入り修正目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，最終葉ノード番号）から、目標歩容上での想定第ｎ床接地位置Ｄｎ（ｎ＝１，２，…，最終葉ノード番号）を減算し、第ｎ接地部位干渉高さＺｎ＿ｉｎｔを求める。それを床形状推定器１３０に備えた機構コンプライアンスモデル（逆モデル）へ入力することによって、各接地部位１０の床反力の推定値である推定第ｎ接地部位床反力を求める。
なお、ここでの機構コンプライアンスモデルは、各接地部位干渉高さの互いの相対関係に応じて推定接地部位床反力の互いの相対関係を求めるものであり、絶対値に意味をもたない。また、第ｎ接地部位の実床反力Ｆｎ＿ａｃｔの検出値がある閾値Ｆｎ＿ｍｉｎ３より小さい場合には、上記機構コンプライアンスモデルにおいても第ｎ接地部位が接地していないものと想定する。
さらに、第ｎ接地部位（ｎ＝１，２，…，最終葉ノード番号）の実床反力Ｆｎａｃｔの検出値から、前記推定第ｎ接地部位床反力を減算し、第ｎ接地部位床反力推定誤差Ｆｆｎ＿ｅｓｔｍ＿ｅｒｒを求める。この第ｎ接地部位床反力推定誤差Ｆｆｎ＿ｅｓｔｍ＿ｅｒｒは、力で表現されているため、換算値Ｃｎ（例えば、バネ定数の逆数のようなもの）によって高さ偏差に換算し、それを第ｎ接地部位床高さ偏差補正量候補値Ｚｆｎ＿ｉｎｃ＿ｃａｎｄとして得る。なお、換算値Ｃｎは、対角行列とは限らない。
次いで、求められた第ｎ接地部位床高さ偏差補正量候補値Ｚｆｎ＿ｉｎｃ＿ｃａｎｄから、前記した「階層相対化処理」によって、階層相対化することで、第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄ（ｎ＝１，２，…，最終ノード番号）を求める。次に、床高さ偏差の推定処理のサブルーチン処理によって、第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄから、推定床高さ偏差（推定第ｎ床高さ偏差）Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，…，最終葉ノード番号）を求める。ここでの床高さ偏差の推定処理のサブルーチン処理は、図４１のＳ５６〜Ｓ７２までの処理と同一である。
なお、特開平１０−２７７９６９号公報に示したように、接地部位として姿勢を制御可能な足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、図４１のＳ７０に関して説明した如く、接地部位床傾斜偏差推定処理を行なうのが望ましい。
ここで、第３実施形態と第４実施形態（本実施形態）とを比較する。第４実施形態は、簡単に言えば、第３実施形態の床形状推定器１３０のブロック線図を等価変換したものになっている。したがって、第４実施形態の作用効果は、第３実施形態の作用効果と同じである。また、第３実施形態と同様、床形状の推定値に基づいて運動を補正することは必須ではない。
尚、第３および第４実施形態において、床反力センサとして、分布圧センサを用いても良い。６軸力センサ３４などの力センサよりも分布圧センサの方が実床反力による接地部位の変形をより細緻に推定することができるので、床形状推定の精度を向上させることができる。
さらには、本出願人による特許第３０３５０５１号公報の図１に示す如く、接地部位に接触センサを複数個配置（例えば接地部位の四隅に配置）し、その出力信号に基づいて接地部位がどちらの方向に浮きやすいかを判定しても良い。そして、接地していない接触センサが増々浮く方向での床形状の推定を中断したり、あるいは、全ての接触センサが浮いているときにも床形状の推定を中断するようにしてもよい。
［第５実施形態］
前記第１〜第４実施形態では、子ノードを持つノードの、該子ノードの総数が、３個以下の場合を例にとって説明したが、子ノードの数が４つ以上の場合にも、重み付き平均の概念を持ち込むことによって、以下に示すようにモーメントおよび傾斜角の概念を拡張できる。
目標値、実際値、偏差のいずれに関しても概念を拡張できるので、ここでは、目標値、実際値、偏差の区別をせず、一般的な接地部位床反力の組Ｆｎ（ｎ＝１，２，．．．）に対するモーメントを、以下のように拡張して、定義する。
第ｎ接地部位の床反力の組Ｆｎ（ｎ＝１，２，．．．，最終葉ノード番号）を階層相対化して、第ｎノード相対床反力Ｆｎ＿ｒｅｌ（ｎ＝１，２，．．．，最終ノード番号）を求める。
第ｎノードの子ノードの数をｒとする。
第ｎノードの第ｊ番目の子ノードの識別番号がａｊ（ｊ＝１，２，…，ｒ）であるとする。
第ｎノードの第ｊ番目の子ノードの重みが、Ｗｊ（ｊ＝１，２，…，ｒ）であるとする。
第ｎノードの第ｊ番目の子ノードの重みを第ｊ要素として持つ列ベクトルをＵＷｎとする。
すなわち、下式３８のようにＵＷｎを定義する。ここで「Ｔ」は、転置を意味し、行ベクトルの転置によって列ベクトルを表現することとする。

ベクトルＵＷｎに直交する（すなわちベクトルＵＷｎとの内積が０となる）ｒ−１個の互いに独立なベクトルをＲ（１），Ｒ（２），…，Ｒ（ｒ−１）とする。Ｒ（ｊ）（ｊ＝１，２，…，ｒ−１）は、ｒ行１列の列ベクトルである。なお、Ｒ（ｊ）（ｊ＝１，２，…，ｒ−１）は、演算の容易さと制御精度を考慮すると、互いに直交することが望ましい。Ｒ（ｊ）を第ｊ列（ｊ＝１，２，…，ｒ−１）とする行列をＨｎとする。Ｈｎは、ｒ行ｒ−１列の行列である。
第ｎノードの第ｊ番目の子ノードの相対床反力Ｆａｊ＿ｒｅｌを第ｊ要素（ｊ＝１，２，…，ｒ）とする列ベクトルを第ｎグループ相対床反力ベクトルＦｎ＿ｒｅｌ＿ｃとする。
すなわち、次式３９により、Ｆｎ＿ｒｅｌ＿ｃを定義する。

Ｆｎ＿ｒｅｌ＿ｃは、ｒ行１列の列ベクトルである。
次式４０を満足する列ベクトルＭｎ＿ｅｘｐを第ｎノード拡張床反力モーメントと呼ぶ。

Ｍｎ＿ｅｘｐは、ｒ−１行１列の列ベクトルである。
以上のように、接地部位床反力の組Ｆｎ（ｎ＝１，２，…）に対し、第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐを定義する。
すなわち、接地部位床反力Ｆｎ（ｎ＝１，２，…）を基に、階層化したノード相対床反力Ｆｎ＿ｒｅｌ（ｎ＝１，２，．．．）を求め、さらに第ｎノードのすべての子ノードａｊ（ｊ＝ａ１，ａ２，…，ａｒ）の相対床反力を要素とするベクトル（Ｆａ１＿ｒｅｌ，Ｆａ２＿ｒｅｌ，…，Ｆａｒ＿ｒｅｌ）^Ｔを、第ｎノードのすべての子ノードの重みを要素とするベクトル（Ｗａ１，Ｗａ２，…，Ｗａｒ）^Ｔに直交するある所定の互いに独立なベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐと呼ぶ。
なお、第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐの第ｊ番目要素（第ｊ行目要素）を第ｎノード拡張床反力モーメント第ｊ成分と呼ぶ。
また、実接地部位床反力の組Ｆｎ（ｎ＝１，２，…）に対する第ｎノード拡張床反力モーメントを実第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐ＿ａｃｔと呼ぶ。
また、目標接地部位床反力の組Ｆｎ（ｎ＝１，２，…）に対する第ｎノード拡張床反力モーメントを目標第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐ＿ｒｅｆと呼ぶ。目標第ｎノード拡張床反力モーメントは、通常０ベクトルに設定される。
また、接地部位床反力偏差の組Ｆｎ（ｎ＝１，２，…）に対する第ｎノード拡張床反力モーメントを第ｎノード拡張床反力モーメント偏差Ｍｎ＿ｅｘｐ＿ｅｒｒと呼ぶ。
同様に、一般的な接地部位高さの組Ｚｎ（ｎ＝１，２，．．．）に対する傾斜角を、以下のように拡張して、定義する。
接地部位高さの組Ｚｎ（ｎ＝１，２，…，最終葉ノード番号）を階層相対化して、第ｎノード相対高さＺｎ＿ｒｅｌ（ｎ＝１，２，…，最終ノード番号）を求める。
第ｎノードの第ｊ番目の子ノードの相対高さＺａｊ＿ｒｅｌを第ｊ要素（ｊ＝１，２，…，ｒ）とする列ベクトルを第ｎノード相対高さベクトルＺｎ＿ｒｅｌ＿ｃとする。
すなわち、次式４１によりＺｎ＿ｒｅｌ＿ｃを定義する。

次式４２を満足する列ベクトルθｎ＿ｅｘｐを第ｎノード拡張床反力傾斜角と呼ぶ。

θｎ＿ｅｘｐは、ｒ−１行１列の列ベクトルである。
すなわち、接地部位高さＺｎ（ｎ＝１，２，…）を基に、階層化したノード相対高さＺｎ＿ｒｅｌ（ｎ＝１，２，…）を求め、さらに第ｎノードのすべての子ノードａｊ（ｊ＝１，２，…，ｒ）の相対高さを要素とするベクトル（Ｚａ１＿ｒｅｌ，Ｚａ２＿ｒｅｌ，…，Ｚａｒ＿ｒｅｌ）^Ｔを、第ｎノードのすべての子ノードの重みを要素とするベクトル（Ｗａ１，Ｗａ２，…，Ｗａｒ）^Ｔに直交する前記所定の互いに独立なベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノード拡張傾斜角θｎと呼ぶ。
上記のように定義された拡張床反力モーメントと拡張傾斜角とを用いることにより、第ａノードの子ノードの数が４つ以上の場合にも、コンプライアンス制御および床形状推定の処理は、ほとんどそのまま拡張される。以下にその例を第５の実施形態として説明する。なお、第５実施形態は、ロボットの制御装置の機能的構成は、前記図２に示したものと同様とする。また、この場合、階層型コンプライアンス動作決定部は補償全床反力モーメント分配器、補償角決定部、および床形状推定器以外の機能的構成は、前記図３５を参照して説明したものと同様でよい。
［補償全床反力モーメント分配器の処理の拡張について］
例えば、階層型コンプライアンス動作決定部の中の補償全床反力モーメント分配器の処理は、次のように拡張される。
ロボットの姿勢を復元させるために目標第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐ＿ｒｅｌ（通常０）に付加的に発生すべき拡張床反力モーメントを第ｎノード補償拡張床反力モーメントＭｎ＿ｅｘｐ＿ｄｍｄと呼ぶ。
第ｎノード補償拡張床反力モーメントＭｎ＿ｅｘｐ＿ｄｍｄと第ｎノード補償床反力モーメントＭｎ＿ｄｍｄの間には、次式４３の関係がある。ただし、Ｃｎ＿ｍｅｃｈは、各目標ノード床反力中心点とロボットの機構のコンプライアンス特性とによって決定される所定の係数行列（２行ｒ−１列の行列）である。

一方、前記式１０を一般化すると次式４４になる。

なお、Σは、全ての第ｎノードについての総和を意味する。
よって、補償全床反力モーメント分配器では、式４３と式４４を満足するように、第ｎノード補償拡張床反力モーメントＭｎ＿ｅｘｐ＿ｄｍｄ（ｎ＝１，２，…）および第ｎノード補償床反力モーメントＭｎ＿ｄｍｄを決定すれば良い。
［補償角決定部（θｎ決定部）の処理の拡張について］
第５実施形態における各第ｎノードの補償角決定部の処理は、例えば第３実施形態のθ１４５決定部（図２５および図３３を参照）の処理において、Ｍ１４５ａｃｔを求めるまでの処理を、各接地部位の実床反力を基に前記の実第ｎノード拡張床反力モーメントＭｎ＿ｅｘｐ＿ａｃｔを求める処理に置き換え、１４５をｎに置き換え、図３３中の補償フィルタ、ローパスフィルタおよびゲインＫｎの次元を第ｎノードの子ノード数から１を減じた数に拡張すれば良い。上記の置き換えを行なった処理によって、第ｎノードの補償角θｎを求めることができる。
［拡張床反力モーメントを用いた床形状推定ついて］
以下のように、床形状推定器の処理の一部に拡張床反力モーメントの概念を用いた手法を組み込んでも良い。
すなわち、前記第４実施形態で説明した床形状推定器の図５６に示した処理の代わりに、図５７のブロック図で示す処理を用いて、床形状偏差を推定する。
図５７を参照して、この処理を説明すると、まず、前記図５６で説明した如く算出される推定第ｎ接地部位床反力と、第ｎ接地部位の実床反との差（各ノード毎の差の組）から、拡張モーメントの定義にしたがって、第ｎ接地部位床反力推定誤差Ｆｆｎ＿ｅｓｔｍ＿ｅｒｒ（ｎ＝１，２，．．．）に対応する第ｎノード拡張床反力モーメント推定誤差Ｍｎ＿ｅｘｐ＿ｅｓｔｍ＿ｅｒｒを求める。なお、第ｎノード拡張床反力モーメント推定誤差を決定する上で必要となる前記ベクトルＲ（ｊ）は、各ノードの重みを基に決定される。
次いで、次式４５のように、所定の係数マトリックスＫｎ＿ｃｍｐｌを第ｎノード拡張床反力モーメント推定誤差Ｍｎ＿ｅｘｐ＿ｅｓｔｍ＿ｅｒｒに乗算することにより、第ｎノード拡張傾斜角の補正量の候補値である第ｎノード拡張傾斜角補正量候補値θｎ＿ｉｎｃ＿ｃａｎｄを求める。

なお、Ｋｎ＿ｃｍｐｌは、対角行列とは限らない。
次いで、ベクトルＲ（ｊ）（ｊ＝１，２，…ｒ−１）に対する係数をθｎ＿ｉｎｃ＿ｃａｎｄの第ｊ要素として、ベクトルＲ（ｊ）を線形結合したベクトルを求め、これをもって、第ｎノード相対床高さ偏差の補正量の候補値である第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄとする。
すなわち、次式４６によって、Ｚｎ＿ｉｎｃ＿ｃａｎｄを求める。

以上のごとく、第ｎノード床反力推定誤差Ｆｎ＿ｅｓｔｍ＿ｅｒｒから第ｎノード相対床高さ偏差補正量候補値Ｚｎ＿ｉｎｃ＿ｃａｎｄを求めるまでの処理を図５６のものから図５７のものに置き換えればよい。
上記以外の図５７の演算処理は、図５６の演算処理と同様である。
また、第５実施形態における床高さ偏差の推定処理のサブルーチン処理では、第ｎノードの子ノード数が４以上の場合の処理が必要となる。この処理は、前記３子ノード用床形状推定処理（図４１を参照）を拡張し、該３子ノード用床形状推定処理と同様に、いずれかの子ノードｊに対応する接地部位が浮いて発散してしまいそうな場合には、推定第ｊノード相対床高さ偏差Ｚｊ＿ｒｅｌ＿ｅｓｔｍを前回値Ｚｊ＿ｒｅｌ＿ｅｓｔｍ＿ｐに維持する（第ｊノード相対床高さ偏差補正量候補値Ｚｊ＿ｉｎｃ＿ｃａｎｄを０とする）。また、それ以外の子ノードの推定ノード相対床高さ偏差Ｚｋ＿ｒｅｌ＿ｅｓｔｍを前回推定第ｋノード相対床高さ偏差Ｚｋ＿ｒｅｌ＿ｅｓｔｍ＿ｐとＺｎ＿ｉｎｃ＿ｃａｎｄとの和にある所定のバイアス値ｃを加えた値に近づくように決定する。ただし、バイアス値ｃは、第ｎノードのすべての子ノードの推定ノード相対床高さ偏差の重み付き平均が０になるように決定される。
以上のごとく、床形状の推定処理を、４個以上の子ノードを持つノードがある場合にも、拡張することができる。ただし、このような処理を行なうよりは、４個以上の子ノードを持たないように、階層構造を決定した方が、演算処理は容易となる。
［第６実施形態］
次に、本発明の第６実施形態に係る移動ロボットの制御装置を図５８〜図６５を参照して説明する。図５８および図５９は、第６実施形態に係る脚式移動ロボット５１（本実施形態では２足移動ロボットとする）が後述する膝関節５６の部位を床に付けた状況（ひざまずいた状況）を側面示で示している。なお、図５８ではロボット５１を線状のリンクの連結体として模式化して示している。
本実施形態に係わる発明目的は、膝、肘、胴体など、ロボットの脚や腕の先端部以外の部分が床あるいは床に固定（設置）された対象物（床の延長物と考えられる対象物）と接触する状態において、該対象物を含む床から受ける反力（外力）を操作して、ロボットの姿勢を安定に制御することにある。
より具体的には、ロボットがひざまずいた状態やいすに座った状態における姿勢制御を可能とすることにある。
従来の公知の人間型ロボットには、例えば膝には、床反力を検出するセンサが備えられておらず、膝に作用する床反力を制御することはできなかった。例えば従来の人間型ロボットでは、図５８および図５９のようにロボットをひざまずかせた状態では、左右の膝に作用する床反力は、床の凹凸に依存（支配）され、また、床の凹凸に応じて、脚の関節角を調節することが行われていなかった。このため、床の凹凸によって、ロボットの床との接地部位が揺れるなど、その接地部位の接地状態が不安定なものとなる場合もあった。そして、このような状態では、ロボットの動作や作業を行おうとするとロボットの姿勢が不安定にある場合があった。
また、例えばロボットがひざまずいた状態から立ち上がる場合に、実際の床の傾きが目標歩容で想定していた傾きと異なっていた場合には、ロボットの重心位置が適切に制御されず、ロボットの上体の姿勢が傾いたまま立ち上がろうとしてしまうために、該ロボットが転倒してしまう恐れもあった。
また、ロボットが椅子に座ったまま、その姿勢を安定に保ちつつ動作や作業を行う場合において、椅子から立ち上がる時、あるいは椅子に座る時に、ロボットの姿勢を適切な姿勢に復元できるようにするために、ロボットの各脚の足平に作用する床反力のみならず、椅子からロボットの臀部に作用する反力も適切に制御できることが望ましい。
なお、概念的には、椅子などのように床に固定設置された対象物も床の一部と考えれば、前記反力も広義の床反力であると言える。
第６実施形態は、以上のごとき従来方式の問題点に鑑み、前記問題点を解消し、移動ロボットの膝、肘、胴体、臀部など、脚や腕の先端部以外の部分が、床あるいは床の延長物と考えられる対象物と接触して反力を受ける状態において、前記した階層型コンプライアンスの制御を使用しつつ、ロボットの脚や腕の先端部に作用する反力のみならず、脚や腕の先端部以外の部分に作用する反力をも適切に制御し、移動ロボットの姿勢を安定に保つことを可能としたものである。
以下に、図５８および図５９に示すロボット（２足移動ロボット）５１を例にとって、第６実施形態をより具体的に説明する。
まず、ロボット５１の機構的構成を説明すると、このロボット５１では、上体（基体）５３の下端部から２つの脚（リンク機構）５２（一方の脚は図示を省略している）が延設されている。また、上体５３の上部から２つの腕（リンク機構）５４（一方の腕は図示を省略している）が延設されている。各脚５２は、上体５３側の基端部（上体５３との連結箇所）と、中間部と、先端部とにそれぞれ電動モータなどのアクチュエータにより動作する股関節５５、膝関節５６、足首関節５７を備え、足首関節５７を介して各脚５２の先端部に足平５８が連結されている。同様に、各腕５４は、上体５３側の基端部（上体５３との連結箇所）と、中間部と、先端部とにそれぞれ電動モータなどのアクチュエータにより動作する肩関節５９、肘関節６０、手首関節６１を備え、手首関節６１を介して手６２が各腕５４の先端部に連結されている。この例では、各脚２の各関節５５、５６、５７はそれぞれ、例えば３自由度、１自由度、２自由度をもつ関節となっており、足平５８は上体５３に対して６自由度を有する。また、各腕５４の肩関節５９、肘関節６０、手首関節６１はそれぞれの自由度が、上体５３に対して手６２が６自由度以上の自由度を持つように構成されている。
なお、ロボット５１の上体５３には、図示を省略するが、前記第１実施形態などと同様の制御装置５０が搭載されている。また、上体５３の上端には、頭部６３も設けられている。
このロボット１では、図５８および図５９のようにロボット１をひざまずかせた状態（特定姿勢状態）では、各脚５２の足平５８と各膝関節５６の部位（より詳しくは、膝関節５６の近傍で膝関節５６と足首関節５７とを連結するリンク（脛リンク）の表面部。以下、単に膝という）と、各腕５４の手６２とが接地部位となる。そして、本実施形態では、図５９に示すように、接地部位である膝に床反力センサ９０（荷重センサ）を備えている。この床反力センサ９０の本体（センサ部）９２とスポンジ等の柔軟な部材（弾性体）９４とから構成され、本体９２を膝（脚のリンク）に固定し、その本体９２の外側を柔軟な部材（弾性体）９４によって覆っている。なお、膝におけるコンプライアンス制御の精度を良くするために、膝を柔軟な部材９４によって覆うだけでなく、その柔軟な部材９４の表面（接地面）を丸く凸面状に形成することが望ましい。こうすることにより、ロボット５１の目標運動の修正動作と床反力との関係の非線形性が小さくなるので、コンプラインス制御の制御特性が良くなる。なお、図示は省略するが、足平５８と足首関節５７とは６軸力センサなどの床反力センサとコンプラインス機構とを介して連結されている。同様に手６２と手首関節６１とは図示を省略する６軸力センサなどの床反力センサとコンプラインス機構とを介して連結されている。それらの連結構成は、公知のものでよい。
膝関節の接地部位の床反力センサ９０としては、以下のいずれでも良い。
１）接地部位の床への接触面（接地面）に垂直な方向の並進力だけを検出するセンサ
２）接地部位の床への接触面（接地面）に垂直な方向の並進力だけでなく該接触面に垂直な方向以外の方向の並進力も検出するセンサ
３）分布圧センサ
４）並進力以外にモーメントも検出するセンサ（例えば６軸力センサ）
また、荷重を直接的に検出するものでなくとも、前記柔軟な部材９４の変形（ひずみ）を検出する変位センサを用いても良い。
本実施形態においては、床反力センサ９０として、上記２）のセンサを用いることとする。コンプライアンス制御の精度をより一層高めるためは、床反力センサ９０として、上記３）または４）のセンサを用いれば良い。なお、膝の構造としては、本出願人が先に提案した特開２０００−６２７６０号公報に示すごとく、ばねを備えた保護パッド（膝の保護のためのパッド）に床反力センサを追加したものにしても良い。
本実施形態では、上記のようにひざまずいたロボット５１に対して、図６１に示す如く階層構造を設定している。すなわち、接地部位としての右足平５８、左足平５８、右膝、左膝、右手６２、左手６２をそれぞれ葉ノードである第１ノード、第２ノード、第３ノード、第４ノード、第５ノード、第６ノードに対応させる。また、第１ノードと第２ノードとを子ノードとして持つ第１２ノード、第３ノードと第４ノードを子ノードとして持つ第３４ノード、第５ノードと第６ノードを子ノードとして持つ第５６ノード、並びに第１２ノードと第３４ノードを子ノードとして持つ第１２３４ノードを中間ノードとして設定した。さらに、第１２３４ノードと第５６ノードを子ノードとして持つ第１２３４５６ノードを根ノードとして設定した。また、各ノードには、前記第１実施形態などと同様の方針によって、図示の如く、目標床反力中心点Ｑｎ（ｎ＝１，２，３，４，５，６，１２，３４，５６，１２３４，１２３４５６）が対応づけられる。なお、根ノードである第１２３４５６ノードの目標床反力中心点Ｑ１２３４５６は、目標全床反力中心点Ｐと同一である。
本実施形態では、制御装置５０は、前記図２に示したものと同様の機能的構成を有するものとされている。
この場合、本実施形態における歩容生成装置１００が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各手６２の目標位置姿勢軌道と各足平５８の目標位置姿勢軌道と各膝の目標位置軌道とから構成される。この場合、歩容生成装置１００は、想定した床面上で足平５８と手６２と膝が歩容に対する要求通りに接地するように、目標足平位置姿勢（目標第１、２接地部位位置姿勢）と目標手位置姿勢（目標第５、６接地部位位置姿勢）と目標膝位置（目標第３、４接地部位位置）を優先的に決定し、次いで目標ＺＭＰ（目標全床反力中心点）を各接地部位の目標接地点（あるいは目標接地線もしくは目標接地面）を含む最小凸多角形である支持多角形内に決定し、次いで、目標上体位置姿勢を、ロボット１の動力学モデルを使用して、目標足平位置姿勢と目標手位置姿勢と目標膝位置と目標ＺＭＰとを満足するように決定する。
なお、頭部６３が上体５３に対して可動である場合には、その頭部６３の目標位置姿勢軌道が目標運動に含まれる。
また、図２のロボット幾何学モデル（逆キネマティクス演算部）１１０の演算処理は、本実施形態では、後述するように、第１〜第３実施形態と異なる。
また、本実施形態では、階層型コンプライアンス動作決定部１１４は、その一部の処理手法が前記第１〜第３実施形態で説明したものと相違する。図６２は、本実施形態の階層型コンプラインス動作決定部１１４の機能的構成を示している。この機能的構成の処理において、第１〜第３実施形態と相違する主な点を説明すると、補償全床反力モーメント分配器１１４ａは、各中間ノードおよび根ノードのノード補償床反力モーメントだけでなく、葉ノードのうちの第ｎノード（ｎ＝１，２，５，６）のノード補償床反力モーメントＭｎ＿ｄｍｄ、すなわち、目標第ｎ接地部位床反力中心点（ｎ＝１，２，５，６）まわりの要求床反力モーメント（姿勢復元用の要求モーメント）も決定して出力する。これは、ロボット５１では、接地部位としての足平５８および手６２にその目標床反力中心点まわりにモーメント発生できるからである。なお、これらの要求モーメントを含めたノード補償床反力モーメントＭｎ＿ｄｍｄの決定手法は、前記第１および第２実施形態で説明した手法と同じでよい。また、本実施形態では、補償全床反力モーメント分配器１１４ａは、ノード補償床反力モーメントをそれぞれに対応する各ノードの目標床反力中心点まわりに発生させることによって修正される、目標第ｎ接地部位床反力中心点（ｎ＝１，２，５，６）まわりの修正目標第ｎ接地部位床反力モーメントＭｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，５，６）を、修正目標接地部位床反力の並進力成分Ｆｎ＿ｒｅｆｍｄｆｄ（ｎ＝１，２，３，４，５，６）と共に決定して出力する。これらは、機構変形補償量算出部１１４ｎで機構変形補償量を算出するために使用される。
また、補償角決定部（図６２の仮想線で囲んだ部分）は、本実施形態では、各中間ノードおよび根ノードの補償角だけでなく、葉ノードのうちの第ｎノード（ｎ＝１，２，５，６）のノード補償角θｎも決定して出力する。これらの第ｎノード補償角θｎ（ｎ＝１，２，５，６）を決定するθｎ決定部は、特開平１０−２７７９６９号公報に示した足平補償角決定部と同様の処理によって、第ｎノード補償角θｎ（ｎ＝１，２，５，６）を決定する。なお、これらのノード以外の各中間ノードおよび根ノードの補償角θｎ（ｎ＝１２，３４，５６，１２３４，１２３４５６）の決定手法は、前記第１および第２実施形態で説明した手法で同じでよい。
また、本実施形態における修正目標接地部位位置姿勢算出部１１４ｇは、前記第１および第２実施形態で説明した手法によって、各第ｎノード補償角θｎ（ｎ＝１２，３４，５６，１２３４，１２３４５６）を基にしたノードの回転操作を合成することによって、目標接地部位位置を修正し、さらに、第ｎノード補償角θｎ（ｎ＝１，２，５，６）を基に、第ｎ接地部位（ｎ＝１，２，５，６）の目標姿勢を修正し、これによって、各接地部位の修正目標接地部位位置姿勢を求める。
また、本実施形態では、床形状推定器１３０は、前記第３実施形態で説明した手法と同じ手法で推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，３，４，５，６）を求めることに加えて、第ｎ接地部位の接地部位床傾斜偏差θｆｎ（ｎ＝１，２，５，６）を推定する。この接地部位床傾斜偏差θｆｎの推定は、特開平１０−２７７９６９号公報に示した床形状推定器の足平床傾斜を推定する処理と同様の処理によって行なわれる。
本実施形態の階層型コンプラインス動作決定部１１４の、上記した以外の機能的構成の処理手法は、第３実施形態のものと同じである。
以上説明した相違点を除けば、本実施形態における制御装置５０の演算処理は、第１〜第３実施形態と同様である。
次に、本実施形態における前記ロボット幾何学モデル１１０の処理に関して詳説する。
本実施形態のロボット５１においては、上体５３から膝に至るまでの関節の自由度の数が３しかないので（股関節５５に３自由度あるだけなので）、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの和とをそれらの目標に維持したまま、左右の膝の高さの差だけを変えることはできない。言い換えると、強引に左右の膝の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。
同様に、厳密に上体位置姿勢と両膝の位置と両足平の水平位置と左右の足平の高さの和とをそれらの目標に維持したまま、左右の足平の高さの差だけを変えることはできない。言い換えると、強引に左右の足平の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。
同様に、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの差とをそれらの目標に維持したまま、左右の膝の高さの和だけを変えることはできない。言い換えると、強引に左右の膝の高さの和だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じることはなくとも、上体位置が目標位置からずれる。その結果、ロボット５１の全体の重心位置と慣性力とが目標値からずれ、ロボット５１の安定性が低下する。
すなわち、いずれにせよ、階層型コンプライアンス動作決定部１１４において決定された機構変形補償付き修正目標接地部位位置姿勢と目標上体位置姿勢とを同時に厳密に満足することはできないので、ロボット５１の図５８および図５９の姿勢状態において、該ロボット５１の足平５８もしくは膝と床との間でこじりあるいは滑りが生じたり、ロボット５１の全体の重心位置と慣性力とが目標値からずれ、ロボット５１の安定性が低下したりする。なお、接地部位のうちの手６２は、上体５３に対して６自由度以上の自由度を持つので、腕５４の手６２以外の部位で床に接地するような姿勢をロボット５１に採らせない限り、手６２と床との間でこじりなどの滑りが発生することは無い。
そこで、本実施形態においては、ロボット５１の接地部位のうちの膝および足平５８と床との間で極力こじりあるいは滑りが生じず、かつ上体５３の重心位置（特に水平位置）が極力変わらないようにしつつ、左右の膝の高さの差の変化などに応じて上体５３の姿勢、あるいは姿勢と位置とを修正することとする。
図６３（ａ），（ｂ）に、その上体５３の姿勢の修正動作の例を示す。なお、図６３（ｃ）については後述する。これらの図６３（ａ），（ｂ），（ｃ）では、腕５４と頭部６３は図示を省略している。
上体５３の姿勢の修正動作の１つは、図６３（ａ）に示すようにロボット５１がひざまずいた状態から、コンプライアンス制御による左右の膝の高さの差の変化に応じて、図６３（ｂ）に示すように、おおよそ上体５３の重心Ｇを通る体幹軸を回転軸として、上体５３を回転させる（矢印ｙ１のように回転させる）動作である。
また、図６３のようなロボット５１の膝付き姿勢においては、左右の足平５８，５８の高さの差の変化によって、左右の膝の高さの差はおおよそ左右の足平５８，５８の高さの差の変化の半分だけ変化する。そこで、コンプラインス制御による左右の足平５８，５８の高さの差の変化に応じて、左右の膝の高さの差の変化に応じた上体５３の姿勢修正と同様の動作を、左右の足平の高さの差の変化の場合に較べて半分の量だけ行なうようにする。
さらに、このように上体５３の姿勢を修正するとき、接地部位（膝および足平５８）と床との間で極力こじりあるいは滑りが生じず、かつ上体５３の重心Ｇ位置（特にその水平位置）、あるいはその重心Ｇと全床反力中心点と結ぶ線分の傾きが極力変わらないようにしつつ、コンプラインアンス制御による左右の膝の高さの和の変化に応じて上体５３の位置と姿勢とを修正することとする。
図６４に、左右の膝の高さの和の変化に応じて上体５３の位置と姿勢とを修正する動作を視覚的に示す。すなわち、点線で示すロボット５１の姿勢から、実線で示すロボット５１の姿勢のように、コンプライアンス制御により両膝を下げる動作に伴い、上体５３の下端部（あるいは腰部）を矢印ｙ３で示す如く前方側にずらし、上体５３の傾きを矢印ｙ２で示す如く後ろに（上体５３の起立方向に）ずらすようにする。すなわち、上体５３の重心Ｇ位置（あるいは上体５３の所定の代表点の位置）、特にその水平位置を維持しつつ上体５３を後ろに傾けるようにする。あるいは、該重心Ｇと目標全床反力中心点Ｐとを結ぶ線分の傾きを維持しつつ上体５３を後ろに傾けるようにする。また、コンプラインス制御により両膝を上げる動作に伴い、上記と逆に、上体５３の下端部（あるいは腰部）を後ろにずらし、上体の傾きを前にずらすようにする。すなわち、上体重心Ｇ位置（あるいは上体の所定の代表点の位置）、特にその水平位置を維持しつつ上体５３を前に傾けるようにする。あるいは、該重心Ｇと目標全床反力中心点Ｐとを結ぶ線分の傾きを維持しつつ上体５３を後ろに傾けるようにする。なお、図６４中Ｑ１”，Ｑ３”は、それぞれ上記のように上体５３の位置姿勢を修正した後の足平５８の目標床反力中心点、膝の目標床反力中心点を示しており、この例では、Ｑ１”は修正前の足平５８の目標床反力中心点Ｑ１と同じである。
以上が、上体５３の姿勢や位置の修正動作の概要である。
本実施形態におけるロボット幾何学モデル（逆キネマティクス演算部）１１０の処理機能を図６５のブロック図に示す。
逆キネマティクス演算部１１０では、まず、機構変形補償入り修正目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，６）と歩容生成装置１００が決定した各足平５８および膝の目標接地部位位置姿勢とを基に、歩容生成装置１００が決定した左右の膝の目標位置に対する、左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄおよび左右の膝の高さの和の修正量Ｚｋｎｅｅｓｕｍｍｄｆｄと、歩容生成装置１００が決定した左右の足平の目標位置に対する、左右の足平５８，５８の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄを求める。
具体的には、以下の式４７、４８、４９によって、左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄ、左右の膝の高さの和の修正量Ｚｋｎｅｅｓｕｍｍｄｆｄ、左右の足平の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄを求める。

なお、式４７〜４９の「部位位置」はより正確には、その「部位位置」の高さ成分（鉛直方向成分）である。
次いで、左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄ、左右の膝の高さの和の修正量Ｚｋｎｅｅｓｕｍｍｄｆｄおよび左右の足平の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄを基に、上体位置姿勢修正量を求める。
上体位置姿勢修正量は、膝高さ差用上体位置修正量Ｘｂｋｎｅｅｄｉｆｆｍｄｆｄ、膝高さ差用上体姿勢修正量θｂｋｎｅｅｄｉｆｆｍｄｆｄ、膝高さ和用上体位置修正量Ｘｂｋｎｅｅｓｕｍｍｄｆｄ、膝高さ和用上体姿勢修正量θｂｋｎｅｅｓｕｍｍｄｆｄおよび足平高さ差用上体位置修正量Ｘｂｆｏｏｔｄｉｆｆｍｄｆｄ、足平高さ差用上体姿勢修正量θｂｆｏｏｔｄｉｆｆｍｄｆｄから成る。具体的にこれらの値は次のように求められる。
まず、左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄに応じて、これによって接地部位（膝および足平５８）と床との間でこじりあるいは滑りが生じないための、膝高さ差用上体位置修正量Ｘｂｋｎｅｅｄｉｆｆｍｄｆｄと膝高さ差用上体姿勢修正量θｂｋｎｅｅｄｉｆｆｍｄｆｄとを、左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄとその瞬間（現在時刻）におけるロボット５１の目標姿勢（目標運動）とを基に幾何学演算によって求める。
より具体的には、例えば、次式５０によって、Ｘｂｋｎｅｅｄｉｆｆｍｄｆｄとθｂｋｎｅｅｄｉｆｆｍｄｆｄとを求める。ただし、ＫｘｋｎｅｅｄｉｆｆとＫｔｈｋｎｅｅｄｉｆｆとは、ロボット５１の目標姿勢に応じた比例係数である。

これ以外にも、あらかじめ、いくつかの代表的なロボット５１の目標姿勢に対して、左右の膝の高さの差の修正量（あるいは第３４ノード補償角θ３４）と上体位置姿勢の修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄを基にＸｂｋｎｅｅｄｉｆｆｍｄｆｄおよびθｂｋｎｅｅｄｉｆｆｍｄｆｄを求めても良い。なお、本実施形態では、左右の膝の高さの差の修正量に対して、上体５３をその体幹軸まわり回転させて上体姿勢を修正するので、膝高さ差用上体位置修正量Ｘｂｋｎｅｅｄｉｆｆｍｄｆｄは０で良い。
次いで、上記と同様に、左右の足平５８，５８の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄに応じて、これによって接地部位（膝および足平５８）と床との間でこじりあるいは滑りが生じないための、足平高さ差用上体位置修正量Ｘｂｆｏｏｔｄｉｆｆｍｄｆｄと足平高さ差用上体姿勢修正量θｂｆｏｏｔｄｉｆｆｍｄｆｄとを、左右の足平の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄとその瞬間（現在時刻）におけるロボット５１の目標姿勢（目標運動）とを基に幾何学演算によって求める。
より具体的には、例えば、次式５１によって、Ｘｂｆｏｏｔｄｉｆｆｍｄｆｄとθｂｆｏｏｔｄｉｆｆｍｄｆｄとを求める。ただし、ＫｘｆｏｏｔｄｉｆｆとＫｔｈｆｏｏｔｄｉｆｆとは、ロボット５１の目標姿勢に応じた比例係数である。前述したように足平５８の高さ差の影響は膝の高さの差の影響のほぼ半分であることから、ＫｘｆｏｏｔｄｉｆｆとＫｔｈｆｏｏｔｄｉｆｆとは、それぞれ、ＫｘｋｎｅｅｄｉｆｆとＫｔｈｋｎｅｅｄｉｆｆの２分の１とする。

なお、本実施形態では、左右の足平５８，５８の高さの差の修正量に対して、上体５３をその体幹軸まわり回転させて上体姿勢を修正するので、足平高さ差用上体位置修正量Ｘｂｆｏｏｔｄｉｆｆｍｄｆｄは０で良い。
次いで、上記と同様に、左右の膝の高さの和の修正量Ｚｓｕｍｍｄｆｄに応じて、これによって接地部位（膝および足平）と床との間でこじりあるいは滑りが生じないための、膝高さ和用上体位置修正量Ｘｂｓｕｍｍｄｆｄと膝高さ和用上体姿勢修正量θｂｓｕｍｍｄｆｄとを、左右の膝の高さの和の修正量Ｚｓｕｍｍｄｆｄとその瞬間（現在時刻）におけるロボット５１の目標姿勢を基に幾何学演算によって求める。
これ以外にも、あらかじめ、いくつかの代表的な目標姿勢に対して、左右の膝の高さの和の修正量と上体姿勢の修正量の関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの和の修正量Ｚｓｕｍｍｄｆｄとを基にＸｂｓｕｍｍｄｆｄとθｂｓｕｍｍｄｆｄとを求めても良い。
なお、次のように、上体位置姿勢の修正量を決定してもよい。すなわち、左右の膝の高さの差を、その差を生じさせる第３４ノード補償角θ３４に換算し、左右の膝の高さの和を、その和を生じさせる第１２３４ノード補償角θ１２３４に換算し、左右の足平の高さの差を、その差を生じさせる第１２ノード補償角θ１２に換算する。そして、これらの換算された補償角を基に、幾何学演算によって上体位置姿勢修正量を決定するようにしても良い。あるいは、あらかじめ、いくつかの代表的なロボット５１の目標姿勢に対して、換算された補償角と上体位置姿勢修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと換算された補償角とを基に、上体位置姿勢修正量を決定するようにしても良い。
次いで、目標上体位置姿勢を上体位置姿勢修正量だけ移動（回転移動と平行移動）して、こじり補正入り目標上体位置姿勢を求める。具体的には、体幹軸（あるいは所定の回転軸（ほぼ鉛直面内の回転軸））まわりに目標上体位置姿勢を膝高さ差用上体姿勢修正量と足平高さ差用上体姿勢修正量との和（θｂｋｎｅｅｄｉｆｆｍｄｆｄ＋θｂｆｏｏｔｄｉｆｆｍｄｆｄ）だけ回転移動し、さらにこれを左右軸まわりに膝高さ和用上体姿勢修正量θｂｆｏｏｔｓｕｍｍｄｆｄだけ回転移動し、さらにこれをロボット５１の前後方向で膝高さ差用上体位置修正量と膝高さ和用上体位置修正量と足平高さ差用上体位置修正量の和（Ｘｂｋｎｅｅｄｉｆｆｍｄｆｄ＋Ｘｂｋｎｅｅｓｕｍｍｄｆｄ＋Ｘｂｆｏｏｔｄｉｆｆｍｄｆｄ）だけ平行移動することによって、こじり補正入り目標上体位置姿勢を求める。
次いで、こじり補正入り目標上体位置姿勢と機構変形補償入り目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，６）とを基に、ロボット５１の関節変位指令を求める。
以上のごとく逆キネマティクス演算部１１０の処理が実行される。
すなわち、第６実施形態は、ロボット５１の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合には、階層型コンプライアンス動作によって目標接地部位位置姿勢を修正するだけでなく、上体位置の点（上体の代表点）あるいは上体を含む複数部位の重み付き平均位置（この場合の重みは各部位の質量比となっていることが望ましい）の点（全体重心など）、特にその点の水平位置を保持するように、あるいは、その点と目標全床反力中心点（目標ＺＭＰ）とを結ぶ線の角度を目標歩容における角度にできるだけ保持するように、目標上体位置姿勢も修正するようにした。
また、別の言い方をすれば、ロボット５１の全体の姿勢制御の安定性を高めるために、目標全床反力中心点（目標ＺＭＰ）まわりに発生する実床反力モーメントの制御性を優先し、ある所定の接地部位Ａ（ここでは膝）の相対高さまたは傾斜角度の操作量、あるいはある所定の接地部位Ａを子孫ノードとして持つノードの相対高さまたは傾斜角度の操作量としての補償高さおよび補償角のうちの少なくともいずれか一方を決定する手段と、前記補償高さおよび補償角のうちの少なくともいずれか一方に応じて、上体位置あるいは上体を含む複数部位の前記重み付き平均位置を概ね目標歩容での位置に保持しつつ、上体の姿勢および位置のうちの少なくともいずれか一方の修正量を決定する手段と、前記所定の接地部位Ａを除く所定の接地部位Ｂ（ここでは足平）の位置および姿勢のうちの少なくともいずれか一方と、前記修正量によって修正した上体位置姿勢とに基づいて、関節変位を決定する逆キネマティクス演算手段を備えるようにした。
なお、第６実施形態では、ロボット５１の上体５３の体幹軸まわりに上体姿勢を回転させる代わりに、図６４（ｃ）のように、鉛直軸を回転軸として、ロボット５１の腰（上体５３の下端部）を中心に上体５３を回転させるようにしても良い。ただし、この場合には、上体５３の重心Ｇ位置の左右方向へのずれが大きくなる。あるいは、上体５３の体幹軸と鉛直軸との中間的な軸を回転軸として上体姿勢を回転させるようにしても良い。あるいは上体５３の位置と姿勢とを両膝の高さの差の修正量や両足平５８，５８の高さの差の修正量に応じて同時に修正しても良い。また、上体５３の重心Ｇ位置が変わらないようにする代わりに、ロボット５１の全体重心位置あるいは上体５３の代表点位置が代わらないように上体５３の位置姿勢を修正しても良い。
いずれにせよ、コンプライアンス動作などによるロボット５１の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合に、接地部位のこじりなどの滑りが生じないように、上体５３の位置および姿勢のうちの少なくともいずれか一方を目標歩容の位置姿勢から修正するようにすればよい。また、上体５３の位置および姿勢のうちの少なくともいずれか一方を修正する代わりに、上体５３以外の所定の部位の位置および姿勢のうちの少なくともいずれか一方を修正するようにしても良い。
［第７実施形態］
次いで、本発明の第７実施形態に係る移動ロボットの制御装置を説明する。
第６実施形態においては、接地部位のこじりなどの滑りを防止し、かつロボット５１の全体重心位置と慣性力とができるだけずれないようにするために、上体位置姿勢を修正するようにした。本実施形態（第７実施形態）においては、接地部位のこじりなどの滑りをある程度までは許容し、優先的にロボット５１の全体重心位置と慣性力とがずれないように、関節変位（股関節５５と膝関節５６の変位）あるいは接地部位位置姿勢を修正するようにした。
すなわち、本実施形態においては、逆キネマティクス演算部１１０は、第６実施形態で説明した図６５の処理に代えて、図６６のブロック図で示す処理を行なうようにした。この相違点を除けば、本実施形態は、第６実施形態と同じである。
本実施形態の逆キネマティクス演算部１１０を図６６を参照して説明すると、まず、各機構変形補償入り修正目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，６）（詳しくは位置姿勢のうちの位置）と各目標第ｎ接地部位位置姿勢（ｎ＝１，２，…，６）（詳しくは位置姿勢のうちの位置）とを基に、前記式４７、式４８および式４９にしたがって左右の膝の高さの差の修正量Ｚｋｎｅｅｄｉｆｆｍｄｆｄ、左右の膝の高さの和の修正量Ｚｋｎｅｅｓｕｍｍｄｆｄ、および左右の足平の高さの差の修正量Ｚｆｏｏｔｄｉｆｆｍｄｆｄを求め、さらに、左右の膝の高さの差を、その差を生じさせる第３４ノード補償角θ３４に換算する。同様に、左右の膝の高さの和を、その和を生じさせる第１２３４ノード補償角θ１２３４に換算し、左右の足平の高さの差を、その差を生じさせる第１２ノード補償角θ１２に換算する。
次いで、換算された各ノード補償角を基に、関節変位修正量を決定する。これは次のように行なわれる。まず、目標歩容の上体位置姿勢を固定し、且つ目標全床反力中心点（目標第１２３４５６ノード床反力中心点（図６１の点Ｐ））とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）とを結ぶ線分の角度が変わらないようにしつつ、関節変位を修正した場合を想定し、この場合に、各接地部位（膝および足平５８）に対応した第ｎノード（ｎ＝１２，３４，１２３４）の傾斜角度（補償角）が単位摂動量になるための第ｊ関節変位修正量（ｊ＝１，２，…，関節の総数）を求め、これを第ｎノード補償角に対する第ｊ関節変位の感度Ｌｎ＿ｊとする。
より一般的には、目標歩容の関節変位を修正した場合に、目標全床反力中心点（目標第１２３４５６ノード床反力中心点）とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）を結ぶ線分と、各接地部位（膝および足平５８）に対応した第ｎノード（ｎ＝１２，３４，１２３４）の傾斜角度との間の相対角度が単位摂動量になるための第ｊ関節変位修正量（ｊ＝１，２，…，関節の総数）を求め、これを第ｎノード補償角に対する第ｊ関節変位の感度Ｌｎ＿ｊとする。
具体的には、Ｌｎ＿ｋｎｅｅ＿ｒを第ｎノード補償角（ｎ＝１２，３４，１２３４）に対する右膝関節変位の感度、Ｌｎ＿ｋｎｅｅ＿ｌを第ｎノード補償角に対する左膝関節変位の感度、Ｌｎ＿ｈｉｐ＿ｒを第ｎノード補償角に対する右股ピッチ関節変位の感度、Ｌｎ＿ｈｉｐ＿ｌを第ｎノード補償角に対する左股ピッチ関節変位の感度とすると、図５８および図５９に示すロボット５１の膝付き姿勢においては、各感度は次式５２のように設定する。

ただし、ａ１２、ａ３４は所定の定数である。
次いで、次式５３〜５６により各関節変位修正量を決定する。ただし、θｋｎｅｅ＿ｒは、右膝関節変位修正量、θｋｎｅｅ＿ｌは、左膝関節変位修正量、θｈｉｐ＿ｒは、右股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）、θｈｉｐ＿ｌは、左股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）とする。

なお、他の関節変位修正量も同様に求めるが、説明を容易にするために、ここでは省略する。
すなわち、第１２３４補償角θ１２３４に対しては、図６７に示すごとく両膝関節５６の変位（角度）をθ１２３４と同量だけ修正する。また、第１２補償角θ１２に対しては、右膝関節５６の変位（角度）をθ１２に比例して修正しつつ、左膝関節５６の変位（角度）を右膝関節５６の変位の修正量に−１を乗じた量だけ修正する。つまり右膝関節５６の変位（角度）を図６７に示すごとく修正し、左膝関節５６の変位（角度）はその逆に修正する。また、第３４補償角θ３４に対しては、右股関節５５のピッチ方向の変位（角度）をθ３４に比例して修正し、左股関節５５のピッチ方向の変位（角度）を右股関節５５のピッチ方向の変位の修正量に−１を乗じた量だけ修正するようにした。つまり右股関節５５のピッチ方向の変位（角度）を図６８に示すごとく修正し、左股関節５５のピッチ方向の変位（角度）はその逆に修正する。なお、図６７のＱ１，Ｑ１”はそれぞれ上記のような関節変位の修正前の足平５８の目標床反力中心点、修正後の足平５８の目標床反力中心点を示している。また、図６８のＱ３，Ｑ３”はそれぞれ上記のような関節変位の修正前の膝の目標床反力中心点、修正後の膝の目標床反力中心点を示している。
このような関節変位の修正により、目標全床反力中心点（目標第１２３４５６ノード床反力中心点）とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）を結ぶ線分と、接地部位（膝および足平５８）に対応した第ｎノード（ｎ＝１２，３４，１２３４）の傾斜角度との間の相対角度が、前記換算された各ノード補償角θ１２，θ２３，θ１２３４だけ変化する。したがって、実床反力が忠実に制御され、ロボット５１の姿勢安定性および接地性が向上する。
上記の如く関節変位指令（目標関節変位）を直接修正する代わりに、略同一の目標関節変位を生じさせるために優先的に位置姿勢を修正する接地部位（具体的には、足平５８、手６２）の修正後の目標位置姿勢である優先修正目標接地部位位置姿勢を決定するようにし、決定された優先修正目標接地部位位置姿勢を基に逆キネマティクス演算によって関節変位指令を決定するようにしても良い。
なお、関節の自由度不足のため、目標上体位置姿勢とすべての修正目標接地部位位置姿勢とを満足するように逆キネマティクス演算によって関節変位を決定することはできないので、逆キネマティクス演算を行なうときには、すべての修正目標接地部位位置姿勢のうちの一部の修正目標接地部位位置姿勢を用いることとなる。この一部の修正目標接地部位位置姿勢を前記優先修正目標接地部位位置姿勢と呼ぶ。
たとえば、図６７の状況においては、第１２３４ノード補償角θ１２３４に対しては、膝を中心として目標足平位置を回転移動させた位置を優先修正目標足平位置と決定すれば良い。
なお、第６実施形態および第７実施形態では、一例として、ロボット５１のひざまずき状況におけるコンプライアンス動作について説明したが、ロボットが椅子に座った状況でのコンプライアンス動作に適応する場合には、次のようにすれば良い。以下に、それを第８実施形態として説明する。
［第８実施形態］
図７０は本実施形態のロボットの要部の構成を示している。このロボット７１は、臀部７２の底面部の左右にそれぞれ床反力を検出する床反力センサ７３，７３（荷重センサ。例えば６軸力センサ）を備える。なお、左右に床反力センサ７３，７３を設ける代わりに、臀部７２の底面部の左右に加わる力の合力を検出する１つの床反力センサを備えるようにしてもよい。
この場合、また、床反力センサ７３，７３の外側は、図示の如くスポンジ等の柔軟な部材（弾性体）７４によって覆われている。なお、臀部７２の接地部位におけるコンプライアンス制御の精度を良くするために、柔軟な部材によって覆うだけでなく、図示の如く柔軟な部材（弾性体）７４の表面（接地面）を丸く凸面状に形成することが望ましい。特に、ロボット７１がその臀部７２を介して椅子などに座った状態において床反力センサ７３，７３の検知位置（センサ本体の位置）と上記部材７４の凸面の頂点との水平位置を一致させるように、部材７４を設けておくことが望ましい。このようにすることで、ロボット７１の接地部位の位置姿勢の修正動作と床反力との関係の非線形性が小さくなるので、ロボット７１のコンプライアンス制御などの制御特性が良好になる。
なお、ロボット７１では、臀部７２の左右の側部から、脚（リンク機構）５２，５５が延設されている。脚５５，５５の構造は、例えばその関節を含めて前記第６実施形態のものと同一とされている。従って、脚５，５５についての第６実施形態と同一の参照符号を用い、説明を省略する。ただし、本実形態では、脚５５の膝に床反力センサを備えていなくてもよい。
また、図示のように、臀部７２の上側には、胴体（上体）７７が設けられ、この胴体７７の上部の両側部から肩関節７８，７８を介して腕７９，７９が延設されている。なお、腕７９は、前記第６実施形態のロボット５１のものと同一構造でもよい。さらに、胴体７７は、関節８０を介して臀部７２に連結されている。この場合、関節８０は、臀部７２に対して胴体う７７をヨー方向に旋回させる胴体旋回用関節８０ａと、胴体７７を臀部７２に対して前後および左右方向に傾斜させる胴体屈曲用関節８０ｂとから構成されている。ロボット７１に上述のように備えられた各関節は、図示しないアクチュエータにより動作する。また、臀部７２または胴体７７には、図示を省略するが、前記第１実施形態などと同様の制御装置５０が搭載されている。
本実施形態では、臀部７２を介して椅子などに腰掛けるロボット７１に対して、図７０に示す如く階層構造を設定すればよい。すなわち、接地部位としての右足平５８、左足平５８、臀部７２の底面部の右側部（右側床反力センサ７３の取り付け部位）、臀部７２の底面部の左側部（左側床反力センサ７３の取り付け部位）をそれぞれ葉ノードである第１ノード、第２ノード、第３ノード、第４ノードに対応させる。また、第１ノードと第２ノードとを子ノードとして持つ第１２ノード、第３ノードと第４ノードを子ノードとして持つ第３４ノードを中間ノードとして設定し、第１２ノードと第３４ノードを子ノードとして持つ第１２３４ノードを根ノードとして設定した。また、各ノードには、前記第１実施形態などと同様の方針によって、図示の如く、目標床反力中心点Ｑｎ（ｎ＝１，２，３，４，１２，３４，１２３４）が対応づけられる。なお、根ノードである第１２３４ノードの目標床反力中心点Ｑ１２３４は、目標全床反力中心点Ｐと同一である。
本実施形態では、制御装置５０は、前記図２に示したものと同様の機能的構成を有するものとされている。
この場合、本実施形態における歩容生成装置１００が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各足平５８の目標位置姿勢軌道と臀部７２の目標位置姿勢軌道とから構成される。目標運動のうちの上体位置姿勢軌道は、本実施形態では、胴体７７の位置姿勢軌道を意味する。なお、目標運動には、各腕７９の先端部の位置姿勢軌道も含まれる。また、目標全床反力中心点Ｐは、実際の床面上にはなく、空中の仮想面上にある。
また、本実施形態では、階層型コンプライアンス動作決定部１１４は、その機能的構成要素は、前記第６実施形態のものと同様である（図６２を参照）。ただし、本実施形態では、補償全床反力モーメント分配器は、図７０に示した階層構造における各中間ノードおよび根ノードのノード補償床反力モーメントと、各足平５８に対応する葉ノードのノード補償床反力モーメントとを決定して出力する。また、補償角決定部は、図７０に示した階層構造における各中間ノードおよび根ノードのノード補償角と、各足平５８に対応する葉ノードのノード補償角とを決定して出力する。この場合、これらのノード補償床反力モーメントおよびノード補償角の基本的な決定手法は、前記第１〜第３実施形態や第６実施形態で説明した手法と同じでよい。また、床形状推定器は、前記第６実施形態の場合と同様に、前記第３実施形態で説明した手法と同じ手法で推定第ｎ床高さ偏差Ｚｆｎ＿ｅｓｔｍ（ｎ＝１，２，３，４）を求めることに加えて、各足平５８の接地部位床傾斜偏差θｆｎ（ｎ＝１，２）を推定する。
また、制御装置５０の機能的構成要素であるロボット幾何学モデル（逆キネマティクス演算部）は、前記第６実施形態と同様の手法によって、基本的には、前記機構変形補償入り修正目標接地部位位置姿勢と目標上体位置姿勢とを基に、臀部７２の接地部位と足平５８（接地部位）とでこじりなどの滑りが発生しないように、それらの接地部位の位置姿勢を修正すると共に、上体（胴体）７７の位置姿勢を修正する。
以上説明した以外の制御装置５０の処理は、前記第６実施形態と同様でよい。
次に、以上説明した実施形態に係わるいくつかの変形態様を説明する。
床反力センサによって直接的に検出できない実ノード床反力は、床反力センサによって直接的に検出される他の実接地部位床反力、加速度センサの検出値、傾斜計の検出値などを用いて、オブザーバにより推定するか、もしくは、簡易的な代数関係によって推定すればよい。例えば、階層型コンプライアンス動作は、目標全床反力中心点まわりに接地部位を回転させる回転型のコンプライアンス動作であるので、コンプライアンス動作を行なっても、ロボットの全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）は、ロボットの目標歩容（目標運動）における全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）にほぼ一致する。したがって、すべての実接地部位床反力鉛直成分の和は、ロボットの目標歩容（目標運動）における全体重心の鉛直加速度と重力加速度との和にロボットの全体質量を乗じた値にほぼ一致する。
よって、実第ｎノード床反力が直接的に検出できない場合、まず、第ｎノードを先祖ノードに持たず、かつ、第ｎノードそのものでないすべての葉ノードの実床反力（実接地部位床反力）の和（以降、これを実第ｎノード外床反力と呼ぶ）を求める。
次いで、次式５７によって、実第ｎノード床反力の推定値である推定第ｎノード床反力を求める。

上体などに加速度センサを備えている場合には、加速度センサの検出値と、目標歩容および実関節変位のうちの少なくともいずれか一方によりロボット全体の重心加速度（以降、推定全体重心加速度と呼ぶ）を推定し、次式５８によって、実第ｎノード床反力の推定値である推定第ｎノード床反力を求めても良い。

例えば、前記第６実施形態において、膝の床反力を検知する床反力センサが備わっていない場合、あるいは、膝の床反力を検知する床反力センサが故障した場合には、推定第３４ノード床反力を次式５９によって求める。

さらに、推定第３４ノード床反力を実第３４ノード床反力の代わりに用いて、コンプライアンス動作処理によって第１２３４ノード補償角θ１２３４を決定し、また床形状推定処理によって推定第１２３４ノード相対床高さ偏差Ｚ１２３４ｒｅｌ＿ｅｓｔｍを推定する。
第３４ノード補償角θ３４の決定では、想定した床通りであると仮定して、機構変形補償入り修正目標第３接地部位位置姿勢、機構変形補償入り修正目標第４接地部位位置姿勢と推定第３４ノード床反力を基に、実第３ノード床反力の推定値である推定第３ノード床反力と実第４ノード床反力の推定値である推定第４ノード床反力とを求める。
さらに、推定第３ノード床反力と推定第４ノード床反力を実第３ノード床反力と実第４ノード床反力の代わりに用いて、コンプライアンス動作処理によって第３４ノード補償角θ３４を決定する。
ロボットの姿勢の復元要求モーメント（補償全床反力モーメント）を基に、前述の如く復元要求モーメントを加えることと等価になるように目標ノード床反力中心点まわりの修正目標ノード床反力モーメントを決定する代わりに、復元要求モーメント（補償全床反力モーメント）を基に、目標ノード床反力中心点を修正した修正目標ノード床反力中心点を決定しても良い。この場合、目標ノード床反力中心点まわりの目標ノード床反力モーメントは修正しないで０のままとする。
また、第ｎノード床反力の内で制御できなかった成分（予想される、または検出された制御偏差成分）を補うために、この成分に応じて、第ｎノードの親ノードの目標床反力を修正しても良い。すなわち、実第ｎノード床反力と目標第ｎノード床反力との差の前回制御周期の値あるいは前記差をローパスフィルタに通した値を第ｎノードの目標床反力に付加しても良い。
床形状の推定処理においては、コンプライアンス制御用のツリー構造と異なるツリー構造を設定してもよい。そのツリー構造は、根ノードと葉ノードとからなる２層のものであってもよい。また、各ノードの重みもコンプライアンス制御用のものと異なるものとしてもよい。ノード床反力が０となるような時期（そのノードに属する全ての接地部位、またはそのノードに対応する接地部位が空中を移動する時期）でそのノードの重みが０であれば良い。
また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノード重みを決定し、これを要素とするベクトルを、床形状推定に用いる重みとして用いても良い。
床形状推定の処理で、各点（各接地部位）の推定相対床高さ偏差を求める際に、実床反力と機構コンプライアンスモデルと姿勢センサの検出値とを用いる代わりに、コンプライアンス機構などの変形量を検出する変形量検出器を備え、その変形量検出器の検出値と姿勢センサの検出値とを用いるようにしてもよい。
床形状を相対的に推定するだけでなく、グローバルに推定（グローバル座標系での床形状の推定）したい場合には、次のようにしてもよい。
１）ロボットの上体のグローバル座標系での高さ（以下、グローバル高さという）を検知できる場合における床のグローバル高さの推定：
上体などロボットのある所定部位のグローバル高さを検出する検出器（加速度センサを用いた上体高さ推定装置（例えば本願出願人によるＰＣＴ／ＪＰ０３／０５４４８号に記載されている推定装置）あるいは視覚センサ等の外界センサなど）を用いる。推定された上体のグローバル高さと、関節変位（実関節変位あるいは目標関節変位）、実上体姿勢傾斜の検出値および実床反力の検出値を基に、推定床高さ偏差のグローバル座標系での値を求める。従って、根ノードの推定床高さ偏差は、すべての接地部位のグローバルな推定床高さ偏差の重み付き平均値としての意味を持つこととなる。
２）上体のグローバル高さがわからない状況での床のグローバル高さの推定：
ある推定第ｎ床高さ偏差のグローバル値（グループ内相対値ではなくグローバル座標系における偏差）がわかっているならば、それと推定各相対床高さ偏差を基に、すべての点（接地部位）の推定床高さ偏差のグローバル値を求める。ロボットの移動のスタート点での推定第ｎ床高さ偏差のグローバル値がわかっていれば、ロボットの移動を続けることで、逐次新しく接地した接地部位の床高さ偏差のグローバル値を求められるので、これを記憶することにより、ロボットの移動軌跡上での床高さのマップを作成できる。
また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノード重み（各ノードの重みの値を修正したもの）を決定し、これを、床形状推定において、実第ｎノード相対床高さ等を定義するために用いる重みとしても良い。また、床形状推定において、実第ｎノード相対床高さ等を定義するために用いる重みは、目標床反力分配器が決定した前述の重みと必ずしも同一のものにする必要はない。
補足すると、実第ｎノード相対床高さ等を定義するための重みは、目標床反力分配器が決定する重みに必ずしも一致させなくてはならない訳ではないが、一致させるならば、（この場合、自動的に、根ノードの目標床反力中心点は目標全床反力中心点に一致する、）コンプライアンス動作用の第ｎノード補償角（ｎ＝１，２，…）に第ｎノード推定床傾斜偏差を付加するだけで、床形状偏差の影響を打ち消すことができるので、床形状偏差の影響を打ち消すための演算量を低減することができる。
さらに、推定床形状偏差を床形状の地図情報として記憶しておき、次回同一場所を歩行する時に、記憶しておいた床形状の地図情報を基に目標歩容を生成するようにしても良い。
尚、前述の各実施形態における演算処理機能を示すブロック線図もしくはフローチャートにおける演算処理順序を等価変形をしても良い。
また、本発明は、移動ロボットに限らず、アクティブサスペンションを持つ車輪型移動体にも応用することが可能である。

以上の如く、本発明は、床反力を適切に制御して、高い安定性で円滑な動作を可能とする脚式移動ロボットなどの移動体を提供し得るものとして有用である。

Claims (26)

1. 互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
   前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類し、
   複数の接地部位を子孫ノードとして持つノードである第Ｃノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量を該第Ｃノード毎に決定し、その決定した修正量をすべての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とする移動体の制御装置。
2. 前記ツリー構造における各ノードに重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、
   前記ノード動作制御手段は、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノードの目標高さは前記目標運動における目標高さに維持されることを特徴とする請求の範囲第１項に記載の移動体の制御装置。
3. 互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
   前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ツリー構造における各ノードに重みが設定されており、
   少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、前記目標運動に、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、
   前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、前記ノード動作制御手段が、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記目標運動を決定したとき、該修正目標運動における該第Ｃノードの目標高さは前記目標運動における目標高さに維持されることを特徴とする移動体の制御装置。
4. 前記ツリー構造において子ノードを持つ各ノードに対し、該各ノードの全ての子ノードの重みは、その全ての子ノードの重みの総和が１となるように設定されていることを特徴とする請求の範囲第２項または第３項に記載の移動体の制御装置。
5. 前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第Ｄノードとしたとき、前記重みを設定する手段は、各第Ｄノードの重み、または該第Ｄノードの祖先ノードのうちの少なくともいずれか１つのノードの重みを０に設定することを特徴とする請求の範囲第２項または第３項に記載の移動体の制御装置。
6. 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、
   前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第１項または第２項に記載の移動体の制御装置。
7. 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、
   前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第３項に記載の移動体の制御装置。
8. 前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定することを特徴とする請求の範囲第６項または第７項に記載の移動体の制御装置。
9. 前記移動体は、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっていることを特徴とする請求の範囲第６項に記載の移動体の制御装置。
10. 前記移動体は、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第Ｃノードに属する接地部位となっていることを特徴とする請求の範囲第７項に記載の移動体の制御装置。
11. 前記中間部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求の範囲第９項または第１０項に記載の移動体の制御装置。
12. 前記移動体は、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられていることを特徴とする請求の範囲第６項に記載の移動体の制御装置。
13. 前記移動体は、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられており、前記第Ｃノードに属する接地部位には、少なくとも前記基体の接地部位が含まれることを特徴とする請求の範囲第７項に記載の移動体の制御装置。
14. 前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求の範囲第１２項または第１３項に記載の移動体の制御装置。
15. 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されており、
    前記修正目標運動を満足するように前記関節を動作させたときに前記第Ｃノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、
    前記ノード動作制御手段は、少なくとも前記第Ｃノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第Ｃノードに属する各接地部位に作用したときに前記基体と該第Ｃノードに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第Ｃノードに属する複数の接地部位の目標高さの第２の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第１項または第３項に記載の移動体の制御装置。
16. 少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメータを床形状を表す床形状パラメータとして推定する床形状推定手段を備え、
    前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメータの推定値に応じた、前記第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第１項または第３項に記載の移動体の制御装置。
17. 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
    前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位の実床反力とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求の範囲第１６項に記載の移動体の制御装置。
18. 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、
    前記ツリー構造における各ノードである第Ａノードに対し、該第Ａノードが前記葉ノードである場合には、該葉ノードである接地部位に作用する床反力を第Ａノードのノード床反力と定義し、且つ、第Ａノードが子ノードを持つ場合には、該第Ａノードのすべての子ノードのノード床反力の合力を第Ａノードのノード床反力と定義したとき、少なくとも前記第Ｃノードの各子ノードのノード床反力の実際値である実ノード床反力を該第Ｃノードに属する各接地部位の実床反力から逐次決定する手段を備え、
    前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求の範囲第１６項に記載の移動体の制御装置。
19. 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結され、
    該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、
    前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求の範囲第１６項に記載の移動体の制御装置。
20. 前記第Ｃノードに属する前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定することを特徴とする請求の範囲第１９項に記載の移動体の制御装置。
21. 前記ツリー構造における各ノードの重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
    前記床形状推定手段が推定する前記床形状パラメータは、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることを特徴とする請求の範囲第１６に記載の移動体の制御装置。
22. 前記床形状推定手段は、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定することを特徴とする請求の範囲第２１項に記載の移動体の制御装置。
23. 前記床形状推定手段は、前記床形状パラメータの変動を緩和するようにローパスフィルタを用いて該床形状パラメータを推定することを特徴とする請求の範囲第１６項に記載の移動体の制御装置。
24. 前記ツリー構造の各ノードには床形状の推定用の重みが設定されており、
    前記ツリー構造における各葉ノードに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に０を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードについて階層相対化する処理として定義し、
    前記葉ノードである接地部位のそれぞれに作用する床反力Ｆｎ（ｎ＝１，２，…）を基に、各ノードについて階層相対化したノード相対床反力Ｆｎ＿ｒｅｌ（ｎ＝１，２，…）を求め、さらに子ノードを持つ任意のノードである第ｎノードのすべての子ノードａｊ（ｊ＝１，２，…，ｒ。ｒは第ｎノードの子ノードの総数）のノード相対床反力を要素とするベクトル（Ｆａ１＿ｒｅｌ，Ｆａ２＿ｒｅｌ，…，Ｆａｒ＿ｒｅｌ）を、第ｎノードのすべての子ノードの重みを要素とするベクトル（Ｗａ１，Ｗａ２，…，Ｗａｒ）にそれぞれ直交する所定の互いに独立な複数のベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張床反力モーメントＭｎ＿ｅｘｐと定義し、
    また、前記葉ノードである接地部位のそれぞれの接地面の高さＺｎ（ｎ＝１，２，…）を基に、各ノードについて階層相対化したノード相対高さＺｎ＿ｒｅｌ（ｎ＝１，２，…）を求め、さらに前記第ｎノードのすべての子ノードａｊ（ｊ＝１，２，…，ｒ。ｒは第ｎノードの子ノードの総数）のノード相対高さを要素とするベクトル（Ｚａ１＿ｒｅｌ，Ｚａ２＿ｒｅｌ，…，Ｚａｒ＿ｒｅｌ）を、前記所定の互いに独立な複数のベクトルＲ（ｊ）（ｊ＝１，２，…，ｒ−１）の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張傾斜角θｎと定義したとき、
    前記床形状推定手段は、少なくとも前記第Ｃノードの前記ノード拡張床反力モーメントＭｎ＿ｅｘｐ（ｎ＝Ｃ）またはノード拡張傾斜角θｎ（ｎ＝Ｃ）を用いて前記床形状パラメータを推定することを特徴とする請求の範囲第１６項に記載の移動体の制御装置。
25. 前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
    前記床形状パラメータに、前記第Ｃノードの各子ノードの接地面の相対高さを前記ノード拡張傾斜角を用いて表したパラメータが含まれることを特徴とする請求の範囲第２４に記載の移動体の制御装置。
26. 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
    前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを推定することを特徴とする請求の範囲第２４項に記載の移動体の制御装置。

Images