WO2001087548A1

WO2001087548A1 - Dispositif servant a deduire la forme du sol pour robot mobile pourvu de jambes

Info

Publication number: WO2001087548A1
Application number: PCT/JP2001/004118
Authority: WO
Inventors: Toru Takenaka; Tadaaki Hasegawa; Takashi Matsumoto
Original assignee: Honda Giken Kogyo Kabushiki Kaisha
Priority date: 2000-05-19
Filing date: 2001-05-17
Publication date: 2001-11-22
Also published as: EP1291136A1; US6920374B2; EP1291136A4; JP2001322076A; DE60141016D1; JP3634238B2; US20030125839A1; EP1291136B1

Description

明細書脚式移動ロボッ卜の床形状推定装置技術分野

この発明は脚式移動ロボッ卜の床形状推定装置、より具体的には脚式移動 πボッ卜が接地する床の傾斜などの形状を推定する装置に関し、さらには推定した床形状に基づいて床反力を制御することを可能とする装置に関する。背景技術

床形状を推定する技術としては、本出願人が先に特開平 6 - 3 1 6 5 8号公報で提案した技術、および論文「路面形状に偏差のある環境に対する適応能力を持つ 2足歩行ロボットの開発」（日本ロボット学会誌、第 1 4巻第 4号、 1 9 9 6 年 5月号）記載の技術が知られている。

以下に後者（ロボット学会論文記載技術）の概要を説明し、その問題点を指摘する。

この従来技術では、足平部が、足平上面部、クッション部、足底の上下 3層構造となっていて、足平上面部と足底の間の相対角度と間隔を検出するためにポテンショメータを足平の 4隅に備えると共に、足平上面部の絶対傾斜を検出するための傾斜計を備える。また、足底の四隅にはスパイクを備える。尚、この明細書において『足平』は足部を意味するものとして使用する。

この従来技術にあっては、遊脚の足平の着地時に、ポテンショメ一夕と傾斜計の情報を基に、ならい動作によって足平を床面にならわせようとする。その結果、足底の四隅のスパイクが床に接地した状態になり、足底は床面に平行になる。このとき、傾斜計により得られる足平上面部の絶対位置姿勢と、ポテンショメ一夕から得られる足平上面部と足底の間の相対角度と間隔を基に、幾何学的演算により床の高さと傾きが検出される。

しかしながら、この従来技術には以下のような問題点があった。即ち、ならい動作によって足平が床面にならうまで、床反力を操作する姿勢安定化制御を行うことができなかった。またグリップを増すために足底にゴム等の柔らかい材料を貝占ると、推定誤差が増えてしまう不都合があった。

さらに、足平がベたに着地しない（即ち、足底が着地の瞬間に床に面接触しない）歩行では、両脚支持期の間に床の高さを正確に推定することができなかった。また、直立時など両足平が床にベたに接地している状態では、各足平の接地面傾斜と両足平接地面間の高低差を同時には推定することができなかった。また、接地の瞬間しか推定することができなかったので、着地した後に床が変形したときに変形後の床形状を推定することができなかった。

他方、前記した従来技術の前者、即ち、特開平 6 - 3 1 6 5 8号公報記載技術の場合、床反力を操作する姿勢安定化制御を行いながら、床の傾斜を推定することは可能であつたが、両脚支持期では両足平の接地面と交差または接する平面の傾きだけを推定すると共に、片脚支持期では支持脚の接地面の傾斜だけを推定するに止まっていた。即ち、これらの傾きを同時に推定することはできなかった。発明の開示

従って、この発明の目的は、従来技術では困難であった床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定できるようにした脚式移動ロボットの床形状推定装置を提供するとあ

さらに、本出願人は先に特開平 1 0— 2 7 7 9 6 9号公報において、脚式移動ロボットの姿勢傾斜を検出し、目標全床反力中心点まわりの補償全床反力モ一メントを決定し、各足平に分配して目標全床反力中心点まわりと目標各足平床反力中心点まわりに、それぞれ所定角度回転させるように足平の位置姿勢を決定し、よって広域的のみならず局所的な傾斜や突起を有する床でも安定した姿勢で歩行させることができる技術を提案している。

従って、この発明の第 2の目的は床形状の推定結果を基に足平の軌道（パターン）を修正することにより、そのような先に提案した技術で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれによる床反力の誤差に起因する定常偏差を解消するようにした脚式移動ロボットの床形状推定装置を提供することにある。

上記の目的を達成するために、請求項 1項にあっては、少なくとも上体と、前記上体に第 1の関節を介して連結されると共に、その先端に第 2の関節を介して連結される足部を備えた複数本の脚部を備えると共に、前記足部に接地する床面から作用する床反力に応じて変形するコンプライアンス機構を備え、少なくとも前記上体に対する前記足部の実位置姿勢を検出し、前記足部の目標位置姿勢に追従させるように前記関節を駆動する関節駆動手段を備えた脚式移動ロボットの床形状推定装置において、前記上体の鉛直軸方向に对する傾斜を検出する傾斜検出手段、前記足部の実位置姿勢および/または前記足部の目標位置姿勢と前記検出された傾斜に基づき、前記足部が接地する床面を含む歩行環境における前記足部の位置姿勢を算出する対歩行環境足部位置姿勢算出手段、前記床面から作用する床反力による前記コンプライアンス機構の変形が加えられた前記歩行環境における足部の変形位置姿勢を、前記コンプライアンス機構をモデル化して得た機構コンプライアンスモデルを用いて算出する変形足部位置姿勢算出手段、および前記算出された足部の変形位置姿勢に基づき、前記足部が接地するそれぞれの床面の形状（推定第 n足平床傾斜（偏差））およびそれらの相対位置関係（推定両脚間床傾斜（偏差）、あるいは推定両脚間床高低差（偏差））を示す床形状推定値を算出する床形状推定値算出手段、を備える如く構成した。これにより、従来技術では困難であった各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができる。

更には、前記モデルが、それぞれの足部（各足平）に作用する床反力と、それによるそれぞれ足部の位置姿勢の変化を記述する第 1のモデル（足平機構コンプライアンスモデル）と、目標全床反力中心点に作用する床反力と、それによるそれぞれ足部間の相対位置の変化を記述する第 2のモデル（両脚間機構コンプライアンスモデル）からなる如く構成した。これにより、より簡易な構成で各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができる。

更には、前記床形状推定値算出手段は、高周波成分を除去するフィル夕を備える如く構成した。これにより、各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、より一層精度良く推定することができると共に、制御系の発振を防止することができる。

更には、前記算出された床形状推定値に基づいて前記足部の目標位置姿勢を修正する修正手段を備える如く構成した。これにより、前記した効果に加え、複合コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれによる床反力の誤差に起因する定常偏差を解消することができる。

更には、前記ロボットの少なくとも前記足部の目標位置姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記口ボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反カを検出する実床反力検出手段、および前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモ一メントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量を決定する足部回転量決定手段、を備えると共に、前記修正手段は、前記算出された床形状推定値に基づいて前記足部回転量を修正する足部回転量修正手段、および前記修正された足部回転量に基づいて前記足部の位置姿勢が回転するように前記目標位置姿勢を修正する足部位置姿勢修正手段、を備える如く構成した。これにより、前記した効果に加え、脚式移動ロボットに作用する床反力を干渉を生じることなく、容易かつ適切に制御することができる。

更には、前記ロボットの少なくとも前記足部の目標位置姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記口ボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反カを検出する実床反力検出手段、および少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量を決定する足部回転量決定手段、を備えると共に、前記修正手段は、前記算出された床形状推定値に基づいて前記足部回転量を修正する足部回転量修正手段、および前記修正された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正する足部位置姿勢修正手段、を備える如く構成した。これにより、前記した効果に加え、脚式移動ロボットに作用する床反力を干渉を生じることなく、容易かつ適切に制御することができる。

更には、前記足部位置姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正する如く構成した。これにより、前記した効果に加え、脚式移動ロボットに作用する床反力を干渉を生じることなく、容易かつより適切に制御することができる。

更には、前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反カモ一メントから前記足部に作用する床反力モ一メントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量を決定する足部移動量決定手段、を備え、前記足部位置姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置姿勢を修正する如く構成した。これにより、前記した効果に加え、特に姿勢制御に重要な全床反力を一層適切に制御することができる。

更には、前記足部位置姿勢修正手段は、前記ロボットの姿勢偏差に基づいて前記目標位置姿勢をさらに修正する如く構成した。これにより、前記した効果に加え、ロボットの姿勢をより一層安定に制御することができる。

更には、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モ一メントを求め、前記足部回転量決定手段おょぴまたは前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反カモ —メントに基づいて前記足部の回転量およびノまたは移動量を決定する如く構成した。これにより、前記した効果に加え、床反力を姿勢安定化のために、より適切に制御することができる。

更には、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求める如く構成した。これにより、前記した効果に加え、姿勢安定化能力をより一層向上させることができる。

更には、前記足部回転量決定手段および/または前記足部移動量決定手段は、前記姿勢安定化補償全床反カモ一メントが前記複数本の脚部のそれぞれに分配されるように、前記足部の回転量および/または移動量を決定する如く構成した。これにより、前記した効果に加え、複数の脚部の負荷を適正に分配することができ、床面との圧力分布も局所的に過大に作用することがない。

更には、前記ロボットの少なくとも前記足部の目標位置姿勢を含む連動パターンと、前記ロボットに作用する全床反力の目標軌跡パターンからなる前記ロボットの歩容を生成する歩容生成手段、前記ロボットの姿勢安定化のための補償全床反力を算出する姿勢安定化補償全床反力算出手段、前記足部に作用する実床反力を検出する足部実床反力検出手段、および前記目標歩容の全床反力と前記補償全床反力を分配する床反力分配手段、を備えると共に、前記修正手段は、前記分配された目標歩容の床反力と補償床反力と前記検出された足部実床反力および前記算出された床形状推定値に基づいて前記目標足部位置姿勢を修正する如く構成した。これにより、前記した効果に加え、足部の修正量を一層適切に分配することができるので、姿勢安定化のためのより大きな復元力とより高い接地性を得ることができる。

更には、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの傾き偏差に基^'いて求める如く構成した。これにより、前記した効果に加え、姿勢安定化能力をより一層向上させることができる。図面の簡単な説明

第 1図は、この発明の一つの実施の形態に係る床形状推定装置を適用した脚式移動ロボット、より具体的には 2足歩行ロボットを全体的に示す概略図である。第 2図は、第 1図に示す脚式移動ロボットの足部の構造を示す説明側面図である。

第 3図は、第 1図に示す脚式移動ロボットの制御ュニットの詳細を示すブロック図である。

第 4図は、先に提案した脚式移動ロボットの制御装置およびこの発明の一つの実施の形態に係る脚式移動 Πボットの床形状推定装置の構成および動作を機能的に示すプロック図である。

第 5図は、第 1図に示す脚式移動ロボットが平地を歩行するときの運動パ夕一ンの一例を示す説明図である。

第 6図は、第 5図の運動パターンに対応する目標全床反力中心点（目標 Z M P ) 軌跡の床面上軌跡を示す説明図である。

第 7図は、第 5図の運動パターンに対応する目標全床反力中心点（目標 Z M P ) 軌跡のタイム ·チャートである。

第 8図は、第 5図の運動パターンに対応する所定の条件を満たすように設定した目標第 1足平床反力中心点軌跡のタイム ·チャートである。

第 9図は、第 5図の運動バターンに対応する所定の条件を満たすように設定した目標第 2足平床反力中心点軌跡のタイム ·チヤートである。

第 1 0図は、第 4図と同様に、先に提案した脚式移動ロボットの制御装置の動作を示すフロー 'チャートである。

第 1 1図は、第 1 0図フロー ·チヤ一トの内の両脚補償角などの演算処理を行う、第 4図に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第 1足平と第 2足平に実各足平床反力が作用している状況を示す説明図である。

第 1 2図は、第 1 1図に示す状況における目標全床反力の設定を示す説明図である。

第 1 3図は、第 1 1図に示す状況における目標各足平床反力の分配を示す説明図である。

第 1 4図は、第 1 1図に示す状況における補償全床反力モーメントを示す説明図である。

第 1 5図は、第 1 1図に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線べクトル Vを示す説明図である。

第 1 6図は、第 1 1図に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標 Z M P ) まわりに、所定角度 0 d b Vだけ回転させたときの状態を示す説明図である。第 1 7図は、第 1 1図に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度 0 n X , θ n yだけ回転させたときの状態を示す説明図でめる。

第 1 8図は、第 4図の複合コンプライアンス動作決定部の演算処理を示すプロック図である。

第 1 9図は、第 1 8図に示す補償全床反力モーメント分配器、の演算処理を示すブロック図である。

第 2 0図は、第 1 8図に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム · チャートである。第 2 1図は、第 2 0図の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。

第 2 2図は、第 2 1図と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。

第 2 3図は、両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメント V方向成分 M d m d d b Vを示す説明図である。

第 2 4図は、第 1 8図に示す両脚補償角決定部の演算処理を示すブロック図である。

第 2 5図は、第 1 8図に示す各足平の補償角決定部の演算処理を示すブロック図である。

第 2 6図は、第 1 8図に示す機構変形補償入り修正目標足平位置姿勢算出部の演算処理を説明するための説明図である。

第 2 7図は、第 2 6図と同様に、第 1 8図に示す機構変形補償入り修正目標足平位置姿勢算出部の演算処理を説明するための説明図である。

第 2 8図は、第 1 0図フロー 'チヤ一トの内の両脚補償角などの決定作業を示すサブルーチン · フロー 'チヤ一トである。

第 2 9図は、先に提案した制御装置の変形例を示す第 1 6図と同様の説明図で、足平位置の修正動作の別の例を示す説明図である。

第 3 0図は、この発明の一つの実施の形態に係る脚式移動ロボッ卜の床形状推定装置の構成を示す、第 1 8図と同様なブロック図である。第 3 1図は、第 3 0図に示す装置が床形状を推定するときに使用する定義の説明図である。

第 3 2図は、同様に第 3 0図に示す装置が床形状を推定するときに使用する定義の説明図である。

第 3 3図は、第 3 0図に示す床形状推定器の処理を機能的に示すブロック図である。

第 3 4図は、第 3 3図に示す床形状推定器の機構コンプライアンスモデルの処理を機能的に示すプロック図である。

第 3 5図は、同様に第 3 3図に示す床形状推定器の機構コンプライアンスモデルの処理を機能的に示すプロック図である。

第 3 6図は、この発明の一つの実施の形態における足平床傾斜偏差推定値の発散を説明するための説明図である。

第 3 7図は、この発明の一つの実施の形態における足平床傾斜偏差推定中断器の処理を示すフロー ·チャートである。

第 3 8図は、第 3 7図の処理を説明するタイム ·チヤ一トである。

第 3 9図は、第 3 7図の処理を説明する説明図である。

第 4 0図は、この発明の一つの実施の形態における両脚間床傾斜偏差推定中断器の処理を示すフロー■チャートである。

第 4 1図は、第 4 0図の処理を説明するタイム ·チヤ一トである。

第 4 2図は、この発明の第 2の実施の形態に係る脚式移動ロボットの床形状推定装置の構成を示す、第 3 0図と同様なブロック図である。

第 4 3図は、第 4 2図に示す第 2の実施の形態で使用する定義の説明図である ο

第 4 4図は、第 2の実施の形態における床形状推定器の両脚間床傾斜偏差推定部の処理を機能的に示すプロック図である。

第 4 5図は、第 2の実施の形態における床形状推定器の足平床傾斜偏差推定部の処理を機能的に示すプロック図である。

第 4 6図は、第 1および第 2の実施の形態に係る脚式移動ロボットの床形状推定装置で使用するセンサの別の例を示す説明図である。第 4 7図は、第 1および第 2の実施の形態に係る脚式移動ロボットの床形状推定装置で使用するセンサのさらに別の例を示す説明図である。発明を実施するための最良の形態

以下、添付図面を参照してこの発明の一つの実施の形態に係る脚式移動ロボットの床形状推定装置を説明する。尚、脚式移動ロボットとしては 2足歩行ロボットを例にとる。

第 1図は、この実施の形態に係る床形状推定装置を適用した脚式移動ロボット、より具体的には 2足歩行ロボットを全体的に示す概略図である。

