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

Abstract

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

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つ以上の接地部位を有する移動ロボ ッ ト等の移動体の目標運動と目標床反力とから構成される目標歩容を決 定する目標歩容決定手段と、 前記接地部位のそれぞれに作用する実床反 力を検出または推定する床反力検出手段とを備え、 前記移動体の歩容を 目標歩容に追従させるように該移動体の動作制御を行う移動体の制御装 置において、

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

前記ツリー構造における各ノードである第 Bノードに対し、 該第 Bノ 一ドが前記葉ノードである場合には、 該葉ノ一ドである接地部位に作用 する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 Bノードが 子ノードを持つ場合には、 該第 Bノードのすべての子ノ一ドのノード床 反力の合力を第 Bノードのノード床反力と定義したとき、

各ノー ドのノード床反力の実際値としての実ノード床反力を前記検出 または推定された各接地部位の実床反力から決定する手段と、

各ノー ドのノード床反力の目標値としての目標ノ一ド床反力を少なく とも前記目標歩容の目標床反力に基づき決定する手段と、 複数の接地部位を子孫ノードとして持つノードである第 Cノードのそ れぞれに対し、 少なく とも該第 Cノードの各子ノードの実ノード床反力 の間の相対関係と該第 Cノ一ドの各子ノードの目標ノード床反力の間の 相対関係とを基に、 該第 Cノードに属する複数の接地部位の間の目標相 対高さの修正量を該第 Cノード毎に決定すると共に、 その決定した修正 量をすベての第 C ノ一ドについて合成してなる修正量を基に、 前記目標 歩容の目標運動に、 各第 Cノードに属する複数の接地部位の間の目標相 対高さの第 1の修正を少なくとも付加した修正目標運動を決定し、 その 決定した修正目標運動を満足するように前記関節を動作させるノード動 作制御手段とを備えたことを特徴とするものである。

なお、 本発明においては、 子ノードを持つ各ノードに対し、 その 「ノ ードに属する接地部位」 とは、 より正確に言えば、 その 「ノードに子孫 ノードとして属する」 ということを意味するものである。 このことは、 第 1発明に限らず、 本願のいずれの発明についても同じである。

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

また、 本発明の移動体の制御装置の第 2発明は、 互いの相対高さを操 作可能に複数の関節を介して連結された 3つ以上の接地部位を有する移 動ロポッ ト等の移動体の目標運動と目標床反力とから構成される目標歩 容を決定する目標歩容決定手段と、 前記接地部位のそれぞれに作用する 実床反力を検出または推定する床反力検出手段とを備え、 前記移動体の 歩容を目標歩容に追従させるように該移動体の動作制御を行う移動体の 制御装置において、

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

前記ツリー構造における各ノードである第 Bノードのそれぞれに対し、 該第 Bノードが前記葉ノ一ドである場合には、 該葉ノ一ドである接地部 位に作用する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 A ノードが子ノ一ドを持つ場合には、 該第 Bノードのすべての子ノードの ノード床反力の合力を第 Bノードのノード床反力と定義したとき、 複数の接地部位を子孫ノ一ドと''して持つ中間ノ一ドである所定の第 C ノードに対し、

少なくとも前記第 Cノ一ドの各子ノードのノード床反力の実際値とし ての実ノ一ド床反力を前記検出または推定された各接地部位の実床反力 から決定する手段と、

少なくとも前記第 Cノ一ドの各子ノードのノード床反力の目標値とし ての目標ノード床反力を少なくとも前記目標歩容の目標床反力に基づき 決定する手段と、

少なくとも該第 Cノ一ドの各子ノードの実ノード床反力の間の相対関 係と該第 Cノードの各子ノードの目標ノ一ド床反力の間の相対関係とを 基に、 前記目標歩容の目標運動に、 前記第 Cノードに属する複数の接地 部位の間の目標相対高さの第 1の修正を少なく とも付加した修正目標運 動を決定し、 その決定された修正目標運動を満足するするように前記関 節を動作させるノ一ド動作制御手段を備えたことを特徴とするものであ る。

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

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

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

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

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

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

同様に、 上記第 4発明によれば、 前記第 Cノードのみに対して、 該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を前 記目標運動に付加して前記修正目標運動を決定したときには、 該第 Cノ

—ドの子ノードの上記重み付き平均値が修正目標運動と目標運動と同じ になるように前記第 1の修正が目標運動に付加される。

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

また、 本発明の移動体の制御装置の第 5発明は、 互いの相対高さを操 作可能に複数の関節を介して連結された 3つ以上の接地部位を有する移 動ロポッ ト等の移動体の目標運動と目標床反力とから構成される目標歩 容を決定する目標歩容決定手段と、 前記接地部位のそれぞれに作用する 実床反力を検出または推定する床反力検出手段とを備え、 前記移動体の 歩容を目標歩容に追従させるように該移動体の動作制御を行う移動体の 制御装置において、

前記接地部位のそれぞれが葉ノードとなり、 且つ、 該葉ノードと該葉 ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが 存在するように前記接地部位をッリ一構造状に分類すると共に、 前記ッリ一構造における各ノ一ドである第 Bノー に対し、 該第 Bノ ードが前記葉ノードである場合には、 該葉ノードである接地部位に作用 する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 Bノードが 子ノ一ドを持つ場合には、 該第 Bノ一ドのすべての子ノ一ドのノード床 反力の合力を第 Bノードのノード床反力と定義し、 さらに前記第 Bノー ドのノード床反力のモーメ ントの水平成分または床面平行成分が 0とな る点を第 Bノードのノード床反力中心点と定義したとき、

各ノードのノード床反力中心点の目標位置である目標ノード床反力中 心点を前記目標歩容の目標運動と目標床反力とのうちの少なくともいず れか一方に応じて決定する手段と、

各ノードのノード床反力の実際値としての実ノード床反力を前記検出 または推定された各接地部位の実床反力から決定する手段と、

複数の接地部位を子孫ノ一ドとして持つノードである第 Cノードのそ れぞれに対し、

前記第 Cノードの実ノー ド床反力が該第 Cノードの目標ノード床反力 中心点に作用するモーメン トである実モーメント （Me— act) と該第 C ノードの目標ノード床反力中心点に作用すべきモーメントの目標値 (Mc_dmd) との差 （Mc— act— Mc— dmd) と、

前記第 Cノードの実モーメント （Mc_act) から該第 Cノードの各子 ノードの実ノード床反力が該子ノードの目標ノ一ド床反力中心点に作用 するモ一メントを減じてなるモーメント （Mc— f) と該第 Cノードの目 標ノード床反力中心点に作用すべきモーメントの目標値 （Mc— dmd) と の差 （Mc— f—Mc— dmd) と、

前記第 Cノードの実ノ一 ド床反力のモーメントの水平成分または床面 平行成分が 0となる点の位置と該第 Cノードの目標ノード床反力中心点 の位置との差の値 （Δ P ) と、

前記第 Cノードの実ノー ド床反力のモーメント （Mc_act) から該第 Cノードの各子ノードの実ノード床反力が該子ノードの目標ノード床反 力中心点に作用するモーメ ントを減じてなるモーメン卜の水平成分また は床面平行成分が 0となる点の位置と該第 Cノードの目標ノード床反力 中心点の位置との差の値 （Δ P f) とのうちのいずれかを制御量として、 該制御量に応じて、 該制御量が 0に近づくように、 該第 Cノードに属す る複数の接地部位の間の目標相対高さの修正量を該第 Cノ一ド毎に決定 すると共に、 その決定した修正量をすベての第 Cノードについて合成し てなる修正量を基に、 前記目標歩容の目標運動に、 各第 Cノードに属す る複数の接地部位の間の目標相対高さの第 1の修正を少なくとも付加し た修正目標運動を決定し、 その決定した修正目標運動を満足するように 前記関節を動作させるノード動作制御手段とを備えたことを特徴とする ものである。

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

また、 本発明の移動体の制御装置の第 6発明は、 互いの相対高さを操 作可能に複数の関節を介して連結された 3つ以上の接地部位を有する移 動ロポッ 卜等の移動体の目標運動と目標床反力とから構成される目標歩 容を決定する目標歩容決定手段と、 前記接地部位のそれぞれに作用する 実床反力を検出または推定する床反力検出手段とを備え、 前記移動体の 歩容を目標歩容に追従させるように該移動体の動作制御を行う移動体の 制御装置において、

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

前記ッリ一構造における各ノ一ドである第 Bノードのそれぞれに対し、 該第 Bノードが前記葉ノードである場合には、 該葉ノ ドである接地部 位に作用する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 B ノードが子ノードを持つ場合には、 該第 Bノードのすべての子ノードの ノード床反力の合力を第 Bノードのノード床反力と定義し、 さらに前記 第 Aノードのノード床反力のモーメントの、 水平成分または床面平行成 分が 0となる点を第 B ノードのノード床反力中心点と定義したとき、 複数の接地部位を子孫ノードとして持つ中間ノードである所定の第 C ノードに対し、

少なくとも前記第 Cノードのノード床反力中心点の目標位置である目 標ノ一ド床反力中心点を少なくとも前記目標歩容に応じて決定する手段 と、

少なくとも前記第 C ノードのノード床反力の実際値としての実ノード 床反力を前記検出または推定された各接地部位の実床反力から決定する 手段と、 2 前記第 Cノ一ドの実ノード床反力が該第 Cノードの目標ノード床反力 中心点に作用するモーメントである実モーメント （Mc— act) と該第 C ノー ドの目標ノード床反力中^ ί!、点に作用すべきモーメントの目標値 (Mc_dmd) との差 （Mc一 act— Mc一 dmd) と、

前記第 Cノードの実モーメン ト （Mc— act) から該第 Cノードの各子 ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用 するモーメントを減じてなるモーメント （Mc— f) と該第 Cノードの目 標ノ一ド床反力中心点に作用すべきモーメントの目標値 （Mc— dmd) と の差 （Mc— f— Mc— dmd) と、

前記第 Cノ一ドの実ノード床反力のモーメントの水平成分または床面 平行成分が 0となる点の位置と該第 Cノードの目標ノ一ド床反力中心点 の位置との差の値 （Δ P ) と、

前記第 Cノードの実ノード床反力のモーメント （Mc— act) から該第 Cノードの各子ノードの実ノ一 ド床反力が該子ノードの目標ノード床反 力中心点に作用するモーメントを減じてなるモーメントの水平成分また は床面平行成分が 0となる点の位置と該第 Cノードの目標ノード床反力 中心点の位置との差の値 （Δ Ρ ί) とのうちのいずれかを制御量として、 該制御量に応じて、 該制御量が 0に近づくように、 前記目標歩容の目標 運動に、 前記第 Cノードに属する複数の接地部位の間の目標相対高さの 第 1の修正を少なくとも付加した修正目標運動を決定し、 その決定され た修正目標運動を満足するするように前記関節を動作させるノード動作 制御手段を備えたことを特徴とするものである。

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

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

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

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

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

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

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

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

なお、 前記第 7、 第 8発明では、 前記目標ノード床反力中心点を決定 する手段は、 前記第 Cノードの各子ノードの目標ノード床反力中心点の 位置の、 前記重みを用いた重み付き平均値が該第 Cノードの目標ノード 床反力中心点となるように、 該第 Cノ一ドおよびその子孫ノ一ドのそれ ぞれの目標ノード床反力中心点と重みとを決定することが好ましい （第

9発明）。

前記第 5発明では、 前記ノード動作制御手段は、 前記各第 Cノード毎 に、 該第 Cノードの各子ノードの目標ノード床反力中心点を、 少なくと も前記制御量に応じて決定した回転量だけ該第 Cノードの目標ノード床 反力中心点のまわりに回転させたときの該 Cノ一ドの各子ノードの目標 ノード床反力中心点の移動量に応じて、 該第 cノードに属する複数の接 地部位の間の目標相対高さの修正量を決定することが好ましい （第 1 0 発明）。

同様に前記第 6発明では、 前記ノード動作制御手段は、 前記第 Cノー ドの各子ノードの目標ノード床反力中心点を、 少なく とも前記制御量に 応じて決定した回転量だけ該第 Cノードの目標ノード床反力中心点のま わりに回転させたときの該第 Cノードの各子ノードの目標ノード床反力 中心点の移動量に応じて、 該第 Cノードに属する複数の接地部位の間の 目標相対高さの第 1の修正を前記目標運動に付加した前記修正目標運動 を決定 fることが好ましい （第 1 1発明）。

同様に前記第 7発明では、 前記ノード動作制御手段は、 前記第 Cノー ドの各子ノ一ドの目標ノード床反力中心点を、 少なくとも前記制御量に 応じて決定した回転量だけ該第 Cノードの目標ノード床反力中心点のま わりに回転させたときの該第 Cノードの各子ノ一ドの目標ノード床反力 中心点の移動量に応じて、 該第 Cノードの子ノ一ドの相互の相対高さを 操作することによって、 第 Cノードに属する複数の接地部位の間の目標 相対高さの第 1の修正を前記目標運動に付加した修正目標運動を決定す ることが好ましい （第 1 2発明）。

これらの第 1 0〜第 1 2発明によれば、 第 Cノードの目標高さが修正 後目標運動と目標運動とで同じになる （修正後目標運動における第 Cノ ―ドの目標高さが目標運動における目標高さに維持される） 該第 Cノー ドに属する複数の接地部位の間の相対高さの第 1の修正を決定すること を容易に行なうことができる。

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

また、 前記第 3、 第 4、 第 7、 第 8発明では、 前記各ノードの重みを 可変的に設定する手段を備え、 非接地状態である各接地部位にそれぞれ 対応する葉ノードを第 Dノードとしたとき、 前記重みを設定する手段は、 各第 Dノードの重み、 または該第 Dノ一ドの祖先ノードのうちの少なく ともいずれか 1つのノードの重みを 0に設定することが好ましい （第 1 4発明）。

この第 1 4発明によれば、 第 Cノードの各子ノードの実ノード床反力 の間の相対関係を該第 Cノードの各子ノードの目標ノード床反力の間の 相対関係に近づけ、 あるいは、 前記制御量を 0に近づけるために寄与で きない非接地状態の接地部位の高さが無駄に修正されるのを防止できる。 なお、 第 1 4発明は、 第 1 3発明と複合してもよい。

