WO2005051612A1

WO2005051612A1 - 移動ロボットの制御装置

Info

Publication number: WO2005051612A1
Application number: PCT/JP2004/018096
Authority: WO
Inventors: Toru Takenaka; Takashi Matsumoto; Takahide Yoshiike
Original assignee: Honda Motor Co., Ltd.
Priority date: 2003-11-27
Filing date: 2004-11-29
Publication date: 2005-06-09
Also published as: WO2005051608A2; KR20060126649A; US20070152620A1; US20070126387A1; EP1698440A1; EP1695799A4; US7541764B2; EP1698440A4; EP1695799A1; WO2005051608A3; WO2005051611A1; EP1698439A4; EP1695799B1; JPWO2005051612A1; KR20060126647A; JPWO2005051611A1; KR101112500B1; JPWO2005051608A1; DE602004032467D1; KR101112501B1

Abstract

基体５３と、これに連結された複数のリンク機構５２、５４を備える。基体５３とリンク機構（脚）５２の先端部（足平）５８との間の所定の部位（膝）に外力を検出するセンサ９０が設けられている。ロボット６１が所定の部位（膝）を接地させた動作姿勢において、センサ９０で検出した外力（床反力）を目標外力に近づけるように少なくとも基体５３と所定の部位（膝）との間の関節５５の変位を制御する。ロボット６１の脚や腕の先端部以外の部分が、床などと接触して外力を受ける状態にで脚や腕の先端部以外の部分に作用する外力を適切に制御してロボット６１の姿勢を安定に保つ。

Description

明細書移動ロポッ卜の制御装置技術分野

本発明は、 2足移動ロボットなどの移動ロポットの制御装置に関し、より詳しくは、膝、肘、胴体など、口ポットの脚や腕の先端部外の部分を接地させる場合などに移動口ポットに作用する床反力（外力）を制御する装置に関する。背景技術

2足移動ロボット等の脚式移動口ポットを、その脚の先端部を接地させり離床させたりしながら移動させる場合に、それらの脚の先端^に作用する床反力を制御する技術としては、本願出願人による特開平 5— 3 0 5 5 8 5号公報（特許文献 1 )、特開平 1 0 — 2 7 7 9 6 9号公報 (特許文献 2 )、特開 2 0 0 1— 3 2 2 0 7 6号公報（特許文献 3 ) に見られる技術が知られている。

しかし、これらの技術は、元々、脚の先端部を床に接地させてロポットの移動を行なうことを想定したものであり、床反力を検出するセンサを脚の先端部に備えている。また、腕を備えたロボットにある作業を行わせる場合に、その腕の先端部に作用する力を制御するために、該腕の先端部に力センサを備えたものも知られている.。

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

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

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

本発明はかかる背景に鑑みてなされたものであり、移動ロボットの膝、肘、胴体、臀部など、脚や腕の先端部以外の部分が、床あるいは床の延長物と考えられる対象物と接触して外力を受ける状態において、ロポットの脚や腕の先端部に作用する外力のみならず、脚や腕の先端部以外の部分に作用する外力をも適切に制御し、移動ロボッ卜の姿勢を安定に保つことを可能とする移動ロポットの制御装置を提供することを目的とする。発明の開示本発明の移動口ポットの制御装置の第 1発明は、基体と、該基体の移動のために該基体に連結されて外部と接触する複数のリンク機構とを備えると共に、該基体と各リンク機構の先端部との間に、該基体に対して各リンク機構の先端部を可動とする複数の関節が設けられ、前記複数のリンク機構のうちの 1つ以上の特定リンク機構の先端部と前記基体との間の所定部位で外部と接触する特定動作姿勢に動作可能な脚式移動ロボットの制御装置において、

前記特定動作姿勢で前記所定部位に作用する外力を検出または推定する外力検出手段と、

前記特定動作姿勢における前記所定部位の外力の目標値である目標外力を決定する目標外力決定手段と、

前記検出または推定された外力が前記目標外力に近づくように、少なくとも前記所定部位と前記基体との間に存在する関節の変位を制御する関節変位制御手段とを備えたことを特徴とするものである。

かかる第 1発明によれば、前記特定姿勢動作において、前記所定部位が外部と接触する。この所定部位は、前記基体と前記特定リンク機構の先端部との間で該特定リンク機構に設けられている部位であるが、その所定部位に作用する外力は前記外力検出手段によって検出または推定される。そして、この検出または推定された外力が前記目標外力決定手段で決定した目標外力に近づくように、少なくとも前記所定部位と前記基体との間に存在する関節の変位を制御することによって、該所定部位に作用する外力を適切に制御することができる。その結果、口ポットの姿勢を安定に保つことが可能となる。

かかる第 1発明では、前記特定リンク機構は例えば脚体である（第 2 発明）。この場合には、前記所定部位は、該脚体の先端部と基体との間の中間部位（例えば膝など）となる。従って、例えば人間型口ポットの膝付き姿勢で、その膝に作用する外力を制御できる。なお、ロボットに備える前記リンク機構は、脚体だけに限らず、腕体を含んでいてもよい。その場合、前記特定リンク機構は、脚体だけでもよいが、腕体だけでもよく、あるいは、脚体と腕体との両者を含んでいてもよい。

また、前記第 1発明（あるいは第 1、第 2発明）では、例えば、前記特定リンク機構は、前記基体に関節を介して連結された臀部から延設された脚体であり、前記所定の部位は、前記臀部である（第 3発明）。この場合、. ロポットが腰掛ける場合などにおいて、臀部に作用する外力を制御して口ポットの姿勢（特に上体などの基体の姿勢）を適切な安定姿勢に制御できる。