図示の如く、 2足歩行ロボット（以下「ロボット」という） 1は左右それぞれの脚部（脚部リンク） 2に 6個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。 6個の関節は上から順に、股（腰部）の脚部回旋（回転）用の関節 1 0 R， 1 0 L (右側を R、左側をとする。以下同じ）、股（腰部）のロール方向（X軸まわり）の関節 1 2R, 1 2 L、股（腰部）のピッチ方向（Y軸まわり）の関節 1 4R, 1 4 L、膝部のピッチ方向の関節 1 6 R, 1 6 L、足首のピッチ方向の関節 1 8 R, 1 8 L、同ロール方向の関節 2 0 R, 20 Lから構成される。

関節 1 8 R CL) , 2 0 R (L) の下部には足平（足部） 22 R, 22 Lが取着されると共に、最上位には上体（基体） 24が設けられ、その内部にマイクロコンピュータからなる制御ユニット 2 6 (後述）などが格納される。上記において股関節（あるいは腰関節）は関節 1 O R (L) , 1 2R (L) , 1 4 R (L) から、足関節（足首関節）は関節 1 8 R (L)， 2 0 R (L) から構成される。また股関節と膝関節とは大腿リンク 28 R， 2 8 L、膝関節と足関節とは下腿リンク 3 0 R, 3 0 Lで連結される。

上記の構成により、脚部 2は左右の足についてそれぞれ 6つの自由度を与えられ、歩行中にこれらの 6 * 2 = 1 2個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に 3次元空間を歩行させることができる（この明細書で「*」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す'）。尚、この明細書で後述する上体 2 4の位置およびその速度は、上体 2 4の所定位置、具体的には上体 2 4の重心位置などの代表点の位置およびその移動速度を意味する。

第 1図に示す如く、足関節の下方には公知の 6軸力センサ 3 4が取着され、力の 3方向成分 F x， F y， F zとモーメントの 3方向成分 M x , M y , M zで足部の着地の有無および床反力（接地荷重）などを示す信号を出力する。また、上体 2 4には傾斜センサ 3 6が設置され、 Z軸（鉛直方向（重力方向））に対する傾きとその角速度に応じた信号を出力する。また各関節の電動モータにはロー夕リエンコーダが設けられ、その回転量を示す信号を出力する。

第 2図に示すように、足平 2 2 R ( L ) の上方には、ばね機構 3 8が装備されると共に、足底にはゴムなどからなる足底弾性体 4 0が貼られてコンプライアンス機構 4 2を構成する。ばね機構 3 8は具体的には、足平 2 2 R ( L ) に取り付けられた方形状のガイド部材（図示せず）と、足首関節 1 8 R ( L ) および 6軸力センサ 3 4側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材（図示せず）とからなる。

図中に実線で表示された足平 2 2 R ( L ) は、床反力を受けていないときの状態を示す。床反力を受けると、コンプライアンス機構 4 2においてパネ機構 3 8 と足底弾性体 4 0がたわみ、足平は図中に点線で表示された位置姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は前記した特開平 5— 3 0 5 5 8 4号に記載されているので、詳細な説明は省略する。

さらに、第 1図では図示を省略するが、ロボット 1の適宜な位置にはジョイスティック 4 4が設けられ、外部から必要に応じて直進歩行しているロボット 1を旋回させるなど歩容に対する要求を入力できるように構成される。

第 3図は制御ュニット 2 6の詳細を示すプロック図であり、マイクロ . コンビュ一夕から構成される。そこにおいて傾斜センサ 3 6などの出力は AZD変換器 5 0でデジタル値に変換され、その出力はバス 5 2を介して R AM 5 4に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ 5 6を介して R AM 5 4内に入力される。制御ュニット 2 6の内部には C P Uからなる第 1、第 2の演算装置 6 0， 6 2 が設けられており、第 1の演算装置 6 0は後述の如く、ロボット 1が接地する床 (床面）の形状を推定すると共に、 R O M 6 4に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、 R AM 5 4に送出する。また第 2の演算装置 6 2は R A M 5 4からその指合と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出して D ZA変換器 6 6とサーボアンプを介して各関節を駆動する電動モータに出力する。

この実施の形態に係る脚式移動ロボットの床形状推定装置は、概説すると、本出願人が先に提案した前記の特開平 6 - 3 1 6 5 8号公報記載の技術の改良である。より具体的には、同様に本出願人が先に提案した前記の特開平 1 0— 2 7 7 9 6 9号公報記載技術を基礎とし、その上に特開平 6 - 3 1 6 5 8号記載技術の手法を拡張しながら適応させたものである。

即ち、その特開平 6— 3 1 6 5 8号記載技術においては、少なくとも Z M Pまわりの全床反力モーメントの制御偏差を基に、両脚支持期では両脚間傾斜偏差を推定して両脚補償角を修正し、片脚支持期では各足平床傾斜偏差を推定して各足平補償角を修正していた（特開平 6 - 3 1 6 5 8号公報記載技術では足首角を修正していたので、厳密には足平補償角を修正することとは異なるが、ここではほぼ同じと考える）。

尚、上記で、 Z M P (Zero Moment Poin t)とは、運動パターン（軌道）によつて発生する慣性力と重力の合力の床（面）上の作用点まわりのモーメント力 \ 鉛直軸まわりの成分を除き、 0である点を意味する。

それに対し、この実施の形態に係る脚式移動ロボットの床形状推定装置においては、少なくとも全床反力モーメントの制御偏差（あるいはそれから目標各足平床反力中心点まわりの各足平床反力モーメン卜の制御偏差を減じたもの）を基に床形状、より具体的には両脚間床傾斜偏差を推定する。さらには、必要に応じて推定値に基づいて両脚補償角を修正する。また、少なくとも目標各足平床反力中心点まわりの各足平床反力モーメントの制御偏差を基に床形状、より具体的には各足平床傾斜偏差を推定する。さらには、必要に応じて推定値に基づいて各足平補償角を修正するようにした。この結果、両脚支持期であっても各足平床傾斜偏差を精度良く推定することができ、各足平補償角を修正することができる。

さらに、両脚間床傾斜偏差および各足平床傾斜偏差の推定において、前記したコンプライアンス機構 4 2をモデル化した、床反力と脚部（脚部リンク） 2の変形の関係を表わす機構コンプライアンスモデルを用いることにより、制御則に単なる積分を加えて定常制御偏差を低減する従来手法では得られない良好な制御特性が得られるようにした。即ち、制御則に単なる積分を加えると位相が遅れるが、この実施の形態に係る装置ではオブザーバを用いた場合と同様に、制御の遅れが生じることがない。

ところで、その先に提案した前記特開平 1 0— 2 7 7 9 6 9号公報記載技術は、予期しない凹凸形状の床面に遭遇して姿勢の安定を失ったとき、床反力を適切に制御して安定した姿勢を回復させることを意図していた。

即ち、平坦な床面を想定して生成された歩容に基づいて歩行制御されていた口ボット 1が予期しない凹凸を踏むと、過大な床反力が生じてロボット 1は傾く力、そのような状況に対処すべく、本出願人は別の提案技術（特開平 5— 3 0 5 5 8 6号公報記載技術）において前記したコンプライアンス機構 4 2を用いると共に、傾斜センサにより上体傾斜を検出して必要な復元モ一メント要求量を求め、それに目標全床反力中心点（目標 Z M P ) まわりの実全床反力モーメント成分が一致するように足平軌道（パターン）を修正して各足平を上下させていた。

しかしながら、その制御は、長い距離でゆったりと変化する大域（広域）的な傾斜やうねりには有効であるが、局所的な傾きや段差には十分対応しきれなかつた。

本出願人は、さらに、別の提案技術（特開平 5 - 3 0 5 5 8 4号公報記載技術 ) において、同様に前記したコンプライアンス機構 4 2を用いて予期しなかった床反力モーメントを打ち消す方向に足関節 1 8 R ( L )， 2 O R ( L ) を駆動する制御も提案していた。

従って、予期しない凹凸形状の床面に遭遇して姿勢の安定を失ったときには、上記した 2種の提案技術を併用することも考えられるが、そうすると、制御が干渉する不都合があった。

そのような制御の干渉を回避すべく、先に提案した前記特開平 1 0— 2 7 7 9 6 9号公報記載技術では、局所的な凹凸や傾斜などを含む予期しない床形状の変化に起因して姿勢の安定を失ったとき、床反力を適切に制御するようにしているこの実施の形態に係る脚式移動ロボットの床形状推定装置は、その先に提案した前記特開平 1 0— 2 7 7 9 6 9号公報記載技術を前提としているので、理解の便宜上、最初に、その先に提案した前記特開平 1 0— 2 7 7 9 6 9号公報記載技術を説明する。尚、以下、その先に提案した前記特開平 1 0— 2 7 7 9 6 9号公報記載技術を「複合コンプライアンス制御」と呼ぶ。

第 4図は、その複合コンプライアンス制御の装置の構成および動作を機能的に示すプロック図である。

同図の説明に入る前に、その複合コンプライアンス制御で使用した定義を説明する（尚、定義しない用語に関しては、本出願人が前記した技術とは別に提案した出願（特開平 1 0— 8 6 0 8 1号）で使用した定義に従う）。

『歩容』は、ロボット工学における一般的な定義と異なり、目標運動パターンと床反力パターンを合わせたものを指称する意味で使用する。但し、床反カバ夕ーンとしては、例え『Z M P軌跡だけ』というように、部分情報であっても良い。そのため、目標運動パターンだけを出力して床反力パターンに関する情報を出力しない装置に対して「歩容生成装置」と言う言葉を用いない。

各脚には、通し番号をつける。第 n脚に作用する床反力を第 n足平床反力という（n ： 1または 2。以下同じ）。全脚に作用する床反力を合成したものを全床反力という（ロボット工学では一般的には床反力と呼ばれるが、足平床反力と区別するためにここでは『全床反力』という）。

足平床反力は作用点とそこにかかる力と力のモーメントによって表現され、同一の足平床反力に対して、表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が 0でかつ作用点が足底上にある表記が存在する。この表現における作用点を、ここでは足平床反力中心点という（本出願人が別途提案した後述する特開平 6 - 7 9 6 5 7号では『接地圧重心点』と称した）。

同様に、全床反力は、作用点と、そこにかかる力と力のモーメントによって表現され、同一の全床反力に対して表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が 0でかつ作用点が床面上にある表現が存在する。この表現における作用点を、ここでは全床反力中心点という。尚

、この明細書で「床面」は、本出願人が先に提案した特開平 5 - 3 1 8 3 4 0号公報に記載される仮想的な床面であっても良い。

全床反力の目標値を目標全床反力という。目標全床反力は、通常、目標運動パターンに対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標 Z M Pに一致する。

尚、前記した Z M Pの目標値を目標 Z M Pという。目標 Z M Pは、合力の垂直方向力成分が 0でない限り、一義的に求められる。以下の説明では、理解しやすくするために、目標 Z M Pという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所が多い。

各足平床反力の目標値を目標各足平床反力という。但し、目標全床反力とは異なり、目標運動パターンが決まっていても目標各足平床反力は一義的には決定されない。実際のロボットに作用する全床反力を実全床反力という。実際のロボットに作用する各足平床反力を実各足平床反力という。

上記した定義を前提とし、以下、第 4図を参照して複合コンプライアンス制御装置の全体構成を概説する。

図示の如く、複合コンプライアンス制御装置は歩容生成器 1 0 0を備え、歩容生成器 1 0 0は目標歩容を生成し、出力する。目標歩容は、前述の定義の通り、目標運動パ夕一ンと目標床反力パターン、より具体的には目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点（目標 Z M P ) 軌道および目標全床反力軌道（あるいはパターン）からなる。目標床反カバ夕一ンは、このように、目標全床反力中心点軌跡を含む（後述する機構変形補償を行わないならば、目標床反力パターンとしては目標全床反力中心点軌跡だけでも良い）。

歩容生成器 1 0 0が出力する目標全床反力は、目標運動パターンに対して動力学的に平衡する全床反力である。従って、目標全床反力中心点は、目標 Z M Pに一致する。

第 5図にロボット 1が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標 Z M P軌道の床面上軌跡を第 6図に、タイム ·チヤ一トを第 7図に示す。この歩容の期間に床に接触したままの足平を「第 1足平」、もう一方を「第 2足平」と呼ぶ。

第 4図の説明に戻ると、この装置は目標床反力分配器 1 0 2を備え、目標床反力分配器 1 0 2は、目標全床反力中心点（目標 Z M P ) と目標足平位置姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器 1 0 0から歩容のパラメ一夕（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期 ·時刻（例えば、現在時刻が両脚支持期の初めから 0. lsecであるなど）などの情報も必要に応じて取り込む。

第 5図に示すような歩容に対して、目標床反力分配器 1 0 2は、目標各足平床反力中心点が以下の条件を満足するように設定する。

条件 1 ) 目標各足平床反力中心点軌跡は連続である。

条件 2 ) 両脚支持期では、目標第 1足平床反力中心点はかかとに、目標第 2足平床反力中心点はつまさきに存在する。

条件 3 ) このとき目標第 1足平床反力中心点と目標第 2足平床反力中心点を結ぶ線分上に、目標全床反力中心点が存在する。

条件 4 ) 片脚支持期では、目標第 1足平床反力中心点は、目標全床反力中心点に —致する。

条件 5 ) 片脚支持期の間に、目標第 2足平床反力中心点は、つまさきからかかとに移動する。

これら条件を満足する目標第 1足平床反力中心点軌跡のタイム ·チャートを第 8図に、目標第 2足平床反力中心点軌跡のタイム，チャートを第 9図に示す。尚、この図では足首（関節 1 8 R ( L )， 2 0 R ( L ) ) から足平 2 2 R ( L ) への垂直投影点、より具体的には足底への垂直投影点を原点とし、第 1図に示すように足平前方向を X軸の正の向き、足平左方向を Y軸の正の向きにとる。

目標床反力分配器 1 0 2は、さらに、付随的ではあるが、目標各足平床反力も決定して出力する。目標各足平床反力は、コンプライアンス機構 4 2のたわみ補償のために必要である。

次式を用いて上記のように設定された目標各足平床反力中心点に対応する目標各足平床反力を決定すれば、目標各足平床反力の合力は目標全床反力に一致しなければならないと言う条件を満足する。

目標第 1足平床反力

=目標全床反力 * (目標第 2足平床反力中心点と目標 Z M Pの距離） / (目標第 1足平床反力中心点と目標第 2足平床反力中心点の距離）

目標第 2足平床反力

=目標全床反力 * (目標第 1足平床反力中心点と目標 Z M Pの距離） Z (目標第 1足平床反力中心点と目標第 2足平床反力中心点の距離）

• · ·式 1 このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術 (特開平 6— 7 9 6 5 7号）に記述されている。

第 4図の説明に戻ると、この装置は姿勢安定化制御演算部 1 0 4を備え、姿勢安定化制御演算部 1 0 4はセンサ情報に基づいてロボット 1の状態を推定し、補償全床反力を算出する。即ち、実際にロボット 1が歩行あるいは直立しているときなどには後述する変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたとしても、ロボット 1の位置姿勢は必ずしも望ましい位置姿勢にならない。

ロボット 1の姿勢を長期的に安定化させるためには、ロボットを望ましい位置姿勢に復元させるために必要な力とモ一メントを求め、これを目標全床反力中心点（目標 Z M P ) を作用点として付加的に発生させる必要がある。この付加的な力とモーメントを補償全床反力という。また、補償全床反力のモーメント成分を補償全床反力モーメントという。

尚、脚式移動ロボットの目標歩容が床反力以外の反力を環境から受けるように想定し、それを例えば、目標対象物反力と称し、先に述べた目標 Z M Pの定義を次のように拡張しても良い。即ち、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、床面上のある点に作用するモ一メントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標 Z M Pとするようにしても良い。もし、ロボット 1が完全剛体であって、変位コント口一ラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、コンプライアンス機構 42のたわみによって生じるロボット全体の位置姿勢の摂動的な運動は、以下の 6自由度に分解できる。

モ一ド 1 ) 目標全床反力中心点（目標 ZMP) を中心とした前後軸まわり回転（即ち、左右傾き）

モード 2) 目標全床反力中心点（目標 ZMP) を中心とした左右軸まわり回転（即ち、前後傾き）

モード 3) 目標全床反力中心点（目標 ZMP) を中心とした鉛直軸まわり回転（即ち、スピン）

モード 4) 莳後平行移動揺れ

モード 5) 左右平行移動揺れ

モード 6) 上下平行移動揺れ

この内で、モード 4とモード 5は、コンプライアンス機構 4 2が前後左右方向の剪断力を受けてたわむことによって発生するものである。コンプライアンス機構 42の剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。

残り 4自由度の内、モ一ド 3とモ一ド 6はここでの説明と直接の関連を有しないので、モード 1 とモード 2に対する制御を説明する。モード 1 とモード 2に対する制御は、これがないとほとんどの場合ロボットが転倒するので、重要度は極めて高い。

モード 1を制御するための操作量は、補償全床反力の前後軸（X軸）まわりモ —メント成分である。モード 2を制御するための操作量は、補償全床反力の左右軸（Y軸）まわりモーメント成分である。従って、補償全床反力の成分の内、前後軸方向モーメント成分と左右軸方向モ一メント成分だけを求めれば良い。他の成分は、この実施の形態（および第 2の実施の形態）では用いないので 0で良い ο

尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を「補償全床反力モーメント Mdmd」（詳しくは目標全床反力中心点（目標 ZMP) まわりの補償全床反力モーメント Mdmd) と呼ぶ。第 5図および第 6図に示す如く、ロボットの前方向を X軸、左横方向を Y軸、上方向を Z軸にとり、第 1足平の足首直下の床面上の点を原点とした座標系を「支持脚座標系」と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、 Mdmdの X成分を Mdmd x、 Y成分を Mdmd y、 Z成分を Mdmd zと記述する。上体 2 4の傾斜偏差（即ち、実上体傾斜一目標上体傾斜） 0 e r rの X 成分を S e r r x, Y成分を S e r r y、これらの時間微分値を（ d 0 e r r x /d t ) , (ά θ e r r y/d t) と記述する。

Mdmd xおよび Mdmd yは、例えば次式の制御則によって決定される。 Mdmdx = - Kthx Θ errx - Kwx (d Θ errx I dt)

Mdmdy = - thy Θ erry - Kwy (d0 erry I dt)

• · ■式 2 ここで、 K t h x, K t h y, K w xおよび K w yは、上体傾斜安定化制御ゲィンである。

後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。

第 4図の説明に戻ると、この装置は実各足平床反力検出器 1 0 8を備え、実各足平床反力検出器 1 0 8は、 6軸力センサ 3 4によって実各足平床反力（その合力が実全床反力）を検出する。さらに、関節のエンコーダによって検出される実変位（および Zまたは変位指令）に基づき、上体 2 4に固定された座標系に対する各足平 2 2 R (L) の相対位置姿勢を算出し、それによつて 6軸力センサ 3 4 の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、支持脚座標系に変換する。

この装置はロボット幾何学モデル（逆キネマテイクス演算部） 1 1 0を備え、ロボット幾何学モデル 1 1 0は、上体位置姿勢と足平位置姿勢を入力されると、それらを満足する各関節変位を算出する。この実施の形態におけるロボット 1のような 1脚あたりの関節自由度が 6である場合には、各関節変位は一義的に求まる。

ここでは逆キネマティタスの解の式を直接的に求めておき、式に上体位置姿勢と足平位置姿勢を代入するだけで各関節変位を得るようにした。即ち、ロボット幾何学モデル 1 1 0は、目標上体位置姿勢と複合コンプライアンス動作決定部で修正された修正目標足平位置姿勢軌道（機構変形補償入り修正目標足平位置姿勢軌道）を入力し、それらから 1 2個の関節（ 1 O R ( L ) など）の関節変位指令 (値）を算出する。

この装置は変位コントロ一ラ 1 1 2 (前記した第 2の演算装置 6 2に同じ）を備え、変位コントローラ 1 1 2は、ロボット幾何学モデル（逆キネマティクス演算部） 1 1 0で算出された関節変位指令（値）を目標値としてロボット 1の 1 2 個の関節の変位を追従制御する。

この装置は前記した複合コンプライアンス動作決定部 1 1 4を備え、複合コンプライアンス動作決定部 1 1 4は以下の 2つの要求を満足させようと、目標足平位置姿勢軌道を修正する。

要求 1 ) ロボットの位置姿勢制御のために、実全床反力を姿勢安定化制御演算部 1 0 4が出力する補償全床反力（モーメント M d m d ) と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメント M d m dに追従させる。

要求 2 ) 各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。

尚、通常は実全床反力を補償全床反力と目標全床反力の合力に一致させながら目標各足平床反力中心点まわりの実各足平床反力モーメントを 0にすることが、物理的に不可能な場合が多い。従って、要求 1 ) と要求 2 ) は完全に両立させることはできず、ある点で妥協しなくてはならない。

上記を前提として第 1 0図フロー ' チャート（構造化フロー，チャート）を参照してこの装置の動作を説明する。尚、図の左端に該当する処理を行う第 4図に示す装置の構成要素を示す。

先ず S 1 0において装置を初期化し、 S 1 2を経て S 1 4に進み、タイマ割り込みを待機する。タイマ割り込みは 5 O m sごとになされ、即ち、制御周期は 5 0 m sである続いて S I 6に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときは S 22に進むと共に、肯定されるときは S 1 8に進んでタイマ tをイニシャライズし、 S 2 0に進んで目標歩容パラメ一夕を設定する。前記の如く、歩容パラメ一夕は、運動パラメ一夕と床反力パラメ一夕（ZM P軌道パラメ一夕）から構成される。

続いて S 22に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置姿勢、目標各足平位置姿勢、および目標 ZMP位置から構成される。尚、ここで『姿勢』は X, Υ, Z 空間における『向き』を意味する。

続いて S 24に進んで目標各足平床反力中心点を求める。これは目標床反力分配器の説明で述べたように行う。具体的には、第 8図および第 9図に示すように、設定した目標各足平床反力中心点軌跡の現在時刻 tにおける値を求めることで行う。