また、 前記第 1、 第 2、 第 5、 第 6発明 （あるいは第 1〜第 1 4発 明） では、 前記移動体の基体などの所定の部位の実姿勢傾斜を検出また は推定する実姿勢傾斜検出手段を備え、

前記ノード動作制御手段は、 前記修正目標運動を決定するとき、 前記 実姿勢傾斜と前記目標歩容の目標運動における前記所定の部位の目標姿 勢傾斜との偏差を 0に近づけるように、 該偏差に応じた前記第 Cノード に属する複数の接地部位の間の目標相対高さの修正を前記目標運動にさ らに付加して前記修正目標運動を決定することが好ましい （第 1 5発 明）。

この第 1 5発明によれば、 前記姿勢傾斜偏差を 0に近づけるように第 Cノードに属する複数の接地部位の間の目標相対高さの修正を目標運動 にさらに付加して前記修正目標運動を決定するので、 口ポッ トの姿勢の 安定性をさらに高めることができる。

また、 特に前記第 5発明、 第 6発明では、 前記移動体の基体などの所 定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段を備え、 前記ノード動作制御手段は、 前記第 Cノ ードの前記目標ノード床反力 中心点に作用すべきモーメントの目標値と、 該第 Cノードの前記目標ノ ―ド床反力中心点とのうちの少なくともいずれかを、 前記実姿勢傾斜と 前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿 勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0 に近づけるように修正する手 段を備え、 前記修正されたモ一メントの目標値または目標ノード床反力 中心点を、 その修正前のモーメントの目標値または目標ノード床反力中 心点の代わりに用いて前記制御量を決定するようにしてもよい （第 1 6 発明）。

この第 1 6発明によれば、 前記姿勢傾斜偏差を 0に近づけるように、 第 Cノ一ドの目標床反力中心点に作用すべきモーメントの目標値、 ある いは、 その目標床反力中心点の位置を修正した上で、 これらの修正した モーメントの目標値または目標ノード床反力中心点を、 その修正前のモ ーメントの目標値または目標ノード床反力中心点の代わりに用いて前記 制御量を 0を決定するので、 この制御量を 0に近づけるための前記第 C ノードに係る第 1の修正を目標運動に付加して修正目標運動が決定され ることとなる。 この結果、 前記姿勢傾斜偏差を 0に近づけるように移動 体に作用する床反力を適切に制御しつつ、 移動体の姿勢の安定性を高め ることができる。 なお、 この第 1 6発明は、 前記第 7〜第 1 4発明と複 合してもよい。

また、 前記第 5発明、 第 6発明では、 前記各接地部位は、 前記移動体 の基体に対して移動可能に該基体に連結機構を介して連結され、

前記ッリ一構造における子ノ一ドを有するノードである第 Eノードの それぞれに対し、 該第 Eノード床反力の目標値である目標ノード床反力 が該第 Eノードの前記目標ノード床反力中心点に作用するモ一メントの 水平成分または床面平行成分が 0になるように、 該第 Eノードに属する 各葉ノードの目標ノード床反力の並進力成分を少なくとも前記目標歩容 の目標床反力に基づき決定する目標ノード床反力分配手段を有し、 前記ノード動作制御手段は、 少なく とも前記第 Cノードに属する各葉 ノードの目標ノード床反力の並進力成分を基に、 該目標ノード床反力の 並進力成分が該第 Cノ一ドに属する各接地部位に作用したときに前記基 体と該第 Cノードに属する各接地部位との間の連結機構および該接地部 位で発生する変形量を推定する手段を備え、 前記修正目標運動を決定す るとき、 前記推定した変形量を打ち消すための、 前記第 Cノードに属す る複数の接地部位の'それぞれの目標高さの第 2の修正を前記目標運動に さらに付加して前記修正目標運動を決定することが好ましい （第 1 7発 明）。

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

また、 前記第 3、 第 4、 第 7、 第 8発明では、 前記各接地部位は、 前 記移動体の基体に対して移動可能に該基体に連結機構を介して連結され、 前記ツリー構造において親ノードを有するノ一 ドである第 Eノードの それぞれに対し、 該第 Eノード床反力の目標値である目標ノード床反力 の並進力成分を、 第 Eノードの親ノードの目標ノ 一ド床反力の並進力成 分に第 Eノードの前記重みを乗じた値になるよう に、 第 Eノードの目標 ノ一ド床反力の並進力成分を少なくとも前記目標歩容の目標床反力に基 づき決定する目標ノード床反力分配手段を有し、

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

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

補足すると、 前記第 1 7発明における目標ノード床反力分配手段と、 前記第 1 8発明における目標ノ一ド床反力分配手段とは結果的に同じよ うに葉ノードの目標ノ一ド床反力を決定することとなる。

また、 前記第 5、 第 6発明では、 前記各接地部位は、 前記移動体の基 体に対して移動可能に該基体に連結機構を介して連結され、

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定す る実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾 斜との偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけ るように少なくとも前記第 Cノードの目標ノード床反力中心点を修正す る手段と、

各ノ一ドのノード床反力の目標値である目標ノード床反力のうち、 少 なくとも前記第 Cノードの各子孫ノ一ドのノード床反力の目標値である 目標ノ一ド床反力の並進力成分を、 該第 C ノードの前記修正された目標 ノード床反力中心点に作用する目標ノード床反力のモ一メントの水平成 分または床面平行成分が 0になるように少なくとも前記目標歩容の目標 床反力に基づき決定する目標ノード床反力分配手段とを有し、

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

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

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定 する実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾 斜との偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけ るように少なくとも前記第 Cノードの目標床反力中心点に作用すべきモ ーメントの目標値を決定する手段と、

各ノードのノード床反力の目標値である目標ノー ド床反力のうち、 少 なくとも前記第 Cノードの各子孫ノードのノード床反力の目標値である 目標ノード床反力の並進力成分を、 該第 Cノードの前記目標ノード床反 力中心点に作用する目標ノード床反力のモーメントが前記決定された目 標値になるように少なくとも前記目標歩容の床反力に基づき決定する目 標ノード床反力分配手段とを有し、

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

これらの第 1 9発明、 第 2 0発明によれば、 前記第 1 6発明と同様に 前記姿勢傾斜偏差を 0に近づけるための修正を含む前記制御量を 0に近 づけるように第 Cノードに属する接地部位の間の目標相対高さの第 1の 修正が目標運動に付加されると同時に、 前記第 1 7発明と同様に前記連 結機構や接地部材の変形量を打ち消すための、 第 Cノードに属する接地 部位の目標高さの第 2の修正を目標運動さらに付加して修正目標運動が 決定される。 従って、 これらの第 1 9、 第 2 0発明によれば、 連結機構 や接地部位の変形の影響を補償し、 且つ、 前記姿勢傾斜偏差を 0に近づ けつつ、 適切な床反力を移動体に作用させることが可能となり、 移動体 の姿勢の安定性をより一層高めることができる。

また、 各ノードに重みが設定されている前記第 3、 第 4、 第 7、 第 8 発明では、 前記各接地部位が、 前記移動体の基体に対して移動可能に該 基体に連結機構を介して連結されている場合に、

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定す る実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢と の偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけるよ うに少なくとも前記第 Cノ一ドの各子孫ノ一ドの重みを修正する手段と、 各ノードのノード床反力の目標値である目標ノード床反力のうち、 少 なく とも前記第 Cノードの各子孫ノードのノード床反力の目標値である 目標ノード床反力の並進力成分を、 各子孫ノードの親ノードの目標ノー ド床反力の並進力成分に該子孫ノードの前記修正された重みを乗じた値 になるように、 少なくとも前記目標歩容の目標床反力に基づき決定する 目標ノード床反力分配手段とを有し、

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

この第 2 1発明によれば、 姿勢傾斜偏差を 0に近づけるように第 Cノ 一ドの各子孫ノードの重みを修正するので、 その重みを使用して前記第 3、 第 4、 第 7、 第 8発明の如く第 Cノードに属する接地部位の間の目 標相対高さの第 1の修正を目標運動に付加した修正目標運動を決定する ことで、 姿勢傾斜偏差を 0に近づけつつ、 適切な床反力が移動体に作用 するように修正目標運動が決定されることとなる。 同時に、、 前記第 1 8発明と同様に第 Cノードに属する接地部位の目標高さの第 2の修正を 目標運動にさらに付加して修正目標運動を決定することで、 その修正目 標運動は前記連結機構や接地部材の変形量を打ち消すように目標運動を 修正したものとなる。 従って、 第 2 1発明によれば、 前記第 1 9、 第 2 0発明と同様に、 連結機構や接地部位の変形の影響を補償し、 且つ、 前 記姿勢傾斜偏差を 0に近づけつつ、 適切な床反力を移動体に 用させる ことが可能となり、 移動体の姿勢の安定性をより一層高めることができ る。

補足すると、 第 2 1発明における目標ノード床反力分配手 と、 前記 第 1 9発明における目標ノード床反力分配手段と、 第 2 0発 B月における 目標ノード床反力分配手段とは結果的に同じように葉ノードの目標ノー ド床反力を決定することとなる。

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

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

前記第 2 2発明では、 より好ましくは、 前記ノード動作制御手段は、 前記目標歩容の目標床反力により定まる前記移動体の目標全床反力中心 点と、 前記修正目標運動における前記移動体の全体重心または該移動体 の基体の所定の代表点とを結ぶ線分の向きが、 前記目標歩容における当 該線分の向きとほぼ同じになるように前記修正目標運動を決定する （第 2 4発明）。 そして、 このことは、 前記第 2 3発明においても同じであ る （第 2 5発明）。

これらの第 2 4、 第 2 5発明によれば、 基体の目標姿勢の修正をさら に付加して目標運動を修正したときに、 移動体に作用する重力と慣性力 との合力の向きが目標歩容における適正な向きから大きくずれるような ことを防止できるので、 移動体の安定性を適正に保ちつつ、 前記修正目 標運動を決定できる。

