JP3726009B2 - 脚式移動ロボットの床形状推定装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
この発明は脚式移動ロボットの床形状推定装置、より具体的には脚式移動ロボットが接地する床の傾斜などの形状を推定すると共に、推定精度が低下する恐れがある状況では床形状の推定を中断する装置に関し、さらには推定した床形状に基づいて床反力を制御することを可能とする装置に関する。
【０００２】
【従来の技術】
床形状を推定する技術としては、本出願人が先に特開平６−３１６５８号公報で提案した技術、および論文「路面形状に偏差のある環境に対する適応能力を持つ２足歩行ロボットの開発」日本ロボット学会誌、第１４巻第４号、１９９６年５月号）記載の技術が知られている。
【０００３】
以下に後者（ロボット学会論文記載技術）の概要を説明し、その問題点を指摘する。
【０００４】
この従来技術では、足平部が、足平上面部、クッション部、足底の上下３層構造となっていて、足平上面部と足底の間の相対角度と間隔を検出するためにポテンショメータを足平の４隅に備えると共に、足平上面部の絶対傾斜を検出するための傾斜計を備える。また、足底の四隅にはスパイクを備える。尚、この明細書において『足平』は足部を意味するものとして使用する。
【０００５】
この従来技術にあっては、遊脚の足平の着地時に、ポテンショメータと傾斜計の情報を基に、ならい動作によって足平を床面にならわせようとする。その結果、足底の四隅のスパイクが床に接地した状態になり、足底は床面に平行になる。このとき、傾斜計により得られる足平上面部の絶対位置姿勢と、ポテンショメータから得られる足平上面部と足底の間の相対角度と間隔を基に、幾何学的演算により床の高さと傾きが検出される。
【０００６】
【発明が解決しようとする課題】
しかしながら、この従来技術には以下のような問題点があった。即ち、ならい動作によって足平が床面にならうまで、床反力を操作する姿勢安定化制御を行うことができなかった。またグリップを増すために足底にゴム等の柔らかい材料を貼ると、推定誤差が増えてしまう不都合があった。
【０００７】
さらに、足平がべたに着地しない（即ち、足底が着地の瞬間に床に面接触しない）歩行では、両脚支持期の間に床の高さを正確に推定することができなかった。また、直立時など両足平が床にべたに接地している状態では、各足平の接地面傾斜と両足平接地面間の高低差を同時には推定することができなかった。また、接地の瞬間しか推定することができなかったので、着地した後に床が変形したときに変形後の床形状を推定することができなかった。
【０００８】
他方、前記した従来技術の前者、即ち、特開平６−３１６５８号公報記載技術の場合、床反力を操作する姿勢安定化制御を行いながら、床の傾斜を推定することは可能であったが、両脚支持期では両足平の接地面と交差または接する平面の傾きだけを推定すると共に、片脚支持期では支持脚の接地面の傾斜だけを推定するに止まっていた。即ち、これらの傾きを同時に推定することはできなかった。
【０００９】
【発明が解決しようとする課題】
従って、この発明の目的は、従来技術では困難であった床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定すると共に、推定精度が低下する恐れがあるときは、床形状の推定を中断するようにした脚式移動ロボットの床形状推定装置を提供することにある。
【００１０】
【課題を解決するための手段】
上記の目的を達成するために、請求項１項にあっては、少なくとも上体と、前記上体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部を備えると共に、前記足部に接触する床面から作用する床反力に応じて変形するコンプライアンス機構を備え、少なくとも前記上体に対する前記足部の実位置姿勢が、前記上体に対する前記足部の目標位置姿勢に追従するように前記関節を駆動する関節駆動手段を備えた脚式移動ロボットの床形状推定装置において、前記上体の鉛直軸方向に対する傾斜を検出する傾斜検出手段、前記ロボットの実関節変位、目標関節変位および前記足部の目標位置姿勢の少なくともひとつと前記検出された傾斜に基づき、前記コンプライアンス機構をモデル化して得た機構コンプライアンスモデルを用いて前記足部が接触するそれぞれの床面の形状および前記床面同士の相対位置関係を示す床形状推定値を決定する床形状推定値決定手段を備えると共に、前記床形状推定値決定手段は、前記床形状推定値の床面形状と実際の床面形状との差の絶対値が増加する可能性があるか否かを判定し、前記絶対値が増加する可能性があると判定した場合、前記床形状推定値の床面形状を保持または所定の値に変更する一方、前記床形状推定値の床面同士の相対位置関係と実際の床面同士の相対位置関係との差の絶対値が増加する可能性があるか否かを判定し、前記絶対値が増加する可能性があると判定した場合、前記床形状推定値の床面同士の相対位置関係を保持または所定の値に変更する推定中断手段、を備える如く構成した。
【００１１】
これにより、従来技術では困難であった床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができると共に、推定精度が低下する恐れがある状況では床形状推定を中断するので、推定値が不適正なものとなることがない。
【００１４】
請求項２項にあっては、前記推定中断手段は、少なくとも前記ロボットの歩行時期に基づいて前記絶対値が増加する可能性があるか否か判定する如く構成した
【００１５】
これにより、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【００１６】
請求項３項にあっては、前記推定中断手段は、少なくとも前記足部と床面との接触状況に基づいて前記絶対値が増加する可能性があるか否か判定する如く構成した。
【００１７】
これにより、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【００１８】
請求項４項にあっては、前記推定中断手段は、少なくとも前記足部に作用する前記床反力に基づいて前記絶対値が増加する可能性があるか否か判定する如く構成した。
【００１９】
これにより、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【００２０】
請求項５項にあっては、前記推定中断手段は、少なくとも前記足部に作用する前記床反力の中心点位置に基づいて前記絶対値が増加する可能性があるか否か判定する如く構成した。
【００２１】
これにより、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【００２２】
請求項６項にあっては、さらに、少なくとも前記算出された床形状推定値に基づいて前記足部の目標位置姿勢を修正する修正手段を備える如く構成した。
【００２３】
これにより、前記した効果に加え、複合コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれに起因する床反力の定常偏差を解消することができる。
【００２４】
【発明の実施の形態】
以下、添付図面を参照してこの発明の一つの実施の形態に係る脚式移動ロボットの床形状推定装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００２５】
図１は、この実施の形態に係る床形状推定装置を適用した脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。
【００２６】
図示の如く、２足歩行ロボット（以下「ロボット」という）１は左右それぞれの脚部（脚部リンク）２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。６個の関節は上から順に、股（腰部）の脚部回旋（回転）用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｘ軸まわり）の関節１２Ｒ，１２Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。
【００２７】
関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。
【００２８】
上記の構成により、脚部２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。
【００２９】
尚、この明細書で後述する上体２４の位置およびその速度は、上体２４の所定位置、具体的には上体２４の重心位置などの代表点の位置およびその移動速度を意味する。
【００３０】
図１に示す如く、足関節の下方には公知の６軸力センサ３４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。また、上体２４には傾斜センサ３６が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。
【００３１】
図２に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３８が装備されると共に、足底にはゴムなどからなる足底弾性体４０が貼られてコンプライアンス機構４２を構成する。ばね機構３８は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材（図示せず）と、足首関節１８Ｒ（Ｌ）および６軸力センサ３４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材（図示せず）とからなる。
【００３２】
図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けると、コンプライアンス機構４２においてばね機構３８と足底弾性体４０がたわみ、足平は図中に点線で表示された位置姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。
【００３３】
さらに、図１では図示を省略するが、ロボット１の適宜な位置にはジョイスティック４４が設けられ、外部から必要に応じて直進歩行しているロボット１を旋回させるなど歩容に対する要求を入力できるように構成される。
【００３４】
図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ３６などの出力はＡ／Ｄ変換器５０でデジタル値に変換され、その出力はバス５２を介してＲＡＭ５４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ５６を介してＲＡＭ５４内に入力される。
【００３５】
制御ユニット２６の内部にはＣＰＵからなる第１、第２の演算装置６０，６２が設けられており、第１の演算装置６０は後述の如く、ロボット１が接地する床（床面）の形状を推定すると共に、ＲＯＭ６４に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、ＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出してＤ／Ａ変換器６６とサーボアンプを介して各関節を駆動する電動モータに出力する。
【００３６】
この実施の形態に係る脚式移動ロボットの床形状推定装置は、概説すると、本出願人が先に提案した前記の特開平６−３１６５８号公報記載の技術の改良である。より具体的には、同様に本出願人が先に提案した前記の特開平１０−２７７９６９号公報記載技術を基礎とし、その上に特開平６−３１６５８号記載技術の手法を拡張しながら適応させたものである。
【００３７】
即ち、その特開平６−３１６５８号記載技術においては、少なくともＺＭＰまわりの全床反力モーメントの制御偏差を基に、両脚支持期では両脚間傾斜偏差を推定して両脚補償角を修正し、片脚支持期では各足平床傾斜偏差を推定して各足平補償角を修正していた（特開平６−３１６５８号公報記載技術では足首角を修正していたので、厳密には足平補償角を修正することとは異なるが、ここではほぼ同じと考える）。
【００３８】
尚、上記で、ＺＭＰ（Zero Moment Point)とは、運動パターン（軌道）によって発生する慣性力と重力の合力の床（面）上の作用点まわりのモーメントが、鉛直軸まわりの成分を除き、０である点を意味する。
【００３９】
それに対し、この実施の形態に係る脚式移動ロボットの床形状推定装置においては、少なくとも全床反力モーメントの制御偏差（あるいはそれから目標各足平床反力中心点まわりの各足平床反力モーメントの制御偏差を減じたもの）を基に床形状、より具体的には両脚間床傾斜偏差を推定する。さらには、必要に応じて推定値に基づいて両脚補償角を修正する。また、少なくとも目標各足平床反力中心点まわりの各足平床反力モーメントの制御偏差を基に床形状、より具体的には各足平床傾斜偏差を推定する。さらには、必要に応じて推定値に基づいて各足平補償角を修正するようにした。この結果、両脚支持期であっても各足平床傾斜偏差を精度良く推定することができ、各足平補償角を修正することができる。
【００４０】
さらに、両脚間床傾斜偏差および各足平床傾斜偏差の推定において、前記したコンプライアンス機構４２をモデル化した、床反力と脚部（脚部リンク）２の変形の関係を表わす機構コンプライアンスモデルを用いることにより、制御則に単なる積分を加えて定常制御偏差を低減する従来手法では得られない良好な制御特性が得られるようにした。即ち、制御則に単なる積分を加えると位相が遅れるが、この実施の形態に係る装置ではオブザーバを用いた場合と同様に、制御の遅れが生じることがない。
【００４１】
ところで、その先に提案した前記特開平１０−２７７９６９号公報記載技術は、予期しない凹凸形状の床面に遭遇して姿勢の安定を失ったとき、床反力を適切に制御して安定した姿勢を回復させることを意図していた。
【００４２】
即ち、平坦な床面を想定して生成された歩容に基づいて歩行制御されていたロボット１が予期しない凹凸を踏むと、過大な床反力が生じてロボット１は傾くが、そのような状況に対処すべく、本出願人は別の提案技術（特開平５−３０５５８６号公報記載技術）において前記したコンプライアンス機構４２を用いると共に、傾斜センサにより上体傾斜を検出して必要な復元モーメント要求量を求め、それに目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメント成分が一致するように足平軌道（パターン）を修正して各足平を上下させていた。
【００４３】
しかしながら、その制御は、長い距離でゆったりと変化する大域（広域）的な傾斜やうねりには有効であるが、局所的な傾きや段差には十分対応しきれなかった。
【００４４】
本出願人は、さらに、別の提案技術（特開平５−３０５５８４号公報記載技術）において、同様に前記したコンプライアンス機構４２を用いて予期しなかった床反力モーメントを打ち消す方向に足関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）を駆動する制御も提案していた。
【００４５】
従って、予期しない凹凸形状の床面に遭遇して姿勢の安定を失ったときには、上記した２種の提案技術を併用することも考えられるが、そうすると、制御が干渉する不都合があった。
【００４６】
そのような制御の干渉を回避すべく、先に提案した前記特開平１０−２７７９６９号公報記載技術では、局所的な凹凸や傾斜などを含む予期しない床形状の変化に起因して姿勢の安定を失ったとき、床反力を適切に制御するようにしている。
【００４７】
この実施の形態に係る脚式移動ロボットの床形状推定装置は、その先に提案した前記特開平１０−２７７９６９号公報記載技術を前提としているので、理解の便宜上、最初に、その先に提案した前記特開平１０−２７７９６９号公報記載技術を説明する。尚、以下、その先に提案した前記特開平１０−２７７９６９号公報記載技術を「複合コンプライアンス制御」と呼ぶ。
【００４８】
図４は、その複合コンプライアンス制御の装置の構成および動作を機能的に示すブロック図である。
【００４９】
同図の説明に入る前に、その複合コンプライアンス制御で使用した定義を説明する（尚、定義しない用語に関しては、本出願人が前記した技術とは別に提案した出願（特開平１０−８６０８１号）で使用した定義に従う）。
【００５０】
『歩容』は、ロボット工学における一般的な定義と異なり、目標運動パターンと床反力パターンを合わせたものを指称する意味で使用する。但し、床反力パターンとしては、例えば『ＺＭＰ軌跡だけ』というように、部分情報であっても良い。そのため、目標運動パターンだけを出力して床反力パターンに関する情報を出力しない装置に対して「歩容生成装置」と言う言葉を用いない。
【００５１】
各脚には、通し番号をつける。第ｎ脚に作用する床反力を第ｎ足平床反力という（ｎ：１または２。以下同じ）。全脚に作用する床反力を合成したものを全床反力という（ロボット工学では一般的には床反力と呼ばれるが、足平床反力と区別するためにここでは『全床反力』という）。
【００５２】
足平床反力は、作用点と、そこにかかる力と力のモーメントによって表現され、同一の足平床反力に対して、表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が足底上にある表記が存在する。この表現における作用点を、ここでは足平床反力中心点という（本出願人が別途提案した後述する特開平６−７９６５７号では『接地圧重心点』と称した）。
【００５３】
同様に、全床反力は、作用点と、そこにかかる力と力のモーメントによって表現され、同一の全床反力に対して表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が床面上にある表現が存在する。この表現における作用点を、ここでは全床反力中心点という。尚、この明細書で「床面」は、本出願人が先に提案した特開平５−３１８３４０号公報に記載される仮想的な床面であっても良い。
【００５４】
全床反力の目標値を目標全床反力という。目標全床反力は、通常、目標運動パターンに対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標ＺＭＰに一致する。
【００５５】
尚、前記したＺＭＰの目標値を目標ＺＭＰという。目標ＺＭＰは、合力の垂直方向力成分が０でない限り、一義的に求められる。以下の説明では、理解しやすくするために、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所が多い。
【００５６】
各足平床反力の目標値を目標各足平床反力という。但し、目標全床反力とは異なり、目標運動パターンが決まっていても目標各足平床反力は一義的には決定されない。実際のロボットに作用する全床反力を実全床反力という。実際のロボットに作用する各足平床反力を実各足平床反力という。
【００５７】
上記した定義を前提とし、以下、図４を参照して複合コンプライアンス制御装置の全体構成を概説する。
【００５８】
図示の如く、複合コンプライアンス制御装置は歩容生成器１００を備え、歩容生成器１００は目標歩容を生成し、出力する。目標歩容は、前述の定義の通り、目標運動パターンと目標床反力パターン、より具体的には目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点（目標ＺＭＰ）軌道および目標全床反力軌道（あるいはパターン）からなる。目標床反力パターンは、このように、目標全床反力中心点軌跡を含む（後述する機構変形補償を行わないならば、目標床反力パターンとしては目標全床反力中心点軌跡だけでも良い）。
【００５９】
歩容生成器１００が出力する目標全床反力は、目標運動パターンに対して動力学的に平衡する全床反力である。従って、目標全床反力中心点は、目標ＺＭＰに一致する。
【００６０】
図５にロボット１が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標ＺＭＰ軌道の床面上軌跡を図６に、タイム・チャートを図７に示す。この歩容の期間に床に接触したままの足平を「第１足平」、もう一方を「第２足平」と呼ぶ。
【００６１】
図４の説明に戻ると、この装置は目標床反力分配器１０２を備え、目標床反力分配器１０２は、目標全床反力中心点（目標ＺＭＰ）と目標足平位置姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器１００から歩容のパラメータ（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期・時刻（例えば、現在時刻が両脚支持期の初めから0.1secであるなど）などの情報も必要に応じて取り込む。
【００６２】
図５に示すような歩容に対して、目標床反力分配器１０２は、目標各足平床反力中心点が以下の条件を満足するように設定する。
条件１）目標各足平床反力中心点軌跡は連続である。
条件２）両脚支持期では、目標第１足平床反力中心点はかかとに、目標第２足平床反力中心点はつまさきに存在する。
条件３）このとき目標第１足平床反力中心点と目標第２足平床反力中心点を結ぶ線分上に、目標全床反力中心点が存在する。
条件４）片脚支持期では、目標第１足平床反力中心点は、目標全床反力中心点に一致する。
条件５）片脚支持期の間に、目標第２足平床反力中心点は、つまさきからかかとに移動する。
【００６３】
これら条件を満足する目標第１足平床反力中心点軌跡のタイム・チャートを図８に、目標第２足平床反力中心点軌跡のタイム・チャートを図９に示す。尚、この図では足首（関節１８Ｒ（Ｌ），２０Ｒ（Ｌ））から足平２２Ｒ（Ｌ）への垂直投影点、より具体的には足底への垂直投影点を原点とし、図１に示すように足平前方向をＸ軸の正の向き、足平左方向をＹ軸の正の向きにとる。
【００６４】
目標床反力分配器１０２は、さらに、付随的ではあるが、目標各足平床反力も決定して出力する。目標各足平床反力は、コンプライアンス機構４２のたわみ補償のために必要である。
【００６５】
次式を用いて上記のように設定された目標各足平床反力中心点に対応する目標各足平床反力を決定すれば、目標各足平床反力の合力は目標全床反力に一致しなければならないと言う条件を満足する。
【００６６】
目標第１足平床反力＝目標全床反力＊（目標第２足平床反力中心点と目標ＺＭＰの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離）
目標第２足平床反力＝目標全床反力＊（目標第１足平床反力中心点と目標ＺＭＰの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離）・・・式１
【００６７】
このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術（特開平６−７９６５７号）に記述されている。
【００６８】
図４の説明に戻ると、この装置は姿勢安定化制御演算部１０４を備え、姿勢安定化制御演算部１０４はセンサ情報に基づいてロボット１の状態を推定し、補償全床反力を算出する。即ち、実際にロボット１が歩行あるいは直立しているときなどには後述する変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたとしても、ロボット１の位置姿勢は必ずしも望ましい位置姿勢にならない。
【００６９】
ロボット１の姿勢を長期的に安定化させるためには、ロボットを望ましい位置姿勢に復元させるために必要な力とモーメントを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な力とモーメントを補償全床反力という。また、補償全床反力のモーメント成分を補償全床反力モーメントという。
【００７０】
尚、脚式移動ロボットの目標歩容が床反力以外の反力を環境から受けるように想定し、それを例えば、目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標ＺＭＰとするようにしても良い。
【００７１】
もし、ロボット１が完全剛体であって、変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、コンプライアンス機構４２のたわみによって生じるロボット全体の位置姿勢の摂動的な運動は、以下の６自由度に分解できる。
【００７２】
モード１）目標全床反力中心点（目標ＺＭＰ）を中心とした前後軸まわり回転（即ち、左右傾き）
モード２）目標全床反力中心点（目標ＺＭＰ）を中心とした左右軸まわり回転（即ち、前後傾き）
モード３）目標全床反力中心点（目標ＺＭＰ）を中心とした鉛直軸まわり回転（即ち、スピン）
モード４）前後平行移動揺れ
モード５）左右平行移動揺れ
モード６）上下平行移動揺れ
【００７３】
この内で、モード４とモード５は、コンプライアンス機構４２が前後左右方向の剪断力を受けてたわむことによって発生するものである。コンプライアンス機構４２の剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。
【００７４】
残り４自由度の内、モード３とモード６はここでの説明と直接の関連を有しないので、モード１とモード２に対する制御を説明する。モード１とモード２に対する制御は、これがないとほとんどの場合ロボットが転倒するので、重要度は極めて高い。
【００７５】
モード１を制御するための操作量は、補償全床反力の前後軸（Ｘ軸）まわりモーメント成分である。モード２を制御するための操作量は、補償全床反力の左右軸（Ｙ軸）まわりモーメント成分である。従って、補償全床反力の成分の内、前後軸方向モーメント成分と左右軸方向モーメント成分だけを求めれば良い。他の成分は、この実施の形態（および第２の実施の形態）では用いないので０で良い。
【００７６】
尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を「補償全床反力モーメントＭｄｍｄ」（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）と呼ぶ。図５および図６に示す如く、ロボットの前方向をＸ軸、左横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直下の床面上の点を原点とした座標系を「支持脚座標系」と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、ＭｄｍｄのＸ成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体傾斜−目標上体傾斜）θｅｒｒのＸ成分をθｅｒｒｘ，Ｙ成分をθｅｒｒｙ、これらの時間微分値を（ｄθｅｒｒｘ／ｄｔ），（ｄθｅｒｒｙ／ｄｔ）と記述する。
【００７７】
ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次式の制御則によって決定される。

ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、上体傾斜安定化制御ゲインである。
【００７８】
後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。
【００７９】
図４の説明に戻ると、この装置は実各足平床反力検出器１０８を備え、実各足平床反力検出器１０８は、６軸力センサ３４によって実各足平床反力（その合力が実全床反力）を検出する。さらに、関節のエンコーダによって検出される実変位（および／または変位指令）に基づき、上体２４に固定された座標系に対する各足平２２Ｒ（Ｌ）の相対位置姿勢を算出し、それによって６軸力センサ３４の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、支持脚座標系に変換する。
【００８０】
この装置はロボット幾何学モデル（逆キネマティクス演算部）１１０を備え、ロボット幾何学モデル１１０は、上体位置姿勢と足平位置姿勢を入力されると、それらを満足する各関節変位を算出する。この実施の形態におけるロボット１のような１脚あたりの関節自由度が６である場合には、各関節変位は一義的に求まる。
【００８１】
ここでは逆キネマティクスの解の式を直接的に求めておき、式に上体位置姿勢と足平位置姿勢を代入するだけで各関節変位を得るようにした。即ち、ロボット幾何学モデル１１０は、目標上体位置姿勢と複合コンプライアンス動作決定部で修正された修正目標足平位置姿勢軌道（機構変形補償入り修正目標足平位置姿勢軌道）を入力し、それらから１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出する。
【００８２】
この装置は変位コントローラ１１２（前記した第２の演算装置６２に同じ）を備え、変位コントローラ１１２は、ロボット幾何学モデル（逆キネマティクス演算部）１１０で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
【００８３】
この装置は前記した複合コンプライアンス動作決定部１１４を備え、複合コンプライアンス動作決定部１１４は以下の２つの要求を満足させようと、目標足平位置姿勢軌道を修正する。
【００８４】
要求１）ロボットの位置姿勢制御のために、実全床反力を姿勢安定化制御演算部１０４が出力する補償全床反力（モーメントＭｄｍｄ）と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメントＭｄｍｄに追従させる。
【００８５】
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【００８６】
尚、通常は実全床反力を補償全床反力と目標全床反力の合力に一致させながら目標各足平床反力中心点まわりの実各足平床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、要求１）と要求２）は完全に両立させることはできず、ある点で妥協しなくてはならない。
【００８７】
上記を前提として図１０フロー・チャート（構造化フロー・チャート）を参照してこの装置の動作を説明する。尚、図の左端に該当する処理を行う図４装置の構成要素を示す。
【００８８】
先ずＳ１０において装置を初期化し、Ｓ１２を経てＳ１４に進み、タイマ割り込みを待機する。タイマ割り込みは５０ｍｓごとになされ、即ち、制御周期は５０ｍｓである
【００８９】
続いてＳ１６に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときはＳ２２に進むと共に、肯定されるときはＳ１８に進んでタイマｔをイニシャライズし、Ｓ２０に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメータと床反力パラメータ（ＺＭＰ軌道パラメータ）から構成される。
【００９０】
続いてＳ２２に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置姿勢、目標各足平位置姿勢、および目標ＺＭＰ位置から構成される。尚、ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意味する。
【００９１】
続いてＳ２４に進んで目標各足平床反力中心点を求める。これは目標床反力分配器の説明で述べたように行う。具体的には、図８および図９に示すように、設定した目標各足平床反力中心点軌跡の現在時刻ｔにおける値を求めることで行う。
【００９２】
続いてＳ２６に進んで目標各足平床反力を求める。これは目標床反力分配器の説明で述べた式１を用いて目標各足平床反力を演算することで行う。
【００９３】
続いてＳ２８に進み、前記した傾斜センサ３６などの出力から上体２４の傾斜などロボット１の状態を検出する。
【００９４】
続いてＳ３０に進み、ロボット１の状態などから姿勢を安定化するための（目標全床反力中心点（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを求める。具体的には、前記した式２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを演算する。
【００９５】
続いてＳ３２に進んで実各足平床反力を検出する。これは前記の如く、６軸力センサ３４の出力から検出する。
【００９６】
続いてＳ３４に進み、両脚補償角θｄｂｖおよび各足平補償角θｎｘ（ｙ）を決定する。これは、前記した複合コンプライアンス動作決定部１１４が行う作業である。
【００９７】
その複合コンプライアンス動作決定部１１４の作業について説明する。説明の便宜のため、両脚支持期において図１１に示すように第１足平２２Ｒ（Ｌ）と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用している状況と仮定する。
【００９８】
ここでベクトルＦｎａｃｔは第ｎ足平床反力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎａｃｔの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。
【００９９】
この瞬間の目標全床反力は、図１２に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標ＺＭＰ）における目標全床反力モーメントベクトルＭｓｕｍｒｅｆは垂直である（定義により、目標ＺＭＰは目標全床反力モーメントの水平方向成分が０である点であるから）。
【０１００】
これを式１に従って目標各足平床反力に分配すると、図１３に示すようになる。同図において、ベクトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、Ｍｎａｃｔと同様である。
【０１０１】
説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。
【０１０２】
前述の姿勢安定化制御演算部１０４では、ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒｙに基づいて補償全床反力モーメントＭｄｍｄを算出する。この実施の形態では鉛直軸（Ｚ軸）まわりのスピンを制御しないので、補償全床反力モーメントＭｄｍｄの鉛直軸成分は０である。上体位置の揺れも制御しないので、補償全床反力の力成分（Ｆｄｍｄ）も０である。この状態に対応する補償全床反力モーメントＭｄｍｄを図１４に示す。
【０１０３】
姿勢を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと補償全床反力モーメントＭｄｍｄの和の水平成分に追従させれば良い。
【０１０４】
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向成分は０である。従って、前後左右の姿勢傾きを復元させるためには、目標ＺＭＰまわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分に追従させれば良い。
【０１０５】
複合コンプライアンス動作決定部１１４は、前記した以下の要求をできる限り満足するように足平の位置姿勢を修正する。
要求１）ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に追従させる。
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１０６】
但し、前述の通り、要求１）と要求２）は、完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１０７】
足平の位置姿勢の修正は次のように行う。
１）目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベクトルＶを求める。Ｖの大きさは１とする。Ｖを図１５に示す。
【０１０８】
２）目標第１足平床反力中心点Ｑ１の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移動する。移動した後の点をＱ１’とする。同様に、目標第２足平床反力中心点Ｑ２の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに回転角θｄｂｖだけ回転移動する。移動した後の点をＱ２’とする。
【０１０９】
この回転角θｄｂｖを両脚補償角という。始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ１’とする。同様に、始点がＱ２、終点がＱ２’のベクトルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ２’を示す。
【０１１０】
３）目標第１足平を、姿勢は変えずにベクトルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。同様に、目標第２足平を、姿勢は変えずにベクトルＱ２Ｑ２’だけ平行移動させる。移動後の目標各足平を図１６に太線で示す。
【０１１１】
４）次に、目標第１足平をＱ１’を中心に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同様に、目標第２足平をＱ２’を中心に前後方向軸（Ｘ軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわりに回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙをそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角という。回転後の目標各足平を図１７に太線で示す。
【０１１２】
以上の補償動作量が過大でなければ、接地圧力分布は変わっても、接地領域（足底面の圧力が正の領域）は変わらない。このような場合には、補償動作量に比例して各足平に装着されたコンプライアンス機構４２が変形し、変形量に応じた実各足平床反力が発生する。この結果、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。
【０１１３】
特性１）両脚補償角θｄｂｖだけを操作して目標各足平位置を移動させると、下がった足平の実足平床反力の力成分が増加し、上がった足平の実足平床反力の力成分が減少する。このとき、修正目標各足平床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。
【０１１４】
特性２）第ｎ足平Ｘ補償角だけを操作して目標第ｎ足平姿勢を回転させると、目標第ｎ足平床反力中心点に作用する実第ｎ足平床反力のモーメントのＸ成分だけが変化し、その他の床反力成分は少ししか変化しない。同様に、第ｎ足平Ｙ補償角だけを操作して目標第ｎ足平姿勢を回転させると、実第ｎ足平床反力のモーメントのＹ成分だけが変化し、その他の床反力成分は少ししか変化しない。
【０１１５】
特性３）両脚補償角θｄｂｖ、各足平Ｘ補償角および各足平Ｙ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
【０１１６】
特性１および特性２は、これらの操作に独立性があることを示し、特性３はこれらの操作に線形性があることを示していると言える。
【０１１７】
図１８は複合コンプライアンス動作決定部１１４の演算処理を示すブロック図であり、同図を参照してその処理を説明する。
【０１１８】
概説すると、補償全床反力モーメント分配器１１４ａにおいては補償全床反力モーメントＭｄｍｄの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部１１４ｂおよび第ｎ足平Ｘ（Ｙ）補償角決定部１１４ｃ，１１４ｄ，１１４ｅ，１１４ｆにおいて前述の補償角θｄｂｖおよびθｎｘ（ｙ）を決定する。
【０１１９】
次に、決定された各種補償角に基づいて修正目標足平位置姿勢算出部１１４ｇは、補償された足平位置姿勢（これを修正目標足平位置姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置姿勢算出部１１４ｈは、目標各足平床反力によって発生が予想されるコンプライアンス機構４２の変形量を求め、それらを打ち消すように修正目標足平位置姿勢をさらに修正する。
【０１２０】
以下詳説すると、補償全床反力モーメント分配器１１４ａは、補償全床反力モーメントＭｄｍｄを、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モーメントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配する。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角（足平上下量）θｄｂｖを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分が作るモーメントの目標値である。
【０１２１】
両脚補償モーメントＭｄｍｄｄｂのＶ方向まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクトルＶは複合コンプライアンス動作決定部１１４の説明で定義したベクトルである。Ｖに直交し、鉛直方向にも直交するベクトルをＵとすると、両脚補償モーメントＭｄｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定される。両脚補償角θｄｂｖを操作しても、床反力のＵ方向モーメント成分を発生することはできないからである。
【０１２２】
ここでは補償全床反力モーメントＭｄｍｄの鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成分Ｍｄｍｄｄｂｚも０に設定される。
【０１２３】
第１足平補償モーメントＭｄｍｄ１は、第１足平補償角θ１ｘ，θ１ｙを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントである。第１足平補償モーメントＭｄｍｄ１のＸ成分をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄ１ｙと記述する。第２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ２ｘ，θ２ｙを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントである。第２足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。
【０１２４】
分配は、例えば次のように行う。