続いて S 2 6に進んで目標各足平床反力を求める。これは目標床反力分配器の説明で述べた式 1を用いて目標各足平床反力を演算することで行う。

続いて S 2 8に進み、前記した傾斜センサ 3 6などの出力から上体 2 4の傾斜などロボット 1の状態を検出する。

続いて S 3 0に進み、ロボット 1の状態などから姿勢を安定化するための（目標全床反力中心点（目標 ZMP) まわりの）補償全床反力モーメント Mdmd X ， Mdmd yを求める。具体的には、前記した式 2に従って補償全床反力モーメント Mdmd x, Mdmd yを演算する。，

続いて S 3 2に進んで実各足平床反力を検出する。これは前記の如く、 6軸力センサ 3 4の出力から検出する。

続いて S 34に進み、両脚補償角 0 d b vおよび各足平補償角 0n x (y) を決定する。これは、前記した複合コンプライアンス動作決定部 1 1 4が行う作業 C'あ。

その複合コンプライアンス動作決定部 1 1 4の作業について説明する。説明の便宜のため、両脚支持期において第 1 1図に示すように第 1足平 22 R (L) と第 2足平 22 L (R) に実各足平床反力が作用している状況と仮定する。ここでベクトル Fn a c tは第 n足平床反力の力成分を表す。ベクトル Mn a c tは第 n足平床反力のモ一メント成分を表す。べクトル Mn a c tの向きは、向きに対して時計回りの乇一メントが床から足平に作用していることを表す。この瞬間の目標全床反力は、第 1 2図に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標 ZMP) における目標全床反力モーメントべクトル Ms umr e f は垂直である（定義により、目標 ZMPは目標全床反カモ一メントの水平方向成分が 0である点であるから）。

これを式 1に従って目標各足平床反力に分配すると、第 1 3図に示すようになる。同図において、ベクトル Fn r e f は目標第 n足平床反力の力成分を表す。べクトル Mn r e f は目標第 n足平床反力のモーメント成分を表す。べクトル M n r e f の向きの表現は、 Mn a c tと同様である。

説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。

前述の姿勢安定化制御演算部 1 0 4では、ロボット 1の上体傾斜偏差検出値 0 e r r x, Θ e r r yに基づいて補償全床反力モーメント Mdmdを算出する。この実施の形態では鉛直軸（Z軸）まわりのスピンを制御しないので、補償全床反カ乇一メント Mdmdの鉛直軸成分は 0である。上体位置の揺れも制御しないので、補償全床反力の力成分（F dmd) も 0である。この状態に対応する補償全床反力モーメント Mdmdを第 1 4図に示す。

姿勢を復元させるためには、目標全床反力中心点（目標 ZMP) まわりの実全床反カ乇一メントの水平成分を、目標全床反力モーメント Ms umr e ίと補償全床反力モーメント Mdmdの和の水平成分に追従させれば良い。

一方、目標全床反力中心点（目標 ZMP).では目標全床反力モーメント Ms u mr e f の水平方向成分は 0である。従って、前後左右の姿勢傾きを復元させるためには、目標 ZMPまわりの実全床反力モーメントの水平成分を、 Mdmdの水平成分に追従させれば良い。

複合コンプライアンス動作決定部 1 1 4は、前記した以下の要求をできる限り満足するように足平の位置姿勢を修正する。

要求 1 ) ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標 ZMP) まわりの実全床反力モーメントの水平方向（X， Y軸方向）成分を、補償全床反力モーメント Mdmdの水平方向成分に追従させる。

要求 2) 各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反カモ一メントの絶対値を小さくする。

但し、前述の通り、要求 1 ) と要求 2) は、完全に両立させることはできず、ある点で妥協しなくてはならない。

足平の位置姿勢の修正は次のように行う。

1 ) 目標第 1足平床反力中心点 Q 1 と目標第 2足平床反力中心点 Q 2を含み、かつ水平面と垂直な平面の法線べクトル Vを求める。 Vの大きさは 1とする。 Vを第 1 5図に示す。

2) 目標第 1足平床反力中心点 Q 1の座標を、目標全床反力中心点（目標 ZM P) を回転中心に法線ベクトル Vまわりに、ある回転角 S d b Vだけ回転移動する。移動した後の点を Q 1 ' とする。同様に、目標第 2足平床反力中心点 Q 2の座標を、目標全床反力中心点（目標 ZMP) を回転中心に法線ベクトル Vまわりに回転角 0 d b Vだけ回転移動する。移動した後の点を Q 2' とする。

この回転角 0 db vを両脚補償角という。始点が Q 1、終点が Q 1 ' のべクトルをベクトル Q 1 Q 1 ' とする。同様に、始点が Q 2、終点が Q 2' のベクトルをベクトル Q 2 Q 2' とする。第 1 6図に Q Γ と Q 2' を示す。

3) 目標第 1足平を、姿勢は変えずにべクトル Q 1 Q Γ だけ平行移動（ほぼ上下移動）させる。同様に、目標第 2足平を、姿勢は変えずにベクトル Q 2 Q 2

' だけ平行移動させる。移動後の目標各足平を第 1 6図に太線で示す。

4) 次に、目標第 1足平を Q 1 ' を中心に、前後方向軸（X軸）まわりに回転角 0 1 X、左右方向軸（Y軸）まわりに回転角 Θ 1 yだけ回転させる。同様に、目標第 2足平を Q 2' を中心に前後方向軸（X軸）まわりに回転角 02 X、左右方向軸（Y軸）まわりに回転角 02 yだけ回転させる。回転角 0 nx, 0 n yをそれぞれ第 n足平 X補償角、第 n足平 Y補償角という。回転後の目標各足平を第

1 7図に太線で示す。

以上の補償動作量が過大でなければ、接地圧力分布は変わっても、接地領域（足底面の! £力が正の領域）は変わらない。このような場合には、補償動作量に比例して各足平に装着されたコンプライアンス機構 4 2が変形し、変形量に応じた実各足平床反力が発生する。この結果、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。

特性 1 ) 両脚補償角 Θ d b vだけを操作して目標各足平位置を移動させると、下がつた足平の実足平床反力の力成分が増加し、上がつた足平の実足平床反力の力成分が減少する。このとき、修正目標各足平床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。

特性 2 ) 第 n足平 X補償角だけを操作して目標第 n足平姿勢を回転させると、目標第 n足平床反力中心点に作用する実第 n足平床反力のモーメン卜の X成分だけが変化し、その他の床反力成分は少ししか変化しない。同様に、第 n足平 Y補償角だけを操作して目標第 n足平姿勢を回転させると、実第 n足平床反力のモーメントの Y成分だけが変化し、その他の床反力成分は少ししか変化しない。

特性 3 ) 両脚補償角 0 d b v、各足平 X補償角および各足平 Y補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。

特性 1および特性 2は、これらの操作に独立性があることを示し、特性 3はこれらの操作に線形性があることを示していると言える。

第 1 8図は複合コンプライアンス動作決定部 1 1 4の演算処理を示すブロック図であり、同図を参照してその処理を説明する。

概説すると、補償全床反力モーメント分配器 1 1 4 aにおいては補償全床反力モーメント M d m dの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部 1 1 4 bおよび第 n足平 X ( Y) 補償角決定部 1 1 4 c， 1 1 4 d , 1 1 4 e , 1 1 4 f において前述の補償角 0 d b vおよび 0 n x ( y ) を決定する。

次に、決定された各種補償角に基づいて修正目標足平位置姿勢算出部 1 1 4 g は、補償された足平位置姿勢（これを修正目標足平位置姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置姿勢算出部 1 1 4 hは、目標各足平床反力によって発生が予想されるコンプライアンス機構 4 2 の変形量を求め、それらを打ち消すように修正目標足平位置姿勢をさらに修正す ο 以下詳説すると、補償全床反力モーメント分配器 1 1 4 aは、補償全床反カモ一メント Mdm dを、両脚補償モーメント Mdm d d b、各足平補償モ一メント Mdmd l x, y, M d m d 2 x , yに分配する。両脚補償モーメント M d m d d bは、両脚補償角（足平上下量） 0 d b vを操作することによって目標全床反力中心点（目標 ZMP) まわりに各足平床反力の力成分が作るモーメントの目標値である。

両脚補償乇一メント Mdmd d bの V方向まわりの成分を Mdmd d b vと記述する。尚、べクトル Vは複合コンプライアンス動作決定部 1 1 4の説明で定義したベクトルである。 Vに直交し、鉛直方向にも直交するベクトルを Uとすると、両脚補償モ一メント Mdmd dbの U方向成分 Mdmd d b uは 0に設定される。両脚補償角 0 d b vを操作しても、床反力の U方向モーメント成分を発生することはできないからである。

ここでは補償全床反カ乇一メント Mdmdの鉛直方向成分が 0なので、 Mdm d d bの鉛直方向成分 Mdmd d b zも 0に設定される。

第 1足平補償モーメント Mdmd 1は、第 1足平補償角 0 I χ, Θ I yを操作することによって目標第 1足平床反力中心点まわりに発生させたいモーメン卜である。第 1足平補償モ一メント Mdmd 1の X成分を Mdmd 1 x、 Y成分を M d m d 1 yと記述する。第 2足平補償モ一メント Mdmd 2は、第 2足平補償角 Θ 2 χ, Θ 2 yを操作することによって目標第 2足平床反力中心点まわりに発生させたいモーメントである。第 2足平補償乇一メント Mdmd 2の X成分を Md md 2 x、 Y成分を Mdm d 2 yと記述する。

分配は、例えば次のように行う。

Mdmddbv = Wdbx* Mdmdx + Wdby * Mdmdy

式 3

Mdmdlx = Wlx * (Mdmdx - Wint* Vx * Mdmddbv)

Mdmdly = Wly * (Mdmdy 一 Wint* Vy * Mdmddbv)

Mdmd2x 二 W2x * (Mdmdx - Wint* Vx * Mdmddbv)

Mdrad2y 二 W2y * (Mdmdy - Wint* Vy * Mdmddbv)

式 4 ここで、 Wd bx, Wdby, Wl x' Wl y, W2 x, W2yおよび Wi n tは分配用重み変数である。 Vxはベクトル Vの X成分の値、 Vyはベクトル V の Y成分の値である。この中で、 Wi n tは、両脚補償角を操作することによつて発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。

式 3と式 4の演算処理を行う補償全床反力モーメント分配器 1 1 4 aのプロック図を第 1 9図に示す。

歩行時の分配用重み変数 Wdbx， Wdby, Wl x, W 1 y, W2 x, W2 yおよび Wi n tの設定例を第 20図に示す。第 20図のパターンは、以下の注意点を考慮して決定することが望ましい。

注意点 1) 両脚補償角と各足平補償角が不連続的に変化すると、関節に過大なトルクが発生する。そこで、両脚補償角と各足平補償角を連続的に変化させるために、分配用重み変数は連続的に変化させる。

注意点 2 ) 両脚補償角および各足平補償角を操作することによつて発生する実床反力モーメントが、なるべく補償全床反カモ一メント Mdmdに近い値になるように、分配用重み変数を決定する。

この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントの V方向成分 Mdmd d b v、各足平補償モーメント Mdmd 1, Mdmd 2を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。

この状況では目標全床反力中心点（目標 ZMP) まわりの実全床反カモ一メントの水平方向成分を、補償全床反カモ一メント Mdmdの水平方向成分に一致させるため（即ち、前述の複合コンプライアンス動作決定部に対する要求 1を満足するため）に、式 5と式 6の両方をなるベく満足するように重みを設定すべきである。

Mdmddbv * Vx + Mdmdlx + Mdmd2x = Mdmdx

• ■ ·式 5

Mdmddbv * Vy + Mdradly十 Mdmd2y = Mdmdy

- · ■式 6 これに式 3、式 4を代入すると、式 5は式 7に、式 6は式 8に変換される。 (Wdbx * Mdmdx + Wdby* Mdmdy) *Vx

+ Wlx* (Mdmdx - Wint *Vx* (Wdbx* Mdmdx + Wdby 氺 Mdmdy))

+ W2x* (Mdmdx一 Wint *Vx* (Wdbx * Mdmdx + Wdby * Mdmdy))

= dmdx

• · ·式 7

(Wdbx 氺 Mdmdx + Wdby* Mdmdy) *Vy

+ Wly* (Mdmdy - Wint *Vy* (Wdbx * Mdmdx + Wdby * Mdmdy))

4- W2y* (Mdmdy - Wint *Vy* (Wdbx * Mdmdx + Wdby * Mdmdy))

= Mdmdy

• - ，式 8

Mdmd と M dm d yが任意の値を取っても、式 7と式 8が恒等的に成立するためには、式 9、式 1 0、および式 1 1を同時に満足すれば良い。

Wint = 1 · ■ ·式 9

Wlx + W2x =1 · · ·式 1 0

Wly + W2y =1 · ■ ·式 1 1 即ち、以上の状況では式 9、式 1 0および式 1 1を同時に満足するように、重みを決定すれば良い。

歩行時では M dm d d b vを目標にして両脚補償角 d b vを操作して足平の位置を修正しても、実全床反力モーメントの発生量が Mdmd d b vに較べて不足する場合がある。例えば第 2 1図のように両脚支持期の初期にロボットが後傾して第 1足平が未だ着地していない状況では、 Θ d b vによって第 1足平の位置を下げても、実床反力は変化しない。

同様に、 Mdmd 2を目標にして第 2足平補償角 02を操作して第 2足平の角度を修正しても、実床反力モーメントの増加量が Mdmd 2に較べて不足する場合がある。例えば、第 2 2図のように両脚支持期の後半にロボットが後傾している状況では、 02によって第 2足平のかかとを下げても実床反力は変化しない。従って、式 5、式 6を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメント Mdm dに届かない場合がある。このようなことが生じる可能性が高い状況では、式 5 、式 6の左辺の値を右辺の値で割った値が 1 より大きくなるようにすべきである ο

歩行時の分配用重み変数設定例である第 2 0図では、 W i n tを 0に設定することによって、第 2 1図の状況のように、両脚補償角 0 dbv を操作しても実全床反カモ一メントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。

好都合なことに、第 2 1図のように後傾すると第 2足平のかかとが結果的に下がつて床に接地しやすくなるので、第 2足平補償角を操作することによって実全床反力モ一メントを発生させることができるようになる。

また、後傾していないときには両脚補償角 d b vを操作することによる実全床反力モーメントが発生するが、第 2足平のかかとが床に接地しないので、第 2 足平補償角を操作しても実全床反力モーメントは発生しない。

つまり、両脚補償角 0 d b vが有効に働くときには各足平補償角が有効に働かず、各足平補償角が有効に働くときには両脚補償角 0 d b vが有効に働かないので、結果的に両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量は、ほぼ補償全床反力モーメント M d m dに等しくなる o

状況によっては、両脚補償角および各足平補償角を操作することによって発生する実床反カモ一メン卜の総量が補償全床反カモ一メント M d m dよりも大きくなってしまう場合がある。

しかし、この場合でも、 M d m dが姿勢安定化のためのフィ一ドバック操作量であるならば、あまり問題にならない。何故ならば、 M d m dの大きさが多少違つていても、一般的に制御系に言えることであるが、制御系のオープンループゲインが多少変化するだけで、クローズドループ特性はほとんど変わらないからである。

注意点 3 ) 片脚支持期では、両脚補償角用の分配用重み変数である W d b x， W d b yの絶対値を小さくする。片脚支持期では両脚補償角を変化させても、接地していない足平が無駄に上下するだけで、実各足平床反力は変化しないからである。

注意点 4) 足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきである。

注意点 5 ) 両脚補償角を操作することによつて制御できる実全床反力モ一メントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。

例えば、両脚補償角 0 d b Vを操作することによって発生する実全床反力モ一メントの向きは必ず V方向であり、 V方向に直交する成分を発生させることはできない。一方、各足平補償角を操作することによって発生できる実全床反カモ一メン卜の向きは、足平の接地状況によって制約を受ける。

例えば、足平 22R (L) のつまさきのエッジだけ、またはかかとのエッジだけが接地している場合には、エツジ線方向にモーメントを発生することはできなレ、。両脚支持期では、この特性を考慮して、なるべく無駄なく両脚補償角および各足平補償角を操作する。

例えば、両脚補償角を操作するための分配重み Wdb X， Wd byは次のように決定する。

X成分が Wdb x、 Y成分が Wdby、 Z成分が 0のベクトルを Wd bとすると、式 3はベクトル Wd bと Mdmdの内積になっている。従って、 Mdmdをべクトル Wd b方向成分とその直交成分に分解し、べクトル Wdb方向成分だけを抽出して、べクトル Wdbの大きさを乗じたものが、式 3によって求められる dm d d b vであると言える。

この場合の Mdmd d b vを第 23図に示す。これは、両脚補償角を操作することによって実全床反力モーメントの Wd b方向成分を制御するフィ一ドバック制御系を構成することを意味する。もし、 Wdb方向がベクトル Vと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントの Wd b方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。

従って、無駄な動きを減らしたい場合には、 Wdb方向をベクトル V方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反カ乇一メント M d m dの W d b方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、 Wdbと Vの内積が 1になるように設定する。一部を各足平補償角に頼らせたいならば、 Wd bと Vの内積が 1より小さくなるように設定する。

ところで、足平の横幅が狭い場合には、各足平補償角を操作することによって発生し得る実各足平床反力モーメントの X成分は小さくなる。この場合には、 W d b Xを大きめに設定する。このように設定すると、 Wd b方向とべクトル V方向は一致しなくなり、両脚補償角の変動が増加するが、安定性が増す。

両脚補償角決定部 1 1 4 bの処理についてさらに詳説すると、第 24図は両脚補償角決定部 1 1 4 bの演算処理のブロック図であり、両脚補償角 S d b vは図示の如く演算される。

第 24図を参照して説明すると、目標第 1足平床反力中心点 Q 1に作用する F 1 a c tと目標第 2足平床反力中心点 Q 2に作用する F 2 a c "t力目標全床反力中心点 Pのまわりに発生させるモ一メント Mf 1 f 2 a c tを、次式により求める。

Mflf2act = PQl*Flact + PQ2*F2act ' · ·式 1 2 ここで、 PQ 1は始点が P、終点が Q 1のベクトル、 PQ 2は始点が P、終点が Q 2のべクトルである。

また、式 1 2の代わりに、次式を用いても実際上はほとんど問題がない。

Mflf2act = PQl^Flact + PQ2*F2act+ Mlac M2act · . .式 1 2 a 式 1 2 aは、目標全床反力中心点まわりに作用する実全床反カ乇一メント Ms υ ma c tを算出する式になっている。尚、式 1 2は、目標全床反力中心点まわりに作用する実全床反力モーメントから、目標各足平床反力中心点まわりに作用する実各足平床反カモ一メントを減じたものになっている。

次に、 Mf 1 f 2 a c tのべクトル V方向成分 Mf 1 f 2 a c t vを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトル V は前述の動作説明において第 1 5図に示した Vである。