あるいは、 前記第 2 2発明では、 前記ノード動作制御手段は、 前記修 正目標運動における前記移動体の全体重心の水平位置または該移動体の 基体の所定の代表点の水平位置が前記目標運動における当該水平位置と ほぼ同じになるように前記修正目標運動を決定するようにしてもよい (第 2 6発明）。 そして、 このことは、 前記第 2 3発明においても同じ である （第 2 7発明）。

これらの第 2 6発明、 第 2 7発明によれば、 基体の目標姿勢の第 2の 修正を目標運動に付加して修正目標運動を決定したときに、 移動体の全 体重心が、 目標歩容における適正な位置に対して水平方向に大きくずれ るようなことを防止できるので、 移動体の安定性を適正に保ちつつ、 第

Cノ一ドの接地部位の間の相対高さと基体の姿勢とを修正できる。

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

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

上記第 2 8発明では、 前記中間部位には、 その接地時に弾性変形する 弾性体が設けられていることが好ましい （第 3 0発明）。 そして、 この ことは、 上記第 2 9発明においても同じである （第 3 1発明）。

これによれば、 前記中間部位の間の相対高さの修正による床反ガの調 整能力が高まって、.移動体 （口ポッ ト） の姿勢の安定性の確保が容易に なる。

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

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

例えば 1つ以上の脚 （リンク機構） が基体 （上体など） から延設され た口ポッ トに腰掛動作を行なわせる場合、 その基体で接地することとな る。 そしてこのような場合には、 関節の自由度が不足して、 その基体で 接地することとなり、 このような場合には、 前記こじりなどの滑りが生 じる場合がある。 従って、 第 2 2発明、 第 2 3発明は、 それぞれ第 3 2 発明、 第 3 3発明の如く移動体が構成されている場合にも適している。 上記第 3 2発明では、 前記基体の接地部位には、 その接地時に弾性変 形する弾性体が設けられていることが好ましい （第 3 4発明）。 そして、 このことは上記第 3 3発明においても同じである （第 3 5発明）。

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

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

前記第 1、 第 2、 第 5、 第 6発明 （あるいは第 1〜第 3 5発明） では、 前記ツリー構造における各ノードに重みが設定されており、

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

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

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

前記ノ一ド動作制御手段は、 少なくとも前記第 Cノードの前記ノード 拡張床反力モーメント Mn_exp(n=C)またはノード拡張傾斜角 Θ n(n=C) を用いて該第 Cノ一ドに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加した前記修正目標運動を決定するように してもよい （第 3 6発明）。

上記のように定義されるノード拡張床反力モーメントゃ、 ノード拡張 傾斜角を用いることで、 第 Cノードに属する接地部位が 4つ以上である ような場合であっても、 適切に第 Cノードに属する複数の接地部位の間 の目標相対高さを修正できる。

より具体的には、 第 3 6発明では、 少なくとも前記目標歩容の目標床 反力に基づき、 少なくとも前記第 Cノードに属する各接地部位に作用す べき目標床反力を決定する手段を備え、

前記ノード動作制御手段は、 前記第 Cノ一ドに属する各接地部位の目 標床反力を基に該第 Cノードの前記ノ一ド拡張モーメントの目標値であ る目標ノード拡張モーメントを決定する手段と、 前記第 Cノ一ドに属す る各接地部位の実床反力を基に該第 Cノードの前記ノード拡張モーメン トの実際値である実ノード拡張モーメントを決定する手段と、 前記実ノ 一ド拡張モーメントと前記目標ノード拡張モーメントとに応じて、 前記 第 Cノードの前記ノード拡張傾斜角の操作量'を決定する手段とを備え、 前記決定したノード拡張傾斜角の操作量を基に、 前記第 Cノードに属す る複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付 加した前記修正目標運動を決定する （第 3 7発明）。

この第 3 7発明によれば、 前記実ノード拡張モーメントと前記目標ノ 一ド拡張モーメン卜とに応じて （例えばそれらの偏差を 0に近づけるよ うに）、 前記第 Cノードの前記ノード拡張傾斜角の操作量を決定したと き、 その操作量は第 Cノードの子ノ一ドの相対高さの操作量を表すもの となる。 従って、 このノード拡張傾斜角の操作量を基に、 第 Cノ一ドに 属する複数の接地部位の間の目標相対高さの第 1の修正を目標運動に付 加して修正目標運動を決定することを適切に行なうことができる。

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

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

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

この第 3 8発明では、 少なくとも前記第 Cノ一ドの各子ノードのノ一 ド床反力の実際値である実ノード床反力を該第 Cノ一ドに属する各接地 部位の実床反力から決定する手段を備え、

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

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

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

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

これらの第 3 9発明、 第 4 0発明によれば、 前記床形状パラメ一夕を リアルタイムで逐次更新しながら、 適切に推定できる。 補足すると、 第

4 0発明では、 前記連結機構は、 その全てが必ずしも関節を備えている 必要はない。

なお、 第 4 0発明では、 例えば前記変形量検出手段は、 各接地部位の 実床反力に基づき前記変形量を推定する （第 4 1発明）。

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

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

前記床形状推定手段が推定する前記床形状パラメ一夕は、 前記第 Cノ ―ドの複数の各子ノ一ドの接地面の高さの間の相対関係を規定するパラ メータであることが好ましい （第 4 2発明）。

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

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

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

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

また、 前記第 3 8発明 （あるいは第 3 8〜第 4 4発明） では、 前記ッ リ一構造における各ノードに重みが設定されており、

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

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

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

前記床形状推定手段は、 少なくとも前記第 Cノードの前記ノード拡張 床反力モーメント Mn— exp(n=C)またはノード拡張傾斜角 Θ n(n=C)を用 いて前記床形状パラメータを推定するようにしてもよい （第 4 5発明）。

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

前記床形状パラメ一夕に、 前記第 Cノードの各子ノードの接地面の相 対高さを前記ノード拡張傾斜角を用いて表したパラメ一夕が含まれるこ とが好ましい （第 4 6発明）。 これによれば、 第 Cノードに属する接地部位が 4つ以上であっても、 前記ノード拡張傾斜角を用いて、 床形状パラメ一夕を適切に推定できる。

また、 前記第 4 5発明では、 少なくとも前記目標歩容の目標床反力に 基づき、 少なくとも前記第 Cノードに属する各接地部位に作用すべき目 標床反力を決定する手段を備え、

前記ノード動作制御手段は、 前記第 Cノードに属する各接地部位の目 標床反力を基に該第 Cノードの前記ノード拡張モーメントの目標値であ る目標ノード拡張モーメントを決定する手段と、 前記第 Cノードに属す る各接地部位の実床反力を基に該第 cノードの前記ノード拡張モーメン トの実際値である実ノード拡張モーメントを決定する手段と、 前記実ノ 一ド拡張モーメントと前記目標ノード拡張モーメントと応じて、 前記第

Cノードの前記ノード拡張傾斜角の操作量を決定する手段とを備え、 前 記決定したノード拡張傾斜角の操作量を基に、 前記第 Cノードに属する 複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加 した前記修正目標運動を決定することが好ましい （第 4 7発明）。

これによれば、 第 Cノードに属する接地部位が 4つ以上の場合でも、 ノ一ド拡張傾斜角とノード拡張モーメントを使用した床反力の制御を適 切に行いながら、 床形状パラメ一夕を推定できる。

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

図 1は、 本発明の第 1実施形態および第 2実施形態に係るロポッ 卜の 全体外観図、 図 2は第 1実施形態の制御装置の機能的構成を示すブロッ ク図、 図 3 ( a) 〜 （ c ) および図 4 ( a) 〜 （ c ) は、 第 1実施形態 の口ポッ ト （4脚口ポッ ト） の動作を説明するための図、 図 5 ( a) 〜 ( c ) は第 1実施形態における重みの設定例を示すグラフ、 図 5 (d) は第 1実施形態における Z MP (目標全床反力中心点） の設定例を示す グラフである。 図 6は第 1実施形態のノードの階層構造を示す図、 図 7 は目標ノード床反力中心点と重みとの関係を示す図、 図 8は目標ノード 床反力と重みとの関係を示す図、 図 9は第 1実施形態の制御装置のメイ ンルーチン処理を示すフローチャートである。 図 1 0〜図 1 4はそれぞ れ第 1実施形態のロボッ ト （4脚口ポッ ト） の目標ノード床反力並進力 成分、 実ノード床反力並進力成分、 実ノード床反力モーメント、 補償全 床反力モーメント、 ノ一ド補償床反カモ一メントを例示する図である。 図 1 5および図 1 6は第 1実施形態におけるノード補償角による位置修 正 （コンプライアンス動作） を説明するための図、 図 1 7〜図 2 1はそ れぞれ第 1実施形態における階層型コンプライアンス動作決定部、 補寧 全床反力モーメント分配器、 01423 決定部 （補償角決定部）、 Θ 14 決 定部 （補償角決定部）、 機構変形補償量算出部の機能的構成を示すプロ ック図、 図 2 2は補償角を決定する処理を示すフローチャート、 図 2 3 は第 1実施形態のロボッ トに係る階層構造の他の例を説明するための図 である。 図 24は第 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 決定部 （補償角決定部）、 Θ 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 接地部位とは、 このようなルールの基で分類 （区別） されて識 別番号 η ( η=1，2,3 ·· ) を割り振られた接地部位の識別名称である。

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

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

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

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

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

後述するコンプライアンス制御 （床反力制御） の全床反力 （ロボッ ト のすベての接地部位に作用する実床反力の合力） の目標を目標全床反力 と呼ぶ。 目標全床反力のモーメント水平成分が 0 となる点を目標全床反 力中心点と呼ぶ。 4 移動ロボッ トの目標歩容の運動 （目標歩容におけるロボッ トの各部位 の運動） が発生する慣性力と該ロボットに作用する重力との合力のモー メント水平成分が 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 と称し ている。 ここで、 前記図 1に示したように、 口ポッ ト 1のおおよそ前方 向を X軸、 おおよそ左横方向を Y軸、 上方向を Z軸とした、 床に固定さ れた所定の座標系 （X Y Z座標系） を 「支持脚座標系」 または 「グロ一 バル座標系」 と呼ぶ。 以降、 特に断らない限り、 位置、 姿勢、 並進力お よびモーメントはこの支持脚座標系 （グローバル座標系） の座標成分の 組によつそ表現されるものとする。 支持脚座標系 （グローバル座標系） の原点は、 定常的に単一の点に固定されている必要はなく、 ロボッ ト 1 の移動途中で床に対する原点位置を変更してもよい。 例えば、 ロボッ ト 1の所定の脚が着地する毎に支持脚座標系 （グローバル座標系） の原点 位置を変更してもよい。

図 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を備え ている。 以下に、 制御装置 5 0のこれらの要素の概要を説明する。

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

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

また、 歩容生成装置 1 0 0が決定する目標上体位置姿勢軌道は、 少な くとも、 口ポッ ト 1の目標運動によって発生する慣性力とロボッ ト 1に 作用する重力との合力が目標 Z M Pまわりに作用するモーメン卜の水平 成分が 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 2には、 歩容生成装 置 1 0 0で決定された目標歩容のうち、 目標全床反力中心点軌道、 目標 全床反力軌道および目標接地部位軌道とが入力される。 なお、 歩容生成 装置 1 0 0が目標歩容を決定するために使用した歩容パラメ一夕 （ロボ ッ ト 1の遊脚の接地部位 1 0の着地予定位置、 着地予定時刻など） も目 標床反力分配器 1 0 2に入力してもよい。 そして、 目標床反力分配器 1 0 2は、 これらの入力を基に、 上記のように階層化された各ノードの床 反力中心点の目標位置である目標ノード床反力中心点 （目標第 nノード 床反力中心点） Qn (n=l, 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の目標接地部位位置から一義的に定まる。 逆に言えば、 葉ノード の目標ノード床反力中心点 (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 というように表す。 また、 演算子 「 *」 は、 スカラーとスカラーと の組もしくはスカラーとベク トルとの組に対しては、 乗算を意味し、 ベ ク トルとベク トルとの組に対しては、 外積 （ベクトル積） を意味する。 根ノード以外の各ノードの重み Wn ( n=l, 2, 3, 4, 14,23) と各中間ノー ドの目標ノード床反力中心点 Q14， Q23 とは、 目標第 14ノード床反力 中心点 Q14が、 Q1Q14： Q14Q4= W4： Wl = (l— W1)： W1 という関係 式を満たす線分 Q 1Q4 の内分点、 目標第 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) が決定される。 なお、 これらの式 1〜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 (

) と重み 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) を決定すると、 式 1〜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 が連続的に 変化するように設定されるので、 目標ノード床反力中心点 Q14， Q23 も連続的に移動するように決定される。 つまり、 すべての目標ノード床 反力中心点 （目標第 n ノード床反力中心点 （n=l，2，3，4,14，23,1423)) は、 連続的に移動するように設定される。

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

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

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

= nノー卜直み

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

*目標全床反力 …式 4 a

目標第 nノ一ド床反力

=第 11ノード重み

*第 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= 1 ) とされ、 また、 第 2 接地部位 1 0および第 3接地部位 1 0が遊脚となって離床する時期では、 重み W23 が 0 (W14= 1 ) とされる。 このように、 子ノードを持つ中 間ノードの重みは、 その中間ノードに葉ノードとして属する全ての接地 部位が接地していない時期では 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 ( = 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ノード (n= 14,23，1423)の目標ノード床反力中心点 は、 その第 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であるような場合には、 0 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) が入力 eれる。

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

Mdmdx および Mdmdy は、 具体的には、 例えば次式 5， 6のフィ一 ドパック ！!御則 （ここでは P D制御則） によって決定される。 すなわち、 Mdmdxおよび Mdmdy は、 上体姿勢傾斜偏差 0 berrx， Θ berry を 0に 近づけるように決定される。 Mdmdx= — Kthx * 0 berrx- Kwx * (d 0 berrx/dt) …式 5

Mdmdy = ― Kthy * Θ berry - Kwy * (d 0 berry/dt) …式 6 ここで、 Kthx, Kthy, Kwx および Kwy は、 所定のゲインである。 また、 （ d Θ berrx/dt), ( d Θ berry/dt) は、 それぞれ上体姿勢傾斜偏 差 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は、 目標全床反力と補償全 床反力との合力に実全床反力を近づけるように目標接地部位軌道を修正 し、 その修 IE後の目標接地部位軌道である機構変形補償付き修正目標接 地部位位置姿勢軌道を出力するものである。 なお、 本実施形態では、 各 接地部位 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の制 御周期毎のタイマ害 [Jり込みを待つ状態となる。 制御周期は例えば 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に進み、 上記歩容パラメ一夕を基に、 目標歩容の瞬時値 を決定する。 ここで 「瞬時値」 は制御周期ごとの値を意味し、 目標歩容 瞬時値は、 目標上体位置姿勢、 目標接地部位位置 （目標接地部位軌道の 瞬時値）、 目標全床反力、 および目標全床反力中心点ィ i置 （目標 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) と各 ノ ー ド の 目 標 床反 力 中 心点 （ 目 標 ノ ー ド 床反 力 中 心点） Qn(n=l，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に示す。 この図において、 べク ト ル Fn— ref (n=l,2, 3, 4,14,23) が目標第 nノ一ド床反力の並進力成分を 表している。 また、 Ftotalref は目標全床反力 （=目標第 1423 ノード 床反力） の並進力成分を表す。 前記図 8に示したように、 葉ノードでな い任意の第 nノー ド（n=14，23,1423)の目標第 nノード床反力は、 第 nノ ードのすベての子ノードの目標ノード床反力の合力に決定される。 従つ て、 F14ref = Flref + F4ref、 F23ref= F2ref + F3ref Ftotalref( = F1423ref) = 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では、 各接地部位 1 0 (各葉ノード） の目標ノード 床反力のモーメン ト鉛直成分も 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(= F1423 act) = 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ノード床反力のモーメント成分を表す。 こ の場合、 各ノードの実床反力の並進力成分と同様に、 一般的には、 葉ノ ー ドでないノー ドの実床反力のモーメン ト成分 Ml4act、 M23act、 M1423act ( = Mtotalact) は、 そのノードのすべての子ノードの実床反 力の合力のモーメント成分 （そのノードの目標床反力中心点 Q 14, Q23, Q1423を作用点とするモーメント成分） とする。

本実施形態のロボッ ト 1 においては、 前記したように各接地部位 1 0

(各葉ノード） に床反力モーメントを発生させることができないので、 各葉ノードの実床反力 （実接地部位床反力） のモーメント成分は、 常に

0になる。 よって、 各葉ノードの実ノ一ド床反力のモーメント成分の図 示は省略する。

葉ノードでないノードの実床反力モーメント成分 （本実施形態では、 M14act , M23act , M 1423act) は、 一般的に、 0にならない。 例えば 目標第 14ノ一ド床反力中心点 Q 14のまわりには、 一般には、 第 1接地 部位 1 0 (第 1ノード） および第 4接地部位 1 0 (第 4ノード） の実床 反力の並進力成分の水平成分によってモーメントが発生する。 ただし、 本実施形態のロボッ ト 1においては、 脚の先端部にフリージョイントで ある球関節 1 2を備えているので、 M14act の線分 Q1Q4 と同方向の成 分、 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 ( = Ml423ref) と補償全床反力モ一メ ント 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 ノード補償角 0 14、 第 23 ノード補償角 0 23、 第 1423 ノード補償角 0 1423 がある。 すなわち、 葉ノード以外のノードの補償角である。 そして、 階層型コンプライアン ス動作決定部 1 1 4は、 これら の補償角 0 14, Θ 23, Θ 1423 を基に、 目標第 nノード床反力中心点 Qn ( n=l, 2, 3, 4, 14,23) を修正してなる目 標第 nノ一ド床反力中心点 Qn'を決定する。

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

図 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ノード 床反力中心点 Q1 の座標 （位置） を、 目標第 14 ノード床皮力中心点 Q1 を回転中心として法線べク トル V14まわり （Q14を通って V14に 平行な軸まわり） に、 前記第 14ノード補償角 0 14だけ回転移動する。 この回転移動による Q1 の移動後の点を Q1'とする。 同様に、 目標第 4 ノード床反力中心点 Q4 の座標 （位置） を、 目標第 14 ノード床反力中 心点を回転中心として法線べク トル V14まわりに第 14ノ一ド補償角 Θ 14だけ回転移動する。 この回転移動による Q4の移動後の点を Q4'とす る。 換言すれば、 線分 Q 1Q4 をその内分点である Q14 を回転中心とし て V14まわりに 0 14だけ回転させて得られる線分の端点を Q l'， Q4'と する。 従って、 第 14 ノー ド補償角 0 14は、 第 14 ノードの目標床反力 中心点 Q 14の位置を動かさずに、 第 14ノードの子ノードである第 1 ノ ードおよび第 4ノードのそれぞれの目標床反力中心点 Ql， Q4 の位置 の相対関係を動かすための操作量である。

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

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

次に、 Q1'をベク トル Q14— Q 14'だけ移動した点 Q 1"を求める。 同様 に、 Q4'をベク トル Q14— Q14'だけ移動した点 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軸） まわりに、 ある回転角 0 n— Xだけ回 転させると共に、 左右方向軸 （Y軸） まわりに、 ある回転角 0 n_ yだ け回転させる。 これにより各接地部位の目標姿勢を修正する。 この場合 の回転角 0 n ( 0 n— x、 0 n_y からなる 2次元量） を第 n接地部位補償 角と呼び、 0 n_ xを第 n接地部位補償角 X成分、 θ η一 yを第 n接地部位 補償角 Y成分と呼ぶ。 第 n接地部位補償角 0 η は、 本願出願人が先に提 案した特開平 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 1ιを備えている。

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

第 1423 ノード補償床反力モー ント Ml423dmd の前記ベク トル 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補償角 Θ 14を 操作することによって発生する各接地部位 1 0 (詳しくは第 1および第 4接地部位） の床反力の並進力成分が、 目標第 14 ノード床反力中心点 まわりに発生すべきモーメントの目標値である。 第 14 ノード補償'床反力モーメント M14din d のべク トル 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補償角 0 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ノード補償角 6· 23を 操作しても、 床反力モーメントの U23 方向成分を発生することはでき ないからである。 なお、 本実施形態では、 M23dmd の鉛直方向成分も 0に設定される。 .

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

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

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

M1423dmd= (Pmdfd一 P) * Ftotalref •式 Ί

M14dmd= (Q 14mdfd - Q14) * Fl4ref ··式 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 の端点に近づき過ぎ 8 ず、 かつ、 線分 Q 14Q23上にあること。 このように、 修正目標第 1423 ノード床反力中心点 （修正目標全床反力中心点） Pmdfd が存在すべき 範囲を、 修正目標第 1423 ノード床反力中心点 （修正目標全床反力中心 点） の存在許容範囲と呼ぶ。

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

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

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

Q23mdfdは、 目標第 23 ノード床反力中心点 Q23から線分 Q2Q3 の 端点に近づき過ぎず、 かつ、 線分 Q2Q3 上にあること。 このように、 修正目標第 23 ノード床反力中心点 Q23mdfd が存在すべき範囲を、 修 正目標第 23ノード床反力中心点の存在許容範囲と呼ぶ。 一方、 ロボッ ト 1に実際に発生する姿勢復元力 （上体 2 4の傾きの、 目標上体姿勢の傾きへの復元力） を適切にするためには、 第 1423 ノー ド補償床反カモ一メント M1423dmd、 第 14 ノード補償床反力モーメン ト M14dmdおよび第 23 ノード補償床反力モーメント M23dmd の合力 が、 補償全床反力モーメント Mdmd に略一致すべきである。 すなわち、 以下の式 1 0を略満足すべきである。

Mdmd = M1423dmd+M14dmd+M23dnid …式 1 0 そこで、 本実施形態では、 第 1423 ノード補償床反力モーメン ト M1423dmd、 第 14 ノード補償床反力モ一メント Ml4dmdおよび第 23 ノード補償床反力モーメント M23dmd は、 それらと前記式 7、 式 8 、 式 9とによって定まる修正目標ノード床反力中心点 Prndfd, Q14mdfd, Q23mdfd の位置が前記修正ノード存在位置条件 1 )、 2 ) および 3 ) を 満足している限りは、 次式 1 1 〜 1 3によって決定される。

M1423dmd= Matl423 * Mdmd * V1423 …式 1 1

M14dmd = 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 ノ一ド補償床反カモ一メント 23dmd が修正ノ ード存在位置条件 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、 M14dmd、 M23dmd を決定する。 図 1 4には、 このようにして決定された各ノード補償床反カモ一メント M1423dmd、 M14dmd, 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、 （¾23 と目標全床反力 Ftotalref とから、 各目標ノー ド床反力を求める決定する手法と同様の手法によって、 各接地部位 1 0 の目標床反力中心点 Qn(n=l，2,3，4)、 修正全床反力中心点 Pmdfd、 各修 正目標ノ一 ド床反力中心点 Q14mdfd、 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に示す機能ブロック図により表される。 すなわち、 目標ゾ 一ド床反力 中心点（¾11(11=1，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) お よ び 目 標 床 反 力

4, 14,23, 1423)と、 上記存在許容範囲とを基に、 修正目 標ノ一ド床反力中心点 Qn—mdfd ( n=14, 23, 1423)およびノ一ド補償床反 力モーメント Mn— dmd(n=14，23, 1423)が決定される。 また、 修正目標 ノード床反力中心点 Qn— mdfd(n=14，23, 1423)と、 各接地き 15位 1 0の目 標床反力中心点 Qn(n=l，2, 3，4)と、 目標全床反力 Ftotalref とから、 修 正目標接地部位床反力 Fn— refmdfd ( n=l，2，3,4) が求められる。

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

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