【０１２５】
ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数である。ＶｘはベクトルＶのＸ成分の値、ＶｙはベクトルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚補償角を操作することによって発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。
【０１２６】
式３と式４の演算処理を行う補償全床反力モーメント分配器１１４ａのブロック図を図１９に示す。
【０１２７】
歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの設定例を図２０に示す。図２０のパターンは、以下の注意点を考慮して決定することが望ましい。
【０１２８】
注意点１）両脚補償角と各足平補償角が不連続的に変化すると、関節に過大なトルクが発生する。そこで、両脚補償角と各足平補償角を連続的に変化させるために、分配用重み変数は連続的に変化させる。
【０１２９】
注意点２）両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントが、なるべく補償全床反力モーメントＭｄｍｄに近い値になるように、分配用重み変数を決定する。
【０１３０】
この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。
【０１３１】
この状況では目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に一致させるため（即ち、前述の複合コンプライアンス動作決定部に対する要求１を満足するため）に、式５と式６の両方をなるべく満足するように重みを設定すべきである。
【０１３２】

【０１３３】
これに式３、式４を代入すると、式５は式７に、式６は式８に変換される。

【０１３４】
ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っても、式７と式８が恒等的に成立するためには、式９、式１０、および式１１を同時に満足すれば良い。

即ち、以上の状況では式９、式１０および式１１を同時に満足するように、重みを決定すれば良い。
【０１３５】
歩行時ではＭｄｍｄｄｂｖを目標にして両脚補償角θｄｂｖを操作して足平の位置を修正しても、実全床反力モーメントの発生量がＭｄｍｄｄｂｖに較べて不足する場合がある。例えば図２１のように両脚支持期の初期にロボットが後傾して第１足平が未だ着地していない状況では、θｄｂｖによって第１足平の位置を下げても、実床反力は変化しない。
【０１３６】
同様に、Ｍｄｍｄ２を目標にして第２足平補償角θ２を操作して第２足平の角度を修正しても、実床反力モーメントの増加量がＭｄｍｄ２に較べて不足する場合がある。例えば、図２２のように両脚支持期の後半にロボットが後傾している状況では、θ２によって第２足平のかかとを下げても実床反力は変化しない。
【０１３７】
従って、式５、式６を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメントＭｄｍｄに届かない場合がある。このようなことが生じる可能性が高い状況では、式５、式６の左辺の値を右辺の値で割った値が１より大きくなるようにすべきである。
【０１３８】
歩行時の分配用重み変数設定例である図２０では、Ｗｉｎｔを０に設定することによって、図２１の状況のように、両脚補償角θdbv を操作しても実全床反力モーメントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。
【０１３９】
好都合なことに、図２１のように後傾すると第２足平のかかとが結果的に下がって床に接地しやすくなるので、第２足平補償角を操作することによって実全床反力モーメントを発生させることができるようになる。
【０１４０】
また、後傾していないときには両脚補償角θｄｂｖを操作することによる実全床反力モーメントが発生するが、第２足平のかかとが床に接地しないので、第２足平補償角を操作しても実全床反力モーメントは発生しない。
【０１４１】
つまり、両脚補償角θｄｂｖが有効に働くときには各足平補償角が有効に働かず、各足平補償角が有効に働くときには両脚補償角θｄｂｖが有効に働かないので、結果的に両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量は、ほぼ補償全床反力モーメントＭｄｍｄに等しくなる。
【０１４２】
状況によっては、両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量が補償全床反力モーメントＭｄｍｄよりも大きくなってしまう場合がある。
【０１４３】
しかし、この場合でも、Ｍｄｍｄが姿勢安定化のためのフィードバック操作量であるならば、あまり問題にならない。何故ならば、Ｍｄｍｄの大きさが多少違っていても、一般的に制御系に言えることであるが、制御系のオープンループゲインが多少変化するだけで、クローズドループ特性はほとんど変わらないからである。
【０１４４】
注意点３）片脚支持期では、両脚補償角用の分配用重み変数であるＷｄｂｘ，Ｗｄｂｙの絶対値を小さくする。片脚支持期では両脚補償角を変化させても、接地していない足平が無駄に上下するだけで、実各足平床反力は変化しないからである。
【０１４５】
注意点４）足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきである。
【０１４６】
注意点５）両脚補償角を操作することによって制御できる実全床反力モーメントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。
【０１４７】
例えば、両脚補償角θｄｂｖを操作することによって発生する実全床反力モーメントの向きは必ずＶ方向であり、Ｖ方向に直交する成分を発生させることはできない。一方、各足平補償角を操作することによって発生できる実全床反力モーメントの向きは、足平の接地状況によって制約を受ける。
【０１４８】
例えば、足平２２Ｒ（Ｌ）のつまさきのエッジだけ、またはかかとのエッジだけが接地している場合には、エッジ線方向にモーメントを発生することはできない。両脚支持期では、この特性を考慮して、なるべく無駄なく両脚補償角および各足平補償角を操作する。
【０１４９】
例えば、両脚補償角を操作するための分配重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。
【０１５０】
Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ成分が０のベクトルをＷｄｂとすると、式３はベクトルＷｄｂとＭｄｍｄの内積になっている。従って、ＭｄｍｄをベクトルＷｄｂ方向成分とその直交成分に分解し、ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄｂの大きさを乗じたものが、式３によって求められるＭｄｍｄｄｂｖであると言える。
【０１５１】
この場合のＭｄｍｄｄｂｖを図２３に示す。これは、両脚補償角を操作することによって実全床反力モーメントのＷｄｂ方向成分を制御するフィードバック制御系を構成することを意味する。もし、Ｗｄｂ方向がベクトルＶと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントのＷｄｂ方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。
【０１５２】
従って、無駄な動きを減らしたい場合には、Ｗｄｂ方向をベクトルＶ方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反力モーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、ＷｄｂとＶの内積が１になるように設定する。一部を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積が１より小さくなるように設定する。
【０１５３】
ところで、足平の横幅が狭い場合には、各足平補償角を操作することによって発生し得る実各足平床反力モーメントのＸ成分は小さくなる。この場合には、Ｗｄｂｘを大きめに設定する。このように設定すると、Ｗｄｂ方向とベクトルＶ方向は一致しなくなり、両脚補償角の変動が増加するが、安定性が増す。
【０１５４】
両脚補償角決定部１１４ｂの処理についてさらに詳説すると、図２４は両脚補償角決定部１１４ｂの演算処理のブロック図であり、両脚補償角θｄｂｖは図示の如く演算される。
【０１５５】
図２４を参照して説明すると、目標第１足平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ２ａｃｔを、次式により求める。
【０１５６】