Mflf2actv = Mflf2act - V · ■ ·式 1 3 次に、 M f 1 f 2 a c t vを口一パスフィル夕 1 1 4 iに通し、 M f 1 f 2 a c ΐ v f i 1 tを得る。

次に、両脚補償モーメント V方向成分 Mdmd db Vを補償フィルタ 1 1 4 j に通し、それを、 Mi 1 ί 2 a c t V f i 1 tから減じ、偏差モ一メント V方向成分 Md i f f vを得る。

尚、補償フィルタ 1 1 4 jは、 Mdmd d b vから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。

次に、コンプライアンス機構 4 2の変形による両脚補償モーメント V方向成分への影響を打ち消すための両脚機構変形補償角 f f d b vを求める。これは、いわゆるフィ一ドフォヮ一ド補償である。

具体的には、両脚補償モ一メント V方向成分 Mdmd d b vと変形量との関係を表す機構コンプライアンスモデルを用い、目標第 1足平床反力中心点 Q 1 と目標第 2足平床反力中心点 Q 2を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角 0 f f d b vとすれば良い。

両脚機構変形補償角 0 f f d b Vは、近似的には次式により求めれば良い。

^ffdbv =- a *Mdmddbv · · ·式 1 4 ここで αは所定の定数である。

最後に次式によって両脚補償角 Θ d b Vを得る。ここで K d bは制御ゲインであり、通常、これは正の値に設定する。

^dbv = Kdb *Mdiffv+ ^ffdbv · · ·式 1 5 第 n足平補償角決定部について説明すると、第 2 5図はその中の第 1足平 X補償角決定部 1 1 4 cの演算処理を示すブロック図であり、第 1足平 X補償角決定部 1 1 4 cは第 1足平 X補償角 0 1 Xを図示の如く演算する。説明は省略するが、第 1足平 Y補償角 01 Ύ 第 2足平 X補償角 02 x、第 2足平 Y補償角 02 y も同様に求める。ここでは第 1足平 X補償角 0 1 Xを求めるアルゴリズムだけを説明する。第 1足平床反力モーメント X成分 M 1 a c t xをローパスフィル夕 1 1 4 kに通して M 1 a c t f i 1 t xを得る。第 1足平補償モーメント X成分 Mdmd 1 Xを補償フィル夕 1 1 4 mに通し、それを、 M 1 a c t f i 1 t xカヽら減じ、偏差モーメント Md i f f 1 xを得る。両脚補償角決定と同様、補償フィルタ 1 1 4mは、 Mdmd 1 xから実全床反力までの伝達関数の周波数応答特性を改善するものである。

次に、両脚補償角決定と同様、コンプライアンス機構 4 2の変形による第 1足平補償モーメント X成分への影響を打ち消すための第 1足平 X機構変形補償角 Θ f f i xを求める。これは、いわゆるフィードフォワード補償である。

具体的には、第 1足平補償モーメント X方向成分 Mdmd 1 xと変形量との関係を表す機構コンプライアンスモデルを用い、第 1足平の変形角度を求め、それの極性を反転したものを第 1足平 X機構変形補償角 0 f f 1 xとすれば良い。第 1足平 X機構変形補償角 S f ί 1 Xは、近似的には次式により求めれば良い ο

0fflx=- alx*Mdmdlx · · ·式 1 6 ここで 1 Xは所定の定数である。

最後に次式によって第 1足平 X補償角 S 1 Xを得る。ここで K 1 Xは制御ゲインであり、通常、これも正の値に設定する。

θΐχ = Klx* Mdifflx+ 0fflx · · ·式 1 7 . 尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。、

第 1 8図に戻って説明を続けると、修正目標足平位置姿勢算出部 1 1 4 gは、両脚補償角 0 d b v、第 1足平 X補償角 0 1 x、第 1足平 Y補償角 0 1 y、第 2 足平 X補償角 02 x、第 2足平 Y補償角 02 yに基づき、前述の複合コンプライアンス動作の足平位置姿勢修正手法に従って目標足平位置姿勢を修正し、修正目標足平位置姿勢を得る。

機構変形量算出部 1 1 4 nは、目標各足平床反力によって発生が予想されるコンプライアンス機構 4 2の変形量を求める。

機構変形補償入り修正目標足平位置姿勢算出部 1 1 4 hは、算出された機構変形量を打ち消すように、修正目標足平位置姿勢をさらに修正し、機構変形補償入り修正目標足平位置姿勢を得る。

例えば、第 2 6図に示すような機構変形量が予想されるときには、機構変形補償入り修正目標足平位置姿勢は、第 2 7図に実線で示す位置姿勢に修正される。即ち、第 2 7図に示す機構変形補償後の足平 2 2 R (L) が目標足平床反カを受けて変形したときの位置姿勢が、第 2 6図に示す機構変形補償前の足平位置姿勢に一致するように、機構変形補償入り修正目標足平位置姿勢を算出する。

機構変形補償は、コンプライアンス機構 4 2の変形によって生じる実足平位置姿勢のずれをフィ—ドフォヮード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。

上記を前提として第 1 0図フロー ·チヤ一トの説明に戻ると、前記の如く、 S 3 4において上記した補償角を決定する。

第 2 8図はその作業を示すサブルーチン 'フロー 'チャートである。

同図を参照して説明すると、先ず S 1 0 0において前記したべクトル Vを求め、 S 1 0 2に進んで分配用重み変数を第 2 0図に示すように設定し、現在時刻 t でのこれらの値を求める。続いて S 1 0 4に進み、式 3および式 4によって補償全床反力モ一メント M dm dを両脚補償モ一メント Mdmd d b vと各足平補償モーメント Mdmd n x (y) に分配し、 S 1 0 6に進んで既述の如く両脚補償角 0 d b Vを求め、 S 1 0 8に進んで各足平補償角 0 n X (y) を求める。次いで第 1 0図フロー ·チャートの S 3 6に進み、目標各足平床反力に基づいて機構変形補償量を算出し、 S 3 8に進んで目標足平位置姿勢を補償角 Θ d b V , θ η χ (y) に応じて修正し、さらにこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置姿勢を得る。

次いで S 4 0に進み、上体位置姿勢と機構変形補償入り修正足平位置姿勢から関節変位指令（値）を算出し、 S 4 2に進んで実関節変位を算出された関節変位指令（値）に追従させるようサーボ制御し、 S 4 4に進んで時刻を△ t更新し、 S 1 4に戻って上記の処理を繰り返す。

上記の如く構成したので、これによつて、概括すれば、実全床反力の制御と実各足平床反力の制御が殆ど干渉しないようになり、それらを容易に制御することができる。

即ち、この複合コンプライアンス制御装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平 5— 3 0 5 5 8 4号公報で提案した足首コンブラィアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この複合コンプライアンス制御装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモ一メントを望ましい値に制御するようにした。

この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。

さらには、ロボットに作用する床反力、より具体的には目標全床反力中心点（目標 Z M P ) まわりの実全床反力モ一メントと目標各足平中心点まわりの実各足平床反力モーメントを容易かつ適切に制御することができる。換言すれば、先に提案した技術を併用する場合に比較し、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。

従って、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。

また、脚式移動ロボットの姿勢安定化制御を容易に実現できると共に、脚式移動ロボッ卜が受ける着地衝撃を低減することができ、脚式移動ロボッ卜の接地性を高め、歩行時のスリップやスピンを防止することができる。さらに、脚式移動ロボットのァクチユエ一夕の負荷を低減することができる。

また、特開平 5 - 3 0 5 5 8 6号公報で提案した技術では実全床反力（各足平床反力の合力）の目標全床反力中心点（目標 Z M P ) まわりのモーメント成分を検出し、その値が望ましい値になるように制御していたが、この複合コンプライァンス制御装置においては、目標各足平床反力中心点に作用する足平床反力の内のモーメント成分を除いた並進力成分の合成が目標全床反力中心点（目標 Z M P ) まわりに作用するモーメントを検出し、その値を望ましい値になるように制御するように変更した（尚、この点は先に提案した制御手法であっても良い）。第 2 9図は複合コンプライアンス制御の変形例を示す、第 1 6図と同様な説明図である。

変形例にあっては補償動作を簡易化して同様の作用効果を得るようにした。即ち、各足平の床反力の力成分を操作する足平位置修正動作の手法としては、第 1 6図に示した手法に代え、第 2 9図に示すように、鉛直方向にだけ移動させるようにした。このとき、第 1足平鉛直方向移動量 Z 1 と第 2足平鉛直方向移動量 Z 2は、次式によって求める。

Z1 =—線分 PQ1 の長さ * ^ d v

Z2 = 線分 PQ2 の長さ * 0 dbv

. . ·式 1 8 但し、ここで、 0 d b vには式 1 5で求められる値を代入する。

以上を前提としてこの発明の一つの実施の形態に係る脚式移動口ボットの床形状推定装置を説明する。

この実施の形態に係る脚式移動ロボットの床形状推定装置の構成は、第 4図に示した複合コンプライアンス制御の構成、より具体的には第 1 8図に示した構成と同様である。ただし、第 3 0図に示す如く、補償全床反力モーメント分配器 1 1 4 aなどで示される複合コンプライアンス動作決定部は、第 1 8図に示した構成と異なり、新たに床形状推定器 1 3 0と、床形状推定器 1 3 0が出力する各種の床形状推定値をそれぞれの補償角に加算する加算器 1 3 2 ( 1 3 2 a , 1 3 2 b ) を備えるようにした。

同図の説明に入る前に、床形状推定装置 1 3 0が推定する上で使用する概念と用語を以下のように定義する。

第 3 1図に示すように、目標歩容において想定された床（あるいは床面）を「想定床」と呼ぶ。ロボットが歩行している実際の床を「実床」と呼ぶ。また想定床形状に対する実床形状のずれを「床形状偏差」と呼ぶ。

前記した複合コンプライ了ンス制御において定義された目標第 η足平床反力中心点 Q nは、第 n足平の足底（床との接触面）に設定された点であった。従って、目標第 n足平床反力中心点 Q nは、足平 2 2 R ( L ) と共に移動する。これに対応して、目標第 n足平床反力中心点 Q nと接することが想定された想定床面上の点を「想定第 n床接点 D n」と呼ぶ。

この定義から明らかなように、第 n足平が接地している時期では、目標第 n足平床反力中心点 Q nと想定第 n床接点 D nは、支持脚座標系から見て同一座標になる。これに対し、実際にロボット 1が歩行しているときに、実第 n足平の足底面上における目標第 n足平床反力中心点 Q nに相当する点が、実床に接触する点を「実第 n床接点 D n a c t」と呼ぶ。

これらの点の関係を表す一例を第 3 1図に示す。第 3 1図は、目標第 1足平床反力中心点 Q 1 と目標第 2足平床反力中心点 Q 2を通る垂直面の法線方向（複合コンプライアンス制御におけるべクトル Vの方向）に向いて側方から歩行しているロボット 1を見たところである。

ロボット 1の目標姿勢（目標歩容のこの瞬間における姿勢）および前記垂直面における想定床の断面を細線で示す。ロボット 1の実足平位置姿勢と前記垂直面における実床の断面を太線で示す。この状況における実第 n床接点は、実床面上の点であり、第 3 1に示す位置になる。

さらに、床形状偏差を定量的に表現する指標として、両脚間床高低差偏差、両脚間床傾斜偏差、足平床傾斜偏差を以下のように定義する。

第 2床接点の高さと第 1床接点の高さの差を「両脚間床高低差」と呼び、以下のように算出する。

両脚間床高低差二第 2床接点の高さ-第 1床接点の高さ

• · ·式 1 9

実両脚間床高低差と想定両脚間床高低差との差を「両脚間床高低差偏差」と呼ぶ。始点を第 1床接点、終点を第 2床接点とする線分の傾斜を「両脚間床傾斜」と呼び、以下のように算出する。

両脚間床傾斜二 atan ( (第 2床接点の高さ一第 1床接点の高さ）

/ (第 2床接点と第 1床接点の間の水平距離））

. · · · 式 2 0

ここで atanは、三角関数である夕ンジヱントの逆関数を表す。実両脚間床傾斜と想定両脚間床傾斜との差を「両脚間床傾斜偏差」と呼ぶ。それを第 3 1図に示す。この状況において、両脚間床傾斜偏差は正である。第 n床接点における床面の傾斜を「第 n足平床傾斜」と呼ぶ。

実第 n足平床傾斜と想定第 n足平床傾斜との差を「第 n足平床傾斜偏差」と呼ぶ。これを第 3 2図に示す。第 3 2図はロボット 1を後ろから見た図である。口ボット 1の目標姿勢（目標歩容のこの瞬間における姿勢）および想定床の断面を細線で示す。ロボット 1の実足平位置姿勢と実床の断面を太線で示す。この状況において、第 1足平床傾斜偏差は負である。

尚、両脚間床傾斜および両脚間床傾斜偏差は、前述のベクトル V方向まわりの回転角で表される。第 n足平床傾斜および第 n足平床傾斜偏差は、 X方向の回転角と Y方向の回転角で表される。

また、両脚間床傾斜偏差および第 n足平床傾斜偏差と複合コンプライアンス制御の各種補償角との間には、以下の関係がある。即ち、両脚間床傾斜偏差は、両脚支持期において、目標歩容通りに歩行しているロボット 1の目標第 1足平床反力中心点 Q 1 と目標第 2足平床反力中心点 Q 2を結ぶ線分を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な両脚補償角に相当する σ

また、第 η足平床傾斜偏差は、目標歩容通りに歩行しているロボットの第 η足平を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な第 η足平補償角に相当する。

従って、歩行中に両脚間床傾斜偏差を推定し、両脚間床傾斜偏差の推定値を両脚補償角に加えれば、両脚間床傾斜偏差があっても、実全床反力は想定床を歩行しているときと同一になる。また、歩行中に第 η足平床傾斜偏差を推定し、第 η 足平補償角の推定値を第 η補償角に加えれば、第 η足平床傾斜偏差があつても、実第 η足平床反力は想定床を歩行しているときと同一になる。

尚、複合コンプライアンス制御の両脚補償動作として変形例で示した鉛直方向にのみ移動させる手法を用いる場合には、両脚間床傾斜偏差の推定値の代わりに、両脚間床高低差偏差の推定値を用いれば良い。本質的な違いはないので、以降の説明では両脚間床傾斜偏差の推定値を用いる場合のみを説明する。第 3 0図に示す構成における床形状推定器 1 3 0への入力は、大きく分けて以下のようになる。

1 ) 各種目標床反力中心点（目標全床反力中心点、目標第 n足平床反力中心点）

2 ) 想定床面形状（想定第 n床接点の座標、想定第 n足平床傾斜）、

3 ) 最終的に実ロボットが追従すべき目標姿勢（機構変形入り修正目標足平位置姿勢）または実関節変位またはこれらの周波数重み付き平均、

4 ) 上体傾斜偏差、

5 ) 実床反力（実第 n足平床反力の力成分とモーメント成分）

尚、床形状推定器 1 3 0への入力は、第 3 0図に示すものを用いると、最も推定精度を高くすることができるが、それ以外のものを選んでも良い。それに関しては第 2の実施の形態で述べる。

床形状推定器 1 3 0の処理機能プロック図を第 3 3図に示す。

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