さらに、前記第 1発明（あるいは第 1〜第 3発明）では、前記移動口ポッ卜の前記基体などの第 2の所定部位の実姿勢を検出する実姿勢検出手段と、該第 2の所定部位の目標姿勢を決定する目標運動決定手段とを備え、前記目標外力決定手段は、少なくとも前記第 2の所定部位の実姿勢と目標姿勢との偏差に応じて前記目標外力を決定することが好ましい (第 4発明）。

この第 4発明によれば、前記第 2の所定部位（例えば基体）の実姿勢を所要の目標姿勢に復元するように前記所定部位に作用する外力を制御できるので、ロポットの姿勢の安定性を高めることができる。

さらに第 1発明（あるいは第 1〜第 4発明）では、前記移動ロボットの前記基体などの第 2の所定部位の実姿勢を検出する実姿勢検出手段と、該第 2の所定部位の目標姿勢を決定する目標運動決定手段とを備え、前記関節制御手段は、前記第 2の所定の部位の実姿勢と目標姿勢との偏差に応じて外力の操作量を決定する手段とを備え、前記目標外力と前記外力の操作量との合力に前記検出または推定された外力を近づけるように前記関節の変位を制御することが好ましい（第 5発明）。この第 5発明によれば、前記所定部位に作用する外力を目標外力に近づけつつ、前記第 2の所定部位（例えば基体）の実姿勢を目標姿勢に近づけるように関節の変位を制御するので、ロボットの姿勢を安定化すると同時に、所定部位に作用する外力を適切に制御できる。

なお、第 4発明および第 5発明における第 2の所定の部位は、前記特定動作姿勢で外部と接触する所定部位と同じであってもよい。図面の簡単な説明

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

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

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

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

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

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

なお、接地部位床反力、全床反力などの床反力は、一般的には、力の作用点と、その作用点にかかる並進力およびモーメントとの組によって表現され、同一の床反力に対して、それを表現する作用点と、並進力およびモーメントとの組は無限にある。その中には、鉛直軸まわりの成分 9

(鉛直成分）を除くモーメント成分（モーメントの水平成分）が 0となるような点を作用点として用いる床反力の表現が存在する。 'この表現における作用点を、本明細書では床反力中心点という。また、床反力中心点を、床反力がその点に発生するモーメントの床面平行成分（床面に平行な成分）が 0となるような点として定義してもよい。

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

0である。つまり、床反力中心点は任意に設定できる。ただし、ロポッ卜の動作制御を円滑に行なう上では、目標とする床反力中心点は、連続的に移動することが望ましい。そこで、本明細書の実施^態では、接地部位床反力の床反力中心点は、接地部位が空中を移動するときに、接地部位と共に移動する作用点であるとする。

本明細書の実施形態においては、少なくとも実床反力（口ポットに実際に作用する床反力）に応じて各接地部位の位置姿勢の修正を行なうコンプライアンス制御動作において、単に、各接地部位毎に、互いに無関係に（独立して）コンプライアンス制御動作を行うのではない。本明細書の実施形態は、各接地部位をツリー構造で分類し、少なくとも分類された各グループに作用する実床反力（各グループに含まれる全ての接地部位に作用する実床反力もしくはその合力）に基づき、接地部位の位置姿勢の修正動作を決定することに特徴がある。尚、以降、ツリー構造で分類することを「階層化」と呼ぶこともある。本明細書の実施形態における脚式移動口ポットの歩容生成装置が生成する変数の目標値には、変数名の頭に「目標」を付ける。実際の脚式移動口ポットにおける当該変数の値（真の値はわからないので、実際には、検出値または推定値）には、変数名の頭に「実」を付ける。前記「実床反力」はその例である。

後述するコンプライアンス制御（床反力制御）の全床反力（ロボットのすベての接地部位に作用する実床反力の合力）の目標を目標全床反力と呼ぶ。目標全床反力のモーメント水平成分が 0となる点を目標全床反力中心点と呼ぶ。

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

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

尚、移動口ポットの目標歩容で該口ポットが床反力以外の反力（外力）を環境から受けるように想定してもよい。そして、その床反力以外の反力（外力）を、例えば目標対象物反力と称し、先に述べた目標 Z M

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

以上を前提として、まず、本発明に関連する参考例および本発明の実施形態を以下に詳細に説明する。なお、本明細書では、説明の便宜上、まず、しばらくの間、参考例を説明し、その後に本発明の実施形態を説明する。参考例は、第 1〜第 5参考例まである。

[第 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 軸力センサ 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の位置および姿勢の目標値の軌道である目標上体位置姿勢軌道とから構成される。なお、後述する第 1参考例のように、腕や頭部に係る関節を備えるロボットにおける歩容生成装置は、腕や頭部の目標位置姿勢軌道も目標運動の構成要素として決定して出力する。 . また、歩容生成装置 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の目標姿勢の軌道を生成しない（生成する必要がない）。従って、本参考例では、前記目標接地部位軌道は、目標接地部位位置軌道と同じものを意味する。

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

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

また、歩容生成装置 1 0 0が決定する目標上体位置姿勢軌道は、少なくとも、ロボット 1の目標運動によって発生する慣性力とロポット 1に作用する重力との合力が目標 Z MPまわりに作用するモ一メン卜の水平成分が 0になるようにロポット 1の動力学モデルなどを使用して決定される。なお、「上体位置」は、上体 2 4のある代表点の位置である。

また、歩容生成装置 1 0 0が決定する目標全床反力は、目標全床反力中心点に作用する並進力およびモーメントの目標値から構成され、本参考例では、口ポット 1の目標運動によって発生する慣性力とロポット 1 に作用する重力との合力に目標全床反力中心点で釣り合うように決定される。なお、目標全床反力中心点（目標 ZMP) まわりの目標全床反力のモーメント水平成分は 0である。補足すると、目標全床反力中心点に作用する並進力およぴモ一メントの全ての成分を目標値として決定する必要はない。例えばロボット 1の鉛直軸まわりの姿勢もしくは床反力を制御しない場合には、目標全床反力のモーメン卜の鉛直軸まわりの成分 ( Z軸方向成分）を決定する必要はない。

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

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

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

目標床反力分配器 1 0 2には、歩容生成装置 1 0 0で決定された目標歩容のうち、目標全床反力中心点軌道、目標全床反力軌道および目標接地部位軌道とが入力される。なお、歩容生成装置 1 0 0が目標歩容を決定するために使用した歩容パラメ一夕（ロボット 1の遊脚の接地部位 1 0の着地予定位置、着地予定時刻など）も目標床反力分配器 1 0 2に入力してもよい。そして、目標床反力分配器 1 0 2は、これらの入力を基に、上記のように階層化された各ノードの床反力中心点の目標位置である目標ノード床反力中心点（目標第 n ノード床反力中心点） Qn (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の目標接地部位位置から一義的に定まる。逆に言えば、葉ノードの目標ノード床反力中心点 Qn (n=l,2，3,4) は、第 n接地部位 1 0の目標位置を規定するものとなる。なお、以降の説明では、葉ノードの目標ノード床反力中心点を目標接地部位床反力中心点ということもある。また、各目標接地部位軌道は、連続的に変化するように設定されるので、 Qn ( n=l, 2,3,4) も連続的に変化するものとなる。

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

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

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

Q14=Q1* W1 + Q4* W4 ， W1+ 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 (η=14,23,1423) は、そのノードのすべての子ノードの目標ノード床反力中心点の内分点に設定される。図 7は、各ノードの目標ノード床反力中心点 Qn ( n=l, 2, 3, 4,14,23, 1423 ) と重み Wn (n=l, 2, 3, 4,14,23) との関係を示す図である。なお、図 3 ( b ) 中の L23, L14, L 1423 はそれぞれ線分 Q2Q3、 Q1Q4、 Q23Q14 の長さである。

補足すると、 Q1〜Q4 および P (-Q1423)は前記した如く決定されるので、 Wn (n=l, 2,3, 4,14,23) を決定すると、式 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参照）のたわみ補償（より詳しくはコンプライアンス機構 4 2および各脚のリンク機構のたわみによる各接地部位 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 = l ) とされ、また、第.2接地部位 1 0および第 3接地部位 1 0が遊脚となって離床する時期では、重み W23 が 0 (W14= l ) とされる。このように、子ノ一ドを持つ中間ノードの重みは、その中間ノードに葉ノードとして属する全ての接地部位が接地していない時期では 0に設定される。逆に言えば、中間ノードの重みは、その中間ノードに属するいずれかの接地部位が接地している時期（厳密にはいずれかの接地部位に 0でない床反力が作用している時期）では 0には設定されない。

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

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

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

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

C ) 根ノードを除く各ノードの目標ノード床反力中心点 Qn (n=l, 2, 3,4, 14,23) と重み Wn (n=l,2,3，4，14,23) とは、前記式 1〜 3 の関係式を満たす。換言すれば、子ノ一ドを持つ任意の第 nノード (n=14,23, 1423)の目標ノ一ド床反力中心点 Qn は、その第 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 MP) を作用点として付加的に発生させる必要がある。この付加的な並進力とモーメントとを補償全床反力という。また、補償全床反力のモーメント成分を「補償全床反力モーメント Mdmd」（詳しくは目標全床反力中心点（目標 Z MP) まわりの補償全床反力モーメント Mdmd) と呼ぶ。

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

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

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