ここで、ＰＱ１は始点がＰ、終点がＱ１のベクトル、ＰＱ２は始点がＰ、終点がＱ２のベクトルである。
【０１５７】
また、式１２の代わりに、次式を用いても実際上はほとんど問題がない。

式１２ａは、目標全床反力中心点まわりに作用する実全床反力モーメントＭｓｕｍａｃｔを算出する式になっている。尚、式１２は、目標全床反力中心点まわりに作用する実全床反力モーメントから、目標各足平床反力中心点まわりに作用する実各足平床反力モーメントを減じたものになっている。
【０１５８】
次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトルＶは前述の動作説明において図１５に示したＶである。

【０１５９】
次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィルタ１１４ｉに通し、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔを得る。
【０１６０】
次に、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを補償フィルタ１１４ｊに通し、それを、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ方向成分Ｍｄｉｆｆｖを得る。
【０１６１】
尚、補償フィルタ１１４ｊは、Ｍｄｍｄｄｂｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０１６２】
次に、コンプライアンス機構４２の変形による両脚補償モーメントＶ方向成分への影響を打ち消すための両脚機構変形補償角θｆｆｄｂｖを求める。これは、いわゆるフィードフォワード補償である。
【０１６３】
具体的には、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプライアンスモデルを用い、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角θｆｆｄｂｖとすれば良い。
【０１６４】
両脚機構変形補償角θｆｆｄｂｖは、近似的には次式により求めれば良い。

ここでαは所定の定数である。
【０１６５】
最後に次式によって両脚補償角θｄｂｖを得る。ここでＫｄｂは制御ゲインであり、通常、これは正の値に設定する。

【０１６６】
第ｎ足平補償角決定部について説明すると、図２５はその中の第１足平Ｘ補償角決定部１１４ｃの演算処理を示すブロック図であり、第１足平Ｘ補償角決定部１１４ｃは第１足平Ｘ補償角θ１ｘを図示の如く演算する。説明は省略するが、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙも同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求めるアルゴリズムだけを説明する。
【０１６７】
第１足平床反力モーメントＸ成分Ｍ１ａｃｔｘをローパスフィルタ１１４ｋに通してＭ１ａｃｔｆｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを補償フィルタ１１４ｍに通し、それを、Ｍ１ａｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１ｘを得る。両脚補償角決定と同様、補償フィルタ１１４ｍは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周波数応答特性を改善するものである。
【０１６８】
次に、両脚補償角決定と同様、コンプライアンス機構４２の変形による第１足平補償モーメントＸ成分への影響を打ち消すための第１足平Ｘ機構変形補償角θｆｆ１ｘを求める。これは、いわゆるフィードフォワード補償である。
【０１６９】
具体的には、第１足平補償モーメントＸ方向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプライアンスモデルを用い、第１足平の変形角度を求め、それの極性を反転したものを第１足平Ｘ機構変形補償角θｆｆ１ｘとすれば良い。
【０１７０】
第１足平Ｘ機構変形補償角θｆｆ１ｘは、近似的には次式により求めれば良い。

ここでα１ｘは所定の定数である。
【０１７１】
最後に次式によって第１足平Ｘ補償角θ１ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、これも正の値に設定する。

尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。
【０１７２】
図１８に戻って説明を続けると、修正目標足平位置姿勢算出部１１４ｇは、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙに基づき、前述の複合コンプライアンス動作の足平位置姿勢修正手法に従って目標足平位置姿勢を修正し、修正目標足平位置姿勢を得る。
【０１７３】
機構変形量算出部１１４ｎは、目標各足平床反力によって発生が予想されるコンプライアンス機構４２の変形量を求める。
【０１７４】
機構変形補償入り修正目標足平位置姿勢算出部１１４ｈは、算出された機構変形量を打ち消すように、修正目標足平位置姿勢をさらに修正し、機構変形補償入り修正目標足平位置姿勢を得る。
【０１７５】
例えば、図２６に示すような機構変形量が予想されるときには、機構変形補償入り修正目標足平位置姿勢は、図２７に実線で示す位置姿勢に修正される。即ち、図２７に示す機構変形補償後の足平２２Ｒ（Ｌ）が目標足平床反力を受けて変形したときの位置姿勢が、図２６に示す機構変形補償前の足平位置姿勢に一致するように、機構変形補償入り修正目標足平位置姿勢を算出する。
【０１７６】
機構変形補償は、コンプライアンス機構４２の変形によって生じる実足平位置姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。
【０１７７】
上記を前提として図１０フロー・チャートの説明に戻ると、前記の如く、Ｓ３４において上記した補償角を決定する。
【０１７８】
図２８はその作業を示すサブルーチン・フロー・チャートである。
【０１７９】
同図を参照して説明すると、先ずＳ１００において前記したベクトルＶを求め、Ｓ１０２に進んで分配用重み変数を図２０に示すように設定し、現在時刻ｔでのこれらの値を求める。続いてＳ１０４に進み、式３および式４によって補償全床反力モーメントＭｄｍｄを両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モーメントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進んで各足平補償角θｎｘ（ｙ）を求める。
【０１８０】
次いで図１０フロー・チャートのＳ３６に進み、目標各足平床反力に基づいて機構変形補償量を算出し、Ｓ３８に進んで目標足平位置姿勢を補償角θｄｂｖ，θｎｘ（ｙ）に応じて修正し、さらにこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置姿勢を得る。
【０１８１】
次いでＳ４０に進み、上体位置姿勢と機構変形補償入り修正足平位置姿勢から関節変位指令（値）を算出し、Ｓ４２に進んで実関節変位を算出された関節変位指令（値）に追従させるようサーボ制御し、Ｓ４４に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理を繰り返す。
【０１８２】
上記の如く構成したので、これによって、概括すれば、実全床反力の制御と実各足平床反力の制御が殆ど干渉しないようになり、それらを容易に制御することができる。
【０１８３】
即ち、この複合コンプライアンス制御装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平５−３０５５８４号公報で提案した足首コンプライアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この複合コンプライアンス制御装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモーメントを望ましい値に制御するようにした。
【０１８４】
この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。
【０１８５】
さらには、ロボットに作用する床反力、より具体的には目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントと目標各足平中心点まわりの実各足平床反力モーメントを容易かつ適切に制御することができる。換言すれば、先に提案した技術を併用する場合に比較し、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【０１８６】
従って、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０１８７】
また、脚式移動ロボットの姿勢安定化制御を容易に実現できると共に、脚式移動ロボットが受ける着地衝撃を低減することができ、脚式移動ロボットの接地性を高め、歩行時のスリップやスピンを防止することができる。さらに、脚式移動ロボットのアクチュエータの負荷を低減することができる。
【０１８８】
また、特開平５−３０５５８６号公報で提案した技術では実全床反力（各足平床反力の合力）の目標全床反力中心点（目標ＺＭＰ）まわりのモーメント成分を検出し、その値が望ましい値になるように制御していたが、この複合コンプライアンス制御装置においては、目標各足平床反力中心点に作用する足平床反力の内のモーメント成分を除いた並進力成分の合成が目標全床反力中心点（目標ＺＭＰ）まわりに作用するモーメントを検出し、その値を望ましい値になるように制御するように変更した（尚、この点は先に提案した制御手法であっても良い）。
【０１８９】
図２９は複合コンプライアンス制御の変形例を示す、図１６と同様な説明図である。
【０１９０】
変形例にあっては補償動作を簡易化して同様の作用効果を得るようにした。即ち、各足平の床反力の力成分を操作する足平位置修正動作の手法としては、図１６に示した手法に代え、図２９に示すように、鉛直方向にだけ移動させるようにした。このとき、第１足平鉛直方向移動量Ｚ１と第２足平鉛直方向移動量Ｚ２は、次式によって求める。