機構コンプライアンスモデル 1 3 4は、ロボット幾何学モデル 1 1 0 (第 4図に示す）への入力、即ち、機構変形補償入り修正目標足平位置姿勢が、実床反力を受けたときのコンプライアンス機構 4 2の変形量を求め、求めた変形量を機構変形補償入り修正目標足平位置姿勢に加えることにより、機構変形後の推定各足平位置姿勢を求める。ここで、機構変形後の推定各足平位置姿勢は、上体 2 4に対する相対位置姿勢で表現される。

ロボットの関節変位制御の追従遅れが無視できない場合、第 3 4図に示すように、機構変形補償入り修正目標足平位置姿勢に、追従遅れに相当する口—パスフィル夕 1 3 6を通して機構コンプライアンスモデル 1 3 4に入力すれば良い。あるいは、第 3 5図に示すように、実関節変位からロボット幾何学モデル 1 3 7 (第 4図に 1 1 0と示すのと同様のモデル）を介して機構変形なし実足平位置姿勢を求め、求めた値を機構変形補償入り修正目標足平位置姿勢の代わりに機構コンプライアンスモデル 1 3 4に入力すれば良い。

あるいは、周波数重みを用い、前記機構変形なし実足平位置姿勢と機構変形補償入り修正目標足平位置姿勢の重みつき平均を求め、それを機構変形補償入り修正目標足平位置姿勢の代わりに機構コンプライアンスモデル 1 3 4に入力すれば良い。

厳密には、前記した機構変形なし実足平位置姿勢を用いるべきであるが、関節変位制御の能力が高ければ、上記のいずれでも大差はない。

また、第 4図においてロボット幾何学モデル 1 1 0への入力に機構変形補償が含まれなレ、場合、機構変形補償のない修正目標足平位置姿勢を機構コンプライアンス乇デル 1 3 4に入力すれば良い。

第 3 3図の説明に戻ると、床形状推定器 1 3 0は両脚間床傾斜偏差推定部 1 3 8を備える。即ち、同図において、機構変形後の推定各足平位置姿勢、目標各足平床反力中心点の目標足平に対する相対座標、想定各床接点および上体傾斜偏差から推定両脚間床傾斜偏差を推定するまでのブロックを「両脚間床傾斜偏差推定部」と呼ぶ。以下にこの部分の処理を説明する。

先ず、機構変形後の推定各足平位置姿勢と目標各足平床反力中心点 Q nの目標足平に対する相対座標とから、機構変形後の目標各足平床反力中心点 Q n " の座標を算出する。ここで、機構変形後の目標各足平床反力中心点 Q n " は、実上体に対する相対位置で表わされる。

より具体的には、目標第 n足平床反力中心点 Q nの足平に対する相対位置をべクトル R n、機構変形後の推定第 n足平位置をべクトル U n、機構変形後の推定第 n足平姿勢をマトリックス A nで表わすと、機構変形後の目標第 n足平床反力中心点 Q n " は次式によって計算される。

Qn" = AnRn+ Un

• · ·式 2 1

次に、 Q 1 " を始点、 Q 2 " を終点とするべクトル Q 1 " Q 2 " の実上体に対するべクトル Vまわりの傾き 0 f d b V " を算出する。ここでべクトル Vは、複合コンプライアンス制御で定義されたべクトルである。

—方、想定第 n床接点 Dnの目標上体に対する相対座標から、 D 1を始点、 D 2 を終点とするべクトル D 1 D 2の目標上体に対するべクトル Vまわりの傾き Θ f d b vを算出する。ここで、想定第 n床接点 Dnは、先に定義したように、第 n足平が接地している時期においては、目標第 n足平床反力中心点 Qnと同一位置とする。目標歩容において第 n足平が接地する直前の時期では、次に接地する時に想定されている目標第 II足平床反力中心点 Q の位置を想定第 n床接点 D nとする。目標歩容において第 n足平が離床した直後の時期では、離床時に想定されていた目標第 n足平床反力中心点 Q nの位置を想定第 n床接点 D nとする。

次に、前記の 0 f d b V " に、上体傾斜偏差の V方向成分 0 e r r vを加えることにより、 Q 1 " を始点、 Q 2 " を終点とするベクトル Q 1 " Q 2 " の目標上体に対するべクトル Vまわりの傾きを求め、それから前記の 0 f d b Vを減じることにより、 V方向瞬間両脚間床傾斜偏差計算値 0 f'db e r r Vを得る。次に、それから直前の推定両脚間床傾斜偏差 0 f db e s tmvを減じることにより、 0 f db e r r d i f f vを得る。次に、両脚間床傾斜偏差推定中断器 1 40において、両脚間床傾斜偏差の推定を実行すべきか中断すべきかを判断する

両脚間床傾斜偏差推定中断器 1 40は、両脚間床傾斜偏差の推定を実行（続行 ) すべきと判断したならば、両脚間床傾斜偏差推定中断器 1 40への入力である前記 0 f d b e r r d i f f vをそのまま出力し、中断すべきと判断したならば、 0を出力する。両脚間床傾斜偏差推定中断器 1 40に関する詳細は後述する。次に、両脚間床傾斜偏差推定中断器 1 40の出力を積分定数が Kdb e s tm である積分器 1 42で積分することにより、推定両脚間床傾斜偏差 0 f d b e s tmvを得る。推定両脚間床傾斜偏差 S ί d b e s tmvは、第 31図に示す両脚間床傾斜偏差の推定値であり、 Vまわりの角度で表わされる。以上が、両脚間床傾斜偏差推定部 1 38の処理である。

さらに、推定両脚間床傾斜偏差 0 f db e s tmvは、第 30図に示すように、両脚補償角 0 d b Vに加算され、床形状推定入り両脚補償角 0 db v' として修正目標足平位置姿勢算出部 1 1 4 gへ入力される。

以上の処理を毎制御周期に繰り返し実行することにより、推定両脚間床傾斜偏差 0 f db e s tmvが実際の両脚間床傾斜偏差に収束する。また、足平位置姿勢は推定両脚間床傾斜偏差 0 f d b e s tmvに従って修正され、両脚間床傾斜偏差が実床反力に及ぼす影響を吸収する。

尚、両実足平が接地しているならば、前記した Qn" は第 3 1図の実第 n床接点 Dn a c tに一致するはずである。従って、前記べクトル Q 1 " Q 2" と前記べクトル D 1 D 2の傾きの差 0 f d b e r r vは、第 30図における両脚間床傾斜偏差に一致するはずである。

しかし、実際にはロボット 1の機械系や制御系の振動および電気的なノイズにより、上記計算アルゴリズムによって得られた 0 f d b e r r Vは、実際の両脚間床傾斜偏差を中心値として激しく変動する。従って、 0 f d b e r r vをそのまま両脚間床傾斜偏差の推定値として用いて、これを第 30図に示すように両脚補償角に加算すると、ロボット 1の足平 22 R (L) は発振するかあるいは激しく振動する。

ところで、第 33図の両脚間床傾斜偏差推定中断器 1 40が常に推定実行（続行）と判断すれば、 0 f d b e r r vから推定両脚間床傾斜偏差 0 f d b e s ΐ mvまでの伝達関数は、 1次遅れの口一パスフィルタになる。即ち、推定両脚間床傾斜偏差 0 i db e s tmvは ^ f db e r r vに口一パスフィルタを通したものとなる。従って、推定両脚間床傾斜偏差 0 f d b e s tmvを第 30図に示すように両脚補償角に加算しても、足平 22R (L) の発振や振動を生じ難くなる。

ちなみに、この例では、口一パスフィル夕の時定数は 1 ZK d b e & t mである。第 33図に示す例以外にも、 0 i db e r r vから 0 f db e s tmvまでの伝達関数が口一パスフィル夕になるように構成すれば良い。これにより、足平 22R (L) の発振や振動を防止することができる。

次いで、両脚間床傾斜偏差推定部 1 38の特徴を説明する。

姿勢安定化制御のために必要な両脚補償モーメント V方向成分 Mdmd d b V を発生させようと、第 30図における両脚補償角 d b Vを変化させ、最終的な目標足平位置姿勢である機構変形補償入り修正自標足平位置姿勢を変更させても、両実足平が接地している限り、機構変形後の目標各足平床反力中心点 Qn" の実際の位置は、実第 n床接点 Dn a c tに一致したまま動かず、その代わりに、コンプライアンス機構 42が変形したり、上体 24の位置姿勢が変化する。即ち、機構変形補償入り修正目標足平位置姿勢を変更させた分を打ち消すように、コンプライアンス機構 4 2が変形したり、上体 2 4の位置姿勢が変化する。

この実施の形態における両脚間床傾斜偏差推定部 1 3 8は、機構変形補償入り修正目標足平位置姿勢から推定両脚間床傾斜偏差 0 f d b e s t m vを算出するまでに、実床反力を基に機構コンプライアンスモデル 1 3 4によって算出された機構部変形量と、傾斜センサ 3 6によって検出された実上体姿勢傾斜偏差を加えているので、機構変形補償入り修正目標足平位置姿勢が変動しても、推定両脚間床傾斜偏差 0 f d b e s t m vはその影響を受けない。

これは、姿勢制御と両脚間床傾斜偏差推定が非干渉化されていることを意味する。従って、推定両脚間床傾斜偏差 0 f d b e s t m vを第 3 0図に示すように両脚補償角に加算しても、制御系の安定余裕（発振のし難さ）はほとんど低下しない。この点は、前記したロボット学会論文に示す従来技術では得られない利点でめ。

また、推定両脚間床傾斜偏差 Θ f d b e s t m vを第 3 0図に示すように両脚補償角に加算すると、両脚間床傾斜偏差が存在していても、その影響を打ち消すことができるので、実全床反力は、定常的には想定床を歩行している場合と同一になる。しかも、両脚間床傾斜偏差の推定と補償角の修正は毎制御周期に実行されるので、床形状が途中で変化しても、変化後の両脚間床傾斜偏差を推定し、推定値に基づいてその影響を打ち消すことができる。

ただし、両脚間床傾斜偏差推定部 1 3 8には前述したように口一パスフィルタが含まれるので、両脚が接地したときや床形状が途中で変化したときには、過渡的には、実全床反力は、両脚間床傾斜偏差の影響を受ける。しかし、その後、時定数 1 ZK d b e s t mでその影響が減衰する。

さらに、第 3 3図に示すように、床形状推定器 1 3 0は足平床傾斜偏差推定部 1 4 4を備える。即ち、同図において、機構変形後の推定第 1足平位置姿勢、想定第 1足平床傾斜および上体傾斜偏差 X成分から推定第 1足平床傾斜偏差 X成分を推定するまでのブロックを「足平床傾斜偏差推定部」 1 4 4、より具体的には「第 1足平床傾斜偏差 X成分推定部」と呼ぶ。この部分の処理を説明する。先ず、機構変形後の推定第 1足平位置姿勢と想定第 1足平床傾斜から、想定第 1足平床傾斜に対する機構変形後の推定第 1足平位置姿勢の傾斜の X成分 0 f 1 e r r X " を算出する。

これは、目標第 1足平姿勢傾斜と想定第 1足平床傾斜との差にコンプライアンス機構 4 2のたわみ角（第 2図）を実床反力から推定した値を加算した値に相当する。ただし、第 2図では Y成分を示す。

次に、前記の 0 f 1 e r r X " に、上体傾斜偏差の X方向成分 0 e r r xを加えて瞬間第 1足平床傾斜偏差 X成分計算値 Θ f 1 e r r xを得る。

次に、それから直前の推定第 1足平床傾斜偏差 0 f 1 e s t m xを減じることにより、 0 f 1 e r r d i ί f xを得る。

次に、足平床傾斜偏差推定中断器 1 4 6、より具体的には第 1足平床傾斜偏差 X成分推定中断器において、第 1足平床傾斜偏差 X成分の推定を実行すべきか中断すべきかを判断する。

第 1足平床傾斜偏差 X成分推定中断器は、第 1足平床傾斜偏差 X成分の推定を実行すべきと判断したならば、第 1足平床傾斜偏差 X成分推定中断器への入力である前記 S f 1 e r r d i f f xをそのまま出力し、中断すべきと判断したならば、 0を出力する。尚、足平床傾斜偏差推定中断器 1 4 6、より具体的には第 1 足平床傾斜偏差 X成分推定中断器に関する詳細は後述する。

次に、第 1足平床傾斜偏差 X成分推定中断器の出力を積分定数が K 1 e s t m xである積分器 1 4 8で積分することにより、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m Xを得る。

以上が、第 1足平床傾斜偏差 X成分推定部の処理である。

推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xは、第 3 0図に示すように、第 1足平 X補償角 0 1 Xに加算され、床形状推定入り第 1足平 X補償角 0 1 X ' として修正目標足平位置姿勢算出部 1 1 4 gへ入力される。

以上の処理を毎制御周期に繰り返し実行することにより、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xが実際の第 1足平床傾斜偏差 X成分に収束する。また、足平位置姿勢は、推定第 1足平床傾斜偏差 X成分 f 1 e s t m xに従って修正され、第 1足平床傾斜偏差 X成分が実床反力に及ぼす影響を吸収する。尚、第 3 3図では第 1足平床傾斜偏差 X成分推定部 1 4 4のみ図示したが、それ以外にも第 1足平床傾斜偏差 Y成分推定部、第 2足平床傾斜偏差 X成分推定部、第 2足平床傾斜偏差 Y成分推定部が設けられ、第 1足平床傾斜偏差 Y成分、第 2足平床傾斜偏差 X成分および第 2足平床傾斜偏差 Y成分に関しても同様の処理が行われる。

足平床傾斜偏差推定に関しても、両脚間床傾斜偏差推定と同様に以下のことが口 'える。

実第 1足平がベたに接地しているならば（より厳密には、実第 1足平と実床が面接触しているか、あるいはつまさきエツジゃかかとエツジで接触しているように Y方向に線接触しているならば）、瞬間第 1足平床傾斜偏差 X成分計算値 0 f 1 e r r Xは、実際の第 1足平床傾斜偏差 X成分に一致するはずである。

しかし、実際には、ロボット 1'の機械系や制御系の振動および電気的なノイズにより、上記計算アルゴリズムによって得られた 0 f 1 e r r xは、実際の第 1 足平床傾斜偏差 X成分を中心値として激しく変動する。

従って、 0 f 1 e r r xをそのまま第 1足平床傾斜偏差 X成分の推定値として用い、それを第 3 0図に示すように第 1足平 X補償角に加算すると、ロボット 1 の足平 2 2 R ( L ) は、発振するかあるいは激しく振動する。

ところで、第 3 3図の第 1足平床傾斜偏差 X成分推定中断器が常に推定実行（続行）と判断すれば、 Θ f 1 e r r xから推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xまでの伝達関数は、 1次遅れの口一パスフィル夕になる。即ち、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xは 0 f 1 e r r xにローパスフィル夕を通したものとなる。

従って、推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xを第 3 0図に示すように第 1足平補償角に加算しても、足平の発振や振動を生じ難くなる。ちなみに、この例では、ローパスフィル夕の時定数は 1 ZK 1 e s t m xである。

第 3 3図に示す例以外にも、両脚間床傾斜偏差推定と同様に、 0 ί 1 e r r X から 0 ί 1 e s t m xまでの伝達関数が口一パスフィル夕になるように構成すれば良い。これにより、足平 2 2 R ( L ) の発振や振動を防止することができる。次いで、足平床傾斜偏差推定部 1 4 4の特徴を説明する。

両脚間床傾斜偏差推定と同様に以下のことが言える。姿勢安定化制御のために必要な第 1足平補償モーメント X成分 M d m d 1 xを発生させようと、第 3 0図における第 1足平補償角 Θ 1 Xを変化させて、最終的な目標足平位置姿勢である機構変形補償入り修正目標足平位置姿勢を変更させても、実第 1足平がベたに接地している限り、実足底面の姿勢は変わらず、その代わりに、コンプライアンス機構 4 2が変形したり、上体 2 4の位置姿勢が変化する。即ち、両脚間床傾斜偏差推定と同様に、機構変形補償入り修正目標足平位置姿勢を変更させた分を打ち消すように、コンプライアンス機構 4 2が変形したり、上体 2 4の位置姿勢が変化する。

この実施の形態における第 1足平床傾斜偏差 X成分推定部は、機構変形補償入り修正目標足平位置姿勢から推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xを算出するまでに、実床反力を基に機構コンプライアンスモデル 1 3 4によって算出されたコンプライアンス機構変形量と、傾斜センサ 3 6によって検出された実上体姿勢傾斜偏差を加えているので、機構変形補償入り修正目標足平位置姿勢が変動しても、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xはその影響を受けなレ、。

これは、姿勢制御と第 1足平床傾斜偏差推定が非干渉化されていることを意味する。従って、推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xを第 3 0図に示すように第 1足平補償角に加算しても、両脚間床傾斜偏差推定と同様に、制御系の安定余裕はほとんど低下しなレ、。

また、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xを第 3 0図に示すように第 1足平補償角に加算すると、第 1足平床傾斜偏差が存在していても、その影響を打ち消すことができるので、実第 1足平床反力は、定常的には想定床を歩行している場合と同一になる。

しかも、第 1足平床傾斜偏差の推定と補償角の修正は毎制御周期に実行されるので、床形状が途中で変化しても、変化後の第 1足平床傾斜偏差を推定し、その影響を打ち消すことができる。

ただし、第 1足平床傾斜偏差推定器には前記のローパスフィルタが含まれるので、第 1足平が接地したときや床形状が途中で変化したときには、過渡的には、第 1足平床傾斜偏差の影響を受け、その後、時定数 1 ZK 1 e s t m xでその影響が減衰する。

上記したこの実施の形態における床形状推定の特徴を総合的に説明する。

この実施の形態における床形状推定が前提とする複合コンプライアンス制御においては、各補償角を操作して実全床反力および各足平床反力を制御しても、その特徵として、それぞれの制御は非干渉化されている。

従って、推定両脚間床傾斜偏差 S f d e s tmvを求めることと、それを両脚補償角 S d b Vに加算することと、推定各足平床傾斜偏差 f 1 e s tmx, θ f 1 e s t my, Θ f 2 e s t mx, 0 f 2 e s t m yを求めることと、それらをそれぞれ各足平補償角 0 1 χ， Θ 1 y, θ 2 χ, Θ 2 yに加算することを同時に行っても、干渉し合って発振することはほとんどない。

その特徴と前述した両脚間床傾斜偏差推定部 1 3 8と足平床傾斜偏差推定部 1 4 4の各特徴により結果的に以下の利点が得られる。 .

1 ) 姿勢復元力制御のために両脚補償角 0 d b Vと各足平補償角 0 1 X, Θ 1 y ， θ 2 x, 02 yを操作して実床反力を制御することと、両脚間床傾斜偏差を推定することと、これを両脚補償角 0 db vに加算することと、各足平床傾斜偏差を推定することと、それらを各足平補償角に加算することを同時に行うことができる。

簡単に言えば、床形状偏差の複数のパラメ一夕を同時に推定しながら、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作も同時に実行することができる。さらには、姿勢制御のための脚の補償動作も同時に実行することがでさる。

2) 床形状偏差が大きくても制御が破綻し難い（複合コンプライアンス制御は微小角に限定した近似を行っていないから）。

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

4) 姿勢安定化制御との干渉による発振が生じにくいので、推定部 1 3 8, 1 4 4のゲイン (Kd b e s tm, K l e s tmx, K 1 e s t m y , K 2 e s t m x, K 2 e s tmy) を大きくすることができ、その結果、推定時定数を短くすることができる。それによつて、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作の応答性を高めることができるので、床形状偏差が床反力に及ぼす過渡的な影響が短時間で解消される。

5 ) 床形状偏差推定と床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作が常時実行されるので、途中で床形状が変化しても、過渡的にはその影響を受けるが、その後、推定の時定数でその影響が減衰する。

次いで、この実施の形態における前記した推定中断器 1 4 0 , 1 4 6について説明する。

先ず、第 1足平床傾斜偏差 Y成分推定中断器の役割について説明する。

第 1足平床傾斜偏差推定 Y成分中断器は、第 1足平床傾斜偏差 Y成分の推定が正常に実行できるか否かを判断し、正常に実行できないと判断したときには推定を中断する。

より具体的には、原則的に、推定第 1足平床傾斜偏差 Y成分の精度が低下しそうな状況、および推定第 1足平床傾斜偏差 Y成分が発散しそうな状況では、推定を中断するために、 0を出力し（即ち、積分入力に 0を入れて積分値をホールドする）、それ以外の状況では、推定を実行するために、第 1足平床傾斜偏差 Y成分推定中断器への入力をそのまま出力する。

もし、第 1足平床傾斜偏差 Y成分推定中断器が動作せず、常に第 1足平床傾斜偏差 Y成分推定中断器への入力をそのまま出力し続けると、ある状況では推定第 1足平床傾斜偏差 Y成分が発散する。

以下にこの実施の形態において推定第 1足平床傾斜偏差 γ成分が発散する状況を説明する。

例として、第 3 6図に示すように、ロボット 1が片脚支持期に前傾したために、姿勢安定化制御によって第 1足平 Y補償角を操作して大きな姿勢復元力を得ようと試みた結果、本来べたに接地すべき実第 1足平のかかとが床から浮いてしまつた状況を仮定する。また、このとき実床形状は想定床形状と一致していると仮疋^る。

この状況では、第 3 6図に示される想定第 1足平床傾斜 Y成分に対する機構変形後の推定第 1足平姿勢の傾斜 Y成分 0 f 1 e r r y " に上体傾斜偏差 Y成分 Θ e r r yを加えた f 1 e r r yは、同図に示すように、正の値になる。

第 1足平床傾斜偏差 Y成分推定中断器が、常に入力である Θ f 1 e r r d i f f yをそのまま出力し続ける場合には、前述したように、 0 f 1 e r r yから推定両脚間床傾斜偏差 0 f 1 e s t myまでの伝達関数は、 1次遅れになる。

即ち、 0 f l e s tmyは、 0 f l e r r yにやや遅れて追従する。 0 f 1 e s tmyは、第 1足平 Y補償角 Θ 1 yに加算され、床形状推定入り第 1足平 Y補償角 01 y' になる。第 30図では 01 x' のみ図示しているが、 01 yも同様な構成になる。

次に、目標第 1足平位置姿勢に床形状推定入り第 1足平 Y補償角 Θ 1 y' が加えられ、さらに目標床反力（目標歩容の床反力）から算出される機構変形量が減じられ（即ち、機構変形量を打ち消す機構変形補償量が加えられ）、機構変形補償入り修正目標第 1足平位置姿勢が算出される。

さらに機構コンプライアンスモデル 1 34によって、実床反力から計算される機構変形量が求められ、機構変形補償入り修正目標第 1足平位置姿勢に加えられて機構変形後の推定第 1足平位置姿勢が求められる。この傾き Y成分から想定第 1足平傾斜の Y成分が減じられ、さらに上体傾斜偏差 Y成分 0 e r r yが加えられることにより、前記 Θ f 1 e r r yの値が更新される。