なお、 式 1 4の代わりに、 次式 1 5を用いて M1423actを算出しても 実際上はほとんど問題がない。

Ml423act=P_Ql* Flact+P_Q2* F2act

+ P_Q3 * F3act + P_Q4 * F4act

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

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

Mn— act =∑ (Qn— Qm * Fm— act) …式 1 6 m e {第 nノードの子孫である葉ノードの番号の集合 } であるとすると 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にならな い。 第 1423 ノード補償角 0 1423 は、 一般的には、 上記のように求め ら れる M1423actと先に補償全床反力モーメント分配器 1 1 4 aで求め ら れた第 1423 ノ一 ド補償床反力モーメ ン ト M1423dmd との偏 差 ( M1423act - M1423dmd) を 0に近づけるようにフィードバック制御 則などにより決定すればよい。 例えば該偏差に所定のゲイン行列 （ 3 次 の対角行列） を乗算することで 0 1423を求めればよい。

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

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

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

次に、 上記 M1423actv をローパスフィ ルタ 1 1 4 i に通し、 M1423actvfilt を得る。 また、 前記第 1423 ノード補償床反力モーメン ト M1423dmdの V1423方向の成分 M1423dmdvを補償フィル夕 1 1 4 j に通し、 M1423dnidvfilt を得る。 なお、 M1423dmdv は、 前記式 1 8と同様に、 M1423dmd と V1423 との内積演算により求められる。 そ して、 M1423dmdvfilt を M1423actvfilt から減じたものを、 偏差モ一 メント V1423方向成分 M1423errvとして得る。

尚、 前記補償フィル夕 1 1 4 j は、 制御系における M1423dmdv か ら実全床反力モーメントまでの伝達関数の周波数応答特性を改善するも のである。

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

Θ 1423 = K1423 * M1423errv …式 1 9 すなわち、 偏差モーメント V1423 方向成分 M1423errv に (I御ゲイン K1423を乗じることにより、 第 1423ノ一ド補償角 0 1423を得る。

次に、 図 1 7の捕償角決定部 （ Θ 14 決定部） 1 1 4 cの処理を、 図 2 0を参照して説明すると、 目標第 1 ノード床反力中心点 Q1 に実第 1 ノード床反力 （第 1接地部位 1 0の実床反力） の並進力成分 Flact が 作用すると共に、 目標第 4ノード床反力中心点 Q4 に実第 4ノード床反 力 （第 4接地部位 1 0の実床反力） の並進力成分 F4act と作用した場 合にそれらの並進力成分が、 目標第 1 4ノード床反力中心点 Q 14 のま わりに発生するモ一メント M14actを、 一般式である前記式 1 6を用い て求める。

より具体的には、 次式 2 0により求める。 M14act=Q14_Ql* Flact+Q14_Q4* F4act …式 2 0 ここで、 Q14— Q1 は始点が Q14、 終点が Q1 のべク トル、 Q14— Q4は始 点が（¾14、 終点が Q4のベクトルである。

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

M14act=Q14_Ql* Flact+Q14_Q4* F4act + Mlact+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 ノード補償角 014 は、 一般的には、 上記のように求められる M14act と先に補償全床反カモ一メント分配器 1 1 4 aで求められた第

14 ノー ド補償床反力モーメン ト M1423dmd との偏差 （Ml4act— M14dmd) を 0に近づけるようにフィードバック制御則などにより求め ればよい。

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

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

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

次に、 上記 M14actv を口一パスフィ ルタ 1 1 4 i ' に通し、 M14actvfilt を得る。 また、 前記第 14 ノード補償床反力モーメント M14dmd の V14方向の成分 Ml4dmdv を捕償フィル夕 1 1 4 j ， に通 し、 Ml4dmdvfilt を得る。 なお、 M14dmdv は、 M14dmd と V14 との 内積演算により求められる。 そして、 M14dmdvfilt を、 M14actvfilt力 ら減じたものを、 偏差モ一メント V14方向成分 M14errv として得る。 尚、 補償フィルタ 1 1 4 j ' は、 制御系における M14dmdvから実全床 反カモ一メントまでの伝達関数の周波数応答特性を改善するものである。 最後に次式 2 3のフィードバック制御則 （ここでは比例制御則） の演 算によって前記第 14 ノード補償角 Θ 14 を得る。 ここで K14 は制御ゲ インであり、 通常、 これは正の値に設定する。 θ 14= K14 * M14errv …式 2 3 図 1 7の補償角決定部 （ Θ 23決定部） 1 1 4 dの処理は、 第 14ノー ド補償角 θ 14 決定部 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 1423， Θ 14, Θ 23 を決定するよ う に して も よ い。 すなわち 、 子 ノ ー ド を持つ各第 n ノ ー ド (n=14，23，1423)に対し、 その第 nノードの実ノード床反力 （第 nノード の全ての子ノードの実ノード床反力の合力） のモ一メン卜の水平成分が 0となるような床反力中心点を実第 nノード床反力中心点として求める。 あるいは、 第 nノード（n=14,23，1423)の実ノード床反力のモーメントか ら、 その各子ノードの実ノード床反力が該子ノードの目標ノード床反力 中心点に作用するモーメントを減じてなるモーメントの水平成分が 0と なるような床反力中心点を実第 nノード床反力中心点として求める。 例 えば第 14 ノードに関する実第 14 ノ一ド床反力中心点は、 前記式 1 6 または式 1 7により求めた M14actの水平成分を、 Flact と F4actとの 合力 （実第 14 ノード床反力の並進力成分） の鉛直成分で割った値だけ、 目標第 14 ノード床反力中心点を線分 Q1Q4上でずらした点として求め られる。 第 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 ηは、 前記補償全床反カモ一メント分配器 1 1 4 aから出力される 各接地部位 1 0の修正目標接地部位床反力 Fn_refmdfd (n=l,2, 3，4) (目 標接地部位床反力に、 第 1423ノ一ド補償床反力モ一メント M1423dmd、 第 14ノ一ド補償床反カモ一メント M14dmdおよび第 23 ノード補償床 反力モーメント M23dmd を付加することによって修正された目標接地 部位床反力） によって発生が予想される各脚 # 1〜# 4の変形量 （各脚 のリンク機構およびコンプライアンス機構 4 2の変形量） である機構変 形量

，2，3，4)を求める。 機構変形量 En— mdfd(n=l,2，3，4)は、 各脚に作用する力 （あるいは床反力） と該脚の変形との関係を表す機構 コンプライアンスモデルを用いて求められる。 そして、 機構変形補償量 算出部 1 1 4 nは、 機構変形量 En— mdfd(n=l,2, 3，4)を打ち消すための 機構変形補償量 En— cmpn(n=l，2，3,4)を求める。 機構変形補償量 En一 cmpn は、 各機構変形量に En— 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 ノード床反力の並進力成分？11—& (11=1，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 1 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 1 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 4に進み、 前記姿勢安定化制御演算部 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は V nに直交していればよい。

次いで S 1 0 8 に進み、 実第 n ノ一ド床反力モーメント Mn一 act ( n=14,23，1423)の Vn方向成分 Mn— actv と Un方向成分 Mn— actu と を抽出する。 これは、 Mn— act と V n、 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 と V n、 Un との内積演算を行なえばよ い。 なお、 第 1実施形態のように葉ノードでない任意の第 nノードの子 ノードの数が 2以下の場合には、 第 n ノード補償床反力モーメント Mn— dmdの Un方向成分 Mn— 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、 6» 14、 Θ 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に進んで、 時刻を Δ 1; (制御装置 5 0の演算処理周期） だけ更新し、 S 1 4に戻って上記の処理を繰り返す。

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

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

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

また、 上記と同じ理由から、 コンプライアンス制御においては、 ロボ ッ ト全体の傾き角偏差 Θ berr の変化速度 d 0 berr/dt とこれに応じて発 生するモーメントの増加量 Δ Μ—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を求める。

Z14= -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 + Ζ23 …式 3 0

Z3=Q23Q3* θ 23 + Z23 …式 3 1 但し、 ここで、 014 には、 前記式 2 3で求められる値を代入し、 023 には、 Θ 14と同様にして求められる値を代入する。 以上のように求めた 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は、 両者の組を着地させた状態 （両者の組を支持脚とした状態） を示 しており、 図中の参照符号 Qn(n=l,2,3,4，5，6)を付した三角形が各接地 部位 1 0の位置に対応し Tいる。

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

第 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の各 P節 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, 0236を決定するものである。

以下に、 第 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= F2i'ef+ F3ref+ F6ref)。 この場合、 中間ノードの親ノードは 根ノードであるので、 F145ref+ 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) は、 そのノードの各子ノードの実ノード床反力の合力の並進力成分に決 定される。 従って、 F145act = 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 ノード補償床反力モーメント Ml45dmdおよ び第 236 ノ一ド補償床反力モーメント M236dmd に分配するものであ る。

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

第 145 ノード補償床反力モーメント M145dmd は、 第 145 補償角 Θ 145 を操作することによって （第 145 ノードに属する第 1、 第 4およ び第 5接地部位 1 0を目標第 145 ノード床反力中心点 Q 145 まわりに Θ 145 だけ回転させることによって） 発生する各接地部位 1 0 (詳しく は第 1、 第 4、 および第 5接地部位） の床反力の並進力成分が、 目標第 145 ノード床反力中心点まわりに発生すべきモ一メントの目標値である。 第 236 ノード補償床反力モーメント M236dmd は、 第 236補償角 Θ 236 を操作することによって （第 236 ノードに属する第 2、 第 3およ び第 6接地部位 1 0を目標第 236 ノ一ド床反力中心点 Q236 まわりに 0 236 だけ回転させることによって） 発生する各接地部位 1 0 (詳しく は第 2、 第 3、 および第 6接地部位） の床反力の並進力成分が、 目標第 236 ノ一ド床反力中心点まわりに発生すべきモーメントの目標値である。

なお、 本実施形態では、 第 1実施形態と同様、 口ポッ ト 1の鉛直軸ま わりの姿勢制御を行なわないので （補償全床反力モーメント Mdmd の 鉛直軸回 り の成分が 0 であるので）、 M145236dmd、 M145dmd、 M236dmd は、 いずれも鉛直軸回りの成分が 0であるモーメント （水平 べク トル) であり、 補償角 0 145236、 0 145、 0 236 は、 水平な軸回り の回転角である。 また、 特に、 M145236dmd は、 線分 Q145Q236 に垂 直な水平軸回りのモ一メントである。

これらのノード補償床反力モーメント M145236dmd， M145dmd, M236dmd は、 基本的には、 次の条件 1 1 )、 1 2 ) を満たすように決 定される。 1 1 ) 第 145 ノード、 第 236 ノード、 第 145236 ノ一ドに対して、 前 記第 1実施形態で定義した如く、 次式 7 a〜9 aの関係式を満たす修正 目 標 ノ ー ド 床 反 力 中 心 点 Q145mdfd 、 Q236mdfd 、 Pmdfd (=Q145236mdfd)を導入したとき、 これらの Q145mdfd、 Q236mdfd、 Pmdfdは、 それぞれの所定の存在許容範囲内に在る。 M145236dmd= (Pmdfd- P) * Ftotalref …式 7 a