但し、ここで、θｄｂｖには式１５で求められる値を代入する。
【０１９１】
以上を前提としてこの発明の一つの実施の形態に係る脚式移動ロボットの床形状推定装置を説明する。
【０１９２】
この実施の形態に係る脚式移動ロボットの床形状推定装置の構成は、図４に示した複合コンプライアンス制御の構成、より具体的には図１８に示した構成と同様である。ただし、図３０に示す如く、補償全床反力モーメント分配器１１４ａなどで示される複合コンプライアンス動作決定部は、図１８に示した構成と異なり、新たに床形状推定器１３０と、床形状推定器１３０が出力する各種の床形状推定値をそれぞれの補償角に加算する加算器１３２（１３２ａ，１３２ｂ）を備えるようにした。
【０１９３】
同図の説明に入る前に、床形状推定装置１３０が推定する上で使用する概念と用語を以下のように定義する。
【０１９４】
図３１に示すように、目標歩容において想定された床（あるいは床面）を「想定床」と呼ぶ。ロボットが歩行している実際の床を「実床」と呼ぶ。また想定床形状に対する実床形状のずれを「床形状偏差」と呼ぶ。
【０１９５】
前記した複合コンプライアンス制御において定義された目標第ｎ足平床反力中心点Ｑｎは、第ｎ足平の足底（床との接触面）に設定された点であった。従って、目標第ｎ足平床反力中心点Ｑｎは、足平２２Ｒ（Ｌ）と共に移動する。これに対応して、目標第ｎ足平床反力中心点Ｑｎと接することが想定された想定床面上の点を「想定第ｎ床接点Ｄｎ」と呼ぶ。
【０１９６】
この定義から明らかなように、第ｎ足平が接地している時期では、目標第ｎ足平床反力中心点Ｑｎと想定第ｎ床接点Ｄｎは、支持脚座標系から見て同一座標になる。これに対し、実際にロボット１が歩行しているときに、実第ｎ足平の足底面上における目標第ｎ足平床反力中心点Ｑｎに相当する点が、実床に接触する点を「実第ｎ床接点Ｄｎａｃｔ」と呼ぶ。
【０１９７】
これらの点の関係を表す一例を図３１に示す。図３１は、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を通る垂直面の法線方向（複合コンプライアンス制御におけるベクトルＶの方向）に向いて側方から歩行しているロボット１を見たところである。
【０１９８】
ロボット１の目標姿勢（目標歩容のこの瞬間における姿勢）および前記垂直面における想定床の断面を細線で示す。ロボット１の実足平位置姿勢と前記垂直面における実床の断面を太線で示す。この状況における実第ｎ床接点は、実床面上の点であり、図３１に示す位置になる。
【０１９９】
さらに、床形状偏差を定量的に表現する指標として、両脚間床高低差偏差、両脚間床傾斜偏差、足平床傾斜偏差を以下のように定義する。
【０２００】
第２床接点の高さと第１床接点の高さの差を「両脚間床高低差」と呼び、以下のように算出する。
両脚間床高低差＝第２床接点の高さ−第１床接点の高さ・・・式１９
【０２０１】
実両脚間床高低差と想定両脚間床高低差との差を「両脚間床高低差偏差」と呼ぶ。始点を第１床接点、終点を第２床接点とする線分の傾斜を「両脚間床傾斜」と呼び、以下のように算出する。
両脚間床傾斜＝atan（（第２床接点の高さ−第１床接点の高さ）／（第２床接点と第１床接点の間の水平距離））・・・ 式２０
ここでatanは、三角関数であるタンジェントの逆関数を表す。
【０２０２】
実両脚間床傾斜と想定両脚間床傾斜との差を「両脚間床傾斜偏差」と呼ぶ。それを図３１に示す。この状況において、両脚間床傾斜偏差は正である。第ｎ床接点における床面の傾斜を「第ｎ足平床傾斜」と呼ぶ。
【０２０３】
実第ｎ足平床傾斜と想定第ｎ足平床傾斜との差を「第ｎ足平床傾斜偏差」と呼ぶ。これを図３２に示す。図３２はロボット１を後ろから見た図である。ロボット１の目標姿勢（目標歩容のこの瞬間における姿勢）および想定床の断面を細線で示す。ロボット１の実足平位置姿勢と実床の断面を太線で示す。この状況において、第１足平床傾斜偏差は負である。
【０２０４】
尚、両脚間床傾斜および両脚間床傾斜偏差は、前述のベクトルＶ方向まわりの回転角で表される。第ｎ足平床傾斜および第ｎ足平床傾斜偏差は、Ｘ方向の回転角とＹ方向の回転角で表される。
【０２０５】
また、両脚間床傾斜偏差および第ｎ足平床傾斜偏差と複合コンプライアンス制御の各種補償角との間には、以下の関係がある。即ち、両脚間床傾斜偏差は、両脚支持期において、目標歩容通りに歩行しているロボット１の目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な両脚補償角に相当する。
【０２０６】
また、第ｎ足平床傾斜偏差は、目標歩容通りに歩行しているロボットの第ｎ足平を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な第ｎ足平補償角に相当する。
【０２０７】
従って、歩行中に両脚間床傾斜偏差を推定し、両脚間床傾斜偏差の推定値を両脚補償角に加えれば、両脚間床傾斜偏差があっても、実全床反力は想定床を歩行しているときと同一になる。また、歩行中に第ｎ足平床傾斜偏差を推定し、第ｎ足平補償角の推定値を第ｎ補償角に加えれば、第ｎ足平床傾斜偏差があっても、実第ｎ足平床反力は想定床を歩行しているときと同一になる。
【０２０８】
尚、複合コンプライアンス制御の両脚補償動作として変形例で示した鉛直方向にのみ移動させる手法を用いる場合には、両脚間床傾斜偏差の推定値の代わりに、両脚間床高低差偏差の推定値を用いれば良い。本質的な違いはないので、以降の説明では両脚間床傾斜偏差の推定値を用いる場合のみを説明する。
【０２０９】
図３０に示す構成における床形状推定器１３０への入力は、大きく分けて以下のようになる。
１）各種目標床反力中心点（目標全床反力中心点、目標第ｎ足平床反力中心点）、
２）想定床面形状（想定第ｎ床接点の座標、想定第ｎ足平床傾斜）、
３）最終的に実ロボットが追従すべき目標姿勢（機構変形入り修正目標足平位置姿勢）または実関節変位またはこれらの周波数重み付き平均、
４）上体傾斜偏差、
５）実床反力（実第ｎ足平床反力の力成分とモーメント成分）
【０２１０】
尚、床形状推定器１３０への入力は、図３０に示すものを用いると、最も推定精度を高くすることができるが、それ以外のものを選んでも良い。それに関しては第２の実施の形態で述べる。
【０２１１】
床形状推定器１３０の処理機能ブロック図を図３３に示す。
【０２１２】
図３３に示す床形状推定器１３０を構成する各機能要素について説明すると、床形状推定器１３０は機構コンプライアンスモデル１３４を備える。
【０２１３】
機構コンプライアンスモデル１３４は、ロボット幾何学モデル１１０（図４に示す）への入力、即ち、機構変形補償入り修正目標足平位置姿勢が、実床反力を受けたときのコンプライアンス機構４２の変形量を求め、求めた変形量を機構変形補償入り修正目標足平位置姿勢に加えることにより、機構変形後の推定各足平位置姿勢を求める。ここで、機構変形後の推定各足平位置姿勢は、上体２４に対する相対位置姿勢で表現される。
【０２１４】
ロボットの関節変位制御の追従遅れが無視できない場合、図３４に示すように、機構変形補償入り修正目標足平位置姿勢に、追従遅れに相当するローパスフィルタ１３６を通して機構コンプライアンスモデル１３４に入力すれば良い。
【０２１５】
あるいは、図３５に示すように、実関節変位からロボット幾何学モデル１３７（図４に１１０と示すのと同様のモデル）を介して機構変形なし実足平位置姿勢を求め、求めた値を機構変形補償入り修正目標足平位置姿勢の代わりに機構コンプライアンスモデル１３４に入力すれば良い。
【０２１６】
あるいは、周波数重みを用い、前記機構変形なし実足平位置姿勢と機構変形補償入り修正目標足平位置姿勢の重みつき平均を求め、それを機構変形補償入り修正目標足平位置姿勢の代わりに機構コンプライアンスモデル１３４に入力すれば良い。
【０２１７】
厳密には、前記した機構変形なし実足平位置姿勢を用いるべきであるが、関節変位制御の能力が高ければ、上記のいずれでも大差はない。
【０２１８】
また、図４においてロボット幾何学モデル１１０への入力に機構変形補償が含まれない場合、機構変形補償のない修正目標足平位置姿勢を機構コンプライアンスモデル１３４に入力すれば良い。
【０２１９】
図３３の説明に戻ると、床形状推定器１３０は両脚間床傾斜偏差推定部１３８を備える。即ち、同図において、機構変形後の推定各足平位置姿勢、目標各足平床反力中心点の目標足平に対する相対座標、想定各床接点および上体傾斜偏差から推定両脚間床傾斜偏差を推定するまでのブロックを「両脚間床傾斜偏差推定部」と呼ぶ。以下にこの部分の処理を説明する。
【０２２０】
先ず、機構変形後の推定各足平位置姿勢と目標各足平床反力中心点Ｑｎの目標足平に対する相対座標とから、機構変形後の目標各足平床反力中心点Ｑｎ”の座標を算出する。ここで、機構変形後の目標各足平床反力中心点Ｑｎ”は、実上体に対する相対位置で表わされる。
【０２２１】
より具体的には、目標第ｎ足平床反力中心点Ｑｎの足平に対する相対位置をベクトルＲｎ、機構変形後の推定第ｎ足平位置をベクトルＵｎ、機構変形後の推定第ｎ足平姿勢をマトリックスＡｎで表わすと、機構変形後の目標第ｎ足平床反力中心点Ｑｎ”は次式によって計算される。