以上の関係から、 Θ f 1 e r r yとこれに 1次遅れで追従する 0 f 1 e s t m yの関係は以下のようになる。尚、目標床反力による第 1足平機構変形の補償角は、先に段落 0 1 35で述べたコンプライアンス機構 42の変形量を打ち消す補償量である。

Θ f lerry= 0 f lestmy

+ Θ ly+目標床反力による第 1足平機構変形の補償角 Y成分 + 0erry +実床反力から計算される第 1足平機構変形角 Y成分

+目標第 1足平姿勢傾斜 Y成分 -想定第 1足平傾斜 Y成分

■ . ·式 22

目標歩容の片足支持期においては、（目標第 1足平姿勢傾斜 Y成分 -想定第 1 足平傾斜の Y成分）は 0である。従って、式 22は次式に変形される。

Θ flerry= ^flestmy

+ 0 ly +目標床反力による第 1足平機構変形の補償角 Y成分 + Θ erry +実床反力から計算される第 1足平機構変形角 Y成分 ■ · ■式 2 3 第 1足平が床にベたに接地しているならば、（S f l e s tmy + 0 1 y +目標床反力による第 1足平機構変形の補償角 Y成分 + Θ e r r y) がいかなる値であっても、実床反力から計算される機構変形角 Y成分はそれを打ち消すので、式 2 3の右辺は 0になる。従って、 0 f 1 e r r yは常に 0となり、また、 Θ f 1 e s tmyは、 0 ί 1 e r r yに 1次遅れで追従するので、 0に徐々に収束する。これは、実床形状が想定床形状と一致している状況を推定器が正確に推定していることを意味する。

ところ力、第 3 6図の状況では（0 f l e s t my + 0 1 y +目標床反力による第 1足平機構変形の補償角 Y成分 +0 e r r y) は大きな正の値となるが、かかとが浮いてしまったために実床反力が十分に大きくなれず、実床反力から計算される機構変形角 Y成分は、前記の値を打ち消すだけの十分な大きさの負の値を持つことができない。即ち、式 2 3の右辺は 0にならない。このような状況では、 θ ϊ I e r r yは 0になれず、 0 f 1 e s t myは 0に収束しない。これは、推定値が間違つていることを意味する。

このような状況の中でも、特に次の式 2 4を満足している状況が続く場合には、 0 f 1 e r r yに近づこうとして 0 f 1 e s t myが増大しても、 0 f 1 e r r yは、式 2 3と次の式 2 4から導かれるように、 0 f l e r r y> 0 f l e s t myの関係を保つので、 0 f 1 e s t myから離れてしまう。

これを繰り返すうちに、 0 f 1 e s t myは正の方向にますます発散する。これは、推定値が発散しつつあることを意味する。

0 ly +目標床反力による第 1足平機構変形の補償角 Y成分 + Θ erry

+実床反力から計算される第 1足平機構変形角 Y成分 > 0

• . '式 2 4

次いで、第 3 7図を参照して第 1の実施の形態における第 1足平床傾斜偏差 Y 成分推定中断器の処理を説明する。この処理は毎制御周期に実行される。

以下に説明すると、先ず S 3 0 0において目標歩容の時期に応じて第 1足平床傾斜 Y成分推定モードを決定する。

具体的には、第 1足平床傾斜 Y成分推定モードは、リセットモード、ホールドモードおよび準備完了モードの 3つのモード状態を取りうるものとし、目標歩容の時間経過に対して、第 3 8図に示す如く遷移するように決定する。ただし、これは第 5図に示す歩容に対するモード遷移パターンである。歩容が異なれば、それに応じてモ一ド遷移パ夕一ンも変えるべきである。

従って S 3 0 2に進み、第 1足平床傾斜 Y成分推定モードに応じて以下の処理を行う。

第 1足平床傾斜 Y成分推定モードがリセットモードである場合は S 3 0 4に進み、推定第 1足平床傾斜偏差 Y成分 0 f 1 e s t m yを強制的に徐々に 0に収束させ、 S 3 0 6に進み、第 1足平床傾斜偏差 X成分推定中断器の出力を 0にする尚、リセットモ一ドは、第 3 8図に示す如く、第 1足平を動かしても床反力に影響がないように、第 1足平が床から完全に離れている期間に存在させる。 1歩前に着地していた第 2床接点の足平床傾斜偏差と次に着地する第 1床接点の足平床傾斜偏差との関係に相関がない場合には、上記のように推定第 1足平床傾斜偏差 Y成分 0 f 1 e s t m yを強制的に徐々に 0に収束させるのが良い。

しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることがわかっているならば、前記関係にある程度の相関があると考えられるので、推定第 1足平床傾斜偏差 Y成分 Θ f 1 e s t m yを、強制的に徐々に 1歩前に着地していた第 2床接点での推定第 2足平床傾斜偏差に 1より小さい正の定数を乗じた値に収束させるようにしても良い。

さらに、 1歩前の推定足平床傾斜偏差 Y成分だけでなく、複数歩前の推定足平床傾斜偏差 Y成分も用いて収束目標値を決定しても良い。また、 1歩前や複数歩前の推定両脚間床傾斜偏差も用いて収束目標値を決定しても良い。

第 1足平床傾斜 Y成分推定モ一ドがホールドモ—ドである場合は S 3 0 8に進み、第 1足平床傾斜偏差 Y成分推定中断器の出力を 0にする。従って、推定第 1 足平床傾斜偏差 Y成分 0 f 1 e s t m yはホールドモードに遷移する直前の値に保持される。

尚、ホールドモードは、推定第 1足平床傾斜偏差 Y成分 0 f 1 e s t m yが発散しそうな時期およびその精度が低下しそうな時期に存在させる。例えば、第 3 8図に示すように、第 1足平のかかとまたはつまさきが床から離れ始める時期から第 1足平が床から完全に離れてしばらく後までの期間に存在させる。

上記以外の時期、即ち、第 1足平床傾斜偏差 Y成分の推定が正常に実行できる時期では、第 1足平床傾斜 Y成分推定モードは準備完了モードになる。

準備完了モードでは、以下の処理を行う。

即ち、 S 3 1 0に進み、実第 1足平床反力モーメント M 1 a c tと実第 1足平床反力の力成分 F 1 a c tから実第 1足平床反力中心点位置 Q 1 a c tを求める。ここで、実第 1足平床反力中心点位置 Q 1 a c tは、その点における実第 1足平床反力モーメン卜の内の鉛直方向を除く方向の成分が 0となる足底上の作用点である。実第 1足平床反力中心点 Q 1 a c tの X座標を Q 1 a c t Xとする。次いで S 3 1 2に進み、あらかじめ設定しておいた第 1足平推定許容最小床反力 F l m i n、第 1足平推定許容領域最大値 Q 1 m a x xおよび第 1足平推定許容領域最小値 Q 1 m i n xに対し、実第 1足平床反力 Z成分（鉛直成分） F 1 a c t zと前記 Q 1 a c t xが以下の条件を満たすか否かを調べる。

条件 1 ) Flactz く Flmin

条件 2 ) Qlactx > Qlmaxx かつ 0 f lerrdiify > 0

条件 3 ) Qlactx く Qlminx かつ S f lerrdif fy く 0

条件 1に関して補足すると、第 1足平推定許容最小床反力 F l m i nは、第 1 足平にそれ以下の床反力しか作用していない場合には第 1足平床傾斜が推定できないと判断される足平床反力設定値であり、ロボット 1の自重より小さな値に設定しておく。換言すれば、条件 1は、第 1足平と床との間の圧力が小さ過ぎて第 1足平床傾斜 Y成分が精度良く推定できない状況を意味する。

一方、条件 2および条件 3に関しては、足平の座標系から見て、つまさきの X 座標を X t o e , かかとの X座標を X h e e 1 とすると、あらかじめ、第 1足平推定許容領域最大値 Q 1 m a X Xの値を、 x t o eよりもやや小さめに設定しておき、第 1足平推定許容領域最小値 Q 1 m i n xの値を、 x t o eよりもやゃ大きめに設定しておく。これらの関係を第 3 9図に示す。

推定が理想的に実行されるならば、第 1足平推定許容領域最大値 Q l m a x x と第 1足平推定許容領域最小値 Q 1 m i n Xは、エッジに一致させても良い。しかし実際には検出誤差が生じ、これにより推定値が発散する場合がある。そこでこれを防ぐために、第 3 9図のようにエッジからやや内側に第 1足平推定許容領域最大値 Q 1 m a X Xと第 1足平推定許容領域最小値 Q 1 m i n xを設定してお

< o

条件 2は、かかとが浮きかかっているのに、積分器入力 0 f 1 e r r d i f f yが正、即ち、推定第 1足平床傾斜がかかとが浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り第 1足平 Y補償角が、ますますかかとが浮く方向に変化する。

条件 3は、つまさきが浮きかかっているのに、積分器入力 Θ f 1 e r r d i f f yが負、即ち、推定第 1足平床傾斜がつまさきが浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り第 1足平 Y補償角が、ますますつまさきが浮く方向に変化する。

第 3 7図フローチャートの説明に戻ると、 S 3 1 2で肯定されて条件 1、条件 2または条件 3のいずれかを満足する場合には、推定を中断すべきであるので、 S 3 1 4に進んで第 1足平床傾斜偏差 Y成分推定中断器の出力を 0にし、それ以外の場合には、推定を実行しても問題がないので、 S 3 1 6に進んで前記出力を 0 f 1 e r r d i f f yにする。

条件 1、条件 2および条件 3の式は、以上に述べた状況を表わしていれば良い。従って、条件 1では、実第 1足平床反力 Z成分（鉛直成分） F 1 a c t zの代わりに、実第 1足平床反力の力成 F 1 a c tの内の足平に垂直な成分（この成分を「F 1 a c t z ' 」と呼ぶ）を用いても良い。

また、条件 2および条件 3では、前記実第 1足平床反力中心点位置 Q 1 a c t の代わりに、実第 1足平床反力モーメントの内の足平に垂直な方向を除く方向の成分が 0となる足底上の作用点（この点を「Q 1 a c t ' 」と呼ぶ）を用いても良い。

また、この実施の形態に係る第 1足平床傾斜偏差 X成分推定中断器の処理について説明すると、第 1足平床傾斜偏差 X成分推定中断器も、第 1足平床傾斜偏差 Y成分推定中断器とほぼ同様の処理を行う。

相違点は、前記条件 2および条件 3において、つまさきまたはかかとが浮く条件の代わりに足平の内エッジ、外エッジが浮く条件を用いることである。即ち、条件 2と条件 3の代わりに、以下の条件 2 ' と条件 3 ' を用いる。

ここで Q 1 m a X yおよび Q 1 m i n yは、第 3 9図のように足平内エツジと外エツジの足平中心側に設定される。

条件 2 ' ) Qlacty > Qlraaxy かつ 0 f lerrdiffx < 0

条件 3 ' ) Qlacty く Qlminy かつ S flerrdiffx > 0

尚、座標系の向きの関係上、不等号が、条件 2、条件 3の場合と一部異なる。また、第 1足平床傾斜偏差 X成分推定モードの遷移タイミングは、第 1足平床傾斜偏差 Y成分推定モードの場合と較べ、準備完了モードの終了時期をもう少し遅らせても良い。つまさきエッジが接地していれば、まだ第 1足平床傾斜偏差 X成分が推定できるからである。

また、第 2足平床傾斜偏差推定中断器の処理も、第 1足平床傾斜偏差推定中断器の処理と同様である。

次いで、第 4 0図を参照して両脚間床傾斜偏差推定中断器 1 4 0の処理を説明する。この処理は毎制御周期に実行される。

この処理は、第 1足平床傾斜偏差 Y成分推定中断器と基本的には同じであるが、細部が異なる。

以下説明すると、先ず S 4 0 0において目標歩容の時期に応じて両脚間床傾斜推定モードを決定する。

具体的には、両脚間床傾斜推定モードは、目標歩容の時間経過に対して、第 4 1図のように決定する。ただし、これも第 5図に示す歩容に対するモードパターンであり歩容が異なれば、それに応じてモードパターンも変えるべきである。次いで S 4 0 2に進み、両脚間床傾斜推定モードに応じて以下のように処理す両脚間床傾斜推定モードがリセットモ一ドである場合は S 4 0 4に進み、推定両脚間床傾斜偏差 f d b e s t m vを強制的に徐々に 0に収束させ、 S 4 0 6 に進んで両脚間床傾斜偏差推定中断器の出力を 0にする。尚、リセットモードは、第 4 1図に示すごとく、床形状推定入り両脚補償角を動かしても全床反力に影響がないように、片脚支持期に存在させる。 1歩前の両脚間床傾斜偏差と次の 1歩の両脚間床傾斜偏差との関係に相関がない場合には、このように推定両脚間床傾斜偏差 0 f d b e s t m vを強制的に徐々に 0に収束させるのが良い。しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることが分かっているならば、前記関係にある程度の相関があると考えられるので、推定両脚間床傾斜偏差 0 f d b e s t m vを、強制的に徐々に 1歩前の両脚間床傾斜偏差に 1より小さい正の定数を乗じた値に収束させるようにしても良い。

さらに、 1歩前の推定両脚間床傾斜偏差だけでなく、複数歩前の推定両脚間床傾斜偏差も用いて収束目標値を決定しても良い。また、 1歩前や複数歩前の推定足平床傾斜偏差も用いて収束目標値を決定しても良い。

両脚間床傾斜推定モードがホールドモ一ドである場合は S 4 0 8に進み、両脚間床傾斜偏差推定中断器の出力を 0にする。従って、推定両脚間床傾斜偏差 0 ί d b e s t m vは、ホールドモードに遷移する直前の値に保持される。

尚、ホールドモードは、推定両脚間床傾斜偏差 Θ ί d b e s t m vが発散しそうな時期およびその精度が低下しそうな時期に存在させる。例えば、第 4 1図に示すように、両脚支持期の終了時点を始めとしてそれからしばらく後までの期間に存在させる。

上記以外の時期、即ち、両脚間床傾斜偏差の推定が正常に実行できる時期では、両脚間床傾斜偏差モードは準備完了モードになる。準備完了モードでは、以下の処理を行う。

先ず S 4 1 0に進み、あらかじめ設定しておいた両脚間床傾斜推定許容最小床反力 F m i n、両脚間床傾斜推定許容比 r m i nに対し、実各足平床反力が以下の条件を満たすか否かを調べる。

条件 4 ) Flactz く Fmin または F2actz く Fmin

条件 5 ) F2actz I ( Flactz +F2actz ) < rmin かつ Θ fdberrdiffv > 0 条件 6 ) Flactz I ( Flactz +F2actz ) く rmin かつ 0 fdberrdiff v < 0 条件 4に関して補足すると、両脚間床傾斜推定許容最小床反力 F m i nは、第 1足平または第 2足平にこれ以下の床反力しか作用していない場合には、両脚間床傾斜が推定できないと判断される足平床反力設定値であり、通常、ロボット 1 の自重より小さな値に設定しておく。換言すれば、条件 4は、第 1足平または第

2足平と床との間の圧力が小さ過ぎて両脚間床傾斜が精度良く推定できない状況思昧する。

一方、条件 5および条件 6に関しては、推定が理想的に実行されるならば、両脚間床傾斜推定許容比 r m i nは 0でも良い。しかし実際には検出誤差が生じ、それにより推定値が発散する場合がある。それを防ぐために、 r m i nには、 1 よりかなり小さレ、正の値を設定しておく。

条件 5は、第 2足平が浮きかかっているのに、積分器入力 f d b e r r d i f f vが正、即ち、推定両脚間床傾斜が第 2足平が浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り両脚補償角は、ますます第 2足平が浮く方向に変化する。

条件 6は、第 1足平が浮きかかっているのに、積分器入力 0 f d b e r r d i f f vが負、即ち、推定両脚間床傾斜が第 1足平が浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り両脚補償角は、ますます第 1足平が浮く方向に変化する。

第 4 0図フローチャートの説明に戻ると、条件 4、条件 5または条件 6のいずれかを満足する場合には推定を中断すべきであるので、 S 4 1 2に進んで両脚間床傾斜偏差推定中断器の出力を 0にし、それ以外の場合には推定を実行しても問題がないので、 S 4 1 4に進んで前記出力を 0 f d b e r r d i f f vにする。条件 4、条件 5および条件 6の式は、以上に述べた状況を表わしていれば良い。従って、実第 n足平床反力 Z成分（鉛直成分） F n a c t zの代わりに、実第 n足平床反力の力成分 F n a c tの内の足平に垂直な成分 F n a c t ' を用いても良い。

この実施の形態は上記の如く構成したので、従来技術では困難であつた床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができる。

さらには、床形状の推定結果を基に足平軌道を修正することにより、床形状が想定していた形状と異なっていても、その影響を吸収して望み通りの床反力を発生させることができる。特に、複合コンプライアンス制御で除去しきれなかつた実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれに起因する床反力の定常偏差を解消することができる。

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

尚、この実施の形態に係る脚式移動ロボッ卜の床形状推定装置は上記した手法で床形状を推定すれば足り、推定値に基づいて床反力を操作して目標位置姿勢などを制御することは必須ではない。

次いで、この発明の第 2の実施の形態に係る脚式移動ロボットの床形状推定装置を説明する。

第 2の実施の形態は、第 1の実施の形態を簡易化したものであり、その全体構成を第 4 2図に示す。第 1の実施の形態とは、床形状推定器への入力と床形状推定器内での処理が異なる。それ以外は第 1の実施の形態と同様である。

第 2の実施の形態においては、詳細は以下に説明するが、簡単に言えば、第 1 の実施の形態の機構コンプライアンスモデル 1 3 4を近似してから、ブロック線図を等価変換するようにした。

第 2の実施の形態に係る床形状推定器の処理を説明する前に、先ず、用語の定義と、第 2の実施の形態で用いられるコンプライアンスモデルの説明を行う。第 4 3図の上部に示すように、機構変形補償入り修正目標足平位置姿勢と目標上体位置姿勢の相対関係を満足するように関節角を変位制御し、同図中部に示すように、目標上体姿勢から上体傾斜偏差 e r rだけ傾けてロボット 1の上体 2 4を空中に支持し、さらに同図下部に示すように、目標各足平床反力に相当する外力を足平に作用させたと仮定したときに、そのような状況にある足平内の目標第 1足平床反力中心点 Q 1 " 'を始点、目標第 2足平床反力中心点 Q 2 " 'を終点とするベクトルの前記 V方向まわりの傾きを「理想変形時両脚間傾斜」と呼ぶ。また、この仮定における第 n足平の姿勢傾斜を「理想変形時第 n足平傾斜」と呼ぶ。上体傾斜偏差、両脚補償角、各足平補償角、推定両脚間床傾斜偏差、推定第 1足平床傾斜偏差および推定第 2足平床傾斜偏差がすべて 0であるならば、理想変形時両脚間傾斜は想定両脚間床傾斜に一致し、理想変形時第 n足平傾斜は想定第 n足平床傾斜に一致する。