M145dmd= (Q145mdfd- Q145) * F145ref …式 8 a

M236dmd=(Q236mdfd-Q236) * F236ref …式 9 a

Q145mdfd, Q236mdfd, Pmdfd の存在可能範囲は、 例えば第 2実施 形態のロボッ ト 1の全ての接地部位 1 0が接地している状態では、 図 2 9 ( a) に示す如く設定される。 すなわち、 Q145mdfdの存在許容範囲 は、 図中の太線の三角形上の領域 （その三角形の辺および内部の領域） であり、 これは、 第 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 ) を満たすように 1

M145236dmd, M145dmd, M236dmdを決定する。

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

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

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

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

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

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

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

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

補償角決定部 （ Θ 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) に所定のゲイン行列 K145 (対角行列） を乗算すること で、 第 145 ノ一ド補償角 Θ 145 が決定される。 Θ 145 は、 ベク トル V の軸回り角度成分とべク トル U の軸回りの角度成分とから構成される。 補償角決定部 （ 0 236 決定部） 1 1 4 dによる第 236 ノード補償角 Θ 236 の決定処理も、 上記した Θ 145決定部 1 1 4 cの処理と同様に行 なわれる。

以上説明した補償角決定部 1 1 4 b〜 1 1 4 dの処理によって、 目標 全床反力中心点 Pに作用する実モ一メント Mact の水平成分を補償全床 反力モーメント Mdmd に近づけるようにノ一ド補償角 0 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に近づけるように、 該制御量に応じてノード補償角 Θ 145236 , Θ 145, Θ 236 を決定する （例えば制御量にあるゲインを乗算すること で、 Θ 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 ノード補償角 Θ 145 (水平べク ト ル） だけ回転移動する。 この回転移動後の Ql , Q4 , Q5 をそれぞれ Q l', Q4", Q5'とする。 従って、 第 145 ノード補償角 Θ 145 は、 第 145 ノードの目標床反力中心点 Q 145 の位置を動かさずに、 第 145 ノード の子ノードである第 1、 第 4、 第 5ノードのそれぞれの目標床反力中心 点 <¾1， Q4, Q4の位置の相対関係を動かすための操作量である。

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

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

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

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

最後に、 第 n接地部位 （n=l,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などの変形による影響を補償するための機構変形補償量 En_cmpn(n=l，2, 3,4, 5,6)を求める。 すなわち、 第 2実施形態における補 償全床反力モーメント分配器 1 1 4 aから出力される各接地部位 1 0の 修正目標接地部位床反力 Fn— refnidfd(n=l，2, 3,4，5，6,) (目標接地部位床 反力に、 各ノード補償床反力モーメント M145236dmd、 M145dmd, M236dmd を付加することによって修正された目標接地部位床反力） に よって発生が予想される各脚 # 1〜# 6のコンプライアンス機構 4 2な どの機構変形量 ∑11—1₁1(1£ (11=1,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実施形態で説明した各補償角 0 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は、 該第 な接地部位の接地面 （底面） に設定されていてもよい。 この場合、 目標 1.2 1 歩容において、 目標第 n接地部位床反力中心点 Q nと接することが想定 された想定床面上の点を 「想定第 n床接点 D n」 と呼ぶ。

この定義から明らかなように、 ロポッ 卜の目標歩容において第 n接地 部位が接地している時期では、 目標第 n接地部位床反力中心点 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が前記した如く決定した前述の重みと同一のものとする。 定義 ：

すべての目標接地部位の高さと傾斜を、 それぞれ対応する実床面の高さ と傾斜に一致させ、 かつ、 任意のノードの目標床反力中心点をそのすベ ての子ノードの目標床反力中心点の所定の重み付き平均 （すなわち所定 の内分比による内分点） で表したときに、 任意の第 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))。

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 だけ回転移動させることにより、 グロ一パル座標系 (支持脚座標系） から見た機構変形後の第 n 推定接地部位位置姿勢 (η= 1, 2，···，最終葉ノード番号）を求める。 次いで、 グローバル座標系から 見た機構変形後の第 n 推定接地部位位置姿勢 (n= l , 2,…，最終葉ノード番 号）と第 n接地部位の目標接地部位位置 （第 n接地部位の代表点の目標 位置） から見た目標第 n 接地部位床反力中心点の位置とを基に、 グロ 一バル座標系から見た機構変形後の目標第 n 接地部位床反力中心点に 対応する点の位置を'求める。 そして、 この求めた点の位置を瞬間推定床 接点位置 （瞬間推定第 n床接点位置） 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— withjbias'は、 第 n接地部位床高 さ偏差の瞬時推定値に相当するものであるが、 本実施形態のように上体 の実高さを検出しない場合には、 一般にバイアス誤差を含んでいる。 そ のため、 Zfn一 with— bias'をバイアス含有瞬間第 n接地部位床高さ偏差と 呼ぶ。

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

次 tゝ で 、 バ イ ア ス 含 有 瞬 間 第 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_witii— bias の重み付き平均を求め、 これをもってバイァ ス含有第 n ノード高さ Zn— with一 bias とする。 ただし、 各子ノードに対 する重みには、 前記目標床反力分配器 1 0 2が決定した重み Wj (j=l，2， …；)を用いる。

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

Zn— with— bias = Zfn

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

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

ただし、 ∑は ； )' £ {第！1ノードの子ノード番号の集合 }である j につ いての総和。

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

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

すなわち、 次式 3 3により第 n ノード相対高さ Zn_relを求める。 た だし、 第 hノードは、 第 nノードの親ノードであるとする。 Zn rel= Zn with bias― Zh with bias …式 ό ό 以上のルールに従って、 すべてのノードに対してノード相対床高さ 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ノードが葉ノードでないならば、

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_rel' ( n=l，2,…，最終ノード番号）

を求める。

次いで、 S 5 2において、 制御装置 5 0の前回.制御周期で求めた、 第 n接地部位床高さ偏差の推定値である推定第 n 接地部位床高さ偏差 (以降、 これを前回推定第 ΪΙ接地部位床高さ偏差 Zfn— estm— p と呼ぶ） を階層相対化して得られる出力を前回推定第 n ノード相対床高さ偏差 Zn— rel— 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 ノード相対床高さ偏差 Zn— rel'と前回推 定第 n ノー ド相対床高さ偏差 Zn_rel— estm— p の差と所定の係数（△ T/(Testm + Δ Τ))との積に設定される。

Zn_mc_cand = (Zn— rel'一 Zn_rel_estm_p)

* A T/(Testm + Δ T)

…式 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 においては、 第 11 ノード要求モー ド （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は、 接地部位が接地して いる状態が O Nに対応しており、 接地していない状態が 0 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 6、 3 7を満足することで ある。 この場合、 .式 3 6、' 3 7のいずれも満足しないときには、 推定許 可条件が成立しないとする。

Fn— act— z> Fn— min …式 3 o

Zn— rel'く Zn— rel— estm— p …式 a f ここで、 Fn_act—zは、 実第 n接地部位床反力（n=l, 2,…，最終葉ノード 番号）の並進力鉛直成分である。 推定許可条件とは、 第 nノード相対床高さ偏差を推定しても (床形状 偏差を推定しても）、 その推定値が発散しない条件である。 ここで発散 とは、 後述するごとく推定された推定第 n床高さ偏差 Zfn_estm (床形 状偏差の推定値） を用いて、 実際の第 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ノード相対床高さ偏差（η=1，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ノードの接地部位力 S床反力モーメントを制御できるか否かを判定する。 子ノード数が 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において、 接地部位床傾斜偏差推定処理が行なわれる。 この処理は、 各接地咅 IH立における床傾斜偏差を推定する処理である。 こ の処理は、 本出願人が先に提案した特開平 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 ノード、 第 j ノードとする。

ここで、 子ノードのモードが 「すべて準備完了」 と判断すると、 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 ノ 一 ド のそれぞれの推定ノ ー ド相対床高 さ 偏差 Zi—reし estm、 Zj_reし estmが求められる （更新される）。 すなわち、 Zi_rel— estm につ いて代表的に説明する と、 Zi一 rel—estm の前回制御周期での値 Zi_rel_estm_p に、 前記 S 5 4で求めた第 j ノード相対床高さ偏差補正 量候補値 Zi_inc_candをカロえることで、 新たな推定第 j ノード相対床高 さ偏差 Zi_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のフローチャート中の式で示す如く、 第 ノードおよび第 j ノ一ドのそれぞれの推定ノード相対^高さ偏差 Zi— reし estm、 ZL reし estm の値が、 前回制御周期での値 Zi— reし estm_p、 Zi_rel_estm_pに維持される。

次に、 図 4 5の S 6 4ひ 0で、 2個の子ノ一ドのモ一ドが 「すべてリ セッ ト」 と判断すると、 S 6 4 0 8において、 2つの子ノードを持つノ ード用グループ内全リセッ ト処理 （第 nノードが持つ 2.つの子'ノ一ドの それぞれの推定ノード相対床高さ偏差をリセッ トする処理） が行なわれ る。 そのリセッ ト処理でま、. 図 4 7のフローチヤ一ト中の式に従って第 i ノー ドおよび第 j ノー ドのそれぞれの推定ノー ド相対床高さ偏差 Zi_rel_estm > Zi— reし estm が 0に徐々に近づいていくように更新され る。 なお、 同式中の Δ Τ、 Testm の意味は、 前記式 3 5のものと同じで ある。 ' こ の リ セ ッ ト処理は、 よ り 一般的 に言え ば、 Zi_rel_estm， Zj_rel_estm を、 それらの重み付き平均値が 0 という条件、 すなわち Wi * Zi_reし estm + Wj * Zj_rel_estm = 0 を満足しつつ、 それぞれ Zi_rel_estm_p , Zj_rel_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で実行される。

以上のように、 第 ηノードの 2個の子ノードのモードがすべて準備完 了である場合で、 且つ、 ノード相対床高さ偏差の推定処 aの発散が生じ る恐れが無い場合にのみ、 その 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ぉょび3 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 のすべてが同時に接地したり離床する場合だけでなく、 各中間ノードの 接地部位のうちのいずれか力 S接地し、 その他の接地部位が離床するよう に口ポッ トの目標歩容を生成する場合についても考慮している。 この場 合、 子ノードが 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) 合力が戸 Jf定の値 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— rel— estm、 Zj— rel— estm、 Zk_rel_estm 力 S求められる （更新される）。 すなわち、 Zi_rel_estm について代表的に説明すると、 Zi_rel— estm の前回制御周 期での値 Zi— reし estm_pに、 前記 S 5 4で求めた第 i ノード相対床高さ 偏差補正量候補値 Zi— inc_caiidを加えることで、 新たな推定第 i ノード 相対床高さ偏差 Zし rel— estm が求められる。 第 j ノード、 第 kノードに ついても同様である。

また、 S. 6 6 0 2の判断結果が N Oである場合には、 床形状偏差の推 定精度が下がり過ぎるので、 実質的な推定処理を実行せずに、 S 6 6 0 6において、 3つの子ノードを持つノード用のグループ内全ホ一ルド処 理 （第 nノードが持つ 3つの子ノードのそれぞれの推定ノード相対床高 'さ偏差を更新せずにホールドする処理） が行わ ήる。 そのホ一ルド処理 では、 図 5 2のフ口一チヤ一 ト中の式で示す如く、 第 i ノード、 第 j ノ . ― ドおよび第 k ノ一 ドのそれぞれの推定ノ ー ド相対床高さ偏差 Zijeし estm、 Zj_rel_estm, Zk_rel_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 ノ 一ド、 第 ノードおよび第 kノ一ドのそれぞれの推定ノード相対床高さ 偏差 Zi_rel_estm Zj_rel_estm , Zk_rel_estm が 0に徐々に近づいて いくように更新される。 なお、 同式中の Δ Τ、 Testm の意味は、 前記式 3 5のものと同じである。

このリセッ ト処理はより一般的に言えば、 Zi— rel— estm, Zj^rel_estm, Zk— rel— 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 に近い値に決定するという処理である。 なお、 Zi— reし estm— p， Zj_rel_estm_p , Zk— rel— 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 ノードが準備^了モード； Tなく、 第 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— rel— estm— p を 徐々に 0に近づけるための第 i ノー ド相対床高さ偏差補正量候補値であ る。 なお、 S 6 6 1 2 4の式中の Δ T、 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 ノー ド相対床高さ偏差補正量候補値 ¾'— 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 といつ.条件を満足" 5一るよ'つ ίこ Z]— iric— cand', Zk— inc— cand'を決定 1 。 . '

次いで、 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— reし estm— p .にカ卩えることで、 新たな Zi— rel— estm, Zj— rel— estm, Zk— reし estmが決定される。

このように Zi— reし estm , Zj— rel— estm , Zk— rel— estm を決定するこ とで、 Wi * Zi—rel— estm + Wj * Zi一 rel— estm + Wk + Zk— rel— estm = 0 を 満足しつつ、 Zj_rel_estm— Zk_rel_estmが Z]— inc— cand— Zk_inc— cand に近づくように Zi— rel— estm， Zi— 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のフローチャート中に示す式 によって、 新たなノード相対床高さ偏差 Zi_reし estm , Zj_rel_estm , Zk— rel— 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 1— estm, Zj— rel— estm, . Zk— rel— estmの 重み付け平均値が 0であるという条件） を満足しつつ、 Zj—reし estm— p , Zk一 reし estm— pよりも 0に近い値に決定する処理である。 補足すると、 第 j ノードおよび第 kノードのモ一ドが共にリセッ トモ一.ドになる時点 までに、 それらのノードの重み Wj , Wk は 0となっているはずである。 従って、 重み Wi， Wj , Wk の和が 1であるという条件から、 この時点 までに Wi が 1 となっ てお り 、 また、 ' Wi * Zi一 rel— estm + Wj * Zj— reし estm + Wk*Zk_rel_estm = 0 で ¾るから、 この時点までに、 Zi reし estm は 0になっている。 このため、 第 i ノードの Zi— rel— estm の値をホールドするということは、 その値を 0に維持するということで ある。 そこで、 図 5 4の処理では、 Zi_reし estm の値を 0に決定するよ うにしている。

なお、 Ζ]·— 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_rel_estm , Z]'— reし 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一 rel— estm— pより も 0に近い値に決定する処理である。 補足すると、 第 kモードがリセッ トモードになる時点までに、 Wk は 0 となっている。