Mdmdy = -Kthy* Θ berry - Kwy * (d 0 berry/dt) …式 6 ここで、 Kthx， Kthy, Kwx および Kwy は、所定のゲインである。また、（ d 0 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の関節変位指令を算出する。

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

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

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

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

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

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

尚、一般に、要求 1 ) 〜 3 ) の全て、あるいは要求 1 )、 2 ) を完全に満足することはできない。例えば実全床反力を補償全床反力と目標全床反力との合力に一致させながら各接地部位 1 0の目標床反力中心点.まわりの実床反カモ一メントを 0にすることが、物理的に不可能な場合が多い。従って、階層型コンプライアンス動作決定部 1 1 4は、一般には、要求 1 ) 〜 3 )、あるいは要求 1 )、 2 ) をできるだけ満たしつつ、ある妥協点で、各接地部位 1 0の目標接地部位軌道を修正することとなる。以上が制御装置 5 0の各機能的手段（機能的構成要素）の概要である。補足すると、前記階層型コンプライアンス動作決定部 1 1 4、姿勢安定化制御演算部 1 0 4およびロポット幾何学モデル（逆キネマティクス演算部） 1 1 0は、本発明におけるノード動作制御手段に相当するものである。

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

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

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

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

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

続いて S 2 4に進んで、各ノードの重み Wn(n=l, 2, 3,4, 14,23) と各ノードの目標床反力中心点（目標ノ一ド床反力中心点） 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(= F1423act) = F14act + F23actである。また、各葉ノードの実床反力の並進力成分 Flact, F2act, F3act, F4actは、前記実床反力検出器 1 0 8で得られる各接地部位 1 0の実床反力（実接地部位床反力）の並進力成分である。なお、図 1 1の点線で示したベクトルは、前記図 1 0に示した目標ノード床反力の並進力成分である。階層型コンプライアンス動作決定部 1 1 4は、上記のように、実床反力検出器 1 0 8で得られた各接地部位 1 0の実床反力から各ノードの実ノード床反力の並進力成分を決定する。

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

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

葉ノードでないノードの実床反力モーメント成分（本参考例では、 M14act， M23act， M1423act) は、一般的に、 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 ( = M1423ref) と補償全床反力モ一メント Mdmdとの和の水平成分に追従させれば良い。

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

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

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

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

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

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

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

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

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

以上のごとく、各接地部位 1 0の目標接地部位位置（詳しくはその位置の相互の相対関係）を修正する処理が第 1参考例における階層型のコンプライアンス動作である。このコンプライアンス動作では、子ノードを持つ各ノ一ド毎に、そのノードの子孫ノードとなっている各接地部位 1 0の目標接地部位位置の間の相対関係（相対的位置関係）の操作量 (修正量）が各補償角 0 14, Θ 23, Θ 1423 に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位 1 0の目標接地部位位置の相互の相対関係が修正されることとなる。上記のように修正された目標接地部位位置姿勢を修正目標接地部位位置姿勢という。

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

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

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

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

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

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

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

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

第 14 ノード補償床反カモ一メント M14dmdのべクトル V14方向成分を M14dmdv と記述する。尚、ベクトル V14 は階層型コンプライアンス動作決定部 1 1 4のコンプライアンス動作の全体的な説明で定義したベクトルである（図 1 5を参照）。 V14 に直交し、且つ鉛直方向にも直交するベクトルを U14とすると、本参考例では、第.14ノード補償床反力モーメント M14dmdの U14方向成分 M14dmduは 0に設定される。これは、本参考例のロボット 1では、第 14ノード補償角 Θ 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ノード補償角 0 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 '·式 7

M14dmd= (Q14mdfd - Q14) * F14ref '·式 8

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

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

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

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

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

以上のごとく、補償全床反力モーメント分配器 1 1 4 aは、各ノード補償床反力モーメント M1423dmd、 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、 Q23 と目標全床反力 Ftotalref とから、各目標ヅード床反力を求める決定する手法と同様の手法によって、各接地部位 1 0 の目標床反力中心点 Qn(n=l,2，3，4)、修正全床反力中心点 Pmdfd、各修正目標ノード床反力中心点 Q14mdfd、 Q23mdfd と目標全床反力 Ftotalref とから、修正目標接地部位床反力 Fn— refmdfd (η=1,2,3,4) を求めればよい。すなわち、各接地部位 1 0 の目標床反力中心点 Qn(n=l,2，3，4)、修正全床反力中心点 Pmdfd、各修正目標ノ一ド床反力中心点 Q14mdfd、 Q23mdfdから、前記式 1〜 3に基づいて各ノードの重みを決定し、その重みを用いて前記式 4に基づいて修正目標接地部位床反力 Fn— refmdfd ( n=l,2, 3,4) を求めればよい。

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

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

次に、前記補償角決定部 1 1 4 b〜 1 1 4 dのうちの補償角決定部 ( 0 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、終点が Q 14のベクトル、 P— Q23は始点が P、終点が Q23のベクトルである。

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

+ P_Q3 * F3act + P_Q4 * F4act

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

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

Mn_act=∑ (Qn_Qm * Fm_act) …式 1 6 m e {第] ノードの子孫である葉ノードの番号の集合 } であるとすると 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 と M1423dmdのべクトル 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 に通し、 M1423dmdvfilt を得る。なお、 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 に制御ゲイン 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ノード床反力中心点 Q14 のまわりに発生するモーメント M14actを、一般式である前記式 1 6を用いて求める。

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

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

また、式 2 0の代わりに、次式 2 1 (前記式 1 7を具体化した式）を用いても、前述の理由により、実際上はほとんど問題がない。 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 ノード補償角 0 14 は、一般的には、上記のように求められる M14act と先に補償全床反力モーメント分配器 1 1 4 aで求められた第 14 ノード補償床反力モーメント Ml423dmd との偏差（M14act— M14dmd) を 0に近づけるようにフィードバック制御則などにより求めればよい。

伹し、本参考例では、 Θ 1423 の場合と同様の理由によって、 M14act の前記べクトル V14方向の成分 M14actv と M14dmd のべクトル V14 方向の成分 M14dmdvとをそれぞれフィル夕に通したものの偏差に応じて 0 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方向の成分 M14dmdv を補償フィル夕 1 1 4 j ' に通し、 M14dmdvfilt を得る。なお、 M14dmdv は、 M14dmd と V14 との内積演算により求められる。そして、 M14dmdvfilt を、 M14actvfiltから減じたものを、偏差モーメント V14方向成分 M14errv として得る。尚、補償フィルタ 1 1 4 j ' は、制御系における M14dmdvから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。最後に次式 2 3のフィードバック制御則（ここでは比例制御則）の演算によって前記第 14 ノード補償角 0 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方向の成分 M23dradv を補償フィル夕に通したもの M23dmdvfilt から減じてなる偏差モーメント V23方向成分 M23errvからフィードパック制御則' （比例制御則）の演算によって、前記第 23ノード補償角 0 23が算出される。

以上の捕償角決定部 1 1 4 b〜 l 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に近づけるように、該制御量に応じてノード補償角 Θ 1423, Θ 14, 0 23 を決定する（例えば制御量にあるゲインを乗算することで、 0 1423, Θ 14, 0 23を決定する）。

図 1 7の修正目標接地部位位置姿勢算出部 1 1 4 gは、第 1423 ノ一ド補償角 0 1423、第 14ノード補償角 0 14、第 23 ノード補償角 0 23に基づき、前述した階層型コンプライアンス動作の目標接地部位位置姿勢の修正手法（図 1 5および図 1 6を参照して説明した手法）に従って各接地部位 1 0の目標位置および目標姿勢である目標接地部位位置姿勢を修正し、修正目標接地部位位置姿勢を得る。ただし、本参考例においては、各脚 # 1〜# 4の先端部にフリージョイント（球関節 1 2 ) があり、意図的に各接地部位 1 0の姿勢を変化させることはできないので、修正目標接地部位位置姿勢は、実際には、修正目標接地部位位置を意味する。図 2 1は、図 1 7の前記機構変形補償量算出部 1 1 4 nの処理を示す機能ブロック図である。図 2 1に示すように、機構変形補償量算出部 1 1 4. nは、前記補償全床反力モーメント分配器 1 1 4 aから出力される各接地部位 1 0の修正目標接地部位床反力 Fn_refmdfd (n=l,2,3,4) (目標接地部位床反力に、第 1423ノード補償床反力モーメント M1423dmd、第 14ノ一ド補償床反力モーメント M14dmdおよび第 23 ノ一ド補償床反力モーメント M23dmd を付加することによって修正された目標接地部位床反力）によって発生が予想される各脚 # 1〜# 4の変形量（各脚のリンク機構およびコンプライアンス機構 4 2の変形量）である機構変形量 En—mdfd(n=l，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 ( 11=1,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 ノ一ド補償角 6> nの Un成分を決定する。ただし、第 1参考例のように葉ノードでない任意の第 nノードの子ノードの数が 2以下の場合には、第 n ノ一ド補償角 U 成分は 0にする。あるいは、 S 1 1 4の処理を実行する必要はない。

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

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

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

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

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

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

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

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

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

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

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

Τ * Δ Μ— 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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

また、第 2参考例における目標床反力分配器 1 0 2では、第 1参考例と同様に、次の条件 A ' ) 〜F ' ) を満たすように、目標ノード床反力中心点と各ノードの重みと目標ノード床反力とを決定する。