想定両脚間床傾斜に推定両脚間床傾斜偏差を加算したものを「推定両脚間床傾斜 J と呼ぶ。これは、 V方向まわりの角度で表わされる。また、想定第 n足平床傾斜に推定第 n足平床傾斜偏差を加算したものを「推定第 n足平床傾斜」と呼ぶ。理想変形時両脚間傾斜と推定両脚間床傾斜との差を「推定両脚干渉角 S d b i n t v」と呼ぶ。これも、 V方向まわりの角度で表わされる。

また、理想変形時第 1足平傾斜と推定第 1足平床傾斜との差を「推定第 1足平干渉角 Θ 1 i n t」と呼ぶ。同様に理想変形時第 2足平傾斜と推定第 2足平床傾斜との差を「推定第 2足平干渉角 S 2 i n t」と呼ぶ。

推定両脚間床傾斜が実両脚間床傾斜に一致していて、かつ実ロボットの足平の目標各足平床反力中心点が接地していると仮定すると、推定両脚干渉角 0 d b i n t vを打ち消すように、実ロボットのコンプライアンス機構 4 2が変形しているはずである。

さらにこの機構変形に応じて、目標全床反力中心点まわりに各足平床反力の力成分が作るモーメントの和の V方向成分が発生するはずである。この成分を「M d b e s t m v」と呼ぶ。

そこで、推定両脚干渉角 Θ d b i n t vと M d b e s t m vとの関係をあらかじめ調べておき、記憶しておく。この関係は、実ロボットのコンプライアンス機構 4 2の特性の内で、両脚干渉角に関する特性だけを抽出したものであり、それ故に、「両脚間機構コンプライアンスモデル」 1 3 4 a (第 4 4図に示す）と呼ヽ

推定第 n足平床傾斜が実第 n足平床傾斜に一致していて、かつ実13ボットの第 n足平がベたに接地していると仮定すると、推定第 n足平干渉角 i n tを打ち消すように、実ロボットのコンプライアンス機構 4 2が変形しているはずである。

さらにこの機構変形に応じて、目標第 1足平床反力中心点まわりに第 1足平床反カ乇一メントが発生するはずである。このモーメントを「M 1 e s t m」と呼ぶ。同様に目標第 2足平床反力中心点まわりに第 2足平床反力モ―メントが発生するはずである。このモーメントを「M2 e s t m」と呼ぶ。

そこで、推定第 n足平干渉角の X成分 0 n i n t Xと前記モ一メントの X成分 Mn e s t mxとの関係をあらかじめ調べておき、記憶しておく。この関係は、実ロボットのコンプライアンス機構 4 2の特性の内で、足平干渉角に関する特性だけを抽出したものであり、それ故に、「足平機構コンプライアンスモデル」と呼ぶ。第 1足平 X成分用足平機構コンプライアンスモデルを第 4 5図に記載の 1 34 bに示す。

次に、第 2の実施の形態における床形状推定器 1 3 0 aの構成を説明する。第 2の実施の形態における床形状推定器 1 3 0 aは、両脚間床傾斜偏差推定部

1 3 8 aと足平床傾斜偏差推定部 1 44 aとから構成される。両脚間床傾斜偏差推定部 1 3 8 aの処理機能ブロック図を第 4 4図に示す。足平床傾斜偏差推定部

1 44 aの処理機能ブロック図を第 45図に示す。

第 4 5図では、推定第 1足平床傾斜偏差 X成分に関する処理機能ブロック図のみを示すが、推定第 1足平床傾斜偏差 Y成分、推定第 2足平床傾斜偏差 X成分、推定第 2足平床傾斜偏差 Y成分についても同様の処理を行う。

先ず、第 2の実施の形態に係る両脚間床傾斜偏差推定部 1 3 8 aの処理を第 4 4図を参照して説明する。

先ず、上体傾斜偏差 0 e r rを V方向成分とそれと直交する方向の成分に分解したときの V方向成分 Θ e r r vを求める。

次に、目標歩容の目標第 1足平床反力中心点 Q 1を始点、目標第 2足平床反力中心点 Q 2を終点とするべクトルの傾斜を求め、求めた傾斜から想定両脚間床傾斜を減じたものを想定両脚干渉角 0 db i n t cmd vとする。

次に、次式により、前述の推定両脚干渉角 0 d b i n t vを求める。

Θ dbintv二 Θ dbv' + Θ errv- θ fdbestmv+ θ dbintcmdv

• · ·式 25

ここで、 0 f db e s tmvは、第 1の実施の形態でも述べたように、推定両脚間床傾斜偏差である。また、 S db v' は、床形状推定入り両脚補償角であり、第 4 3図に示すように、両脚補償角 0 d b vと推定両脚間床傾斜偏差 0 f d b e s t m vの禾 Πでめる。床形状推定入り両脚補償角 0 d b V ' および推定両脚間床傾斜偏差 Θ f d b e s tmvは、それから今回値を求めるものであるから、この処理では記憶しておいた直前の値を用いる。

尚、両脚が接地する想定時刻よりも前から両脚間床傾斜偏差を推定したい場合において、より精度を高めるために、想定両脚干渉角 0 d b i n t cmd Vを加算することが望ましいが、想定両脚干渉角 0 d b i n t cm d vの加算および想定両脚干渉角 Θ d b i n t cm d vを算出する構成（第 4 4図において点線で囲まれる構成）は必ずしも必須ではない。

次に両脚間機構コンプライアンスモデル 1 3 4 aを用いて、推定両脚干渉角 0 d b i n t vによって発生すると予想される前記 Md b e s t m vを求める。一方、目標全床反力中心点まわりに実各足平床反力の力成分が作る乇一メントの和の V方向成分 M f 1 f 2 a c t V (複合コンプライアンス制御の式 1 2および式 1 3で求められるモーメントの V方向成分）を求める。

次に、前記 M f 1 f 2 a c t Vと前記 Md b e s t m vとの差を求めることにより、両脚モーメント推定偏差 Mdb e r rを得る。もし、推定両脚間床傾斜偏差 0 ί db e s t mvが実両脚間床傾斜偏差に一致しているならば、両脚モ一メント推定偏差 Md b e r rは 0になる。逆に、もし両脚モーメント推定偏差 M d b e r r力 0ならば、推定両脚間床傾斜偏差 Θ f d b e s t m vは実両脚閭床傾斜偏差に一致する。

次に、両脚間床傾斜偏差推定中断器 1 4 0 aにおいて、両脚間床傾斜偏差の推定を実行すべきか中断すべきかを判断する。両脚間床傾斜偏差推定中断器 1 4 0 aは、両脚間床傾斜偏差の推定を実行すべきと判断したならば、両脚間床傾斜偏差推定中断器 1 4 0 aへの入力である前記 Md b e r rをそのまま出力し、中断すべきと判断したならば、 0を出力する。両脚間床傾斜偏差推定中断器 1 4 0 a に関する詳細は後述する。

次に、両脚間床傾斜偏差推定中断器 1 4 0 aの出力を積分定数が Kd b e s t m' である積分器 1 4 2 aで積分し、推定両脚間床傾斜偏差 Θ f d b e s t m v を得る。

以上が、第 2の実施の形態における両脚間床傾斜偏差推定部 1 3 8 aの処理である。

さらに、推定両脚間床傾斜偏差 0 f d b e s tmvは、第 4 2図に示すように、両脚補償角 Θ d b Vに加算され、床形状推定入り両脚補償角 Θ db v' として修正目標足平位置姿勢算出部 1 1 4 gへ入力される。

このような処理を毎制御周期に繰り返し実行することにより、両脚モーメント推定偏差 Md b e r rが 0に収束するように、積分器出力である推定両脚間床傾斜偏差 0 f d b e s tmvが変化する。

ところで前述したように、両脚モ一メント推定偏差 Md b e r rが 0になると、推定両脚間床傾斜偏差 Θ f d b e s tmvが実際の両脚間床傾斜偏差に一致する。従って、しばらく経つと、推定両脚間床傾斜偏差 0 f d b e s tmvは、実両脚間床傾斜偏差に一致する。即ち、この処理によって、両脚間床傾斜偏差が推疋される。

尚、両脚間機構コンプライアンスモデル 1 34 aが線形であるならば、その処理系の実両脚間床傾斜偏差から推定両脚間床傾斜偏差までの閉ループ伝達関数は、 1次遅れになる。

その時定数 Tは、両脚間機構コンプライアンスモデル 1 34 aにおける入力（推定両脚干渉角）に対する出力（前記モーメント Mdb e s tmv) の比（絶対値）を Cとすると、次式で求められる。

T 二 1 / ( C* Kdbestm' )

. · ■式 2 6

第 1の実施の形態の積分ゲイン K d b e s tmに対して、第 2の実施の形態の積分ゲイン K d b e s ΐ m' を

Kdbestra' = Kdbestm I C

' ■ · ·式 27

とすることにより、第 1の実施の形態とほぼ同一の特性を持つ両脚間床傾斜偏差推定器 1 3 8 aが得られる。

次に、足平床傾斜偏差推定部 1 44 aの理を第 4 5図を参照して説明する。第 4 5図では、推定第 1足平床傾斜偏差 X成分に関する処理機能ブロック図のみを示すが、推定第 1足平床傾斜偏差 Y成分、推定第 2足平床傾斜偏差 X成分、推定第 2足平床傾斜偏差 Y成分についても同様の処理を行う。

先ず、目標歩容の第 1足平傾斜角を求め、これから想定第 1足平床傾斜傾斜を減じたものの X成分を、想定第 1足平干渉角 X成分 0 1 i n t cmd xとする。次に、次式により前述の推定第 1足平干渉角の X成分 S 1 i n t xを求める。

0 lintx = θ lx' + Θ errx- Θ f lestmx + Θ lintcmdx

■ . .式 28

ここで、 0 f 1 e s tmxは、第 1の実施の形態でも述べたように、推定第 1 足平床傾斜偏差 X成分である。また、 01 χ' は、床形状推定入り第 1足平 X補償角であり、第 4 2図に示すように、第 1足平 X補償角 0 1 Xと推定第 1足平床傾斜偏差 X成分 0 f 1 e s tmxの和である。

床形状推定入り第 1足平 X補償角 Θ 1 x' および推定第 1足平床傾斜偏差 X成分 0 f 1 e s t mxは、それから今回値を求めるものであるから、この処理では記憶しておいた直前の値を用いる。

尚、第 1足平が床にベたに接地する想定時刻よりも前から第 1足平床傾斜偏差を推定したい場合において、より精度を高めるために、想定第 1足平干渉角 X成分 Θ 1 i n t cmd xを加算することが望ましいが、想定第 1足平干渉角 X成分 0 1 i n t cmd xの加算および想定第 1足平干渉角 X成分 0 l i n t cmd x を算出する構成（第 4 5図において点線で囲われる構成）は必ずしも必須ではない。

次に、前記の足平機構コンプライアンスモデル 1 34 bを用いて、推定第 1足平干渉角 X成分 0 l i n t Xによって発生すると予想される前記 Ml e s tmの X成分 M 1 e s tmxを求める。

次に、検出された実第 1足平床反力モ一メント M 1 a c t Xと前記 M l e s t mxとの差を求めることにより、第 1足平床反カモ一メント推定偏差 X成分 M 1 e r r x 得る。

もし、推定第 1足平床傾斜偏差 X成分 0 f 1 e s t mxが実第 1足平床傾斜偏差に一致しているならば、第 1足平床反力モーメント推定偏差 X成分 M l e r r xは 0になる。逆に、もし第 1足平床反力モーメント推定偏差 X成分 M l e r r xが 0ならば、推定第 1足平床傾斜偏差 X成分 S f 1 e s "1111 は実第 1足平床傾斜偏差に一致する。

次に、第 1足平床傾斜偏差推定中断器において第 1足平床傾斜偏差の推定を実行（続行）すべきか中断すべきかを判断する。

第 1足平床傾斜偏差推定中断器は、第 1足平床傾斜偏差の推定を実行すべきと判断したならば、第 1足平床傾斜偏差推定中断器への入力である第 1足平床反力モーメント推定偏差 X成分 M l e r r xをそのまま出力し、中断すべきと判断したならば、 0を出力する。尚、足平床傾斜偏差推定中断器 1 4 6 aに関する詳細は後述する。

次いで、第 1足平床傾斜偏差推定中断器の出力を積分定数が K 1 e s t m x ' である積分器で積分し、推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xを得る ο

以上が、第 2の実施の形態における第 1足平床傾斜偏差 X成分推定部の処理でめる。

さらに、推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xは、第 4 2図に示すように、第 1足平 X補償角 0 1 Xに加算され、床形状推定入り第 1足平 X補償角 θ 1 χ ' として修正目標足平位置姿勢算出部 1 1 4 gへ入力される。

このような処理を毎制御周期に繰り返し実行することにより、第 1足平床反力モーメント推定偏差 X成分 M l e r r xが 0に収束するように、積分器出力である推定第 1足平床傾斜偏差 X成分 0 f 1 e s t m xが変化する。

ところで前述したように、第 1足平床反カモ一メント推定偏差 X成分 M 1 e r r xが 0になると、推定第 1足平床傾斜偏差 X成分 0 f 1 e s 1；111 が実第 1足平床傾斜偏差 X成分に一致する。従って、しばらく経つと、推定第 1足平床傾斜偏差 X成分 Θ f 1 e s t m xは、実第 1足平床傾斜偏差 X成分に一致する。即ち、この処理によって、第 1足平床傾斜偏差 X成分が推定される。

尚、足平機構コンプライアンスモデル 1 3 4 bが線形であるならば、その処理系の実第 1足平床傾斜偏差 X成分から推定第 1足平床傾斜偏差 X成分までの閉ル —プ伝達関数は、 1次遅れになる。

その時定数 Tは、足平機構コンプライアンスモデル 1 3 4 bにおける入力（推定第 1足平干渉角 X成分 0 1 i n t x ) に対する出力（前記モーメント M 1 e s t m x ) の比（絶対値）を C 1 とすると、次式で求められる。