なお、 Zk_reし 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を参照） が実行される。 以上のように、 第 nノードの 3個の子ノ一ドのモードのうち、 2っ以 上の子ノードのモードが準備完了である場合で、 且つ、 ノード相対床高 さ偏差の推定処理の.発散が生じる恐れが無い場合にのみ、 その 2個以上 の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。 そして、 2つ以上の子ノードのモードが準備完了である場合であっても、 推定処理の発散が生じる恐れがある場合には、 3個の子ノードの推定ノ ード相対床高さ偏差の値が、 ホールドされる。

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

図 4 1のフローチャートの処理では、 最後に、 S 7 2において、 第 n ノード（η=1,2，···，最終葉ノード番号）のすベての先祖ノードの推定相対床 高さ偏差と推定第 η ノード相対床高さ偏差との和を求め、 求めた和を 推定第 η接地部位床高さ偏差 （推定第 η床高さ偏差） Zfn一 estm とする。 以上が、 図 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接地部位 （η=1，2, ··' ,最終葉ノード番号） が接地し ているならば、 第 η接地部位の実際の接地面上における目標第 η接地部 . 位床反力中心点 Q ηに相当する点は、 実第 η床接点 D n一 act に一致する はずである。 従って、 前記瞬間第 n ノード相対床高さ偏差 Zn— rel'は、 変動しないはずである。 しかし、 実際には口ポッ ト 1の機械系や制御系 の振動および電気的なノィズにより、 前記床形状推定器 1 3 0の前記し た計算アルゴリズムによって得られた瞬間第 n ノード相対床高さ偏差 Zn一 rel'は、 激しく変動する。 従って、 瞬間第 n ノード相対床高さ偏差 をそのまま推定第 n ノード相対床高さ偏差として用いて、 これを基に 推定第 n床高さ偏差 Zfn_estm を求め （すなわち第 n ノ一ド（η=1,2, ···， 最終葉ノード番号）のすベての先祖ノードの瞬間相対床高さ偏差と瞬間 第 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— rel一 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_reし 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一 estm を図 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が接地した直後や床形 状が途中で変化しだときには、 過渡的には、 実全床反力は、 床形状偏差 の影響を受ける。 しかし、 その後、 時定数 Te stm でその影響が減衰す る。

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

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

2 ) 床形状偏差の推定精度が高い。

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

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

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

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

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

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

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

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

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

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