【０２２２】
次に、Ｑ１”を始点、Ｑ２”を終点とするベクトルＱ１”Ｑ２”の実上体に対するベクトルＶまわりの傾きθｆｄｂｖ”を算出する。ここでベクトルＶは、複合コンプライアンス制御で定義されたベクトルである。
【０２２３】
一方、想定第ｎ床接点Dnの目標上体に対する相対座標から、Ｄ１を始点、Ｄ２を終点とするベクトルＤ１Ｄ２の目標上体に対するベクトルＶまわりの傾きθｆｄｂｖを算出する。
【０２２４】
ここで、想定第ｎ床接点Ｄｎは、先に定義したように、第ｎ足平が接地している時期においては、目標第ｎ足平床反力中心点Ｑｎと同一位置とする。目標歩容において第ｎ足平が接地する直前の時期では、次に接地する時に想定されている目標第ｎ足平床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。目標歩容において第ｎ足平が離床した直後の時期では、離床時に想定されていた目標第ｎ足平床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。
【０２２５】
次に、前記のθｆｄｂｖ”に、上体傾斜偏差のＶ方向成分θｅｒｒｖを加えることにより、Ｑ１”を始点、Ｑ２”を終点とするベクトルＱ１”Ｑ２”の目標上体に対するベクトルＶまわりの傾きを求め、それから前記のθｆｄｂｖを減じることにより、Ｖ方向瞬間両脚間床傾斜偏差計算値θｆｄｂｅｒｒｖを得る。
【０２２６】
次に、それから直前の推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを減じることにより、θｆｄｂｅｒｒｄｉｆｆｖを得る。次に、両脚間床傾斜偏差推定中断器１４０において、両脚間床傾斜偏差の推定を実行すべきか中断すべきかを判断する。
【０２２７】
両脚間床傾斜偏差推定中断器１４０は、両脚間床傾斜偏差の推定を実行（続行）すべきと判断したならば、両脚間床傾斜偏差推定中断器１４０への入力である前記θｆｄｂｅｒｒｄｉｆｆｖをそのまま出力し、中断すべきと判断したならば、０を出力する。両脚間床傾斜偏差推定中断器１４０に関する詳細は後述する。
【０２２８】
次に、両脚間床傾斜偏差推定中断器１４０の出力を積分定数がＫｄｂｅｓｔｍである積分器１４２で積分することにより、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを得る。推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、図３１に示す両脚間床傾斜偏差の推定値であり、Ｖまわりの角度で表わされる。以上が、両脚間床傾斜偏差推定部１３８の処理である。
【０２２９】
さらに、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、図３０に示すように、両脚補償角θｄｂｖに加算され、床形状推定入り両脚補償角θｄｂｖ’として修正目標足平位置姿勢算出部１１４ｇへ入力される。
【０２３０】
以上の処理を毎制御周期に繰り返し実行することにより、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖが実際の両脚間床傾斜偏差に収束する。また、足平位置姿勢は推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖに従って修正され、両脚間床傾斜偏差が実床反力に及ぼす影響を吸収する。
【０２３１】
尚、両実足平が接地しているならば、前記したＱｎ”は図３１の実第ｎ床接点Ｄｎａｃｔに一致するはずである。従って、前記ベクトルＱ１”Ｑ２”と前記ベクトルＤ１Ｄ２の傾きの差θｆｄｂｅｒｒｖは、図３０における両脚間床傾斜偏差に一致するはずである。
【０２３２】
しかし、実際にはロボット１の機械系や制御系の振動および電気的なノイズにより、上記計算アルゴリズムによって得られたθｆｄｂｅｒｒｖは、実際の両脚間床傾斜偏差を中心値として激しく変動する。従って、θｆｄｂｅｒｒｖをそのまま両脚間床傾斜偏差の推定値として用いて、これを図３０に示すように両脚補償角に加算すると、ロボット１の足平２２Ｒ（Ｌ）は発振するかあるいは激しく振動する。
【０２３３】
ところで、図３３の両脚間床傾斜偏差推定中断器１４０が常に推定実行（続行）と判断すれば、θｆｄｂｅｒｒｖから推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖまでの伝達関数は、１次遅れのローパスフィルタになる。即ち、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖはθｆｄｂｅｒｒｖにローパスフィルタを通したものとなる。従って、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを図３０に示すように両脚補償角に加算しても、足平２２Ｒ（Ｌ）の発振や振動を生じ難くなる。
【０２３４】
ちなみに、この例では、ローパスフィルタの時定数は１／Ｋｄｂｅｓｔｍである。図３３に示す例以外にも、θｆｄｂｅｒｒｖからθｆｄｂｅｓｔｍｖまでの伝達関数がローパスフィルタになるように構成すれば良い。これにより、足平２２Ｒ（Ｌ）の発振や振動を防止することができる。
【０２３５】
次いで、両脚間床傾斜偏差推定部１３８の特徴を説明する。
【０２３６】
姿勢安定化制御のために必要な両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを発生させようと、図３０における両脚補償角θｄｂｖを変化させ、最終的な目標足平位置姿勢である機構変形補償入り修正目標足平位置姿勢を変更させても、両実足平が接地している限り、機構変形後の目標各足平床反力中心点Ｑｎ”の実際の位置は、実第ｎ床接点Ｄｎａｃｔに一致したまま動かず、その代わりに、コンプライアンス機構４２が変形したり、上体２４の位置姿勢が変化する。即ち、機構変形補償入り修正目標足平位置姿勢を変更させた分を打ち消すように、コンプライアンス機構４２が変形したり、上体２４の位置姿勢が変化する。
【０２３７】
この実施の形態における両脚間床傾斜偏差推定部１３８は、機構変形補償入り修正目標足平位置姿勢から推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを算出するまでに、実床反力を基に機構コンプライアンスモデル１３４によって算出された機構部変形量と、傾斜センサ３６によって検出された実上体姿勢傾斜偏差を加えているので、機構変形補償入り修正目標足平位置姿勢が変動しても、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖはその影響を受けない。
【０２３８】
これは、姿勢制御と両脚間床傾斜偏差推定が非干渉化されていることを意味する。従って、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを図３０に示すように両脚補償角に加算しても、制御系の安定余裕（発振のし難さ）はほとんど低下しない。この点は、前記したロボット学会論文に示す従来技術では得られない利点である。
【０２３９】
また、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを図３０に示すように両脚補償角に加算すると、両脚間床傾斜偏差が存在していても、その影響を打ち消すことができるので、実全床反力は、定常的には想定床を歩行している場合と同一になる。しかも、両脚間床傾斜偏差の推定と補償角の修正は毎制御周期に実行されるので、床形状が途中で変化しても、変化後の両脚間床傾斜偏差を推定し、推定値に基づいてその影響を打ち消すことができる。
【０２４０】
ただし、両脚間床傾斜偏差推定部１３８には前述したようにローパスフィルタが含まれるので、両脚が接地したときや床形状が途中で変化したときには、過渡的には、実全床反力は、両脚間床傾斜偏差の影響を受ける。しかし、その後、時定数１／Ｋｄｂｅｓｔｍでその影響が減衰する。
【０２４１】
さらに、図３３に示すように、床形状推定器１３０は足平床傾斜偏差推定部１４４を備える。即ち、同図において、機構変形後の推定第１足平位置姿勢、想定第１足平床傾斜および上体傾斜偏差Ｘ成分から推定第１足平床傾斜偏差Ｘ成分を推定するまでのブロックを「足平床傾斜偏差推定部」１４４、より具体的には「第１足平床傾斜偏差Ｘ成分推定部」と呼ぶ。この部分の処理を説明する。
【０２４２】
先ず、機構変形後の推定第１足平位置姿勢と想定第１足平床傾斜から、想定第１足平床傾斜に対する機構変形後の推定第１足平位置姿勢の傾斜のＸ成分θｆ１ｅｒｒｘ”を算出する。
【０２４３】
これは、目標第１足平姿勢傾斜と想定第１足平床傾斜との差にコンプライアンス機構４２のたわみ角（図２）を実床反力から推定した値を加算した値に相当する。ただし、図２ではＹ成分を示す。
【０２４４】
次に、前記のθｆ１ｅｒｒｘ”に、上体傾斜偏差のＸ方向成分θｅｒｒｘを加えて瞬間第１足平床傾斜偏差Ｘ成分計算値θｆ１ｅｒｒｘを得る。
【０２４５】
次に、それから直前の推定第１足平床傾斜偏差θｆ１ｅｓｔｍｘを減じることにより、θｆ１ｅｒｒｄｉｆｆｘを得る。
【０２４６】
次に、足平床傾斜偏差推定中断器１４６、より具体的には第１足平床傾斜偏差Ｘ成分推定中断器において、第１足平床傾斜偏差Ｘ成分の推定を実行すべきか中断すべきかを判断する。
【０２４７】
第１足平床傾斜偏差Ｘ成分推定中断器は、第１足平床傾斜偏差Ｘ成分の推定を実行すべきと判断したならば、第１足平床傾斜偏差Ｘ成分推定中断器への入力である前記θｆ１ｅｒｒｄｉｆｆｘをそのまま出力し、中断すべきと判断したならば、０を出力する。尚、足平床傾斜偏差推定中断器１４６、より具体的には第１足平床傾斜偏差Ｘ成分推定中断器に関する詳細は後述する。
【０２４８】
次に、第１足平床傾斜偏差Ｘ成分推定中断器の出力を積分定数がＫ１ｅｓｔｍｘである積分器１４８で積分することにより、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを得る。
【０２４９】
以上が、第１足平床傾斜偏差Ｘ成分推定部の処理である。
【０２５０】
推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは、図３０に示すように、第１足平Ｘ補償角θ１ｘに加算され、床形状推定入り第１足平Ｘ補償角θ１ｘ’として修正目標足平位置姿勢算出部１１４ｇへ入力される。
【０２５１】
以上の処理を毎制御周期に繰り返し実行することにより、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘが実際の第１足平床傾斜偏差Ｘ成分に収束する。また、足平位置姿勢は、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘに従って修正され、第１足平床傾斜偏差Ｘ成分が実床反力に及ぼす影響を吸収する。
【０２５２】
尚、図３３では第１足平床傾斜偏差Ｘ成分推定部１４４のみ図示したが、それ以外にも第１足平床傾斜偏差Ｙ成分推定部、第２足平床傾斜偏差Ｘ成分推定部、第２足平床傾斜偏差Ｙ成分推定部が設けられ、第１足平床傾斜偏差Ｙ成分、第２足平床傾斜偏差Ｘ成分および第２足平床傾斜偏差Ｙ成分に関しても同様の処理が行われる。
【０２５３】
足平床傾斜偏差推定に関しても、両脚間床傾斜偏差推定と同様に以下のことが言える。
【０２５４】
実第１足平がべたに接地しているならば（より厳密には、実第１足平と実床が面接触しているか、あるいはつまさきエッジやかかとエッジで接触しているようにＹ方向に線接触しているならば）、瞬間第１足平床傾斜偏差Ｘ成分計算値θｆ１ｅｒｒｘは、実際の第１足平床傾斜偏差Ｘ成分に一致するはずである。
【０２５５】
しかし、実際には、ロボット１の機械系や制御系の振動および電気的なノイズにより、上記計算アルゴリズムによって得られたθｆ１ｅｒｒｘは、実際の第１足平床傾斜偏差Ｘ成分を中心値として激しく変動する。
【０２５６】
従って、θｆ１ｅｒｒｘをそのまま第１足平床傾斜偏差Ｘ成分の推定値として用い、それを図３０に示すように第１足平Ｘ補償角に加算すると、ロボット１の足平２２Ｒ（Ｌ）は、発振するかあるいは激しく振動する。
【０２５７】
ところで、図３３の第１足平床傾斜偏差Ｘ成分推定中断器が常に推定実行（続行）と判断すれば、θｆ１ｅｒｒｘから推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘまでの伝達関数は、１次遅れのローパスフィルタになる。即ち、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘはθｆ１ｅｒｒｘにローパスフィルタを通したものとなる。
【０２５８】
従って、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを図３０に示すように第１足平補償角に加算しても、足平の発振や振動を生じ難くなる。ちなみに、この例では、ローパスフィルタの時定数は１／Ｋ１ｅｓｔｍｘである。
【０２５９】
図３３に示す例以外にも、両脚間床傾斜偏差推定と同様に、θｆ１ｅｒｒｘからθｆ１ｅｓｔｍｘまでの伝達関数がローパスフィルタになるように構成すれば良い。これにより、足平２２Ｒ（Ｌ）の発振や振動を防止することができる。
【０２６０】
次いで、足平床傾斜偏差推定部１４４の特徴を説明する。
【０２６１】
両脚間床傾斜偏差推定と同様に以下のことが言える。
【０２６２】
姿勢安定化制御のために必要な第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを発生させようと、図３０における第１足平補償角θ１ｘを変化させて、最終的な目標足平位置姿勢である機構変形補償入り修正目標足平位置姿勢を変更させても、実第１足平がべたに接地している限り、実足底面の姿勢は変わらず、その代わりに、コンプライアンス機構４２が変形したり、上体２４の位置姿勢が変化する。即ち、両脚間床傾斜偏差推定と同様に、機構変形補償入り修正目標足平位置姿勢を変更させた分を打ち消すように、コンプライアンス機構４２が変形したり、上体２４の位置姿勢が変化する。
【０２６３】
この実施の形態における第１足平床傾斜偏差Ｘ成分推定部は、機構変形補償入り修正目標足平位置姿勢から推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを算出するまでに、実床反力を基に機構コンプライアンスモデル１３４によって算出されたコンプライアンス機構変形量と、傾斜センサ３６によって検出された実上体姿勢傾斜偏差を加えているので、機構変形補償入り修正目標足平位置姿勢が変動しても、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘはその影響を受けない。
【０２６４】
これは、姿勢制御と第１足平床傾斜偏差推定が非干渉化されていることを意味する。従って、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを図３０に示すように第１足平補償角に加算しても、両脚間床傾斜偏差推定と同様に、制御系の安定余裕はほとんど低下しない。
【０２６５】
また、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを図３０に示すように第１足平補償角に加算すると、第１足平床傾斜偏差が存在していても、その影響を打ち消すことができるので、実第１足平床反力は、定常的には想定床を歩行している場合と同一になる。
【０２６６】
しかも、第１足平床傾斜偏差の推定と補償角の修正は毎制御周期に実行されるので、床形状が途中で変化しても、変化後の第１足平床傾斜偏差を推定し、その影響を打ち消すことができる。
【０２６７】
ただし、第１足平床傾斜偏差推定器には前記のローパスフィルタが含まれるので、第１足平が接地したときや床形状が途中で変化したときには、過渡的には、第１足平床傾斜偏差の影響を受け、その後、時定数１／Ｋ１ｅｓｔｍｘでその影響が減衰する。
【０２６８】
上記したこの実施の形態における床形状推定の特徴を総合的に説明する。
【０２６９】
この実施の形態における床形状推定が前提とする複合コンプライアンス制御においては、各補償角を操作して実全床反力および各足平床反力を制御しても、その特徴として、それぞれの制御は非干渉化されている。
【０２７０】
従って、推定両脚間床傾斜偏差θｆｄｅｓｔｍｖを求めることと、それを両脚補償角θｄｂｖに加算することと、推定各足平床傾斜偏差θｆ１ｅｓｔｍｘ，θｆ１ｅｓｔｍｙ，θｆ２ｅｓｔｍｘ，θｆ２ｅｓｔｍｙを求めることと、それらをそれぞれ各足平補償角θ１ｘ，θ１ｙ，θ２ｘ，θ２ｙに加算することを同時に行っても、干渉し合って発振することはほとんどない。
【０２７１】
その特徴と前述した両脚間床傾斜偏差推定部１３８と足平床傾斜偏差推定部１４４の各特徴により結果的に以下の利点が得られる。
【０２７２】
１）姿勢復元力制御のために両脚補償角θｄｂｖと各足平補償角θ１ｘ，θ１ｙ，θ２ｘ，θ２ｙを操作して実床反力を制御することと、両脚間床傾斜偏差を推定することと、これを両脚補償角θｄｂｖに加算することと、各足平床傾斜偏差を推定することと、それらを各足平補償角に加算することを同時に行うことができる。
【０２７３】
簡単に言えば、床形状偏差の複数のパラメータを同時に推定しながら、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作も同時に実行することができる。さらには、姿勢制御のための脚の補償動作も同時に実行することができる。
【０２７４】
２）床形状偏差が大きくても制御が破綻し難い（複合コンプライアンス制御は微小角に限定した近似を行っていないから）。
３）床形状偏差の推定精度が高い。
【０２７５】
４）姿勢安定化制御との干渉による発振が生じにくいので、推定部１３８，１４４のゲイン（Ｋｄｂｅｓｔｍ，Ｋ１ｅｓｔｍｘ，Ｋ１ｅｓｔｍｙ，Ｋ２ｅｓｔｍｘ，Ｋ２ｅｓｔｍｙ）を大きくすることができ、その結果、推定時定数を短くすることができる。それによって、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作の応答性を高めることができるので、床形状偏差が床反力に及ぼす過渡的な影響が短時間で解消される。
５）床形状偏差推定と床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作が常時実行されるので、途中で床形状が変化しても、過渡的にはその影響を受けるが、その後、推定の時定数でその影響が減衰する。
【０２７６】
次いで、この実施の形態における前記した推定中断器１４０，１４６について説明する。
【０２７７】
先ず、第１足平床傾斜偏差Ｙ成分推定中断器の役割について説明する。
【０２７８】
第１足平床傾斜偏差推定Ｙ成分中断器は、第１足平床傾斜偏差Ｙ成分の推定が正常に実行できるか否かを判断し、正常に実行できないと判断したときには推定を中断する。
【０２７９】
より具体的には、原則的に、推定第１足平床傾斜偏差Ｙ成分の精度が低下する恐れがある状況、および推定第１足平床傾斜偏差Ｙ成分が発散する恐れがある状況では、推定を中断するために、０を出力し（即ち、積分入力に０を入れて積分値をホールドする）、それ以外の状況では、推定を実行するために、第１足平床傾斜偏差Ｙ成分推定中断器への入力をそのまま出力する。
【０２８０】
もし、第１足平床傾斜偏差Ｙ成分推定中断器が動作せず、常に第１足平床傾斜偏差Ｙ成分推定中断器への入力をそのまま出力し続けると、ある状況では推定第１足平床傾斜偏差Ｙ成分が発散する。
【０２８１】
以下にこの実施の形態において推定第１足平床傾斜偏差Ｙ成分が発散する状況を説明する。
【０２８２】
例として、図３６に示すように、ロボット１が片脚支持期に前傾したために、姿勢安定化制御によって第１足平Ｙ補償角を操作して大きな姿勢復元力を得ようと試みた結果、本来べたに接地すべき実第１足平のかかとが床から浮いてしまった状況を仮定する。また、このとき実床形状は想定床形状と一致していると仮定する。
【０２８３】
この状況では、図３６に示される想定第１足平床傾斜Ｙ成分に対する機構変形後の推定第１足平姿勢の傾斜Ｙ成分θｆ１ｅｒｒｙ”に上体傾斜偏差Ｙ成分θｅｒｒｙを加えたθｆ１ｅｒｒｙは、同図に示すように、正の値になる。
【０２８４】
第１足平床傾斜偏差Ｙ成分推定中断器が、常に入力であるθｆ１ｅｒｒｄｉｆｆｙをそのまま出力し続ける場合には、前述したように、θｆ１ｅｒｒｙから推定両脚間床傾斜偏差θｆ１ｅｓｔｍｙまでの伝達関数は、１次遅れになる。
【０２８５】
即ち、θｆ１ｅｓｔｍｙは、θｆ１ｅｒｒｙにやや遅れて追従する。θｆ１ｅｓｔｍｙは、第１足平Ｙ補償角θ１ｙに加算され、床形状推定入り第１足平Ｙ補償角θ１ｙ’になる。図３０ではθ１ｘ’のみ図示しているが、θ１ｙも同様な構成になる。
【０２８６】
次に、目標第１足平位置姿勢に床形状推定入り第１足平Ｙ補償角θ１ｙ’が加えられ、さらに目標床反力（目標歩容の床反力）から算出される機構変形量が減じられ（即ち、機構変形量を打ち消す機構変形補償量が加えられ）、機構変形補償入り修正目標第１足平位置姿勢が算出される。
【０２８７】
さらに機構コンプライアンスモデル１３４によって、実床反力から計算される機構変形量が求められ、機構変形補償入り修正目標第１足平位置姿勢に加えられて機構変形後の推定第１足平位置姿勢が求められる。この傾きＹ成分から想定第１足平傾斜のＹ成分が減じられ、さらに上体傾斜偏差Ｙ成分θｅｒｒｙが加えられることにより、前記θｆ１ｅｒｒｙの値が更新される。
【０２８８】
以上の関係から、θｆ１ｅｒｒｙとこれに１次遅れで追従するθｆ１ｅｓｔｍｙの関係は以下のようになる。尚、目標床反力による第１足平機構変形の補償角は、先に段落０１３５で述べたコンプライアンス機構４２の変形量を打ち消す補償量である。
θf1erry＝θf1estmy＋θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry＋実床反力から計算される第１足平機構変形角Y成分＋目標第１足平姿勢傾斜Y成分−想定第１足平傾斜Y成分 ・・・式２２
【０２８９】
目標歩容の片足支持期においては、（目標第１足平姿勢傾斜Ｙ成分−想定第１足平傾斜のY成分）は０である。従って、式２２は次式に変形される。
θf1erry＝θf1estmy＋θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry ＋実床反力から計算される第１足平機構変形角Y成分 ・・・式２３
【０２９０】
第１足平が床にべたに接地しているならば、（θｆ１ｅｓｔｍｙ＋θ１ｙ＋目標床反力による第１足平機構変形の補償角Ｙ成分＋θｅｒｒｙ）がいかなる値であっても、実床反力から計算される機構変形角Ｙ成分はそれを打ち消すので、式２３の右辺は０になる。従って、θｆ１ｅｒｒｙは常に０となり、また、θｆ１ｅｓｔｍｙは、θｆ１ｅｒｒｙに１次遅れで追従するので、０に徐々に収束する。これは、実床形状が想定床形状と一致している状況を推定器が正確に推定していることを意味する。
【０２９１】
ところが、図３６の状況では（θｆ１ｅｓｔｍｙ＋θ１ｙ＋目標床反力による第１足平機構変形の補償角Ｙ成分＋θｅｒｒｙ）は大きな正の値となるが、かかとが浮いてしまったために実床反力が十分に大きくなれず、実床反力から計算される機構変形角Ｙ成分は、前記の値を打ち消すだけの十分な大きさの負の値を持つことができない。即ち、式２３の右辺は０にならない。このような状況では、θｆ１ｅｒｒｙは０になれず、θｆ１ｅｓｔｍｙは０に収束しない。これは、推定値が間違っていることを意味する。
【０２９２】
このような状況の中でも、特に次の式２４を満足している状況が続く場合には、θｆ１ｅｒｒｙに近づこうとしてθｆ１ｅｓｔｍｙが増大しても、θｆ１ｅｒｒｙは、式２３と次の式２４から導かれるように、θｆ１ｅｒｒｙ＞θｆ１ｅｓｔｍｙの関係を保つので、θｆ１ｅｓｔｍｙから離れてしまう。
【０２９３】
これを繰り返すうちに、θｆ１ｅｓｔｍｙは正の方向にますます発散する。これは、推定値が発散しつつあることを意味する。
θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry＋実床反力から計算される第１足平機構変形角Y成分 > 0・・・式２４
【０２９４】
次いで、図３７を参照して第１の実施の形態における第１足平床傾斜偏差Ｙ成分推定中断器の処理を説明する。この処理は毎制御周期に実行される。
【０２９５】
以下に説明すると、先ずＳ３００において目標歩容の時期に応じて第１足平床傾斜Ｙ成分推定モードを決定する。
【０２９６】
具体的には、第１足平床傾斜Ｙ成分推定モードは、リセットモード、ホールドモードおよび準備完了モードの３つのモード状態を取りうるものとし、目標歩容の時間経過に対して、図３８に示す如く遷移するように決定する。ただし、これは図５に示す歩容に対するモード遷移パターンである。歩容が異なれば、それに応じてモード遷移パターンも変えるべきである。
【０２９７】
従ってＳ３０２に進み、第１足平床傾斜Ｙ成分推定モードに応じて以下の処理を行う。
【０２９８】
第１足平床傾斜Y成分推定モードがリセットモードである場合はＳ３０４に進み、推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙを強制的に徐々に０に収束させ、Ｓ３０６に進み、第１足平床傾斜偏差Ｘ成分推定中断器の出力を０にする。
【０２９９】
尚、リセットモードは、図３８に示す如く、第１足平を動かしても床反力に影響がないように、第１足平が床から完全に離れている期間に存在させる。１歩前に着地していた第２床接点の足平床傾斜偏差と次に着地する第１床接点の足平床傾斜偏差との関係に相関がない場合には、上記のように推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙを強制的に徐々に０に収束させるのが良い。
【０３００】
しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることがわかっているならば、前記関係にある程度の相関があると考えられるので、推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙを、強制的に徐々に１歩前に着地していた第２床接点での推定第２足平床傾斜偏差に１より小さい正の定数を乗じた値に収束させるようにしても良い。
【０３０１】
さらに、１歩前の推定足平床傾斜偏差Y成分だけでなく、複数歩前の推定足平床傾斜偏差Y成分も用いて収束目標値を決定しても良い。また、１歩前や複数歩前の推定両脚間床傾斜偏差も用いて収束目標値を決定しても良い。
【０３０２】
第１足平床傾斜Ｙ成分推定モードがホールドモードである場合はＳ３０８に進み、第１足平床傾斜偏差Ｙ成分推定中断器の出力を０にする。従って、推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙはホールドモードに遷移する直前の値に保持される。
【０３０３】
尚、ホールドモードは、推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙが発散する恐れがある時期およびその精度が低下する恐れがある時期に存在させる。例えば、図３８に示すように、第１足平のかかとまたはつまさきが床から離れ始める時期から第１足平が床から完全に離れてしばらく後までの期間に存在させる。
【０３０４】
上記以外の時期、即ち、第１足平床傾斜偏差Ｙ成分の推定が正常に実行できる時期では、第１足平床傾斜Ｙ成分推定モードは準備完了モードになる。
【０３０５】
準備完了モードでは、以下の処理を行う。
【０３０６】
即ち、Ｓ３１０に進み、実第１足平床反力モーメントＭ１ａｃｔと実第１足平床反力の力成分Ｆ１ａｃｔから実第１足平床反力中心点位置Ｑ１ａｃｔを求める。ここで、実第１足平床反力中心点位置Ｑ１ａｃｔは、その点における実第１足平床反力モーメントの内の鉛直方向を除く方向の成分が０となる足底上の作用点である。実第１足平床反力中心点Ｑ１ａｃｔのＸ座標をＱ１ａｃｔｘとする。
【０３０７】
次いでＳ３１２に進み、あらかじめ設定しておいた第１足平推定許容最小床反力Ｆ１ｍｉｎ、第１足平推定許容領域最大値Ｑ１ｍａｘｘおよび第１足平推定許容領域最小値Ｑ１ｍｉｎｘに対し、実第１足平床反力Ｚ成分（鉛直成分）Ｆ１ａｃｔｚと前記Ｑ１ａｃｔｘが以下の条件を満たすか否かを調べる。
【０３０８】
条件１）F1actz < F1min
条件２）Q1actx > Q1maxx かつθf1errdiffy > 0
条件３）Q1actx < Q1minx かつθf1errdiffy < 0
【０３０９】
条件１に関して補足すると、第１足平推定許容最小床反力Ｆ１ｍｉｎは、第１足平にそれ以下の床反力しか作用していない場合には第１足平床傾斜が推定できないと判断される足平床反力設定値であり、ロボット１の自重より小さな値に設定しておく。換言すれば、条件１は、第１足平と床との間の圧力が小さ過ぎて第１足平床傾斜Ｙ成分が精度良く推定できない状況を意味する。
【０３１０】
一方、条件２および条件３に関しては、足平の座標系から見て、つまさきのＸ座標をｘｔｏｅ、かかとのX座標をｘｈｅｅｌとすると、あらかじめ、第１足平推定許容領域最大値Ｑ１ｍａｘｘの値を、ｘｔｏｅよりもやや小さめに設定しておき、第１足平推定許容領域最小値Ｑ１ｍｉｎｘの値を、ｘｔｏｅよりもやや大きめに設定しておく。これらの関係を図３９に示す。
【０３１１】
推定が理想的に実行されるならば、第１足平推定許容領域最大値Ｑ１ｍａｘｘと第１足平推定許容領域最小値Ｑ１ｍｉｎｘは、エッジに一致させても良い。しかし実際には検出誤差が生じ、これにより推定値が発散する場合がある。そこでこれを防ぐために、図３９のようにエッジからやや内側に第１足平推定許容領域最大値Ｑ１ｍａｘｘと第１足平推定許容領域最小値Ｑ１ｍｉｎｘを設定しておく。
【０３１２】
条件２は、かかとが浮きかかっているのに、積分器入力θｆ１ｅｒｒｄｉｆｆｙが正、即ち、推定第１足平床傾斜がかかとが浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り第１足平Ｙ補償角が、ますますかかとが浮く方向に変化する。
【０３１３】
条件３は、つまさきが浮きかかっているのに、積分器入力θｆ１ｅｒｒｄｉｆｆｙが負、即ち、推定第１足平床傾斜がつまさきが浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り第１足平Ｙ補償角が、ますますつまさきが浮く方向に変化する。
【０３１４】
図３７フローチャートの説明に戻ると、Ｓ３１２で肯定されて条件１、条件２または条件３のいずれかを満足する場合には、推定を中断すべきであるので、Ｓ３１４に進んで第１足平床傾斜偏差Ｙ成分推定中断器の出力を０にし、それ以外の場合には、推定を実行しても問題がないので、Ｓ３１６に進んで前記出力をθｆ１ｅｒｒｄｉｆｆｙにする。
【０３１５】
条件１、条件２および条件３の式は、以上に述べた状況を表わしていれば良い。従って、条件１では、実第１足平床反力Ｚ成分（鉛直成分）Ｆ１ａｃｔｚの代わりに、実第１足平床反力の力成Ｆ１ａｃｔの内の足平に垂直な成分（この成分を「Ｆ１ａｃｔｚ’」と呼ぶ）を用いても良い。
【０３１６】
また、条件２および条件３では、前記実第１足平床反力中心点位置Ｑ１ａｃｔの代わりに、実第１足平床反力モーメントの内の足平に垂直な方向を除く方向の成分が０となる足底上の作用点（この点を「Ｑ１ａｃｔ’」と呼ぶ）を用いても良い。
【０３１７】
また、この実施の形態に係る第１足平床傾斜偏差Ｘ成分推定中断器の処理について説明すると、第１足平床傾斜偏差Ｘ成分推定中断器も、第１足平床傾斜偏差Ｙ成分推定中断器とほぼ同様の処理を行う。
【０３１８】
相違点は、前記条件２および条件３において、つまさきまたはかかとが浮く条件の代わりに足平の内エッジ、外エッジが浮く条件を用いることである。即ち、条件２と条件３の代わりに、以下の条件２’と条件３’を用いる。
【０３１９】
ここでＱ１ｍａｘｙおよびＱ１ｍｉｎｙは、図３９のように足平内エッジと外エッジの足平中心側に設定される。
条件２’）Q1acty > Q1maxy かつθf1errdiffx < 0
条件３’）Q1acty < Q1miny かつθf1errdiffx > 0
【０３２０】
尚、座標系の向きの関係上、不等号が、条件２、条件３の場合と一部異なる。また、第１足平床傾斜偏差Ｘ成分推定モードの遷移タイミングは、第１足平床傾斜偏差Ｙ成分推定モードの場合と較べ、準備完了モードの終了時期をもう少し遅らせても良い。つまさきエッジが接地していれば、まだ第１足平床傾斜偏差Ｘ成分が推定できるからである。
【０３２１】
また、第２足平床傾斜偏差推定中断器の処理も、第１足平床傾斜偏差推定中断器の処理と同様である。
【０３２２】
次いで、図４０を参照して両脚間床傾斜偏差推定中断器１４０の処理を説明する。この処理は毎制御周期に実行される。
【０３２３】
この処理は、第１足平床傾斜偏差Ｙ成分推定中断器と基本的には同じであるが、細部が異なる。
【０３２４】
以下説明すると、先ずＳ４００において目標歩容の時期に応じて両脚間床傾斜推定モードを決定する。
【０３２５】
具体的には、両脚間床傾斜推定モードは、目標歩容の時間経過に対して、図４１のように決定する。ただし、これも図５に示す歩容に対するモードパターンであり歩容が異なれば、それに応じてモードパターンも変えるべきである。
【０３２６】
次いでＳ４０２に進み、両脚間床傾斜推定モードに応じて以下のように処理する。
【０３２７】
両脚間床傾斜推定モードがリセットモードである場合はＳ４０４に進み、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを強制的に徐々に０に収束させ、Ｓ４０６に進んで両脚間床傾斜偏差推定中断器の出力を０にする。尚、リセットモードは、図４１に示すごとく、床形状推定入り両脚補償角を動かしても全床反力に影響がないように、片脚支持期に存在させる。
【０３２８】
１歩前の両脚間床傾斜偏差と次の１歩の両脚間床傾斜偏差との関係に相関がない場合には、このように推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを強制的に徐々に０に収束させるのが良い。しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることが分かっているならば、前記関係にある程度の相関があると考えられるので、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを、強制的に徐々に１歩前の両脚間床傾斜偏差に１より小さい正の定数を乗じた値に収束させるようにしても良い。
【０３２９】
さらに、１歩前の推定両脚間床傾斜偏差だけでなく、複数歩前の推定両脚間床傾斜偏差も用いて収束目標値を決定しても良い。また、１歩前や複数歩前の推定足平床傾斜偏差も用いて収束目標値を決定しても良い。
【０３３０】
両脚間床傾斜推定モードがホールドモードである場合はＳ４０８に進み、両脚間床傾斜偏差推定中断器の出力を０にする。従って、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、ホールドモードに遷移する直前の値に保持される。
【０３３１】
尚、ホールドモードは、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖが発散する恐れがある時期およびその精度が低下する恐れがある時期に存在させる。例えば、図４１に示すように、両脚支持期の終了時点を始めとしてそれからしばらく後までの期間に存在させる。
【０３３２】
上記以外の時期、即ち、両脚間床傾斜偏差の推定が正常に実行できる時期では、両脚間床傾斜偏差モードは準備完了モードになる。準備完了モードでは、以下の処理を行う。
【０３３３】
先ずＳ４１０に進み、あらかじめ設定しておいた両脚間床傾斜推定許容最小床反力Ｆｍｉｎ、両脚間床傾斜推定許容比ｒｍｉｎに対し、実各足平床反力が以下の条件を満たすか否かを調べる。
条件４）F1actz < Fmin かつ F2actz < Fmin
条件５）F2actz / ( F1actz ＋F2actz ) < rmin かつθfdberrdiffv > 0
条件６）F1actz / ( F1actz ＋F2actz ) < rmin かつθfdberrdiffv < 0
【０３３４】
条件４に関して補足すると、両脚間床傾斜推定許容最小床反力Ｆｍｉｎは、第１足平および第２足平にこれ以下の床反力しか作用していない場合には、両脚間床傾斜が推定できないと判断される足平床反力設定値であり、通常、ロボット１の自重より小さな値に設定しておく。換言すれば、条件４は、第１足平および第２足平と床との間の圧力が小さ過ぎて両脚間床傾斜が精度良く推定できない状況を意味する。
【０３３５】
一方、条件５および条件６に関しては、推定が理想的に実行されるならば、両脚間床傾斜推定許容比ｒｍｉｎは０でも良い。しかし実際には検出誤差が生じ、それにより推定値が発散する場合がある。それを防ぐために、ｒｍｉｎには、１よりかなり小さい正の値を設定しておく。
【０３３６】
条件５は、第２足平が浮きかかっているのに、積分器入力θｆｄｂｅｒｒｄｉｆｆｖが正、即ち、推定両脚間床傾斜が第２足平が浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り両脚補償角は、ますます第２足平が浮く方向に変化する。
【０３３７】
条件６は、第１足平が浮きかかっているのに、積分器入力θｆｄｂｅｒｒｄｉｆｆｖが負、即ち、推定両脚間床傾斜が第１足平が浮く方向に変化しつつある状況を意味する。このまま推定を続行すると、床形状推定入り両脚補償角は、ますます第１足平が浮く方向に変化する。
【０３３８】
図４０フローチャートの説明に戻ると、条件４、条件５または条件６のいずれかを満足する場合には推定を中断すべきであるので、Ｓ４１２に進んで両脚間床傾斜偏差推定中断器の出力を０にし、それ以外の場合には推定を実行しても問題がないので、Ｓ４１４に進んで前記出力をθｆｄｂｅｒｒｄｉｆｆｖにする。
【０３３９】
条件４、条件５および条件６の式は、以上に述べた状況を表わしていれば良い。従って、実第ｎ足平床反力Ｚ成分（鉛直成分）Ｆｎａｃｔｚの代わりに、実第ｎ足平床反力の力成分Ｆｎａｃｔの内の足平に垂直な成分Ｆｎａｃｔ’を用いても良い。
【０３４０】
この実施の形態は上記の如く構成したので、従来技術では困難であった床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができる。
【０３４１】
さらには、床形状の推定結果を基に足平軌道を修正することにより、床形状が想定していた形状と異なっていても、その影響を吸収して望み通りの床反力を発生させることができる。
【０３４２】
特に、複合コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれに起因する床反力の定常偏差を解消することができる。
【０３４３】
さらに、推定精度が低下する恐れがある状況あるいは推定値が発散する恐れがある状況では床形状推定を中断するように構成したので、前記した作用効果に加え、推定値が不適正なものとなることがない。
【０３４４】
尚、この実施の形態に係る脚式移動ロボットの床形状推定装置は上記した手法で床形状を推定すれば足り、推定値に基づいて床反力を操作して目標位置姿勢などを制御することは必須ではない。
【０３４５】
次いで、この発明の第２の実施の形態に係る脚式移動ロボットの床形状推定装置を説明する。
【０３４６】
第２の実施の形態は、第１の実施の形態を簡易化したものであり、その全体構成を図４２に示す。第１の実施の形態とは、床形状推定器への入力と床形状推定器内での処理が異なる。それ以外は第１の実施の形態と同様である。
【０３４７】
第２の実施の形態においては、詳細は以下に説明するが、簡単に言えば、第１の実施の形態の機構コンプライアンスモデル１３４を近似してから、ブロック線図を等価変換するようにした。
【０３４８】
第２の実施の形態に係る床形状推定器の処理を説明する前に、先ず、用語の定義と、第２の実施の形態で用いられるコンプライアンスモデルの説明を行う。
【０３４９】
図４３の上部に示すように、機構変形補償入り修正目標足平位置姿勢と目標上体位置姿勢の相対関係を満足するように関節角を変位制御し、同図中部に示すように、目標上体姿勢から上体傾斜偏差θｅｒｒだけ傾けてロボット１の上体２４を空中に支持し、さらに同図下部に示すように、目標各足平床反力に相当する外力を足平に作用させたと仮定したときに、そのような状況にある足平内の目標第１足平床反力中心点Ｑ１"'を始点、目標第２足平床反力中心点Ｑ２"'を終点とするベクトルの前記Ｖ方向まわりの傾きを「理想変形時両脚間傾斜」と呼ぶ。
【０３５０】
また、この仮定における第ｎ足平の姿勢傾斜を「理想変形時第ｎ足平傾斜」と呼ぶ。上体傾斜偏差、両脚補償角、各足平補償角、推定両脚間床傾斜偏差、推定第１足平床傾斜偏差および推定第２足平床傾斜偏差がすべて０であるならば、理想変形時両脚間傾斜は想定両脚間床傾斜に一致し、理想変形時第ｎ足平傾斜は想定第ｎ足平床傾斜に一致する。
【０３５１】
想定両脚間床傾斜に推定両脚間床傾斜偏差を加算したものを「推定両脚間床傾斜」と呼ぶ。これは、Ｖ方向まわりの角度で表わされる。また、想定第ｎ足平床傾斜に推定第ｎ足平床傾斜偏差を加算したものを「推定第ｎ足平床傾斜」と呼ぶ。理想変形時両脚間傾斜と推定両脚間床傾斜との差を「推定両脚干渉角θｄｂｉｎｔｖ」と呼ぶ。これも、Ｖ方向まわりの角度で表わされる。
【０３５２】
また、理想変形時第１足平傾斜と推定第１足平床傾斜との差を「推定第１足平干渉角θ１ｉｎｔ」と呼ぶ。同様に理想変形時第２足平傾斜と推定第２足平床傾斜との差を「推定第２足平干渉角θ２ｉｎｔ」と呼ぶ。
【０３５３】
推定両脚間床傾斜が実両脚間床傾斜に一致していて、かつ実ロボットの足平の目標各足平床反力中心点が接地していると仮定すると、推定両脚干渉角θｄｂｉｎｔｖを打ち消すように、実ロボットのコンプライアンス機構４２が変形しているはずである。
【０３５４】
さらにこの機構変形に応じて、目標全床反力中心点まわりに各足平床反力の力成分が作るモーメントの和のＶ方向成分が発生するはずである。この成分を「Ｍｄｂｅｓｔｍｖ」と呼ぶ。
【０３５５】
そこで、推定両脚干渉角θｄｂｉｎｔｖとＭｄｂｅｓｔｍｖとの関係をあらかじめ調べておき、記憶しておく。この関係は、実ロボットのコンプライアンス機構４２の特性の内で、両脚干渉角に関する特性だけを抽出したものであり、それ故に、「両脚間機構コンプライアンスモデル」１３４ａ（図４４に示す）と呼ぶ。
【０３５６】
推定第ｎ足平床傾斜が実第ｎ足平床傾斜に一致していて、かつ実ロボットの第ｎ足平がべたに接地していると仮定すると、推定第ｎ足平干渉角θｎｉｎｔを打ち消すように、実ロボットのコンプライアンス機構４２が変形しているはずである。
【０３５７】
さらにこの機構変形に応じて、目標第１足平床反力中心点まわりに第１足平床反力モーメントが発生するはずである。このモーメントを「Ｍ１ｅｓｔｍ」と呼ぶ。同様に目標第２足平床反力中心点まわりに第２足平床反力モーメントが発生するはずである。このモーメントを「Ｍ２ｅｓｔｍ」と呼ぶ。
【０３５８】
そこで、推定第ｎ足平干渉角のＸ成分θｎｉｎｔｘと前記モーメントのＸ成分Ｍｎｅｓｔｍｘとの関係をあらかじめ調べておき、記憶しておく。この関係は、実ロボットのコンプライアンス機構４２の特性の内で、足平干渉角に関する特性だけを抽出したものであり、それ故に、「足平機構コンプライアンスモデル」と呼ぶ。第１足平Ｘ成分用足平機構コンプライアンスモデルを図４５に記載の１３４ｂに示す。
【０３５９】
次に、第２の実施の形態における床形状推定器１３０ａの構成を説明する。
【０３６０】
第２の実施の形態における床形状推定器１３０ａは、両脚間床傾斜偏差推定部１３８ａと足平床傾斜偏差推定部１４４ａとから構成される。両脚間床傾斜偏差推定部１３８ａの処理機能ブロック図を図４４に示す。足平床傾斜偏差推定部１４４ａの処理機能ブロック図を図４５に示す。
【０３６１】
図４５では、推定第１足平床傾斜偏差Ｘ成分に関する処理機能ブロック図のみを示すが、推定第１足平床傾斜偏差Ｙ成分、推定第２足平床傾斜偏差Ｘ成分、推定第２足平床傾斜偏差Ｙ成分についても同様の処理を行う。
【０３６２】
先ず、第２の実施の形態に係る両脚間床傾斜偏差推定部１３８ａの処理を図４４を参照して説明する。
【０３６３】
先ず、上体傾斜偏差θｅｒｒをＶ方向成分とそれと直交する方向の成分に分解したときのＶ方向成分θｅｒｒｖを求める。
【０３６４】
次に、目標歩容の目標第１足平床反力中心点Ｑ１を始点、目標第２足平床反力中心点Ｑ２を終点とするベクトルの傾斜を求め、求めた傾斜から想定両脚間床傾斜を減じたものを想定両脚干渉角θｄｂｉｎｔｃｍｄｖとする。
【０３６５】
次に、次式により、前述の推定両脚干渉角θｄｂｉｎｔｖを求める。