Α ' ) 各葉ノードの目標ノード床反力中心点 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が決定する目標床反力を基に、目標ノード床反力中心点 Qn(n=l,2,3，4，5，6)を決定すればよい。

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

C，）子ノードを持つ任意の第 nノ一ド（n=145, 236, 145236)の目標ノード床反力中心点 Qnは、その第 nノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。より具体的には、第 145 ノードの目標ノード床反力中心点 Q145は、その子ノード（葉ノード）である第 1、 4、 5ノードの目標ノード床反力中心点 Ql， Q4, Q5 を頂点とする三角形の内分点（三角形の境界の点を含む）となり、第 236 ノ一ドの目標ノード床反力中心点 Q236は、その子ノード（葉ノード）である第 2、 3、 6ノードの目標ノード床反力中心点 Q2， Q3, Q6 を頂点とする三角形の内分点（三角形の境界の点を含む）となる。また、第 145236 ノード（根ノ一ド）の目標ノード床反力中心点 Q145236 ( =目標全床反力中心点 P ) は、その子ノード（中間ノード）である第 145、 236 ノードの目標ノード床反力中心点 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の各関節 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は、それぞれ 0 145236、 Θ 145, 0 236を決定するものである。

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

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

すなわち、図 2 6を参照して、各葉ノードの目標ノード床反力の並進力成分 Fn_ref(n=l，2,3，4,5，6)は、目標床反力分配器 1 0 2で決定された目標ノード床反力の並進力成分と同一とされ、根ノードの目標ノード床反力の並進力成分 F145236ref は、歩容生成装置 1 .0 0で決定された目標全床反力の並進力成分 Ftotalref と同一とされる。また、葉ノードおよび根ノード以外の中間ノードの目標ノード床反力の並進力成分 Fn— ref(n=145，236)は、その中間ノ一ドの各子ノードの目標ノード床反力の合力の並進力成分に決定される（ F145ref = Flref + F4ref+ F5ref、 F236ref = F2ref + F3ref + F6ref) この場合、中間ノードの親ノードは根ノードであるので、 F145ref+ F236i'ef= 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 ノード補償床反力モーメント M145dmdおよび第 236 ノード補償床反力モーメント M236dmd に分配するものである。

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

第 145 ノ一ド補償床反力モーメント M145dmd は、第 145補償角 Θ 145 を操作することによって（第 145 ノードに属する第 1、第 4および第 5接地部位 1 0を目標第 145 ノ一ド床反力中心点 <¾ 145 まわりに 0 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、 Θ 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

(=<¾145236mdfd)を導入したとき、これらの 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 ) を満たすように M145236dmd, Ml45dmd, M236dmdを決定する。

1 3 ) M145236dmd+ M145dmd + M236dmd ベクトル 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に近づけるようにノード補償角 0 145236、 Θ 145, 0 236 を決定する。図 3 0および図 3 1 にその例を示す。この場合、本参考例では、 0 145 は、図 3 0に示す如く、偏差 M145act— M145dmd と同方向で、目標第 145 ノード床反力中心点 Q145 を通る軸回りの回転角、 0 236は、図 3 0に示す如く、偏差 M236act— M236dmd と同方向で、目標第 236 ノード床反力中心点 Q236 を通る軸回りの回転角である。また、 145236 は、図 3 1 に示す如く、偏差 M145236act— M145236dmd と同方向（線分 Q145Q236 に垂直な水平方向）で、目標全床反力中心点 P を通る軸回りの回転角である。

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

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

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

次いで、前記第 1参考例と同様に、この M145236actv をローパスフィル夕に通したもの M145236actvfilt と、前記第 145236ノ一ド補償床反力モーメント M145236dmdのべクトル V方向の成分 M145236dmdv を補償フィル夕に通したもの M145236dmdvfiltとの偏差 M145236errv ( == M145236actvfilt - M145236dmdvfilt) に所定のゲイン K145236 を乗算することで、第 145236ノード補償角 Θ 145236が決定される。補償角決定部（ Θ 145決定部） 1 1 4 cでは、まず、目標第 1、第 4、第 5ノード床反力中心点 Ql， Q4, Q5 にそれぞれ実第 1、第 4、第 5 ノード床反力の並進力成分 Flact, F4act, F5act が作用した場合に、第 145 ノード床反力中心点 Q145 のまわりに発生するモーメント M145act を算出する。この場合、算出する M145act は、互いに直交する水平な単位べクトルであるべクトル V145および U145のそれぞれの方向の成分から構成される。ベクトル V145 または U145の向きは、任意でよい。

