WO2005051608A2

WO2005051608A2 - 移動体の制御装置

Info

Publication number: WO2005051608A2
Application number: PCT/JP2004/018072
Authority: WO
Inventors: Toru Takenaka; Takashi Matsumoto; Takahide Yoshiike
Original assignee: Honda Motor Co Ltd; Toru Takenaka; Takashi Matsumoto; Takahide Yoshiike
Priority date: 2003-11-27
Filing date: 2004-11-29
Publication date: 2005-06-09
Also published as: WO2005051608A3; US20070013506A1; EP1698439A4; US20070126387A1; KR101112500B1; KR101112501B1; KR101112499B1; US7541764B2; KR20060126650A; US7606634B2; KR20060126649A; EP1695799A4; EP1698440A4; JP4126064B2; JPWO2005051608A1; JPWO2005051612A1; EP1698439A2; US7603199B2; EP1695799B1; US20070152620A1

Description

明細書移動体の制御装置技術分野

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

本願出願人による特開平 5 - 3 0 5 5 8 5号公報（特許文献 1 ) には 2足移動口ポットの両脚のコンプライアンス制御技術が開示されている, この技術は、 4脚等の口ポットにも適用可能なものである。この場合、同公報の技術のコンプラインス制御では、全脚を仮想平面上に接触させた状態で、仮想平面を傾ける動作を行なうので、床の大域的なうねりや傾斜には対応できる。しかし、同公報の技術では、局所的な床の傾斜や凹凸があると、荷重（床反力）の配分が適切に行われない場合があった, その結果、接地すべき面が浮いて接地部位（足平）が揺れたり、着地衝撃が大きく、足平の接地性の低下することがあった。さらにその結果、口ポットの滑りやスピンが発生し易くなる恐れがあった。また、ロボッ卜の姿勢を所望の姿勢に復元させる床反カモ一メン卜が適切に各脚先端部に分配されるように制御できず、ロボットの姿勢が揺れることがあつた。また、口ポットのある部位が振動し始めると、その振動が減衰し難くいという不具合が生じることもあった。また、床反力の各脚先端部への分配制御が適切に行なわれないために、一部のァクチユエ一夕の負荷が過大になる場合もあった。

さらに、本願出願人による特開平 1 0— 2 7 7 9 6 9号公報（特許文献 2 )、特開 2 0 0 1 — 3 2 2 0 7 6号公報（特許文献 3 ) には、複合コンプライアンス制御の技術、および床形状の推定技術が開示されている。これらの技術では、 2足移動口ポットにおいて、各脚先端部への床反力の適切な分配ができ、かつ、床形状を複合的に、具体的には各脚先端部の接地面傾斜と各接地面間の高低差とを同時に複合的に、精度良く推定することができた。これらの公報の技術は、原理的には 4脚ロポット等の多脚ロボットに対応可能なものではあるが、それを具体的に適用するための技術については、記述されていない。

また、 4脚口ポットの脚先端に備えた触覚（荷重センサまたは接触センサ）を用いて着地時に床の高さを推定しょうとする手法も知られている。しかし、この手法は、シーケンス的な手法であり、軟弱地盤のように床が着地後に変形するような場合には対応できなかった。特に、ロボッ卜の高速移動時等、ロボットの本体が激しく振動しているときには、床の高さを正しく推定できず、各脚への荷重の分配が適切に行われない恐れがあった。

また、ロボットの各脚のコンプライアンス制御を脚毎に独立に行う技術も知られている。しかし、この技術では、口ポットの上体の揺れを通して脚同士の干渉が生じるにもかかわらず、このことが考慮されていなレ^ すなわち、各脚のコンプライアンス制御が個別に行なわれる。このため、各脚のコンプライアンス制御が互いに協調し合うことがないので、適切に各脚に床反力を分配することができなかった。

本発明はかかる背景に鑑みてなされたものであり、脚式移動ロボットや、アクティブサスペンションを持つ車輪型移動体などの移動体が接地する床に未知の傾斜や凹凸がある場合にも、その影響を受けにくく適切に床反力を制御する装置を提供することを目的とする。さらに床形状を精度よく推定しながら、床反力を適切に制御できる装置を提供することを目的とする。発明の開示

本発明の移動体の制御装置の第 1発明は、互いの相対高さを操作可能に複数の関節を介して連結された 3つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、

前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をッリ一構造状に分類し、

複数の接地部位を子孫ノードとして持つノードである第 Cノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が 0に近づくように、該第 Cノードに属する複数の接地部位の間の目標相対高さの修正量を該第 Cノード毎に決定し、その決定した修正量をすベての第 Cノードについて合成してなる修正量を基に、前記目標運動に、各第 Cノードに属する複数接地部位の間の目標相対高さの第 1の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とするものである。なお、本発明においては、子ノードを持つ各ノードに対し、その「ノードに属する接地部位」とは、より正確に言えば、その「ノードに子孫ノードとして属する」ということを意味するものである。このことは、第 1発明に限らず、本願のいずれの発明についても同じである。

かかる第 1発明によれば、移動体が備える 3つ以上の接地部位をッリ一構造状に分類し、子ノードを持つ第 Cノードのそれぞれに対し、姿勢傾斜偏差が 0に近づくように、該第 Cノードに属する複数の接地部位の間の目標相対高さの修正量（換言すれば操作量）を該第 Cノード毎に決定する。そして、それらの修正量を全ての第 Cノードについて合成してなる修正量を基に、前記目標運動に、各第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を付加した修正目標運動を決定する。そして、その決定した修正目標運動を満足するように移動体の関節を動作させる。これにより、個々の接地部位の高さをそれぞれ独立に (各別）の操作する場合に較べて、複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を 0に近づけるように操作することができる。その結果、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、口ポットの姿勢の安定性を高めることができる。

かかる第 1発明では、好ましくは、前記ツリー構造における各ノードに重みが設定されており、該ツリー構造における各ノードである第 Bノードに対し、該第 Bノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第 Bノードの高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第 Bノ一ドの高さとして定義したとき、

前記ノード動作制御手段は、各第 Cノードのうちの任意の一つのノ一ドのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノ一ドの目標高さは前記目標運動における目標高さに維持される（第 2発明）。

この第 2発明によれば、各第 Cノードの目標高さは、この第 Cノードの子ノードの目標高さ（これは、第 Cノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、各第 Cノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標歩容の目標運動に付加して前記修正目標運動を決定したときには、前記一つのノードの子ノードの上記重み付き平均値が、修正目標運動と目標運動とで同じになるように前記第 1の修正が目標運動に付加される。

この結果、各第 Cノード毎に、それに属する複数の接地部位の床反力の相互のバランスを採りながら、各 Cノードに属する複数の接地部位の間の目標相対高さを修正することができることとなる。ひいては、ロボッ卜の姿勢の安定性をさらに高めることができる。

また、本発明の移動体の制御装置の第 3発明は、互いの相対高さを操作可能に複数の関節を介して連結された 3つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、

前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ッリ一構造における各ノードに重みが設定されており、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が

0に近づくように、前記目標運動に、該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、

前記ツリー構造における各ノードである第 Bノードに対し、該第 Bノ ―ドが前記葉ノ一ドである場合には、該葉ノ一ドである接地部位の高さを第 Bノードの高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第 Bノードの高さとして定義したとき、前記ノード動作制御手段が、前記第 Cノードのみに対して、該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加して前記目標運動を決定したとき、該修正目標運動における該第 Cノードの目標高さは前記目標運動における目標高さに維持されることを特徴とするものである。

この第 3発明によれば、前記所定の第 Cノードの目標高さは、この第 Cノードの子ノードの目標高さ（これは、第 Cノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、前記第 Cノードのみに対して、該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加して前記修正目標運動を決定したときには、該第 Cノードの子ノードの上記重み付き平均値が修正目標運動と目標運動と同じになるように前記第 1の修正が目標運動に付加される。

この結果、第 3発明によれば、第 Cノードに属する複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を 0に近づけるように操作することができる。このため、第 1発明や第 2発明と同様に、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、口ポットの姿勢の安定性を高めることができる。

なお、前記第 2発明、第 3発明においては、前記ツリー構造において子ノードを持つ各ノードに対し、該各ノードの全ての子ノードの重みはその全ての子ノードの重みの総和が 1 となるように設定されていることが好ましい（第 4発明）。

また、前記第 2発明、第 3発明では、前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第 Dノードとしたとき、前記重みを設定する手段は、各第 D ノードの重み、または該第 Dノードの祖先ノードのうちの少なくともいずれか 1つのノードの重みを 0に設定することが好ましい（第 5発明）, この第 5発明によれば、前記姿勢傾斜偏差を 0に近づけるために寄与できない非接地状態の接地部位の高さが無駄に修正されるのを防止できる。なお、第 5発明は、第 4発明と複合してもよい。

また、前記第 1発明、第 2発明では、前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されている場合に、前記ノ一ド動作制御手段は、前記修正目標運動を決定するとき、前記第 Cノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい（第 6発明）。そして、このことは、前記第 3発明においても同じである（第 7発明）。

ここで、移動体の基体に連結されている接地部位が多数あると、移動体の関節の自由度が不足して、それらの接地部位の間の相対高さの修正が機構的な制限を受け、決定した目標相対高さを満足するように強制的に関節を動作させると、いくつかの接地部位でこじりなどの滑りが発生する場合がある。そして、そのような滑りが生じる状況では、各接地部位の床反力が不適切なものとなると共にロポッ卜の姿勢の安定性が却つて損なわれる恐れがある。このような場合には、第 6発明、第 7発明の如く、第 Cノードに属する接地部位の間の目標相対高さを修正するだけでなく、基体の目標姿勢の修正を目標運動にさらに付加して、前記修正目標運動を決定することことにより、上記こじりなどの滑りを抑制することができる。従って、接地部位に無理な床反力が作用したりすることなく、それらの床反力を適正なものとしつつ、移動体の姿勢の安定性を確保することができる。

前記第 6発明、第 7発明では、より好ましくは、前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定する（第 8発明）。この第 8発明によれば、基体の目標姿勢の修正を目標運動に付加して修正目標運動を決定したときに、移動体の全体重心が、目標歩容における適正な位置に対して水平方向に大きくずれるようなことを防止できるので、移動体の安定性を適正に保ちつつ、第 Cノードの接地部位の間の相対高さと基体の姿勢とを修正できる。

また、前記第 6発明は、前記移動体が、その基体から延設された複数のリンク機構を備える口ポットであり、該複数のリンク機構のうちの少なくとも 1つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっている場合に適している（第 9発明）。

また、第 7発明は、前記移動体が、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも 1つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第 Cノードに属する接地部位となっている場合に適している（第 1 0発明）。

すなわち、複数のリンク機構が基体から延設された口ポットでは、それらのリンク機構の先端部で接地することが通常的であるので、該リンク機構の中間部位で接地させようとした場合（例えば人間型移動ロボットをその脚の中間部（膝）で接地させようとした場合）には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第 6発明、第 7発明は、それぞれ第 9発明、第 1 0発明の如く移動体が構成されている場合に適している。

これらの第 9発明、第 1 0発明では、前記中間部位には、その接地時に弹性変形する弾性体が設けられていることが好ましい（第 1 1発明）, これによれば、前記中間部位の間の相対高さの修正による床反力の調整能力が高まって、移動体（ロボット）の姿勢の安定性の確保が容易になる。

また、前記第 6発明は、前記移動体が、その基体から延設されて 1つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも 1つのリンク機構と前記基体とに前記接地部位が設けられている場合にも適している（第 1 2発明）。

また、第 7発明は、前記移動体が、その基体から延設されて 1つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも 1つのリンク機構と前記基体とに前記接地部位が設けられており、前記第 Cノードに属する接地部位には、少なくとも前記基体の接地部位が含まれる場合に適している（第 1 3発明）。例えば 1つ以上の脚（リンク機構）が基体（上体など）から延設されたロポットに腰掛動作を行なわせる場合、その基体で接地することとなる。そしてこのような場合には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第 6発明、第 7発明は、それぞれ第 1 2発明、第 1 3発明の如く移動体が構成されている場合にも適している。

これらの第 1 2発明、第 1 3発明では、前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることが好ましい（第 1 4発明）。

これによれば、前記基体における接地部位の間の相対高さの修正による床反力の調整能力が高まって、移動体の姿勢の安定性の確保が容易になる。

なお、前記第 6〜第 1 4発明は、前記第 2発明、第 4発明、第 5発明と複合してもよい。

また、前記第 1発明、第 3発明では（あるいは第 1〜第 1 4発明では）、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されている場合に、前記修正目標運動を満足するように前記関節を動作させたときに前記第 Cノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、

前記ノード動作制御手段は、少なくとも前記第 Cノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第 Cノードに属する各接地部位に作用したときに前記基体と該第 Cノ一ドに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第 Cノ一ドに属する複数の接地部位の目標高さの第 2の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい (第 1 5発明）。

この第 1 5発明によれば、前記連結機構や接地部位が該接地部位の接地時に受ける床反力によってたわみなどの変形が生じることを考慮し、その変形量を目標床反力の並進力成分を基に推定する。そして、その推定した変形量を打ち消すための、前記第 Cノードに属する各接地部位の目標高さの第 2の修正を目標運動にさらに付加して前記修正目標運動を決定する。このため、第 1 5発明によれば、連結機構や接地部位の変形の影響を補償しつつ、適正に前記姿勢傾斜偏差を 0に近づける床反力を移動体に作用させることが可能となり、移動体の姿勢の安定性をより一層高めることができる。 ·

また、前記第 1発明、第 3発明（あるいは第 1〜第 1 5発明）では、少なくとも前記第 Cノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメ一夕を床形状を表す床形状パラメ一夕として推定する床形状推定手段を備え、

前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメータの推定値に応じた、前記第 Cノードに属する複数の接地部位の間の目標相対高さの第 3の修正をさらに付加して前記修正目標運動を決定することが好ましい（第 1 6発明）。

この第 1 6発明によれば、少なくとも前記第 Cノードに属する複数の接地部位の接地面の間の相対高さを規定する床形状パラメ一夕の推定値に応じた、第 Cノードに属する複数の接地部位の間の目標相対高さの第 3の修正を目標運動にさらに付加するので、実際の床形状が前記目標運動で想定したものと相違しても、接地部位の接地面を、それが接地すベきときに実際の床面に適切に接地させ、' ひいては、前記姿勢傾斜偏差を 0に近づけるような床反力を適切に移動体に作用させることができる。従って、移動体の姿勢の安定性をより一層高めることができる。

この第 1 6発明では、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記床形状推定手段は、前記床形状パラメ一夕の推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードに属する各接地部位の実床反力とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第 1 7発明）。

あるいは、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、

前記ツリー構造における各ノードである第 Aノードに対し、該第 Aノ —ドが前記葉ノードである場合には、該葉ノードである接地部位に作用する床反力を第 Aノードのノード床反力と定義し、且つ、第 Aノードが子ノードを持つ場合には、該第 Aノ一ドのすベての子ノードのノード床反力の合力を第 Aノードのノード床反力と定義したとき、少なくとも前記第 Cノードの各子ノードのノード床反力の実際値である実ノード床反力を該第 Cノードに属する各接地部位の実床反力から逐次決定する手段を備え、

前記床形状推定手段は、前記床形状パラメ一夕の推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメ一夕を逐次更新しつつ推定することが好ましい（第 1 8発明）。

あるいは、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されているときには、

該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、前記床形状推定手段は、前記床形状パラメ一夕の推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメ一夕を逐次更新しつつ推定することが好ましい（第 1 9発明）。

これらの第 1 7発明、第 1 8発明、第 1 9発明によれば、前記床形状パラメ一夕をリアルタイムで逐次更新しながら、適切に推定できる。補足すると、 ^_ 第 1 9発明では、前記連結機構は、その全てが必ずしも関節を備えている必要はない。

なお、第 1 9発明では、例えば前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定する（第 2 0発明）。

このように実床反力を検出または推定し、それを用いることで、前記連結機構や接地部位の変形量を適切に推定できる。

前記第 1 6発明（あるいは前記第 1 6〜第 2 0発明）においては、前記ッリー構造における各ノ一ドの重みが設定されており、該ッリ一構造における各ノードである第 Bソードに対し、該第 Bノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第 Bノ —ドの接地面の高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第 Bノードの接地面の高さとして定義したとき，前記床形状推定手段が推定する前記床形状パラメータは、前記第 Cノードの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることが好ましい（第 2 1発明）。

この第 2 1発明によれば、上記のように、子ノードを持つ第 Bノードの接地面の高さをその子ノードの重み付き平均値として定義することによって、前記第 Cノードの複数の各子ノードの接地面の高さの間の相対関係によって、該第 Cノードに属する複数の接地部位が接地する床の形状に係わる前記床形状パラメ一夕を決定できる。従って、床形状パラメ —夕を各接地部位の絶対高さを把握したりすることなく推定できる。そのため、床形状の推定処理が容易になる。

この第 2 1発明では、前記床形状推定手段は、前記第 Cノードの各子ノードのうちの少なくとも 1つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメ一夕を推定することが好ましい（第 2 2発明）。

すなわち、前記第 Cノードの各子ノードのうちの少なくとも 1つが浮きそうな状態であるということは、その子ノードの接地面が実際の床面から離れたか、もしくは離れそうな状態であるので、その子ノードの接地面の高さは実際の床面の高さを反映していない。そこで、第 2 2発明では、このような状態では、当該浮きそうな状態の子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメ一夕を推定する。これにより、床形状パラメータが実際の床面形状を反映していない値に発散してしまうような事態を防止でき、実際の床面形状を反映した床形状パラメ一夕を推定できる。ひいては、不適切な床形状パラメ一夕を使用して、接地部位の相対高さを修正し、移動体の姿勢の安定性が損なわれるような事態を防止できる。

また、前記第 1 6発明（あるいは第 1 6〜第 2 2発明）では、前記床形状推定手段は、前記床形状パラメ一夕の変動を緩和するようにローパスフィル夕を用いて該床形状パラメ一夕を推定することが好ましい（第 2 3発明）。このようにローパスフィル夕を使用することで、移動体の移動時の振動などによって床形状パラメ一夕が頻繁に変動したりするのを防止できる。

また、前記第 1 6発明（あるいは第 1 6〜第 2 3発明）では、前記ッリー構造の各ノードには床形状の推定用の重みが設定されており、前記ツリー構造における各葉ノ一ドに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に 0を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードについて階層相対化する処理として ¾|し、

前記葉ノードである接地部位のそれぞれに作用する床反力 Fn(n= l,2, …）を基に、各ノードについて階層相対化したノード相対床反力 Fn_rel(n=l,2，"，）を求め、さらに子ノードを持つ任意のノードである第 nノードのすべての子ノード aj (j = l,2，〜，r。 rは第 nノードの子ノ一ドの総数）のノード相対床反力を要素とするべクトル（Fa l_rel,Fa2— rel, -••，Far— rel)を、第 nノ一ドのすべての子ノ一ドの重みを要素とするベクトル（Wal，Wa2，"'，War)にそれぞれ直交する所定の互いに独立な複数のベクトル R(j) (j= l，2,…，！：—l)の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第 nノ二ドのノ一ド拡張床反力モーメン卜 Mn_expと定義し、

また、前記葉ノードで.ある接地部位のそれぞれの接地面の高さ Zn (_η=1,2, ··· )を基に、各ノードについて階層相対化したノード相対高さ Zn_rel(n=l，2, "' )を求め、さらに前記第 nノードのすべての子ノード _aj (j= l，2 ' ,r。 r は第 nノードの子ノードの総数）のノード相対高さを要素とするベクトル（Za l— rel, Za2_rel,…，Zar— rel)を、前記所定の互いに独立な複数のベクトル R(j) (j= l，2,… 1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第 nノードのノード拡張傾斜角 0 n と定義したとき、

前記床形状推定手段は、少なくとも前記第 Cノードの前記ノード拡張床反力モ一メント Mn_exp(n=C)またはノード拡張傾斜角 Θ n(n=C)を用いて前記床形状パラメータを推定するようにしてもよい（第 2 4発明） < 上記のように定義されるノード拡張床反力モーメントゃ、ノード拡張傾斜角を用いることで、第 Cノードに属する接地部位が 4つ以上であるような場合であっても、床形状を表す床形状パラメータを適切に推定でさる。

この第 2 4発明では、前記ツリー構造における各ノードである第 Bノードに対し、該第 Bノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第 Bノードの接地面の高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第 Bノ一ドの接地面の高さとして定義したとき、

前記床形状パラメ一夕に、前記第 Cノードの各子ノードの接地面の相対高さを前記ノ一ド拡張傾斜角を用いて表したパラメ一夕が含まれることが好ましい（第 2 5発明）。

これによれば、第 Cノードに属する接地部位が 4つ以上であっても、前記ノ一ド拡張傾斜角を用いて、床形状パラメータを適切に推定できる。

また、第 2 4発明では、より具体的には、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、

前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノ一ド拡張モ一メントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメ一夕を推定する（第 2 5発明）。

この第 2 5発明によれば、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定すると共に、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定するので、その決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを逐次更新しながら求めることができる。

なお、前記第 1〜第 2 5発明においては、「床反力」は、本来の意味での床（もしくは地面）から移動体に作用する反力だけを意味するものではなくでなく、移動体が接触する床以外の対象物から受ける反力をも含むものとする。そして、接地部位は、床（もしくは地面）への接触箇所だけでなく、移動体の対象物との接触箇所をも含むものとする。図面の簡単な説明

図 1は、本発明の第 1実施形態および第 2実施形態に係る口ポットの全体外観図、図 2は第 1実施形態の制御装置の機能的構成を示すブロック図、図 3 ( a ) 〜（ c ) および図 4 ( a ) 〜（ c ) は、第 1実施形態のロボット（4脚ロボット）の動作を説明するための図、図 5 ( a ) 〜 ( c ) は第 1実施形態における蓽みの設定例を示すグラフ、図 5 ( d ) は第 1実施形態における Z M P (目標全床反力中心点）の設定例を示すグラフである。図 6は第 1実施形態のノードの階層構造を示す図、図 7 は目標ノード床反力中心点と重みとの関係を示す図、図 8は目標ノード床反力と重みとの関係を示す図、図 9は第 1実施形態の制御装置のメインルーチン処理を示すフローチャートである。図 1 0〜図 1 4はそれぞれ第 1実施形態のロボット（4脚ロボット）の目標ノード床反力並進力成分、実ノード床反力並進力成分、実ノード床反力モーメント、補償全床反力モーメント、ノード補償床反力モーメントを例示する図である。図 1 5および図 1 6は第 1実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図、図 1 7〜図 2 1はそれぞれ第 1実施形態における階層型コンプライアンス動作決定部、補償全床反力モーメント分配器、 0 1423 決定部（補償角決定部）、 0 14 決定部（補償角決定部）、機構変形補償量算出部の機能的構成を示すプロック図、図 2 2は補償角を決定する処理を示すフローチャート、図 2 3 は第 1実施形態のロボットに係る階層構造の他の例を説明するための図である。図 2 4は第 2実施形態のロボット（ 6脚ロボット）の階層構造を説明するための図、図 2 5は第 2実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図、図 2 6〜図 2 8はそれぞれ第 2実施形態のロボット（ 6脚ロボット）の目標ノード床反力の並進力成分、実ノード床反力の並進力成分、実ノード床反力モーメントを例示する図、図 2 9 ( a ) , ( b ) は第 2実施形態におけるノード補償床反力モーメントの設定手法を説明するための図、図 3 0および図 3 1 は第 2実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図である。図 3 2および図 3 3はそれぞれ第 2実施形態における 0 145236 決定部 ' (補償角決定部）、 0 145 決定部 (.補償角決定部）の機能を示すブロック図、図 3 4は第 2実施形態の口ポットに係る階層構造の他の例を説明するための図である。図 3 5は第 3実施形態における階層型コンプライアンス動作決定部の機能的構成を示すプロック図、図 3 6は第 3実施形態における制御装置のメインルーチン処理を示すフローチャート、図 3 7〜図 3 9は第 3実施形態における概念および用語を説明するための図、図 4 0は第 3実施形態における床形状推定器の機能を示すブロック図、図 4 1は図 4 0における床高さ偏差推定処理サブルーチンの処理を示すフローチヤ一トである。図 4 2 および図 4 3はそれぞれ 4脚口ポット、 6脚口ポットにおける階層相対化の処理を説明するための図である。図 4 4は床形状推定に係る各ノ一ドの要求モードの設定例を示す図、図 4 5〜図 5 5は床形状推定処理を示すフローチャートである。図 5 6は第 4実施形態における床形状推定器の機能を示すブロック図、図 5 7は第 5実施形態における床形状推定器の機能を示すブロック図である。図 5 8および図 5 9は第 6実施形態におけるロボット（ 2足移動口ポット）が膝を付いた状態を示す図、図 6 0は第 6実施形態における口ポットの膝の床反力センサの構成を示す図、図 6 1は第 6実施形態におけるノードの階層構造を示す図、図 6 2 は第 6実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図である。図 6 3 ( a ) 〜（ c ) は第 6実施形態における口ポットの上体の姿勢の修正手法を説明するための図、図 6 4は第 6実施形態における口ポットの上体の位置姿勢の修正手法を説明するための図、図 6 5は第 6実施形態における逆キネマテイクス演算部の機能を示すブロック図である。図 6 6は第 7実施形態における逆キネマティクス演算部の機能を示すブロック図、図 6 7および図 6 8は第 7実施形態におけるロポッ卜の姿勢の修正手法を説明するための図である。図 6 9は第 8実施形態におけるロボットの構成を示す図、図 7 0は第 8実施形態におけるノードの階層構造を示す図である。発明を実施するための最良の形態

まず、本発明の実施形態の説明で用いる主な用語について説明してお <。

本願出願人が先に提案した特開平 1 0— 2 7 7 9 6 9号公報の複合コンプライアンス制御の技術においては、ロボットの脚先端部（足平）から床反力を受けることを前提として、脚毎に床反力を分別した。これに対して、本明細書の実施形態では、例えば人間型ロボットがひざまづぃた状況や腰掛けた状況、腕を接地させた状況など、脚先端部以外からも床反力を受ける状況までも考慮する。このため、脚式移動口ポットの所要の動作において床と接して反力を受ける該ロボットの部位を「接地部位」と呼ぶ。この場合、「床」は、通常の意味での床（あるいは地面）だけではなく、該床（あるいは地面）に固定された椅子（口ポットが腰掛ける椅子）等のように、口ポットがその動作において接触して反力を受ける対象物も、「床」に含まれるものとする。従って、例えば 2足移動ロボッ卜が椅子などに腰掛けるときには、該ロボッ卜の腰部は接地部位に相当するものとなる。なお、脚式移動口ポットの通常的な移動時 ( 2足移動口ポットの歩行時など）には、各脚の先端部（足平など）が接地部位に相当するもの"となることはもちろんである。

接地部位を分類（区別）するに当たっては、口ポットの同一のリンク (単一の剛体に相当する部分）上で接地部位が複数箇所に分離されて分布している場合（同一のリンクの、互いに分離した複数の部分が接地している場合）でも、すなわち同一リンク上で複数の接地面がつながっていなくても、これらをまとめてひとつの接地部位と定義する。例えば、スパイクピンが 4本装着された接地部位がそれらのスパイクピンを介して接地する場合には、 4本のスパイクピンのそれぞれの接地面をまとめてひとつの接地部位とみなす。ただし、それらの接地面を必ずしも一つにまとめる必要はない。

第 n 接地部位とは、このようなルールの基で分類（区別）されて識別番号 n ( 11=1,2，3 '）を割り振られた接地部位の識別名称である。

接地部位床反力とは、接地部位に作用する床反力とし、特に、第 n 接地部位に作用する床反力を第 n 接地部位床反力という。また、全床反力とは、すべての接地部位に作用する床反力の合力とする。更に、床反力中心点とは、床反力がその点に発生するモーメントの水平成分が 0 となるような作用点とする。

なお、接地部位床反力、全床反力などの床反力は、一般的には、力の作用点と、その作用点にかかる並進力およびモーメントとの組によって表現され、同一の床反力に対して、それを表現する作用点と、並進力およびモーメントとの組は無限にある。その中には、鉛直軸まわりの成分（鉛直成分）を除くモーメント成分（モーメントの水平成分）が 0となるような点を作用点として用いる床反力の表現が存在する。この表現における作用点を、本明細書では床反力中心点という。また、床反力中心点を、床反力がその点に発生するモーメントの床面平行成分（床面に平行な成分）が 0となるような点として定義してもよい。

尚、この明細書で「床面」は、本願出願人が先に提案した特開平 5— 3 1 8 3 4 0号公報に記載される仮想的な床面（目標歩容上で想定される床面であって、実際の床面とは必ずしも一致しない）であっても良い。補足すると、接地部位の床反力中心点は、その接地部位が接地している状態では、通常、その接地面（床との接触面）上に設定される。一方、接地部位が空中を移動しているときの接地部位床反力は常に 0であるので、どの点を作用点としても該接地部位床反力のモ一メント水平成分は 0である。つまり、床反力中心点は任意に設定できる。ただし、ロポッ卜の動作制御を円滑に行なう上では、目標とする床反力中心点は、連続的に移動することが望ましい。そこで、本明細書め実施形態では、接地部位床反力の床反力中心点は、接地部位が空中を移動するときに、接地部位と共に移動する作用点であるとする。

本明細書の実施形態においては、少なくとも実床反力（口ポットに実際に作用する床反力）に応じて各接地部位の位置姿勢の修正を行なうコンプライアンス制御動作において、単に、各接地部位毎に、互いに無関係に（独立して）コンプライアンス制御動作を行うのではない。本明細書の実施形態は、各接地部位をツリー構造で分類し、少なくとも分類された各グループに作用する実床反力（各グループに含まれる全ての接地部位に作用する実床反力もじくはその合力）に基づき、接地部位の位置姿勢の修正動作を決定することに特徴がある。尚、以降、ツリー構造で分類することを「階層化」と呼ぶこともある。

本明細書の実施形態における脚式移動ロポッ卜の歩容生成装置が生成する変数の目標値には、変数名の頭に「目標」を付ける。実際の脚式移動ロボットにおける当該変数の値（真の値はわからないので、実際には. 検出値または推定値）には、変数名の頭に「実」を付ける。前記「実床反力」はその例である。

後述するコンプライアンス制御（床反力制御）の全床反力（ロボットのすベての接地部位に作用する実床反力の合力）の目標を目標全床反力と呼ぶ。目標全床反力のモーメント水平成分が 0となる点を目標全床反力中心点と呼ぶ。 - 移動口ポットの目標歩容の運動（目標歩容における口ポットの各部位の運動）が発生する慣性力と該ロボッ卜に作用する重力との合力のモーメント水平成分が 0となる床面上の点を目標 Z M Pと呼ぶ。なお、目標歩容の運動は、目標歩容におけるロポッ卜の各部位の目標位置姿勢の時系列によって定まるので、その各部位の目標位置姿勢の時系列を総称的に目標歩容の運動もしくは目標運動という。補足すると、これらの時系列を規定できるならば、上記の如く定義した目標運動と異なる表現で目標運動を記述してもよい。例えば、口ポットの各関節の目標変位の時系列と基体などの特定の部位の目標位置姿勢の時系列との組を目標運動としてもよい。

目標全床反力は、通常、目標歩容の運動パターン（目標運動の時系列パターン）に対して動力学的に平衡する全床反力である。従って、通常目標全床反力中心点は、目標 Z M Pに一致する。そこで、以下の説明では、多くの場合、目標全床反力中心点と目標 Z M Pとを区別せずに使用する。ただし、例外的に、本願出願人が先に提案した特許第 3 2 6 9 8 5 2号などにて説明している口ポットの制御の場合には、目標全床反力中心点と目標 Z M Pとは一致するとは限らない。以下の説明では、目標 Z M Pという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所もある。

尚、移動ロボットの目標歩容で該口ポットが床反力以外の反力（外力）を環境から受けるように想定してもよい。そして、その床反力以外の反力（外力）を、例えば目標対象物反力と称し、先に述べた目標 Z M Pの定義を次のように拡張しても良い。即ち、口ポットの目標歩容の運動パターンによって発生する慣性力と該ロポットに作用する重力と目標対象物反力との合力を動力学的に求め、その合力が床面上のある点に発生するモーメントが、鉛直軸まわりの成分を除いて 0であるならば、その点を改めて目標 Z M Pとするようにしても良い。ただし、目標対象物反力も、床反力の一形態としてとらえるならば、目標 Z M Pの定義は、先に述べた定義のままでよい。

.以上を前提として、本発明の実施形態を以下に詳細に説明する。 [第 1実施形態]

図 1は、第 1および第 2実施形態に係る一般的な多脚の移動ロポット (脚式移動口ポット）の外観図である。ただし、図 1では、口ポット 1 は、第 1脚 # 1〜第 6脚 # 6までの 6個の脚（脚体）を有するものとして図示しているが、第 1実施形態においては、第 5脚 # 5および第 6脚 # 6は無いものとする。すなわち、第 1実施形態では、口ポット 1は、第 1脚 # 1〜第 4脚 # 4の 4個の脚（脚体）を有する 4脚口ポットであるとする。図 1では、第 2実施形態に係るロボット 1の構成要素については、括弧付きの参照符号を付している。

図 1に示すように、第 1実施形態の口ポット 1 ( 4脚口ポット）では. 該ロボット 1の基体である上体 2 4の右側部から 2個の脚（第 1脚 # 1 . 第 3脚 # 3 ) が前後方向に並んで延設され、これと同様に上体 2 4の左側部から 2個の脚（第 2脚 # 2、第 4脚 # 4 ) が前後方向に並んで延設されている。各脚 # 1〜 # 4の先端部には、フリージョイントである球関節 1 2を介して円盤状の接地部位 1 0が取り付けられている。以降、接地部位 1 0を各脚 # 1〜# 4毎に区別する必要があるときは、それぞれを第 1接地部位、第 2接地部位、第 3接地部位、第 4接地部位という, 各接地部位 1 0は、その中心点を球関節 1 2の中心点に一致させて該球関節 1 2に係合されており、該接地部位 1 0の中心点（厳密には球関. 節 1 2 ) には、床反力モーメント（床反力のモーメント成分）が作用しないようになっている。すなわち接地部位 1 0の中心点の床反力モーメント（実床反力モーメント）は常に 0となる。

図示のロボット 1では、各脚 # 1〜# 4は、ロボット 1の上体 2 4寄りの部分と中間部とにそれぞれ関節 1 4 , 1 5を備えると共に、各脚 # 1.〜# 4の先端部近傍（球関節 1 2と関節 1 4とを連結するリンクの球関節 1 2寄りの部分）に、パネなどの弾性部材から構成されたコンブラィアンス機構 4 2と、接地部位 1 0に作用する実床反力を検出する床反力検出手段としての 6軸力センサ 3 4とが介装されている。図示の例では、関節 1 4は 2軸回りの回転が可能とされ、関節 1 5は 1軸回りの回転が可能とされている。なお、コンプライアンス機構とし、接地部位の底面にスポンジやゴムなどの弾性体を設けてもよい。

各関節 1 4， 1 5の変位動作（各軸回りの回転動作）は、図示を省略する電動モー夕などのァクチユエ一タにより行なわれる。そして、各関節 1 4 , 1 5の実際の変位量（各軸回りの回転角）である実関節変位が, 図示しない口一タリエンコーダ等のセンサにより検出される。なお、 6 軸力センサ 3 4は、 3軸方向の並進力と 3軸回りのモーメントとを検出可能なものであるが、第 1実施形態のロボット 1では、前記したように接地部位 1 0の中心点に実床反力モーメントが作用しない。従って、 6 軸力センサ 3 4の代わりに、 3軸方向の並進力を検出する 3軸力センサ，あるいは、実床反力の並進力の鉛直成分だけを検出する力センサを用いてもよい。

上体 2 4には、マイクロコンピュータゃァクチユエ一夕駆動回路等を含む電子回路ユニットから構成された制御装置 5 0と、上体 2 4の姿勢を検出する姿勢センサ 3 6と、図示しない電源（ 2次電池、キャパシタなど）とが内蔵されている。姿勢センサ 3 6は、例えば加速度センサとジャイロセンサとから構成されている。なお、本明細書では、「姿勢」は一般的には空間的な向きを意味する（但し、ロボットの全体の「姿勢」は、ロボットの運動の瞬時値を意味する）。そして、本実施形態では、姿勢センサ 3 6は、上体 2 4の姿勢のうち、例えば鉛直方向に対するピッチ方向およびロール方向の姿勢傾斜（傾き角）を検出する。すなわち、姿勢センサ 3 6は上体 2 4の実姿勢傾斜を検出する実姿勢傾斜検出手段として機能する。

なお、口ポット 1の上体 2 4のョ一方向の姿勢（回転角）も制御するような場合には、上体 2 4のョ一方向の回転角も（換言すれば上体 2 4 の 3軸方向の姿勢を）姿勢センサ 3 6で検出するようにしてもよい。図 2は、前記制御装置 5 0の機能的構成および動作を示すブロック図である。なお、図 2では、図 1の口ポット 1から、制御装置 5 0、姿勢センサ 3 6および 6軸力センサ 3 4を除いたものを実ロボット 1 と称し 5 ている。ここで、前記図 1に示したように、口ポット 1のおおよそ前方向を X軸、おおよそ左横方向を Y軸、上方向を Z軸とした、床に固定された所定の座標系（X Y Z座標系）を「支持脚座標系」または「グロ一バル座標系」と呼ぶ。以降、特に断らない限り、位置、姿勢、並進力およびモーメントはこの支持脚座標系（グローバル座標系）の座標成分の0 組によって表現されるものとする。支持脚座標系（グローバル座標系）、の原点は、定常的に単一の点に固定されている必要はなく、口ポット 1 の移動途中で床に対する原点位置を変更してもよい。例えば、口ポット

' 1の所定の脚が着地する毎に支持脚座標系（グローバル座標系）の原点位置を変更してもよい。

5 図 2に示すように、制御装置 5 0は、その機能的構成要素（機能的手段）として、歩容生成装置 1 0 0、目標床反力分配器 1 0 2、ロボット幾何学モデル（逆キネマティクス演算部） 1 1 0、階層型コンプライアンス動作決定部 1 1 4、変位コントローラ 1 1 2、実床反力検出器 1 0 8、姿勢偏差演算部 1 0 3、および姿勢安定化制御演算部 1 0 4を備え0 ている。以下に、制御装置 5 0のこれらの要素の概要を説明する。

歩容生成装置 1 0 0は、本発明における目標歩容決定手段あるいは目標運動決定手段としての機能を持ち、ロボット 1の動作を規定する目標歩容を生成（決定）して出力する。目標歩容は、本実施形態では、ロボットの目標運動の軌道（口ポットの各部位の目標位置姿勢の時系列）と5 目標床反力の軌道（ロボットが床から受ける反力の作用点の目標位置とその作用点にかかる並進力およびモ一メントの目標値との組の時系列）とから構成される。なお、本明細書では、「軌道」は、時系列パターン (時間的変化のパターン）を意味する。

歩容生成装置 1 0 0が出力する目標運動の軌道は、ロボット 1の各接地部位 1 0の位置および姿勢の目標値の軌道である目標接地部位軌道と口ポット 1の上体 2 4の位置および姿勢の目標値の軌道である目標上体位置姿勢軌道とから構成される。なお、後述する第 6実施形態のように腕や頭部に係る関節を備えるロポッ卜における歩容生成装置は、腕や頭部の目標位置姿勢軌道も目標運動の構成要素として決定して出力する。

また、歩容生成装置 1 0 0が出力する目標床反力の軌道は、本実施形態では、ロボット 1の全床反力中心点の目標位置の軌道である目標全床反力中心点軌道と、この目標全床反力中心点を作用点とする全床反力の目標値の軌道である目標全床反力軌道とから構成される。本実施形態では、目標全床反力中心点軌道は、 Z M Pの目標位置の軌道である目標 Z M P軌道と同一とされている。

各接地部位 1 0の位置は、該接地部位 1 0のある代表点の位置であり . その代表点は、例えば、各接地部位 1 0の中心点（球関節 1 2の中心点）の、該接地部位 1 0の接地面（底面）への鉛直方向の投影点あるいは該接地部位 1 0の中心点（球関節 1 2の中心点）とされている。以降, 各接地部位 1 0の代表点の位置を単に接地部位位置と呼ぶ。この接地部位位置の目標値の軌道（目標接地部位位置軌道）が歩容生成装置 1 0 0 により決定される前記目標接地部位軌道に含まれる。

ここで、本実施形態の口ポット 1では、各接地部位 1 0はフリージョイントである球関節 1 2に係合されているので、各接地部位 1 0の姿勢を制御することはできない。このため、本実施形態では、歩容生成装置 1 0 0は、実際には各接地部位 1 0の目標姿勢の軌道を生成しない（生成する必要がない）。従って、本実施形態では、前記目標接地部位軌道は、目標接地部位位置軌道と同じものを意味する。

但し、後述する第 6実施形態の口ポットのように、各接地部位が各脚の先端部にその姿勢を制御可能に'取り付けられている場合（各脚の先端部にァクチユエ一夕により動作可能な関節を介して接地部位が取り付けられている場合）には、各接地部位の目標姿勢の軌道も、前記目標接地部位軌道に含まれるべきものである。本明細書では、このような場合も一般的に考慮するために、しばしば「接地部位位置姿勢」という用語を用いるが、それは、本実施形態では、実質的には「接地部位位置」のことを意味する。

図 3 ( a ) 〜図 3 ( c ) および図 4 ( a ) 〜図 4 ( c ) を参照して、目標接地部位軌道（目標接地部位位置軌道）および目標全床反力中心点軌道をより具体的に説明する。第 1実施形態では、脚 # 1〜# 4のうち，第 1脚 # 1および第 4脚 # 4の組を支持脚、第 2脚 # 2および第 3脚 # 3の組を遊脚とする時期と、全ての脚 # 1〜# 4を支持脚とする時期と，第 1脚 # 1および第 4脚 # 4の組を遊脚、第 2脚 # 2および第 3脚 # 3 の組を支持脚とする時期とを順番に繰り返しつつ、遊脚となる脚の組を離床させて空中で移動させた後、所望の位置に着地させるという脚 # 1 〜 # 4の運動を行なうことによりロポット 1の移動を行なうようにしている。支持脚は、接地してロボット 1の自重を支える脚（ 0でない床反力を作用させるべき脚）であり、遊脚は支持脚でない脚である。

図 3 ( a ) 〜図 3 ( c ) および図 4 ( a ) 〜図 4 ( c ) は、上記の如くロボット 1を移動させるときの各脚 # 1〜# 4の先端の目標接地部位位置（詳しくは水平面（X Y平面）上での位置）を時系列的に順番に (時刻 tl〜t6の順に）示したものである。これらの図中の参照符号 Q1 ~ Q4 に対応する三角形が、それぞれ第 1〜第 4接地部位 1 0の目標接地部位位置（第 1〜第 4接地部位 1 0の前記代表点の水平面（X Y平面）上での位置）を示している。

補足すると、図 3および図 4中の参照符号 Q1〜Q4 を付した三角形は、正確には、各接地部位 1 0に係る後述の目標ノード床反力中心点 (目標接地部位床反力中心点）の位置を示すものである。ただし、本実施形態では、各接地部位 1 0の代表点を前記した如く設定しているので, 第 1〜第 4接地部位 1 0のそれぞれの目標接地部位位置（代表点の目標位置）は、後述する目標ノード床反力中心点 Qn ( n=l,2,3,4) のそれぞれの位置に一致するか、もしくは、該目標ノード床反力中心点 Qn ( n=l,2,3,4) のそれぞれの位置と一定の位置関係を有する（水平面 ( X Y平面）上での位置は一致する）。このため、図 3および図 4中の参照符号 Q l , Q2, Q3, Q4 に対応する三角形は、第 1〜第 4接地部位 1 0のそれぞれの後述の目標ノード床反力中心点を表すと同時に、第 1 〜第 4接地部位 1 0のそれぞれの位置を表している。

図 3 ( a ) は、第 2脚 # 2および第 3脚 # 3の組を支持脚とし、遊脚としての第 1脚 # 1および第 4脚 # 4の組を着地させる瞬間（時刻 tl) での各接地部位 1 0の目標接地部位位置を示し、図 3 ( b ) は、全ての脚 # 1〜# 4が支持脚となっている状態の時刻 t2 での目標接地部位位置を示し、図 3 ( c ) は、第 1脚 # 1および第 4脚 # 4の組を支持脚とし、遊脚としての第 2脚 # ' 2および第 3脚 # 3の組を離床させる (空中に持ち上げる）直前の時刻 t3 での目標接地部位位置を示している。また、図 4 ( a ) は、第 1脚 # 1および第 4脚 # 4の組を支持脚とし、遊脚としての第 2脚 # 2および第 3脚 # 3の組を空中に持ち上げた状態の時刻 t4 での目標接地部位位置を示し、図 4 ( b ) は、第 1脚 # 1および第 4脚 # 4の組を支持脚とし、遊脚としての第 2脚 # 2および第 3脚 # 3の組を着地させた瞬間（時刻 t5) での目標接地部位位置を示し、図 4 ( c ) は、第 2脚 # 2および第 3脚 # 3の組を支持脚とし、遊脚としての第 1脚 # 1および第 4脚 # 4の組を離床させる直前の時刻 t6 での目標接地部位位置を示している。なお、図 4 ( a ) では遊脚である第 2脚 # 2および第 3脚 # 3の目標接地部位位置を破線の三角形で示している。補足すると、遊脚の接地部位 1 0の鉛直方向（ Z軸方向）の位置の軌道は、床面から所定の高さまで上昇した後、下降して、再び着地するように決定される。

これらの図 3および図 4における点 Pは、目標全床反力中心点（目標 Z MP) を示している。目標全床反力中心点軌道は、 Z MPの存在可能範囲（いわゆる支持多角形に相当する床面上の領域）内で該範囲の境界に近づきすぎない位置（例えば Z M Pの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。第 1実施形態では. 2つの脚 # 1， # 4または # 2， # 3が支持脚となるときには（図 3 ( a), ( c )、図 4 ( a) 〜（ c ) を参照）、それらの脚の接地部位 1 0 , 1 0の代表点を結ぶ線分上で、その線分の端点に近づき過ぎないように目標全床反力中心点が設定される。また、全ての脚 # 1〜# 4が支持脚となるときには（図 3 (b ) を参照）、全ての接地部位 1 0の代表点を頂角とする多角形内で、該多角形の境界に近づき過ぎないように目標全床反力中心点が設定される。図 5 ( d ) は、このように決定された目標全床反力中心点（目標 Z MP) の位置の X軸方向（ロボット 1の進行方向）の成分 Z MPx の軌道を例示したグラフである。なお、図 5 ( a) 〜（ c ) は後述の重みの設定例を示すグラフである。

また、歩容生成装置 1 0 0が決定する目標上体位置姿勢軌道は、少なぐとも、口ポット 1の目標運動によって発生する慣性力とロボット 1に作用する重力との合力が目標 Z MPまわりに作用するモーメントの水平成分が 0になるように口ポット 1の動力学モデルなどを使用して決定される。なお、「上体位置」は、上体 2 4のある代表点の位置である。また、歩容-生成装置 1 0 0が決定する目標全床反力は、目標全床反力中心点に作用する並進力およびモーメントの目標値から構成され、本実施形態では、口ポット 1 の目標運動によって発生する慣性力とロボッ卜 1 に作用する重力との合力に目標全床反力中心点で釣り合うように決定される。なお、目標全床反力中心点（目標 Z M P ) まわりの目標全床反力のモーメント水平成分は 0である。補足すると、目標全床反力中心点に作用する並進力およびモーメン卜の全ての成分を目標値として決定する必要はない。例えばロポヅト 1の鉛直軸まわりの姿勢もしくは床反力を制御しない場合には、目標全床反力のモーメントの鉛直軸まわりの成分（Z軸方向成分）を決定する必要はない。

なお、歩容生成装置 1 0 0が決定する目標接地部位軌道（目標接地部位位置軌道）は、後述する階層型コンプライアンス動作決定部 1 1 4にて修正される。

前記目標床反力分配器 1 0 2は、口ポット 1の第 1〜第 4接地部位 1 0をツリー構造状にグループ分けし（すなわち階層化し）、階層化された各グループに、ツリー構造のノードを対応させる。したがって、以下の説明において、ノードをグループに置き換えて表現しても構わない。各ノードは、 1つ以上の接地部位 1 0から構成されるグループである。第 1実施形態においては、図 6に示す如く接地部位 1 0が階層化される。すなわち、第 n 接地部位 1 0 ( n=l,2,3，4) を第 n ノードに対応させ、第 1 ノードと第 4ノードとを子ノードとして持つノードを第 14 ノードとし、第 2 ノードと第 3 ノードとを子ノードとして持つノードを第 23 ノードとし、第 14 ノードと第 23 ノードとを子ノードとして持つノ一ドを第 1423 ノードとする。従って、第 1〜第 4ノードは、それぞれ第 1、第 2、第 3、第 4接地部位 1 0から成るノード、第 14 ノードは，第 1接地部位 1 0 と第 4接地部位 1 0 とから成るノード、第 23 ノードは、第 2接地部位 1 0 と第 3接地部位 1 0とから成るノード、第 1423 ノードは全ての接地部位 1 0から成るノードである。

ツリー構造における一般的な呼び方に従い、子ノードを持たないノードを葉ノードと呼び、親ノードを持たないノードを根ノードと呼ぶ。したがって、第 nノード（n=l,2,3,4) は葉ノード、第 1423ノードは根ノードである。なお、本明細書では、ノードを区別するために、葉ノードには、それに対応する接地部位（もしくは脚）と同じ番号（1，2，3 '）を割り付け、葉ノード以外のノードには、葉ノードより大きい番号を割り付けることとする。また、葉ノードおよび根ノード以外のノードを中間ノードと呼ぶ。第 1実施形態では、中間ノードは第 14ノード、第 23 ノードである。

目標床反力分配器 1 0 2には、歩容生成装置 1 0 0で決定された目標歩容のうち、目標全床反力中心点軌道、目標全床反力軌道および目標接地部位軌道とが入力される。なお、歩容生成装置 1 0 0が目標歩容を決定するために使用した歩容パラメータ（ロボット 1の遊脚の接地部位 1 0の着地予定位置、着地予定時刻など）も目標床反力分配器 1 0 2に入力してもよい。そして、目標床反力分配器 1 0 2は、これらの入力を基に、上記のように階層化された各ノードの床反力中心点の目標位置である目標ノード床反力中心点（目標第 n ノード床反力中心点） Qn ( 11=1,2,3,4,14,23,1423) と、根ノードを除く各ノードの重み Wn (n=l, 2, 3, 4,14,23) とを決定する。ただし、各重み Wnは 1以下の非負の値とする。 ·

目標ノード床反力中心点 Qn (n=l, 2, 3, 4,14,23) および重み Wn (n=l，2，3, 4,14,23) の決定手法を図 3 ( b )、図 5および図 7などを参照しつつ以下に詳説する。図 3 (b ) には、同図示の状態における Qn と Wn との関係が示され、図 5 ( a ) 〜（ c ) には Wnの設定例が示され、図 7には重みと目標ノード床反力中心点との関係が示されている。根ノードである第 1423 ノードの目標ノード床反力中心点（目標第 1423 ノード床反力中心点） Q1423 は、目標全床反力中心点（図 3および図 4中の点 P ) と同一に決定される（P = Q1423)。従って、以降の説明では、目標第 1423 ノード床反力中心点を示す符号として、多くの場合、目標全床反力中心点と同じ参照符号 Pを用いる。

また、根ノードの重み W1423 は、表現の統一性を保っために、便宜上「 1」に設定される。 '

第 n 接地部位 1 0 ( n=l,2,3,4) の目標床反力中心点（目標第 n接地部位床反力中心点とも言う）は、本実施形態においては、常に第 n接地部位 1 0の中心点（球関節 1 2の中心点）に設定される。そして、第 n 接地部位 1 0 に対応する葉ノードの目標ノード床反力中心点 Qn ( n=l,2,3,4) は、第 n接地部位 1 0の目標床反力中心点と同一に決定される。従って、目標ノード床反力中心点 Qn ( n=l，2,3，4) も常に第 n 接地部位 1 0の中心点（この点の位置は該第 n接地部位 1 0の目標接地部位位置から一義的に定まる）に設定される。このように決定した葉ノ —ドの目標ノード床反力中心点 Qn ( n=l,2,3,4) の位置は、第 n接地部位 1 0の目標接地部位位置から一義的に定まる。逆に言えば、葉ノードの目標ノード床反力中心点 Qn '( n=l,2，3，4) は、第 n接地部位 1 0の目標位置を規定するものとなる。なお、以降の説明では、葉ノードの目標ノード床反力中心点を目標接地部位床反力中心点ということもある。また、各目標接地部位軌道は、連続的に変化するように設定されるので、 Qn ( n=l,2,3,4) も連続的に変化するものとなる。

補足すると、本実施形態では、第 n葉ノード（第 n接地部位 1 0 ) の目標床反力中心点 Qn ( n=l,2, 3,4) を第 n接地部位 1 0の中心点に設定したが、接地部位の姿勢を制御できるロボットでは、各葉ノード（各接地部位）の目標床反力中心点は、例えば、その葉ノードに対応する接地部位の目標位置姿勢により定まる該接地部位の接地面（目標歩容で想定される床面との接触面）内に設定すればよい。

以降、一般に、任意の 2つの点 A, Bを結ぶ線分またはその長さを AB というように表す。また、演算子「 * j は、スカラーとスカラーとの組もしくはスカラーとベクトルとの組に対しては、乗算を意味し、ベクトルとベクトルとの組に対しては、外積（ベクトル積）を意味する。根ノード以外の各ノードの重み Wn (n=l, 2,3,4, 14, 23) と各中間ノードの目標ノード床反力中心点 Q14, Q23 とは、目標第 14 ノード床反力中心点 Q14が、 Q1Q14： Q14Q4=W4： Wl = (l— W1)： W1 という関係式を満たす線分 Q1Q4 の内分点、目標第 23 ノード床反力中心点 Q23 が、 Q2Q23： Q23Q3 = W3： W2=(l— W2)： W2 という関係を満たす線分 Q2Q3の内分点、目標第 1423 ノード床反力中心点 P (=目標全床反力中心点）力 Q14P： PQ23 = W23： W14=(l-W14)： W14 を満たす線分 Q14Q23の内分点となるように決定される。

言い換えると、少なくとも次式 1 , 2 , 3を満足するように、 Q14， Q23, Wn (n=l,2,3,4,14,23) が決定される。なお、これらの式：！〜 3 中の Qn (n=l，2，3，4，14,23) は、その点の位置（位置べクトル）を意味している。 '

Q14=Q1* W1 + Q4* W4 , Wl + W4= 1 …式 1

Q23=Q2* W2+Q3* W3 , W2 + W3= 1 …式 2

P =Q14* W14 + Q23* W23 , W14 + W23= 1 …式 3 Wn (n=l, 2, 3, 4,14,23) が 1以下の非負の値であることから、上式 1 , 2 , 3の右辺における Qn (n=l,2,3,4,14,23) の係数（重み）は、すべて非負の値となる。

上式 1〜 3は、子ノードを持つ各ノード（すなわち葉ノードでない各ノード）の目標ノード床反力中心点の位置が、所定の非負の重みを用いて、そのノードの子ノードの目標ノード床反力中心点の位置の重み付き平均の位置に設定されることを意味する。言い換えると、前記図 3 (b) および図 7に示す如く、子ノードを持つ各ノードの目標床ノード '反力中心点 Qn (n=14,23,1423) は、そのノードのすべての子ノードの目標ノード床反力中心点の内分点に設定される。図 7は、各ノードの目標ノード床反力中心点 Qn ( n=l,2,3,4, 14,23, 1423 ) と重み Wn (n=l，2, 3, 4,14,23) との関係を示す図である。なお、図 3 ( b ) 中の L23, L14, L 1423 はそれぞれ線分 Q2Q3、 Q1Q4, Q23Q14 の長さである。

補足すると、 Q1〜Q4 および P (=Q1423)は前記した如く決定されるので、 Wn (n=l，2，3，4, 14,23) を決定すると、式：！〜 3を満たす Q14, Q23 は一義的に定まり、逆に、 Q14, Q23 を決定すると、式 1〜 3を満たす Wn (n=l,2,3,4,14,23) は一義的に定まる。従って、重み Wn (n=l,2,3,4,14,23) を決定した上で、前記式 1〜 3に基づいて中間ノードの目標ノード床反力中心点 Q14， Q23 を決定しても、中間ノードの目標ノード床反力中心点 Q14', Q23 を決定した上で、前記式 1〜 3 に基づいて重み Wn (n=l，2，3, 4, 14,23) を決定しても、どちらでもよい。中間ノードの目標ノード床反力中心点 Q14， Q23 は、図 3 ( a ) 〜図 3 ( c ) および図 4 ( a) 〜図 4 ( c ) に時系列的に示したように、各接地部位 1 0の移動に伴って移動する。この時の重み W14 (= 1 — W23), Wl (= 1 — W4), W3 (= 1 一 W2)は、それぞれ例えば前記図 5 ( a) 〜（ c ) のグラフで示す如く連続的に変化するように決定される。第 nノード（n=l，2,3，4) の Qn の軌道および重み Wn が連続的に変化するように設定されるので、目標ノード床反力中心点 Q 14， Q23 も連続的に移動するように決定される。つまり、すべての目標ノード床反力中心点（目標第 n ノード床反力中心点（n= l,2, 3，4，14,23, 1423) ) は、連続的に移動するように設定される。

目標床反力分配器 1 0 2は、上記のように決定した各ノードの目標ノ —ド床反力中心点を出力する。なお、根ノードの目標床反力中心点は、歩容生成装置 1 0 0が決定する目標全床反力中心点と同じであるので、目標床反力分配器 1 0 2から出力する必要はない。

目標床反力分配器 1 0 2は、さらに、各ノードの目標床反力中心点に作用する床反力の目標値である目標ノード床反力を決定して出力する。出力する目標ノード床反力には、少なくとも葉ノードである第 η ノード（n=l，2，3,4) の目標ノード床反力中心点 Qn に作用する目標ノード床反力（目標第 nノード床反力）、すなわち、各接地部位 1 0の目標床反力が含まれる。この目標ノード床反力は、後述する階層型コンプラインァス動作決定部 1 1 4の処理で実行される、各脚 # 1〜# 4のコンプライアンス機構 4 2など（図 1参照）のたわみ補償（より詳しくはコンプライアンス機構 4 2および各脚のリンク機構のたわみによる各接地部位 1 0の位置ずれの補償）等のために必要である。補足すると、ロボット 1が、各脚 # 1〜# 4の先端部以外にもコンプライアンス機構を備える場合には、コンプライアンス機構の変形を求めるために、目標第 14 ノード床反力、目標第 23 ノード床反力（中間ノードの目標ノード床反力）も決定して出力することが望ましい。

一般的に、各ノードの目標ノー F床反力中心点に作用する目標床反力 (目標ノード床反力）は、目標全床反力と各ノードの重みとから決定すればよい。具体的には、任意のノードの目標床反力は、そのノードの重みとそのノードのすべての先祖ノードの重みとの積に目標全床反力を乗じることによって決定すればよい。すなわち、次式 4 a (または式 4 b ) によって目標第 nノード床反力が算出される。目標第 nノード床反力

=第 11 ノード重み

*第 nノードのすべての先祖ノードの重みの積

* 目標全床反力 …式 4 a

目標第 nノード床反力

=第 nノ一ド堇み

*第 nノードのすべての先祖ノード（根ノードを除く）の重みの積 * 目標全床反力 …式 4 b あるいは、葉ノードでない任意の第 nノードの目標床反力が、第 nノ —ドのすベての子ノードの目標床反力の和（合力）に一致し、かつ、根ノードの目標床反力が目標全床反力に一致するように、目標各ノード床反力が決定される。この関係を示したものが図 8である。

すなわち、第 nノードの目標床反力である目標第 n ノード床反力を Fn ( n=l, 2, 3，4, 14,23, 1423)、目標全床反力を Ftotalref で表すと、 F n は、図 8 中に示す式によって、 Ftotalref と、重み Wn ( n=l,2,3,4, 14,23) とから決定される。図 8 中の式は前記式 4 a または 4 bと等価である。

以上のごとく各ノードの目標床反ガ中心点（目標ノード床反力中心点）と各ノードの目標床反力（目標ノード床反力）とを決定することは、第 nノードのすべての子ノードの目標床反力の合力が目標第 nノ一ド床反力中心点まわりに発生するモーメントの水平成分が 0 となるように各ノードの目標床反力中心点と目標床反力とを決定していることになる。従って、目標ノード床反力のモーメント水平成分は、いずれのノードについても 0である。

なお、接地していない（非接地状態の）接地部位 1 0の目標床反力 (接地していない接地部位 1 0に対応する葉ノードの目標ノード床反力）が 0 となるように、各目標ノード床反力中心点 Qn ( n=l, 2, 3, 4, 14,23)、各重み Wn(n=l,2,3，4，14，23) および各目標ノード床反力 F n ( n=l, 2, 3,4, 14,23) の組が決定される。従って、第 1実施形態では、図 5 ( a ) に示したように、第 1接地部位 1 0および第 4接地部位 1 0が遊脚となって離床する時期（該遊脚の接地部位 1 0が空中を移動する時期）では、重み W14 が 0 ( W23 = l ) とされ、また、第 2 接地部位 1 0および第 3接地部位 1 0が遊脚となって離床する時期では、重み W23 が 0 ( W14= l ) とされる。このように、子ノードを持つ中間ノードの重みは、その中間ノードに葉ノードとして属する全ての接地部位が接地していない時期では 0に設定される。逆に言えば、中間ノードの重みは、その中間ノードに属するいずれかの接地部位が接地している時期（厳密にはいずれかの接地部位に 0でない床反力が作用している時期）では 0には設定されない。

補足すると、第 1実施形態では、中間ノード Q14 に属する第 1接地部位 1 0および第 4接地部位 1 0のいずれか一方のみを非接地状態とする時期を有しないため、これらの接地部位 1 0， 1 0に対応する重み Wl , W4 は 0または 1 となる峙期を持たないものとなっている。ただし、第 1接地部位 1 0および第 4接地部位 1 0のいずれか一方のみが非接地状態となる時期を有するように、目標接地部位軌道を決定するようにした場合には、その時期で非接地状態となる接地部位 1 0に対応する重みを 0に設定し、且つ、接地状態となる接地部位 1 0に対応する重みを 1 に設定するようにすればよい。この場合、その時期における中間ノ —ド Q14 の重みは、 0で無い値に設定される。中間ノード Q23、並びにこれに属する葉ノードである第 2接地部位 1 0および第 3接地部位 1 0に係る重みについても同様である。一般的に言えば、子ノードを持つノードの重みは、そのノードに属する接地部位のいずれかが接地状態であるときには、 0で無い値に設定され、そのノードに属する接地部位の全てが非接地状態となるときに、 0に設定される。

上記のように求めた各目標ノード床反力は連続的に変化するので、口ポット 1の衝撃の少ない移動（歩行）を実現するために適している。以上の如く、第 1実施形態では、各目標ノード床反力中心点 Qn (n=l, 2, 3, 4, 14,23, 1423) , 各重み Wn(n=l，2，3,4, 14,23) および各目標各床反力 F n (n=l, 2, 3,4, 14, 23,1423) は、目標床反力分配器 1 0 2によって次の条件 A) 〜G) を満たすように決定される。

A) 各葉ノードの目標ノード床反力中心点 Qn(n=l，2,3，4)は、該葉ノ一ドに対応する接地部位 1 0の中心点に一致する。より一般的には、 Qn(n=l，2，3,4)は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。補足すると、例えば各接地部位 1 0の目標床反力中心点を歩容生成装置 1 0 0で決定するようにしてもよく、その場合には、歩容生成装置 1 0 0が決定する目標床反力を基に、目標ノード床反力中心点 Qn(n=l,2,3，4)を決定すればよい。

B) 根ノードの目標ノード床反力中心点は、目標全床反力中心点 Pに一致する。

C ) 根ノードを除く各ノードの目標ノード床反力中心点 Qn (n=l，2, 3, 4,14,23) と重み Wn (n=l,2,3,4,14，23) とは、前記式 1〜 3 の関係式を満たす。換言すれば、子ノードを持つ任意の第 nノード (!1=14，23,1423)の目標ノード床反カ中心点（¾11 は、その第 nノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。 D ) 子ノードを持つ任意の第 nノード（n= 14，23，1423 ) の目標ノード床反力 Fn は、該第 nノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノ一ド（第 1423 ノード）の目標ノード床反力 F1423は目標全床反力 Ftotalref に一致する。換言すれば、第 n ノード（n= l,2, 3，4, 14，23，1423) の目標床反力 Fn および重み Wn が図 8中の関係式を満たす。

E ) 接地していない接地部位 1 0に対応する葉ノードの目標ノード床反力は 0となる。

F ) Qn, Wn, Fn ( n= l, 2, 3, 4, 14, 23, 1423) は連続的に変化する。

G ) 非接地状態の接地部位 1 0に対応する葉ノードの重み、または、その葉ノードの祖先ノードのいずれか 1つの重みは 0とされる。

なお、目標ノード床反力を決定する際に、前記のごとく重みを基に決定する代わりに、各目標ノード床反力中心点を基に決定しても良い。すなわち、前記 A ) 〜C ) の条件を満たすように各目標ノード床反力中心点を決定した上で、その目標ノード床反力中心点と、前記式 1〜3とを基に、各重みを決定し、その決定した重みを用いて前記式 4により目標各ノード床反力を決定するようにしても良い。

図 2の説明に戻って、前記姿勢偏差演算部 1 0 3は、ロボット 1に目標上体姿勢に対する実上体姿勢の偏差を算出して出力するものである。本実施形態では、該姿勢偏差演算部 1 0 3には、前記姿勢センサ 3 6で検出された上体 2 4の鉛直方向に対する傾き角（以下、実上体姿勢傾斜という）と、歩容生成装置 1 0 0で決定された目標上体位置姿勢（詳しくは、目標上体姿勢のうちの鉛直方向に対する傾き角。以下、目標上体姿勢傾斜という）とが入力され、これらの偏差 0 berr (実上体姿勢傾斜 —目標上体姿勢傾斜。以下、上体姿勢傾斜偏差 0 berr という）が算出される。算出する 0 berr は、 X軸まわり成分（ロール方向成分） Θ berrx と、 Y軸まわり成分（ピッチ方向成分） 0 berry とから構成される。なお、目標上体姿勢傾斜が例えば定常的に 0であるような場合には. 6» berr =実上体姿勢傾斜であるので、姿勢センサ 3 6の検出値（実上体姿勢傾斜）をそのまま、上体姿勢傾斜偏差 0 berr として出力すればよい。

前記姿勢安定化制御演算部 1 0 4は、一般的に言えば、前記上体姿勢傾斜偏差など、ロボット 1に備えられたセンサの情報に基づいて検出または推定されるロボッ卜 1の状態に応じて口ポット 1の姿勢を安定化するための全床反力の補償量（目標全床反力の補正量）である補償全床反力を算出するものである。

ロボット 1の姿勢を長期的に安定化させるためには、ロボット 1の上体 2 4などの所定の部位の実際の位置姿勢を望ましい位置姿勢に復元させるために必要な並進力とモーメントとを求め、これを目標全床反力中心点（目標 Z M P ) を作用点として付加的に発生させる必要がある。この付加的な並進力とモーメントとを補償全床反力という。また、補償全床反力のモーメント成分を「補償全床反力モーメント M dmd」（詳しくは目標全床反力中心点（目標 Z M P ) まわりの補償全床反力モーメント M dmd) と呼ぶ。

本実施形態では、姿勢安定化制御演算部 1 0 4は、実上体姿勢傾斜を目標上体姿勢傾斜に復元する（近づける）ように補償全床反力モーメント Mdmd を算出する。このために、姿勢安定化制御演算部 1 0 4には、前記姿勢偏差演算部 1 0 3で求められ'た上体姿勢傾斜偏差 0 berr ( Θ berrx, Θ berry) が入力される。

姿勢安定化制御演算部 1 0 4は、入力された上体姿勢傾斜偏差 0 berr を基に、前記補償全床反力モーメント Mdmd を算出する。算出する Mdmd は、 X軸まわり成分 M dmdx と、 Y軸まわり成分 M dmdy とから構成される。

Mdmdx および Mdmdy は、具体的には、例えば次式 5 , 6のフィ一ドバック制御則（ここでは P D制御則）によって決定される。すなわち Mdmdxおよび Mdmdy は、上体姿勢傾斜偏差 0 berrx, 0 berry を 0に近づけるように決定される。

Mdmdx^ -Kthx* Θ berrx- Kwx * (άθ berrx/dt) …式 5

Mdmdy = - Kthy * Θ berry - Kwy * (d Θ berry/dt) …式 6 ここで、 Kthx， Kthy, Kwx および Kwy は、所定のゲインである。また、（ d 0 berrx/dt)，（ d 0 berry/dt) 1ま、それぞれ上体姿勢傾斜偏差 0berrx、 0 berryの時間微分値である。

本実施形態においては、補償全床反力モーメント Mdmd の Z軸まわり成分（ョ一方向成分） Mdmdz を用いないので、 Mdmdz の決定を行なわないが、口ポット 1のスピン（鉛直軸回りの滑り）を防止したりするために Mdmdz を決定するようにしても良い。 Mdmdz の決定法に関しては、本願出願人が先に提案した特願 2 0 0 3 — 1 8 5 6 1 3号ゃ特願 2 0 0 3— 1 8 5 9 3 0号にて詳細に説明されている。また、例えばロボット 1の実際の重心の位置を目標歩容での重心の位置に復元するために、その重心の位置偏差に応じて補償全床反力の並進力を決定するようにすることも可能である。

前記床反力検出器 1 0 8は、各脚 # 1〜 # 4の 6軸力センサ 3 4の出ガを基に、実際のロボット 1の各接地部位 1 0に作用する床反力の実際値である実床反力（すなわち葉ノードの実床反力（実ノード床反力））を検出する。さらに、床反力検出器 1 0 8は、口ポット 1の各関節 1 4， 1 5に備えたエンコーダ等のセンサ（図示しない）によって検出される各脚 # 1〜 # 4の各関節 1 4 , 1 5の実関節変位（各関節 1 4 , 1 5の各回転軸まわりの実回転角）に基づき、上体 2 4に固定された座標系に対する各接地部位 1 0の相対位置姿勢（第 1実施形態では相対位置）を算出する。なお、このとき、実関節変位の代わりに、各関節 1 4 , 1 5 の変位指令値（回転角指令値）である関節変位指令を用いたり、あるいは、実関節変位と関節変位指令との両者を使用してもよい。そして、算出した各接地部位 1 0の相対位置姿勢を基に、各脚 # 1〜# 4毎の 6軸力センサ 3 4の検出値（これは 6軸力センサ 3 4などに固定された口一カル座標系での値である）を座標変換して、上体 2 4に固定された座標系で表現された実床反力を算出した後、これを支持脚座標系（グローバル座標系）で表現された実床反力に変換する。支持脚座標系への座標変換には、姿勢センサ 3 6の検出値、あるいは目標上体姿勢傾斜を用いればよい。補足すると、第 1実施形態のロボット 1では、前記したように接地部位 1 0の中心点に床反力モーメントが作用しないので、各接地部位 1 0の実床反力のうちのモーメント成分を検出する必要はない。この場合、前記したように 6軸力センサ 3 4の代わりに、 3軸力センサを用いて、実床反力の 3軸方向の並進力成分を検出したり、 1軸床反力センサを用いて実床反力の並進力鉛直成分だけを検出するようにしてもよい, 前記ロポット幾何学モデル（逆キネマテイクス演算部） 1 1 0は、各接地部位位置姿勢の最終的な目標軌道（これは後述の階層型コンプライアンス動作決定部 1 1 4により決定される）と目標上体位置姿勢などを基に、逆キネマテイクスの演算によって、それらを満足する口ポット 1 の.各関節 1 4， 1 5の変位（回転角）の指令値である各関節変位指令を算出する。本実施形態では、逆キネマテイクスの演算の解の式をあらかじめ求めておき、その式に目標上体位置姿勢と各接地部位の最終的な目標位置を代入するだけで各関節変位指令を算出するようにした。すなわち、ロポット幾何学モデル 1 1 0は、歩容生成装置 1 0 0で決定された目標上体位置姿勢軌道と階層型コンプライアンス動作決定部 1 1 4で後述する如く修正された修正目標接地部位軌道（機構変形補償入り修正目標接地部位軌道）とが入力され、それらの入力値から逆キネマテイクスの演算によって、各脚 # 1〜# 4の各関節 1 4 , 1 5の関節変位指令を算出する。

なお、後述する第 6の実施形態のように腕関節や首関節など脚関節以外の関節がある口ポットの場合には、手や頭部などの上体に対する相対位置姿勢を基に、逆キネマテイクスの演算によって、脚関節以外の関節の変位が決定される。

前記変位コントローラ 1 1 2は、ロボット 1 の各関節 1 4， 1 5の実関節変位（検出値）と、口ポット幾何学モデル（逆キネマテイクス演算部） 1 1 0で算出された関節変位指令とが入力され、該関節変位指令を目標値として、この目標値に実関節変位を追従させるように各関節 1 4 , 1 5のァクチユエ一夕（図示しない）を制御（フィードバック制御）する。

階層型コンプライアンス動作決定部 1 1 4は、目標全床反力と補償全床反力との合力に実全床反力を近づけるように目標接地部位軌道を修正し、その修正後の目標接地部位軌道である機構変形補償付き修正目標接地部位位置姿勢軌道を出力するものである。なお、本実施形態では、各接地部位 1 0の姿勢は制御できないので、機構変形補償付き修正目標接地部位位置姿勢軌道は、実際には、機構変形補償付き修正目標接地部位位置軌道である。

階層型コンプライアンス動作決定部 1 1 4は、一般的には、以下の 3 つの要求をできるだけ満足させるように各接地部位 1 0の目標接地部位軌道を修正するものである。要求 1 ) ロボット 1の位置姿勢を安定化するために、実全床反力を姿勢安定化制御演算部 1 0 4 が出力する補償全床反力（モーメント Mdmd) と目標全床反力との合力に追従させる。なお、第 1実施形態では口ポット 1の上体 2 4の姿勢傾斜（鉛直方向に対する傾き）を安定化するため、目標全床反力中心点まわりの実全床反力モーメントの水平成分を補償全床反力モーメント M dmd の水平成分 Mdmdx, Mdmdy に追従させる。補足すると、目標全床反力中心点まわりの目標全床反カモ一メントの水平成分は 0であるので、これと Mdmd との合力は、 Mdmd に一致する。

要求 2 ) 接地しているべき複数の接地部位 1 0に対し、その一部の接地部位 1 0に実床反力が集中し、それ以外のある一部の接地部位 1 0の実床反力が減少し過ぎ、その実床反力が減少した接地部位 1 0の接地性が極端に低下しないように、できる限り葉ノードでない各ノードの目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。なお、第 1実施形態では、第 14ノード、第 23ノード、第 1423ノードの目標床反力中心点まわりの実床反力モーメン卜の絶対値をできる限り小さくする。

要求 3 ) 接地しているべき各接地部位 1 0の接地性を確保するように、すなわち各接地部位 1 0における口一カルな接地圧分布（実床反力の分布）が偏って、該接地部位 1 0の局所的な接地性が低下しないように、できるかぎり各接地部位 1 0 (各葉ノード）の目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。但し、第 1実施形態の口ポット 1では、各接地部位 1 0の目標床反力中心点まわりの実床反カモ —メントは常に 0となるので、この要求 3 ) は考慮する必要はない。

尚、一般に、要求 1 ) 〜 3 ) の全て、あるいは要求 1 )、 2 ) を完全に満足することはできない。例えば実全床反力を補償全床反力と目標全床反力との合力に一致させながら各接地部位 1 0の目標床反力中心点まわりの実床反力モーメントを 0にすることが、物理的に不可能な場合が多い。従って、階層型コンプライアンス動作決定部 1 1 4は、一般には要求 1 ) 〜 3 )、あるいは要求 1 )、 2 ) をできるだけ満たしつつ、ある妥協点で、各接地部位 1 0の目標接地部位軌道を修正することとなる。

以上が制御装置 5 0の各機能的手段（機能的構成要素）の概要である, 補足すると、前記階層型コンプライアンス動作決定部 1 1 4、姿勢安定化制御演算部 1 0 4およびロポット幾何学モデル（逆キネマティクス演算部） 1 1 0は、本発明におけるノード動作制御手段に相当するものである。

次に、図 9のフローチャートを参照しつつ、制御装置 5 0の全体的な動作（演算処理）をより詳細に説明する。図 9は制御装置 5 0のメインルーチン処理を示すフローチャート（構造化フローチャート）である。尚、図 9の左端には、該当する処理を行う制御装置 5 0の構成要素を示す。

先ず、 S 1 0において制御装置 5 0の初期化が行なわれた後、 S 1 2 を経て S 1 4に進み、制御装置 5 0の演算処理は、該制御装置 5 0の制御周期毎の夕イマ割り込みを待つ状態となる。制御周期は例えば 5 0 m Sである。

続いて S 1 6に進んで歩容の切り替わり目か否か判断し、その判断結果が N Oであるときは後述の S 2 2に進む。また、 S 1 6の判断結果が Y E Sであるときは S 1 8に進んで時刻 t を 0に初期化した後、 S 2 0 に進んで歩容パラメ一夕を設定する。本実施形態では、例えば口ポット 1のある所定の脚（例えば # 1 ) が離床してから次に離床するまで（もしくは着地してから次に着地するまで）の所定期間分の目標歩容を単位として、その所定期間分の目標歩容を規定するパラメ一夕（目標歩容を決定するためのアルゴリズムで使用するパラメ一夕）である歩容パラメ —夕が S 2 0で設定される。そして、 S 1 6でいうところの「歩容の切り替り目」は、上記所定期間分の目標歩容の切り替り目のことである。なお、目標歩容の切り替り目の判断は、時刻や、上記所定の脚の 6軸力センサ 3 4の検出値などを基に行えばよい。

S 2 0で設定される歩容パラメ一夕は、口ポット 1 の目標運動軌道 (詳しくは目標接地部位軌道および目標上体位置姿勢軌道）を規定する運動パラメ一夕と、目標床反力軌道（詳しくは目標全床反力軌道および目標全床反力中心点軌道）を規定する床反力パラメ一夕とから構成される。補足すると、口ポット 1の目標運動が決まれば、その目標運動によつて発生する慣性力とロボット 1に作用する重力との合力の符号を反転させたものを目標全床反力とすればよいので、床反力パラメ一夕は、目標全床反力中心点軌道のみを規定するものでもよい。また、目標接地部位軌道（より一般的には目標上体位置姿勢以外の目標運動のパラメ一夕）と目標全床反力中心点とを決定した上で、口ポット 1の動力学モデルを使用し、口ポット 1の目標上体位置姿勢を含む目標運動が発生する慣性力と口ポット 1に作用する重力との合力が目標全床反力中心点（目標 Z M P ) まわりに発生するモーメントの水平成分が 0になるように、目標上体位置姿勢を決定するような場合には、運動パラメ一夕には、目標上体位置姿勢軌道を規定するパラメ一夕が含まれていなくてもよい。続いて S 2 2に進み、上記歩容パラメ一夕を基に、目標歩容の瞬時値を決定する。ここで「瞬時値」は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置姿勢、目標接地部位位置（目標接地部位軌道の瞬時値）、目標全床反力、および目標全床反力中心点位置（目標 Z M P 位置）の瞬時値から構成される。なお、本実施形態では、接地部位 1 0 の姿勢を制御できないので、各接地部位 1 0の目標姿勢の瞬時値は決定しない。各接地部位の姿勢を制御できる場合には、前記歩容パラメータに各接地部位の目標姿勢を規定するパラメータを含めておき、そのパラメータを基に各接地部位の目標姿勢の瞬時値を決定すればよい。

上記 S 1 4〜S 2 2の処理は、前記歩容生成装置 1 0 0により実行される処理である。

続いて S 2 4に進んで、各ノードの重み Wn(n=l,2,3,4，14，23) と各ノードの目標床反力中'心点（目標ノード床反力中心点） <¾!1(11=1，2,3,4, 14，23)とが求められる。この処理は目標床反力分配器 1 0 -2によって前記した如く実行される処理である。

続いて S 2 6に進んで、目標ノード床反力（少なくとも各接地部位 1 0 (各葉ノード）の目標床反力を含む）が求められる。第 1実施形態では、各葉ノードの目標ノード床反力 '（各目標接地部位床反力）が求められる。この S 2 6処理も、目標床反力分配器 1 0 2によって前記した如く実行される処理である。なお、前述の通り、各脚 # 1〜# 4の先端部以外にもコンプライアンス機構を設けた場合には、さらに葉ノードでない各中間ノードの目標床反力も求めた方が良い。補足すると、目標ノード床反力のモーメント水平成分は 0である。

続いて S 2 8に進み、姿勢センサ 3 6などの出力から前記実上体姿勢傾斜などのロボット 1の状態を検出する。第 1実施形態では、前記姿勢偏差演算部 1 0 3によって、姿勢センサ 3 6による実上体姿勢傾斜の検出値が取り込まれ、その検出値と目標上体位置姿勢のうちの目標上体姿勢傾斜（現在時刻の瞬時値）とから、上体姿勢傾斜偏差 0 berr が算出される。

続いて S 3 0に進み、 S 2 8で検出したロボット 1の状態などから口ポット 1の姿勢を安定化するための補償全床反力を求める。第 1実施形態では、前記姿勢安定化制御演算部 1 0 4によって、前記上体姿勢傾斜偏差 0 berr から、目標全床反力中心点（目標 Z M P ) まわりの補償全床反力モーメント Mdmd の水平成分 Mdmdx, Mdmdy が前記式 5および式 6に従って算出される。

続いて S 3 2に進んで各接地部位 1 0の実床反力を検出する。これは前記実床反力検出器 1 0 8によって実行される処理であり、前記の如く . 6軸力センサ 3 4によって検出された各接地部位 1 0毎の実床反力を支持脚座標系（グロ一バル座標系）に変換したものが求められる。以降、各接地部位 1 0の実床反力を実接地部位床反力ということがある。

続いて S 3 4から S 3 8まで、前記階層型コンプライアンス動作決定部 1 1 4の処理が実行される。

以下、しばらくの間、これらの S 3 4〜 S 3 8の処理を具体的に説明する前に、階層型コンプライアンス動作決定部 1 1 4の全体的な処理を説明する。なお、この説明は、理解の便宜上、主に前記図 3 ( b ) に示す状態（口ポット 1 の全ての脚 # 1〜# 4が支持脚となっている状態）を例にとつて行なう。

階層型コンプライアンス動作決定部 1 1 4は、目標床反力分配器 1 0 2で決定された各葉ノードの目標ノード床反力などを基に、葉ノードを除く各ノードの目標ノード床反力の並進力成分とモーメント成分（より詳しくは目標ノード床反力中心点を作用点とする並進力成分とモ一メント成分）とを決定する。

図 3 ( b ) の状態における各ノードの目標床反力（各目標ノード床反力）の並進力成分を、例示的に図 1 0に示す。この図において、べクトル F n一 ref ( n=l,2, 3,4, 14,23) が目標第 nノード床反力の並進力成分を表している。また、 F totalref は目標全床反力（=目標第 1423 ノード床反力）の並進力成分を表す。前記図 8に示したように、葉ノードでない任意の第 nノード（n= 14,23，1423)の目標第 nノード床反力は、第 nノ —ドのすベての子ノードの目標ノード床反力の合力に決定される。従つて、 F14ref = Flref + F4ref、 F23ref = F2ref + F3ref , Ftotalref( = Fl423ref) = F14ref+ F23ref である。階層型コンプライアンス動作決定部 1 1 4は、目標床反力分配器 1 0 2で決定された各葉ノード（各接地部位 1 0 ) の目標ノード床反力の並進力成分 Fn_ref(n= l，2，3,4)から、上記の如く、各中間ノードの目標ノード床反力の並進力成分 F14ref

( = Flref + F4ref) F23ref ( = F2ref + F3ref ) を求める。なお、 Ftotalref( = F1423ref)は、前記歩容生成装置 1 0 0で決定された目標全床反力の並進力成分に設定される。

さらに、階層型コンプライアンス動作決定部 1 1 4は、目標ノード床反力の並進力成分の場合と同様に、葉ノードを除く各ノードの目標ノード床反力の、目標ノード床反力中心点を作用点とするモーメント成分を決定する。ただし、この場合、目標ノード床反力中心点 Qn ( n=l, 2, 3,4, 14,23) の定義より、目標第 n ノード床反力のモーメント水平成分は、常に 0に設定される。目標第 1423 ノード床反力（=目標全床反力）のモーメント水平成分も 0に設定される。

本実施形態のロポット 1 においては、各接地部位 1 0は各脚 # 1〜# 4の先端部の球関節 1 2 (フリ一ジョイント）に係合しているので、各接地部位 1 0 (各葉ノード）に床反力モーメント（水平成分および鉛直成分）を発生させることができない。このため、階層型コンプライアンス動作決定部 1 1 4では、各接地部位 ί θ (各葉ノード）の目標ノード床.反力のモ一メント鉛直成分も 0に設定される。

葉ノードでないノードの目標ノード床反力モーメント鉛直成分は、口ポット 1 の目標運動に動力学的に釣り合うように決定すると、一般的には 0ではない値も採り得るが、本実施形態では、ロボット 1 の姿勢の鉛直軸まわりの回転（ョ一方向の回転）に関する制御を行なわない。このため、本実施形態では、葉ノードでないノードの目標ノード床反力のモ —メントの鉛直成分の設定は省略する。このようなことから、図 3 ( b ) の状態における各目標ソード床反力のモーメント成分の図示は省略する。尚、ロボット 1 の姿勢の鉛直軸まわりの回転に関する制御を行なう場合には、各ノードの目標床反カモ一メント鉛直成分も設定すべきである。

補足すると、葉ノードを含めた各ノードの目標ノード床反力の並進力成分およびモーメント成分の決定を、前記目標床反力分配器 1 0 2で行なうようにした場合には、その決定を階層型コンプラインアンス動作決定部 1 1 4で行なう必要はない。

また、階層型コンプライアンス動作決定部 1 1 4は、各ノードの実床反力である実ノード床反力の並進力成分およびモーメント成分も決定する。

図 3 ( b ) の状態における各ノードの実床反力（実ノード床反力）の並進力成分を、例示的に図 1 1 に示す。この図において、ベクトル F n— act ( n=l,2, 3,4, 14,23) が実第 nノード床反力の並進力成分を表している。また、 F totalact は実全床反力（=実第 1423 ノード床反力）の並進力成分を表す。一般に、葉ノードでない各ノードの実床反力の並進力成分は、そのノードのすべての子ノードの実床反力の並進力成分の合力とする。従って、第 14 ノード、第 23 ノードおよび第 1423 ノードの実床反力の並進力成分は F14act = Flact + F4act、 F23act = F2act + F3act、 Ftotalact(= F1423act) = F14act + F23actである。また、各葉ノードの実床反力の並進力成分 Flact, F2act, F3act, F4actは、前記実床反力検出器 1 0 8で得られる各接地部位 1 0の実床反力（実接地部位床反力）の並進力成分である。なお、図 1 1の点線で示したベクトルは、前記図 1 0に示した目標ノード床反力の並進力成分である。階層型コンプライアンス動作決定部 1 1 4は、上記のように、実床反力検出器 1 0 8で得られた各接地部位 1 0の実床反力から各ノードの実ノード床反力の並進力成分を決定する。

また、図 3 ( b ) の状態における各ノードの実床反力のモーメント成分を、例示的に図 1 2 に示す。この図において、ベクトル Mn— act ( n=14,23, 1423 ) は実第 nノード床反力のモーメント成分を表す。この場合、各ノードの実床反力の並進力成分と同様に、一般的には、葉ノ — ドでないノードの実床反力のモーメント成分 M14act、 M23act、 Ml423act ( = Mtotalact) は、そのノードのすべての子ノードの実床反力の合力のモーメント成分（そのノードの目標床反力中心点 Q 14, Q23 , Q 1423を作用点とするモーメント成分）とする。

本実施形態のロボット 1 においては、前記したように各接地部位 1 0 (各葉ノード）に床反力モーメントを発生させることができないので、各葉ノードの実床反力（実接地部位床反力）のモーメント成分は、常に 0になる。よって、各葉ノードの実ノード床反力のモーメント成分の図示は省略する。

葉ノードでないノードの実床反力モーメント成分（本実施形態では、 M14act , M23act , M1423act) 'は、一般的に、 0 にならない。例えば目標第 14 ノード床反力中心点 Q 14のまわりには、一般には、第 1接地部位 1 0 (第 1 ノード）および第 4接地部位 1 0 (第 4ノード）の実床反力の並進力成分の水平成分によってモーメン卜が発生する。ただし、本実施形態のロポット 1 においては、脚の先端部にフリージョイントである球関節 1 2を備えているので、 M14act の線分 Q 1Q4 と同方向の成分、 M23actの線分 Q2Q3 と同方向の成分は 0 となる。

尚、各脚の先端部の関節にァクチユエ一夕を備えたロボット（各脚の先端部の接地部位の姿勢を制御可能な口ポット）においては、該ロボッ卜の姿勢の鉛直軸まわり回転に関する制御を行なわない場合でも、本願出願人が先に提案した特開平 1 0— 2 7 7 9 6 9号公報の実施例に示すごとく、各接地部位の目標床反力モーメント水平成分も設定し、また、各接地部位の実床反力モーメントも検出する。そして、各接地部位の実床反力モーメント水平成分を目標床反力モーメント水平成分に近づけるように、あるいは、各接地部位の実床反力モーメントを、目標床反カモーメント水平成分とその接地部位補償床反力モーメントとの和（ベクトル和）に近づけるように、各接地部位の姿勢の補正動作を行うべきである。

階層型コンプライアンス動作決定部 1 1 4の処理をより詳細に説明する。この場合、図 3 ( b ) に示す状態で、ロボット 1 の上体 2 4の姿勢が左後ろ側に倒れそうな状態となり、姿勢安定化制御演算部 1 0 4が決定した補償全床反力モーメント Mdmd が、図 1 3のようになっている状況を想定する。

ロボット 1の上体 2 4の姿勢（鉛直方向に対する傾き）を復元させる (目標上体姿勢における傾きに復元させる'）ためには、目標全床反力中心点（目標 Z M P ) まわりの実全床反力モーメントの水平成分を、目標全床反力モーメント Mtotalref ( = M1423ref) と補償全床反力モーメント Mdmdとの和の水平成分に追従させれば良い。

一方、目標全床反力中心点（目標 Z M P ) では目標全床反カモ一メント Mtotalref の水平成分は 0である。従って、口ポット 1の上体 2 4の前後左右の姿勢（傾き）を復元させるためには、目標全床反力中心点 (目標 Z M P ) まわりの実全床反力モーメントの水平成分を、 M dmd の水平成分（Mdmdx, Mdmdy) に追従させれば良い。また、本実施形態では、各接地部位 1 0の目標床反力中心点まわりの実床反力モーメン卜は 0である。

そこで、第 1実施形態における階層型コンプライアンス動作決定部 1 1 4は、前記した要求 1 )、 2 ) の要求をできる限り満足するよう歩容生成装置 1 0 0で決定した各接地部位 1 0の目標接地部位位置（特に高さ方向の位置）を修正する。

この修正を行なうために、階層型コンプラインァス動作決定部 1 1 4 は、補償角というものを決定する。この補償角は、接地部位 1 0の位置の間の相対関係をある点まわりの回転操作によって修正する（本実施形態では上下方向で修正する）ための操作量（回転量）である。本実施形態においては、その補償角としては、第 14 ノード補償角 Θ 14、第 23 ノード補償角 023、第 1423 ノード補償角 01423 がある。すなわち、葉ノード以外のノードの補償角である。そして、階層型コンプライアンス動作決定部 1 1 4は、これらの補償角 Θ 14， Θ 23, 01423 を基に、目標第 ηノード床反力中心点 Qn (n=l, 2, 3, 4,14,23) を修正してなる目標第 nノード床反力中心点 Qn'を決定する。

図 1 5に示すように、第 14 ノード補償角 Θ 14 は、線分 Q1Q4 と線分 Q1'Q4'とが成す角度、第 23 ノード補償角 Θ 23 は、線分 Q2Q3 と線分 Q2'Q3'とが成す角度である。また、図 1 6に示すように、第 1423 ノード補償角 01423 は、線分 Q14Q23 と線分 Q14'Q23'とが成す角度である。これらのノード補償角 014, Θ 23, 01423 の決定手法は後述する。

図 1 5および図 1 6を参照して、補償角 Θ 14, Θ 23, Θ 1423 を基にした目標第 nノード床反力中心点 Qn (n=l, 2, 3,4,14,23) の修正は次のように行なわれる。

図 1 5を参照して、目標第 1 ノード床反力中心点 Q1 (第 1接地部位 1 0の目標床反力中心点）と目標第 4ノード床反力中心点 Q4 (第 2接地部位 1 0の目標床反力中心点）とを含みかつ水平面と垂直な平面の法線ベクトル V14 を求める。 V14 の大きさは 1 とする。目標第 1 ノード床反力中心点 Q 1 の座標（位置）を、目標第 14 ノード床反力中心点 Q 14を回転中心として法線べクトル V14まわり（Q 14を通って V14に平行な軸まわり）に、前記第 14ノード補償角 0 14だけ回転移動する。この回転移動による Q 1 の移動後の点を Q 1'とする。同様に、目標第 4 ノード床反力中心点 Q4 の座標（位置）を、目標第 14 ノード床反力中心点を回転中心として法線べクトル V14まわりに第 14ノード補償角 Θ 14だけ回転移動する。この回転移動による Q4の移動後の点を Q4'とする。換言すれば、線分 Q 1Q4 をその内分点である Q 14 を回転中心として V14まわりに 0 14だけ回転させて得られる線分の端点を Ql'， Q4'とする。従って、第 14 ノ一ド補償角 0 14 は、第 14 ノードの目標床反力中心点 Q 14の位置を動かさずに、第 14ノードの子ノードである第 1 ノードおよび第 4ノードのそれぞれの目標床反力中心点 Q l， Q4 の位置の相対関係を動かすための操作量である。

また、目標第 2ノード床反力中心点 Q2 と目標第 3ノード床反力中心点 <¾3 を含みかつ水平面と垂直な平面の法線ベクトル V23 を求める。 V23 の大きさは 1 とする。目標第 2ノード床反力中心点 Q2 の座標（位置）を、目標第 23ノード床反力中心点 Q23を回転中心として法線べクトル V23 まわり（Q23 を通って V23 に平行な軸まわり）に、第 23 ノード補償角 0 23 だけ回転移動する。図 1 5に示すように、この回転移動による Q2 の移動後の点を Q2'とする。同様に、目標第 3接地部位床反力中心点 Q3 の座標（位置）を、目標第 23 ノード床反力中心点を回転中心として法線ベクトル V23 まわりに第 23 ノード補償角 Θ 23 だけ回転移動する。この回転移動による Q3 の移動後の点を Q3'とする。換言すれば、線分 Q2Q3 をその内分点である Q23 を回転中心として V23 まわりに 0 23 だけ回転させて得られる線分の端点を Q2'， Q3'とする。従って、第 23 ノード補償角 0 23 は、第 23 ノ一ドの目標床反力中心点 Q23 の位置を動かさずに、第 23 ノードの子ノ一ドである第 2ノードおよび第 3 ノードのそれぞれの目標床反力中心点 Q2， Q3 の位置の相対関係を動かすための操作量である。

次に、図 1 6を参照して、目標第 14ノード床反力中心点 Q 14と目標第 23ノード床反力中心点 Q23を含みかつ水平面と垂直な平面の法線べクトル V1423 を求める。 V1423 の大きさは 1 とする。目標第 14 ノード床反力中心点 Q 14 の座標（位置）を、目標全床反力中心点 P ( =Q 1423) を回転中心として法線ベクトル V1423 まわり（Pを通って V1423 に平行な軸まわり）に、第 423 補償角 0 1423 だけ回転移動する。この回転移動による Q 14 の移動後の点を Q 14'とする。同様に、目標第 23接地部位床反力中心点 Q23の座標（位置）を、目標全床反力中心点 Pを回転中心として法線ベクトル V1423 まわりに第 1423 補償角 Θ 1423 だけ回転移動する。この回転移動による Q23 の移動後の点を Q23'とする。 ^言すれば、線分 Q 14Q23 をその内分点である P ( =Q 1423)を回転中心として V1423 まわりに 0 1423 だけ回転させて得られる線分の端点を Q 14'， Q23'とする。従って、第 1423 ノード補償角 0 1423 は、第 1423 ノードの目標床反力中心点 Pの位置を動かさずに、第 1423ノードの子ノードである第 14ノードおよび第 23ノードのそれぞれの目標床反力中心点 Q 14， Q23 の位置の相対関係を動かすための操作量である。 '

なお、以降、一般に、始点が A、終点が Bであるベクトルをベクトル A— Bと表記する。

次に、 Q 1'をベクトル Q 14— Q14'だけ移動した点 Q 1"を求める。同様に、 Q4'をベクトル Q 14— Q 14'だけ移動した点 Q4"を求める。また、 Q2' をベクトル Q23— Q23'だけ移動した点 Q2"を求める。同様に、 Q3'をべクトル Q23_Q23'だけ移動した点 Q3' 'を求める。

次いで、第 n接地部位（n=l，2,3,4) の目標接地部位位置をベクトル Qn_Qn"だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位 1 0の目標接地部位位置が修正されることとなる。補足すると、各ノ一ドの目標床反力中心点と同様に、各葉ノードの位置を該葉ノ一ドに対応する接地部位 1 0の位置とし、子ノードを持つ各ノードの位置を、そのノードの全ての子ノードの位置の、前記の如く設定された重みを用いた重み付け平均の位置として定義したとき、上記のように、第 14 ノード、第 23 ノード、第 1423 ノードのそれぞれの各子ノードの目標床反力中心点を補償角 0 14、 Θ 23 , Θ 1423 に応じて動かす処理は、第 14 ノード、第 23 ノード、第 1423 ノ一ドのそれぞれの各子ノードの位置を補償角 0 14、 Θ 23、 0 1423に応じて動かすことと同じことである。尚、各脚の先端部にフリージョイントがない口ポット（接地部位の姿勢を制御可能な口ポット）においては、上記平行移動を各接地部位の姿勢（目標姿勢）を変えずに行なった後に、さらに、第 n接地部位を Q _n"を中心に、前後方向軸（X軸）まわりに、ある回転角 Xだけ回転させると共に、左右方向軸（Y軸）まわりに、ある回転角 0 n— yだけ回転させる。これにより各接地部位の目標姿勢を修正する。この場合の回転角 0 n ( 0 n_x、 6 n_y からなる 2次元量）を第 n接地部位補償角と呼び、 Xを第 n接地部位補償角 X成分、 yを第 n接地部位補償角 Y成分と呼ぶ。第 n接地部位補償角 0 n は、本願出願人が先に提案した特開平 1 0 — 2 7 7 9 6 9号公報に記載された手法に従って決定すればよい。

補足すると、前記各ノード補償角は、そのノードの目標ノード床反力中心点を作用点として、そこに本来発生すべき目標床反力モーメント (その水平成分は 0 ) とノード補償床反力モーメント Mn_dmd との合力が発生するように決定される。この場合、ノード補償床反力モーメント Mn— dmdの基となる補償全床反力モーメント Mdmdは、実姿勢傾斜偏差を 0に近づけるように決定されたものである。従って、各ノード補償角は、目標全床反力中心点まわりの床反力モーメントを所望のモーメント（この場合は補償全床反力モーメント）に近づけつつ、実姿勢傾斜偏差を 0に近づけるよう各接地部位 1 0の相対位置関係を操作するための操作量として機能することとなる。

以上のごとく、各接地部位 1 0の目標接地部位位置（詳しくはその位置の相互の相対関係）を修正する処理が第 1実施形態における階層型のコンプライアンス動作である。このコンプライアンス動作では、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位 1 0の目標接地部位位置の間の相対関係（相対的位置関係）の操作量

(修正量）が各補償角 0 14， Θ 23, Θ 1423 に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位 1 0の目標接地部位位置の相互の相対関係が修正されることとなる。上記のように修正された目標接地部位位置姿勢を修正目標接地部位位置姿勢という。

このようなコンプライアンス動作では、一般に、前記補償角（補償動作量）が過大でなければ、各接地部位 1 0の接地圧力分布は変わっても、接地領域（接地面の圧力が正の領域）は変わらない。このような場合には、補償角に比例して各接地部位 1 0に装着されたコンプライアンス機構 4 2などが変形し、その変形量に応じた各接地部位 1 0の実床反力が発生する。この結果、補償角と補償角によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性、すなわち線形特性を持つ。特性 1 ) 葉ノードでないノードの目標床反力中心点まわりの補償角だけを操作して各接地部位 1 0の目標接地部位位置を移動させると、下がつた接地部位 1 0の実床反力の並進力成分が増加し、上がった接地部位 1 0の実床反力の並進力成分が減少する。このとき、各接地部位 1 0 (各葉ノード）の修正された目標床反力中心点まわりの実床反力モーメン卜は、ほとんど変化しない。

特性 2 ) 第 n接地部位補償角だけを操作して第 n接地部位の目標姿勢を回転させると、第 n接地部位の目標床反力中心点に作用する実床反力のモーメント成分が変化し、並進力成分はあまり変化しない。

特性 3 ) 葉ノードでないノードの目標床反力中心点まわりの補償角と第 n接地部位補償角を同時に操作すると、各接地部位 1 0の実床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。

図 1 7は、本実施形態における階層型コンプライアンス動作決定部 1 1 4の機能を示すブロック図であり、同図を参照して階層型コンプライアンス動作決定部 1 1 4の処理をさらに詳細に説明する。

階層型コンプライアンス動作決定部 1 1 4は、その機能的手段として. 補償全床反力モーメント分配器 1 1 4 a、補償角決定部 1 1 4 b， 1 1 4 c , 1 1 4 d、修正目標接地部位位置姿勢算出部 1 1 4 g、機構変形補償量算出部 1 1 4 n、および機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hを備えている。

補償全床反力モーメント分配器 1 1 4 aは、前記補償全床反力モ一メント Mdmd ( Mdmdx, Mdmdy) を、第 1423 ノード補償床反力モ一メント M1423dmd、第 14 ノード補償床反力モーメント M14dmd および第 23 ノ一ド補償床反力モーメント M23dmdに分配するものである。第 1423 ノ一ド補償床反力モーメント M1423dmdは、第 1423補償角 0 1423 を操作することによって発生する各接地部位 1 0 (詳しくは第 1〜第 4接地部位）の床反力の並進力成分が、目標第 1423 ノード床反力中心点（=目標全床反力中心点（目標 Z M P ) ) まわりに発生すべきモ一メントの目標値である。

第 1423 ノード補償床反力モーメント M1423dmd の前記ベクトル V1423 方向の成分（V1423 方向の軸まわりの成分）を M1423dmdv と記述する。尚、ベクトル V1423 は階層型コンプライアンス動作決定部 1 1 4のコンプライアンス動作の全体的な説明で定義したべクトルである（図 1 6参照）。 V1423 に直交し、且つ鉛直方向にも直交するべクトルを U1423 とすると、本実施形態では、第 1423 ノード補償床反カモ一メント M1423dmd の U1423 方向の成分（U1423 方向の軸回りの成分） M1423dmdu は 0に設定される。これは、本実施形態の口ポット 1 では、第 1423 ノード補償角 0 1423 を操作しても、床反力モーメントの U1423 方向成分を発生することはできないからである。また、本実施形態では鉛直軸まわりの口ポット 1の姿勢の回転を制御しないので、 M1423dmdの鉛直方向成分 M1423dmdz も 0に設定される。

第 14ノード補償床反力モーメント M14dmdは、第 14補償角 0 14を操作することによって発生する各接地部位 1 0 (詳しくは第 1および第 4接地部位）の床反力の並進力成分が、目標第 14 ノード床反力中心点まわりに発生すべきモーメントの目標値である。

第 14 ノ一ド補償床反力モーメント M14dmd のべクトル V14方向成分を M14dmdv と記述する。尚、ベクトル V14 は階層型コンプライアンス動作決定部 1 1 4のコンプライアンス動作の全体的な説明で定義したベクトルである（図 1 5を参照）。 V14 に直交し、且つ鉛直方向にも直交するベクトルを U14とすると、本実施形態では、第 14ノード補償床反力モーメント M14dmdの U14方向成分 M14dmduは 0に設定される。これは、本実施形態の口ポット 1では、第 14 ノード補償角 0 14を操作しても、床反力モーメントの U14 方向成分を発生することはできないからである。なお、本実施形態では、 M14dmd の鉛直方向成分も 0に設定される。

第 23 ノード補償床反力モーメント M23dmdは、第 23補償角 23を操作することによって発生する各接地部位 1 0 (詳しくは第 2および第 3接地部位）の床反力の並進力成分が、目標第 23 ノード床反力中心点まわりに発生すべきモーメントの目標値である。

第 23 ノード補償床反力モーメント M23dmd のべクトル V23 方向成分を M23dmdv と記述する：尚、ベクトル V23 は階層型コンプライアンス動作決定部 1 1 4のコンプライアンス動作の全体的な説明で定義したべクトルである（図 1 5を参照）。 V23 に直交し、且つ鉛直方向にも直交するベクトルを U23 とすると、本実施形態では、第 23 ノード補償床反力モーメント M23dmdの U23方向成分 M23dmduは 0に設定される。これは、本実施形態のロボット 1では、第 23 ノード補償角 0 23 を操作しても、床反力モーメントの U23 方向成分を発生することはできないからである。なお、本実施形態では、 M23dmd の鉛直方向成分も 0に設定される。

第 1423 ノ一ド補償床反力モーメント M1423dmd、第 14 ノード補償床反力モーメント Ml4dmd および第 23 ノード補償床反力モーメント M23dmdは、例えば次のように決定される。

任意の第 n ノードに対して、目標第 nノード床反力中心点に第 nノ一ド補償床反力モーメントを加えることと等価になるように、第 nノード補償床反力モーメン卜の水平成分を 0にしたまま、目標第 nノード床反力中心点を修正した場合における修正された目標第 nノード床反力中心点位置を、修正目標第 nノード床反力中心点位置と呼ぶ。

修正目標第 1423 ノード床反力中心点 Pmdfd ( = Ql423mdfd) , 修正目標第 1 4ノード床反力中心点 Ql4mdfd、修正目標第 2 3 ノ一ド床反力中心点 Q23mdfd と各ノード補償床反力モーメントとの関係を次式 7 〜 9に示す。なお、目標第 1423 ノード床反力中心点 Q1423 は全床反力中心点 P と一致するので、修正目標第 1423 ノード床反力中心点 Pmdfdは、修正目標全床反力中心点ということもある。

M1423dmd= (Pmdfd- P) * Ftotalref …式 7

M14dmd= (Q14mdfd- Q14) * F14ref …式 8

M23dmd=(Q23mdfd-Q23)* F23ref …式 9 なお、式 7中の（Pmdfd— P)など、 2つの点の差は、それらの点の位置ベクトルの差を意味する。また、 Ftotalref, F14ref、 F23ref は図 1 0に示したように、それぞれ第 1423 ノード、第 14ノード、第 23ノ一ドの目標ノード床反力の並進力成分である。

口ポット 1の接地性を高く保っためには、接地すべき接地部位 1 0の床反力が 0に近づき過ぎたり、負の値になろうとするように制御すべきではない。したがって、以下の条件 1 ) 〜 3 ) を満足すべきである。修正ノード存在位置条件 1 )

Pmdfd は、全床反力中心点 Pから線分 Q14Q23 の端点に近づき過ぎず、かつ、線分 Q14Q23 上にあること。このように、修正目標第 1423 ノード床反力中心点（修正目標全床反力中心点） Pmdfd が存在すべき範囲を、修正目標第 1423 ノード床反力中心点（修正目標全床反力中心点）の存在許容範囲と呼ぶ。 - 修正ノード存在位置条件 2 )

Q14mdfdは、目標第 14 ノード床反力中心点 Q14から線分 Q1Q4の端点に近づき過ぎず、かつ、線分 Q1Q4 上にあること。このように、修正目標第 14 ノード床反力中心点 Q14mdfd が存在すべき範囲を、修正目標第 14ノード床反力中心点の存在許容範囲と呼ぶ。

修正ノード存在位置条件 3 )

Q23mdfd は、目標第 23 ノード床反力中心点 Q23 から線分 Q2Q3 の端点に近づき過ぎず、かつ、線分 Q2Q3 上にあること。このように、修正目標第 23 ノード床反力中心点 Q23mdfd が存在すべき範囲を、修正目標第 23 ノ一ド床反力中心点の存在許容範囲と呼ぶ。一方、ロボット 1 に実際に発生する姿勢復元力（上体 2 4の傾きの、目標上体姿勢の傾きへの復元力）を適切にするためには、第 1423 ノード補償床反力モーメント Ml423dmd、第 14 ノード補償床反カモ一メント M14dmdおよび第 23 ノード補償床反力モ一メント M23dmd の合力が、補償全床反力モーメント Mdmd に略一致すべきである。すなわち. 以下の式 1 0を略満足すべきである。 Mdmd = M1423dmd+M14dmd+M23dmd …式 1 0 そこで、本実施形態では、第 1423 ノード補償床反力モーメント M1423dmd、第 14 ノード補償床反力モーメント M14dmd および第 23 ノード補償床反力モーメント M23dmd は、それらと前記式 7、式 8、式 9 とによって定まる修正目標ノード床反力中心点 Pmdfd, Q l4mdfd, Q23mdfd の位置が前記修正ノード存在位置条件 1 )、 2 ) および 3 ) を満足している限りは、次式 1 1〜 1 3によって決定される。

M1423dmd = Matl423 * Mdmd * V1423 ···式 1 1

Ml4dmd = Matl4 * Mdmd * V14 ···式 1 2

M23dmd = Mat23 * Mdmd * V23 ···式 1 3 ただし、 Matl423、 Matl4、 Mat23 はゲイン行列（第 3要素が 0である 1行 3列の行列）であり、これらは、式 1 1、式 1 2および式 1 3よつて求められる M1423dmd、 M14dmd、 M23dmd 力式 1 0を満足するように設定される。

より厳密には、式 1 0、式 1 1、式 1 2および式 1 3からなる連立方 '程式が Mdmd の値にかかわらず恒等的に成立するように、ゲイン行列 Matl423、 Matl4、 Mat23 'が決定される。上記連立方程式が恒等的に成立するためのゲイン行列は一義的に決定されるわけではないので、例えば、修正ノード存在位置条件 1 )、 2 ) および 3 ) のいずれを特に重視するかによって、それに適切なゲイン行列を決定すれば良い。補足すると、各補償角 0 1423, Θ 14, Θ 23 を連続的に変化させるために、ゲイン行列は連続的に変化させることが望ましい。また、ロボット 1の直立時であるか否かや、移動形態の違いなどによって、ゲイン行列の設定方針を変えても良い。

式 1 1、式 1 2および式 1 3にしたがって決定される第 1423 ノード補償床反力モ一メント M1423dmd、第 14ノード補償床反力モ一メント M14dmd および第 23 ノード補償床反力モ一メント M23dmd が修正ノード存在位置条件 1 )、 2 ) および 3 ) のいずれかを満足していない場合には、該条件 1 )、 2 )、 3 ) を満足するように、これらを修正する。より具体的には、修正目標第 nノード床反力中心点（n=1423，14,23 ) のうち、前記存在許容範囲を越えた修正目標ノード床反力中心点をその存在許容範囲の境界の点に設定し、その他の修正目標ノード床反力中心点を、式 1 1、式 1 2および式 1 3を可能な限り満足するように（左辺と右辺の差の絶対値がなるべく小さくなるように）決定する。ただし、一般的にフィードバック制御においてはフィードバック量が少々変っても、制御対象の挙動が大きく変わることはないので、式 1 1、式 1 2および式 1 3 を厳密あるいは強引に満足させる必要はない。

以上のごとく、補償全床反力モーメント分配器 1 1 4 aは、各ノード補償床反力モーメント M1423dmd、 Ml4dmd、 M23dmd を決定する。図 1 4には、このようにして決定された各ノード補償床反力モーメント M1423dmd、 Ml4dmd M23dmd の例を示した。図中の Mdmd は、前記図 1 3に示したものと同じである。

また、補償全床反力モーメント分配器 1 1 4 aは、各接地部位 1 0の目標床反力（各葉ノードの目標ノード床反力）である目標接地部位床反力、各ノード補償床反力モーメント M1423dmd、 M14dmd、 M23dmd を基に、目標接地部位床反力に各ノード補償床反力モーメントを付加することによって修正される各接地部位 1 0の目標床反力である修正目標接地部位床反力 Fn_refmdfd ( n= l ,2，3,4) を求める。

この際、各接地部位 1 0の目標床反力中心点（葉ノードの目標ノード床反力中心点） Qn(n= l，2, 3，4)、目標全床反力中心点 P、各目標ノード床反力中心点 Q 14、 Q23 と目標全床反力 Ftotalref とから、各目標ノード床反力を求める決定する手法と同様の手法によって、各接地部位 1 0 の目標床反力中心点 Qn(n= l,2, 3,4)、修正全床反力中心点 Pmdfd、各修正目標ノード床反力中心点 <¾ 14mdfd、 Q23mdfd と目標全床反力 Ftotalref とから、修正目標接地部位床反力 Fn_refmdfd ( n=l，2，3，4) を求めればよい。すなわち、各接地部位 1 0 の目標床反力中心点 Qn(n= l，2，3，4)、修正全床反力中心点 Pmdfd、各修正目標ノード床反力中心点 Q 14mdfd、 Q23mdfd から、前記式 1〜 3に基づいて各ノードの重みを決定し、その重みを用いて前記式 4に基づいて修正目標接地部位床反力 Fn— refmdfd ( n=l，2，3，4) を求めればよい。

このことから明らかなように、子ノードを持つ第 nノードの目標第 n ノード床反力中心点にノード補償床反力モーメントを発生させる（目標第 nノード床反力中心点に作用する目標床反力のモーメント成分を修正する）ということは、その第 nノードの子ノードの重みを修正することと同等である。

以上説明した補償全床反力モーメント分配器 1 1 4 aの処理は、図 1 8に示す機能ブロック図により表される。すなわち、目標ノード床反力中心点 Qn(n=l，2，3, 4,14,23)を基に、第 14 ノード、第 23 ノードおよび修正目標ノード床反力中心点 Qn— mdfd (n=14,23,1423) の存在許容範囲が前記修正ノード存在位置条件 1 )、 2 )、 3 ) に従って決定される。また、補償全床反力モーメント Mdmd と、各ノードの目標床反力中心点 Qn(n=l，2,3,4, 14,23, 1423) および目標床反力 ?11— 11=1,2，3，4，14，23，1423)と、上記存在許容範囲とを基に、修正目標ノ一ド床反力中心点 Qn— mdfd (n=14,23，1423)およびノード補償床反力モーメント Mn_dmd(n=14,23,1423)が決定される。また、修正目標ノード床反力中心点 Qn— mdfd(n=14,23,1423)と、各接地部位 1 0の目標床反力中心点 Qn(n=l,2,3,4)と、目標全床反力 Ftotalref とから、修正目標接地部位床反力 Fn— refmdfd (n=l,2,3,4) が求められる。

次に、前記補償角決定部 1 1 4 〜 1 1 4 01のぅちの補償角決定部 ( Θ 1423 決定部） 1 1 4 bの処理を図 1 9のブロック図を参照して説明する。目標第 14ノード床反力中心点 Q14に実第 1 ノード床反力と実第 4ノード床反力との合力の並進力成分（Flact+F4act) が作用すると共に、目標第 23 ノード床反力中心点 Q23 に実第 2 ノード床反力と実第 3.ノード床反力との合力の並進力成分（F2act+F3act) が作用した場合に、それらの並進力成分が、目標全床反力中心点 P (目標第 1423 ノード床反力中心点）のまわりに発生するモーメント Ml423actを、次式 1 4により求める。 M1423act = P_Q 14 * (Flact+F4act) + P_Q23 * (F2act+F3act)

…式 1 4 ここで、 P— Q 14 は始点が P、終点が Q 14 のべクトル、 P— Q23 は始点が P、終点が Q23のベクトルである。

なお、式 1 4の代わりに、次式 1 5を用いて M1423actを算出しても実際上はほとんど問題がない。 M1423act = P_Q l * Flact + P— Q2氺 F2act

+ P_Q3 * F3act + P_Q4 * F4act

…式 1 5 式 1 5の右辺は、第 1〜第 4ノード（各葉ノード）の実床反力の並進力成分 Fn_act(n= l,2，3，4)から目標全床反力中心点 Pまわりに作用する実全床反力モーメント Mtotalactを算出する式になっている。補足すると、式 1 4は、目標全床反力中心点 Pまわりに作用する実全床反カモ一メント Mtotalactから、目標第 14 ノード床反力中心点 Q 14まわりに作用する実床反力モーメン卜と目標第 23 ノード床反力中心点 Q23まわりに作用する実床反力モ一メントとを減じたものになっている。

一般的に、葉ノードである任意の第 n ノードの実第 n ノード床反力モーメント Mn— act は、第 n接地部位の実床反力モーメントとする。また、葉ノードでない任意の第 n ノードに対し、そのすベての子ノードの床反力（ここでの子ノードの床反力は、厳密には、該子ノードの目標床反力中心点に作用する床反力）が目標第 n ノード床反力中心点に作用するモーメントを、実第 nノード床反力モーメント Mn_act と呼ぶ。式 1 4および式 1 5のように、実第 n ノード床反力モーメントは、子ノードの実床反力モーメントを含む定義と含まない定義とがあるがいずれを用いても構わない。特に、子ノードの実床反力モーメントの水平成分を制御するフィードバック制御の応答性が高ければ、子ノードの実床反力モーメントの水平成分はただちに 0に収束するので、いずれの定義を用いても、第 nノードの実床反力の制御の挙動はあまり変らない。以下に示す式 1 6は、式 1 4に対応した実第 n ノード床反力モーメントの算出の一般式、式 1 7は、式 1 5に対応した実第 n ノード床反力モーメントの算出の一般式である。ただし、式 1 6および式 1 7において、 Qn— Qm は始点が Qn、終点が Qm のべクトルである。また、葉ノードである第 m ノードの実床反力モ一メント Mm_act は、実床反力検出器で検出された各接地部位の実床反力モーメントとする。 m {第11ノードの子ノードの番号の集合 } とすると

Mn_act =∑ (Qn_Qm * Fm_act) …式 1 6 m e {第11ノードの子孫である葉ノードの番号の集合 } であるとすると Mn— act =∑ (Qn_Qm * Fm— act + Mm— act) …式 1 7 なお、式 1 6、 1 7中の∑は、 mについての総和を意味する。補足すると、第 1実施形態では、葉ノード（各接地部位）の目標床反力中心点まわりの実床反力モーメントが 0となることから、前記式 1 5の右辺では、式 1 7の Mm— act の成分が含まれていない。接地部位の姿勢を制御可能な口ポットでは、一般には、式 1 7の Mm— act は 0にならない _c 第 1423 ノード補償角 0 1423 は、一般的には、上記のように求められる M1423actと先に補償全床反力モ一メント分配器 1 1 4 aで求められた第 1423 ノード補償床反力モーメント M 1423dmd との偏差 ( M1423act - M1423dmd) を 0に近づけるようにフィ一ドバック制御則などにより決定すればよい。例えば該偏差に所定のゲイン行列（ 3次の対角行列）を乗算することで 1423を求めればよい。

但し、本実施形態では、前記ベクトル V1423 の方向の軸回りの第 1423 ノード補償角 0 1423 を決定すればよいので、 Ml423act のべクトル V1423方向の成分 M1423actvと M 1423dmdのべクトル V1423方向の成分 M1423dmdv との偏差に応じて 0 1423 を決定すればよい。そして、この際、本実施形態では、床反力の制御の応答性や安定性を高めるために、 M1423actv と Ml423dmdv とをフィル夕に通した上で、それらの偏差に応じて 0 1423を決定することとした。

具体的には、上記の如く求めた M 1423act のべクトル V1423 方向の成分 Ml423actv を抽出する。これは、ベクトルの内積演算「 · 」を用いた次式 1 8によって得られる。

M1423actv = Ml423act - V1423 …式 1 8 以上のように M 1423actv を算出する処理が図 1 9の参照符号 1 1 4 kを付した演算部により実行される。

次に、上記 M1423actv を口一パスフィルタ 1 1 4 i に通し、 Ml423actvfilt を得る。また、前記第 1423 ノード補償床反カモ一メント M1423dmdの V1423方向の成分 M l423dmdvを補償フィル夕 1 1 4 j に通し、 M1423dmdvfilt を得る。なお、 M1423dmdv は、前記式 1 8と同様に、 Ml423dmd と V1423 との内積演算により求められる。そして、 M1423dmdvfilt を M1423actvfilt から減じたものを、偏差モ一メント V1423方向成分 M1423errvとして得る。尚、前記補償フィルタ 1 1 4 j は、制御系における M1423dmdv から実全床反力モーメン卜までの伝達関数の周波数応答特性を改善するものである。

最後に次式 1 9のフィードバック制御則（ここでは比例制御則）の演算によって前記第 1423 ノード補償角 01423 を得る。ここで K1423 は制御ゲインであり、通常、これは正の値に設定する。

Θ 1423=K1423* M1423errv …式 1 9 すなわち、偏差モーメント V1423 方向成分 M1423errv に制御ゲイン K1423を乗じることにより、第 1423 ノード補償角 1423を得る。次に、図 1 7の補償角決定部（ 014 決定部） 1 1 4 cの処理を、図 2 0を参照して説明すると、目標第 1 ノード床反力中心点 Q1 に実第 1 ノード床反力（第 1接地部位 1 0の実床反力）の並進力成分 Flact が作用すると共に、目標第 4ノード床反力中心点 Q4 に実第 4ノード床反力（第 4接地部位 1 0の実床反力）の並進力成分 F4act と作用した場合にそれらの並進力成分が、目標第 1 4ノード床反力中心点 Q14 のまわりに発生するモーメント M14act を、一般式である前記式 1 6を用いて求める。

より具体的には、次式 2 0により求める。

M14act=Q14_Ql* Flact + Q14_Q4 * F4act …式 2 0 ここで、 Q14— Q1 は始点が Q14、終点が Q1 のべクトル、 Q14— Q4 は始点が Q14、終点が Q4のベクトルである。

また、式 2 0の代わりに、次式 2 1 (前記式 1 7を具体化した式）を用いても、前述の理由により、実際上はほとんど問題がない。

Ml4act=Ql4_Ql * Flact + Q14_Q4 * F4act + M lact + M4act

…式 2 1 だたし、 Mlact は、実第 1 ノード床反力モーメント、 M4act は、実第 4ノード床反力モーメントである。なお、第 1実施形態では各脚 # 1 〜 # 4の先端部にフリージョイント（球関節 1 2 ) を備えているので、 Mlact、 M4actは 0である。

式 2 1は、第 14 ノードのすべての葉ノードの実床反力の合力が目標第 14 ノード床反力中心点まわりに作用するモーメントを算出する式になっている。尚、式 2 0は、第 1 4ノードのすべての葉ノードの実床反力の合力が目標第 1 4床反力中心点まわりに作用するモーメントから、実第 1 ノード床反力モーメントと実第 4ノード床反力モーメントとを減じたものになっている。

第 14 ノード補償角 Θ 14 は、一般的には、上記のように求められる M14act と先に補償全床反カモ一メント分配器 1 1 4 aで求められた第 14 ノード補償床反力モーメント M1423dmd との偏差（M14act— M14dmd) を 0に近づけるようにフィードバック制御則などにより求めればよい。

但し、本実施形態では、 Θ 1423 の場合と同様の理由によって、 M14actの前記べクトル V14方向の成分 Ml4actv と M14dmdのべクトル V14 方向の成分 M14dmdv とをそれぞれフィルタに通したものの偏差に応じて 0 14を決定することとした。

すなわち、上記の如くもとめた Ml4act のべクトル V14 方向成分 M14actv を抽出する。これは、ベクトルの内積演算を用いた次式 2 2 によつて得られる。

Ml4actv=M14act · V14 式 2 2 以上のように M14actv を算出する処理が図 2 0の参照符号 1 1 4 k ' を付した演算部により実行される。

次に、上記 M14actv を口一パスフィル夕 1 1 4 i ，に通し、 M14actvfilt を得る。また；前記第 14 ノード補償床反力モーメント M14dmd の V14方向の成分 M14dmdv を補償フィル夕 1 1 4 j ' に通し、 M14dmdvfilt を得る。なお、 M14dmdv は、 M14dmd と V14 との内積演算により求められる。そして、 M14dmdvfilt を、 M14actvfiltから減じたものを、偏差モーメント V14方向成分 M14errv として得る。尚、補償フィル夕 1 1 4 j ' は、制御系における M14dmdvから実全床反力モーメン卜までの伝達関数の周波数応答特性を改善するものである, 最後に次式 2 3のフィードバック制御則（ここでは比例制御則）の演算によって前記第 14 ノード補償角 Θ 14 を得る。ここで K14 は制御ゲインであり、通常、これは正の値に設定する。

Θ 14=K14* M14errv …式 2 3 図 1 7の補償角決定部（ 023決定部） 1 1 4 dの処理は、第 14ノード補償角 014 決定部 1 1 4 cの処理と同様であるので、ここでは詳細な説明を省略する。その処理の概要は、次の通りである。第 23 ノードの子ノードの実床反力によって目標第 23 ノード床反力中心点 Q23のまわりに発生するモーメント M23act のべクトル V23 方向の成分 M23actv が前記式 1 6または 1 7に基づいて算出される。そして、その M23actv をローパスフィル夕に通したもの M23actvfilt を、前記第 23 ノード補償床反力モーメント M23dmdの V23方向の成分 M23dmdv を補償フィル夕に通したもの M23dmdvfilt から減じてなる偏差モーメント V23方向成分 M23errvからフィードバック制御則（比例制御則）の演算によって、前記第 23 ノード補償角 0 23が算出される。

以上の補償角決定部 1 1 4 b〜 1 1 4 dの処理によって、目標全床反力中心点 Pに作用する実モーメント Mact の水平成分を補償全床反カモ —メント Mdmdに近づけるようにノード補償角 0 1423 , Θ 14, 0 23の組が決定されることとなる。なお、 0 1423 は、本実施形態では、前記式 1 9 により求めたが、式 1 9 の右辺の M1423err の代わりに、 M1423act+M14act+M23act (実ノード床反力モーメントの総和）と、 M1423dmd との偏差、あるいは、実ノード床反力モーメントの総和と M1423dmd とをそれぞれフィル夕に通したものの偏差に応じて決定するようにしてもよい。

補足すると、本実施形態では、各ノードの目標ノード床反力中心点 Qn を変更せずに、その Qn を作用点として、ノード補償床反力モーメントを決定した。そして、このノード床反力補償モーメント（より正確には、該ノ一ド補償床反力モーメントと Qn を作用点とする目標ノード床反力のモーメント成分との合力）と Qn を作用点とする実ノード床反力モーメントとの偏差を制御量とし、この制御量を 0に近づけるようにノード補償角を決定するようにした。このようにノード補償角を決定する代わりに次のようにノード補償角 0 1'423， Θ 14, 0 23 を決定するよう.にしてもよい。すなわち、子ノードを持つ各第 n ノード (n= 14，23, 1423)に対し、その第 nノードの実ノード床反力（第 nノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が 0 となるような床反力中心点を実第 nノード床反力中心点として求める。あるいは、第 nノード（n=14，23, 1423)の実ノード床反力のモーメントから、その各子ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が 0となるような床反力中心点を実第 nノード床反力中心点として求める。例えば第 14 ノードに関する実第 14 ノード床反力中心点は、前記式 1 6 または式 1 7により求めた Ml4actの水平成分を、 Flactと F4act との合力（実第 14 ノード床反力の並進力成分）の鉛直成分で割った値だけ目標第 14 ノード床反力中心点を線分 Q 1Q4上でずらした点として求められる。第 23 ノード、第 1423 ノードのノード床反力中心点についても同様である。そして、第 nノード（n=14, 23, 1423)の目標ノード床反力中心点と、上記の如く求めた実第 nノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィル夕に通した値の偏差を制御量とし、その制御量を 0に近づけるように、該制御量に応じてノード補償角 0 1423， Θ 14, 0 23 を決定する（例えば制御量にあるゲインを乗算することで、 0 1423， Θ 14, 0 23を決定する）。

図 1 7の修正目標接地部位位置姿勢算出部 1 1 4 gは、第 1423 ノ一ド補償角 0 1423、第 14ノード補償角 0 14、第 23 ノード補償角 0 23に基づき、前述した階層型コンプライアンス動作の目標接地部位位置姿勢の修正手法（図 1 5および図 1 '6を参照して説明した手法）に従って各接地部位 1 0の目標位置および目標姿勢である目標接地部位位置姿勢を , 修正し、修正目標接地部位位置姿勢を得る。ただし、本実施形態においては、各脚 # 1〜 # 4の先端部にフリージョイント（球関節 1 2 ) があり、意図的に各接地部位 1 0の姿勢を変化させることはできないので、 — 修正目標接地部位位置姿勢は、実際には、修正目標接地部位位置を意味する。

図 2 1は、図 1 7の前記機構変形補償量算出部 1 1 4 nの処理を示す機能ブロック図である。図 2 1 に示すように、機構変形補償量算出部 1 1 4 nは、前記補償全床反力モーメント分配器 1 1 4 aから出力される各接地部位 1 0の修正目標接地部位床反力 Fn_refmdfd (n= l,2, 3,4) (目標接地部位床反力に、第 1423 ノード補償床反力モ一メント M1423dmd、第 14 ノード補償床反力モーメント Ml4dmdおよび第 23 ノード補償床反力モーメント M23dmd を付加することによって修正された目標接地 '部位床反力）によって発生が予想される各脚 # 1〜# 4の変形量（各脚のリンク機構およびコンプライアンス機構 4 2の変形量）である機構変形量 En— mdfd(n=l, 2, 3，4)を求める。機構変形量 En— mdfd(n=l，2，3,4)は、各脚に作用する力（あるいは床反力）と該脚の変形との関係を表す機構コンプライアンスモデルを用いて求められる。そして、機構変形補償量算出部 1 1 4 ηは、機構変形量 En_mdfd(n= l，2，3，4)を打ち消すための機構変形補償量 En— cmpn(n=l，2, 3,4)を求める。機構変形補償量 En— cmpn は、各機構変形量に E_n_mdfd に（— 1) を乗じることにより求められる。

図 1 7の機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 h は、算出された機構変形量 En— mdfd を打ち消すように、各接地部位 1 0の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部 1 1 4 gで求められ位置姿勢）をさらに修正し、各接地部位 1 0の機構変形補償入り修正目標接地部位位置姿勢を得る。機構変形補償入り修正目標接地部位位置姿勢は、各接地部位 1 0の修正目標接地部位位置姿勢に、それぞれに対応する機構変形補償量 En_cmpn を付加することにより.決定される。

例えば、修正目標接地部位床反力によって、ある接地部位 1 0に対応する脚のコンプライアンス機構 4 2などが鉛直方向に zだけ縮むと算出された場合には、その接地部位 1 0の目標位置を zだけ下げるように修正目標接地部位床反力が修正される。即ち、機構変形補償後の接地部位 1 0の接地面（底面）が目標接地部位床反力を受けて変形したときの位置姿勢が、機構変形補償前の接地部位の接地面の目標位置姿勢に一致するように、機構変形補償入り修正目標接地部位位置姿勢を算出する。なお、これについての詳しい説明は、特開平 1 0— 2 7 7 9 6 9号公報に詳細に説明されている。また、本実施形態の口ポット 1では、接地部位 1 0の姿勢を制御できないので、機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hで実際に修正されるものは、各接地部位 1 0の修正目標接地部位位置である。

上記のような機構変形補償は、コンプライアンス機構 4 2などの変形によって生じる実接地部位位置姿勢のずれをフィ一ドフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い形態でのロボット 1の移動を実現することができる。

以上が、階層型コンプライアンス動作決定部 1 1 4の処理の詳細である。

上記を前提として図 9のフローチャートの説明に戻ると、 S 3 4において、前記の如く補償角を決定する。図 2 2は、その補償角の決定処理のサブルーチンを一般化して示したフローチヤ一卜である。

同図を参照して説明すると、先ず S 1 0 0において、各接地部位 1 0 の実床反力（前記実床反力検出器 1 0 8で得られる実床反力）を基に、実第 n ノード床反力の並進力成分 Fn_act(n=l, 2, 3,4, 14, 23, 1423)を算出する。この場合、各葉ノードの実ノード床反力の並進力成分 Fn一 act(n= l,2,3，4)は、前記実床反力検出器 1 0 8による各接地部位 1 0の実床反力の検出値の並進力成分である。また、葉ノードでない各ノ —ドの実ノード床反力の並進力成分 Fn_act(n=14,23, 1423)は、前記した如く、そのノードの子ノードの実ノード床反力の合力の並進力成分である。

次いで S I 0 2 に進み、実第 n ノード床反力モーメント Mn一 act (n=l,2，3, 4,14,23,1423)を算出する。第 1実施形態のロボット 1では、各葉ノードの実ノード床反力モーメント Mn_act (n=l，2,3，4) は 0である。また、葉ノードでない各ノードの実ノード床反力モーメント Mn— act(n=14，23,1423)は、 S I 0 0で得られた Fn— act(n=l,2，3，4)と、目標ノード床反力中心点 Qn(n=l,2,3，4,14，23,1423)とから前記式 1 6または式 1 7に基づき算出される。

次いで S 1 0 に進み、前記姿勢安定化制御演算部 1 0 4で求められた補償全床反力モーメント Mdmd を基に、第 n ノード補償床反カモ一メント Mn— dmd (n=14，23,1423)を決定する。この処理は、前記補償全床反力モーメント分配器 1 1 4 aにより前記した通り行なわれる。

次いで S 1 0 6に進み、階層型コンプライアンス動作の説明で定義したべクトル V1423， V14, V23 およびこれに直交したべクトル U1423, U14, U23 を求める。ただし、第 1実施形態では、ベクトル U1423, U14, U23の方向の床反力モーメントを発生することはできないので、 U1423, U14, U23を求める必要はない。

補足すると、後述する第 2実施形態の如く、第 nノードの子ノードの数が 3以上の場合には、 Vn は、' 時間的に急激に変化しなければ、どの向きにとっても良いので、 Vn の向きは、支持脚座標系の X軸の向きや、ロボットの上体の向きに合わせるなど、適当に決定すれば良い。また、 Unは Vnに直交していればよい。 '

次いで S 1 0 8 に進み、実第 n ノード床反力モーメント Mn— act (n=14, 23,1423)の Vn方向成分 Mn— actv と Un方向成分 Mn— actu とを抽出する。これは、 Mn一 act と Vn、 Un との内積演算を行なえばよい。なお、第 1実施形態のように葉ノードでない任意の第 nノードの子ノ一ドの数が 2以下の場合には、 Un 方向成分 Mn_actu は 0である。従つて、 Mn— actuは求める必要はない。

次いで S 1 1 0に進み、第 n ノ一ド補償床反力モーメント Mn_dmd (n=14，23,1423)の Vn 方向成分 Mn— dmdv と Un 方向成分 Mn— dmdu を抽出する。これは、 Mn— dmd と Vn、 Un との内積演算を行なえばよい。なお、第 1実施形態のように葉ノードでない任意の第 nノードの子ノードの数が 2以下の場合には、第 n ノード補償床反力モーメント Mn— dmdの Un方向成分 ] VTn— dmduは 0にする。あるいは、 Mn— dmdu を求める必要はない。

次いで S 1 1 2に進み、 Mn— actvをフィル夕に通した値と Mn_dmdv をフィル夕に通した値の差にゲイン Kn を乗じることにより（より一般的には、当該差からフィードバック制御則により）第 η ノード補償角 0 _η の Vn 成分を決定する。第 1実施形態では、この処理は、第 14 ノード、第 23 ノード、第 1423 ノードについて、前記各補償角決定部 1 1 4 b , 1 1 4 c , 1 1 4 dにより前記した如く実行される。

次いで S 1 1 4 に進み、 Mn— actu をフィル夕に通した値と Mn.dmdu をフィル夕に通した値の差にゲイン Kn を乗じることにより (より一般的には当該差からフィードバック制御則により）第 n ノ一ド補償角 0 n の Un成分を決定する。ただし、第 1実施形態のように葉ノードでない任意の第 nノードの子ノードの数が 2以下の場合には、第 n ノード補償角 U 成分は 0にする。あるいは、 S 1 1 4の処理を実行する必要はない。 '

以上が、図 9の S 3 4のサブルーチン処理である。補足すると、 S 1 0 6〜 S 1 1 4の処理は、目標第 nノード床反力中心点に作用する実第 nノード床反力モーメントが第 nノード補償床反力モーメント（より正確には第 nノード補償床反力モーメントと目標第 nノード床反力中心点に作用する目標ノード床反力モーメントとの合力）に収束するように第 nノード補償角を決定する処理であると言える。

次いで、図 9のフローチャートの S 3 6に進み、前記機構変形補償量を算出する。この処理は、前記機構変形補償量算出部 1 1 4 nにより前記した如く行なわれる。

次いで S 3 8に進んで目標接地部位位置姿勢を S 3 4で求めた補償角に応じて修正し、さらにこれを S 3 6で求めた機構変形補償量に応じて修正することによって、各接地部位 1 0の機構変形補償入り修正目標接地部位位置姿勢を得る。第 1実施形態では、前記修正目標接地部位位置姿勢算出部 1 1 4 gによって、前記した如く（前記 1 5および図 1 6を参照して説明した如く）補償角 0 1423、 Θ 14, 0 23 に基づき各接地部位 1 0の修正目標接地部位位置を求める。そして、この求めた修正目標接地部位位置が前記機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 h によって、前記した如く前記機構変形補償量 En— cmpn(n=l,2, 3, 4)に応じてさらに修正され、それによつて、各接地部位 1 0の機構変形補償入り修正目標接地部位位置が得られる。

以上説明した図 9の S 3 2 〜 S 3 8の処理が、階層型コンプライアンス動作決定部 1 1 4の処理である。

次いで S 4 0に進み、目標上体位置姿勢と機構変形補償入り修正接地部位位置姿勢（第 1実施形態では機構変形補償入り修正接地部位位置）から口ポット 1の関節変位指令を算出する。この処理は、前記ロボット幾何学モデル 1 1 0により前記した如く実行される。

次いで S .4 2に進んで、実関節変位を関節変位指令に追従させる。この追従制御（サーポ制御）は、前記変位コントローラ 1 1 2により実行される。

次いで S 4 4に進んで、時刻を A t (制御装置 5 0の演算処理周期）だけ更新し、 S 1 4に戻って上記の処理を繰り返す。

以上説明した第 1実施形態の如き階層型のコンプアライアンス動作によって、接地部位が 3つ以上ある移動口ポットにおいても、各ノード床反力の制御が互いに殆ど干渉しないようになり、各ノード床反力を容易かつ適切に制御することができる。したがって、制御の干渉がなく、各ノードの実床反力が望ましい値からずれたり発振することがない。このため、床面の大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボッ卜に作用する床反力を適切に制御することができる。また、移動ロボッ卜の姿勢を安定化する制御を容易に実現できると共に、移動ロボッ卜が受ける着地衝撃を低減することができ、移動ロボットの接地性を高め、移動時のスリップやスピンを防止することができる。さらに、移動ロポッ卜のァクチユエ一夕の負荷を低減することができる。したがって、各接地部位の床反力を適切に制御し、高い姿勢安定性を得ることができる。

口ポットの姿勢の安定化に対する効果を補足すると、ロボッ卜が想定した床面上を想定した通りに歩行している状態から、ロポット全体が傾いた場合に、傾き角偏差 berr ( 0 berrx, Θ berry) とこれに応じて発生する目標全床反力中心点まわりのモーメント水平成分の増加量 Δ Μ との関係が、比例関係にあることが望ましい。そうでなくても、ある回転行列 T、ある対角行列 diag(a,b)に対し、次式 2 4が成立することが望ましい。なお、 T, diag(a，b)は 2次の正方行列である。

T * A M = diag(a,b) * T * 6> berr …式 2 4 これらの関係を満足しない場合には、ロポットの姿勢が傾いた状態から戻る時に、直線的に傾き角偏差 0 berr が 0に収束せずに、味噌擂り運動を起こすおそれがある。例えば、口ポットの上体が前に傾いた状態では、後ろに戻す復元力以外に、横方向への転倒力（復元力）が余計に作用し、傾き角偏差 0 berr が直線的に 0に戻らずに、後ろ横方向に復元力が働いて、傾き角偏差 0 berrが渦巻き状に 0に収束していく。

また、上記と同じ理由から、コンプライアンス制御においては、ロボット全体の傾き角偏差 Θ berr の変化速度 d 0 berr/dt とこれに応じて発生するモーメントの増加量 Δ M— d との関係も、比例関係にあることが望ましい。そうでなくても、ある回転行列を T、ある対角行列 diag(e，f)に対し、次式 2 5 が成立することが望ましい。なお、 T , diag(a,b)は 2次の正方行列である。

T * △ M_d = diag(e,f) * T * d 0 berr/dt …式 2 5 一般的に、口ポットの各脚に独立にコンプライアンス制御を掛けると, これらの関係を満たさず、味噌擂り運動を起こすおそれがある。第 1実施形態に示した階層型コンプライアンス制御においては、前記式 2 4 , 2 5の関係を満足することができるので、ロポッ卜の姿勢制御の収束性が高く、また、発振や振動を防止することができる。前記第 1実施形態においては、目標接地部位位置を、各接地部位の姿勢を変えずに（姿勢を制御せずに）、各ノード床反力中心点まわりに回転移動させるように補正（修正）動作を行うことで、各接地部位 1 0の間の相対高さを目標接地部位位置の間の相対高さから修正した。但し、目標接地部位位置を鉛直方向にだけ移動させるようにして、各接地部位 1 0の高さだけを修正するようにしても良い。具体的には、以下の手順によって目標接地部位位置を修正する。

まず、以下の式 2 6 , 2 7により、目標第 14 ノード床反力中心点の鉛直位置修正量 Z14 と目標第 23 ノード床反力中心点の鉛直位置修正量 Z23を求める。

214= -PQ14* Θ 1423 …式 2 6

Z23 = PQ23 * Θ 1423 …式 2 7

但し、ここで、 01423には'前記式 1 9で求められる値を代入する。

次いで、各接地部位の目標床反力中心点（各葉ノードの目標ノード床反力中心点）の鉛直位置修正量 Zn(n=l,2，3,4)を次式により求める。

Zl= -Q14Q1* Θ 14+Z14 …式 2 8

Z4=Q14Q4* Θ 14+Z14 …式 2 9

Z2= -Q23Q2* θ 23 + Z23 …式 3 0

Z3=Q23Q3* θ 23 + Z23 …式 3 1 但し、ここで、 014 には、前記式 2 3で求められる値を代入し、 023 には、 014と同様にして求められる値を代入する。

以上のように求めた Zl、 Z2 Z3、 Z4 を目標接地部位位置に鉛直方向に加えることにより、修正目標接地部位位置を得る。

また、前記第 1実施形態では、接地部位 1 0を前記図 6に示した如く階層化したが、その階層構造は、必ずしも 1つの階層構造にあらかじめ決めておく必要はない。例えばトロット、ギャロップなど、ロボット 1 の移動形態（移動時の脚の運動形態）などに応じて、階層構造を変更しても良い。例えば、図 2 3に示す如く接地部位 1 0を階層化してもよい。図 2 3は、例えばロボット 1の全ての脚 # 1〜# 4を支持脚とする時期において、第 1接地部位 1 0と第 2接地部位 1 0との組を第 12 ノードとし、第 3接地部位 1 0と第 4接地部位 1 0との組を第 34 ノードとした場合の例を前記図 3 ( b ) と同様に図示したものである。この例では各ノードの目標ノード床反力中心点 Qn(n= l，2，3，4，12, 34, 1234)は図示の如く設定される。このようにすることで、場合によっては、前記コンプライアンス動作と、後述の床形状推定をより的確に行なうことが可能となる。

[第 2実施形態]

以下に前記図 1並びに図 2 4〜図 3 4を参照しつつ、本発明の第 2実施形態を説明する。図 1を参照して、第 2実施形態の口ポット 1が第 1 実施形態のロボッ卜と異なる点を中心に説明すると、第 2実施形態の口ボット 1は、第 1〜第 4脚 # 1〜 # 4に加えて、これらの脚 # 1〜# 4 と同じ構造の第 5脚 # 5および第 6脚 # 6を備える。すなわち、第 2実施形態の口ポット 1は 6脚ロボットである。第 5脚 # 5は、第 3脚 # 3 の後側でロボット 1の上体 2 4の右側部から延設され、第 6脚 # 6は、第 4脚 # 4の後側でロポット 1の上体 2 4の左側部から延設されている < これ以外のロポッ卜 1の機構的構造は、第 1実施形態と同一であるので，第 1実施形態と同一の参照符号を付して説明を省略する。

この第 2実施形態のロボット 1 ( 6脚ロボット）の階層型コンプライアンス制御の手法の基本的な考え方は第 1実施形態のものと同じである ₍ 但し、第 2実施形態の階層型コンプライアンス制御では、 3つの子ノードを持つノードがあることと、それに対応して制御処理が拡張されていることとが第 1実施形態のものと相違している。

そこで、以降、その相違点を中心に第 2実施形態の口ポット 1の制御処理について説明する。なお、第 2実施形態の説明で用いる参照符号および用語は、第 1実施形態と同等の意味を持つものは、第 1実施形態と同じ参照符号および用語を用い、詳細な説明を省略する。

図 2 4は、第 2実施形態における階層構造を説明するための図であり第 1実施形態における図 3 ( b ) に対応する図である。図 2 4に示す如く、第 2実施形態では、 6個の接地部位 1 0のそれぞれを葉ノード（第 1〜第 6ノード）とし、その 6個の全ての接地部位 1 0の組を根ノード (第 145236 ノード）としていると共に、第 1脚 # 1、第 4脚 # 4、第 5脚 # 5のそれぞれの接地部位である第 1、第 4、第 5接地部位 1 0の組を第 145 ノード、第 2脚 # 2、第 3脚 # 3、第 6脚 # 6のそれぞれの接地部位である第 2、第 3、第 6接地部位 1 0の組を第 236 ノードとしている。つまり、第 145 ノードは第 1、第 4、第 5ノード（ 3個の葉ノード）を子ノードとして持つ中間ノード、第 236 ノードは第 2 . 第 3、第 6ノード（ 3個の葉ノード）を子ノードとして持つ中間ノードである。

第 2実施形態では、例えば第 145 ノードの 3個の接地部位 1 0の組と、第 236 ノードの 3個の接地部位 1 0の組とを交互に持ち上げて着地させることにより、ロボット 1の移動を行なうようにしている。図 2 4は、両者の組を着地させた状態（両者の組を支持脚とした状態）を示しており、図中の参照符号 <¾ 11 ('11=1,2,3,4，5,6)を付した三角形が各接地部位 1 0の位置に対応している。

なお、 Qn(n=l，2，3,4,5，6)は、葉ノードである第 1〜第 6の各接地部位 1 0の目標床反力中心点（目標ノード床反力中心点）、 Q145, Q236 は、それぞれ第 145 ノード、第 236 ノ一ドの目標床反力中心点（目標ノード床反力中心点）、 P は根ノード（第 145236 ノード）の目標床反力中心点（目標ノード床反力中心点） Q145236 としての目標全床反力中心点（=目標 Z M P ) を示すものである。第 1実施形態と同様に、 Qn(n=l,2,3,4,5,6)は、それぞれに対応する接地部位 1 0の中心点に一致している。

第 2実施形態における制御装置 5 0の全体的な機能的構成は、前記第 1実施形態に関して説明した図 2のものと同じである。

この場合、第 2実施形態における歩容生成装置 1 0 0は、第 1実施形態のものと同様に、ロボット 1の目標運動軌道（目標接地部位軌道、目標上体位置姿勢軌道）と目標床反力軌道（目標全床反力中心点軌道、目標全床反力軌道）とを決定じて出力する。但し、第 2実施形態では、目標接地部位軌道は、 6個の接地部位 1 0のそれぞれの目標接地部位位置の軌道である。なお、各接地部位をその姿勢を制御可能に設けた場合には、目標接地部位姿勢の軌道も目標接地部位軌道に含まれる。また、目標全床反力中心点軌道は、第 2実施形態における脚 # 1〜# 6の運動形態（特に支持脚の着地予定位置）に合わせて、 Z M Pの存在可能範囲内で該範囲の境界に近づきすぎない位置（例えば Z M Pの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。また、第 2実施形態における目標床反力分配器 1 0 2では、第 1実施形態と同様に、次の条件 A ' ) 〜F ' ) を満たすように、目標ノード床反力中心点と各ノードの重みと目標ノード床反力とを決定する。

Α ' ) 各葉ノ一ドの目標ノード床反カ中心点（¾ 11(11=1，2,3，4,5,6)は、該葉ノードに対応する接地部位 1 0の中心点に一致する。より一般的には、 <¾11 (11=1,2,3，4,5，6)は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。なお、例えば各接地部位 1 0の目標床反力中心点を歩容生成装置 1 0 0で決定した場合には、歩容生成装置 1 0 0が決定する目標床反力を基に、目標ノード床反力中心点 Qn(n=l，2,3，4，5,6)を決定すればよい。

B ' ) 根ノードの目標床ノード反力中心点は、目標全床反力中心点 Pに一致する。

C ' ) 子ノードを持つ任意の第 nノード（n=145，236，145236)の目標ノード床反力中心点 Qn は、その第 nノ一ドの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。より具体的には、第 145 ノードの目標ノード床反力中心点 Q145 は、その子ノード（葉ノード）である第 1、 4、 5 ノードの目標ノード床反力中心点 Ql , Q4, Q5 を頂点とする三角形の内分点（三角形の境界の点を含む）となり、第 236 ノードの目標ノード床反力中心点 Q236 は、その子ノード（葉ノード）である第 2、 3、 6 ノードの目標ノード床反力中心点 Q2， Q3, Q6 を頂点とする三角形の内分点（三角形の境界の点を含む）となる。また、第 145236 ノード（根ノード）の目標ノード床反力中心点 Q145236 ( =目標全床反力中心点 P ) は、その子ノ "ド（中間ノード）である第 145、 236 ノードの目標ノード床反力中心点 Q 145, Q236 を結ぶ線分の内分点となる。

D ' ) 子ノードを持つ任意の第 nノード（n=145, 236, 145236) の目標ノ —ド床反力 Fn は、該第 nノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノード（第 145236 ノード）の目標ノード床反力 F145236 は目標全床反力 Ftotalref に一致する。あるいは、各ノードの目標ノード床反力 Fn(n=l，2, 3,4,5,6, 145, 236, 145236)は，各ノードの重みと前記式 4 aまたは 4 bの関係を有する。

E ' ) 接地していない接地部位 1 0 に対応する葉ノードの目標ノード床反力は 0 となる。 ·

F. ' ) 各ノード（第 n ノード（_n=l, 2, 3，4, 145,236, 145236) ) の目標ノード床反力中心点、重み、目標ノード床反力は連続的に変化する。

G ' ) 非接地状態の接地部位 1 0 に対応する葉ノードの重み、または、その葉ノ一ドの祖先ノードのいずれか 1つの重みは 0 とされる。なお、根ノードの重みは、前記第 1実施形態と同様、便宜的に「 1」に ¾t /£した。

第 2実施形態における姿勢偏差演算部 1 0 3および姿勢安定化制御演算部 1 0 4の処理は、第 1実施形態と同じであり、補償全床反力モーメント Mdmd ( Mdmdx, Mdmdy) が第 1実施形態で説明した如く決定される。

また、第 2実施形態における実床反力検出器 1 0 8は、第 1実施形態のものと同様に、各接地部位 1 0 (本実施形態では 6個の接地部位）に作用する実床反力を各脚 # 1 〜 # 6に備えた 6軸力センサ 3 4の出力から検出し、それを支持脚座標系（床に固定された図 1に示すグローバル座標系）で表現してなる実床反力に変換する。

また、第 2実施形態におけるロボット幾何学モデル（逆キネマテイクス演算部） 1 1 0は、第 1実施形態のものと同様に、各接地部位位置の最終的な目標軌道と目標上体位置姿勢などを基に、逆キネマテイクスの演算によって、それらを満足する口ポット 1の各関節変位指令を算出する。

また、第 2実施形態における変位コントローラ 1 1 2は、第 1実施形態のものと同様に、前記各関節変位指令に実関節変位を追従させるようにロボット 1の各関節 1 4 , 1 5のァクチユエ一夕（図示しない）を制御する。

また、第 2実施形態における階層型コンプライアンス動作決定部 1 1 4は、第 1実施形態のものと同様に前記要求 1 )、 2 ) をできるだけ満足するように各接地部位 1 0の目標接地部位軌道を修正する。但し、本実施形態では、 3つの子ノードを持つ中間ノードを有するため、階層型コンプライアンス動作決定部 1 1 4の具体的な処理は、第 1実施形態のものより多少複雑になる。なお、各接地部位 1 0の姿勢を制御可能に口ポットを構成した場合には、階層型コンプライアンス動作決定部 1 1 4 は、前記要求 3 ) もできるだけ満足するように各接地部位 1 0の目標接地部位軌道を修正すべきである。

図 2 5は、第 2実施形態の階層型コンプライアンス動作決定部 1 1 4 の機能を示すブロック図であり、第 1実施形態における図 1 7に対応している。同図 2 5を参照して、第 2実施形態の階層型コンプライアンス動作決定部 1 1 4も、第 1実施形態と同様に、補償全床反力モーメント分配器 1 1 4 a、補償角決定部 1 1 4 b， 1 1 4 c， 1 1 4 dと、修正目標接地部位位置姿勢算出部 1 1 4 gと、機構変形補償量算出部 1 1 4 nと、機構変形補償入り修正目標接地部位位置姿勢決定部 1 1 4 hとを構成要素（機能的手段）として備える。なお、第 2実施形態では、補償角は、第 145236 ノード、第 145 ノード、第 236 ノードにそれぞれ係わる第 145236補償角 Θ 145236、第 145 補償角 Θ 145 および第 236 補償角 Θ 236 があり、補償角決定部 1 1 4 b , 1 1 4 c , 1 1 4 dは、それぞれ 0145236、 Θ 145, 236を決定するものである。

以下に、第 2実施形態における階層型コンプライアンス動作決定部 1 1 4の処理を、第 1実施形態のものと異なる点を中心に説明する。

第 2実施形態における階層型コンプライアンス動作決定部 1 1 4は、第 1実施形態のものと同様に、目標床反力分配器 1 0 2の出力などを基に、各ノードの目標ノード床反力の並進力成分およびモーメント成分を決定すると共に、実床反力検出器 1 0 8の出力を基に、各ノードの実ノ一ド床反力の並進力成分およびモーメント成分を決定する。

すなわち、図 2 6を参照して、各葉ノードの目標ノード床反力の並進力成分 Fn_ref(n=l,2,3,4，5,6 、目標床反力分配器 1 0 2で決定された目標ノード床反力の並進力成分と同一とされ、根ノードの目標ノード床反力の並進力成分 F145236ref は、歩容生成装置 1 0 0で決定された目標全床反力の並進力成分 Ftotalref と同一とされる。また、葉ノードおよび根ノード以外の中間ノードの目標ノード床反力の並進力成分 Fn— ref(n=145，236)は、その中間ノードの各子ノードの目標ノード床反力の合力の並進力成分に決定される（F145ref = Flref + F4ref + F5ref、 F236ref= F2ref+ F3ref+ F6ref)。この場合、中間ノードの親ノードは根ノードであるので、 Fl45ref+ F236ref= Ftotalref である。なお、図 2 6は、第 2実施形態のロボット 1 の全ての接地部位 1 0が接地している状態での各ノードの目標ノード床反力の並進力成分 Fn_ref を例示している。

また、各ノードの目標ノード床反力のモーメント成分（詳しくは該ノ —ドの目標ノード床反力中心点を作用点とする目標ノード床反力のモーメント成分）は、いずれも 0 とされる。

また、図 3 0を参照して、各葉ノードの実ノード床反力の並進力成分 Fn_act(n= l，2，3,4，5，6 、実床反力検出器 1 0 8で検出された各接地部位 1 0の実床反力の並進力成分と同一とされる。そして、子ノードをもっ各ノードの実ノ一ド床反力の並進力成分 Fn—act(n= 145, 236, 145236) は、そのノードの各子ノードの実ノード床反力の合力の並進力成分に決定される。従って、 Fl45act = Flact + F4act + F5act, F236act = F2act + F3act + F6act、 F145236act ( = Ftotalact) = F145act + F236act である。なお、図 3 0は、第 2実施形態のロボット 1の全ての接地部位 1 0が接地している状態での各ノードの実ノード床反力の並進力成分 Fn— actを例示している。 '

また、図 3 1 を参照して、各葉ノードの実ノード床反力のモーメント成分（該葉ノードの目標ノード床反力中心点を作用点とするモーメント成分） Mn_act(n= l,2，3,4, 5，6)は、基本的には、実床反力検出器 1 0 8 で検出された各接地部位 1 0の実床反力のモーメント成分と同一とされる。ただし、本実施形態では、各接地部位 1 0の中心点、すなわち該接地部位 1 0の目標床反力中心点に実モーメントは発生しないので、各葉ノードの実ノード床反力のモーメント成分は 0に設定される。そして、子ノードをもつ各第 n ノード（n= 145，236, 145236)の実ノ一ド床反力のモーメント成分 Mn_act は、基本的にはその第 nノードの各子ノードの実ノード床反力の合力が、該第 nノードの目標ノード床反力中心点に作用するモーメント成分（これは一般に 0 にならない）に決定される。第 2実施形態における前記補償全床反力モーメント分配器 1 1 4 aは，前記補償全床反力モーメント Mdmd を前記補償全床反力モーメント Mdmd ( Mdmdx, Mdmdy) を、第 145236 ノード補償床反カモ一メント M145236dmd、第 145 ノード補償床反力モーメント M145dmd および第 236 ノード補償床反力モーメント M236dmd に分配するものである。

第 145236 ノード補償床反力モーメント M1423dmd は、第 145236 補償角 0 145236 を操作することによって（第 1 、第 4、第 5接地部位の組と、第 2 、第 3 、第 5 接地部位の組とを目標全床反力中心点 P(=Q 145236)まわりに 0 145236 だけ回転させることによって）発生する各接地部位 1 0の床反力の並進力成分が、目標目標全床反力中心点 P (目標 Z M P ) まわりに発生すべきモーメントの目標値である。

第 145 ノード補償床反力モーメント M 145dmd は、第 145 補償角 Θ 145 を操作することによって（第 145 ノードに属する第 1、第 4および第 5接地部位 1 0を目標第 145 ノード床反力中心点 Q 145 まわりに 6». 145 だけ回転させることによって）発生する各接地部位 1 0 (詳しくは第 1、第 4、および第 5接地部位）の床反力の並進力成分が、目標第 145 ノ―ド床反力中心点まわりに発生すべきモーメントの目標値である。第 236 ノード補償床反力モーメント M236dmd は、第 236 補償角 Θ 236 を操作することによって（第 236 ノードに属する第 2、第 3および第 6接地部位 1 0を目標第 236 ノード床反力中心点 Q236 まわりに 0236 だけ回転させることによって）発生する各接地部位 1 0 (詳しくは第 2、第 3、および第 6接地部位）の床反力の並進力成分が、目標第 236 ノード床反力中心点まわりに発生すべきモーメン卜の目標値である, なお、本実施形態では、第 1実施形態と同様、ロボット 1の鉛直軸まわりの姿勢制御を行なわないので（補償全床反力モーメント Mdmd の鉛直軸回りの成分が 0 で ·あるので）、 M145236dmd、 M145dmd、 M236dmd は、いずれも鉛直軸回りの成分が 0であるモーメント（水平ベクトル）であり、補償角 0145236、 Θ 145, 0236 は、水平な軸回りの回転角である。また、特に、 M145236dmd は、線分 Q145Q236 に垂直な水平軸回りのモーメントである。

これらのノード補償床反力モーメント M145236dmd, M145dmd, M236dmd は、基本的には、次の条件 1 1 )、 1 2 ) を満たすように決定される。

1 1 ) 第 145 ノード、第 236 ノード、第 145236 ノードに対して、前記第 1実施形態で定義した如く、次式 7 a〜 9 aの関係式を満たす修正目標ノ一ド床反力中心点 Q145mdfd 、 Q236mdfd 、 Pmdfd (=Ql45236mdfd)を導入したとき、これらの Ql45mdfd、 Q236mdfd, Pmdfdは、それぞれの所定の存在許容範囲内に在る。

M145236dmd= (Pmdfd -P) * Ftotalref …式 7 a

M145dmd=(Q145mdfd-Ql45)* Fl45ref …式 8 a

M236dmd=(Q236mdfd-Q236)* F236ref …式 9 a Q145mdfd、 Q236mdfd、 Pmdfd の存在可能範囲は、例えば第 2実施形態の口ポット 1の全ての接地部位 1 0が接地している状態では、図 2 9 ( a ) に示す如く設定される。すなわち、 Ql45mdfdの存在許容範囲は、図中の太線の三角形上の領域（その三角形の辺および内部の領域）であり、これは、第 145 ノードの子ノードの目標ノード床反力中心点 Q1,Q4,Q5 を頂点する三角形の内部で、該三角形 Q1Q4Q4 の境界に近づき過ぎないように設定された領域である。 Q236mdfdの存在許容範囲も同様である。また、 Pmdfd の存在許容範囲は、図中の太線の線分上の領域であり、これは、第 145236 ノード（根ノード）の子ノードの目標床反力中心点 Q145，Q236 を結ぶ線分 Q145Q236 上で、該線分 Q145Q236の端点に近づきすぎないように設定された領域である。

1 2 ) Mdmd=M145236dmd + M145dmd+ M236dmd をほぼ満足する, また、本実施形態では、根ノード（第 145236 ノード）の子ノードが 2つであることから、 M145236dmd は、前記第 1実施形態における各ノード補償床反力モーメントと同様に、線分 Q145Q236 に直交する水平な単位ベクトル（これを V145236 で表す）と同方向のベクトルに制限される。そこで、本実施形態では、次の条件 1 3 ) を満たすように M145236dmd, M145dmd, M236dmdを決定する。

1 3 ) M145236dmd+M145dmd+M236dmdの、ベク卜ル V145236方向の成分は、可能な限り、 Mdmd のべクトル V145236 方向の成分に近い値とする。これらの条件 1 1 ) 〜： I 3 ) を満たす M145236dmd、 M145dmd、 M236dmd は、例えば次のように決定される。まず、 M145236dmd を Mdmd の V145236 方向の成分に決定する。ただし、前記式 7 aにより定まる修正目標ノード床反力中心点 Pmdfd がその存在許容範囲に収まらない場合には、 Pmdfd がその存在許容範囲の境界の点になるように Ml45236dmd を修正する。次いで、 Mdmd から上記の如く決定した M l45236dmd を差し引いたべクトルが、 Mdmdl45 と Mdmd236 との和にほぼ一致し、且つ、 Mdmdl45 + Mdmd23 の V145236 方向の成分が Mdmd— Mdmdl45236の V145236成分にできるだけ近いものとなり且つ前記条件 1 1 ) を満たすように、 Mdmdl45， Mdmd236 を決定する。なお、この場合、 Mdmdl45 , Mdmd236 は、互いに平行なべクトルとする。 '

このように決定された M145236dmd、 M 145dmd、 M236dmd の例を図 2 9 ( b ) に示す。補足すると、 M145236dmd は、前記したように線分 Q 145Q236に垂直な水平べクトルである。

なお、ロボット 1の鉛直軸回りの姿勢も制御するような場合には、 M145236dmd、 M145dmd、 M236dmd の鉛直成分も決定するようにしてもよい。

第 2実施形態における各補償角決定部 1 1 4 b〜 1 1 4 dは、基本的には、それぞれ第 nノード（n=145236，145, 236)の、ノード補償床反力モーメントと、実ノード床反力モーメント（目標ノード床反力中心点を作用点とするモーメント）との偏差に応じて、その偏差を 0に近づけるようにノード補償角 Θ 145236、 Θ 145 , 0 236 を決定する。図 3 0および図 3 1にその例を示す。この場合、本実施形態では、 0 145 は、図 3 0に示す如く、偏差 M145act—M145dmd と同方向で、目標第 145 ノ一ド床反力中心点 Q 145 を通る軸回りの回転角、 0 236は、図 3 0に示す如く、偏差 M236act— M236dmd と同方向で、目標第 236 ノード床反力中心点 Q236 を通る軸回りの回転角である。また、 0 145236 は、図 3 1 に示す如く、偏差 M145236act— M145236dmd と同方向（線分 Q145Q236 に垂直な水平方向）で、目標全床反力中心点 P を通る軸回りの回転角である。

但し、本実施形態においても、第 1実施形態と同様に、第 nノード (n=145236，145，236)の、ノード補償床反力モーメントをフィルタに通したものと、実ノード床反力モーメントをフィルタに通したものとの偏差に応じて、ノード補償角 0145236、 Θ 145, 6> 236 を決定するようにした。 ―

図 3 2は上記の如くノ一ド補償角 Θ 145236 を決定する補償角決定部 1 1 4 b ( Θ 145236 決定部）の機能を示すプロック図、図 3 3は上記の如くノ一ド補償角 Θ 145 を決定する補償角決定部 1 1 4 c (0145 決定部）の機能を示すブロック図である。なお、補償角決定部 1 1 4 d ( 0236決定部）の処理は、 Θ 145 決定部 1 1 4 bの処理と同様であるので詳細な説明および図示を省略する。

補償角決定部（ 0145236 決定部） 1 1 4 bでは、まず、目標第 145 ノード床反力中心点 Q145 に実第 1、第 4、第 5ノード床反力の合力の並進力成分（Flact+F4act+F5act) が作用すると共に、目標第 236 ノ一ド床反力中心点 Q236に実第 2、第 3、第 6ノード床反力の合力の並進力成分（F2act+F3act+F6act) が作用した場合に、目標全床反力中心点 P (目標第 145236 ノード床反力中心点）のまわりに発生するモ一メント M145236act を前記式 1 6または 1 7に基づいて求める。そして、その求めた M145236act の、ベクトル V145236 方向の成分 M145236actv を内積演算によって求める。ここでべクトル V145236は、線分 Q145Q236 に垂直で且つ水平な単位ベクトルである。なお、第 145236 ノードに関しては、 V145236 に垂直で且つ水平な単位ベクトル U145236 の方向の軸回りに子ノ一ド（第 145 ノ一ドおよび第 236 ノ一ド）の目標床反力中心点の位置を操作できないので、 M145236act のべクトル U145236方向の成分は求める必要はない。

次いで、前記第 1実施形態と同様に、この Ml45236actv をローパスフィルタに通したもの M 145236actvfilt と、前記第 145236ノ一ド補償床反力モーメント M 145236dmd のべクトル V 方向の成分 M145236dmdv を補償フィル夕に通したもの M145236dmdvfilt との偏差 M 145236errv ( = M145236actvfilt - M145236dmdvfilt ) に所定のゲイン K145236 を乗算することで、第 145236 ノ一ド補償角 Θ 145236 が決定される。

補償角決定部（ 0 145決定部） 1 1 4 cでは、まず、目標第 1、第 4、第 5ノード床反力中心点 Q l， Q4, Q5 にそれぞれ実第 1、第 4、第 5 ノード床反力の並進力成分 Flact, F4act， F5act が作用した場合に、第 145 ノード床反力中心点 Q 145 のまわりに発生するモーメント M145act を算出する。この場合、算出する M145act は、互いに直交する水平な単位べクトルであるべクトル V145および U145のそれぞれの方向の成分から構成される。ベクトル V145 または U145の向きは、任意でよい。

次いで、この M145act をローパスフィルタに通したもの M145actfilt と、前記第 145 ノード補償床反力モーメント M145dmd (ベクトル V方向の成分およびベクトル U 方向の成分）を補償フィル夕に通したもの M145dmdfilt との偏差 M145err ( = M145actfilt - M145dmdfilt) に所定のゲイン行列 Κί45 (対角行列）を乗算することで.、第 145 ノード補償角 Θ 145 が決定される。 Θ 145 は、ベクトル V の軸回り角度成分とベクトル U の軸回りの角度成分とから構成される。補償角決定部（ 0 236 決定部） 1 1 4 dによる第 236 ノード補償角 Θ 236 の決定処理も、上記した 0 145決定部 1 1 4 cの処理と同様に行なわれる。

以上説明した補償角決定部 1 1 4 b〜 1 1 4 dの処理によって、目標全床反力中心点 Pに作用する実モーメント Mact の水平成分を補償全床反力モーメント Mdmd に近づけるようにノード補償角 Θ 145236， Θ 145， 0 236の組が決定されることとなる。

なお、第 1実施形態に関して補足説明したように、次のようにノード補償角 0 145236, Θ 145, 0 236 を決定するようにしてもよい。すなわち、子ノードを持つ各第 nソード（n=145, 236, 145236)に対し、その第 nノードの実ノード床反力（第 nノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が 0となるような床反力中心点を実第 n ノード床反力中心点として求める。あるいは、第 n ノード (n=145, 236, 145236)の実ノ一ド床反力のモ一メントから、その各子ノ —ドの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が 0 となるような床反力中心点を実第 nノード床反力中心点求める。そして、第 n ノード (n=145, 236， 145236)の目標ノード床反力中心点と、上記の如く求めた実第 nノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィルタに通した値の偏差を制御量とし、その制御量を 0に近づけるように、該制御量に応じてノード補償角 0 145236, Θ 145, Θ 236 を決定する（例えば制御量にあるゲインを乗算することで、 0 145236, Θ 145, 0 236を決定する）。

図 2 5に示した第 2実施形態における修正目標接地部位位置姿勢算出部 1 1 4 gは、次のように、各接地部位 1 0の目標接地部位位置姿勢 (図 1の口ポットでは実際には目標接地部位位置）を修正し、修正目標接地部位位置姿勢を得る。すなわち、図 3 0および図 3 1を参照して、第 145 ノードの子ノードである第 1、第 4および第 5 ノードの目標床反力中心点 Q l， Q4, Q5 をそれぞれ、第 145 ノードの目標床反力中心点 Q 145 を回転中心として、第 145 ノード補償角 0 145 (水平べクトル）だけ回転移動する。この回転移動後の Q l , Q4 , Q5 をそれぞれ Q l', Q4*, Q5'とする。従って、第 145 ノード補償角 Θ 145 は、第 145 ノードの目標床反力中心点 Q145 の位置を動かさずに、第 145 ノードの子ノードである第 1、第 4、第 5ノードのそれぞれの目標床反力中心点 Q l , Q4, Q4の位置の相対関係を動かすための操作量である。

同様に、第 236 ノードの子ノードである第 2、第 3および第 6ノードの目標床反力中心点 Q2， Q3, Q6 をそれぞれ、第 236 ノードの目標床反力中心点 Q236 を回転中心として、第 236 ノード補償角 6> 236 (水平ベクトル）だけ回転移動する。この回転移動後の Q2， Q3, Q6 をそれぞれ Q2'， Q3', Q6'とする。従って、第 236 ノード補償角 0 236は、第 236 ノードの目標床反力中心点 Q236 の位置を動かさずに、第 236 ノードの子ノードである第 2、第 3、第 6ノードのそれぞれの目標床反力中心点 Q2， Q3, Q6 の位置の相対関係を動かすための操作量である。

これらの回転移動を視覚的に示したものが図 3 0である。

次いで、第 145236 ノードの子ノードである第 145 および第 236 ノードの目標床反力中心点 Q 145, Q236 をそれぞれ、第 145236 ノードの目標床反力中心点 P (目標全床反力中心点）を回転中心として、前記偏差 M145236errv のベクトル（水平ベクトル）と同方向（線分 Q 145Q236 と直交する水平方向）の軸心まわりに第 145236 ノ一ド補償角 0 145236だけ回転移動する。この回転移動後の Q 145， Q236 を図 3 1に示す如くそれぞれ Q 145'， Q236'とする。従って、第 145236 ノード補償角 0 145236 は、第 145236 ノード（根ノード）の目標床反力中心点 Pの位置を動かさずに、第 145236 ノードの子ノードである第 145、第 236 ノードのそれぞれの目標床反力中心点 Q 145, Q236 の位置の相対関係を動かすための操作量である。

次いで、図 3 1 を参照して、先の回転移動後の目標ノード床反力中心点 Q l'， Q4', Q5'をベクトル Q 145一 Q 145'だけ平行移動し、これにより . 第 1、第 4および第 5 ノードの最終的な修正後の目標ノード床反力中心点 Q l", Q4" , Q5"を得る。同様に、先の回転移動後の目標ノード床反力中心点 Q2'， Q3', Q6'をベクトル Q236— Q236'だけ平行移動し、これにより、第 2、第 3および第 6ノードの最終的な修正後の目標ノード床反力中心点 Q2"， Q3", を得る。

最後に、第 n接地部位（11= 1，2, 3，4，5,6)の目標接地部位位置を、べクトル Qn_Qn"だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位 1 0の目標接地部位位置（より正確には各接地部位 1 0の目標接地部位位置の相対関係）が修正されることとなる。すなわち、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位

1 0の目標接地部位位置の間の相対関係（相対的位置関係）の操作量 (修正量）が各補償角 0 145, Θ 236, 0 145236 に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位 1 0の目標接地部位位置の相互の相対関係が修正されることとなる。

なお、接地部位の姿勢を制御可能として、接地部位の目標床反力中心点まわりに床反力モーメントを発生させることができる口ポットでは、各接地部位の目標接地部位床反力中心点（目標ノード床反力中心点）まわりの足平姿勢回転動作を、特開平 1 0 — 2 7 7 9 6 9号公報（複合コンプライアンス制御）に示した手法で行なえば良い。すなわち、第 1実施形態に関して補足説明した如く、第 n接地部位を上記の平行移動後の Qn"を中心として、該第 n接地部位の目標姿勢を修正するようにすればよい。

図 2 5に示した第 2実施形態における機構変形補償量算出部 1 1 4 η は、前記第 1実施形態のものと同様に、各各脚 # 1〜 # 6のコンプライアンス機構 4 2などの変形による影響を補償するための機構変形補償量 £ 11—(：11^ 11 (11= 1,2，3，4, 5，6)を求める。すなわち、第 2実施形態における補償全床反力モーメント分配器 1 1 4 aから出力される各接地部位 1 0の修正目標接地部位床反力 Fn_refmdfd(n=l, 2, 3, 4, 5,6，）（目標接地部位床反力に、各ノード補償床反力モーメント M145236dmd、 M145dmd、 M236dmd を付加することによって修正された目標接地部位床反力）によって発生が予想される各脚 # 1〜 # 6のコンプライアンス機構 4 2などの機構変形量 En_mdfd(n= l,2,3,4, 5,6)を機構コンプライアンスモデルを用いて求める。この En一 mdfd に（_ 1 ) を乗算したものを機構変形補償量 En_cmpnとして求める。

図 2 5に示した第 2実施形態における機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hは、第 1実施形態のものと同様に、機構変形補償入り修正目標接地部位位置姿勢は、各接地部位 1 0の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部 1 1 4 gで求められ位置姿勢）に、それぞれに対応する機構変形補償量 En__Cmpn を付加することにより決定される。

以上が、本実施形態（第 2実施形態）における階層型コンプライアンス動作決定部 1 1 4の処理の詳細である。

以上説明した以外の制御装置 5 0の演算処理は、第 1実施形態と同じである。

なお、第 2実施形態における階層構造は、第 1実施形態の場合と同様に、ロボット 1の動作形態（移動形態）などに応じて変更しても良い。例えば、図 3 4に示す如く、階層構造を設定してもよい。この図示の例では、第 1接地部位および第 2接地部位を子ノード（葉ノード）としてもつ第 12 ノードと、第 3接地部位および第 4接地部位を子ノード（葉ノード）としてもつ第 34 ノードと、第 5接地部位および第 6接地部位を子ノード（葉ノード）としてもつ第 56 ノードと、第 56 ノードおよび第 34 ノードを子ノードとしてもつ第 3456 ノードとを中間ノードとして備え、根ノードは、第 3456 ノードと、第 12 ノードとを子ノードとしてもつものとされている。このようにすることで、コンプライアンス動作および後述の床形状推定をより的確に行なうことが可能となる場合がある。なお、図中の符号の意味は、前記図 3 ( b ) や図 2 4などに示したものと同様である。 ' [第 3実施形態]

次に、第 2実施形態のロボット 1 ( 6脚ロボット）に床形状推定機能とその推定結果に応じたロポット 1の動作補正機能とを付加した第 3実施形態について説明する。なお、本実施形態では、主に、第 2実施形態で示した 6脚ロボットを中心として説明するが、 4脚ロポットについても補足的に説明を付加する場合がある。また、本実施形態の理解の便宜上、 2脚口ポットについても言及する場合がある。

本実施形態におけるロポット 1の機構的構成は、第 1または第 2実施形態で説明した図 1に示したもの（ただし、 6個の脚 # 1〜 # 6を有するロボット 1 ) と同じである。従って、口ポット 1の機構的構成の説明は省略する。また、本実施形態におけるロボット 1に備えた制御装置 5 0の機能的構成も、前記図 2に示したものと同じである。但し、本実施形態では、図 2中の階層型コンプライアンス動作決定部 1 1 4には、新たな機能が付加されており、その点が第 2実施形態のものと相違している。そして、階層型コンプライアンス動作決定部 1 1 4以外の制御装置 5 0の構成要素の処理は、第 2実施形態と同一である。そこで、本実施形態での説明では、階層型コンプライアンス動作決定部 1 1 4の処理を中心に説明し、これ以外の制御装置 5 0の処理については、詳細な説明を省略する。

図 3 5は、本実施形態における階層型コンプライアンス動作決定部 1 1 4の処理機能を示すブロック図である。このうち、第 2実施形態のものと異なる機能を説明すると、本実施形態における階層型コンプライアンス動作決定部 1 1 4には、新たに、床形状推定手段としての床形状推定器 1 3 0と、床形状推定器 1 3 0が出力（推定）する推定床形状偏差 (より詳しくは各接地部位 1 0に係る後述の床高さ偏差の推定値）を修正目標接地部位位置姿勢に加算する加算器 1 3 2とが付加され、加算器 1 3 2の出力を、修正目標接地部位位置姿勢の代わりに機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hに入力するようにしている, これ以外の階層型コンプライアンス動作決定部 1 1 4の構成要素の処理は第 2実施形態のものと同一である。

また、これに伴い、本実施形態での制御装置 5 0のメインルーチンの制御処理は、その一部が前記図 9のフローチャートに示した制御処理と相違している。図 3 6は本実施形態における制御装置 5 0のメインルーチンの制御処理を示すフローチャートである。図示の如く、本実施形態では、 S 3 6の後に、新たに S 3 7で床形状偏差を推定する処理（床形状推定器 1 3 0の処理）が追加される。さらに、 S 3 8 ' では、前記第 2実施形態で説明した各補償角 145236, Θ 145, 0 236 と S 3 7で推定された床形状偏差とに応じて各目標接地部位位置姿勢が修正され、その修正後の目標接地部位位置姿勢をさらに機構変形補償量に応じて修正することにより、最終的な目標接地部位位置姿勢たる機構変形補償入り修正目標接地部位位置姿勢が得られる。この場合、より詳しくは、各補償角 0 145236, Θ 145 , 0 236 に応じて第 2実施形態と同様に修正目標接地部位位置姿勢を求めた後、その修正目標接地部位位置姿勢が床形状偏差に応じて修正され、さらに、これが機構変形補償量に応じて修正されることで、機構変形補償入り修正目標接地部位位置姿勢が得られる。上記した事項以外は、図 9の処理と同じである。

以降、本実施形態の第 2実施形態と相違する点を具体的に説明する。まず、本実施形態の詳細な説明に入る前に、床形状推定器 1 3 0が推定する上で使用する概念と用語を以下のように定義する。なお、ここでの説明は、便宜上、本実施形態の 6脚ロボット 1に限らずに、一般的なロボッ卜の簡略的な図を使用して行なう。

図 3 7、図 3 8および図 3 9に示すように、目標歩容において想定された床（あるいは床面）を「想定床」と呼ぶ。口ポットが移動する実際の床を「実床」と呼ぶ。なお、説明の便宜上、図 3 7では第 1実施形態で説明した 4脚ロボットを図示し、図 3 8および図 3 9では、 2脚ロボットを図示しているが、以下に説明する用語の意味は、本実施形態における 6脚ロポット 1を含む任意の多脚ロボットにおいても同様である。前記第 1および第 2実施形態で説明した階層型コンプライアンス制御において定義された目標第 n接地部位床反力中心点 Q nは、第 n接地部位の中心点に設定された点であつたが、その床反力中心点 Qnは、該第な接地部位の接地面（底面）に設定されていてもよい。この場合、目標歩容において、目標第 n接地部位床反力中心点 Q nと接することが想定された想定床面上の点を「想定第 n床接点 D n」と呼ぶ。

この定義から明らかなように、口ポットの目標歩容において第 n接地部位が接地している時期では、目標第 ii接地部位床反力中心点 Q nと想定第 n床接点 D n とは、支持脚座標系（グロ一バル座標系）から見て同一座標になる。これに対し、実際に口ポット 1が移動しているときに、実第 n接地部位の底面上における目標第 n接地部位床反力中心点 Q nに対応する点が、実床に接触する点を「実第 n床接点 D _nact」と呼ぶ。これらの点の関係を表す例を図 3 7、図 3 8および図 3 9に示す。尚図 3 7は 4脚ロボットを、目標第 1接地部位床反力中心点 Q 1 と目標第 2接地部位床反力中心点 Q 2 とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図 3 8は移動（歩行）している 2脚ロボットを目標第 1接地部位床反力中心点 Q 1 と目標第 2接地部位床反力中心点 Q 2とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図 3 9はほぼ直立姿勢の 2脚ロボットを目標第 1接地部位床反力中心点 Q 1 と目標第 2接地部位床反力中心点 Q 2とを通る垂直面の法線方向から (すなわちほぼ背面から）見た図である。

これらの図 3 7〜図 3 9には、前記垂直面における想定床の断面が細線で示され、前記垂直面における実床の断面が太線で示されている。なお、図 3 7では、ロボッ卜の目標姿勢（目標歩容の瞬時値におけるロボッ卜の全体的な姿勢）と、実姿勢とがそれぞれ破線、実線で示されている。また、図 3 8および図 3 9では、口ポットの目標姿勢（目標歩容の瞬時値における口ポットの全体的な姿勢）と、実接地部位位置姿勢とがそれぞれ細線、太線で示されている。これらの状況における実第 n床接点は、実床面上の点であり、図 3 7、図 3 8および図 3 9に示す位置になる。

想定床面に対する実床面の形状偏差を床形状偏差と呼ぶ。床形状偏差を定量的に表現する指標として、第 nノード床高さ偏差、第 nノード床傾斜偏差を以下のように定義する。

第 n床接点における床面の高さを「第 n接地部位床高さ」と呼ぶ。葉である第 nノードに対して、実第 n接地部位床高さと想定第 n接地部位床高さとの差を「第 n接地部位床高さ偏差」あるいは、「第 nノード床高さ偏差」と呼ぶ。第 n床接点における床面の傾斜を「第 n接地部位床傾斜」と呼ぶ。葉である第 nノードに対して、実第 n接地部位床傾斜と想定第 n接地部位床傾斜との差を「第 n接地部位床傾斜偏差」あるいは「第 nノード床傾斜偏差」と呼ぶ。接地部位床傾斜偏差の例を図 3 9に示す。

葉ノードである、あらゆる第 j ノードに対して、目標第 j 接地部位位置姿勢と想定第 j 床面の高さと傾斜（詳しくは、想定第 j 床接点における想定床面の高さと傾斜）の関係と、ノード補償角の組を用いたコンプライアンス動作によって修正された修正目標第 j 接地部位位置姿勢と実第 j 床面の高さと傾斜（詳しくは、実第 j 床接点における実床面の高さと傾斜）の関係とがー致するためのノード補償角の組を「ノード床傾斜偏差（の組）」と呼び、このうちの第 nノード補償角に対応するノード床傾斜偏差の成分を「第 nノード床傾斜偏差」と呼ぶ。このように定義した「第 nノード床傾斜偏差」は、第 nノードが葉ノードである場合、先に上記で葉ノドに関して定義した「第 nノード床傾斜偏差」（=実第 n接地部位床傾斜と想定第 n接地部位床傾斜との差）に一致するものである。

結局、ノード床傾斜偏差の組は、目標歩容通りに移動しているロポットの全ての接地部位を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な補償量に相当する。

従って、歩行中に床形状偏差を推定し、推定した床形状偏差を目標接地部位位置姿勢に加えれば、床形状偏差があっても、各ノードの実床反力モーメントは想定床を歩行しているときと同一になる。当然、葉ノードの実床反力モーメントである接地部位の実床反力モーメントも目標接地部位床反力モーメントに一致する。

通常は、上記のごとく定義される「第 nノード床傾斜偏差」を用いて床形状を表現して構わないが、あるノードが 3つの子ノードを持ち、 3 つの子ノードの目標床反力中心点が同一直線上に並ぶ場合や、あるノードが 4つ以上の子ノードを持つ場合などでは、床形状を表現することが困難になる。

そこで、以降の説明においては、葉でない第 nノードに対しては、より一般的に適用できる以下の表現を用いることとする。以下の定義における所定の重み付き平均を求めるために用いる重みは、前記目標床反力分配器 1 0 2が前記した如く決定した前述の重みと同一のものとする。

¾ si ：

すべての目標接地部位の高さと傾斜を、それぞれ対応する実床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすベての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第 nノードに対し、第 nノードの目標床反力中心点の高さ（鉛直方向位置）から第 nノードの親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「実第 nノード相対床高さ」と呼ぶ。

¾ ：

すべての目標接地部位の高さと傾斜を、それぞれ対応する想定床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすベての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第 nノードに対し、第 nノードの目標床反力中心点の高さ —(鉛直方向位置）から第 nノードの-親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「想定第 nノード相対床高さ」と呼ぶ。

実第 nノード相対床高さから想定第 nノード相対床高さを減じた高さを「第 nノード相対床高さ偏差」と呼ぶ。第 nノード相対床高さ偏差は、以下のように定義しても、同一値となる。定義：

すべての目標接地部位の高さと姿勢を、それぞれ対応する接地部位床高さ偏差と接地部位床傾斜偏差に一致させ、かつ、任意のノードの目標床反力中心点をそのすベての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第 nノードに対し、第 nノードの目標床反力中心点高さから第 nノードの親ノードの目標床反力中心点高さを減じた高さを「第 nノ一ド相対床高さ偏差」と呼ぶ。

上記した第 nノード相対床高さ偏差の定義から明らかなように、子ノ —ドを持つ各ノードに対し、その全ての子ノードの第 nノード相対床高さ偏差は、床高さ偏差の相対関係を表すものとなる。また、子ノードを持つ各ノードに対し、その全ての子ノ一ドの重み付き平均値は 0となる, 本実施形態（第 3実施形態）では、実床面と目標歩容上での想定床面とのずれ（すなわち床形状偏差）を補償して、接地すべき接地部位 1 0 を適正に実床面に接地させるように各接地部位 1 0の目標位置姿勢の修正を行なう。そして、本実施形態では、上記のように定義される第 nノ一ド相対床高さ偏差を床形状偏差を表す床形状パラメータとして用い、これを基に、各第 n接地部位床高さ偏差（床形状偏差）を推定する。そして、推定した各第 n接地部位床高さ偏差（以下、単に第 n床高さ偏差ということがある）に応じて第 n接地部位 1 0の目標位置を修正するようにした。

以上を前提として、以下に本実施形態の要部の詳細を説明する。

図 3 5に示した階層型コンプライアンス動作決定部 1 1 4の床形状推定器 1 3 0への入力は、一般的には大きく分けて以下のようになる。 1 ) 各目標床反力中心点（目標全床反力中心点 P、目標第 n接地部位床反力中心点 Qn(n=l,2,3,4,5,6) ) o

2 ) 想定床面形状（想定第 n床接点の座標、想定第 n接地部位床傾斜）,

3 ) 最終的に実ロボットが追従すべき目標姿勢（機構変形入り修正目標接地部位位置姿勢）または実関節変位またはこれらの周波数重み付き平均（周波数特性を持つ重みによる重み付き平均）。

4 ) 上体姿勢傾斜偏差。

5 ) 実床反力（実第 n接地部位床反力の並進力成分とモーメント成分）, なお、本実施形態におけるロボット 1では、各接地部位 1 0の姿勢を制御できないので、想定床面形状のうちの、想定第 n接地部位傾斜は不要である。

また、本実施形態におけるロボット 1では、実床反力のうちの実第 n 接地部位床反力のモーメント成分は 0になるので、これも床形状推定器 1 3 0に入力する必要は無い。ただし、以降の本実施形態の説明では、各接地部位 1 0の姿勢を制御できる場合も考慮し、第 n接地部位床反力には、一般に、実第 n接地部位床反力のモーメント成分も含まれるものとする。また、以降の説明では、接地部位の個数が 4個や 6個以外の場合にも容易に本実施形態を拡張的に適用できるように、接地部位の総数をしばしば「最終葉ノード番号」' と表す。例えば図 1の 6脚ロボット 1 では、最終葉ノード番号は、「 6」である。

図 4 0は、床形状推定器 1 3 0の処理機能を示すブロック図である。図 4 0に示す床形状推定器 1 3 0を構成する各機能要素について説明すると、床形状推定器 1 3 0は機構コンプライアンスモデル 1 3 4を備える。

機構コンプライアンスモデル 1 3 4は、各接地部位 1 0が実床反力 Fn act(n=l,2, "'，最終葉ノード番号）を受けたときの各脚 # 1 〜 # 6のコンプライアンス機構 4 2など（各脚のコンプライアンス機構 4 2およびリンク機構）の変形量を求め、求めた変形量を、前記ロボット幾何学モデル 1 1 0 (図 2参照）への入力である各機構変形補償入り修正目標接地部位位置姿勢に加えることにより、機構変形後の第 n接地部位（各接地部位）の位置姿勢の推定値（より詳しくは、実上体姿勢が目標上体姿勢に一致していると仮定した場合の第 n接地部位位置姿勢の推定値）である推定第 n接地部位位置姿勢 (_n=l, 2, ···，最終葉ノ一ド番号）を求める, なお、このときに用いる各機構変形補償入り修正目標接地部位位置姿勢は、前回制御周期で求めた値などの過去値である。

機構変形後の各推定接地部位位置姿勢を求める際に、ロボット 1の関節変位の制御の追従遅れが無視できない場合には、機構変形補償入り修正目標接地部位位置姿勢をそのまま用いる代わりに、機構変形補償入り修正目標接地部位位置姿勢を、追従遅れに相当する口一パスフィル夕に通してなる位置姿勢を用いれば良い。

あるいは、口ポット 1の実関節変位の検出値からロボット幾何学モデル（図 2のロポット幾何学モデル 1 1 0と同様のモデル）を介して機構変形が無い場合の実接地部位位置姿勢である機構変形なし実接地部位位置姿勢を求め、求めた値を機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。

あるいは、周波数重み（周波数特性を持つ重み）を用い、前記機構変形なし実接地部位位置姿勢と機構変形補償入り修正目標接地部位位置姿勢の重み付き平均を求め、それを機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。

厳密には、前記した機構変形なし実接地部位位置姿勢を用いるべきであるが、関節変位制御の能力が高ければ、上記のいずれを用いても大差はない。なお、機構コンプライアンスモデル 1 3 4については、本出願人が先に提案した特開平 1 0 — 2 7 7 9 6 9号公報において詳細が説明してあるので、ここではこれ以上の説明を省略する。

補足すると、図 2においてロボット幾何学モデル 1 1 0への入力に機構変形補償が含まれない場合、すなわち、機構変形補償のない修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部 1 1 4 gで求められる位置姿勢）をロボット幾何学モデル 1 1 0に入力する場合には. その機構変形補償のない修正目標接地部位位置姿勢を床形状推定器 1 3 0の機構コンプライアンスモデル 1 3 4に入力すれば良い。

図 4 0の説明に戻ると、次いで、機構変形後の推定各接地部位位置姿勢（η= 1,2, ···，6)を、目標全床反力中心点 Ρ を回転中心として前記上体姿勢傾斜偏差 0 berr だけ回転移動させることにより、グローバル座標系 (支持脚座標系）から見た機構変形後の第 η 推定接地部位位置姿勢 (η=1, 2, ··· ,最終葉ノード番号）を求める。次いで、グローバル座標系から見た機構変形後の第 η 推定接地部位位置姿勢（η= 1, 2，…，最終葉ノード番号）と第 η接地部位の目標接地部位位置（第 η接地部位の代表点の目標位置）から見た目標第 η 接地部位床反力中心点の位置とを基に、グロ一バル座標系から見た機構変形後の目標第 η 接地部位床反力中心点に対応する点の位置を求める。そして、この求めた点の位置を瞬間推定床接点位置（瞬間推定第 η床接点位置） Qn一 estm'とする。この Qn_estm' は、実第 n床接点の瞬時位置の推定値に相当するものである。尚、瞬間推定床接点位置 Qn— estm'の求め方の詳細は、特開平 1 0 — 2 7 7 9 6 9号公報中の式 2 1の Q "を求めることに相当するので、ここではこれ以上の説明を省略する。

次いで、瞬間推定第 n床接点位置 Qn一 estm'から想定第 n床接点位置 Dn を減じたものを、バイアス含有瞬間第 n接地部位床高さ偏差 Zfn— with_bias'として得る。この Zfn— with— bias'は、第 n接地部位床高さ偏差の瞬時推定値に相当するものであるが、本実施形態のように上体の実高さを検出しない場合には、一般にバイアス誤差を含んでいる。そのため、 Zfn— with_bias'をバイァス含有瞬間第 n接地部位床高さ偏差と呼ぶ。

ここで、想定第 n床接点 D nは、先に定義したように、第 n接地部位が接地している時期においては、目標第 n接地部位床反力中心点 Q nと同一位置とする。目標歩容において第 n接地部位が接地する直前の時期では、次に接地する時に想定されている目標第 n接地部位床反力中心点 Q nの位置を想定第 n床接点 D nとする。目標歩容において第 n接地部位が離床した直後の時期では、離床時に想定されていた目標第 n接地部位床反力中心点 Q nの位置を想定第 n床接点 D nとする。

次いで、バイアス含有瞬間第 n 接地部位床高さ偏差 Zfn_with_bias^,(n=l,2,…，最終葉ノード番号）と第 n接地部位床反力 Fn— act ( η=1,2，···，最終葉ノード番号）とを基に、図 4 1のフローチヤ一卜で示す床高さ偏差推定処理サブルーチンによって、推定第 ηノード床高さ偏差 Zfn— estm ( η=1，2，···，最終葉ノード番号）を求める。

ここで、床高さ偏差推定処理サブルーチン等に用いられる「階層相対化処理」（または「階層相対化」 > について説明する。

階層相対化処理とは、一般的に定義すると、すべての葉ノードに対する入力値（ある所定種類の状態量の値）の組に対して、すべてのノードの出力値を決定する処理である。より詳しくは、階層化相対処理は、葉ノードでない任意のノードのすべての子ノードに対応する出力値の重み付き平均が 0であり、かつ、任意の葉ノードの入力値（状態量）が、そのノードの出力値とそのノードのすべての先袓ノードの出力値との和に一致するように各ノ一ド出力値を決定する処理のことである。以降、葉ノードに対するある種類の入力（状態量） AA から、階層相対化処理によってある種類の出力 BB の値を求めることを、「AA を階層相対化して BBを求める」と言う。

以下に階層相対化処理のアルゴリズムを説明する。一般的に、階層相対化処理の入力を第 n接地部位高さ Zfn( nは葉ノ一ド番号）、出力を第 n ノード相対高さ Zn— rel( n = l,2, "'，最終ノード番号）とする。なお、「最終ノード番号」は、全てのノード番号のうちの最大番号を意味し、例えば第 2実施形態で説明した階層構造では、最終ノード番号 = 145236 である。また、このアルゴリズムの説明では、「第 n接地部位高さ」は、前記第 n接地部位床高さ、あるいは第 n接地部位床高さ偏差など、階層化相対化処理の入力を総称的に表した名称であり、「第 nノード相対高さ」は、後述する第 n接地部位相対床高さ偏差など、階層相対化処理の出力を総称的に表した名称である。

まず、バイアス含有第 n ノード高さ Zn_with_bias を以下のように決定する。すなわち、第 n ノードが葉ノード（すなわち接地部位）であるならば、バイアス含有第 nノード高さ Zn_with_biasに第 n接地部位高さ Zfn の値を代入する。第 n ノードが葉ノード（すなわち接地部位）でないならば、第 nノードのすべての子ノードのバイアス含有第 n ノード高さ Zn— withj ias の重み付き平均を求め、これをもってバイァス含有第 n ノード高さ Zn一 withjbias とする。ただし、各子ノードに対する重みには、前記目標床反力分配器 1 0 2が決定した重み Wj(j=l，2， …；)を用いる。 ·

すなわち、以下の式 3 2 によって、バイアス含有第 n ノード高さ Zn_with_biasを得る。第 nノードが葉ノードである場合、 Zn— with— bias = Zfn

第 nノードが葉ノードでない場合、

Zn— with— bias =∑ (Zj— with—bias * Wj)

ただし、 ∑は〗£ {第 nノードの子ノ一ド番号の集合 }である j についての総和。

…式 3 2 以上のルールに従って、すべてのノードに対してバイァス含有ノード高さ Zn— with— bias ( η=1，2, ···，最終ノード番号）を求める。

最後に、第 η ノードの親ノード（これは第 h ノードであるとする）のバイアス含有高さ Zh— with— bias をバイアス含有第 n ノード高さ Zn— with— bias から減じることにより、第 n ノード相対高さ Zn— rel を求める。

すなわち、次式 3 3により第 n ノード相対高さ Zn— rel を求める。ただし、第 hノードは、第 nノードの親ノードであるとする。

Zn_rel = Zn— with— bias— Zh— with— bias …式 3 o 以上のルールに従って、すベてのノードに対してノード相対床高さ Zn_rel(n は各ノードの番号）を求める。ただし、根ノードに対するのード相対高さ Zk— rel(k は根ノードの番号）は 0とする。図 4 2に第 1 実施形態で示した 4脚口ポットのノード相対高さの算出例を示し、図 4 3.に本実施形態における 6脚ロポッ卜のノード相対高さの算出例を示すバイアス含有第 n ノード高さ Zn— reし with— bias は、同一の結果が得られる以下の方法によって求めても良い。

第 n ノードが葉ノード（すなわち接地部位）であるならば、バイァス含有第 nノード高さ Zn— with— biasに第 n接地部位高さ Zfnの値を代入する。第 n ノードが葉ノード（すなわち接地部位）でないならば、第 n ノードのすべての子孫の葉ノードのバイアス含有高さの重み付き平均を求め、これをもってバイアス含有第 n ノード高さ Zn— with— bias とする。ただし、ここでの各葉ノード j に対する重み Wj'は、そのノード j に対して前記目標床反力分配器 1 0 2が決定した重み Wj とそのノード jの祖先ノードかつ第 nノードの子孫ノードであるすベてのノードに対して前記目標床反力分配器 1 0 2が決定した重みとの積とする。すなわち、以下の式 3 4によって、バイアス含有第 n ノード高さ Zn— with— biasを得る。第 nノードが葉ノードであるならば、

第 nノードが葉ノードでないならば、

Zn— with— bias =∑ (Zj— with—bias * Wj')

ただし、 ∑は j e {ノード _nの子孫の葉ノ一ド番号の集合 }である j についての総和。

…式 3 4 補足すると、この場合の重み Wj'は、目標第 j ノード床反力が 0でなければ、各葉ノードの目標床反力を目標第 j ノード床反力で割った値に一致する。

以上が階層相対化処理である。

次いで、床高さ偏差の推定処理のサブルーチン処理を、それを示す図 4 1のフローチャート等を用いて以下に説明する。なお、この処理では、階層相対化処理の入力として、第 n接地部位床高さ偏差を用い、出力を第 nノード相対床高さ偏差とする。そして、これらの入力および出力の参照符号として、前記式 3 2 〜 3 4などに示したものと同様の符号を使用する。

図 4 1を参照して、まず、 S 5 0において、前記バイアス含有瞬間第 n接地部位床高さ偏差 Zfn— with— bias'を階層相対化して瞬間第 nノード相対床高さ偏差 Zn_rer ( n二 1,2, ··· ,最終ノード番号）

を求める。

次いで、 S 5 2において、' 制御装置 5 0の前回制御周期で求めた、第 n接地部位床高さ偏差の推定値である推定第 n 接地部位床高さ偏差 (以降、これを前回推定第 n接地部位床高さ偏差 Zfn_estm_p と呼ぶ）を階層相対化して得られる出力を前回推定第 n ノード相対床高さ偏差 Zn— reし estm_p(n=l,2, "'，最終ノード番号）として求める。ただし、この階層相対化処理で使用する各ノードの重みには、制御装置 5 0の今回 (現在）の制御周期で前記目標床反力分配器 1 0 2が決定した重み Wn(n=l,2，"'，最終ノード番号）を用いる。

補足すると、一般的に、今回の制御周期で決定された重み（以下、今回重みという）と前回の制御周期で決定された重み（以下、前回重みという）とが異なる場合には、実際の床の形状に変化がなくても、前回重みを用いて求めた各ノ一ドの前回推定相対床高さ偏差の値と今回重みを用いて求めた各ノ一ドの前回推定相対床高さ偏差の値とは異なる値となつてしまう。そこで、前回の制御周期で求めた各ノードの推定相対床高さ偏差をそのまま各ノードの前回推定ノード相対床高さ偏差として用いず、上記のごとく、今回重みを用いて各ノードの前回推定ノード相対床高さ偏差を改めて計算するようにした。

次いで、 S 5 4において、次式 3 5により、第 nノード相対床高さ偏差補正量候補値 Zn inc cand(n=l,2, "、最終ノード番号）を求める。この Zn_inc_candは、第 nノード相対床高さ偏差を今回の制御周期で更新するときの該偏差の補正量の仮値を意味するものである。

この場合、第 nノード相対床高さ偏差補正量候補値 Zn一 inc— cand は次式 3 5に示す如く、瞬間第 n ノード相対床高さ偏差 Z_n_rel'と前回推定第 n ノード相対床高さ偏差 Zn— rel— estm_p の差と所定の係数（Δ T/(Testm + Δ Τ))との積に設定される。

Zn_inc_cand = (Zn— rel'― Zn_rel_estm_p)

* Δ T/(Testm + Δ Τ)

…式 3 5

ここで、式 3 5中の Testm は第 nノード床高さ偏差の推定（更新）における時定数（一次遅れの時定数）であり、 Δ Τは制御装置 5 0の制御周期である。

このように、 Zn— inc— candは設定したとき、制御装置 5 0の制御周期毎に、 Zn— inc— cand を Zn— rel— estm— p に加算していくことで、その加算結果の値は、徐々に Zn_rel'に近づいていくように変化する。

次いで、 S 5 6において、目標歩容の時期に応じて各ノードのモードの要求値であるノード要求モード（第 n要求モード mdn_dmd) を決定する。 '

具体的には、前記 6脚ロボット 1 においては、第 n ノード要求モード（n= l，2, 3,4, 5,6)、第 145 ノード要求モード mdl45dmd、第 236 ノ ― ド要求モード md236dmd および第 145236 ノード要求モード mdl45236dmd を、目標歩容の時間経過に対して図 4 4のタイミングチヤートで示すように設定し、これにしたがって現在の要求モードを決定する。ただし、歩容が異なれば、それに応じてノード要求モードのバターンも変えるべきである。なお、図 4 4の第 1段（最上段）および第 2 段のタイミングチャートにおける O N Z O F Fは、接地部位が接地している状態が〇 Nに対応しており、接地していない状態が O F Fに対応している。

補足すると、本願出願人が、特開平 1 0— 2 7 7 9 6 9号公報に示したように、接地部位として足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、接地部位の床傾斜偏差推定に対応する要求モードも、同公報に説明されているように設定すべきである。

図 4 4に示すように、各ノードのモード（以下、単にノードモードという）には、準備完了モード、ホールドモード、およびリセットモードがある。

準備完了モードは、床形状の推定を行なうモードである。ホールドモードは、床形状の推定値（推定第 n接地部位床高さ偏差）をホールドする（直前の値を維持する）モードである。ホールドモードは、床形状推定値が発散する恐れがある時期と、床形状推定値の精度が低下する恐れがある時期に存在させる。また、リセットモードは、床形状推定値を、次の床形状の推定を開始する（次の準備完了モードの開始）までに所定の初期値に移行させるモ一ドである。

葉ノードである第 nノード（n=l,2,3,4，5，6 ) に対しては、目標歩容上で第 n接地部位が接地しているならば（すなわち目標第 n接地部位床反力が 0でないならば）、第 nノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第 n接地部位が離床した（すなわち目標第 n接地部位床反力が 0になった）ならば、その直後は第 nノード要求モードがホールドモードに設定される。そして、その後しばらくしたら第 nノード要求モードがリセットモードに設定される。さらに、目標歩容上で第 n接地部位が接地する直前から、第 nノード要求モードが準備完了モードに設定される。

また、葉ノードでない第 n ノード（n=145，236, 145236)、すなわち、子ノードをもつ第 nノードに対しては、その第 nノードに属する少なくとも 1つの接地部位が目標歩容上で接地している場合には、第 nノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第 nノードに属するすべての接地部位が離床したならば、その直後までは第 nノード要求モードがホールドに設定される。なお、第 nノードに属するすべての接地部位が離床する直前から第 nノード要求モードをホールドモードに設定するようにしてもよい。そして、その後しばらくしたら第 nノード要求モードがリセットモードに設定される。さらに、目標歩容上で第 nノードに属する少なくとも 1つの接地部位が接地する直前から、第 nノード要求モードが準備完了モードに設定される。

次いで、 S 5 8において、推定許可条件を満足するか否かを判定し、その判定結果とノード要求モードに基づき各ノードのモードを最終的に決定する。決定される各ノードのモードは、前記準備完了モード、ホールドモード、およびリセットノードのいずれかである。前記ノード要求モードは、目標歩容上での各接地部位の接地、非接地に応じて決定したものであつたが、 S 5 8では、各接地部位の実際の接地、非接地などを考慮して、各ノードのモードが決定される。

推定許可条件とは、以下のいずれかの式 3 6 、 3 7を満足することである。この場合、式 3 6、 3 7のいずれも満足しないときには、推定許可条件が成立しないとする。

Fn_act_z> Fn_min …式 ο

Zn— rel'\ Zn_rel_estm p …式 ό ί ここで、 Fn— act一 zは、実第 n接地部位床反力（n= l，2,…，最終葉ノード番号）の並進力鉛直成分である。推定許可条件とは、第 nノード相対床高さ偏差を推定しても（床形状偏差を推定しても）、その推定値が発散しない条件である。ここで発散とは、後述するごとく推定された推定第 n床高さ偏差 Zfn__{e S}tm (床形状偏差の推定値）を用いて、実際の第 n 床高さ偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正する補正動作を行った場合に、推定第 n床高さ偏差 Zfn一 estmが増加し続け、第 n接地部位が床から離れて行く（床からさらに浮いていく）状況を意味する。

Zfn— estm の推定が理想的に実行されるならば、所定の許容値 Fn_min は、 0で良いが、実際には Fn— act の検出誤差が生じ、それにより推定値が発散する場合がある。それを防ぐために、 Fn_min は、 Fn— actの検出誤差よりも十分に大きい値に設定する。

本実施形態では、推定許可条件とは、以上に述べた発散が生じない条件である。ただし、推定許可条件は、床形状の所要の推定精度を確保できる条件であってもよい。従って、実第 n接地部位床反力 F n— act の並進力鉛直成分の代わりに、実第 n接地部位床反力 F n__act の並進力成分の内の想定床面（あるいは推定床面（推定した実床面））に垂直な成分を用いても良い。

なお、床形状偏差（実床面と想定床面との形状偏差）を推定するだけで歩行動作に反映しない場合、すなわち、床形状偏差の推定値を用いて床形状偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正しない場合には、床形状偏差の推定値が発散することはない。

推定許可条件を満足するか否かの判定結果とノード要求モードに基づき各ノードのモードを決定する。第 n ノード要求モードが準備完了モードであり、かつ、推定許可条件を満足するならば、第 nノードモードを準備完了モードとする。

第 n ノード要求モードが準備完了モードであり、かつ、推定許可条件を満足しないならば、第 nノードモードをホールドモードとする。

第 n ノード要求モードがホールドモードであるならば、第 nノードモ —ドをホールドモードとする。

第 n ノード要求モードがリセットモードであるならば、. 第 nノードモ —ドをリセットモードとする。従って、第 nノードのモードは、要求モードが準備完了モードであり . 且つ、推定許可条件を満足する場合に限って、最終的に準備完了モードの決定される。

次いで、 S 6 0〜S 7 0において、すべてのノードに対し、ノードに対応する第 nノード相対床高さ偏差（n=l, 2，···，最終ノード番号）を推定する。

補足すると、特開平 1 0— 2 7 7 9 6 9号公報に示したように、姿勢を制御可能な接地部位（足平）を持ち、接地部位における床傾斜偏差を推定することができるロボッ卜の場合には、接地部位に対応した床傾斜偏差も推定することが望ましい。

このため、 S 6 0〜 S 7 0の処理は、接地部位の姿勢を制御可能な場合でも、床形状偏差の推定を行なうことができるように構成されている < 以下に S 6 0〜 S 7 0の処理を具体的に説明すると、 S 6 2において, 第 n ノードの子ノードの個数を判定する。このとき、子ノード数（子ノードの個数）が 2個の場合には、 S 6 4において、その個数に対応した床形状偏差の推定処理である 2子ノード用床形状推定処理が行なわれる。また、子ノード数が 3個の場合には、 S 6 6において、その個数に対応した床形状偏差の推定処理である 3子ノード用床形状推定処理が行なわれる。また、子ノード数が 0個の場合には、 S 6 8において、該第 nノードの接地部位が床反力モーメントを制御できるか否かを判定する, 子ノード数が 0の場合の処理についてさらに説明すると、第 nノードの子ノード数が 0の場合、第 nノードは葉ノードであり、第 nノードは接地部位に対応している。この場合、上記の如く S 6 8において、まず. 第 n接地部位が床反力モーメントを制御できるか否かを判定する。ここで、例えば、通常の 2足歩行口ポットのように、接地部位として姿勢を制御可能な足平を持ち、脚にフリージョイントを含まないロボットの場合には、接地部位で床反力モーメントを発生することができる。ただし. 本実施形態の口ポット 1 ( 6脚ロボット）においては、各接地部位 1 0 がフリージョイントである球関節 1 2に係合しているため、いずれの接地部位 1 0も床反カモ一メントを発生することができない。この場合には、子ノード数が 0である第 nノードに対する処理は実行されない。

一方、 S 6 8において、床反力モーメントを制御できると判定された場合には、 S 7 0において、接地部位床傾斜偏差推定処理が行なわれる _t この処理は、各接地部位における床傾斜偏差を推定する処理である。この処理は、本出願人が先に提案した特開平 1 0— 2 7 7 9 6 9号公報の中の床傾斜推定処理における足平床反力中心点を本実施形態における第 n接地部位の目標床反力中心点に置き換えた処理である。したがって、本明細書では、これ以上の詳細説明を省略する。

次に、前記 2子ノード用床形状推定処理を図 4 5〜図 4 8を参照して説明する。図 4 5は 2子ノード用床形状推定処理のサブルーチン処理を示すフローチャート、図 4 6、図 4 7はそれぞれ図 4 5の S 6 4 0 4、 S 6 4 0 8のサブルーチン処理を示すフ口一チャート、図 4 8は図 4 5 の S 6 4 0 6および S 6 4 1 0のサブルーチン処理を示すフローチヤ一卜である。

2子ノード用床形状推定処理では、図 4 5のフローチャートに示されるように、 S 6 4 0 0において、 2個の子ノードをもつ第 nノードのすベての子ノ一ドのモード（図 4 1の S 5 8で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」、および、「その他（else)」の 3つに分けられる。なお、以降の説明では、第 nノードの 2個の子ノードを第 i ノード、第】ノードとする。

ここで、子ノードのモードが「すべて準備完了」と判断すると、 S 6 4 0 2において、第 nノードの子ノードの実ノード床反力の並進力鉛直成分 Fi— act— z, Fj_act_z の合力 Fn— z ( = Fi— act— z + Fj— act— z) が所定の値 Fn— min2 より大きいか否かを判断する。 Fn_z は、換言すれば、第 nノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。

S 6 4 0 2の判断結果が Y E Sである場合には、 S 6 4 0 4において， 2つの子ノードを持つノード用のグループ内全推定処理（第 nノードが持つ 2つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図 4 6のフローチャート中に示す式に従って第 nノードの 2つの子ノードである第 i ノードおよび第 j ノ — ドのそれぞれの推定ノード相対床高さ偏差 Zし reし estm、 Zj—rel_estmが求められる（更新される）。すなわち、 Zし reし estm について代表的に説明すると、 Zi—relestm の前回制御周期での値 Zi_rel_estm_p に、前記 S 5 4で求めた第 j ノード相対床高さ偏差補正量候補値 Zi— inc_candを加えることで、新たな推定第 j ノード相対床高さ偏差 Zし reし estmが求められる。第 j ノードについても同様である。また、第 nノードの 2個の子ノードの床反力の合力 Fn z が所定の値 Fn_min2 より小さい場合（ S 6 4 0 2の判断結果が N Oである場合）には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、 S 6 4 0 6において、 2つの子ノードを持つノード用のグループ内全ホールド処理（第 nノードが持つ 2つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図 4 8のフローチヤ一ト中の式で示す如く . 第 i ノードおよび第 j ノードのそれぞれの推定ノード相対床高さ偏差 Zし rel_estm、 Zi_rel_estmの値が、前回制御周期での値 Zi— rel—estm— p， Zi_rel_estm_pに維持される。

次に、図 4 5の S 6 4 0 0で、 2個の子ノードのモードが「すべてリセット」と判断すると、 S 6 4 0 8において、 2つの子ノードを持つノ一ド用グループ内全リセット処理（第 nノードが持つ 2つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行なわれる。そのリセット処理では、図 4 7のフローチャート中の式に従って第 i ノードおよび第 j ノードのそれぞれの推定ノード相対床高さ偏差 Zi_rel_estm、 Zi_rel_estm が 0に徐々に近づいていくように更新される。なお、同式中の Δ Τ、 Testm の意味は、前記式 3 5のものと同じである。

このリセット処理は、より一般的に言えば、 Zi_reし estm , Zj_rel_estm を、それらの重み付き平均値が 0 という条件、すなわち Wi * Zi— reし estm + Wj * Zj_rel_estm = 0 を満足しつつ、それぞれ Zi_rel_estm_p , Zj_reし estm— p よりも' 0に近い値に決定するといぅ処理である。なお、このリセット処理は、一次遅れ要素を含むため、ノード相対床高さ偏差が完全に 0に戻るためには無限時間を要する。そこで、本出願人が先に提案した有限整定関数発生器（特開平 5 — 3 2 4 1 1 5 号公報）を用いて、ノード相対床高さ偏差を徐々に 0にリセットするようにしてもよい。

また、図 4 5の S 6 4 0 0で、 2個の子ノードのモードが「その他」である場合、すなわち、「すべて準備完了」、「すべてリセット」のいずれでもない場合（例えば 2個の子ノードのモードがすべてホールドモードである場合）には、 S 6 4 0 6と同じ処理（図 4 8を参照）が S 6 4 1 0で実行される。

以上のように、第 nノードの 2個の子ノードのモードがすべて準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その 2個の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、子ノードのモードがすべて準備完了である場合であっても、推定処理の発散が生じる恐れがある場合には、 2個の子ノードの推定ノード相対床高さ偏差の値が、ホールドされる。また、 2個の子ノードの実ノード床反力がいずれも 0となる状況、すなわち、第 nノードに属するすべての接地部位 1 0に実床反力が作用しない状況では、 2個の子ノードのノード相対床高さ偏差は、徐々に 0になるようにリセッ卜される。

次に、図 4 1の S 6 6の 3子ノード床形状推定処理を図 4 9〜図 5 5 を参照して説明する。図 4 9は 3子ノード用床形状推定処理のサブルーチン処理を示すフローチャート；図 5 0、図 5 1はそれぞれ図 4 9の S 6 6 0 4、 S 6 6 0 8のサブルーチン処理を示すフローチャート、図 5 2は図 4 9の S 6 6 0 6、 S 6 6 1 4および S 6 6 1 8のサブルーチン処理を示すフローチャート、図 5 3、図 5 4はそれぞれ図 4 9の S 6 6 1 2、 S 6 6 1 6のサブルーチン処理を示すフローチャートである。

3子ノード用床形状推定処理では、図 4 9のフローチャートに示されるように、 S 6 6 0 0において、 3個の子ノードをもつ第 nノードのすベての子ノードのモード（図 4 1の S 5 8で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」、「 2 つの子ノードのみ準備完了」、「 1つの子ノードのみホールドで、残りがリセット」、「 2つの子ノードのみホールドで、残りがリセット」、および「その他（else)」の 6つに分けられる。

ここで、図 4 9のフローチャートでは、 3子ノード用床形状推定処理をより一般化して説明するために、前記第 2実施形態で説明した如く、各中間ノード（第 145 ノード、第 236 ノード）に属する接地部位 1 0 のすべてが同時に接地したり離床する場合だけでなく、各中間ノードの接地部位のうちのいずれかが接地し、その他の接地部位が離床するようにロボットの目標歩容を生成する場合についても考慮している。この場合、子ノードが 2つの時と違い、 3つの子ノードのモードが、「 2つの子ノードのみ準備完了」、「 1つの子ノードのみホ一ルドで、残りがリセット」、もしくは「 2つの子ノードのみホールドで、残りがリセット」の場合も考慮する必要がある。なお、以降の説明では、第 nノードの 3 個の子ノードを第 i ノード、第 j ノード、.第 kノードとする。

ここで、子ノードのモードが「すべて準備完了」と判断すると、 S 6 6 0 2において、第 nノードの子ノードの実ノード床反力の並進力鉛直成分 Fi— act— z， Fj_act_z , Fk— act— ζ の合力 Fn— ζ ( = Fi— act— ζ + Fj— act— z + Fk— act_z) 合力が所定の値 Fn_min2 より大きいか否かを判断する。 Fn— z は、換言すれば、第 nノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。

S 6 6 0 2の判断結果が Y E Sである場合には、 S 6 6 0 4において 3つの子ノードを持つノード用グループ内全推定処理（第 nノードが持つ 3つの子ノ一ドのそれぞれのノ一ド相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図 5 0のフローチャート中に示す式に従って第 nノードの 3つの子ノードである第 i ノード、第 j ノードおよび第 kノ一ドのそれぞれの推定ノ一ド相対床高さ偏差 Zi_reし estm、 Zj_rel_estm , Zk_rel_estm が求められる（更新される）。すなわち、 Zi_rel_estm について代表的に説明すると、 Zi— reし estm の前回制御周期での値 Zし reし estm_pに、前記 S 5 4で求めた第 i ノ一ド相対床高さ偏差補正量候補値 Zi— inc_candを加えることで、新たな推定第 i ノード相対床高さ偏差 Zi— rel— estmが求められる。第 j ノード、第 kノードについても同様である。

また、 S 6 6 0 2の判断結果が N Oである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、 S 6 6 0 6において、 3つの子ノードを持つノード用のグループ内全ホールド処理（第 nノードが持つ 3つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図 5 2のフローチャート中の式で示す如く、第 i ノード、第 j ノ一ドおよび第 k ノードのそれぞれの推定ノード相対床高さ偏差 Zi— reし estm、 Z]'_reし estm、 Zk_reし estm の値が、前回制御周期での値 Zi_rel_estm_p > Zj— rel— estm— p、 Zk— rel—estmに維持 'れる。

また図 4 9の S 6 6 0 0で、 3個の子ノードのモードが「すべてリセット」と判断すると、 S 6 6 0 8において、 3つの子ノードを持つノード用グループ内全リセット処理'（第 nノ一ドが持つ 3つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行われる。そのリセット処理では、図 5 1のフローチャート中の式に従って第 i ノード、第 j ノードおよび第 kノードのそれぞれの推定ノード相対床高さ偏差 Zi— reし estm、 Zj_reし estm、 Zk— reし estm が 0に徐々に近づいていくように更新される。なお、同式中の Δ Τ、 Testm の意味は、前記式 3 5のものと同じである。

このリセット処理はより一般的に言えば、 Zi rel estm, Zj_rel_estm , Zk— reし estm を、それらの重みつき平均値が 0 という条件、すなわち Wi * Zi— rel— estm + Wj * Zj_reし estm + Wk * Zk— rel— estm = 0 を満足しつつ、それてれ Zi_rel_estm_p, Zj_rel_estm_p, Zk_rel_estm_p よりも 0 に近い値に決定するという処理である。なお、 Zし reし estm— p， Zj_rel_estm_p, Zk— reし estm— p を 0にリセットするために、前記有限整定関数発生器（特開平 5— 3 2 4 1 1 5号公報）を用いてもよい。

また、図 4 9の S 6 6 0 0で、 3個の子ノードのモードが「 2つの子ノードのみ準備完了」と判断すると、第 nノードの子ノードの実ノード床反力の合力（第 nノードに属するすべての接地部位 1 0の実床反力の合力）の並進力鉛直成分 Fn— z が所定の値 Fn— min2 より大きいか否かを S 6 6 1 0で判断する。

この判断結果が Y E Sである場合には、 S 6 6 1 2において、 3つの子ノードを持つノード用グループ内部分推定処理（第 nノードが持つ 3 つの子ノ一ドのそれぞれのノード相対床高さ偏差を実質的に推定する処理）が行われる。この処理は、図 5 3のフローチャートに示す如く実行される。なお、ここでは、第 i ノードが準備完了モードでなく、第 j ノ ―ドおよび第 kノードが準備完了モードであるとする。

まず、 S 6 6 1 2 0において第 i ノードのモードが判断される。この判断結果がホールドモードである場合には、 S 6 6 1 2 2において、新たな第 i ノード相対床高さ偏差補正量候補値 Zi_inc_cand'を 0に決定しリセットモードである場合には、 S 6 6 1 2 4において、図中の式により、新たな第 i ノード相対床高さ偏差補正量候補値 Zi_inc_cand'を決定する。 S 6 6 1 2 4で決定される Zi— inc— cand'は、 Zi_reし estm— p を徐々に 0に近づけるための第 i ノード相対床高さ偏差補正量候補値である。なお、 S 6 6 1 2 4の式中の ΔΤ、 Testm の意味は、前記式 3 5のものと同じである。補足すると、 S 6 6 1 2 4で Zi inc cand'を決定するとき、前記有限整定関数発生器（特開平 5 — 3 2 4 1 1 5号公報）を用いてもよい。

次いで、 S 6 6 1 2 6において、図中の式に従って、新たな第 j ノード相対床高さ偏差補正量候補値 Zj_inc— cand'および新たな第 kノ一ド相対床高さ偏差補正量候補値 Zk_inc__Cand'を決定する。

すなわち、 Wi * Zi— inc— cand' + Wj * Zj— inc— cand' + Wk * Zk— inc— cand' = 0 とレう条件 ( Zi— inc— cand' , Zj— inc— cand' , Zk_inc_cand'の重み付き平均値が 0 という条件）と、 Zj_inc— cand'— Zj— inc— cand = Zk— inc— cand' - Zk— inc— cand という条件を満足するように Zj— inc— cand' , Zk— inc— cand'を決定する。

次いで、 S 6 6 1 2 8において、上記の如く決定された Zi— inc_cand'_: Zj_inc_cand , Zk— inc_cand，をそれぞれ第 i ノード、第 j ノード、第 k ノードの相対床高さ偏差の前回制御周期での値 Zi一 reし estm— p， Zj— rel— estm— p , Zk_rel_estm_p に加えることで、新たな Zi— rel— estm , Zj_rel_estm, Zk— reし estmが決定される。

このように Zi— rel— estm， Zj_rel_estm , Zk— rel— estm を決定することで、 Wi * Zi— reし estm + Wj * Zi一 rel— estm + Wk + Zk— rel_estm = 0 を満足しつつ、 Zj_rel_estm— Zk_rel_estm ifi Zj— inc— cand— Zk—inc_cand に近づくように Zi— rel_estm , Zし rel— estm , Zk_rel_estm が決定されることとなる。

また、図 4 9の S 6 6 1 0の判断結果が N Oである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、 S 6 6 1 4において、前記 S 6 6 0 6と同じ処理（図 5 2を参照）が実行される。

また図 4 9の S 6 6 0 0で、 3個の子ノードのモードが「 1つの子ノ —ドのみホ一ルドで、残りがリセット」と判断すると、 S 6 6 1 6において、 1つの子モードのみホールドで、残りがリセットの場合に対応する処理が実行される。その処理で、図 5 4のフローチャート中に示す式によって、新たなノード相対床高さ偏差 Zし reし estm , Zj_reし estm， Zk— reし estmが決定される。なお、ここでは、第 i ノードのモードがホ —ルドモードで、第 j ノードおよび第 kノードのモードがリセットモ一ドであるとする。また、同式中のは、 Δ Τ、 Testm の意味は、前記式 3 5のものと同じである。

図 5 4の処理は、よりニ般的には、 Zし rel— estm を 0 に決定し、 Zj_rel_estm, Zk_rel_estm を、 Wi * Zi_rel_estm + Wj * Zj— rel— estm + + Wk * Zk_rel_estm = 0 ( Zi— rel— estm, Zj— rel— estm, Zk— rel— estmの重み付け平均値が 0であるという条件）を満足しつつ、 Zj_reし estm_p， Zk_rel_estm_pよりも 0に近い値に決定する処理である。補足すると、第 j ノードおよび第 kノードのモードが共にリセットモードになる時点までに、それらのノ一ドの重み W]'， Wk は 0となっているはずである従って、重み Wi， Wj , Wk の和が 1であるという条件から、この時点までに Wi が 1 となっており、また、 Wi * Zi_rel_estm + Wj * Zj_rel_estm + Wk*Zk— rel— estm = 0 であるから、この時点までに、 Zし reし estm は 0になっている。このため、第 i ノードの Zし reし estm の値をホールドするということは、その値を 0に維持するということである。そこで、図 5 4の処理では、 Zし rel_estm の値を 0に決定するようにしている。

なお、 Zj— rel_estm， Zk_rel_estm を徐々に 0にリセットする処理では、前記有限整定関数発生器（特開平 5 — 3 2 4 1 1 5号公報）を用いてもよい。

更に図 4 9の S 6 6 0 0で、子ノードのモードが「 2つの子ノードのみホールドで、残りがリセット」と判断すると、 S 6 6 1 8において、 2つの子モ一ドのみホールドで、残りがリセットの場合の処理が行われる。その処理では、図 5 5のフローチャート中の式に従って、新たなノ —ド相対床高さ偏差 Zi_reし estm， Zj_rel_estm , Zk_rel_estm が決定される。なお、ここでは、第 i ノードおよび第 j モードのモードが共にホールドモードで、第 kノードのモードがリセットモードであるとする < また、同式中の Δ Τ、 Testm の意味は、前記式 3 5のものと同じである, 図 5 5の処理は、より一般的には、 Zし reし estm , Zj_rel_estm をそれぞれ前回制御周期での値にホールドし、 Zk— reし estm を、 Wi * Zi_rel_estm + Wj * Zj_rel_estm + Wk * Zk— rel— estm = 0 ( Zi— rel— estm , Zj_rel_estm , Zk_reし estm の重み付け平均値が 0であるという条件）を満足しつつ、 Zk— reし estm— pよりも 0に近い値に決定する処理である, 補足すると、第 kモードがリセットモードになる時点までに、 Wk は 0 となっている。

なお、 Zk— rel— estm を徐々に 0にリセットする処理では、前記有限整定関数発生器（特開平 5— 3 2 4 1 1 5号公報）を用いてもよい。

更に図 4 9の S 6 6 2 0で「その他」と判断した場合（例えば 3個の子ノードのモードがすべてホールドモードである場合）には、 S 6 6 2 0において、前記 S 6 6 0 6と同じ処理（図 5 2を参照）が実行される _t 以上のように、第 nノードの 3個の子ノードのモードのうち、 2っ以上の子ノードのモードが準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その 2個以上の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、 2つ以上の子ノードのモードが準備完了である場合であっても，推定処理の発散が生じる恐れがある場合には、 3個の子ノードの推定ノ一ド相対床高さ偏差の値が、ホールドされる。

以上のごとく、図 4 1の S 6 0〜S 6 6では、推定第 n ノード相対床高さ偏差 Zn_reし estm(n= l，2，"'，最終ノード番号）が決定される。なお、根ノ一ドの相対床高さ偏差は 0である。

図 4 1のフローチャートの処理では、最後に、 S 7 2において、第 n ノード（η= 1，2, ···，最終葉ノード番号）のすベての先祖ノードの推定相対床高さ偏差と推定第 η ノード相対床高さ偏差との和を求め、求めた和を推定第 η接地部位床高さ偏差（推定第 η床高さ偏差） Zfn_estm とする _c 以上が、図 4 0における床高さ偏差の推定処理のサブルーチン処理である。

次に図 3 5の説明に戻って、上述のように床形状推定器 1 3 0で逐次求められた推定第 n 床高さ偏差 Zfn一 estm(n=l, 2，"'，最終葉ノード番号）は、加算器 1 3 2により、各接地部位 1 0の修正目標接地部位位置姿勢に加算され、これにより、床形状偏差補償入り修正目標接地部位位置姿勢が求められる。そして、この床形状偏差補償入り修正目標接地部位位置姿勢が修正目標接地部位位置姿勢の代わりに、前記機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hへ入力される。

以上の説明した床形状推定器 1 3 0および加算器 1 3 2を含む制御装置 5 0の処理を該制御装置 5 0の制御周期毎に繰り返し実行することにより、推定第 n床高さ偏差 Zfn一 estmが実際の第 n床高さ偏差に収束する。また、各目標接地部位位置姿勢は、その接地部位に対応する推定第 n床高さ偏差 Zfn一 estm に応じて修正され、実際の第 n床高さ偏差が実床反力に及ぼす影響を吸収する。

尚、すべての第 n接地部位（n=l, 2, :·· ,最終葉ノード番号）が接地しているならば、第 n接地部位の実際の接地面上における目標第 n接地部位床反力中心点 Q nに相当する点は、実第 n床接点 D n_act に一致するはずである。従って、前記瞬間第 n ノード相対床高さ偏差 Zn— rel'は、変動しないはずである。しかし、実際には口ポット 1の機械系や制御系の振動および電気的なノイズにより、前記床形状推定器 1 3 0の前記した計算アルゴリズムによって得られた瞬間第 n ノード相対床高さ偏差 Zn— rel'は、激しく変動する。従って、瞬間第 n ノード相対床高さ偏差をそのまま推定第 η ノード相対床高さ偏差として用いて、これを基に推定第 η床高さ偏差 Zfn_estm を求め（すなわち第 n ノード（n=l,2,…，最終葉ノード番号）のすベての先祖ノードの瞬間相対床高さ偏差と瞬間第 n ノード相対床高さ偏差との和をもって推定第 n 床高さ偏差 Zfn.estm とし）、さらに図 · 3 5に示すように、推定第 n 床高さ偏差 Zfn_estm を修正目標接地部位位置姿勢に加算したものを機構変形補償入り修正目標接地部位位置姿勢算出部 1 1 4 hへ入力させると、ロポット 1の接地部位 1 0は発振するかあるいは激しく振動する。

ところで、前記 2つの子ノードを持つノード用グループ内全推定処理， 3つの子ノードを持つノ一ド用グループ内全推定処理、 3つの子ノードを持つノード用グループ内部分推定処理では、瞬間第 n ノード相対床高さ偏差 Zn— rel'力、ら推定第 n ノ一ド相対床高さ偏差 Zn— reし estm までの伝達関数は、 1次遅れのローパスフィル夕になる。即ち、推定第 n ノード相対床高さ偏差 Zn_reし estm は瞬間第 n ノード相対床高さ偏差 Zn_rel'に口一パスフィルタを通したものとなる。従って、推定第 n ノード相対床高さ偏差 Zn— rel一 estm を基に求めた推定第 n 床高さ偏差 Zfn_estm を図 3 5に示すように修正目標接地部位位置姿勢に加算しても、各接地部位 1 0の発振や振動を生じ難くなる。これにより、各接地部位 1 0の発振や振動を防止することができる。

ちなみに、本実施形態では、上記口一パスフィル夕の時定数は

Testm である。なお、本実施形態で説明した床形状推定器 1 3 0の処理以外にも、瞬間第 n ノード相対床高さ偏差 Zn— rel'から推定第 n ノード相対床高さ偏差 Zn rel estm までの伝達関数がローパスフィルタになるように構成することは可能である。次いで、この実施の形態における床形状推定器 1 3 0の特徴を説明する。

ロボット 1の姿勢安定化制御のために必要な各ノード補償モーメントを発生させようと、各ノード補償角を変化させ、最終的な目標接地部位位置姿勢である機構変形補償入り修正目標接地部位位置姿勢を変更させても、各接地部位 1 0が実際に接地している限り、第 n接地部位 1 0の実際の接地面上における目標第 n接地部位床反力中心点 Q nに相当する点は、実第 n床接点 D nact に一致したまま動かない。その代わりに、各脚のコンプライアンス機構 4 2などが変形したり、上体 2 4の位置姿勢が変化する。即ち、機構変形補償入り修正目標接地部位位置姿勢を変更させた分を打ち消すように、コンプライアンス機構 4 2などが変形したり、上体 2 4の位置姿勢が変化する。

本実施形態における床形状推定器 1 3 0は、機構変形補償入り修正目標接地部位位置姿勢から推定第 n 床高さ偏差 Zfn— estm(n= l,2, ，最終葉ノード番号）を算出するまでに、実床反力を基に機構コンプライアンスモデル 1 3 4によって算出されたコンプライアンス機構 4 2などの変形量と上体姿勢傾斜偏差 0 berr による推定第 n床高さ偏差 Zfn— estm への影響を打ち消しているので、上体姿勢傾斜偏差が生じたり、機構変形補償入り修正目標接地部位位置姿勢が変動しても、推定第 n 床高さ偏差 Zfn_estmはその影響を受けない。'

これは、床形状の推定処理が、ロボット 1のコンプライアンス制御や姿勢制御から干渉を受けないことを意味する。従って、推定第 n 床高さ偏差 Zfn__{e S}tm を図 3 5に示すように修正目標接地部位位置姿勢に加算しても、ロボット 1の制御系の安定余裕（発振のし難さ）はほとんど低下しない。すなわち、コンプライアンス制御、姿勢制御、および床形状の推定処理と、床形状偏差の推定値を用いたロポット 1の目標歩容の補正動作とを同時に行なっても、各制御 · 処理が干渉し合って発振することはほとんどない。すなわち、簡単に言えば、床形状偏差の複数のパラメ一夕を同時に推定しながら、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。さらには、姿勢制御のための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。

また、各接地部位 1 0の推定第 n床高さ偏差 Zfn_estmを図 3 5に示すように修正目標接地部位位置姿勢に加算すると、実際に第 n 床高さ偏差が存在していても、その影響を打ち消すことができるので、実全床反力は、定常的には口ポット 1が想定床を移動している場合と同一になる。しかも、推定第 n床高さ偏差 Zfn— estmの推定と、推定第 n床高さ偏差 Zfn— estm を用いた修正目標接地部位位置姿勢の修正とは制御周期毎に逐次実行されるので、床形状が途中で変化しても、変化後の第 n 床高さ偏差を推定し、その推定値に基づいて床形状の変化の影響を打ち消すことができる。ただし、床形状推定器 1 3 0には前述したように口一パスフィルタが含まれるので、各接地部位 1 0が接地した直後や床形状が途中で変化したときには、過渡的には、実全床反力は、床形状偏差の影響を受ける。しかし、その後、時定数 Testm でその影響が減衰する。

また、以下の特徴もある。 '

1. ) 床形状偏差が大きくても制御が破綻し難い。これは、階層型コンプライアンス制御は微小な補償角に限定した近似を行っていないからである。

2 ) 床形状偏差の推定精度が高い。 3 ) ロボッ卜の姿勢安定化制御との干渉による発振が生じにくいので、推定処理の時定数を短くすることができる。それによつて、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作の応答性を高めることができるので、床形状偏差が床反力に及ぼす過渡的な影響が短時間で解消される。

ロボッ卜の移動時の 1歩前の着地期（ある接地部位 1 0が接地状態となる時期）での推定第 n 床高さ偏差と、当該 1歩前の着地期の次の着地期での推定第 n 床高さ偏差との関係に相関がない場合には、本実施形態に示したように推定第 n 床高さ偏差を強制的に徐々に 0に収束させるのが良い。しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることが分かっているならば、前記関係にある程度の相関があると考えられる。この場合には、現在の着地期における推定第 n 床高さ偏差を、強制的に徐々に 1歩前の着地期における推定第 n 床高さ偏差に 1より小さい正の定数を乗じた値に収束させるようにしても良い。

さらに、 1歩前の着地期での推定第 n 床高さ偏差だけでなく、複数歩前の着地期での推定第 n 床高さ偏差も用いて現在の着地期における推定第 n床高さ偏差の収束目標値を決定しても良い。また、現在や 1歩前や複数歩前の着地期における他のノードの推定第 n ノード床高さ偏差および推定第 n ノード床傾斜偏差も用いて収束目標値を決定しても良い。本実施形態（第 3実施形態）は上記の如く構成したので、従来技術では困難であった多くの接地部位を持つロポッ卜においても、床形状の推定、具体的には、各接地部位に対応する床高さ偏差を同時に、換言すれば複合的に、精度良く推定することができる。さらに、口ポットの接地部位をその姿勢を制御可能に設け、床形状の推定処理において床高さ偏差だけでなく、床傾斜偏差も推定する場合には、床傾斜偏差も含めて同時に、精度良く推定することができる。

さらには、床形状の推定結果を基に目標接地部位位置姿勢軌道を修正することにより、床形状が想定していた形状と異なっていても、その影響を吸収して望み通りの床反力を発生させることができる。

' 特に、床形状偏差の推定値による補正がない階層型コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づけることができる。換言すれば床形状の偏差に起因する床反力の定常偏差を解消することができる。

また、床形状の推定精度が低下する恐れがある状況あるいはその推定値が発散する恐れがある状況では床形状の推定を中断するように構成したので、前記した作用効果に加え、推定値が不適切なものとなることがない。

尚、本実施形態に係る口ポットの床形状推定装置 1 3 0は前記した手法で床形状を推定すれば足り、推定値に基づいて運動を補正することは必須ではない。

[第 4実施形態] '

次に、本発明の第 4実施形態に係る移動口ポットの制御装置（特ぃ床形状推定装置）を説明する。第 4実施形態においては、それを簡単に説明すれば、前記第 3実施形態の図 4 0に示した床形状推定器 1 3 0のブロック線図を等価変換したものである。すなわち、直前に推定した床形状の推定値（前回制御周期での推定値などの該推定値の過去値）と、最終的に修正され決定された目標歩容（詳しくはコンプライアンス動作を含む機構変形補償入り修正目標接地部位位置姿勢）（もしくは実関節変位）と、実上体姿勢傾斜偏差 6) berr とを基に、各ノードの床反力を推定し、その推定値と各ノードの実床反力との差を基に、この差を 0に近づけるように床形状の推定値の補正量候補値を決定し、床形状の推定が発散しないと判断されるならば、床形状の推定値の補正量候補値を基に. 5 直前に推定した床形状の推定値を補正するようにした。この処理は、本出願人が先に提案した特開平 1 0 — 2 7 7 9 6 9号公報において、 2足移動ロポットの各脚の足平の床反力中心点まわりの想定されるモーメン卜と実モーメントとの差が 0に近づくように各足平に係る床傾斜を推定する手段、ならびに、全床反力中心点まわりの想定されるモーメントと0 実モーメントとの差が 0に近づくように両脚間の千渉角を推定する手段

、 ' を発展させた階層型の床形状推定手段に相当するものである。

このように本実施形態は、床形状推定器 1 3 0の処理のみが、第 3実施形態と相違するものであり、床形状推定器 1 3 0以外の処理については説明を省略する。

5 図 5 7は、本実施形態における床形状推定器 1 3 0の機能的手段を示すブロック図である。この図 5 7を参照して本実施形態における床形状推定器 1 3 0を以下に説明する。

先ず、各接地部位 1 0の機構変形補償入り修正目標第 n接地部位位置姿勢（_η=1,2，···，最終葉ノード番号）と、各接地部位 1 0の目標接地部位位0 置から見た該接地部位 1 0の目標第 η接地部位床反力中心点と、前記姿勢傾斜偏差 0 berr とを基に、機構変形補償入り修正目標第 n接地部位位置姿勢を、目標全床反力中心点を回転中心として 0 berr だけ回転移動させてなる、回転後機構変形補償入り修正目標第 n接地部位位置姿勢 (η= 1 ,2, ···，最終葉ノード番号）を求める。求められた回転後機構変形補償5 入り修正目標第 η接地部位位置姿勢 (η=1, 2, ··· ,最終葉ノード番号）から、目標歩容上での想定第 η床接地位置 1,2, ··· ,最終葉ノード番号）を減算し、第 n接地部位干渉高さ Zn— int を求める。それを床形状推定器 1 3 0に備えた機構コンプライアンスモデル（逆モデル）へ入力することによって、各接地部位 1 0の床反力の推定値である推定第 n接地部位床反力を求める。

なお、ここでの機構コンプライアンスモデルは、各接地部位干渉高さの互いの相対関係に応じて推定接地部位床反力の互いの相対関係を求めるものであり、絶対値に意味をもたない。また、第 n接地部位の実床反力 Fn_act の検出値がある閾値 Fn— min3 より小さい場合には、上記機構コンプライアンスモデルにおいても第 n接地部位が接地していないものと想定する。

さらに、第 n接地部位（n=l, 2, ···，最終葉ノード番号）の実床反力 Fnact の検出値から、前記推定第 n接地部位床反力を減算し、第 n接地部位床反力推定誤差 Ffn_estm— err を求める。この第 n接地部位床反力推定誤差 Ffn— estm_err は、力で表現されているため、換算値 Cn (例えば、パネ定数の逆数のようなもの）によって高さ偏差に換算し、それを第 n 接地部位床高さ偏差補正量候補値 Zfn— inc— cand として得る。なお、換算値 Cnは、対角行列とは限らない。

次いで、求められた第 n 接地部位床高さ偏差補正量候補値 Zfn_inc_cand から、前記した「階層相対化処理」によって、階層相対化することで、第 n ノード相対床高さ偏差補正量候補値 Zn_inc一 cand(n=l，2，"'，最終ノード番号）を求める。次に、床高さ偏差の推定処理のサブルーチン処理によって、' 第 nノード相対床高さ偏差補正量候補値 Zn_i_nC_cand から、推定床高さ偏差（推定第 n床高さ偏差） Zfn_estm(n= l，2,…，最終葉ノ一ド番号）を求める。ここでの床高さ偏差の推定処理のサブルーチン処理は、図 4 1の S 5 6〜 S 7 2までの処理と同一である。なお、特開平 1 0— 2 7 7 9 6 9号公報に示したように、接地部位として姿勢を制御可能な足平を持ち、接地部位における床傾斜偏差を推定することができるロポッ卜の場合には、図 4 1の S 7 0に関して説明した如く、接地部位床傾斜偏差推定処理を行なうのが望ましい。

ここで、第 3実施形態と第 4実施形態（本実施形態）とを比較する。第 4実施形態は、簡単に言えば、第 3実施形態の床形状推定器 1 3 0のブロック線図を等価変換したものになっている。したがって、第 4実施形態の作用効果は、第 3実施形態の作用効果と同じである。また、第 3 実施形態と同様、床形状の推定値に基づいて運動を補正することは必須ではない。

尚、第 3および第 4実施形態において、床反力センサとして、分布圧センサを用いても良い。 6軸力センサ 3 4などの力センサよりも分布圧センサの方が実床反力による接地部位の変形をより細緻に推定することができるので、床形状推定の精度を向上させることができる。

さらには、本出願人による特許第 3 0 3 5 0 5 1号公報の図 1に示す如く、接地部位に接触センサを複数個配置（例えば接地部位の四隅に配置）し、その出力信号に基づいて接地部位がどちらの方向に浮きやすいかを判定しても良い。そして、接地していない接触センサが増々浮く方向での床形状の推定を中断したり、あるいは、全ての接触センサが浮いているときにも床形状の推定を中断するようにしてもよい。

[第 5実施形態] '

前記第 1〜第 4実施形態では、子ノードを持つノードの、該子ノードの総数が、 3個以下の場合を例にとって説明したが、子ノードの数が 4 つ以上の場合にも、重み付き平均の概念を持ち込むことによって、以下に示すようにモーメントおよび傾斜角の概念を拡張できる。目標値、実際値、偏差のいずれに関しても概念を拡張できるので、ここでは、目標値、実際値、偏差の区別をせず、一般的な接地部位床反力の組 Fn(n= l,2, ...)に対するモーメントを、以下のように拡張して、定義する。第 n接地部位の床反力の組 Fn(n= l,2, ...，最終葉ノード番号）を階層相対化して、第 nノード相対床反力 Fn_rel(n=l，2，...，最終ノード番号）を求める。 '

第 nノードの子ノードの数を rとする。

第 nノ一ドの第 j番目の子ノードの識別番号が aj (j = l，2，一，r)であるとする。

第 nノードの第 j番目の子ノードの重みが、 Wj (j=l，2, ··· ,!·)であるとする。

第 nノードの第 j番目の子ノードの重みを第 j 要素として持つ列べクトルを UWnとする。

すなわち、下式 3 8のように UWn を定義する。ここで「T」は、転置を意味し、行べクトルの転置によって列べクトルを表現することとする。

UWn = (Wal,Wa2, - ,War)T . …式 3 8 ベクトル UWn に直交する（すなわちベクトル UWn との内積が 0となる） r _ l.個の互いに独立なべクトルを R(1),R(2),…，！ l(r— 1)とする。 R(j) (； ί = 1, 2，···，r— 1)は、 r 行 1 列の列べ'クトルである。なお、 R(j) (j = l，2,…，！ r— 1)は、演算の容易さと制御精度を考慮すると、互いに直交することが望ましい。 R(j)を第 j 列 (j = l,2，…，！ >ー 1)とする行列を Hn とする。 Hnは、 r行 r— 1列の行列である。

第 nノードの第 j 番目の子ノードの相対床反力 Faj— rel を第 j 要素 (j= l，2,… ，r)とする列べクトルを第 n グループ相対床反力べクトル Fn_rel_cとする。

すなわち、次式 3 9により、 Fn_reし cを定義する。

Fn_rel_c = (Fal_rel, Fa2— rel,…，Far— rel)^T …式 3 9

Fn— reし cは、 r行 1列の列ベクトルである。

次式 4 0を満足する列べクトル Mn— exp を第 nノード拡張床反カモ一メントと呼ぶ。

Fn— rel— c = Hn * Mn— exp …式 4 0

Mn— expは、 r— 1行 1列の列ベクトルである。

以上のように、接地部位床反力の組 Fn(n=l ,2, '" )に対し、第 nノード拡張床反力モーメント Mn_expを定義する。

すなわち、接地部位床反力 Fn(n= l，2，"'）を基に、階層化したノード相対床反力 Fn_rel(n=l，2, ...)を求め、さらに第 nノードのすべての子ノ —ド aj (j=a l,a2,— , ar)の相対床反力を要素とするベクトル（Fal— rel, ?&2_ 61 '，？& 1)7 を、第 nノードのすべての子ノードの重みを要素とするべクトル（Wal,Wa2,…，War)T に直交するある所定の互いに独立なベクトル R(j) (j=l , 2,…，！ "— 1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第 nノ一ド拡張床反力モ一メント Mn一 exp と呼ぶ。

なお、第 nノード拡張床反力モーメント Mn_exp の第 j 番目要素 (第 j 行目要素）を第 nノード拡張床反力モーメント第 j成分と呼ぶ。また、実接地部位床反力の組 Fn(n=l，2，"'）に対する第 nノード拡張床反力モーメントを実第 nノード拡張床反力モーメント Mn__exp_act と呼ぶ。

また、目標接地部位床反力の組 Fn(n=l，2，"'）に対する第 nノード拡張床反力モーメントを目標第 n ノード拡張床反力モーメント Mn— exp_ref と呼ぶ。目標第 nノード拡張床反力モーメントは、通常 0 べクトルに設定される。

また、接地部位床反力偏差の組？11(11=1，2, )に対する第 11 ノード拡張床反力モーメントを第 n ノ一ド拡張床反力モ一メント偏差 Mn_exp_errと呼ふ。

同様に、一般的な接地部位高さの組 Zn(n=l,2，...）に対する傾斜角を、以下のように拡張して、定義する。

接地部位高さの組 Ζη(η=1，2，···，最終葉ノード番号）を階層相対化して，第 ηノード相対高さ Zn— rel(n=l，2,"'，最終ノ一ド番号）を求める。

第 n ノードの第 j 番目の子ノードの相対高さ Zaj— rel を第 j 要素 (j=l, 2, ···，!·）とする列べクトルを第 nノード相対高さべクトル Zn— rel— c とする。

すなわち、次式 4 1 により Zn_rel— cを定義する。

Zn— rel— c = (Zal_rel, Za2_rel,— ,Zar_rel)^T …式 4 1 次式 4 2を満足する列べクトル 0 n_exp を第 nノード拡張床反力傾斜角と呼ぶ。 '

Zn— rel— c = Hn* Θ n_exp …式 4 2 θ n expは、 r— 1行 1列の列べクトルである。すなわち、接地部位高さ Ζη(η= 1,2, ··· )を基に、階層化したノード相対高さ Zn_rel(n= l, 2，"'）を求め、さらに第 nノードのすべての子ノード aj (j = l,2,…，r)の相対高さを要素とするべクトル（Za l_rel, Za2— rel，〜，Zar_rel)^T を、第 nノードのすべての子ノードの重みを要素とするベクトル（Wal，Wa2, '"，War)^T に直交する前記所定の互いに独立なべクトル R(j) (j= l,2，…，！ ·— 1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第 nノード拡張傾斜角と呼ぶ。

上記のように定義された拡張床反カモ一メン卜と拡張傾斜角とを用いることにより、第 a ノードの子ノードの数が 4つ以上の場合にも、コンプライアンス制御および床形状推定の処理は、ほとんどそのまま拡張される。以下にその例を第 5の実施形態として説明する。なお、第 5実施形態は、口ポットの制御装置の機能的構成は、前記図 2に示したものと同様とする。また、この場合、階層型コンプライアンス動作決定部は，補償全床反力モーメント分配器、補償角決定部、および床形状推定器以外の機能的構成は、前記図 3 5を参照して説明したものと同様でよい。

[補償全床反力モーメント分配器の処理の拡張について]

例えば、階層型コンプライアンス動作決定部の中の補償全床反力モーメント分配器の処理は、次のように拡張される。

口ポットの姿勢を復元させるために目標第 nノード拡張床反力モーメント Mn_exp_rel (通常 0 ) に付加的に発生すべき拡張床反カモ一メントを第 nノード補償拡張床反力モーメント Mn— exp— dmdと呼ぶ。

第 nノード補償拡張床反力モーメント Mn— exp_dmd と第 nノード補償床反力モーメント Mn— dmd の間には、次式 4 3の関係がある。ただし、 Cn_mech は、各目標ノード床反力中心点と口ポットの機構のコンプライアンス特性とによって決定される所定の係数行列（ 2行 _r— 1 列の行列）である。

Mn_dmd= Cn_mech * Mn_exp_dmd …式 4 3 一方、前記式 1 0を一般化すると次式 4 4になる。

Mdmd=∑Mn_dmd …式 4 4 なお、 ∑は、全ての第 nノードについての総和を意味する。

よって、補償全床反力モーメント分配器では、式 4 3と式 4 4を満足するように、第 n ノード補償拡張床反力モーメント Mn__eXp_dmd ( n=l，2，一）および第 nノード補償床反力モーメント Mn— dmd を決定すれば良い。

[補償角決定部（ 0 n決定部）の処理の拡張について]

第 5実施形態における各第 nノードの補償角決定部の処理は、例えば第 3実施形態の 0 145決定部（図 2 5および図 3 3を参照）の処理において、 M145act を求めるまでの処理を、各接地部位の実床反力を基に前記の実第 nノ一ド拡張床反力モ一メント Mn_exp_actを求める処理に置き換え、 145 を nに置き換え、図 3 3中の補償フィルタ、口一パスフィルタおよびゲイン Knの次元を第 ηノードの子ノード数から 1を減じた数に拡張すれば良い。上記の置き換えを行なった処理によって、第 η ノードの補償角 0 ηを求めることができる。

[拡張床反力モーメントを用いた床形状推定ついて]

以下のように、床形状推定器の処理の一部に拡張床反力モーメン卜の概念を用いた手法を組み込んでも良い。

すなわち、前記第 4実施形態で説明した床形状推定器の図 5 6に示した処理の代わりに、図 5 7のブロック図で示す処理を用いて、床形状偏差を推定する。

図 5 7を参照して、この処理を説明すると、まず、前記図 5 6で説明した如く算出される推定第 n接地部位床反力と、第 n接地部位の実床反との差（各ノード毎の差の組）から、拡張モーメントの定義にしたがつて、第 n接地部位床反力推定誤差 Ffn_estm— err (n= l,2，...）に対応する第 nノ一ド拡張床反力モーメント推定誤差 Mn_e_Xp— estm_err を求める, なお、第 nノード拡張床反力モーメント推定誤差を決定する上で必要となる前記べクトル R(j)は、各ノ一ドの重みを基に決定される。

次いで、次式 4 5のように、所定の係数マトリックス Kn_cmpl を第 nノード拡張床反力モーメント推定誤差 Mn— exp_estm— err に乗算することにより、第 nノード拡張傾斜角の補正量の候補値である第 nノード拡張傾斜角補正量候補値 n inc candを求める。

Θ n_inc_cand = Kn_cmpi * Mn_exp_estm_err 式 4 5 なお、 Kn— cmplは、対角行列'とは限らない。

次いで、ベクトル R(j) (j = l, 2，…！ ·— 1)に対する係数を 0 n_inc_cand の第】要素として、ベクトル R(]')を線形結合したベクトルを求め、これをもって、第 nノード相対床高さ偏差の補正量の候補値である第 nノード相対床高さ偏差補正量候補値 Zn— inc— candとする。

すなわち、次式 4 6によって、 Zn— inc— candを求める。

Zn me ca.no. = rln * 0 n inc cana … 4 6 以上のごとく、第 nノード床反力推定誤差 Fn— estm_err から第 nノ —ド相対床高さ偏差補正量候補値 Zn_inc— cand を求めるまでの処理を図 5 6のものから図 5 7のものに置き換えればよい。

上記以外の図 5 7の演算処理は、図 5 6の演算処理と同様である。また、第 5実施形態における床高さ偏差の推定処理のサブルーチン処理では、第 nノードの子ノード数が 4以上の場合の処理が必要となる。この処理は、前記 3子ノード用床形状推定処理（図 4 1を参照）を拡張し、該 3子ノード用床形状推定処理と同様に、いずれかの子ノード j に対応する接地部位が浮いて発散してしまいそうな場合には、推定第； j ノ一ド相対床高さ偏差 Zj— reし estm を前回値 Zj— reし estm一 p に維持する (第 j ノード相対床高さ偏差補正量候補値 Zj_inc— cand を 0とする）。また、それ以外の子ノードの推定ノード相対床高さ偏差 Zk_reし estm を前回推定第 k ノード相対床高さ偏差 Zk— rel— estm— p と Zn— inc_cand との和にある所定のバイアス値 cを加えた値に近づくように決定する。ただし、バイアス値 cは、第 nノードのすべての子ノードの推定ノード相対床高さ偏差の重み付き平均が 0になるように決定される。

以上のごとく、床形状の推定処理を、 4個以上の子ノードを持つノードがある場合にも、拡張することができる。ただし、このような処理を行なうよりは、 4個以上の子ノードを持たないように、階層構造を決定した方が、演算処理は容易となる。

[第 6実施形態]

次に、本発明の第 6実施形態に係る移動口ポットの制御装置を図 5 8 〜図 6 5を参照して説明する。図 5 8および図 5 9は、第 6実施形態に係る脚式移動口ポット 5 1 (本実施形態では 2足移動ロポットとする）が後述する膝関節 5 6の部位を床に付けた状況（ひざまずいた状況）を側面示で示している。なお、図 5 8ではロボット 5 1を線状のリンクの連結体として模式化して示している。

本実施形態に係わる発明目的は、膝、肘、胴体など、ロボットの脚や腕の先端部以外の部分が床あるいは床に固定（設置）された対象物（床の延長物と考えられる対象物）と接触する状態において、該対象物を含む床から受ける反力（外力）を操作して、口ポットの姿勢を安定に制御することにある。

より具体的には、ロボッ卜がひざまずいた状態やいすに座った状態における姿勢制御を可能とすることにある。

従来の公知の人間型口ポットには、例えば膝には、床反力を検出するセンサが備えられておらず、膝に作用する床反力を制御することはできなかった。例えば従来の人間型ロポットでは、図 5 8および図 5 9のようにロボットをひざまずかせた状態では、左右の膝に作用する床反力は床の凹凸に依存（支配）され、また、床の凹凸に応じて、脚の関節角を調節することが行われていなかった。このため、床の凹凸によって、口ボットの床との接地部位が揺れるなど、その接地部位の接地状態が不安定なものとなる場合もあった。そして、このような状態では、口ポットの動作や作業を行おうとするとロボッ卜の姿勢が不安定にある場合があつた。

また、例えば口ポットがひざまずいた状態から立ち上がる場合に、実際の床の傾きが目標歩容で想定していた傾きと異なっていた場合には、口.ポッ卜の重心位置が適切に制御されず、ロポッ卜の上体の姿勢が傾いたまま立ち上がろうとしてしまうために、該ロボットが転倒してしまう恐れもあった。

また、口ポットが椅子に座ったまま、その姿勢を安定に保ちつつ動作や作業を行う場合において、椅子から立ち上がる時、あるいは椅子に座る時に、口ポットの姿勢を適切な姿勢に復元できるようにするために、口ポットの各脚の足平に作用する床反力のみならず、椅子から口ポットの臀部に作用する反力も適切に制御できることが望ましい。

なお、概念的には、椅子などのように床に固定設置された対象物も床の一部と考えれば、前記反力も広義の床反力であると言える。

第 6実施形態は、以上のごとき従来方式の問題点に鑑み、前記問題点を解消し、移動口ポットの膝、肘、胴体、臀部など、脚や腕の先端部以外の部分が、床あるいは床の延長物と考えられる対象物と接触して反力を受ける状態において、前記した階層型コンプライアンスの制御を使用しつつ、口ポットの脚や腕の先端部に作用する反力のみならず、脚や腕の先端部以外の部分に作用する反力をも適切に制御し、移動ロポッ卜の姿勢を安定に保つことを可能としたものである。

以下に、図 5 8および図 5 9に示す口ポット（ 2足移動ロボット） 5 1を例にとって、第 6実施形態をより具体的に説明する。

まず、口ポット 5 1 の機構的構成を説明すると、この口ポット 5 1では、上体（基体） 5 3の下端部から 2つの脚（リンク機構） 5 2 (—方の脚は図示を省略している）が延設されている。また、上体 5 3の上部から 2つの腕（リンク機構） 5 4 (—方の腕は図示を省略している）が延設されている。各脚 5 2は、上体 5 3側の基端部（上体 5 3との連結箇所）と、中間部と、先端部とにそれぞれ電動モー夕などのァクチユエ一夕により動作する股関節 5 5、膝関節 5 6、足首関節 5 7を備え、足首関節 5 7を介して各脚 5 2の先端部に足平 5 8が連結されている。同様に、各腕 5 4は、上体 5 3側の基端部（上体 5 3との連結箇所）と、中間部と、先端部とにそれぞれ電動モー夕などのァクチユエ一夕により動作する肩関節 5 9、肘関節 6 0、手首関節 6 1 を備え、手首関節 6 1 を介して手 6 2が各腕 5 4の先端部に連結されている。この例では、各脚 2の各関節 5 5 、 5 6 、 5 7はそれぞれ、例えば 3自由度、 1 自由度 2自由度をもつ関節となっており、足平 5 8は上体 5 3に対して 6 自由度を有する。また、各腕 5 4の肩関節 5 9、肘関節 6 0、手首関節 6 1 はそれぞれの自由度が、上体 5 3に対して手 6 2が 6自由度以上の自由度を持つように構成されている。

なお、ロボット 5 1の上体 5 3には、図示を省略するが、前記第 1実施形態などと同様の制御装置 5 0が搭載されている。また、上体 5 3の上端には、頭部 6 3も設けられている。

このロボット 1では、図 5 8および図 5 9のように口ポット 1をひざまずかせた状態（特定姿勢状態）では、各脚 5 2の足平 5 8と各膝関節 5 6の部位（より詳しくは、膝関節 5 6の近傍で膝関節 5 6と足首関節 5 7 とを連結するリンク（脛リンク）の表面部。以下、単に膝という）と、各腕 5 4の手 6 2とが接地部位となる。そして、本実施形態では、図 5 9に示すように、接地部位である膝に床反力センサ 9 0 (荷重センサ）を備えている。この床反力センサ 9 0の本体（センサ部） 9 2とスポンジ等の柔軟な部材（弾性体） 9 4とから構成され、本体 9 2を膝 (脚のリンク）に固定し、その本体 9 2の外側を柔軟な部材（弾性体） 9 4によって覆っている。なお；膝におけるコンプライアンス制御の精度を良くするために、膝を柔軟な部材 9 4によって覆うだけでなく、その柔軟な部材 9 4の表面（接地面）を丸く凸面状に形成することが望ましい。こうすることにより、口ポット 5 1の目標運動の修正動作と床反力との関係の非線形性が小さくなるので、コンプラインス制御の制御特性が良くなる。なお、図示は省略するが、足平 5 8と足首関節 5 7 とは 6軸力センサなどの床反力センサとコンプラインス機構とを介して連結されている。同様に手 6 2と手首関節 6 1 とは図示を省略する 6軸カセンサなどの床反力センサとコンプラインス機構とを介して連結されている。それらの連結構成は、公知のものでよい。

膝関節の接地部位の床反力センサ 9 0としては、以下のいずれでも良い。

1 ) 接地部位の床への接触面（接地面）に垂直な方向の並進力だけを検出するセンサ

2 ) 接地部位の床への接触面（接地面）に垂直な方向の並進力だけでなく該接触面に垂直な方向以外の方向の並進力も検出するセンサ

3 ) 分布圧センサ

4 ) 並進力以外にモーメントも検出するセンサ（例えば 6軸力センサ）また、荷重を直接的に検出するものでなくとも、前記柔軟な部材 9 4 の変形（ひずみ）を検出する変位センサを用いても良い。

本実施形態においては、床反力センサ 9 0として、上記 2 ) のセンサを用いることとする。コンプライアンス制御の精度をより一層高めるためは、床反力センサ 9 0として、上記 3 ) または 4 ) のセンサを用いれば良い。なお、膝の構造としては、本出願人が先に提案した特開 2 0 0 0 - 6 2 7 6 0号公報に示すごとく、ばねを備えた保護パッド（膝の保護のためのパッド）に床反力センサを追加したものにしても良い。

本実施形態では、上記のようにひざまずいたロボット 5 1に対して、図 6 1に示す如く階層構造を設定している。すなわち、接地部位としての右足平 5 8、左足平 5 8、右膝、左膝、右手 6 2、左手 6 2をそれぞれ.葉ノードである第 1 ノード、第 2ノード、第 3ノード、第 4ノード、第 5ノード、第 6ノードに対応させる。また、第 1 ノードと第 2ノードとを子ノードとして持つ第 1 2ノード、第 3ノードと第 4ノードを子ノードとして持つ第 3 4ノード、第 5ノードと第 6 ノードを子ノードとして持つ第 5 6ノード、並びに第 1 2ノードと第 3 4ノードを子ノードとして持つ第 1 2 3 4ノードを中間ノードとして設定した。さらに、第 1 2 3 4 ノードと第 5 6ノードを子ノードとして持つ第 1 2 3 4 5 6ノードを根ノードとして設定した。また、各ノードには、前記第 1実施形態などと同様の方針によって、図示の如く、目標床反力中心点 Qn(n=l，2, 3, 4，5，6，12，34，56, 1234, 123456)が対応づけられる。なお、根ノードである第 123456 ノードの目標床反力中心点 Q 123456 は、目標全床反力中心点 Pと同一である。

本実施形態では、制御装置 5 0は、前記図 2に示したものと同様の機能的構成を有するものとされている。

この場合、本実施形態における歩容生成装置 1 0 0が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各手 6 2の目標位置姿勢軌道と各足平 5 8の目標位置姿勢軌道と各膝の目標位置軌道とから構成される。この場合、歩容生成装置 1 0 0は、想定した床面上で足平 5 8と手 6 2 と膝が歩容に対する要求通りに接地するように、目標足平位置姿勢（目標第 1、 2接地部位位置姿勢）と目標手位置姿勢（目標第 5、 6 接地部位位置姿勢）と目標膝位置（目標第 3、 4接地部位位置）を優先的に決定し、次いで目標 Z M P (目標全床反力中心点）を各接地部位の目標接地点（あるいは目標接地線もしくは目標接地面）を含む最小凸多角形である支持多角形内に決定し、次いで、目標上体位置姿勢を、ロボット 1の動力学モデルを使用して、目標足平位置姿勢と目標手位置姿勢と目標膝位置と目標 Z M Pとを満足するように決定する。

なお、頭部 6 3が上体 5 3に対して可動である場合には、その頭部 6 3の目標位置姿勢軌道が目標運動に含まれる。

また、図 2の口ポット幾何学モデル（逆キネマテイクス演算部） 1 1 0の演算処理は、本実施形態では、後述するように、第 1〜第 3実施形態と異なる。

また、本実施形態では、階層型コンプライアンス動作決定部 1 1 4はその一部の処理手法が前記第 1〜第 3実施形態で説明したものと相違する。図 6 2は、本実施形態の階層型コンプラインス動作決定部 1 1 4の機能的構成を示している。この機能的構成の処理において、第 1〜第 3 実施形態と相違する主な点を説明すると、補償全床反力モーメント分配器 1 1 4 aは、各中間ノードおよび根ノードのノード補償床反力モ一メントだけでなく、葉ノ一ドのぅちの第11ノード（11= 1，2, 5，6) のノード補償床反力モーメント Mn_dmd、すなわち、目標第 n接地部位床反力中心点（n= l，2，5, 6) まわりの要求床反力モーメント（姿勢復元用の要求モーメント）も決定して出力する。これは、ロボット 5 1では、接地部位としての足平 5 8および手 6 2にその目標床反力中心点まわりにモ一メント発生できるからである。なお、これらの要求モーメントを含めたノード補償床反力モーメント Mn_dmd の決定手法は、前記第 1および第 2実施形態で説明した手法と同じでよい。また、本実施形態では、補償全床反カモ一メント分配器 1 1 4 aは、ノード補償床反力モーメントをそれぞれに対応する各ノードの目標床反力中心点まわりに発生させることによって修正される、目標第 n接地部位床反力中心点（n= l , 2, 5, 6)まわりの修正目標第 n接地部位床反力モーメント Mn— refmdfd(n= l，2，5，6) を、修正目標接地部位床反力の並進力成分 Fn_refmdfd(n= l，2, 3, 4，5，6) と共に決定して出力する。これらは、機構変形補償量算出部 1 1 4 nで機構変形補償量を算出するために使用される。

また、補償角決定部（図 6 2の仮想線で囲んだ部分）は、本実施形態では、各中間ノードおよび根ノードの補償角だけでなく、葉ノードのうちの第 nノード（_n= l , 2, 5, 6) のノード補償角 0 n も決定して出力する。これらの第 nノード補償角 0 _n(n= l , 2，5, 6) を決定する θ n 決定部は、特開平 1 0 — 2 7 7 9 6 9号公報に示した足平補償角決定部と同様の処理によって、第 nノード補償角 0 n(n=l,2，5，6) を決定する。なお、これらのノード以外の各中間ノードおよび根ノードの補償角 0 n(n=12, 34,56, 1234, 123456)の決定手法は、前記第 1および第 2実施形態で説明した手法で同じでよい。

また、本実施形態における修正目標接地部位位置姿勢算出部 1 1 4 g は、前記第 1および第 2実施形態で説明した手法によって、各第 nノード補償角 θ n(n=12, 34,56,1234, 123456)を基にしたノードの回転操作を合成することによって、目標接地部位位置を修正し、さらに、第 nノード補償角 θ n(n=l,2,5，6)を基に、第 n接地部位（n=l，2,5，6)の目標姿勢を修正し、これによつて、各接地部位の修正目標接地部位位置姿勢を求める。

また、本実施形態では、床形状推定器 1 3 0は、前記第 3実施形態で説明した手法と同じ手法で推定第 n 床高さ偏差 2 _681：₁11(11=1,2,3,4,5，6)を求めることに加ぇて、第 n接地部位の接地部位床傾斜偏差 Θ fn(_n=l，2,5，6)を推定する。この接地部位床傾斜偏差 Θ fn の推定は、特開平 1 0— 2 7 7 9 6 9号公報に示した床形状推定器の足平床傾斜を推定する処理と同様の処理によって行なわれる。

本実施形態の階層型コンプラインス動作決定部 1 1 4の、上記した以外の機能的構成の処理手法は、第 3実施形態のものと同じである。

以上説明した相違点を除けば、本実施形態における制御装置 5 0の演算処理は、第 1〜第 3実施形態と同様である。

次に、本実施形態における前記ロボット幾何学モデル 1 1 0の処理に関して詳説する。

本実施形態のロボット 5 1においては、上体 5 3から膝に至るまでの関節の自由度の数が 3 しかないので（股関節 5 5に 3 自由度あるだけなので）、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの和とをそれらの目標に維持したまま、左右の膝の高さの差だけを変えることはできない。言い換えると、強引に左右の膝の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。

同様に、厳密に上体位置姿勢と両膝の位置と両足平の水平位置と左右の足平の高さの和とをそれらの目標に維持したまま、左右の足平の高さの差だけを変えることはできない。言い換えると、強引に左右の足平の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。

同様に、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの差とをそれらの目標に維持したまま、左右の膝の高さの和だけを変えることはできない。言い換えると、強引に左右の膝の高さの和だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じることはなくとも、上体位置が目標位置からずれる。その結果、ロボット 5 1の全体の重心位置と慣性力とが目標値からずれ、口ポット 5 1の安定性が低下する。

すなわち、いずれにせよ、階層型コンプライアンス動作決定部 1 1 4 において決定された機構変形補償付き修正目標接地部位位置姿勢と目標上体位置姿勢とを同時に厳密に満足することはできないので、ロボヅ卜 5 1の図 5 8および図 5 9の姿勢状態において、該ロボット 5 1の足平 5 8もしくは膝と床との間でこじりあるいは滑りが生じたり、ロボット 5 1の全体の重心位置と慣性力とが目標値からずれ、ロボット 5 1の安定性が低下したりする。なお、接地部位のうちの手 6 2は、上体 5 3に対して 6自由度以上の自由度を持つので、腕 5 4の手 6 2以外の部位で床に接地するような姿勢をロボット 5 1 に採らせない限り、手 6 2 と床との間でこじりなどの滑りが発生することは無い。

そこで、本実施形態においては、ロボット 5 1の接地部位のうちの膝および足平 5 8と床との間で極力こじりあるいは滑りが生じず、かつ上体 5 3の重心位置（特に水平位置）が極力変わらないようにしつつ、左右の膝の高さの差の変化などに応じて上体 5 3の姿勢、あるいは姿勢と位置とを修正することとする。

図 6 3 ( a ) , ( b ) に、その上体 5 3の姿勢の修正動作の例を示す。なお、図 6 3 ( c ) については後述する。これらの図 6 3 ( a ) , ( b ) , ( c ) では、腕 5 4と頭部 6 3は図示を省略している。

上体 5 3の姿勢の修正動作の 1つは、図 6 3 ( a ) に示すようにロボット 5 1がひざまずいた状態から、コンプライアンス制御による左右の膝の高さの差の変化に応じて、図 6 3 ( b ) に示すように、おおよそ上体 5 3の重心 Gを通る体幹軸を回転軸として、上体 5 3を回転させる (矢印 y lのように回転させる）動作である。

また、図 6 3のような口ポット 5 1の膝付き姿勢においては、左右の足平 5 8， 5 8の高さの差の変化によって、左右の膝の高さの差はおおよそ左右の足平 5 8 , 5 8の高さの差の変化の半分だけ変化する。そこで、コンプラインス制御による左右の足平 5 8 , 5 8の高さの差の変化に応じて、左右の膝の高さの差の変化に応じた上体 5 3の姿勢修正と同様の動作を、左右の足平の高さの差の変化の場合に較べて半分の量だけ行なうようにする。

さらに、このように上体 5 3の姿勢を修正するとき、接地部位（膝および足平 5 8 ) と床との間で極力こじりあるいは滑りが生じず、かつ上体 5 3の重心 G位置（特にその水平位置）、あるいはその重心 Gと全床反力中心点と結ぶ線分の傾きが極力変わらないようにしつつ、コンブラインアンス制御による左右の膝の高さの和の変化に応じて上体 5 3の位置と姿勢とを修正することとする。

図 6 4に、左右の膝の高さの和の変化に応じて上体 5 3の位置と姿勢とを修正する動作を視覚的に示す。すなわち、点線で示すロボット 5 1 の姿勢から、実線で示す Dポット 5 1の姿勢のように、コンプライアンス制御により両膝を下げる動作に伴い、上体 5 3の下端部（あるいは腰部）を矢印 y3 で示す如く前方側にずらし、上体 5 3の傾きを矢印 y2 で示す如く後ろに（上体 5 3の起立方向に）ずらすようにする。すなわち、上体 5 3の重心 G位置 '（あるいは上体 5 3の所定の代表点の位置）, 特にその水平位置を維持しつつ上体 5 3を後ろに傾けるようにする。あるいは、該重心 Gと目標全床反力中心点 Pとを結ぶ線分の傾きを維持しつつ上体 5 3を後ろに傾けるようにする。また、コンプラインス制御により両膝を上げる動作に伴い、上記と逆に、上体 5 3の下端部（あるいは腰部）を後ろにずらし、上体の傾きを前にずらすようにする。すなわち、上体重心 G位置（あるいは上体の所定の代表点の位置）、特にその水平位置を維持しつつ上体 5 3を前に傾けるようにする。あるいは、該重心 Gと目標全床反力中心点 Pとを結ぶ線分の傾きを維持しつつ上体 5 3を後ろに傾けるようにする。なお、図 6 4中 Ql"， Q3"は、それぞれ上記のように上体 5 3の位置姿勢を修正した後の足平 5 8の目標床反力中心点、膝の目標床反力中心点を示しており、この例では、 Q1"は修正前の足平 5 8の目標床反力中心点 Q 1 と同じである。

以上が、上体 5 3の姿勢や位置の修正動作の概要である。

本実施形態におけるロボット幾何学'モデル（逆キネマティクス演算部） 1 1 0の処理機能を図 6 5のプロック図に示す。

逆キネマテイクス演算部 1 1 0では、まず、機構変形補償入り修正目標第 n接地部位位置姿勢（11=1,2，ー，6) と歩容生成装置 1 0 0が決定した各足平 5 8および膝の目標接地部位位置姿勢とを基に、歩容生成装置 5 6

1 0 0が決定した左右の膝の目標位置に対する、左右の膝の高さの差の修正量 Zkneediffmdfd および左右の膝の高さの和の修正量 Zkneesummdfd と、歩容生成装置 1 0 0が決定した左右の足平の目標位置に対する、左右の足平 5 8 , 5 8 の高さの差の修正量 Zfootdiffmdfdを求める。

具体的には、以下の式 4 7 、 4 8 、 4 9によって、左右の膝の高さの差の修正量 Zkneediffmdfd、左右の膝の高さの和の修正量 Zkneesummdfd, 左右の足平の高さの差の修正量 Zfootdiffmdfd を求める。

Zkneeaiffmdfd

= 機機構変形補償入り修正目標第 4接地部位位置

-目標第 4接地部位位置

一機機構変形補償入り修正目標第 3接地部位位置

+ 目標第 3接地部位位置

式 4 7

Zkneesummdfd

= 機機構変形補償入り修正目標第 4接地部位位置

一目標第 4接地部位位置

+ 機機構変形補償入り修正目標第 3接地部位位置

一目標第 3接地部位位置

式 4 8

Zfootdiffmdfd

= 機機構変形補償入り修正目標第 2接地部位位置

-目標第 2接地部位位置 -機機構変形補償入り修正目標第 1接地部位位置

+ 目標第 1接地部位位置

…式 4 9 なお、式 4 7〜 4 9の「部位位置」はより正確には、その「部位位置」の高さ成分（鉛直方向成分）である。

次いで、左右の膝の高さの差の修正量 Zkneediffmdfd、左右の膝の高さの和の修正量 Zkneesummdfd および左右の足平の高さの差の修正量 Zfootdiffmdfdを基に、上体位置姿勢修正量を求める。

上体位置姿勢修正量は、膝高さ差用上体位置修正量 Xbkneediffmdfd, 膝高さ差用上体姿勢修正量 Θ bkneediffmdfd, 膝高さ和用上体位置修正量 Xbkneesummdfd、膝高さ和用上体姿勢修正量 Θ bkneesummdfd および足平高さ差用上体位置修正量 Xbfootdiffmdfd、足平高さ差用上体姿勢修正量 0 bfootdiffmdfd から成る。具体的にこれらの値は次のように求められる。

まず、左右の膝の高さの差の修正量 Zkneediffmdfd に応じて、これによって接地部位（膝および足平 5 8 ) と床との間でこじりあるいは滑りが生じないための、膝高さ差用上体位置修正量 Xbkneediffmdfd と膝高さ差用上体姿勢修正量 0 bkne'ediffmdfd とを、左右の膝の高さの差の修正量 Zkneediffmdfd とその瞬間（現在時刻）におけるロポット 5 1 の目標姿勢（目標運動）とを基に幾何学演算によって求める。

より具体的には、例えば、次式 5 0によって、 Xbkneediffmdfd と 0 bkneediffmdfd とを求める。ただし、 Kxkneediff と Kthkneediff とは, ロボット 5 1の目標姿勢に応じた比例係数である。

Xbkneediffmdfd = Kxkneediff * Zkneediffmdfd Θ bkneediffmdfd = Kthkneediff * Zkneediffmdfd

…式 5 0 これ以外にも、あらかじめ、いくつかの代表的な口ポット 5 1の目標姿勢に対して、左右の膝の高さの差の修正量（あるいは第 3 4ノード補償角 Θ 34) と上体位置姿勢の修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの差の修正量 Zkneediffmdfd を基に Xbkneediffmdfdおよび 0 bkneediffmdfd を求めても良い。なお、本実施形態では、左右の膝の高さの差の修正量に対して、上体 5 3をその体幹軸まわり回転させて上体姿勢を修正するので、膝高さ差用上体位置修正量 Xbkneediffmdfdは 0で良い。

次いで、上記と同様に、左右の足平 5 8， 5 8の高さの差の修正量 Zfootdiffmdfd に応じて、これによつて接地部位（膝および足平 5 8 ) と床との間でこじりあるいは滑りが生じないための、足平高さ差用上体位置修正量 Xbfootdiffmdfd と足平高さ差用上体姿勢修正量 Θ bfootdiffmdfd とを、左右の足平の高さの差の修正量 Zfootdiffmdfd とその瞬間（現在時刻）における口ポット 5 1の目標姿勢（目標運動）とを基に幾何学演算によって求める。

より具体的には、例えば、次式 5 1 によって、 Xbfootdiffmdfd と Θ bfootdiffmdfd とを求める。ただし、 Kxfootdiff と Kthfootdiff とは、口ポット 5 1の目標姿勢に応じた比例係数である。前述したように足平 5 8の高さ差の影響は膝の高さの差の影響のほぼ半分であることから、 Kxfootdiff と Kthfootdiff とは、それぞれ、 Kxkneediff と Kthkneediff の 2分の 1 とする。

Xbfootdiffmdfd = Kxfootdiff * Zfootdiffmdfd Θ bfootdiffmdfd= Kthfootdiff * Zfootdiffmdfd

…式 5 1 なお、本実施形態では、左右の足平 5 8 , 5 8の高さの差の修正量に対して、上体 5 3をその体幹軸まわり回転させて上体姿勢を修正するので、足平高さ差用上体位置修正量 Xbfootdiffmdfdは 0で良い。

次いで、上記と同様に、左おの膝の高さの和の修正量 Zsummdfd に応じて、これによつて接地部位（膝および足平）と床との間でこじりあるいは滑りが生じないための、膝高さ和用上体位置修正量 Xbsummdfd と膝高さ和用上体姿勢修正量 0 bsummdfd とを、左右の膝の高さの和の修正量 Zsummdfd とその瞬間（現在時刻）における口ポット 5 1の目標姿勢を基に幾何学演算によって求める。

これ以外にも、あらかじめ、いくつかの代表的な目標姿勢に対して、左右の膝の高さの和の修正量と上体姿勢の修正量の関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの和の修正量 Zsummdfd とを基に Xbsummdfd と 0 bsummdfd とを求めても良い。

なお、次のように、上体位置姿勢の修正量を決定してもよい。すなわち、左右の膝の高さの差を、その差を生じさせる第 34 ノード補償角 0 34 に換算し、左右の膝の高さの和を、その和を生じさせる第 1234 ノード補償角 0 1234 に換算し、左右の足平の高さの差を、その差を生じさせる第 12ノード補償角 0 12に換算する。そして、これらの換算された補償角を基に、幾何学演算によって上体位置姿勢修正量を決定するようにしても良い。あるいは、あらかじめ、いくつかの代表的なロボット 5 1の目標姿勢に対して、換算された補償角と上体位置姿勢修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと換算された補償角とを基に、上体位置姿勢修正量を決定するようにしても良い。

次いで、目標上体位置姿勢を上体位置姿勢修正量だけ移動（回転移動と平行移動）して、こじり補正入り目標上体位置姿勢を求める。具体的には、体幹軸（あるいは所定の回転軸（ほぼ鉛直面内の回転軸））まわりに目標上体位置姿勢を膝高さ差用上体姿勢修正量と足平高さ差用上体姿勢修正量との和（0 bkneediffmdfd+ Θ bfootdiffmdfd)だけ回転移動しさらにこれを左右軸まわりに膝高さ和用上体姿勢修正量 0 bfootsummdfd だけ回転移動し、さらにこれを口ポット 5 1の前後方向で膝高さ差用上体位置修正量と膝高さ和用上体位置修正量と足平高さ差用上体位置修正量の和（Xbkneediffmdfd + Xbkneesummdfd + Xbfootdiffmdfd)だけ平行移動することによって、こじり補正入り目標上体位置姿勢を求める。

次いで、こじり補正入り目標上体位置姿勢と機構変形補償入り目標第 n 接地部位位置姿勢（η= 1,2，···，6)とを基に、ロボット 5 1の関節変位指令を求める。

以上のごとく逆キネマティクス演算部 1 1 0の処理が実行される。すなわち、第 6実施形態は、口ポット 5 1の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合には、階層型コンプライアンス動作によって目標接地部位位置姿勢を修正するだけでなく、上体位置の点（上体の代表点）あるいは上体を含む複数部位の重み付き平均位置（この場合の重みは各部位の質量比となっていることが望ましい）の点（全体重心など）、特にその点の水平位置を保持するように、あるいは、その点と目標全床反力中心点（目標 Ζ Μ Ρ ) とを結ぶ線の角度を目標歩容における角度にできるだけ保持するように、目標上体位置姿勢も修正するようにした。また、別の言い方をすれば、ロボット 5 1の全体の姿勢制御の安定性を高めるために、目標全床反力中心点（目標 Z M P ) まわりに発生する実床反力モーメントの制御性を優先し、ある所定の接地部位 A (ここでは膝）の相対高さまたは傾斜角度の操作量、あるいはある所定の接地部位 Aを子孫ノードとして持つノードの相対高さまたは傾斜角度の操作量としての補償高さおよび補償角のうちの少なくともいずれか一方を決定する手段と、前記補償高さおよび補償角のうちの少なくともいずれか一方に応じて、上体位置あるいは上体を含む複数部位の前記重み付き平均位置を概ね目標歩容での位置に保持しつつ、上体の姿勢および位置のうちの少なくともいずれか一方の修正量を決定する手段と、前記所定の接地部位 Aを除く所定の接地部位 B (ここでは足平）の位置および姿勢のうちの少なくともいずれか一方と、前記修正量によって修正した上体位置姿勢とに基づいて、関節変位を決定する逆キネマティクス演算手段を備えるようにした。

なお、第 6実施形態では、ロボット 5 1の上体 5 3の体幹軸まわりに上体姿勢を回転させる代わりに、図 6 4 ( c ) のように、鉛直軸を回転軸として、ロボット 5 1の腰（上体 5 3の下端部）を中心に上体 5 3を回転させるようにしても良い。ただし、この場合には、上体 5 3の重心 G位置の左右方向へのずれが大きくなる。あるいは、上体 5 3の体幹軸と鉛直軸との中間的な軸を回転軸として上体姿勢を回転させるようにしても良い。あるいは上体 5 3の位置と姿勢とを両膝の高さの差の修正量や両足平 5 8 , 5 8の高さの差の修正量に応じて同時に修正しても良い ₍ また、上体 5 3の重心 G位置が変わらないようにする代わりに、ロポット 5 1の全体重心位置あるいは上体 5 3の代表点位置が代わらないように上体 5 3の位置姿勢を修正しても良い。

いずれにせよ、コンプライアンス動作などによるロポット 5 1の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合に、接地部位のこじりなどの滑りが生じないように、上体 5 3の位置および姿勢のうちの少なくともいずれか一方を目標歩容の位置姿勢から修正するようにすればよい。また、上体 5 3の位置および姿勢のうちの少なくともいずれか一方を修正する代わりに、上体 5 3以外の所定の部位の位置および姿勢のうちの少なくともいずれか一方を修正するようにしても良い。

[第 7実施形態]

次いで、本発明の第 7実施形態に係る移動口ポットの制御装置を説明する。

第 6実施形態においては、接地部位のこじりなどの滑りを防止し、かつロポット 5 1の全体重心位置と慣性力とができるだけずれないようにするために、上体位置姿勢を修正するようにした。本実施形態（第 7実施形態）においては、接地部位のこじりなどの滑りをある程度までは許容し、優先的にロボット 5 1の全体重心位置と慣性力とがずれないように、関節変位（股関節 5 5と膝関節 5 6の変位）あるいは接地部位位置姿勢を修正するようにした。

すなわち、本実施形態においては、逆キネマテイクス演算部 1 1 0は，第 6実施形態で説明した図 6 5の処理に代えて、図 6 6のブロック図で示す処理を行なうようにした。この相違点を除けば、本実施形態は、第 6実施形態と同じである。 ·

本実施形態の逆キネマティクス演算部 1 1 0を図 6 6を参照して説明すると、まず、各機構変形補償入り修正目標第 n接地部位位置姿勢 (η=1,2 ·，6) (詳しくは位置姿勢のうちの位置）と各目標第 η接地部位位置姿勢（11=1,2 '，6) (詳しくは位置姿勢のうちの位置）とを基に、前記式 4 7、式 4 8および式 4 9にしたがって左右の膝の高さの差の修正量 Zkneediffmdfd、左右の膝の高さの和の修正量 Zkneesummdfd、および左右の足平の高さの差の修正量 Zfootdiffmdfd を求め、さらに、左右の膝の高さの差を、その差を生じさせる第 3 4ノード補償角 0 34 に換算する。同様に、左右の膝の高さの和を、その和を生じさせる第 1234ノード補償角 0 1234に換算し、左右の足平の高さの差を、その差を生じさせる第 12ノード補償角 0 12に換算する。

次いで、換算された各ノド補償角を基に、関節変位修正量を決定する。これは次のように行なわれる。まず、目標歩容の上体位置姿勢を固定し、且つ目標全床反力中心点（目標第 123456 ノード床反力中心点 (図 6 1の点 P ) ) とロボット 5 1の全体重心（あるいは、上体重心、あるいは、上体 5 3を含む複数の部位の重心）とを結ぶ線分の角度が変わらないようにしつつ、関節変位を修正した場合を想定し、この場合に，各接地部位（膝および足平 5 8 ) に対応した第 nノード（n=12，34，1234) の傾斜角度（補償角）が単位摂動量になるための第 j 関節変位修正量 (j=l,2,…，関節の総数）を求め、これを第 nノード補償角に対する第 j 関節変位の感度 LnJ とする。

より一般的には、目標歩容の関節変位を修正した場合に、目標全床反力中心点（目標第 123456 ノ一ド床反力中心点）とロボット 5 1の全体重心（あるいは、上体重心、あるいは、上体 5 3を含む複数の部位の重心）を結ぶ線分と、各接地部位（膝および足平 5 8 ) に対応した第 nノ一ド（n=12，34，1234)の傾斜角度との間の相対角度が単位摂動量になるための第 j 関節変位修正量（j=l,2, "'，関節の総数）を求め、これを第 nノ一ド補償角に対する第 j 関節変位の感度 LnJ とする。

具体的には、 Ln— knee— r を第 nノード補償角（n=12,34，1234)に対する右膝関節変位の感度、 Ln knee 1 を第 nノード補償角に対する左膝関節変位の感度、 Ln_hip_r を第 nノード補償角に対する右股ピッチ関節変位の感度、 Ln— hip— 1 を第 nノード補償角に対する左股ピッチ関節変位の感度とすると、図 5 8および図 5 9に示す口ポット 5 1の膝付き姿勢においては、各感度は次式 5 2のように設定する。

Ll234_hip_r= 0、 L1234— hip— 1= 0、 L1234— knee— r = 1、

L1234_knee_l= 1、 L12_hip_r= 0、 L12— hip— 1= 0

L12— knee— r=al2、 L12— knee— 1=— al2、 L34_hip_r = a34

L34— hip— 1=— a34、 L34_knee_r= 0、 L34_knee_l= 0

…式 5 2

ただし、 al2、 a34は所定の定数である。

次いで、次式 5 3〜 5 6により各関節変位修正量を決定する。ただし， Θ knee_r は、右膝関節変位修正量、 0 knee— 1 は、左膝関節変位修正量, Θ hip_r は、右股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）、 Θ hip_l は、左股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）とする。

Θ knee_r= L1234_knee_r * θ 1234+ L12_knee_r * θ 12

+ L34_knee_r* θ 34

…式 5 3

Θ knee_l= L1234_knee_l * Θ 1234+ L12_knee_l * Θ 12

+ L34_knee_l* Θ 34

…式 5 4

Θ hip_r= L1234_hip_r * Θ 1234 + L12_hip_r * Θ 12

+ L34_hip_r * Θ 34

…式 5 5 Θ hip_l= L1234_hip_l * θ 1234 + L12_hipJ * θ 12

+ L34_hip_l * θ 34

…式 5 6 なお、他の関節変位修正量も同様に求めるが、説明を容易にするために、ここでは省略する。

すなわち、第 1234 補償角 Θ 1234 に対しては、図 6 7に示すごとく両膝関節 5 6の変位（角度を Θ 1234 と同量だけ修正する。また、第 12 補償角 0 12 に対しては、右膝関節 5 6の変位（角度）を Θ 12 に比例して修正しつつ、左膝関節 5 6の変位（角度）を右膝関節 5 6の変位の修正量に— 1 を乗じた量だけ修正する。つまり右膝関節 5 6の変位 (角度）を図 6 7に示すごとく修正し、左膝関節 5 6の変位（角度）はその逆に修正する。また、第 34補償角 0 34に対しては、右股関節 5 5 のピッチ方向の変位（角度）を 0 34 に比例して修正し、左股関節 5 5 のピッチ方向の変位（角度）を右股関節 5 5のピッチ方向の変位の修正量に— 1を乗じた量だけ修正するようにした。つまり右股関節 5 5のピツチ方向の変位（角度）を図 6 8に示すごとく修正し、左股関節 5 5のピッチ方向の変位（角度）はその逆に修正する。なお、図 6 7の Ql， Q1"はそれぞれ上記のような関節変位の修正前の足平 5 8の目標床反力中心点、修正後の足平 5 8の目標床反力中心点を示している。また、図 6 8の Q3 , Q3"はそれぞれ上記のような関節変位の修正前の膝の目標床反力中心点、修正後の膝の目標床反力中心点を示している。

このような関節変位の修正により、目標全床反力中心点（目標第 123456 ノード床反力中心点）とロボット 5 1の全体重心（あるいは、上体重心、あるいは、上体 5 3を含む複数の部位の重心）を結ぶ線分と，接地部位（膝および足平 5 8 ) に対応した第 nノード（n=12, 34, 1234)の傾斜角度との間の相対角度が、前記換算された各ノ一ド補償角 Θ 12， Θ 23 , 0 1234 だけ変化する。したがって、実床反力が忠実に制御され, ロボット 5 1の姿勢安定性および接地性が向上する。

上記の如く関節変位指令（目標関節変位）を直接修正する代わりに、略同一の目標関節変位を生じさせるために優先的に位置姿勢を修正する接地部位（具体的には、足平 5 8、手 6 2 ) の修正後の目標位置姿勢である優先修正目標接地部位位置姿勢を決定するようにし、決定された優先修正目標接地部位位置姿勢を基に逆キネマティクス演算によって関節変位指令を決定するようにしても良い。

なお、関節の自由度不足のため、目標上体位置姿勢とすべての修正目標接地部位位置姿勢とを満足するように逆キネマテイクス演算によって関節変位を決定することはできないので、逆キネマティクス演算を行なうときには、すべての修正目標接地部位位置姿勢のうちの一部の修正目標接地部位位置姿勢を用いることとなる。この一部の修正目標接地部位位置姿勢を前記優先修正目標接地部位位置姿勢と呼ぶ。

たとえば、図 6 7の状況においては、第 1234 ノード補償角 Θ 1234 に対しては、膝を中心として目標足平位置を回転移動させた位置を優先修正目標足平位置と決定すれば良い。

なお、第 6実施形態および第.7実施形態では、一例として、口ポット 5 1のひざまずき状況におけるコンプライアンス動作について説明したが、ロボッ卜が椅子に座った状況でのコンプライアンス動作に適応する場合には、次のようにすれば良い。以下に、それを第 8実施形態として説明する。 [第 8実施形態]

図 7 0は本実施形態のロボットの要部の構成を示している。このロボット 7 1は、臀部 7 2の底面部の左右にそれぞれ床反力を検出する床反力センサ 7 3， 7 3 (荷重センサ。例えば 6軸力センサ）を備える。なお、左右に床反力センサ 7 3 , 7 3を設ける代わりに、臀部 7 2の底面部の左右に加わる力の合力を検出する 1つの床反力センサを備えるようにしてもよい。

この場合、また、床反力センサ 7 3 , 7 3の外側は、図示の如くスポンジ等の柔軟な部材（弾性体） 7 4によって覆われている。なお、臀部 7 2の接地部位におけるコンプライアンス制御の精度を良くするために. 柔軟な部材によって覆うだけでなく、図示の如く柔軟な部材（弾性体） 7 4の表面（接地面）を丸く凸面状に形成することが望ましい。特に、ロボット 7 1がその臀部 7 2を介して椅子などに座った状態において床反力センサ 7 3 , 7 3の検知位置（センサ本体の位置）と上記部材 7 4 の凸面の頂点との水平位置を一致させるように、部材 7 4を設けておくことが望ましい。このようにすることで、ロボット 7 1の接地部位の位置姿勢の修正動作と床反力との関係の非線形性が小さくなるので、ロボット 7 1のコンプライアンス制御などの制御特性が良好になる。

なお、ロボット 7 1では、臀部 7 2の左右の側部から、脚（リンク機構） 5 2， 5 5が延設されている。脚 5 5， 5 5の構造は、例えばその関節を含めて前記第 6実施形態のものと同一とされている。従って、脚 5 , 5 5についての第 6実施形態と同一の参照符号を用い、説明を省略する。ただし、本実形態では、脚 5 5の膝に床反力センサを備えていなくてもよい。

また、図示のように、臀部 7 2の上側には、胴体（上体） 7 7が設けられ、この胴体 7 7の上部の両側部から肩関節 7 8 , 7 8を介して腕 7 9 , 7 9が延設されている。なお、腕 7 9は、前記第 6実施形態のロボット 5 1のものと同一構造でもよい。さらに、胴体 7 7は、関節 8 0を介して臀部 7 2に連結されている。この場合、関節 8 0は、臀部 7 2に対して胴体う 7 7をョ一方向に旋回させる胴体旋回用関節 8 0 aと、胴体 7 7を臀部 7 2に対して前後および左右方向に傾斜させる胴体屈曲用関節 8 O bとから構成されている。口ポット 7 1 に上述のように備えられた各関節は、図示しないァクチユエ一夕により動作する。また、臀部 7 2または胴体 7 7には、図示を省略するが、前記第 1実施形態などと同様の制御装置 5 0が搭載されている。

本実施形態では、臀部 7 2を介して椅子などに腰掛けるロポット 7 1 に対して、図 7 0に示す如く階層構造を設定すればよい。すなわち、接地部位としての右足平 5 8、左足平 5 8、臀部 7 2の底面部の右側部 (右側床反力センサ 7 3の取り付け部位）、臀部 7 2の底面部の左側部 (左側床反力センサ 7 3の取り付け部位）をそれぞれ葉ノードである第 1ノード、第 2ノード、第 3ノード、第 4ノードに対応させる。また、第 1 ノードと第 2ノードとを子ノードとして持つ第 1 2ノード、第 3ノ —ドと第 4ノードを子ノードとして持つ第 3 4ノードを中間ノードとして設定し、第 12ノードと第 34ノードを子ノードとして持つ第 1234ノ一ドを根ノードとして設定した。また、各ノードには、前記第 1実施形態などと同様の方針によって、図示の如く、目標床反力中心点 Qn(n=l, 2, 3,4, 12, 34, 1234)が対応づけられる。なお、根ノードである第 1234 ノードの目標床反力中心点 Q 1234は、目標全床反力中心点 Pと同一である。

この場合、本実施形態における歩容生成装置 1 0 0が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各足平 5 8の目標位置姿勢軌道と臀部 7 2の目標位置姿勢軌道とから構成される。目標運動のうちの上体位置姿勢軌道は、本実施形態では、胴体 7 7の位置姿勢軌道を意味する。なお、目標運動には、各腕 7 9の先端部の位置姿勢軌道も含まれる。また、目標全床反力中心点 Pは、実際の床面上にはなく、空中の仮想面上にある。

また、本実施形態では、階層型コンプライアンス動作決定部 1 1 4は. その機能的構成要素は、前記第 6実施形態のものと同様である（図 6 2 を参照）。ただし、本実施形態では、補償全床反力モーメント分配器は. 図 7 0に示した階層構造における各中間ノードおよび根ノードのノード補償床反力モーメントと、各足平 5 8に対応する葉ノードのノ一ド補償床反力モーメントとを決定して出力する。また、補償角決定部は、図 7 0に示した階層構造における各中間ノードおよび根ノードのノード補償角と、各足平 5 8に対応する葉ノ一ドのノ一ド補償角とを決定して出力する。この場合、これらのノード補償床反力モーメントおよびノード補償角の基本的な決定手法は、前記第 1〜第 3実施形態や第 6実施形態で説明した手法と同じでよい。また、床形状推定器は、前記第 6実施形態の場合と同様に、前記第 3実施形態で説明した手法と同じ手法で推定第 n床高さ偏差

求めることに加ぇて、各足平 5 8 の接地部位床傾斜偏差 Θ fn(n= l,2)を推定する。

また、制御装置 5 0の機能的構成要素である口ポット幾何学モデル (逆キネマテイクス演算部）は、前記第 6実施形態と同様の手法によつて、基本的には、前記機構変形補償入り修正目標接地部位位置姿勢と目標上体位置姿勢とを基に、臀部 7 2の接地部位と足平 5 8 (接地部位）とでこじりなどの滑りが発生しないように、それらの接地部位の位置姿勢を修正すると共に、上体（胴体） 7 7の位置姿勢を修正する。

以上説明した以外の制御装置 5 0の処理は、前記第 6実施形態と同様でよい。次に、以上説明した実施形態に係わるいくつかの変形態様を説明する, 床反力センサによって直接的に検出できない実ノード床反力は、床反力センサによって直接的に検出される他の実接地部位床反力、加速度センサの検出値、傾斜計の検出値などを用いて、オブザーバにより推定するか、もしくは、簡易的な代数関係によって推定すればよい。例えば、階層型コンプライアンス動作は、目標全床反力中心点まわりに接地部位を回転させる回転型のコンプライアンス動作であるので、コンプライアンス動作を行なっても、口ポットの全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）は、口ボットの目標歩容（目標運動）における全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）にほぼ一致する。したがって、すべての実接地部位床反力鉛直成分の和は，ロボットの目標歩容（目標運動）における全体重心の鉛直加速度と重力加速度との和にロポッ卜の全体質量を乗じた値にほぼ一致する。

よって、実第 nノード床反力が直接的に検出できない場合、まず、第 nノードを先祖ノードに持たず、かつ、第 nノードそのものでないすべての葉ノードの実床反力（実接地部位床反力）の和（以降、これを実第 nノード外床反力と呼ぶ）を求める。

次いで、次式 5 7によって、実第 nノード床反力の推定値である推定第 nノード床反力を求める。

推定第 nノ一ド床反力

= (目標歩容の全体重心加速度 +重力加速度） *全体質量

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

= (推定全体重心加速度 +重力加速度） *全体質量

一実第 nノード外床反力 …式 5 8 例えば、前記第 6実施形態において、膝の床反カを検知する床反カセンサが備わっていない場合、あるいは、膝の床反カを検知する床反カセンサが故障した場合には、推定第 3 4ノード床反力を次式 5 9によって求める。推定第 34ノード床反力

= (目標歩容の全体重心加速度 +重心加速度） *全体質量

一実第 34ノード外床反力

=目標歩容の全体重心加速度 *全体質量

― (実第 12 ノード床反力 +実第 56ノード床反力）

' …式 5 9 さらに、推定第 34 ノード床反力を実第 34 ノード床反力の代わりに用いて、コンプライアンス動作処理によって第 1234 ノード補償角 0 1234 を決定し、また床形状推定処理によって推定第 1234 ノード相対床高さ偏差 Z1234rel— estm を推定する。

第 34ノード補償角 0 34の決定では、想定した床通りであると仮定して、機構変形補償入り修正目標第 3接地部位位置姿勢、機構変形補償入り修正目標第 4接地部位位置姿勢と推定第 34 ノード床反力を基に、実第 3 ノード床反力の推定値である推定第 3 ノード床反力と実第 4ノード床反力の推定値である推定第 4ノード床反力とを求める。

さらに、推定第 3ノード床反力と推定第 4ノード床反力を実第 3 ノード床反力と実第 4ノード床反力の代わりに用いて、コンプライアンス動作処理によって第 34ノード補償角 0 34を決定する。

口ポットの姿勢の復元要求モーメント（補償全床反力モーメント）を基に、前述の如く復元要求モーメントを加えることと等価になるように目標ノード床反力中心点まわりの修正目標ノード床反力モーメントを決定する代わりに、復元要求モーメント（補償全床反力モーメント）を基に、目標ノード床反力中心点を修正した修正目標ノード床反力中心点を決定しても良い。この場合、目標ノード床反力中心点まわりの目標ノ一ド床反カモ一メントは修正しないで 0のままとする。

また、第 nノード床反力の内で制御できなかった成分（予想される、または検出された制御偏差成分）を補うために、この成分に応じて、第 nノードの親ノードの目標床反力を修正しても良い。すなわち、実第 n ノード床反力と目標第 nノード床反力との差の前回制御周期の値あるいは前記差をローパスフィルタに通した値を第 nノードの目標床反力に付加しても良い。

床形状の推定処理においては、コンプライアンス制御用のツリー構造と異なるツリー構造を設定してもよい。そのツリー構造は、根ノードと葉ノードとからなる 2層のものであってもよい。また、各ノードの重みも.コンプライアンス制御用のものと異なるものとしてもよい。ノード床反力が 0 となるような時期（そのノードに属する全ての接地部位、またはそのノードに対応する接地部位が空中を移動する時期）でそのノードの重みが 0であれば良い。また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノード重みを決定し、これを要素とするべクトルを、床形状推定に用いる重みとして用いても良い。

床形状推定の処理で、各点（各接地部位）の推定相対床高さ偏差を求める際に、実床反力と機構コンプライアンスモデルと姿勢センサの検出値とを用いる代わりに、コンプライアンス機構などの変形量を検出する変形量検出器を備え、その変形量検出器の検出値と姿勢センサの検出値とを用いるようにしてもよい。床形状を相対的に推定するだけでなく、グローバルに推定（グローバル座標系での床形状の推定）したい場合には、次のようにしてもよい。

1 ) 口ポットの上体のグローバル座標系での高さ（以下、グローバル高さという）を検知できる場合における床のグローバル高さの推定：上体などロボットのある所定部位のグローバル高さを検出する検出器 (加速度センサを用いた上体高さ推定装置（例えば本願出願人による P C T / J P 0 3 / 0 5 4 4 8号に記載されている推定装置）あるいは視覚センサ等の外界センサなど）を用いる。推定された上体のグローバル高さと、関節変位（実関節変位あるいは目標関節変位）、実上体姿勢傾斜の検出値および実床反力の検出値を基に、推定床高さ偏差のグローバル座標系での値を求める。従って、根ノードの推定床高さ偏差は、すべての接地部位のグローバルな推定床高さ偏差の重み付き平均値としての意味を持つこととなる。

2 ) 上体のグローバル高さがわからない状況での床のグローバル高さの推定：

ある推定第 n 床高さ偏差のグローバル値（グループ内相対値ではなくグロ一バル座標系における偏差）がわかっているならば、それと推定各相対床高さ偏差を基に、すべての点（接地部位）の推定床高さ偏差のグローバル値を求める。口ポットの移動のスタート点での推定第 n 床高さ偏差のグローバル値がわかっていれば、ロボッ卜の移動を続けることで、逐次新しく接地した接地部位の床高さ偏差のグローバル値を求められるので、これを記憶することにより、口ポットの移動軌跡上での床高さのマツプを作成できる。

また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノニド重み（各ノードの重みの値を修正したもの）を決定し、これを、床形状推定において、実第 nノード相対床高さ等を定義するために用いる重みとしても良い。また、床形状推定において、実第 nノード相対床高さ等を定義するために用いる重みは、目標床反力分配器が決定した前述の重みと必ずしも同一のものにする必要はない。

補足すると、実第 nノード相対床高さ等を定義するための重みは、目標床反力分配器が決定する重みに必ずしも一致させなくてはならない訳ではないが、一致させるならば、（この場合、自動的に、根ノードの目標床反力中心点は目標全床反力中心点に一致する、）コンプライアンス動作用の第 nノード補償角（11=1，2 '）に第 nノード推定床傾斜偏差を付加するだけで、床形状偏差の影響を打ち消すことができるので、床形状偏差の影響を打ち消すための演算量を低減することができる。

さらに、推定床形状偏差を床形状の地図情報として記憶しておき、次回同一場所を歩行する時に、記憶しておいた床形状の地図情報を基に目標歩容を生成するようにしても良い。

尚、前述の各実施形態における演算処理機能を示すブロック線図もしくはフローチャートにおける演算処理順序を等価変形をしても良い。また、本発明は、移動口ポットに限らず、アクティブサスペンションを持つ車輪型移動体にも応用することが可能である。産業上の利用可能性

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

Claims

請求の範囲

1 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以上の接地部位を有する移動ロポッ卜等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、

複数の接地部位を子孫ノードとして持つノードである第 Cノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が 0に近づくように、該第 Cノードに属する複数の接地部位の間の目標相対高さの修正量を該第 Cノード毎に決定し、その決定した修正量をすベての第 Cノードについて合成してなる修正量を基に、前記目標運動に、各第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とする移動体の制御装置。

2 . 前記ツリー構造における各ノードに重みが設定されており、該ッリ一構造における各ノードである第 Bノニドに対し、該第 Bノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第 Bノードの高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第 Bノードの高さとして定義したとき、前記ノード動作制御手段は、各第 Cノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノ一ドの目標高さは前記目標運動における目標高さに維持されることを特徴とする請求の範囲第 1 項に記載の移動体の制御装置。

3 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以上の接地部位を有する移動ロボッ卜等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、

前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ッリ一構造における各ノードに重みが設定されており、

少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が 0に近づくように、前記目標運動に、該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、 '

前記ツリー構造における各ノードである第 Bノードに対し、該第 Bノ ―ドが前記葉ノードである場合には、該葉ノードである接地部位の高さを第 Bノードの高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第 Bノードの高さとして定義したとき、前記ノード動作制御手段が、前記第 Cノードのみに対して、該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加して前記目標運動を決定したとき、該修正目標運動における該第 Cノードの目標高さは前記目標運動における目標高さに維持されることを特徴とする移動体の制御装置。

4 . 前記ツリー構造において子ノードを持つ各ノードに対し、該各ノ一ドの全ての子ノードの重みは、その全ての子ノードの重みの総和が 1 となるように設定されていることを特徴とする請求の範囲第 2項または第 3項に記載の移動体の制御装置。

5 . 前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第 Dノードとしたとき、前記重みを設定する手段は、各第 Dノードの重み、または該第 Dノードの祖先ノードのうちの少なくともいずれか 1つのノードの重みを 0に設定することを特徴とする請求の範囲第 2項または第 3項に記載の移動体の制御装置。

6 . 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、

前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第 Cノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第 1項または第.2項に記載の移動体の制御装置。

7 . 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、

前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第 Cノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第 3項に記載の移動体の制御

5 8 . 前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定することを特徴とする請求の範囲第 6項または第 7項に記載の移動体の制御装置。

0 9 . 前記移動体は、その基体から延設された複数のリンク機構を備える、口ポットであり、該複数のリンク機構のうちの少なくとも 1つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっていることを特徴とする請求の範囲第 6項に記載の移動体の制御装置。

5 1 0 . 前記移動体は、その基体から延設された複数のリンク機構を備える口ポットであり、該複数のリンク機構のうちの少なくとも 1つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第 Cノ一ドに属する接地部位となっていることを特徴とする請求の範囲第 7項に記載の移動体の 0 制御装置。

1 1 . 前記中間部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求の範囲第 9項または第 1 0項に記載の移動体の制御装置。

1 2 . 前記移動体は、その基体から延設されて 1つ以上の関節をそれぞ5 れ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも 1つのリンク機構と前記基体とに前記接地部位が設けられていることを特徴とする請求の範囲第 6項に記載の移動体の制御装置。

1 3 . 前記移動体は、その基体から延設されて 1つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも 1つのリンク機構と前記基体とに前記接地部位が設けられており、前記第 Cノードに属する接地部位には、少なくとも前記基体の接地部位が含まれることを特徴とする請求の範囲第 7項に記載の移動体の制御装置。

1 4 . 前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求の範囲第 1 2項または第 1 3項に記載の移動体の制御装置。

1 5 . 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されており、

前記修正目標運動を満足するように前記関節を動作させたときに前記第 Cノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、

前記ノード動作制御手段は、少なくとも前記第 Cノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第 Cノードに属する各接地部位に作用したときに前記基体と該第 Cノ一ドに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第 Cノ一ドに属する複数の接地部位の目標高さの第 2の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第 1項または第 3項に記載の移動体の制御装置。

1 6 . 少なくとも前記第 Cノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメータを床形状を表す床形状パラメータとして推定する床形状推定手段を備え、前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメ一夕の推定値に応じた、前記第 Cノードに属する複数の接地部位の間の目標相対高さの第 3の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求の範囲第 1項または第 3項に記載の移動体の制御装置。

1 7 . 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、

前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードに属する各接地部位の実床反力とに基づき、前記床形状パラメ一夕を逐次更新しつつ推定することを特徴とする請求の範囲第 1 6項に記載の移動体の制御装置。

1 8 . 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、

前記ツリー構造における各ノードである第 Aノードに対し、該第 Aノ一ドが前記葉ノ一ドである場合には、該葉ノ一ドである接地部位に作用する床反力を第 Aノードのノード床反力と定義し、且つ、第 Aノードが子ノードを持つ場合には、該第 Aノードのすべての子ノードのノード床反力の合力を第 Aノードのノード床反力と定義したとき、少なくとも前記第 Cノードの各子ノードのノード床反力の実際値である実ノード床反力を該第 Cノードに属する各接地部位の実床反力から逐次決定する手段を備え、

前記床形状推定手段は、前記床形状パラメ一夕の推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメ一夕を逐次更新しつつ推定することを特徴とする請求の範囲第 1 6項に記載の移動体の制御装置。

1 9 . 前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結され、

該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、

前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第 Cノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメ一夕を逐次更新しつつ推定することを特徴とする請求の範囲第 1 6 項に記載の移動体の制御装置。

2 0 . 前記第 Cノードに属する前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定することを特徴とする請求の範囲第 1 9項に記載の移動体の制御装置。

2 1 . 前記ツリー構造における各ノードの重みが設定されており、該ッリー構造における各ノードである第 Bノードに対し、該第 Bノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第 Bノードの接地面の高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第 Bノードの接地面の高さとして定義したとき、

前記床形状推定手段が推定する前記床形状パラメ一夕は、前記第 Cノ —ドの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることを特徴とする請求の範囲第 1 6に記載の移動体の制御装置。

2 2 . 前記床形状推定手段は、前記第 Cノードの各子ノードのうちの少なくとも 1つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメ一夕を推定することを特徴とする請求の範囲第 2 1項に記載の移動体の制御装置。

2 3 . 前記床形状推定手段は、前記床形状パラメ一夕の変動を緩和するように口一パスフィル夕を用いて該床形状パラメータを推定することを特徴とする請求の範囲第 1 6項に記載の移動体の制御装置。

2 4 . 前記ツリー構造の各ノードには床形状の推定用の重みが設定されており、

前記ツリー構造における各葉ノードに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に 0を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードにづいて階層相対化する処理として疋し、

前記葉ノ一ドである接地部位のそれぞれに作用する床反力 Fn(n= l ,2， …：)を基に、各ノードについて階層相対化したノード相対床反力 Fn rel(n= l,2，"'）を求め、さらに子ノードを持つ任意のノードである第 nノードのすべての子ノード aj (j = l, 2, ，r。 rは第 nノードの子ノードの総数）のノード相対床反力を要素とするべクトル（Fa l一 rel，Fa2_rel， •••，Far_rel)を、第 n ノードのすべての子ノ一ドの重みを要素とするベクトル（Wal,Wa2，…，War)にそれぞれ直交する所定の互いに独立な複数のベクトル R(j) (j= l,2,…，！ · _ 1)の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第 nノードのノード拡張床反力モ一メン h Mn_exp と定義し、

また、前記葉ノードである接地部位のそれぞれの接地面の高さ Zn (_η= 1, 2, ··· )を基に、各ノ一ドについて階層相対化したノード相対高さ Zn—rel(n=l, 2, '" )を求め、さらに前記第 nノードのすべての子ノード aj （j=l，2,〜，r。 r は第 nノードの子ノードの総数）のノード相対高さを要素とするベクトル（Zal— rel, Za2— rel，…，Zar— rel)を、前記所定の互いに独立な複数のベクトル R(j) (]'=1，2,… 1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第 nノードのノード拡張傾斜角 0 nと定義したとき、

前記床形状推定手段は、少なくとも前記第 Cノードの前記ノード拡張床反カモ一メント Mn_exp(n=C)またはノード拡張傾斜角 Θ n(n=C)を用いて前記床形状パラメ一夕を推定することを特徴とする請求の範囲第 1 6項に記載の移動体の制御装置。'

2 5 . 前記ツリー構造における各ノードである第 Bノードに対し、該第 Bノードが前記葉ノ一ドである場合には、該葉ノードである接地部位の接地面の高さを第 Bノードの接地面の高さと定義し、且つ、第 Bノードが子ノードを持つ場合には、該第 Bノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第 Bノードの接地面の高さとして定義したとき、

前記床形状パラメ一夕に、前記第 Cノードの各子ノードの接地面の相対高さを前記ノード拡張傾斜角を用いて表したパラメータが含まれることを特徴とする請求の範囲第 2 4に記載の移動体の制御装置。

2 6 . 前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、

前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノ —ド拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメ一夕の推定値の過去値とを基に、新たな床形状パラメ一夕を推定することを特徴とする請求の範囲第 2 4項に記載の移動体の制御装置。