【０３６６】
ここで、θｆｄｂｅｓｔｍｖは、第１の実施の形態でも述べたように、推定両脚間床傾斜偏差である。また、θｄｂｖ’は、床形状推定入り両脚補償角であり、図４３に示すように、両脚補償角θｄｂｖと推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖの和である。
【０３６７】
床形状推定入り両脚補償角θｄｂｖ’および推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、それから今回値を求めるものであるから、この処理では記憶しておいた直前の値を用いる。
【０３６８】
尚、両脚が接地する想定時刻よりも前から両脚間床傾斜偏差を推定したい場合において、より精度を高めるために、想定両脚干渉角θｄｂｉｎｔｃｍｄｖを加算することが望ましいが、想定両脚干渉角θｄｂｉｎｔｃｍｄｖの加算および想定両脚干渉角θｄｂｉｎｔｃｍｄｖを算出する構成（図４４において点線で囲まれる構成）は必ずしも必須ではない。
【０３６９】
次に両脚間機構コンプライアンスモデル１３４ａを用いて、推定両脚干渉角θｄｂｉｎｔｖによって発生すると予想される前記Ｍｄｂｅｓｔｍｖを求める。
【０３７０】
一方、目標全床反力中心点まわりに実各足平床反力の力成分が作るモーメントの和のＶ方向成分Ｍｆ１ｆ２ａｃｔｖ（複合コンプライアンス制御の式１２および式１３で求められるモーメントのＶ方向成分）を求める。
【０３７１】
次に、前記Ｍｆ１ｆ２ａｃｔｖと前記Ｍｄｂｅｓｔｍｖとの差を求めることにより、両脚モーメント推定偏差Ｍｄｂｅｒｒを得る。もし、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖが実両脚間床傾斜偏差に一致しているならば、両脚モーメント推定偏差Ｍｄｂｅｒｒは０になる。逆に、もし両脚モーメント推定偏差Ｍｄｂｅｒｒが０ならば、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは実両脚間床傾斜偏差に一致する。
【０３７２】
次に、両脚間床傾斜偏差推定中断器１４０ａにおいて、両脚間床傾斜偏差の推定を実行すべきか中断すべきかを判断する。両脚間床傾斜偏差推定中断器１４０ａは、両脚間床傾斜偏差の推定を実行すべきと判断したならば、両脚間床傾斜偏差推定中断器１４０ａへの入力である前記Ｍｄｂｅｒｒをそのまま出力し、中断すべきと判断したならば、０を出力する。両脚間床傾斜偏差推定中断器１４０ａに関する詳細は後述する。
【０３７３】
次に、両脚間床傾斜偏差推定中断器１４０ａの出力を積分定数がＫｄｂｅｓｔｍ’である積分器１４２ａで積分し、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを得る。
【０３７４】
以上が、第２の実施の形態における両脚間床傾斜偏差推定部１３８ａの処理である。
【０３７５】
さらに、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、図４２に示すように、両脚補償角θｄｂｖに加算され、床形状推定入り両脚補償角θｄｂｖ’として修正目標足平位置姿勢算出部１１４ｇへ入力される。
【０３７６】
このような処理を毎制御周期に繰り返し実行することにより、両脚モーメント推定偏差Ｍｄｂｅｒｒが０に収束するように、積分器出力である推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖが変化する。
【０３７７】
ところで前述したように、両脚モーメント推定偏差Ｍｄｂｅｒｒが０になると、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖが実際の両脚間床傾斜偏差に一致する。従って、しばらく経つと、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、実両脚間床傾斜偏差に一致する。即ち、この処理によって、両脚間床傾斜偏差が推定される。
【０３７８】
尚、両脚間機構コンプライアンスモデル１３４ａが線形であるならば、その処理系の実両脚間床傾斜偏差から推定両脚間床傾斜偏差までの閉ループ伝達関数は、１次遅れになる。
【０３７９】
その時定数Ｔは、両脚間機構コンプライアンスモデル１３４ａにおける入力（推定両脚干渉角）に対する出力（前記モーメントＭｄｂｅｓｔｍｖ）の比（絶対値）をＣとすると、次式で求められる。