次いで、この M145act をローノ、。スフィル夕に通したもの M145actfilt と、前記第 145 ノード補償床反力モーメント M145dmd (べクトル V方向の成分およびべクトル U方向の成分）を補償フィル夕に通したもの M145dmdfilt との偏差 M145err ( = M145actfilt - M145dmdfilt) に所定のゲイン行列 K145 (対角行列）を乗算することで、第 145 ノード補償角 0 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, Θ 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, 0 236 を決定する（例えば制御量にあるゲインを乗算することで、 Θ 145236, Θ 145, 6> 236を決定する）。

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

同様に、第 236.ノードの子ノードである第 2、第 3および第 6ノードの目標床反力中心点 Q2， Q3, <¾6 をそれぞれ、第 236 ノードの目標床反力中心点 Q236 を回転中心として、第 236 ノード補償角 0 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 のベクトル（水平ベクトル）と同方向（線分 Q145Q236 と直交する水平方向）の軸心まわりに第 145236 ノ一ド補償角 Θ 145236だけ回転移動する。この回転移動後の Q145， Q236を図 3 1に示す如くそれぞれ Q145'， Q236'とする。従って、第 145236 ノード補償角 0 145236 は、第 145236 ノード（根ノ一ド）の目標床反力中心点 Pの位置を動かさずに、第 145236 ノードの子ノードである第 145、第 236 ノードのそれぞれの目標床反力中心点 Q145， Q236 の位置の相対関係を動かすための操作量である。 .

次いで、図 3 1を参照して、先の回転移動後の目標ノード床反力中心点 Ql'， Q4', Q5'をベクトル Q145— Q145'だけ平行移動し、これにより、第 1、第 4および第 5ノードの最終的な修正後の目標ノード床反力中心点 Ql", 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の目標接地部位位置の相対関係）が修正されることとなる。すなわち、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位 W

8 6

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

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

図 2 5に示した第 2参考例における機構変形補償量算出部 1 1 4 nは、前記第 1参考例のものと同様に、各各脚 # 1〜# 6のコンプライアンス機構 4 2などの変形による影響を補償するための機構変形補償量！！^^！！^！^！！^^ ^ を求める。すなわち、第 2参考例における補償全床反力モーメント分配器 1 1 4 aから出力される各接地部位 1 0の修正目標接地部位床反力 Fn_refmdfd(n=l, 2, 3,4, 5,6，）（目標接地部位床反力に、各ノード補償床反力モーメント M145236dmd、 M145dmd , M236dmd を付加することによって修正された目標接地部位床反力）によって発生が予想される各脚 # 1〜# 6のコンプライアンス機構 4 2などの機構変形量 £11_111(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参考例で説明した各補償角 Θ 145236, Θ 145， 0 236 と S 3 7で推定された床形状偏差とに応じて各目標接地部位位置姿勢が修正され、その修正後の目標接地部位位置姿勢をさらに機構変形補償量に応じて修正することにより、最終的な目標接地部位位置姿勢たる機構変形補償入り修正目標接地部位位置姿勢が得られる。この場合、より詳しくは、各補償角 0 145236, Θ 145, Θ 236 に応じて第 2参考例と同様に修正目標接地部位位置姿勢を求めた後、その修正目標接地部位位置姿勢が床形状偏^に応じて修正され、さらに、これが機構変形補償量に応じて修正されることで、機構変形補償入り修正目標接地部位位置姿勢が得られる。上記した事項以外は、図 9の処理と同じである。

以降、本参考例の第 2参考例と相違する点を具体的に説明する。

まず、本参考例の詳細な説明に入る前に、床形状推定器 1 3 0が推定する上で使用する概念と用語を以下のように定義する。なお、ここでの説明は、便宜上、本参考例の 6脚口ポット 1に限らずに、一般的なロボットの簡略的な図を使用して行なう。

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

この定義から明らかなように、ロポットの目標歩容において第 n接地部位が接地している時期では、目標第 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では、ロボットの目標姿勢（目標歩容の 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— with— bias'は、第 n接地部位床高さ偏差の瞬時推定値に相当するものであるが、本参考例のように上体の実高さを検出しない場合には、一般にバイアス誤差を含んでいる。そのため、 Zfn— with— bias'をパイァス含有瞬間第 n接地部位床高さ偏差と呼ここで、想定第 n床接点 D nは、先に定義したように、第 n接地部位が接地している時期においては、目標第 n接地部位床反力中心点 Q nと同一位置とする。目標歩容において第 n接地部位が接地する直前の時期では、次に接地する時に想定されている目標第 n接地部位床反力中心点 Q nの位置を想定第 n床接点 D nとする。目標歩容において第 n接地部位が離床した直後の時期では、離床時に想定されていた目標第 n接地部位床反力中心点 Q nの位置を想定第 n床接点 D nとする。

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

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

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

以降、葉ノードに対するある種類の入力（状態量） ΑΑ から、階層相対化処理によってある種類の出力 ΒΒ の値を求めることを、「ΑΑ を階層相対化して ΒΒを求める」と言う。

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

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

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

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

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

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

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

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

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

Zn_rei= Zn_witli_bias― Zh_with_bias …式 3 ΰ 以上のルールに従って、すべてのノードに対してノード相対床高さ Zn_rel(n は各ノードの番号）を求める。ただし、根ノードに対するのード相対高さ Zk— rel(k は根ノードの番号）は 0とする。図 4 2に第 1 参考例で示した 4脚ロポットのノード相対高さの算出例を示し、図 4 3 に本参考例における 6脚口ポットのノード相対高さの算出例を示す。バイアス含有第 n ノード高さ Zn—rel— 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 = Zfn

第 nノードが葉ノードでないならば、 Zn_with_bias =∑ (Ζ]·— with— bias * Wj')

ただし、 ∑は；ノード 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' (η=1，2，···，最終ノード番号）

を永める。

次いで、 S 5 2において、制御装置 5 0の前回制御周期で求めた、第 η接地部位床高さ偏差の推定値である推定第 η 接地部位床高さ偏差 (以降、これを前回推定第 η接地部位床高さ偏差 Zfn— estm— ρ と呼ぶ）を階層相対化して得られる出力を前回推定第 n ノード相対床高さ偏差 Zn— reし estm— ρ(η=1,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_reし estm一 p の差と所定の係数（△ TV(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においては、第 n ノード要求モード（n=l，2,3，4，5，6)、第 145 ノード要求モード mdl45dni(i、第 236 ノ一ド要求モード md236dmd および第 145236 ノ一ド要求モード mdl45236dmd を、目標歩容の時間経過に対して図 4 4の夕イミングチャ一トで示すように設定し、これにしたがって現在の要求モードを決定する。ただし、歩容が異なれば、それに応じてノード要求モードのパターンも変えるべきである。なお、図 4 4の第 1段（最上段）および第 2 段のタイミングチャートにおける O N Z O F Fは、接地部位が接地している状態が O Nに対応しており、接地していない状態が O F Fに対応している。

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

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

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

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

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

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

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

Fn_act_z^ Fn_mm …式 3 6

Zn— rel'\ Zn— rei— estm— p …式 3 7 ここで、 Fn_act_zは、実第 n接地部位床反力（n=l，2,… ,最終葉ノード番号）の並進力鉛直成分である。推定許可条件とは、第 ηノード相対床高さ偏差を推定しても（床形状偏差を推定しても）、その推定値が発散しない条件である。ここで発散とは、後述するごとく推定された推定第 η床高さ偏差 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ノード相対床高さ偏差 (n=l, 2，…，最終ノ一ド番号）を推定する。

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

このため、 S 6 0〜 S 7 0の処理は、接地部位の姿勢を制御可能な場合でも、床形状偏差の推定を行なうことができるように構成されている。

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

一方、 S 6 8において、床反力モーメントを制御できると判定された場合には、 S 7 0において、接地部位床傾斜偏差推定処理が行なわれる。この処理は、各接地部位における床傾斜偏差を推定する処理である。この処理は、本出願人が先に提案した特開平 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で求めた第】ノ一ド相対床高さ偏差補正量候補値 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のフローチャート中の式で示す如く、第 i ノードおょぴ第 j ノードのそれぞれの推定ノード相対床高さ偏差 Zi— reし estm、 Zi— reし estmの値が、前回制御周期での値 Zi— reし estm_p、 Zi— rel— estm— pに維持される。

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

このリセット処理は、より一般的に言えば、 Zi一 rel— estm， Zj_rel_estm を、それらの重み付き平均値が 0 という条件、すなわち Wi * Zi— rel— estm + Wj * Zj— reし 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で実行される。

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

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

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

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

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

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

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

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

このリセット処理はより一般的に言えば、 Zi— rel— estm， Zj— reし estm, Zk— rel— estm を、それらの重みつき平均値が 0という条件、すなわち Wi * Zi_rel_estm + Wj * Zj_rel_estm + Wk * Zk— rel— estm = 0 を満足しつつ、てれぞれ Zi— rel— estm— p ,

, Zk_rel_estm_p よりも 0 に近い値に決定するという処理である。なお、 Zi_rel— estm一 p， Zj— reし estm— p , Zk— rel— estm— ρ を 0にリセットするために、前記有限整定関数発生器（特開平 5— 3 2 4 1 1 5号公報）を用いてもよい。

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

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

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

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

すなわち、 Wi * Zi— inc— cand' + Wj * Zj_inc_cand' + Wk * Zk— inc— cand' = 0 という条件 ( Zi— inc— cand' ， Zj_inc_cand' ， Zk_inc_cand'の重み付き平均値が 0 という条件）と、 Zjjnc一 cand'— Zj_inc_cand = Zk— inc— cand'― Zk— inc— cand という条件を満足するように ¾— inc— cand', Zk— inc— cand'を決疋 o。

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

このように Zi— rel— estm , Zj_rel_estm, Zk^rel— estm を決定することで、 Wi * Zi—rel— estm + Wj * Zi一 rel— estm + Wk + Zk— rel— estm = 0 を満足しつつ、 Zj— rel— estm— Zk— reし estmが Zj_inc_cand― Zk_inc_cand に近づくように Zi_rel_estm， Zし reし 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—rel— estm , Zj_rel_estm , Zk一 reし estmが決定される。なお、ここでは、第 i ノードのモードがホ一ルドモ一ドで、第 j ノ一ドおよび第 kノードのモードがリセットモ一ドであるとする。また、同式中のは、 Δ Τ、 Testm の意味は、前記式 3 5のものと同じである。

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

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

更に図 4 9の S 6 6 0 0で、子ノードのモードが「 2つの子ノードのみホ一ルドで、残りがリセット」と判断すると、 S 6 6 1 8において、 2つの子モードのみホールドで、残りがリセットの場合の処理が行われる。その処理では、図 5 5のフローチャート中の式に従って、新たなノード相対床高さ偏差 Zi— reし estm, Zj— rel— estm, Zk_rel_estm が決定される。なお、ここでは、第 i ノードおよび第 j モードのモードが共にホールドモードで、第 kノードのモードがリセットモ一ドであるとする。また、同式中の Δ Τ、 Testm の意味は、前記式 3 5のものと同じである。図 5 5の処理は、より一般的には、 Zi— rel— estm , Zj_reし estm をそれぞれ前回制御周期での値にホールドし、 Zk— rel— estm を、 Wi * Zi_rel_estm + Wj * Zj_rel_estm + Wk * Zk— rel— estm = 0 ( Zi— rel— estm , Zj_reし 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 ノード（n=l, 2，…，最終葉ノード番号）のすベての先祖ノ一ドの推定相対床高さ偏差と推定第 n ノード相対床高さ偏差との和を求め、求めた和を推定第 n接地部位床高さ偏差（推定第 n床高さ偏差） 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，···，最終葉ノード番号）のすベての先祖ノードの瞬間相対床高さ偏差と瞬間第 η ノード相対床高さ偏差との和をもって推定第 η 床高さ偏差 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が接地した直後や床形状が途中で変化したときには、過渡的には、実全床反力は、床形状偏差の影響を受ける。しかし、その後、時定数 Testm でその影響が減衰する。

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

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

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

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

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

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

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

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

また、床形状の推定精度が低下する恐れがある状況あるいはその推定値が発散する恐れがある状況では床形状の推定を中断するように構成したので、前記した作用効果に加え、推定値が不適切なものとなることがない。尚、本参考例に係るロボットの床形状推定装置 1 3 0は前記した手法で床形状を推定すれば足り、推定値に基づいて運動を補正することは必須ではない。 [第 4の参考例]

次に、本発明の第 4参考例に係る移動ロボットの制御装置（特ぃ床形状推定装置）を説明する。第 4参考例においては、それを簡単に説明すれぱ、前記第 3参考例の図 4 0に示した床形状推定器 1 3 0のプロック線図を等価変換したものである。すなわち、直前に推定した床形状の推定値（前回制御周期での推定値などの該推定値の過去値）と、最終的に修正され決定された目標歩容（詳しくはコンプライアンス動作を含む機構変形補償入り修正目標接地部位位置姿勢）（もしくは実関節変位）と、実上体姿勢傾斜偏差 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接地部位位置姿勢 (η=1,2，···，最終葉ノード番号）と、各接地部位 1 0の目標接地部位位置から見た該接地部位 1 0の目標第 η接地部位床反力中心点と、前記姿勢傾斜偏差 0 berr とを基に、機構変形補償入り修正目標第 n接地部位位置姿勢を、目標全床反力中心点を回転中心として 0 berr だけ回転移動させてなる、回転後機構変形補償入り修正目標第 n接地部位位置姿勢 (η=1，2, ···，最終葉ノード番号）を求める。求められた回転後機構変形補償入り修正目標第 η接地部位位置姿勢 (η=1，2，…，最終葉ノード番号）から、目標歩容上での想定第 η床接地位置 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床高さ偏差） 2 —681：111 (11=1，2, "'，最終葉ノ一ド番号）を求める。ここでの床高さ偏差の推定処理のサブルーチン処理は、図 4 1の S 5 6〜S 7 2までの処理と同一である。

なお、特開平 1 0— 2 7 7 9 6 9号公報に示したように、接地部位として姿勢を制御可能な足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、図 4 1の S 7 0に関して説明した如く、接地部位床傾斜偏差推定処理を行なうのが望ましい。

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

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

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

[第 5参考例]

前記第 1〜第 4参考例では、子ノードを持つノードの、該子ノードの総数が、 3個以下の場合を例にとって説明したが、子ノードの数が 4つ以上の場合にも、重み付き平均の概念を持ち込むことによって、以下に示すようにモーメントおよび傾斜角の概念を拡張できる。

目標値、実際値、偏差のいずれに関しても概念を拡張できるの、ここでは、目標値、実際値、偏差の区別をせず、一般的な接地部位床反力の組 Fn(n=l，2，...）に対するモーメントを、以下のように拡張して、定義する。第 n接地部位の床反力の組 Fn(n=l，2,...，最終葉ノード番号）を階層相対化して、第 nノード相対床反力 Fn一 rel(n=l，2,…，最終ノード番号）を求める。

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

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

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

第 nノードの第 j番目の子ノードの重みを第 j 要素として持つ列べクトルを UWnとする。すなわち、下式 3 8のように UWn を定義する。ここで「T」は、転置を意味し、行べクトルの転置によって列べクトルを表現することとする。

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

第 nノードの第：！番目の子ノードの相対床反力 Faj_rel を第 j 要素 (j=l,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ノードのすべての子ノード aj (j=al,a2,〜，ar)の相対床反力を要素とするベクトル（Fal— rel, 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と呼ぶ。

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

また、接地部位床反力偏差の組 Fn(n=l，2，'"）に対する第 nノード拡張床反力モーメントを第 n ノード拡張床反力モーメント偏差— Mn_exp_errと呼。

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

接地部位高さの組

，最終葉ノード番号）を階層相対化して、第 ηノード相対高さ Ζη一 rel(n=l,2, '"，最終ノード番号）を求める。

第 nノードの第 j 番目の子ノードの相対高さ Zaj— rel を第 j 要素 2 9

(； j=l,2，〜，r)とする列べクトルを第 nノード相対高さベクトル Zn— reし c とする。

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

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

Zn— rel— c = Hn * 0 n_exp …式 4 2

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

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

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

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

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

Mn_dmd= Cn_mech * Mn— exp— dmd "'式 4 3 一方、前記式 1 0を一般化すると次式 4 4になる。

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

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

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

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

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

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

図 5 7を参照して、この処理を説明すると、まず、前記図 5 6で説明した如く算出される推定第 n接地部位床反力と、第 n接地部位の実床反との差（各ノード毎の差の組）から、拡張モーメントの定義にしたがつて、第 n接地部位床反力推定誤差 Ffn一 estm— err (n=l,2，...）に対応する第 nノード拡張床反力モーメント推定誤差 Mn— exp— estm_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は、対角行列とは限らない。

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

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

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

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

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

以上説明した第 1〜第 5参考例を基礎として、以下に本発明の実施形態を説明する。 [第 1実施形態]

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

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

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

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

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

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

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

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

まず、口ポット 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の先端部に連結されている _b この例では、各脚 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 2と各腕 5 4の先端部には、 6軸カセンサなどの外力検出手段が設けられている。

本実施形態では、上記のようにひざまずいた口ポット 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参考例などと同様の方針によって、図示の如く、目標床反力中心点 (¾11(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は、各中間ノードおよび根ノードのノード補償床反力モーメントだけでなく、葉ノードのうちの第 nノード（_n=l，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ノード補償角 Θ η(η=1,2，5，6) を決定する 0 η 決定部は、特開平 1 0— 2 7 7 9 6 9号公報に示した足平補償角決定部と同様の処理によって、第 ηノード補償角 Θ η(η=1，2，^6) を決定する。なお、これらのノード以外の各中間ノードおよび根ノードの補償角 0 η(η=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床高さ偏差 Zfn一 estm(n=l,2，3，4, 5,6) を求めることに加えて、第 n接地部位の接地部位床傾斜偏差 Θ fn(n=l,2, 5,6)を推定する。この接地部位床傾斜偏差 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 8， 5 8の高さの差の変化の半分け変化する。そこで、コンプラインス制御による左右の足平 5 8 , 5 8の高さの差の変化に応じて、左右の膝の高さの差の変化に応じた上体 5 3の姿勢修正と同様の動作を、左右の足平の高さの差の変化の場合に較べて半分の量だけ行なうようにする。

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

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

以上が、上体 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, 左右の足平の高さの差の修正量 Zfootdiff dfd を求める。 . . Zkneediffmdfd

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

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

+ 目標第 3接地部位位置

…式 4 7

Zkneesummdfd

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

一目標第 4接地部位位置

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

一目標第 3接地部位位置

…式 4 8

Zfootdiffmdfd

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

—目標第 2接地部位位置

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

+ 目標第 1接地部位位置

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

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

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

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

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

Xbkneediffmdfd = Kxkneediff * Zkneediffmdfd

Θ bkneediffmdfd= Kthkneediff * Zkneediffmdfd

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

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

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

Xbfootdiffm dfd = Kxfootdiff * Zfootdiffmdfd

Θ bfootdiffmdfd = Kthfootdiff * Zfootdiffmdfd

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

次、いで、上記と同様に、左右の膝の高さの和の修正量 Zsummdfd に応じて、これによつて接地部位（膝および足平）と床との間でこじりあるいは滑りが生じないための、膝高さ和用上体位置修正量 Xbsummdfd と膝高さ和用上体姿勢修正量 0 bsummdfd とを、左右の膝の高さの和の修正量 Zsummdfd とその瞬間（現在時刻）における口ポット 5 1の目標姿勢を基に幾何学演算によって求める。 +これ以外にも、あらかじめ、いくつかの代表的な目標姿勢に対して、左右の膝の高さの和の修正量と上体姿勢の修正量の関係を求めておいて、これをマツプあるいは関数として記憶し、これと左右の膝の高さの和の修正量 Zsmnmdfd とを基に Xbsummdfd と 0 bsummdfd とを求めても良い。

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

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

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

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

また、別の言 Ι 方をすれば、口ポット 5 1の全体の姿勢制御の安定性を高めるために、目標全床反力中心点（目檩 Z M P ) まわりに発生する実床反力モーメントの制御性を優先し、ある所定の接地部位 A (ここでは膝）の相対高さまたは傾斜角度の操作量、あるいはある所定の接地部位 Aを子孫ノードとして持つノードの相対高さまたは傾斜角度の操作量，としての補償高さおよび補償角のうちの少なくともいずれか一方を決定する手段と、前記補償高さおよび補償角のうちの少なくともいずれか一方に応じて、上体位置あるいは上体を含む複数部の前記重み付き平均位置を概ね目標歩容での位置に保持しつつ、上体の姿勢および位置のうちの少なくともいずれか一方の修正量を決定する手段と、前記所定の接地部位 Aを除く所定の接地部位 B (ここでは足平）の位置および姿勢のうちの少なくともいずれか一方と、前記修正量によって修正した上体位置姿勢とに基づいて、関節変位を決定する逆キネマティクス演算手段を備えるようにした。

なお、第 1実施形態では、口ポット 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以外の所定の部位の位置および姿勢のうちの少なくともいずれか一方を修正するようにしても良い。

[第 2実施形態]

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

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

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

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

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

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

具体的には、 Ln— knee— r を第 nノード補償角（n=12，34，1234)に対する右膝関節変位の感度、 Ln— knee_l を第 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_l= 1、 L12— hip— r= 0、 L12— hip— 1= 0

L12_knee_r= al2_¾ L12— knee— 1=— al2、 L34_hip_r= a34

L34_hip_l=一 a34、 L34_knee_r= 0、 L34_knee_l= 0

…式 5 2

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

次いで、次式 5 3〜5 6により各関節変位修正量を決定する。ただし， 5 2

Θ knee_r は、右膝関節変位修正量、 0 knee— 1 は、左膝関節変位修正量、 Θ hip_r は、右股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）、 0 hipJL は、左股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）とする。

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

+ L34_knee_r * θ 34

…式 5 3

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

+ L34— knee— 1 * Θ 34

…式 5 4

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

+ L34_hip_r * Θ 34

"'式 5 5

Θ hip_l= L1234_hip_l * Θ 1234 + L12_hip_l * Θ 12

+ L34_hip_l * Θ 34

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

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

このような ϋ節変位の蓚正により、目標全床反力中心点（目標第

123456 ノード床反力中心点）とロボット 5 1の全体重心（あるいは、上体重心、 .あるいは、上体 5 3を含む複数の部位の重心）を結ぶ線分と、接地部位（膝および足平 5 8 ) に対応した第 ηノード（η=12,34, 1234)の傾斜角虔との間の相対角度が、前記換算された各ノード'補償角 Θ 12 ,

Θ 23, 0 1234 だけ変化する。したがって、実床反力が忠実に制御され、口ポット 5 1の姿勢安定性および接地性が向上する。 - 上記の如く関節変位指令（目璩興節変位）を直接修正するわりに、 , 酪同一の目標関節変位を生じさせるために優先的に位置姿勢しを修正する接地部位（具体的には、足平 5 8 .、手 6 2 ) の修正後の目標位置姿勢である優先修正目標接地部位位置姿勢を決定するようにし、決定された優先修正目標接地部位位置姿勢を基に.逆キネマティクス演算によって関節変位指令を決定するようにしても良い。

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

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

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

[第 3実施形態]

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

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

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

また、図示のように、臀部 7 2の上側には、ロボット 7 1 基体である胴体（上体） 7 7が設けられ、この胴体 7 7の上部の両側部から肩関節 7 8， 7. 8を介して腕 7 9， 7 9が延設されている。なお、腕 7 9は、前記第 2実施形態のロボット 5 1のものと同一構造でもよい。さらに、胴体 7 7は、関節 8 CTを介して臀部 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が搭載されている。また、各脚 5 5および各腕 7 9め先端部には、 6軸力センサなどの外力検出手段が設けられている。

本実施形態では、臀部 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実施形態.などと同様の方針によって、図示の却く、目標床反力中心点 (¾11(11=1，2,3,4, 12,34, 1234)が対応づけられる。なお、根ノードである第 1234ノードの目標床反力中心点 Q1234は、目標全床反力中心点 Pと同一である。

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

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

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

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

以上説明した以外の制御装置 5 0の処理は、前記第 2実施形態と同様でよい。

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

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

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

推定第 ηノード床反力

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

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

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

一-実第 ηノード外床反力 …式 5 8 例えば、前記第 6実施形態において、膝の床反カを検知する床反カセンサが備わっていない場合、あるいは、膝の床反カを検知する床反カセンサが故 I した場合には、推定第 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ノード床反力との差の前回制御周期の値あるいは前記差をローパスフィル夕に通した値を第 nノードの目標床反力に付加しても良い。

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

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

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

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

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

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

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

補足すると、実第 nノード相対床高さ等を定義するための重みは、目標床反力分配器が決定する重みに必ずしも一致させなくてはならない訳ではないが、一致させるならば、（この場合.、自動的に、根ノードの目標床反力中心点は目標全床反力中心点に一致する、）コンプライアンス動作用の第 nノード補償角（_n=l，2,〜）に第 nソード推定床傾斜偏差.を付加するだけで、床形状偏差の影響を打ち消すことができるので、床形状偏差の影響を打ち消すための演算量を低減することができる。 ' ざらに、. 推定床形状偏差を床形状の地図情報として記憶しておき、次回同一場所を歩行する時に、記憶しておいた床形状の地図情報を ¾に目檁歩容を生成するようにしても良い。

尚、前述の各実施形態における演算処理機能を示すブロック線図もしくはフローチヤ一トにおける演算処理順序を等価変形をしても良い。産業上の利用可能性

以上.の如く、本発明は、脚式移動口ポットの脚の先端部以外の部位を接地させるような状況でも床反力を適切に制御して、高い安定性で'円滑な動作を可能とする脚式移動口ポットを提供し得るものとして有用である。

Claims

1' 6 3 請求の範囲

1 . 基体と、該基侔の移動のために該基体に連結されて外部と接触する複数のリンク機構とを備えると共に、 '該基体と各リンク機構の先端部とめ間に、該基体に対して各リンク機構の先端部を可動とする複数の関節が設けられ、前記複数のリンク機構のうちの 1つ以上の特定リンク機構の先端部と前記基体との間の所定部位で外部と接触する特定動作姿勢に動作可能な脚式移動ロポッ卜の制御装置において'、

前記検出または推定された外力が前記目標外力に近づくように；少な ' くとも前記所定部位と前記基-体との間に存在する関節の変位を制御する関節制御手段とを備えたことを特徴とする移動ロボットの制御装置。

2 . 前記特定リンク機構は脚体であ.ることを特徴とする請求の範囲第 1 項に記載の移動口ポットの制御装置。

3 . 前記特定リンク機構は、前記基体に関節を介して連結された臀部から延設された脚体であり、前記所定の部位は、前記臀部であ ·ることを特徴とする請求の範囲第 1項 fc記載の移動.ロボットの制御装置。

4 . 前記移動口ポッドの前記基体などの第 2の所定部位の実姿勢を検出する実姿勢検出手段と、該第 2の所定部位の目標姿勢を決定する目標運動決定手段とを備え、前記目標外力決定手段は、少なくとも前記第 2の ' 所定部位の実姿勢と目標姿勢との偏差に応じて前記目標外力を決定することを特徴とする請求の範囲第 1項に記載の移動ロポットの制御装置。

5 . 前記移動口ポットの前記基体などの第 2の所定部位の実姿勢を検出する実姿勢検出手段と、該第の所定部位の目標姿勢を決定する目標蓮動決定手段とを備え、前記関節制御手段は、前記第 2の所定の.部位の実姿勢と.目標姿勢との偏差に応じて外力の操作量を決定する手段とを備え、前記目標外力と前記外力の操作量との合力に前記検出または推定された外力を近づけるように前記関節の変位を制御することを特徴とする請求の範囲第 1項に記載の移動ロボットの制御装置。