先ず、 各接地部位 1 0の機構変形捕償入り修正目標第 n接地部位位置 姿勢 (n=l, 2,…，最終葉ノード番号）と、 各接地部位 1 0 の目標接地部位位 置から見た該接地部位 1 りの目標第 n接地部位床反ガ中心点と、 前記姿 勢傾斜偏差 0 berr とを基に、 機構変形補償入り修正目標第 n接地部位 位置姿勢を、 目標全床反力中心点を回転中心として 0 berr だけ回転移 動させてなる、 回転後機構変形補償入り修正目標第 n接地部位位置姿勢 (n=l, 2，…，最終葉ノード番号）を求める。 求められた回転後機構変形補償 入り修正目標第 n接地部位位置姿勢 (η=1, 2，···，最終葉ノード番号）から、 . 目標歩容上での想定第 n床接地位置 Dn(n=l，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_inc一 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 つ以上の場合にも、 重み付き平均の概念を持ち込むことによって、 以下 に示すようにモーメントおよび傾斜角の概念を拡張できる。

目標値、 実際値、 偏差のいずれに関しても概念を拡張できるので、 こ こでは、 目標値、 実際値、 偏差の区別をせず、 一般的な接地^ H立床反力 の組 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番目の子ノードの重みが、 W]' (]'=1，2, ··· ,:τ)であると する。 ·

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

すなわち、 下式 3 8のように UWn を定義する。 ここで 「 T」 は、 転 置を意味し、 行ベクトルの転置によって列ベク トルを表現することとす る。 UWn = (Wal,Wa2, - ,War)T …式 3 8 ベク トル UWn に直交する （すなわちべク トル UWn との内積が 0 と なる.） r—1個の互いに独立なベク トルを R(l),R(2)，'''，R(r _ l)とする。 R(j) (； j=l,2，"'，r— 1)は、 r 行 1列の列ベク トルである。 なお、 R(j) (j=l，2,〜,r—l)は、 演算の容易さと制御精度を考慮すると、 互いに直交 することが望ましい。 R(j)を第 j 列（； j = l,2,〜，r— 1)とする行列を Hn と する。 Hnは、 r行 r一 1列の行列である。

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

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

Fn_rel_c = (Fal_rel, Fa2_rel, - ,Far_rel)^T …式 3 9 Fn— rel— 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ノードのすべての子ノ ード a]' (』=&1，&2 "， 1 )の相対床反カを要素とするべク トル^&1_1 1， Fa2_rel, - ,Far_rel)T を、 第 nノ一ドのすべての子ノードの重みを要素 とするべク トル (Wal，Wa2,…，War)T に直交するある所定の互いに独立 なベク トル R(j) (j = l，2，〜，r— 1)の線形結合で表したときの線形結合の係 数を要素とするベクトルを、 第 nノード拡張床反力モーメント Mn_exp と呼ぶ。

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

また、 目標接地部位床反力の組 ？11(11= 1，2 , " ' )に対する第11ノ一'ド拡 張床反力モーメ ン ト を 目標第 n ノ 一 ド拡張床反力モ ーメ ン ト Mn_exp_ref と呼ぶ。 目標第 nノード拡張床反力モーメントは、 通常 0 べク トルに設定される。

また、 接地部位床反力偏差の組 Fn(n=l, 2，···）に対する第 nノード拡 張床反力モーメ ン ト を第：！ ノ ー ド拡張床反力モーメ ン ト偏差 Mn_exp_errこ呼《o、。

" 同様に、 ー般的な接.地部位高さの組

，...）に対する傾斜角を、 以下のように拡張して、 定義する。 '

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

第 nノー ドの第 j 番目の子ノードの相対高さ Zaj— rel を第 j 要素 (； 2，… とする列べク トルを第 nノード相対高さベク トル Zn— rel— c とする。

すなわち、 次式 4 1により Zn— reし 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

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

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

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

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

例えば、 階層型コンプライアンス動作決定部の中の補償全床反力モー メント分配器の処理は、 次のように拡張される。 6 1 ロポッ トの姿勢を復元させるために目標第 nノード拡張床反力モ一メ ント Mn— exp_i，el (通常 0 ) に付加的に発生すべき拡張床反力モーメン トを第 nノ一ド補償拡張床反力モ一メント Mn— exp_dmdと U乎ぶ。

第 nノード補償拡張床反力モーメント Mn_exp— dmd と第 nノード補 償^反力モーメント Mn—dmd の間には、 次式 4 3の関係がある。 ただ し、 Cn— mech は、 各目標ノード床反力中心点と口ポッ トの機構のコン プライアンス特性とによって決定される所定の係数行列 （ 2行 r一 1 列 の行列） である。 Mn_dmd= Cn_m6ch * Mn_exp_dmd …式 4 3 一方、 前記式 1 0を一般化すると次式 4 4になる。 Mdmd=∑Mn_dmd …式 4 4 なお、 ∑は、 全ての第 nノードについての総和を意味する。

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

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

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

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

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

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

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

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

Θ n_inc_cand = Kn_cmpl * Mn exp_estm_err ··· 4 5 なお、 Kn— cmplは、 対角行列とは限らない 次いで、 ベク トル R(j) (]'=1,2 ー 1)に対する係数を 0 n_inc— cand の第】 要素として、 ベクトル R(j)を線形結合したベク トルを求め、 これ をもって、 第 nノー 相対床高さ偏差の補正量の候補値である第 n ノー ド相対床高さ偏差補正量候補値 Zn— inc— candとする。

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

Zn_inc_cand = Hn * Θ n_mc .cand ' …式 4 6 以上のごとく、 第 nノード床反力推定誤差 Fn— est.m— err から第 nノ —ド相対床高さ偏差補正量候補値 Zn— inc— cand を求めるまでの処理を 図 5 6のものから図 5 7のものに置き換えればよい。

上記以外の図 5 7の演算処理は、 図 5 6の演算処理と同様である。 また、 第 5実施形態における床高さ偏差の推定処理のサブルーチン処 理でほ、 第 nノードの子ノ一ド数が 4以上の場合の処理が必要となる。 この処理は、 前記 3子ノード用床形状推定処理 （図 4 1を参照） を拡張 し、 該 3子ノード用床形状推定処理と同様に、 いずれかの子ノード i に 対応する 地部位が浮いて発散してしまいそうな場合には、 推定第 Ί ノ —ド相対床高さ偏差 Zj— reし estm を前回値 Ζ]·— reし estm— p に維持する (第 j ノード相対床高さ偏差補正量候補値 Zj_in_C__Cand を 0 'とする）。 また、 それ以外の子ノードの推定ノード相対床高さ偏差 Zk__rel— estm を前回推定第 k ノード相対床高さ偏差 Zk— rel— estn 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を線状のリンクの 連結体として模式化して示している。 ' 本実施形態に係わる発明目的は、 膝、 肘、 胴体など、 口ポッ トの脚や 腕の先端部以外の部分が床あるいは床に固定 （設置） された対象物 （床 の延長物と考えられる対象物） と接触する状態において、 該対象物を含 む床から受ける反力 （外力） を操作して、 ロボッ トの姿勢を安定に制御 することにある。 '

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

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

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

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

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

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

以下に、 図 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実施形態 な ど と同様の方針によ っ て、 図示の如 く 、 目標床反力 中心点 (¾!1(11=1,2，3，4，5，6，12，34, 56, 1234，123456)が対応づけられる。 なお、 根 ノードである第 123456 ノ一ドの目標床反力中心点 Q123456 は、 目標 全床反力中心点 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) を決定する 0 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 —68 !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を回転させる (矢印 ylのように回転させる） 動作である。

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

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

図 6 4に、 左右の膝の高さの和の変化に応じて上体 5 3の位置と姿勢 とを修正する動作を視覚的に示す。 すなわち、 点線で示すロボッ ト 5 1 の姿勢から、 実線で示す口ポッ ト 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 lと同じである。

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

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

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

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

Zfootdiffmdfdを求める。

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

Zknee iffmdfd

= 機機構変形補償入り修正目標第 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、 膝高さ差用上体姿勢修正量 0 bkneediffmdfd、 膝高さ和用上体位置修正 量 Xbkneesummdfd、 膝高さ和用上体姿勢修正量 0 bkneesummdfd お よび足平高さ差用上体位置修正量 Xbfootdiffmdfd、 足平高さ差用上体 姿勢修正量 Θ bfootdiffmdfd から成る。 具体的にこれらの値は次のよう に求められる。

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

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

Xbkneediffmdfd = Kxkneediff * Zkneediffmdfd

Θ bkneediffmdfd= Kthkneediff * Zkneediffmdfd

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

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

より具体的には、 例えば、 次式 5 1 によって、 Xbfootdiffmdfd ヒ Θ bfootdiffmdfd とを求める。 ただし、 Kxfootdiff と KtMootdiff とは、 口ポッ ト 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ノード補償角 Θ 12に換算する。 そして、 これらの換算され た補償角を基に、 幾何学演算によって上体位置姿勢修正量を決定するよ うにしても良い。 あるいは、 あらかじめ、 いくつかの代表的な口ポッ ト 5 1の目標姿勢に対して、 換算された補償角と上体位置姿勢修正量との 関係を求めておいて、 これをマップあるいは関数として記憶し、 これと 換算された補償角とを基に、 上体位置姿勢修正量を決定するようにして も良い。

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

次いで、 こじり補正入り目標上体位置姿勢と機構変形補償入り目標第 n 接地部位位置姿勢

"'，6)とを基に、 ロボッ ト 5 1の関節変位指 令を求める。 '

以上のごとく逆キネマテイクス演算部 1 1 0の処理が実行される。 すなわち、 第 6実施形態は、 口ポッ ト 5 1の接地部位位置姿勢の修正 動作のために、 幾何学的に自由度が不足している場合には、 階層型コン プライアンス動作によって目標接地部位位置姿勢を修正するだけでなく、 上体位置の点 （上体の代表点） あるいは上体を含む複数部位の重み付き 平均位置 （この場合の重みは各部位の質量比となっていることが望まし い） の点 （全体重心など）、 特にその点の水平位置を保持するように、 あるいは、 その点と目標全床反力中心点 （目標 Z M P ) とを結ぶ線の角 度を目標歩容における角度にできるだけ保持するように、 目標上体位置 姿勢も修正するようにした。

また、 別の言い方をすれば、 ロボッ 卜 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接地部位位置姿勢 (n=l, 2,… ) (詳しくは位置姿勢のうちの位置） と各目標第 n接地部位 位置姿勢 (n=l,2,〜，6) (詳しくは位置姿勢のうちの位置） とを基に、 前. 記式 4 7、 式 4 8および式 4 9にしたがって左右の膝の高さの差の修正 量 Zkneediffmdfd、 左右の膝の高さの和の修正量 Zkneesummdfd、 お よび左右の足平の高さの差の修正量 Zfootdiffmdfd を求め、 さらに、 左 右の膝の高さの差を、 その差を生じさせる第 3 4ノード補償角 0 34 に 換算する。 同様に、 左右の膝の高さの和を、 その和を生じさせる第 1234 ノード補償角 0 12.34に換算し、 左右の足平の高さの差を、 その差 を生じさせる第 12ノ一ド補償角 0 12に換算する。

次いで、 換算された各ノード補償角を基に、 関節変位修正量を決定す る。 これは次のように行なわれる。 まず、 目標歩容の上体位置姿勢を固 定し、 且つ目標全床反力中心点 （目標第 123456 ノード床反力中心点

(図 6 1の点 P ) ) と口ポッ ト 5 1の全体重心 （あるいは、 上体重心、 あ.るいは、 上体 5 3を含む複数の部位の重心） とを結ぶ線分の角度が変 わらないようにしつつ、 関節変位を修正した場合を想定し、 この場合に、 - 各接地部位 （膝および足平 5 8 ) に対応した第 nノード（n=12，34，1234) の傾斜角度 （補償角） が単位摂動量になるための第 j 関節変位修正量 (]'=1,2, ··· ,関節の総数） を求め、 これを第 nノード補償角に対する第 j 関節変位の感度 Ln_j とする。 .

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

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

L1234_hip_r= 0、 L1234_hip— 1= 0、 L1234— knee— r = 1、

L1234— knee— 1= 1、 L12_hip_r= 0、 L12_hip_l= 0

L12Jknee_r = al2、 L12_knee_l= - al2 , L34_hip_r= a34

L34— hip— 1=— a34、 L34— knee— r= 0、 L34— knee— 1= 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 * 0 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= Ll234_hip_l * Θ 1234 + L12— hip— 1ネ Θ 12 、

+ L34_hip_l * 0 34

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

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

このような関節変位の修正により、 目標全床反力中心点 （目標第 123456 ノード床反力中心点） とロボッ ト 5 1の全体重心 （あるいは、 上体重心、 あるいは、 上体 5 3を含む複数の部位の重心） を結ぶ線分と、 接地部位 （膝および足平 5 8 ) に対応した第 nノード（_n=12, 34, 1234)の 傾斜角度との間の相対角度が、 前記換算された各ノード補償角 S 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のコンプライアンス制御などの制御特性が良好になる。

なお、 ロポッ ト Ί. 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 0 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ノ一ドの目標 J¾反力中心点 Q1234は、 目標全床反力中心点 Pと同 一である。

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

この場合、 本実施形態における歩容生成装置 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床高さ偏差 Zfn_estm(n=l,2,3,4)を求めることに加えて、 各足平 5 8 の接地部位床傾斜偏差 Θ f_n(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ノード補償角 Θ 34を決定する。

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

また、 第 nノード床反力の内で制御できなかった成分 （予想される、 - または検出された制御偏差成分） を補うために、 この成分に応じて、 第 nノードの親ノードの目標床反力を修正しても良い。 すなわち、 実第 n ノード床反力と目標第 nノ一ド床反力との差 φ前回制御周期の値あるい は前記差をローパスフィル夕に通した値を第 ηノードの目標床反力に付 加しても良い。 また、 前記各実施形態では、 補償全床反力モーメントを目標全床反力 中心点まわりに発生させるようにしたが、 これは本発明では必須ではな い。 その場合、 例えば補償全床反力モーメント Mdmd を 0として、 前 記各実施形態の処理を実行するようにすればよい。

床形状の推定処理においては、 コンプライアンス制御用のッリ一構造 と異なるツリー構造を設定してもよい。 そのツリー構造は、 根ノードと 葉ノードとからなる 2層のものであってもよい。 また、 各ノードの重み もコンプライアンス制御用のものと異なるものとしてもよい。 ノード床 反力が 0となるような時期 （そのノードに属する全ての接地部位、 また はそのノードに対応する接地部位が空中を移動する時期） でそのノード の重みが 0であれば良い。

また、 床形状の推定処理においては、 前述の修正目標ノード床反力中 心点に基づいて、 各修正ノード重みを決定し、 これを要素とするべク ト ルを、 床形状推定に用いる重みとして用いても良い。

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

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

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

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

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

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

尚、 前記第 1、 第 2実施形態において、 演算処理機能を示すブロック 線図は演算処理順序を変えるなどの等価変形をしても良い。

また、 本発明は、 移動口ポッ トに限らず、 ァクティブサスペンション を持つ車輪型移動体にも応用することが可能である。 産業上の利用可能性

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

Claims

請 求 の 範 囲

1 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以 上の接地部位を有する移動ロポッ ト等の移動体の目標運動と目標床反力 とから構成される目標歩容を決定する目標歩容決定手段と、 前記接地部 位のそれぞれに作用する実床反力を検出または推定する床反力検出手段 とを備え、 前記移動体の歩容を目標歩容に追従させるように該移動体の 動作制御を行う移動体の制御装置において、

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

前記ツリー構造における各ノードである第 Bノードに対し、 該第 Bノ ―ドが前記葉ノ一ドである場合には、 該葉ノードである接地部位に作用 する床反力を第 Bノードのノード床反力と定義し、 且づ、 第 Bノードが 子ノ一ドを持つ場合には、 該第 Bノードのすべての子ノ一ドのノード床 反力の合力を第 Bノードのノ一ド床反力と定義したとき、

各ノードのノード床反力の実際値としての実ノード床反力を前記検出 または推定された各接地部位の実床反力から決定する手段と、

各ノードのノード床反力の目標値としての目標ノード床反力を少なく とも前記目標歩容の目標床反力に基づき決定する手段と、

複数の接地部位を子孫ノードとして持つノ一ドである第 Cノードのそ れぞれに対し、 少なくとも該第 Cノ一ドの各子ノードの実ノ一ド床反力 の間の相対関係と該第 Cノードの各子ノードの目標ノ一ド床反力の間の 相対関係とを基に、 該第 Cノードに属する複数の接地部位の間の目標相 対高さの修正量を該第 Cノード毎に決定すると共に、 その決定した修正 量をすベての第 Cノードについて合成してなる修正量を基に、 前記目標 歩容の目標運動に、 各第 Cノードに属する複数の接地部位の間の目標相 対高さの第 1の修芷を少なくとも付加した修正目標運動を決定し、 その 決定した修正目標運動を満足するように前記関節を動作させるノ一ド動 作制御手段とを備えたことを特徴とする移動体の制御装置。

2 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以 上の接地部位を有する移動ロポッ ト等の移動体の目標運動と目標床反力 とから構成される目標歩容を決定する目標歩容決定手段と、 前記接地部 位のそれぞれに作用する実床反力を検出または推定する床反力検出手段 とを備え、 前記移動体の歩容を目標歩容に追従させるように該移動体の 動作制御を行う移動体の制御装置において、

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

前記ッリ一構造における各ノードである第 B —ドのそれぞれに対し、 該第 Bノードが前記葉ノードである場合には、 該葉ノ一ドである接地部 位に作用する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 A ノードが子ノードを持つ場合には、 該第 Bノードのすべての子ノ一ドの ノード床反力の合力を第 Bノードのノード床反力と定義したとき、 複数の接地部位を子孫ノードとして持つ中間ノードである所定の第 C ノードに対し、

少なくとも前記第 Cノ一ドの各子ノードのノード床反力の実際値とし ての実ノード床反力を前記検出または推定された各接地部位の実床反力 から決定する手段と、

少なくとも前記第 Cノ一ドの各子ノードのノード床反力の目標値とし ての目標ノード床反力を少なくとも前記目標歩容の目標床反力に基づき 決定する手段と、

少なくとも該第 Cノードの各子ノードの実ノード床反力の間の相対関 係と該第 Cノードの各子ノードの目標ノード床反力の間の相対関係とを 基に、 該第 Cノ一ドの各子ノ一ドの実ノード床反力の間の相対関係が該 第 Cノードの各子ノードの目標ノード床反力の間の相対関係に近づくよ うに、 前記目標歩容の目標運動に、 前記第 Cノ一ドに属する複数の接地 部位の間の目標相対高さの第 1の修正を少なくとも付加した修正目標運 動を決定し、 その決定された修正目標運動を満足するするように前記関 節を動作させるノード動作制御手段を備えたことを特徴とする移動体の 制御装置。

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

前記ノード動作制御手段が、 各第 Cノードのうちの任意の一つのノー ドのみに対して、 該一つのノ一ドに属する複数の接地部位の間の目標相 対高さの第 1の修正を前記目標歩容の目標運動に付加して前記修正目標 運動を決定したとき、 該修正目標運動における該一つのノードの目標高 さは前記目標運動における目標高さに維持されることを特徴とする請求 の範囲第 1項に記載の移動体の制御装置。

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

5 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以 上の接地部位を有する移動ロボッ ト等の移動体の目標運動と目標床反力 とから構成される目標歩容を決定する目標歩容決定手段と、 前記接地部 位のそれぞれに作用する実床反力を検出または推定する床反力検出手段 とを備え、 前記移動体の歩容を目標歩容に追従させるように該移動体の 動作制御を行う移動体の制御装置において、

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

前記ツリー構造における各ノードである第 Bノードに対し、 該第 Bノ 一ドが前記葉ノードである場合には、 該葉ノ一ドである接地部位に作用 する床反力を第 Bノードのノード床反力と定義し、 且つ、 第 Bノードが 子ノードを持つ場合には、 該第 Bノ一ドのすべての子ノ一ドのノ一ド床 反力の合力を第 Bノードのノード床反力と定義し、 さらに前記第 Bノ一 ドのノード床反力のモ一メン卜の水平成分または床面平行成分が 0とな る点を第 Bノードのノード床反力中心点と定義したとき、

各ノードのノード床反力中心点の目標位置である目標ノ一ド床反力中 心点を前記目標歩容の目標運動と目標床反力とのうちの少なくともいず れか一方に応じて決定する手段と、

各ノードのノード床反力の実際値としての実ノード床反力を前記検出 または推定された各接地部位の実床反力から決定する手段と、 複数の接地部位 子孫ノードとして持つノードである第 Cノードのそ れぞれに対し、

前記第 Cノ一ドの実ノ一ド床反力が該第 Cノードの目標ノード床反力 中心点に作用するモーメントである実モーメント （Mc— act) と該第 C ノー ドの目標ノー ド床反力中心点に作用すべきモーメントの目標値 (Mc_dmd) との差 （Mc— act— Mc— dmd) と、

前記第 Cノードの実モーメント （Mc— act) から該第 Cノードの各子 ノー ドの実ノード床反力が該子ノードの目標ノ一ド床反力中心点に作用 するモーメントを減じてなるモーメント （Mc_f) と該第 Cノードの目 標ノ 一ド床反力中心点に作用すべきモーメントの目標値 （Mc_dmd) と の差 （Mc— f— Mc— dmd) と、

前記第 Cノードの実ノード床反力のモーメントの水平成分または床面 平佇成分が 0 となる点の位置と該第 Cノードの目標ノード床反力中心点 の位置との差の値 （Δ P ) と、

前記第 Cノードの実ノード床反力のモーメント （Mc— act) から該第 Cノ一ドの各子ノードの実ノ一ド床反力が該子ノ一ドの目標ノード床反 力中心点に作用するモーメントを減じてなるモーメントの水平成分また は床面平行成分が 0となる点の位置と該第 Cノードの目標ノード床反力 中心点の位置との差の値 （A P f) とのうちのいずれかを制御量として、 該制御量に応じて、 該制御量が 0に近づくように、 該第 Cノードに属す る複数の接地部位の間の目標相対高さの修正量を該第 Cノ一ド毎に決定 すると共に、 その決定した修正量をすベての第 Cノードについて合成し てなる修正量を基に、 前記目標歩容の目標運動に、 各第 Cノードに属す る複数の接地部位の間の目標相対高さの第 1の修正を少なく とも付加し た修正目標運動を決定し、 その決定した修正目標運動を満足するように 前記関節を動作させるノード動作制御手段とを備えたことを特徴とする 移動体の制御装置。 '

6 . 互いの相対高さを操作可能に複数の関節を介して連結された 3っ以 上の接地部位を有する移動ロポッ ト等の移動体の目標運動と目標床反力 とから構成される目標歩容を決定する目標歩容決定手段と、 前記接地部 位のそれぞれに作用する実床反力を検出または推定する床反力検出手段 とを備え、 前記移動体の歩容を目標歩容に追従させるように該移動体の 動作制御を行う移動体の制御装置において、

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

前記ッリ一構造における各ノードである第 Bノードのそれぞれに対し、 該第 Bノードが前記葉ノードである場合には、 該葉ノードである接地部 位に作用する床反力を第 Bノードのノ一ド床反力と定義し、 且つ、 第 B ノードが子ノードを持つ場合には、 該第 Bノ一ドのすべての子ノ一ドの ノード床反力の合力を第 Bノードのノード床反力と定義し、 さらに前記 第 Aノードのノード床反力のモ一メントの水平成分または床面平行成分 が 0となる点を第 Bノードのノ一ド床反力中心点と定義したとき、 複数の接地部位を子孫ノードとして持つ中間ノードである所定の第 C ノードに対し、

少なく とも前記第 Cノードのノ一ド床反力中心点の目標位置である目 標ノード床反力中心点を少なく とも前記目標歩容に応じて決定する手段 と、

少なく とも前記箄 Cノードのノード床反力の実際値としての実ノード 床反力を前記検出または推定された各接地部位の実床反力から決定する 手段と、

前記第 Cノ一ドの実ノード床反力が該第 Cノードの目標ノード床反力 中心点に作用するモーメントである実モーメント （Mc— act) と該第 C ノー ドの目標ノー ド床反力中心^に作用すべきモーメントの目標値 (Mc_dmd) との差 （Mc— act— Mc— dmd) と、

前記第 Cノードの実モーメント （Mc_act) から該第 Cノードの各子 ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用 するモーメントを減じてなるモーメント （Mc— f) と該第 Cノードの目 標ノード床反力中心点に作用すべきモーメントの目標値 （Mc_dmd) と の差 （Mc— f— Mc_dmd) と、

前記第 Cノードの実ノード床反力のモーメントの水平成分または床面 平行成分が 0となる点の位置と該第 Cノ一ドの目標ノード床反力中心点 の位置との差の値 （△ P ) と、

前記第 Cノードの実ノード床反力のモーメント （Mc_act) から該第 Cノードの各子ノ一ドの実ノ一ド床反力が該子ノードめ目標ノード床反 力中心点に作用するモ一メントを減じてなるモーメントの水平成分また は床面平行成分が 0となる点の位置と該第 Cノードの目標ノード床反力 中心点の位置との差の値 （△ P f) とのうちのいずれかを制御量として、 該制御量に応じて、 該制御量が 0に近づくように、 前記目標歩容の目標 運動に、 前記第 Cノードに属する複数の接地部位の間の目標相対高さの 第 1の修正を少なく とも付加した修正目標運動を決定し、 その決定され た修正目標運動を満足するするように前記関節を動作させるノード動作 制御手段を備えたことを特徴とする移動体の制御装置。

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

前記ノード動作制御手段が、 各第 Cノ一ドのうちの任意の一つのノー ドのみに対して、 該一つのノードに属する複数の接地部位の間の目標相 対高さの第 1の修正を前記目標運動に付加して前記修正目標運動を決定 したとさ、 該修正目標運動における該一つのノードの目標高さは前記目 標運動における目標高さに維持されることを特徴とする請求の範囲第 5 項に記載の移動体の制御装置。

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

前記ノード動作制御手段が、 前記第 Cノードのみに対して、 該第 Cノ 一ドに属する複数の接地部位の間の目標相対高さの第 1の修正を前記目 標運動に付加して前記修正目標運動を決定したとき、 該修正目標運動に おける該第 Cノ一ドの目標高さは前記目標運動における目標高さに維持 される ことを特徴とする請求の範囲第 6項に記載の移動体の制御装置。

9 . 前記目標ノード床反力中心点を決定する手段は、 前記第 Cノードの 各子ノードの目標ノード床反力中心点の位置の、 前記重みを用いた重み 付き平均値が該第 Cノードの目標ノード床反力中心点となるように、 該 第 Cノ一ドおよびその子孫ノードのそれぞれの目標ノード床反力中心点 と重みとを決定することを特徴とする請求の範囲第 7項または第 8項に 記載の移動体の制御装置。

1 0 . 前記ノード動作制御手段は、 前記各第 Cノード毎に、 該第 Cノー ドの各子ノードの目標ノード床反力中心点を、 少なく とも前記制御量に 応じて決定した回転量だけ該第 Cノードの目標ノード床反力中心点のま わりに回転させたときの ノードの各子ノ一ドの目標ノード床反力中 、点の移動量に応じて、 該第 Cノ一ドに属する複数の接地部位の間の目 標相対高さの修正量を決定することを特徴とする請求の範囲第 5項に記 載の移動体の制御装置。

1 1 . 前記ノード動作制御手段は、 前記第 Cノードの各子ノードの目標 ノード床反力中心点を、 少なく とも前記制御量に応じて決定した回転量 だけ該第 Cノードの目標ノード床反力中心点のまわりに回転させたとき の該第 Cノードの各子ノ一ドの目標ノード床反力中心点の移動量に応じ て、 該第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の 修正を前記目標運動に付加した前記修正目標運動を決定することを特徴 とする請求の範囲第 6項に記載の移動体の制御装置。

1 2 . 前記ノード動作制御手段は、 前記第 Cノードの各子ノードの目標 ノード床反力中心点を、 少なく とも前記制御量に応じて決定した回転量 だけ該第 Cノードの目標ノード床反力中心点のまわりに回転させたとき の該第 Cノードの各子ノードの目標ノード床反力中心点の移動量に応じ て、 該第 Cノードの子ノードの相互の相対高さを操作することによって、 第 Cノードに属する複数の接地部位の間の目標相対高さの第 1の修正を 前 己目標運動に付加した修正目標運動を決定することを特徴とする請求 の範囲第 7項に記載の移動体の制御装置。

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

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

1 5 . 前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推 定する実姿勢傾斜検出手段を備え、

前記ノー ド動作制御手段は、 前記修正目標運動を決定するとき、 前記 実姿勢傾斜と前記目標歩容の目標運動における前記所定の部位の目標姿 勢傾斜との偏差を 0に近づけるように、 該偏差に応じた前記第 Cノード に属する複数の接地部位の間の目標相対高さの修正を前記目標運動にさ らに付加して前記修正目標運動を決定することを特徴とする請求の範囲 第 1項、 第 2項、 第 5項、 第 6項のいずれかに記載の移動体の制卿装置。

1 6 . 前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推 定する実姿勢傾斜検出手段を備え'、

前記ノー ド動作制御手段は、 前記第 Cノードの前記目標ノード床反力 中心点に作用すべきモーメントの目標値と、 該第 Cノードの前記目標ノ 一ド床反力中心点とのうちの少なく ともいずれかを、 前記実姿勢傾斜と 前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿 勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけるように修正する手 段を備え、 前記修正されたモーメントの目標値または目標ノード床反力 中心点を、 その修正前のモーメントの目標値または目標ノード床反力中 心点の代わ りに用いて前記制御量を決定することを特徴とする請求の範 囲第 5項または第 6項に記載の移動体の制御装置。

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

前記ッリ 一構造における子ノ一ドを有するノードである第 Eノードの それぞれに対し、 該'第 Eノード床反力の目標値である目標ノード床反力 が該第 Eノードの前記目標ノード床反力中心点に作用するモーメントの 水平成分または床面平行成分が 0になるように、 該第 Eノードに属する 各葉ノードの目標ノード床反力の並進力成分を少なく とも前記目標歩容 の目標床反力に基づき決定する目標ノード床反力分配手段を有し、 前記ノード動作制御手段は、 少なくとも前記第 Cノ一ドに属する各葉 ノードの目標ノード床反力の並進力成分を基に、 該目標ノード床反力の 並進力成分が該第 Cノードに属する各接地部位に作用したときに前記基 体と詨第 Cノードに属する各接地部位との間の連結機構および該接地部 位で 生する変形量を推定する手段を備え、 前記修正目標運動を決定す るとぎ、 前記推定した変形量を打ち消すための、 前記第 Cノードに属す る複数の接地部位のそれぞれの目標高さの第 2の修正を前記目標運動に さらに付加して前記修正目標運動を決定することを特徴とする請求の範 囲第 5項または第 6項に記載の移動体の制御装置。

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

前記ツリー構造において親ノ一ドを有するノードである第 Eノードの それぞれに対し、 該第 Eノ一ド床反力の目標値である目標ノード床反力 の並進力成分を、 第 Eノードの親ノードの目標ノ一ド床反力の並進力成 分に第 Eノードの前記重みを乗じた値になるように、 第 Eノードの目標 ノード床反力の並進力成分を少なく とも前記目標歩容の目標床反力に基 づき決定する目標ノ一ド床反力分配手段を有し、

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

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

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定す る実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾 斜との偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけ るように少なくとも前記第 Cノードの目標ノード床反力中心点を修正す る手段と、

各ノードのノード床反力の目標値である目標ノード床反力のうち、 少 なくとも前記第 Cノードの各子孫ノードのノ一ド床反力の目標値である 目標ノード床反力の並進力成分を、 該第 cノードの前記修正された目標 ノード床反力中心点に作用する目標ノード床反力のモーメントの水平成 分または床面平行成分が 0になるように少なく とも前記目標歩容の目標 床反力に基づき決定する目標ノード床反力分配手段とを有し、

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

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

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定す る実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾 斜との偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけ るように少な くとも前記第 Cノ一ドの目標床反力中心点に作用すべきモ ーメン卜の目標値を決定する手段と、

各ノードのノード床反力の目標値である目標ノード床反力のうち、 少 なくとも前記第 Cノ一ドの各子孫ノードのノ一ド床反力の目標値である 目標ノード床反力の並進力成分を、 該第 Cノードの前記目標ノード床反 力中心点に作用する目標ノード床反力のモーメントが前記決定された目 標値になるよ うに少なく とも前記目標歩容の床反力に基づき決定する目 標ノ一ド床反力分配手段とを有し、

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

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

前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定す る実姿勢傾斜検出手段と、

前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢と の偏差である姿勢傾斜偏差に応じて、 該姿勢傾斜偏差を 0に近づけるよ うに少なくとも前記第 Cノードの各子孫ノードの重みを修正する手段と、 各ノードのノード床反力の目標値である目標ノード床反力のうち、 少 なくとも前記第 Cノードの各子孫ノードのノード床反力の目標値である 目標ノード床反力の並進力成分を、 各子孫ノードの親ノードの目標ノー ド床反力の並進力成分に該子孫ノードの前記修正された重みを乗じた値 になるように、 少なくとも前記目標歩容の目標床反力に基づき決定する 目標ノ一ド床反力分配手段とを有し、

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

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

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

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

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

2 4 . 前記ノード動作制御手段は、 前記目標歩容の目標床反力により定 まる前記移動体の目標全床反力中心点と、 前記修正目標運動における前 記移動体の全体重 、または該移動体の基体の所定の代表点とを結ぶ線分 の向きが、 前記目標歩容における当該線分の向きとほぼ同じになるよう に前記修正目標運動を決定することを特徴とする請求の範囲第 2 2項に 記載の移動体の制御装置。

2 5 . 前記ノード動作制御手段は、 前記目標歩容の目標床反力により定 まる前記移動体の目標全床反力中心点と、 前記修正目標運動における前 記移動体の全体重 、または該移動体の基体の所定の代表点とを結ぶ線分 の向きが、 前記目標歩容における当該線分の向きとほぼ同じになるよう に前記修正目標運動を決定することを特徴とする請求の範囲第 2 3項に 記載の移動体の制御装置。

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

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

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

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

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

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

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

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

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

3 5 . 前記基体の接地部位には、 その接地時に弾性変形する弾性体が設 けられていることを特徵とする請求の範囲第 3 3項に記載の移動体の制 御装置。

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

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

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

前記ノード動作制御手段は、 少なくとも前記第 Cノードの前記ノード 拡張床反力モーメ ント Mn_exp またはノード拡張傾斜角 0 nを用いて該 第 Cノードに属する複数.の接地部位の間の目標相対高さの第 1の修正を 前記目標運動に付加した前記修正目標運動を決定することを特徴とする 請求の範囲第 1項、 第 2項、 第 5項、 第 6項のいずれかに記載の移動体 の制御装置。

3 7 . 少なくとも前記目標歩容の目標床反力に基づき、 少なくとも前記 第 Cノードに属する各接地部位に作用すべき目標床反力を決定する手段 を備え、

前記ノード動作制御手段は、 前記第 Cノードに属する各接地部位の目 標床反力を基に該第 Cノードの前記ノード拡張モーメントの目標値であ る目標ノード拡張モーメントを決定する手段と、 前記第 Cノ一ドに属す る各接地部位の実床反力を基に該第 Cノ一ドの前記ノード拡張モーメン トの実際値である実ノード拡張モ一メントを決定する手段と、 前記実ノ 一ド拡張モーメン卜と前記目標ノード拡張モーメントと応じて、 前記第. Cノードの前記ノード拡張傾斜角の操作量を決定する手段とを備え、 前 記決定したノード拡張傾斜角の操作量を基に、 前記第 Cノードに属する 複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付加 した前記修正目標運動を決定することを特徴とする請求の範囲第 3 6項 に記、載の移動体の制御装置。

3 8 . 少なく とも前記第 Cノードに属する複数の接地部位の接地面の間 の相対高さを規定するパラメータを床形状を表す床形状パラメータとし て推定する床形状推定手段を備え、

前記ノード動作制御手段は、 前記修正目標運動を決定するとき、 前記 前記床形状パラメータの推定値に応じた、 前記第 Cノードに属する複数 の接地部位の間の目標相対高さの第 3の修正をさらに付加して前記修正 目標運動を決定することを特徴とする請求の範囲第 1項、 第 2項、 第 5 項、 第 6項のいずれかに記載の移動体の制御装置。

3 9 . 少なく とも前記第 Cノードの各子ノ一ドのノード床反力の実際値 である実ノー ド床反力を該第 Cノードに属する各接地部位の実床反力か ら決定する手段を備え、

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

3 8項に記載の移動体の制御装置。

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

該基体と前記第 C.ノー ドに属する各接地部位との間の連結機構および 該接地部位で発生する変形量を検出または推定する変形量検出手段を備 え、

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

4 1 . 前記変形量検出手段は、 各接地部位の実床反力に基づき前記変形 量を推定することを特徵とする請求の範囲第 4 0項に記載の移動体の制 御装置。 ·

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

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

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

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

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

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

前記床形状推定手段は、 少なくとも前記第 Cノードの前記ノード拡張 床反力モーメント Mn一 expまたはノード拡張傾斜角 θ nを用いて前記床 形状パラメータを推定することを特徴とする請求の範囲第 3 8項に記載 の移動体の制御装置。

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

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

4 7 . 少なくとも前記目標歩容の目標床反力に基づき、 少なくとも前記 第 Cノードに属する各接地部位に作用すべき目標床反力を決定する手段 を備え、

前記ノード動作制御手段は、 前記第 Cノードに属する各接地部位の目 標床反力を基に該第 Cノードの前記ノード拡張モーメントの目標値であ る目標ノード拡張モーメントを決定する手段と、 前記第 Cノ一ドに属す る各接地部位の実床反力を基に該第 cノードの前記ノード拡張モ一メン トの実際値である実ノード拡張モーメントを決定する手段と、 前記実ノ 一ド拡張モーメントと前記目標ノード拡張モーメントとに応じて、 前記 第 Cノードの前記ノード拡張傾斜角の操作量を決定する手段とを備え、 前記決定したノード拡張傾斜角の操作量を基に、 前記第 Cノードに属す る複数の接地部位の間の目標相対高さの第 1の修正を前記目標運動に付 加した前記修正目標運動を決定することを特徴とする請求の範囲第 4 5 に記載の移動体の制御装置