【０３８０】
第１の実施の形態の積分ゲインＫｄｂｅｓｔｍに対して、第２の実施の形態の積分ゲインＫｄｂｅｓｔｍ’を

とすることにより、第１の実施の形態とほぼ同一の特性を持つ両脚間床傾斜偏差推定部１３８ａが得られる。
【０３８１】
次に、足平床傾斜偏差推定部１４４ａの処理を図４５を参照して説明する。
【０３８２】
図４５では、推定第１足平床傾斜偏差Ｘ成分に関する処理機能ブロック図のみを示すが、推定第１足平床傾斜偏差Ｙ成分、推定第２足平床傾斜偏差Ｘ成分、推定第２足平床傾斜偏差Ｙ成分についても同様の処理を行う。
【０３８３】
先ず、目標歩容の第１足平傾斜角を求め、これから想定第１足平床傾斜を減じたもののＸ成分を、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘとする。次に、次式により前述の推定第１足平干渉角のＸ成分θ１ｉｎｔｘを求める。

【０３８４】
ここで、θｆ１ｅｓｔｍｘは、第１の実施の形態でも述べたように、推定第１足平床傾斜偏差Ｘ成分である。また、θ１ｘ’は、床形状推定入り第１足平Ｘ補償角であり、図４２に示すように、第１足平Ｘ補償角θ１ｘと推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘの和である。
【０３８５】
床形状推定入り第１足平Ｘ補償角θ１ｘ’および推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは、それから今回値を求めるものであるから、この処理では記憶しておいた直前の値を用いる。
【０３８６】
尚、第１足平が床にべたに接地する想定時刻よりも前から第１足平床傾斜偏差を推定したい場合において、より精度を高めるために、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘを加算することが望ましいが、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘの加算および想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘを算出する構成（図４５において点線で囲われる構成）は必ずしも必須ではない。
【０３８７】
次に、前記の足平機構コンプライアンスモデル１３４ｂを用いて、推定第１足平干渉角Ｘ成分θ１ｉｎｔｘによって発生すると予想される前記Ｍ１ｅｓｔｍのＸ成分Ｍ１ｅｓｔｍｘを求める。
【０３８８】
次に、検出された実第１足平床反力モーメントＭ１ａｃｔｘと前記Ｍ１ｅｓｔｍｘとの差を求めることにより、第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘを得る。
【０３８９】
もし、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘが実第１足平床傾斜偏差に一致しているならば、第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘは０になる。逆に、もし第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘが０ならば、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは実第１足平床傾斜偏差に一致する。
【０３９０】
次に、第１足平床傾斜偏差推定中断器において第１足平床傾斜偏差の推定を実行（続行）すべきか中断すべきかを判断する。
【０３９１】
第１足平床傾斜偏差推定中断器は、第１足平床傾斜偏差の推定を実行すべきと判断したならば、第１足平床傾斜偏差推定中断器への入力である第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘをそのまま出力し、中断すべきと判断したならば、０を出力する。尚、足平床傾斜偏差推定中断器１４６ａに関する詳細は後述する。
【０３９２】
次いで、第１足平床傾斜偏差推定中断器の出力を積分定数がＫ１ｅｓｔｍｘ’である積分器で積分し、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを得る。
【０３９３】
以上が、第２の実施の形態における第１足平床傾斜偏差X成分推定部の処理である。
【０３９４】
さらに、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは、図４２に示すように、第１足平Ｘ補償角θ１ｘに加算され、床形状推定入り第１足平Ｘ補償角θ１ｘ’として修正目標足平位置姿勢算出部１１４ｇへ入力される。
【０３９５】
このような処理を毎制御周期に繰り返し実行することにより、第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘが０に収束するように、積分器出力である推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘが変化する。
【０３９６】
ところで前述したように、第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘが０になると、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘが実第１足平床傾斜偏差Ｘ成分に一致する。従って、しばらく経つと、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは、実第１足平床傾斜偏差Ｘ成分に一致する。即ち、この処理によって、第１足平床傾斜偏差Ｘ成分が推定される。
【０３９７】
尚、足平機構コンプライアンスモデル１３４ｂが線形であるならば、その処理系の実第１足平床傾斜偏差Ｘ成分から推定第１足平床傾斜偏差Ｘ成分までの閉ループ伝達関数は、１次遅れになる。
【０３９８】
その時定数Ｔは、足平機構コンプライアンスモデル１３４ｂにおける入力（推定第１足平干渉角Ｘ成分θ１ｉｎｔｘ）に対する出力（前記モーメントＭ１ｅｓｔｍｘ）の比（絶対値）をＣ１とすると、次式で求められる。

【０３９９】
第１の実施の形態の積分ゲインＫ１ｅｓｔｍｘに対し、第２の実施の形態の積分ゲインＫ１ｅｓｔｍｘ’を

とすることにより、第１の実施の形態とほぼ同一の特性を持つ第１足平床傾斜偏差推定器が得られる。
【０４００】
尚、目標足平の向きが目標上体の向きと異なる場合には、上体傾斜を各足平座標系で表わせばよい。
【０４０１】
ここで、第１の実施の形態と第２の実施の形態を比較する。
【０４０２】
第１の実施の形態では床形状推定器１３０への入力として、最終的に修正された目標足平位置姿勢を用いていたが、第２の実施の形態では、それを決定するために中間的に生成される変数を用いていると言える。
【０４０３】
第２の実施の形態は、簡単に言えば、第１の実施の形態のコンプライアンスモデルを、両脚干渉角に関するコンプライアンスモデルである両脚間機構コンプライアンスモデル１３４ａと、足平干渉角に関するコンプライアンスモデルである足平機構コンプライアンスモデル１３４ｂに近似してから、ブロック線図を等価変換したものになっている。
【０４０４】
第２の実施の形態は機構コンプライアンスモデルが簡素化されているので、第１の実施の形態に比較すると、やや推定精度が低下するが、機構コンプライアンスの演算が簡単であるので、制御ユニット２６の負荷を軽減することができる。それ以外は、第２の実施の形態は、第１の実施の形態と異ならない。作用効果も同様である。床形状の推定のみでも良いことも、第１の実施の形態と同様である。
【０４０５】
尚、上記において、両脚支持期になる想定時刻（即ち、遊脚の着地想定時刻）よりも前から両脚間床傾斜偏差の推定を開始することがない場合には、式２５から想定両脚干渉角θｄｂｉｎｔｃｍｄｖの項を削除した次式を用いても良い。なぜなら、目標歩容の両脚支持期には、想定両脚干渉角θｄｂｉｎｔｃｍｄｖは０であるからである。

【０４０６】
目標歩容の第１足平のＹ軸に沿ったエッジ（かかとエッジまたはつまさきエッジ）が接地する時刻よりも前から第１足平床傾斜偏差X成分の推定を開始することがない場合には、式２８から想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘの項を削除した次式を用いても良い。なぜなら、目標歩容の第１足平のＹ軸に沿ったエッジが接地しているときには、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘは０であるからである。

【０４０７】
目標歩容の第１足平のＸ軸に沿ったエッジ（即ち、内エッジまたは外エッジ）が接地する時刻よりも前から第１足平床傾斜偏差Ｙ成分の推定を開始することがない場合には、式２８のｘをｙに置き換えた式の代わりに、その式から想定第１足平干渉角Ｙ成分θ１ｉｎｔｃｍｄｙの項を削除した次式を用いても良い。なぜなら、目標歩容の第１足平のＹ軸に沿ったエッジ（即ち、内エッジまたは外エッジ）が接地しているときには、想定第１足平干渉角Ｙ成分θ１ｉｎｔｃｍｄｙは０であるからである。