T = 1 / ( CI * Klestmx' )

. . .式 2 9

第 1の実施の形態の積分ゲイン K 1 e s t m xに対し、第 2の実施の形態の積分ゲイン K l e s t m x ' を

Klestmx' = Klestmx I CI

, . '式 3 0

とすることにより、第 1の実施の形態とほぼ同一の特性を持つ第 1足平床傾斜偏差推定器が得られる。

尚、目標足平の向きが目標上体の向きと異なる場合には、上体傾斜を各足平座標系で表わせばよい。

ここで、第 1の実施の形態と第 2の実施の形態を比較する。

第 1の実施の形態では床形状推定器 1 3 0への入力として、最終的に修正された目標足平位置姿勢を用いていたが、第 2の実施の形態では、それを決定するために中間的に生成される変数を用いていると言える。

第 2の実施の形態は、簡単に言えば、第 1の実施の形態のコンプライアンスモデルを、両脚干渉角に関するコンプライアンスモデルである両脚間機構コンプラィアンスモデル 1 3 4 aと、足平干渉角に関するコンプライアンスモデルである足平機構コンプライアンスモデル 1 3 4 bに近似してから、ブロック線図を等価変換したものになっている。

第 2の実施の形態は機構コンプライアンスモデルが簡素化されているので、第 1の実施の形態に比較すると、やや推定精度が低下するが、機構コンプライアンスの演算が簡単であるので、制御ュニット 2 6の負荷を軽減することができる。それ以外は、第 2の実施の形態は、第 1の実施の形態と異ならない。作用効果も同様である。床形状の推定のみでも良いことも、第 1の実施の形態と同様である尚、上記において、両脚支持期になる想定時刻（即ち、遊脚の着地想定時刻）よりも前から両脚間床傾斜偏差の推定を開始することがない場合には、式 2 5から想定両脚干渉角 S d b i n t c m d vの項を削除した次式を用いても良い。なぜなら、目標歩容の両脚支持期には、想定両脚干渉角 S db i n t cmdvは 0 であるからである。

Θ dbintv= Θ dbv' + Θ errv- Θ fdbestmv

• ■ ·式 3 1

目標歩容の第 1足平の Y軸に沿ったエツジ（かかとエツジまたはつまさきエツジ）が接地する時刻よりも前から第 1足平床傾斜偏差 X成分の推定を開始することがない場合には、式 28から想定第 1足平干渉角 X成分 01 i n t cmdxの項を削除した次式を用いても良い。なぜなら、目標歩容の第 1足平の Y軸に沿つたエッジが接地しているときには、想定第 1足平干渉角 X成分 01 i n t cmd xは 0であるからである。

Θ lintx = θ lx' + Θ errx- 0 f lestmx

. . ·式 32

目標歩容の第 1足平の X軸に沿ったエッジ（即ち、内エッジまたは外エッジ）が接地する時刻よりも前から第 1足平床傾斜偏差 Y成分の推定を開始することがない場合には、式 28の Xを yに置き換えた式の代わりに、その式から想定第 1 足平干渉角 Y成分 Θ 1 i n t cmdyの項を削除した次式を用いても良い。なぜなら、目標歩容の第 1足平の Y軸に沿ったエッジ（即ち、内エッジまたは外エツジ）が接地しているときには、想定第 1足平干渉角 Y成分 0 l i n t cmdyは 0であるからである。

Θ linty = Θ ly' + Θ erry- Θ f lestmy

. . ·式 33

さらに、前記した式 1 2で求められるモーメント Mf 1 ί 2 a c t vの代わりに、式 1 2 aで求められるモ一メント M f 1 f 2 a c t vを用いる場合には、前記 Mdb e s tmvの代わりに、推定両脚干渉角を打ち消す機構変形によって発生するはずの目標全床反力中心点まわりの全床反力モーメントを用いる。

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

次いで、第 2の実施の形態に係る第 1足平床傾斜偏差 Y成分推定中断器の処理を説明する。第 2の実施の形態に係る第 1足平床傾斜偏差 Y成分推定中断器の処理は第 1の実施の形態のそれとほとんど同様であり、具体的には、第 3 7図の 0 f 1 e r r d i f f yを第 1足平床反カモ一メント推定偏差 Y成分 M 1 e r r yに置き換えれぱ良い。

また、両脚間床傾斜偏差推定中断器 1 4 0 aの処理も同様であって、第 4 0図フロー ·チヤ—トに示す処理において ø f d b e r r d i f f vを両脚モ一メント推定偏差 M d b e r rに置き換えれば良い。

ここで、両脚間床傾斜偏差推定中断器 1 4 0 aの役割を説明すると、両脚間床傾斜偏差推定中断器 1 4 0 aは、第 1足平床傾斜偏差推定中断器と同様に、両脚間床傾斜偏差の推定が正常に実行できるか否かを判断し、正常に実行できないと判断したときには推定を中断する。

より具体的には、推定両脚間床傾斜偏差の精度が低下しそうな状況、および推定両脚間床傾斜偏差が発散しそうな状況では、推定を中断するために 0を出力し、それ以外の状況では、推定を継続するために両脚間床傾斜偏差推定中断器への入力をそのまま出力する。

推定両脚間床傾斜偏差は、各足平と床との間の接触圧力が十分にあるときには発散しないが、ある足平が浮きかかっているにもかかわらず、両脚補償角が、その足平が浮く方向に変化しつつある状況で発生する。その発生メカニズムは推定第 1足平床傾斜偏差 Y成分が発散する場合とほぼ同一なので、これ以上の説明は省略する。

上記の如く、第 2の実施の形態においても、床形状推定値、具体的には各足平の接地面傾斜と両脚接地面間の高低差を同時に、精度良く推定することができる ο

さらに、床形状が想定していた形状と異なっていても、望み通りの床形状を発生されることができると共に、床形状のずれに起因する床反力の定常偏差を解消することができる。

また、床形状推定精度が低下しそうな状況あるいは推定値が発散しそうな状況では床形状推定を中断するように構成したので、前記した作用効果に加え、推定値が不適正なものとなることがない。尚、第 1および第 2の実施の形態において、足平 2 2 R (L) が浮きそうな状態の判断を、 6軸力センサ 34によって検出された実第 1足平床反力から前記実床反力中心点位置 Q 1 a c ΐまたは前記 Q 1 a c ΐ ' を算出し、それに基づいて足平 22R (L) がどちらの方向に浮きやすいか（かかとが浮きやすいか、つまさきが浮きやすいか、足平内エッジが浮きやすいか、または足平外エッジが浮きやすいか）を判定した。

し力、し、それに限られるものではなく、第 4 6図に示す如く、足底弾性体 4 0 と足平本体 22 aの間に分布圧センサ 20 0を配置し、その出力信号に基づいて足平 22R (L) がどちらの方向に浮きやすいかを判定しても良い。尚、 6軸力センサ 34よりも分布圧センサ 2 0 0の方が実床反力による足平の変形をより細緻に推定することができるので、床形状推定の精度を向上させることができる。さらには、第 4 7図に示す如く、足平 22R (L) に接触センサ 2 0 2を複数個配置（例えば足平の四隅に配置）し、その出力信号に基づいて足平 2 2R (L ) がどちらの方向に浮きやすいかを判定しても良い。尚、判定のアルゴリズムとして、接地していない接触センサが増々浮く方向への推定を中断する、あるいは、全ての接触センサが浮いているときも推定を中断することなどが考えられる。第 1、第 2の実施の形態では上記の如く、少なくとも上体（基体） 24と、前記上体 24に第 1の関節（股（腰部）関節 1 0、 1 2、 1 4 R, L) を介して連結されると共に、その先端に第 2の関節（足（足首）関節 1 8、 2 O R, L) を介して連結される足部（足平） 22R， Lを備えた複数本の脚部（脚部リンク） 2を備えると共に、前記足部 2 2R, Lに接地する床面から作用する床反力に応じて変形するコンプライアンス機構 4 2を備え、少なくとも前記上体 24に対する前記足部 2 2 R, Lの実位置姿勢を検出し、前記足部 2 2 R， Lの目標位置姿勢に追従させるように前記関節（ 1 0、 1 2R, しなど）を駆動する関節駆動手段（電動モ—夕）を備えた脚式移動ロボットの床形状推定装置において、前記上体 24の鉛直軸方向に対する傾斜を検出する傾斜検出手段（傾斜センサ 3 6) 、前記足部 22 R, Lの実位置姿勢および/または前記足部 2 2 R, Lの目標位置姿勢と前記検出された傾斜に基づき、前記足部 22R， Lが接地する床面を含む歩行環境における前記足部 22 R, Lの位置姿勢（機構変形補償入り修正目標足平位置姿勢）を算出する対歩行環境足部位置姿勢算出手段（制御ュニット 2 6、複合コンプライアンス動作決定部 1 1 4、機構変形補償入り修正目標足平位置姿勢算出部 1 1 4 h、両脚間床傾斜偏差推定部 1 3 8内の上体傾斜偏差の V方向成分 0 e r r vの加算器、足平床傾斜偏差推定部 1 4 4内の上体傾斜偏差の X成分 0 e r r xの加算器）、前記足部床面から作用する床反力による前記コンプライアンス機構 4 2の変形が加えられた前記歩行環境における足部の変形位置姿勢（機構変形後の推定各足平位置姿勢）を、前記コンプライアンス機構 4 2をモデル化して得た機構コンプライアンスモデル 1 3 4を用いて算出する変形足部位置姿勢算出手段（制御ュニット 2 6、複合コンプライアンスモデル 1 3 4、床形状推定器 1 3 0 (特に両脚間床傾斜偏差推定部 1 3 8内の θ ί d b v ' を算出するまでの部分、足平床傾斜偏差推定部 1 4 4内の 0 f 1 e r r x ' を算出するまでの部分））、および前記算出された足部の変形位置姿勢に基づき、前記足部 2 2 R , Lが接地するそれぞれの床面の形状（推定第 n足平床傾斜（偏差） 0 f n e s t m) およびそれらの相対位置関係（推定両脚間床傾斜（偏差）あるいは推定両脚間床高低差（偏差） 0 f d b e s t m v ) を示す床形状推定値を算出する床形状推定値算出手段（制御ュニット 2 6、複合コンプライアンス動作決定部 1 1 4 、床形状推定器 1 3 0、両脚間床傾斜偏差推定部 1 3 8 (特に 0 f d b e r r V 算出後の部分）、足平床傾斜偏差推定部 1 4 4 (特に 0 f 1 e r r x算出後の部分））、を備える如く構成した。

また、前記モデルが、それぞれの足部（各足平） 2 2 R , Lに作用する床反力と、それによるそれぞれの足部の位置姿勢の変化を記述する第 1のモデル（足平機構コンプライアンスモデル 1 3 4 b ) と、目標全床反力中心点に作用する床反力と、それによるそれぞれの足部間の相対位置の変化を記述する第 2のモデル（両脚間機構コンプライアンスモデル 1 3 4 a ) からなる如く構成した。

また、前記床形状推定値算出手段は、高周波成分を除去するフィルタ（両脚間床傾斜偏差推定部 1 3 8内の 0 f d b e r r vから Θ f d b e s t m vを算出する部分と、足平床傾斜偏差推定部 1 4 4内の Θ f 1 e r rから Θ f 1 e s t mとを算出する部分）を備え如く構成した。

さらに、前記算出された床形状推定値に基づいて前記足部 2 2 R， Lの目標位置姿勢を修正する修正手段（制御ュニット 2 6、加算器 1 3 2 ( 1 3 2 a, 1 3 2 b) 、修正目標足平位置姿勢算出部 1 1 4 g) を備える如く構成した。

さらに、前記ロボット 1の少なくとも前記足部 22 R， Lの目標位置姿勢を含む運動パターン（目標上体位置姿勢、目標足平位置姿勢）と、前記ロボット 1に作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（二目標 ZMP) ) を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（制御ユニット 2 6、歩容生成器 1 0 0、 3 1 0から322) 、前記生成された歩容の全床反力を前記足部 22 R， Lのそれぞれに分配したときの前記足部 2 2 R， L 上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（制御ュニット 2 6、目標床反力分配器 1 0 2 、 S 24, S 2 6) 、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（制御ュニット 2 6、 6軸力センサ 3 4、実各足平床反カ検出器 1 0 8、 S 32) 、および前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第 n足平床反力モーメント Ma c t x, y, z) を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角 0 d b v、第 n足平補償角 0 n x, y) を決定する足部回転量決定手段（制御ュニット 2 6、複合コンプライアンス動作決定部 1 1 4、両脚補償角決定部 1 1 4 b、第 n足平補償角決定部 1 1 4 c、 S 3 2, S 3 4, S 1 0 0から S 1 0 8) 、を備えると共に、前記修正手段は、前記算出された床形状推定値に基づいて前記足部回転量を修正（床形状推定入り両脚補償角 0 d b v' , 床形状推定入り第 n足平補償角 Θ η χ' ， y' ) する足部回転量修正手段（制御ュニット 2 6、加算器 1 32 ( 1 32 a， 1 3 2 b) ) 、および前記修正された足部回転量に基づいて前記足部の位置姿勢が回転するように前記目標位置姿勢を修正する足部位置姿勢修正手段（制御ュニット 2 6、修正目標足平位置姿勢算出部 1 1 4 g、 S 3 8， S 4 0) 、を備える如く構成した。さらに、前記ロボット 1の少なくとも前記足部 22 R, Lの目標位置姿勢を含む運動パターン（目標上体位置姿勢、目標足平位置姿勢）と、前記ロボット 1に作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（目標 ZMP) ) を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（制御ユニット 26、歩容生成器 1 00、 ≤ 1 0から322) 、前記生成された歩容の全床反力を前記足部 22 R, Lのそれぞれに分配したときの前記足部 22 R， L 上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（制御ュニット 26、目標床反力分配器 1 02 、 S 24, S 26) 、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（制御ュニット 26、 6軸力センサ 34、実各足平床反カ検出器 1 08、 S 32) 、および少なくとも前記検出された実床反力に基づいて前記足部 22R， Lを回転させる回転量（両脚補償角 0db v、第 n足平補償角 0 n X, y) を決定する足部回転量決定手段（制御ュニット 26、複合コンプライアンス動作決定部 1 1 4、両脚補償角決定部 1 1 4 b、第 n足平補償角決定部 1 1 4 c， d、 S 32， S 34, S 1 00から S 1 08) 、を備えると共に、前記修正手段は、前記算出された床形状推定値に基づいて前記足部回転量を修正（床形状推定入り両脚補償角 S dbv' 、床形状推定入り第 n足平補償角 0ηχ' ， y' ) する足部回転量修正手段（制御ュニット 26、加算器 1 32 ( 1 32 a, 1 32b) ) . および前記修正された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点（目標各足平床反力中心点）あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正する足部位置姿勢修正手段（制御ュニット 26、修正目標足平位置姿勢算出部 1 1 4 g、 S 38, S 40) 、を備える如く構成した。

また、前記足部位置姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正する如く構成した。

さらに、前記ロボットに実際に作用する全床反力モーメント（より正確にはモ一メント成分 PQ 1 *F 1 a c t +PQ2*F 2 a c t+Ml a c t +M2 a c t) 、または前記ロボットに実際に作用する全床反力モーメント（PQ 1 *F 1 a c t +PQ 2 *F 2 a c t +Ml a c t+M2 a c t) から前記足部に作用する床反力モーメント（Ml a c t+M2 a c t) を減算して得たモ一メント（M f 1 f 2 a c t=PQ l *F l a c t+PQ2 *F 2 a c t) のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部 22R, Lを移動させる移動量（Θ d b v) を決定する足部移動量決定手段（制御ュニット 2 6、補償全床反力モーメント分配器 1 1 4 a、両脚補償角決定部 1 1 4 b、 S 1 0 0から S 1 0 6) 、を備え、前記足部位置姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置姿勢を修正する如く構成した。

また、前記足部位置姿勢修正手段は、前記ロボットの姿勢偏差に基づいて前記目標位置姿勢をさらに修正する如く構成した。

また、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメント（補償全床反力 Mdmd) を求め、前記足部回転量決定手段および/または前記足部移動量決定手段は、少なくとも前記検出された実床反力（実各足平床反力）と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部の回転量および Zまたは移動量を決定する（S 34, S 1 0 0から S 1 0 8) 如く構成した。

また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（0 e r r X, y) に基づいて求める（S 2 8) 如く構成した。

また、前記足部回転量決定手段および Zまたは前記足部移動量決定手段は、前記姿勢安定化補償全床反力モ一メントが前記複数本の脚部のそれぞれに分配されるように、前記足部の回転量および/または移動量を決定する（S 3 4， S 1 0 0から S 1 0 8) 如く構成した。

さらに、前記ロボット 1の少なくとも前記足部 2 2 R, Lの目標位置姿勢を含む運動パ夕—ン（目標上体位置姿勢、目標足平位置姿勢）と、前記ロボット 1に作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（二目標 ZMP) ) を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（制御ュニット 2 6、歩容生成器 1 0 0、 S 1 0から S 22) 、前記ロボッ卜の姿勢安定化のための補償全床反力（補償全床反力 Mdmd) を算出する姿勢安定化補償全床反力算出手段（制御ュニット 2 6、姿勢安定化制御演算部 1 04、 S 28， S 30) 、前記足部に作用する実床反力（実各足平床反力）を検出する足部実床反力検出手段（6軸力センサ 3 4、制御ュニット 2 6、実各足平床反力検出器 1 0 8、 S 3 2) 、および前記目標歩容の全床反力と前記補償全床反力を分配する床反力分配手段（制御ュニット 2 6、目標床反力分配器 1 0 2、 S 3 4， S 1 0 0から S 1 0 4) 、を備えると共に、前記修正手段は、前記分配された目標歩容の床反力と補償床反力と前記検出された足部実床反力および前記算出された床形状推定値に基づいて前記目標歩容の足部の位置姿勢を修正（制御ュニット 2 6、補償角決定部 1 1 4 b, 1 1 4 c. 加算器 1 3 2 ( 1 3 2 a , 1 3 2 b ) 、修正足平位置姿勢算出部 1 1 4 g、機構変形補償入り修正目標足平位置姿勢算出部 1 1 4 h、機構変形量算出部 1 1 4 η) する如く構成した。

また、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの傾き偏差（ Θ e r τ X, y) に基づいて求める（S 2 8) 如く構成した。

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

また、' この発明を 2足歩行ロボットに関して説明してきたが、 2足歩行ロボットに限らず、多脚ロボットにも応用することができる。産業上の利用可能性

この発明によれば、従来技術では困難であった各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができる脚式移動ロボットを提供することが可能となる。さらに、構成としても簡易であ o

また、前記した効果に加え、複合コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれによる床反力の誤差に起因する定常偏差を解消することができる脚式移動ロボットを提供することが可能となる。

Claims

請求の範囲

1 . 少なくとも上体と、前記上体に第 1の関節を介して連結されると共に、その先端に第 2の関節を介して連結される足部を備えた複数本の脚部を備えると共に、前記足部に接地する床面から作用する床反力に応じて変形するコンプライアンス機構を備え、少なくとも前記上体に対する前記足部の実位置姿勢を検出し、前記足部の目標位置姿勢に追従させるように前記関節を駆動する関節駆動手段を備えた脚式移動ロボットの床形状推定装置において、

a . 前記上体の鉛直軸方向に対する傾斜を検出する傾斜検出手段、

b . 前記足部の実位置姿勢および Zまたは前記足部の目標位置姿勢と前記検出された傾斜に基づき、前記足部が接地する床面を含む歩行環境における前記足部の位置姿勢を算出する対歩行環境足部位置姿勢算出手段、

c 前記床面から作用する床反力による前記コンプライアンス機構の変形が加えられた、前記歩行環境における足部の変形位置姿勢を、前記コンプライアンス機構をモデル化して得た機構コンプライアンスモデルを用いて算出する変形足部位置姿勢算出手段、

および

d . 前記算出された足部の変形位置姿勢に基づき、前記足部が接地するそれぞれの床面の形状およびそれらの相対位置関係を示す床形状推定値を算出する床形状推定値算出手段、

を備えることを特徴とする脚式移動ロボットの床形状推定装置。

2 . 前記モデルが、それぞれの足部に作用する床反力と、それによるそれぞれの足部の位置姿勢の変化を記述する第 1のモデルと、目標全床反力中心点に作用する床反力と、それによるそれぞれの足部間の相対位置の変化を記述する第 2のモデルからなることを特徴とする請求項 1項記載の脚式移動口ボットの床形状推定

3 . 前記床形状推定値算出手段は、高周波成分を除去するフィルタを備えることを特徴とする請求項 1項または 2項記載の脚式移動ロボットの床形状推定装置。

4 . さらに、

e . 前記算出された床形状推定値に基づいて前記足部の目標位置姿勢を修正する修正手段

を備えることを特徴とする請求項 1項から 3項のいずれかに記載の脚式移動ロボットの床形状推定装置。

5 . さらに、

f . 前記ロボットの少なくとも前記足部の目標位置姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、

g . 前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、

h . 前記足部に作用する実床反力を検出する実床反力検出手段、

および

i . 前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモ一メントを算出し、少なくとも前記算出されたモ一メントに基づいて前記足部を回転させる回転量を決定する足部回転量決定手段、

を備えると共に、前記修正手段は、

j . 前記算出された床形状推定値に基づいて前記足部回転量を修正する足部回転量修正手段、

および

k . 前記修正された足部回転量に基づいて前記足部の位置姿勢が回転するように前記目標位置姿勢を修正する足部位置姿勢修正手段、

を備えることを特徴とする請求項 4項記載の脚式移動ロボットの床形状推定装置

6 . さらに、

1 . 前記ロボットの少なくとも前記足部の目標位置姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、

m. 前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、

n . 前記足部に作用する実床反力を検出する実床反力検出手段、

および

0 . 少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量を決定する足部回転量決定手段、

を備えると共に、前記修正手段は、

P . 前記算出された床形状推定値に基づいて前記足部回転量を修正する足部回転量修正手段、

および

q . 前記修正された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正する足部位置姿勢修正手段、

7 . 前記足部位置姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置姿勢を修正することを特徴とする請求項 5項記載の脚式移動ロボットの床形状推定装置。

8 . さらに、

r . 前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモ一メントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量を決定する足部移動量決定手段、

を備え、前記足部位置姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置姿勢を修正することを特徴とする請求項 5項から 7項のいずれかに記載の脚式移動口ボットの床形状推定装置。

9 . 前記足部位置姿勢修正手段は、前記ロボットの姿勢偏差に基づいて前記目標位置姿勢をさらに修正することを特徴とする請求項 5項から 8項のいずれかに記載の脚式移動ロボットの床形状推定装置。

1 0 . 前記全床反力の目標パターンに付加する姿勢安定化補償全床反カモ一メントを求め、前記足部回転量決定手段および Zまたは前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反力モ一メントに基づいて前記足部の回転量および Zまたは移動量を決定することを特徴とする請求項 5項から 9項のいずれかに記載の脚式移動 αボットの床形状推定装置

1 1 . 前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求めることを特徴とする請求項 1 0項記載の脚式移動ロボットの床形状推定装置。

1 2 . 前記足部回転量決定手段および/または前記足部移動量決定手段は、前記姿勢安定化補償全床反力モーメントが前記複数本の脚部のそれぞれに分配されるように、前記足部の回転量および Ζまたは移動量を決定することを特徴とする請求項 1 0項または 1 1項に記載の脚式移動ロボッ卜の床形状推定装置。

1 3 . さらに、

s . 前記ロボッ卜の少なくとも前記足部の目標位置姿勢を含む運動パターンと、前記ロボッ卜に作用する全床反力の目標軌跡パターンからなる前記ロボットの歩容を生成する歩容生成手段、

t . 前記ロボットの姿勢安定化のための補償全床反力を算出する姿勢安定化補償全床反力算出手段、

U . 前記足部に作用する実床反力を検出する足部実床反力検出手段、

および

V . 前記目標歩容の全床反力と前記補償全床反力を分配する床反力分配手段、を備えると共に、前記修正手段は、前記分配された目標歩容の床反力と補償床反力と前記検出された足部実床反力および前記算出された床形状推定値に基づいて前記目標足部位置姿勢を修正することを特徴とする請求項 4項記載の脚式移動口ボットの床形状推定装置。

1 4 . 前記姿勢安定化補償全床反力を、少なくとも前記ロボットの傾き偏差に基づいて求めることを特徴とする請求項 1 3項記載の脚式移動ロボットの床形状推