【０４０８】
さらに、前記した式１２で求められるモーメントＭｆ１ｆ２ａｃｔｖの代わりに、式１２ａで求められるモーメントＭｆ１ｆ２ａｃｔｖを用いる場合には、前記Ｍｄｂｅｓｔｍｖの代わりに、推定両脚干渉角を打ち消す機構変形によって発生するはずの目標全床反力中心点まわりの全床反力モーメントを用いる。
【０４０９】
さらに、推定床形状偏差を床形状地図情報として記憶しておき、次回同一場所を歩行するときの目標歩容を生成する際の参考にしても良い。
【０４１０】
次いで、第２の実施の形態に係る第１足平床傾斜偏差Ｙ成分推定中断器の処理を説明する。
【０４１１】
第２の実施の形態に係る第１足平床傾斜偏差Ｙ成分推定中断器の処理は第１の実施の形態のそれとほとんど同様であり、具体的には、図３７のθｆ１ｅｒｒｄｉｆｆｙを第１足平床反力モーメント推定偏差Ｙ成分Ｍ１ｅｒｒｙに置き換えれば良い。
【０４１２】
また、両脚間床傾斜偏差推定中断器１４０ａの処理も同様であって、図４０フロー・チャートに示す処理においてθｆｄｂｅｒｒｄｉｆｆｖを両脚モーメント推定偏差Ｍｄｂｅｒｒに置き換えれば良い。
【０４１３】
ここで、両脚間床傾斜偏差推定中断器１４０ａの役割を説明すると、両脚間床傾斜偏差推定中断器１４０ａは、第１足平床傾斜偏差推定中断器と同様に、両脚間床傾斜偏差の推定が正常に実行できるか否かを判断し、正常に実行できないと判断したときには推定を中断する。
【０４１４】
より具体的には、推定両脚間床傾斜偏差の精度が低下する恐れがある状況、および推定両脚間床傾斜偏差が発散する恐れがある状況では、推定を中断するために０を出力し、それ以外の状況では、推定を継続するために両脚間床傾斜偏差推定中断器への入力をそのまま出力する。
【０４１５】
推定両脚間床傾斜偏差は、各足平と床との間の接触圧力が十分にあるときには発散しないが、ある足平が浮きかかっているにもかかわらず、両脚補償角が、その足平が浮く方向に変化しつつある状況で発生する。その発生メカニズムは推定第１足平床傾斜偏差Ｙ成分が発散する場合とほぼ同一なので、これ以上の説明は省略する。
【０４１６】
上記の如く、第２の実施の形態においても、床形状推定値、具体的には各足平の接地面傾斜と両脚接地面間の高低差を同時に、精度良く推定することができる。
【０４１７】
さらに、床形状が想定していた形状と異なっていても、望み通りの床形状を発生されることができると共に、床形状のずれに起因する床反力の定常偏差を解消することができる。
【０４１８】
また、床形状推定精度が低下する恐れがある状況あるいは推定値が発散する恐れがある状況では床形状推定を中断するように構成したので、前記した作用効果に加え、推定値が不適正なものとなることがない。
【０４１９】
尚、第１および第２の実施の形態において、足平２２Ｒ（Ｌ）が浮きそうな状態の判断を、６軸力センサ３４によって検出された実第１足平床反力から前記実床反力中心点位置Ｑ１ａｃｔまたは前記Ｑ１ａｃｔ’を算出し、それに基づいて足平２２Ｒ（Ｌ）がどちらの方向に浮きやすいか（かかとが浮きやすいか、つまさきが浮きやすいか、足平内エッジが浮きやすいか、または足平外エッジが浮きやすいか）を判定した。
【０４２０】
しかし、それに限られるものではなく、図４６に示す如く、足底弾性体４０と足平本体２２ａの間に分布圧センサ２００を配置し、その出力信号に基づいて足平２２Ｒ（Ｌ）がどちらの方向に浮きやすいかを判定しても良い。尚、６軸力センサ３４よりも分布圧センサ２００の方が実床反力による足平の変形をより細緻に推定することができるので、床形状推定の精度を向上させることができる。
【０４２１】
さらには、図４７に示す如く、足平２２Ｒ（Ｌ）に接触センサ２０２を複数個配置（例えば足平の四隅に配置）し、その出力信号に基づいて足平２２Ｒ（Ｌ）がどちらの方向に浮きやすいかを判定しても良い。尚、判定のアルゴリズムとして、接地していない接触センサが増々浮く方向への推定を中断する、あるいは、全ての接触センサが浮いているときも推定を中断することなどが考えられる。
【０４２２】
第１、第２の実施の形態では上記の如く、請求項１項にあっては、少なくとも上体（基体）２４と、前記上体２４に第１の関節（股（腰部）関節１０、１２、１４Ｒ，Ｌ）を介して連結されると共に、その先端に第２の関節（足（足首）関節１８、２０Ｒ，Ｌ）を介して連結される足部（足平）２２Ｒ，Ｌを備えた複数本の脚部（脚部リンク）２を備えると共に、前記足部２２Ｒ，Ｌに接地する床面から作用する床反力に応じて変形するコンプライアンス機構４２を備え、少なくとも前記上体２４に対する前記足部２２Ｒ，Ｌの実位置姿勢を検出し、前記足部２２Ｒ，Ｌの目標位置姿勢に追従させるように前記関節（１０、１２Ｒ，Ｌなど）を駆動する関節駆動手段（電動モータ）を備えた脚式移動ロボットの床形状推定装置において、前記上体２４の鉛直軸方向に対する傾斜を検出する傾斜検出手段（傾斜センサ３６）、前記足部２２Ｒ，Ｌの実位置姿勢および／または前記足部２２Ｒ，Ｌの目標位置姿勢と前記検出された傾斜に基づき、前記足部２２Ｒ，Ｌが接地する床面を含む歩行環境における前記足部２２Ｒ，Ｌの位置姿勢（機構変形補償入り修正目標足平位置姿勢）を算出する対歩行環境足部位置姿勢算出手段（制御ユニット２６、複合コンプライアンス動作決定部１１４、機構変形補償入り修正目標足平位置姿勢算出部１１４ｈ、両脚間床傾斜偏差推定部１３８内の上体傾斜偏差のＶ方向成分θｅｒｒｖの加算器、足平床傾斜偏差推定部１４４内の上体傾斜偏差のＸ成分θｅｒｒｘの加算器）、前記床面から作用する床反力による前記コンプライアンス機構４２の変形が加えられた前記歩行環境における足部の変形位置姿勢（機構変形後の推定各足平位置姿勢）を、前記コンプライアンス機構４２をモデル化して得た機構コンプライアンスモデル１３４を用いて算出する変形足部位置姿勢算出手段（制御ユニット２６、複合コンプライアンスモデル１３４、床形状推定器１３０（特に両脚間床傾斜偏差推定部１３８内のθｆｄｂｖ’を算出するまでの部分、足平床傾斜偏差推定部１４４内のθｆ１ｅｒｒｘ’を算出するまでの部分））、および前記算出された足部の変形位置姿勢に基づき、前記足部２２Ｒ，Ｌが接地するそれぞれの床面の形状（推定第ｎ足平床傾斜（偏差））およびそれらの相対位置関係（推定両脚間床傾斜（偏差））を示す床形状推定値を算出する床形状推定値算出手段（制御ユニット２６、複合コンプライアンス動作決定部１１４、床形状推定器１３０、両脚間床傾斜偏差推定部１３８（特にθｆｄｂｅｒｒｖ算出後の部分）、足平床傾斜偏差推定部１４４（特にθｆ１ｅｒｒｘ算出後の部分））、を備えると共に、前記床形状推定値算出手段は、前記床形状推定値の算出が中断すべき状況にあるか否か判定し、中断すべき状況にないと判定するときは前記床形状推定値の算出を続行する推定中断手段（制御ユニット２６、両脚間床傾斜偏差推定中断器１４０、１４０ａ、足平床傾斜偏差推定部１４４、１４４ａ、足平床傾斜偏差推定中断器１４６、１４６ａ、Ｓ３００からＳ３１６，Ｓ４００からＳ４１４）、を備える如く構成した。
【０４２３】
また、前記推定中断手段は、中断すべき状況にあると判定するときは、前記床形状推定値を保持するか、あるいは所定の値に設定する（Ｓ３０４，Ｓ３０６，Ｓ３０８，Ｓ３１４，Ｓ４０４，Ｓ４０６，Ｓ４０８，Ｓ４１２）如く構成した。
【０４２４】
また、前記推定中断手段は、少なくとも前記ロボットの歩行時期に基づいて中断すべき状況にあるか否か判定する（Ｓ３００，Ｓ４００）如く構成した。
【０４２５】
また、前記推定中断手段は、少なくとも前記足部と床面との接触状況に基づいて中断すべき状況にあるか否か判定する（Ｓ３１２，Ｓ４１０）如く構成した。
【０４２６】
また、前記推定中断手段は、少なくとも前記足部に作用する前記床反力に基づいて中断すべき状況にあるか否か判定する（Ｓ３１２，Ｓ４１０）如く構成した。
【０４２７】
また、前記推定中断手段は、少なくとも前記足部に作用する前記床反力の中心点位置に基づいて前記中断すべき状況にあるか否か判定する（Ｓ３１２，Ｓ４１０）如く構成した。
【０４２８】
さらに、少なくとも前記算出された床形状推定値に基づいて前記足部の目標位置姿勢を修正する修正手段（制御ユニット２６、加算器１３２（１３２ａ，１３２ｂ）、修正足平位置姿勢算出部１１４ｇ、機構変形補償入り修正目標足平位置姿勢算出部１１４ｈ、機構変形量算出部１１４ｎ）、を備えると共に、前記推定中断手段は、前記床形状推定値が発散する恐れがあるときは、前記中断すべき状況にあると判定し（Ｓ３１２，Ｓ４１０）、前記床形状推定値の算出を中断する如く構成した。
【０４２９】
尚、上記した第１、第２の実施の形態において、ブロック線図は演算処理順序を変えるなどの等価変形をしても良い。
【０４３０】
また、この発明を２足歩行ロボットに関して説明してきたが、２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０４３１】
【発明の効果】
請求項１項にあっては、従来技術では困難であった床形状の推定、具体的には各足平の接地面傾斜と両足平接地面間の高低差を同時に、換言すれば複合的に、精度良く推定することができると共に、推定精度が低下する恐れがある状況では床形状推定を中断するので、推定値が不適正なものとなることがない。
【０４３３】
請求項２項にあっては、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【０４３４】
請求項３項にあっては、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【０４３５】
請求項４項にあっては、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【０４３６】
請求項５項にあっては、より的確に中断すべき状況にあるか否かを判定することができ、よって一層推定値が不適正なものとなることがない。
【０４３７】
請求項６項にあっては、前記した効果に加え、複合コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づける、換言すれば床形状のずれによる床反力の誤差に起因する定常偏差を解消することができる。
【図面の簡単な説明】
【図１】この発明の一つの実施の形態に係る床形状推定装置を適用した脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。
【図２】図１に示す脚式移動ロボットの足部の構造を示す説明側面図である。
【図３】図１に示す脚式移動ロボットの制御ユニットの詳細を示すブロック図である。
【図４】先に提案した脚式移動ロボットの制御装置およびこの発明の一つの実施の形態に係る脚式移動ロボットの床形状推定装置の構成および動作を機能的に示すブロック図である。
【図５】図１に示す脚式移動ロボットが平地を歩行するときの運動パターンの一例を示す説明図である。
【図６】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図である。
【図７】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡のタイム・チャートである。
【図８】図５の運動パターンに対応する所定の条件を満たすように設定した目標第１足平床反力中心点軌跡のタイム・チャートである。
【図９】図５の運動パターンに対応する所定の条件を満たすように設定した目標第２足平床反力中心点軌跡のタイム・チャートである。
【図１０】図４と同様に、先に提案した脚式移動ロボットの制御装置の動作を示すフロー・チャートである。
【図１１】図１０フロー・チャートの内の両脚補償角などの演算処理を行う、図４に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第１足平と第２足平に実各足平床反力が作用している状況を示す説明図である。
【図１２】図１１に示す状況における目標全床反力の設定を示す説明図である。
【図１３】図１１に示す状況における目標各足平床反力の分配を示す説明図である。
【図１４】図１１に示す状況における補償全床反力モーメントを示す説明図である。
【図１５】図１１に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線ベクトルＶを示す説明図である。
【図１６】図１１に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標ＺＭＰ）まわりに、所定角度θｄｂｖだけ回転させたときの状態を示す説明図である。
【図１７】図１１に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙだけ回転させたときの状態を示す説明図である。
【図１８】図４の複合コンプライアンス動作決定部の演算処理を示すブロック図である。
【図１９】図１８に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。
【図２０】図１８に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム・チャートである。
【図２１】図２０の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２２】図２１と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２３】両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを示す説明図である。
【図２４】図１８に示す両脚補償角決定部の演算処理を示すブロック図である。
【図２５】図１８に示す各足平の補償角決定部の演算処理を示すブロック図である。
【図２６】図１８に示す機構変形補償入り修正目標足平位置姿勢算出部の演算処理を説明するための説明図である。
【図２７】図２６と同様に、図１８に示す機構変形補償入り修正目標足平位置姿勢算出部の演算処理を説明するための説明図である。
【図２８】図１０フロー・チャートの内の両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。
【図２９】先に提案した制御装置の変形例を示す図１６と同様の説明図で、足平位置の修正動作の別の例を示す説明図である。
【図３０】この発明の一つの実施の形態の係る脚式移動ロボットの床形状推定装置の構成を示す、図１８と同様なブロック図である。
【図３１】図３０に示す装置が床形状を推定するときに使用する定義の説明図である。
【図３２】同様に図３０に示す装置が床形状を推定するときに使用する定義の説明図である。
【図３３】図３０に示す床形状推定器の処理を機能的に示すブロック図である。
【図３４】図３３に示す床形状推定器の機構コンプライアンスモデルの処理を機能的に示すブロック図である。
【図３５】同様に図３３に示す床形状推定器の機構コンプライアンスモデルの処理を機能的に示すブロック図である。
【図３６】この発明の一つの実施の形態における足平床傾斜偏差推定値の発散を説明するための説明図である。
【図３７】この発明の一つの実施の形態における足平床傾斜偏差推定中断器の処理を示すフロー・チャートである。
【図３８】図３７の処理を説明するタイム・チャートである。
【図３９】図３７の処理を説明する説明図である。
【図４０】この発明の一つの実施の形態における両脚間床傾斜偏差推定中断器の処理を示すフロー・チャートである。
【図４１】図４０の処理を説明するタイム・チャートである。
【図４２】この発明の第２の実施の形態に係る脚式移動ロボットの床形状推定装置の構成を示す、図３０と同様なブロック図である。
【図４３】図４２に示す第２の実施の形態で使用する定義の説明図である。
【図４４】第２の実施の形態における床形状推定器の両脚間床傾斜偏差推定部の処理を機能的に示すブロック図である。
【図４５】第２の実施の形態における床形状推定器の足平床傾斜偏差推定部の処理を機能的に示すブロック図である。
【図４６】第１および第２の実施の形態に係る脚式移動ロボットの床形状推定装置で使用するセンサの別の例を示す説明図である。
【図４７】第１および第２の実施の形態に係る脚式移動ロボットの床形状推定装置で使用するセンサのさらに別の例を示す説明図である。
【符号の説明】
１ ２足歩行ロボット（脚式移動ロボット）
２ 脚部（脚部リンク）
１０，１２，１４Ｒ，Ｌ 股関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平（足部）
２４ 上体
２６ 制御ユニット
３４ ６軸力センサ
３６ 傾斜センサ
４２ コンプライアンス機構
６０ 第１の演算装置
６２ 第２の演算装置
１００ 歩容生成器
１０２ 目標床反力分配器
１０４ 姿勢安定化制御演算部
１０８ 実各足平床反力検出器
１１０ ロボット幾何学モデル
１１２ 変位コントローラ
１１４ 複合コンプライアンス動作決定部
１３０，１３０ａ 床形状推定器
１３２（１３２ａ，１３２ｂ） 加算器
１３４，１３４ａ 機構コンプライアンスモデル
１３８，１３８ａ 両脚間床傾斜偏差推定部
１４０，１４０ａ 両脚間床傾斜偏差推定中断器
１４４，１４４ａ 足平床傾斜偏差推定部
１４６，１４６ａ 足平床傾斜偏差推定中断器
２００ 分布圧センサ
２０２ 接触センサ

Claims (6)

1. 少なくとも上体と、前記上体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部を備えると共に、前記足部に接触する床面から作用する床反力に応じて変形するコンプライアンス機構を備え、少なくとも前記上体に対する前記足部の実位置姿勢が、前記上体に対する前記足部の目標位置姿勢に追従するように前記関節を駆動する関節駆動手段を備えた脚式移動ロボットの床形状推定装置において、
   ａ．前記上体の鉛直軸方向に対する傾斜を検出する傾斜検出手段、
   ｂ．前記ロボットの実関節変位、目標関節変位および前記足部の目標位置姿勢の少なくともひとつと前記検出された傾斜に基づき、前記コンプライアンス機構をモデル化して得た機構コンプライアンスモデルを用いて前記足部が接触するそれぞれの床面の形状および前記床面同士の相対位置関係を示す床形状推定値を決定する床形状推定値決定手段、
   を備えると共に、前記床形状推定値決定手段は、
   ｃ．前記床形状推定値の床面形状と実際の床面形状との差の絶対値が増加する可能性があるか否かを判定し、前記絶対値が増加する可能性があると判定した場合、前記床形状推定値の床面形状を保持または所定の値に変更する一方、前記床形状推定値の床面同士の相対位置関係と実際の床面同士の相対位置関係との差の絶対値が増加する可能性があるか否かを判定し、前記絶対値が増加する可能性があると判定した場合、前記床形状推定値の床面同士の相対位置関係を保持または所定の値に変更する推定中断手段、
   を備えることを特徴とする脚式移動ロボットの床形状推定装置。
2. 前記推定中断手段は、少なくとも前記ロボットの歩行時期に基づいて前記絶対値が増加する可能性があるか否か判定することを特徴とする請求項１項記載の脚式移動ロボットの床形状推定装置。
3. 前記推定中断手段は、少なくとも前記足部と床面との接触状況に基づいて前記絶対値が増加する可能性があるか否か判定することを特徴とする請求項１項または２項記載の脚式移動ロボットの床形状推定装置。
4. 前記推定中断手段は、少なくとも前記足部に作用する前記床反力に基づいて前記絶対値が増加する可能性があるか否か判定することを特徴とする請求項１項から３項のいずれかに記載の脚式移動ロボットの床形状推定装置。
5. 前記推定中断手段は、少なくとも前記足部に作用する前記床反力の中心点位置に基づいて前記絶対値が増加する可能性があるか否か判定することを特徴とする請求項４項記載の脚式移動ロボットの床形状推定装置。
6. さらに、
   ｆ．少なくとも前記算出された床形状推定値に基づいて前記足部の目標位置姿勢を修正する修正手段
   を備えることを特徴とする請求項１項から５項のいずれかに記載の脚式移動ロボットの床形状推定装置。

Images