JP4237130B2 - 脚式移動ロボットの制御装置 - Google Patents

Description

この発明は脚式移動ロボットの制御装置、詳しくはその姿勢制御装置に関し、より詳しくは２足歩行ロボットなどの脚式移動ロボットの脚部の動作をコンプライアンス制御し、脚式移動ロボットに作用する床反力を適切に制御するようにしたものに関する。

最も基本的で単純な脚式移動ロボット、より具体的には２足歩行ロボットの制御装置は、目標運動パターン生成装置と関節駆動制御装置から構成される。目標運動パターン生成装置は、少なくとも目標運動パターンを生成する。通常、歩行の運動パターンは、それから動力学的計算によって算出される、即ち、オイラー・ニュートン方程式を解くことによって求められるＺＭＰ軌跡が予め設定しておいた望ましい軌跡になるように生成される。関節駆動制御装置は、歩容生成装置が生成する各関節の変位指令に追従するように各関節を制御する。

ここで、ＺＭＰ（Zero Moment Point)は、運動パターンによって発生する慣性力と重力の合力の床面上の作用点まわりのモーメントが、鉛直軸まわりの成分を除き、０である点を意味する。

尚、その装置においては、歩容生成装置が平らな床面を想定して歩容を生成していたにも関わらず、図４０に示すように、現在、両脚支持期の初期に、前側の足平が予期しない路面を踏んでしまうと、その足平に想定していた以上の過大な床反力が発生し、ロボットが傾斜する。その問題を解決するために、本出願人は、例えば下記の特許文献１において２足歩行の脚式移動ロボットのその種の制御装置を提案している。

そこにおいては、上体傾斜を検出して上体姿勢を復元させるのに必要な復元モーメント要求量を求めると共に、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメント成分を検出し、それを復元モーメント要求量に一致させようと各足平を上下させるように制御している。この実全床反力モーメントは、各実足平床反力の合力が目標全床反力中心点（目標ＺＭＰ）まわりに発生させるモーメントである。

図４０に示すような予期しなかった傾斜があった場合を例にとって先に提案した制御（以下『両脚コンプライアンス制御』という）を説明する。尚、説明のため、この図に示すように各足平に番号を付す。歩容生成部は平らな床面を想定して歩容を生成していたにも関わらず、図４０に示すように、現在、両脚支持期の初期に第１足平が予期しなかった斜面を踏んだため、第１足平に望ましい値よりも大きな足平床反力が発生した瞬間であると仮定する。また、この瞬間ロボットは未だ望ましい姿勢（上体傾斜０）であったと仮定する。

提案した制御装置では、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが検出される。この瞬間では、この実全床反力モーメントは、第１足平床反力の鉛直成分が過大であるため、ロボットを後に転倒させる方向に作用する。

このモーメントを０にしようと、図４１に示すごとく、仮想床面Ａ−Ａ’を想定し、各足平をあたかも仮想床面上に乗せたまま、仮想床面を目標全床反力中心点（目標ＺＭＰ）まわりに適当な角度Δθだけ回転させた位置に各足平の位置を移動させる。

それにより、第１足平床反力の鉛直成分が減少すると共に、第２足平床反力の鉛直成分が増大する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントがほぼ０になる。即ち、床に予期しなかった斜面があっても、両脚コンプライアンス制御が正常に働くので、ロボットを転倒させないで歩行継続させることができる。

しかしながら、この提案技術だけでは両脚支持期に各足平実床反力を制御することがで
きないので、足平の接地点あたりの床形状に予期しない局所的な傾きや凹凸があると、足平の接地性が低下してスピンしやすくなったり、急激な姿勢変化を起こして転倒する場合がある。

例えば、図４２に示すように、両脚支持期に第１足平のつまさきが予期しない突起（段差）を踏んでしまうと、両脚支持期は、第１足平のつまさきが急激に下がりつつある時期であるので、つまさきで床を強く蹴ってしまい、第１足平床反力の鉛直成分が急増する。その結果、目標全床反力中心点（目標ＺＭＰ）まわりに急激に実全床反力モーメントが発生し、最悪の場合、両脚コンプライアンスによって姿勢を復元させようとしても間に合わずに転倒する。

また、両脚支持期で倒れなかったとしても、その直後に第２足平を床から離したとき、目標全床反力中心点（目標ＺＭＰ）は第１足平のかかとにあるにもかかわらず、かかとが浮いているために実全床反力中心点はつまさきにあるので、目標全床反力中心点（目標ＺＭＰ）まわりにロボットを後に倒そうとする実全床反力モーメントが発生し、転倒する。

即ち、この両脚コンプライアンス制御は、長い距離でゆったりと変化する大域的な傾斜やうねりには対応できるが、足平の着地点の局所的な傾きや段差には対応できないと言える。

上記した両脚コンプライアンス制御とは別に、本出願人は、例えば下記の特許文献２において、２足歩行ロボットの足首部にゴムなどのばね特性を持った着地衝撃吸収機構を備えると共に、各足首まわりの実足平床反力モーメント成分を検出し、それを０にしようと各足首を回転させる足首コンプライアンス制御を提案している。

上記した問題点を解決するため、両脚コンプライアンス制御に加えて、この特許文献２で提案する技術（以下『足首コンプライアンス制御』という）を併用することもできる。

その結果、足首コンプライアンス制御によって、図４３に示すように、予期しなかった第１足平床反力モーメントを打ち消す方向に第１足首を回転させ、かかとも床に接地させることができる。従って、その後の片脚支持期になっても上述のようにロボットを転倒させることはない。
特開平５−３０５５８６号公報 特開平５−３０５５８４号公報

しかしながら、上記した両脚コンプライアンス制御および足首コンプライアンス制御を単純に併用するだけでは、２種の制御が干渉しあい、実全床反力と実各足平床反力が望ましい値からずれたり発振してしまう問題があった。

従って、この発明の目的は上記した不都合を解消することにあり、脚式移動ロボットに作用する実床反力を、干渉を生じることなく、容易かつ適切に制御することができる脚式移動ロボットの制御装置を提供することにある。

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

この発明の第３の目的は、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットの姿勢安定化制御を容易にすることができる脚式移動ロボットの制御装置を提供することにある。

この発明の第４の目的は、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットが受ける着地衝撃を低減することができる脚式移動ロボットの制御装置を提供することにある。

さらには、２足歩行ロボットは遊脚を振り出して歩行するが、それによってロボットの鉛直（重力）軸まわりの慣性モーメントが生じ、上体が鉛直軸まわりに回転振動し、それに起因して実各足平床反力モーメントの鉛直成分が振動する。上記振動の振幅が過大になると、実足平床反力モーメントのピーク値が摩擦の限界を超え、その瞬間に足底が滑り、ロボットはスピンする。スピンが大きいと、姿勢安定性を失って転倒する場合もある。従って、上記した制御に加え、そのような振動を低減することが望ましい。

従って、この発明の第５の目的は、脚式移動ロボットに作用する床反力を適切に制御すると共に、床反力モーメントの鉛直成分の振動を低減するようにした脚式移動ロボットの制御装置を提供することにある。

この発明の第６の目的は、さらに、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットの接地性を高め、歩行時のスリップや前述のスピンを防止することができる脚式移動ロボットの制御装置を提供することにある。

この発明の第７の目的は、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットのアクチュエータの負荷を低減することができる脚式移動ロボットの制御装置を提供することにある。

上記の目的を達成するために、請求項１項にあっては、少なくとも基体と、記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標軌跡パターンからなる前記ロボットの目標歩容を生成する歩容生成手段、前記ロボットの姿勢安定化のための補償全床反力を決定する姿勢安定化補償全床反力決定手段、前記足部に作用する足部実床反力を検出する足部実床反力検出手段、前記目標歩容の全床反力と前記補償全床反力を少なくとも前記足部のそれぞれに分配する床反力分配手段、前記分配された目標歩容の全床反力と補償全床反力と前記検出された足部実床反力に基づいて前記目標歩容の足部の位置または姿勢を修正する修正手段、および前記修正された目標歩容の足部の位置または姿勢に基づいて前記第１および第２の関節を変位制御する関節変位制御手段を備える如く構成した。

この請求項および以下の請求項で、「全床反力の目標パターン」は、少なくとも全床反力の中心点軌跡を少なくとも含む、全床反力に関する目標パターンを意味する。また「全床反力」は具体的には、脚部先端を介してロボットに作用する床反力の合力を意味する。尚、「足部」は具体的には、２足歩行ロボットの人間の足に似た足平を意味するが、それ以外の３足以上の脚式移動ロボットの脚部先端を含むと共に、人間の足と異なる爪などから主としてなるものも含む意味で使用する。

請求項２項に係る脚式移動ロボットにあっては、前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの傾き偏差に基づいて求める如く構成した。

請求項３項に係る脚式移動ロボットの制御装置にあっては、前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットのヨーレートに基づいて求める如く構成した。

請求項４項に係る脚式移動ロボットの制御装置にあっては、前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの目標経路からのずれに基づいて求める如く構成した。

請求項５項に係る脚式移動ロボットの制御装置にあっては、前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力の中の所定の成分を零またはその近傍に設定する如く構成した。

請求項６項に係る脚式移動ロボットの制御装置にあっては、前記修正手段は、前記ロボットの姿勢偏差に基づいて前記目標歩容の足部の位置または姿勢をさらに修正する如く構成した。

請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。また、足部の修正量を一層適切に配分することができるので、姿勢安定化のためのより大きな復元力とより高い接地性を得ることができる。

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

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

請求項２項にあっては、姿勢安定化能力を一層向上させることができる。

請求項３項にあっては、前記したと同様の作用効果を得ることができると共に、ロボットのスピンあるいはスリップを防止することができ、姿勢安定化能力を一層向上させることができる。従って、摩擦係数が低い路面を歩行するときも、スピンあるいはスリップを効果的に防止することができ、安定な姿勢で歩行を継続させることができる。

請求項４項にあっては、前記したと同様の作用効果を得ることができると共に、目標経路に沿って安定した姿勢で歩行を継続することができる。

請求項５項にあっては、前記したと同様の作用効果を得ることができると共に、ヨーレートを検出しなくてもロボットのスピンあるいはスリップをかなりの程度まで防止することができ、その意味で姿勢安定化能力を一層向上させることができる。

請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、ロボットに作用する実床反力を一層精度良く制御することができる。

以下、添付図面を参照してこの発明に係る脚式移動ロボットの制御装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。

図１はその脚式移動ロボットの制御装置を全体的に示す概略図である。

図示の如く、２足歩行ロボット１は左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。６個の関節は上から順に、股（腰部）の脚部回旋用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｙ軸まわり）の関節１２Ｒ，１２Ｌ、同ピッチ方向（Ｘ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のロール方向の関節１６Ｒ，１６Ｌ、足首のロール方向の関節１８Ｒ，１８Ｌ、同ピッチ方向の関節２０Ｒ，２０Ｌから構成される。

関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。

上記の構成により、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。

尚、この明細書で後述する上体の位置およびその速度は、上体２４の所定位置、具体的には上体２４の重心位置などの代表点の位置およびその移動速度を意味する。

図１に示す如く、足関節の下方には公知の６軸力センサ４４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。また、上体２４には傾斜センサ６０が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。

図２に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３２が装備されると共に、足底にはゴムなどからなる足底弾性体３４が貼られる。ばね機構３２は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材と、足首関節１８Ｒ（Ｌ）および６軸力センサ４４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材とからなる。

図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けるとバネ機構３２と足底弾性体３４がたわみ、足平は図中に点線で表示された位置・姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は前記した特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。

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

図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ７６を介してＲＡＭ７４内に入力される。

制御ユニット内にはＣＰＵからなる第１、第２の演算装置８０，８２が設けられており、第１の演算装置８０は後述の如く、ＲＯＭ８４に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、ＲＡＭ７４に送出する。また第２の演算装置８２はＲＡＭ７４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出してＤ／Ａ変換器８６とサーボアンプを介して各関節を駆動する電動モータに出力する。

ここで、この明細書および図面で使用する用語について定義する（尚、定義しない用語に関しては、本出願人が前記した技術とは別に提案した出願（特願平８−２１４２６１号）で使用した定義に従う）。

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

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

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

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

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

尚、始めに触れたように、目標ＺＭＰ（Zero Moment Point ）は次のように定義される。即ち、目標運動パターンによって発生する慣性力と重力の合力を動力学的に求め、これが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除き０であるならば、その点を目標ＺＭＰ（Zero Moment Point)という。目標ＺＭＰは、合力の垂直方向力成分が０でない限り、一義的に求められる。以下の説明では、理解しやすくするために、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所が多い。

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

ここで、この発明の課題について再説すると、局所的な傾きや段差に対して先に提案した両脚コンプライアンス制御では良好な姿勢安定性を得ることが困難であると共に、その不都合は足首コンプライアンス制御を用いれば解消することができるが、両者を単純に併用するだけでは干渉しあい、実全床反力と実各足平床反力が望ましい値からずれたり、発振する不都合があった。

その問題点を先の図４０に示す状況で説明すると、第１足平はかかとに予期しなかった過大な床反力を受けているため、第１足首のまわりに過大な実足平床反力モーメントが発生する。足首コンプライアンス制御は、このモーメントを０にしようと第１足首を図４３に示すように回転させる。

しかしながら、足首の回転によって、第１足平のかかと位置が高くなるので、第１足平床反力の鉛直成分が減少する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが変化する。これは、両脚コンプライアンス制御の制御量である実全床反力モーメントが、足首コンプライアンス制御に干渉されることを意味する。

従って、足首コンプライアンス制御による干渉を考慮しないで、両脚コンプライアンス制御を、足首コンプライアンス制御がない場合と同様に働かせると、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが０からずれたり、干渉による振動や発振が生じる。

それを防止する方法のひとつとして、両脚コンプライアンス制御と足首コンプライアンス制御の間の干渉量を求め、それを打ち消すような操作量を加えることによって干渉しないようにすることが考えられるが、歩行中は姿勢が時々刻々と変化し、干渉関係も時々刻々と変化するため、この手法で干渉を回避することは極めて難しい。

また、図４３に示す状況では、第１足平が接触している床は想定していた床よりも登り傾斜なので、第１足平は、目標歩容よりもつまさきを上げるべきである。それにもかかわらず、足首コンプライアンス制御によりつまさきが下がってしまうことは、足首コンプライアンス制御が適切に作用していないとも言える。

以上のように、足首コンプライアンス制御は足平の着地点の局所的な床の傾きや段差には効果があるが、長い距離でゆったりと変化する傾斜やうねりには、却って悪影響を与える場合がある。

従って、実施例に係る装置においては、脚式移動ロボットに作用する床反力、より具体的には、目標全床反力中心点まわりの実全床反力モーメントと、目標各足平床反力中心点まわりの実各足平床反力モーメントを容易かつ適切に制御できるようにした。

また、それによって大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに安定した姿勢でロボットを歩行継続させるようにした。

図４は、この実施例に係る脚式移動ロボットの制御装置（主として図３の第１の演算装置８０に相当）の構成および動作を機能的に示すブロック図である。以下、図４を参照してこの装置の全体構成を概説する。

この装置は歩容生成器を備え、歩容生成器は目標歩容を生成し、出力する。目標歩容は、前述の定義の通り、目標運動パターンと目標床反力パターン、より具体的には目標上体位置・姿勢軌道、目標足平位置・姿勢軌道、目標全床反力中心点（目標ＺＭＰ）軌道および目標全床反力軌道からなる。目標床反力パターンは、このように、目標全床反力中心点軌跡を含む（後述する機構変形補償を行わないならば、目標床反力パターンとしては目標全床反力中心点軌跡だけでも良い）。

この実施例において歩容生成器が出力する目標全床反力は、目標運動パターンに対して動力学的に平衡する全床反力である。従って、目標全床反力中心点は、目標ＺＭＰに一致する。

図５にロボット１が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標ＺＭＰ軌道の床面上軌跡を図６に、タイム・チャートを図７に示す。この歩容の期間に床に接触したままの足平を、第１足平、もう一方を第２足平ということとする。尚、歩容生成器の詳細は先に提案した特願平８−２１４２６１号に詳細に述べられているので、これ以上の説明は省略する。

図４の説明に戻ると、この装置は目標床反力分配器を備え、目標床反力分配器は、目標全床反力中心点（目標ＺＭＰ）と目標足平位置・姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器から歩容のパラメータ（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期・時刻（例えば、現在時刻が両脚支持期の初めから0.1secであるなど）などの情報も必要に応じて取り込む。

図５に示すような歩容に対して、目標床反力分配器は、目標各足平床反力中心点が以下の条件を満足するように設定する。
条件１）目標各足平床反力中心点軌跡は連続である。
条件２）両脚支持期では、目標第１足平床反力中心点はかかとに、目標第２足平床反力中心点はつまさきに存在する。
条件３）このとき目標第１足平床反力中心点と目標第２足平床反力中心点を結ぶ線分上に、目標全床反力中心点が存在する。
条件４）片脚支持期では、目標第１足平床反力中心点は、目標全床反力中心点に一致する。
条件５）片脚支持期の間に、目標第２足平床反力中心点は、つまさきからかかとに移動する。

これら条件を満足する目標第１足平床反力中心点軌跡のタイム・チャートを図８に、目標第２足平床反力中心点軌跡のタイム・チャートを図９に示す。尚、この図では足首（関節１８，２０Ｒ（Ｌ））から足平２２Ｒ（Ｌ）への垂直投影点を原点とし、図１に示すように足平前方向をＸ軸の正の向き、足平左方向をＹ軸の正の向きにとる。

目標床反力分配器は、更に、付随的ではあるが、目標各足平床反力も決定して出力する。目標各足平床反力は、ばね機構３２などのたわみ補償のために必要である。

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

目標第１足平床反力＝目標全床反力＊（目標第２足平床反力中心点と目標ＺＭＰの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離） 目標第２足平床反力＝目標全床反力＊（目標第１足平床反力中心点と目標ＺＭＰの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離）
・・・式１

このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術（特開平６−７９６５７号）に記述されている。

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

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

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

もし、ロボット１が完全剛体であって、変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、足平のばね機構３２および足底弾性体３４のたわみによって生じるロボット全体の位置・姿勢の摂動的な運動は、以下の６自由度に分解できる。

モード１）目標全床反力中心点（目標ＺＭＰ）を中心とした前後軸まわり回転（即ち、左右傾き）
モード２）目標全床反力中心点（目標ＺＭＰ）を中心とした左右軸まわり回転（即ち、前後傾き）
モード３）目標全床反力中心点（目標ＺＭＰ）を中心とした鉛直軸まわり回転（即ち、スピン）
モード４）前後平行移動揺れ
モード５）左右平行移動揺れ
モード６）上下平行移動揺れ

この内で、モード４とモード５は、足平のばね機構３２および弾性体３４が前後左右方向のせん断力を受けてたわむことによって発生するものである。ばね機構３２および足底弾性体３４は剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。

残り４自由度の内、モード３とモード６はこの発明に直接の関連を有しないので、この実施例および後述する第２の実施例ではモード１とモード２に対する制御だけを行うこととする。モード１とモード２に対する制御は、これがないとほとんどの場合ロボットが転倒するので、重要度は極めて高い。尚、第３の実施例でモード３に対する制御を行う。

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

尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を補償全床反力モーメントＭｄｍｄ（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）という。図５に示す如く、ロボットの前方向をＸ軸、左横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直下の床面上の点を原点とした座標系を支持脚座標系と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、ＭｄｍｄのＸ成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体傾斜−目標上体傾斜）θerr のＸ成分をθerrx, Ｙ成分をθerry、これらの時間微分値を(dθerrx / dt)，(dθerry / dt)と記述する。

ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次式の制御則によって決定される。
Mdmdx = - Kthxθerrx - Kwx (dθerrx / dt)
Mdmdy = - Kthyθerry - Kwy (dθerry / dt)
・・・式２
ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、上体傾斜安定化制御ゲインである。

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

図４の説明に戻ると、この装置は実各足平床反力検出器を備え、実各足平床反力検出器は、６軸力センサ４４によって実各足平床反力（その合力が実全床反力）を検出する。更に、関節のエンコーダによって検出される実変位（あるいは変位指令）に基づき、上体に固定された座標系に対する各足平の相対位置・姿勢を算出し、それによって６軸力センサ４４の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、更に、支持脚座標系に変換する。

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

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

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

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

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

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

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

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

先ずＳ１０において装置を初期化し、Ｓ１２を経てＳ１４に進み、タイマ割り込みを待機する。タイマ割り込みは５０ｍｓごとになされ、即ち、制御周期は５０ｍｓである

続いてＳ１６に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときはＳ２２に進むと共に、肯定されるときはＳ１８に進んでタイマｔをイニシャライズし、Ｓ２０に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメータと床反力パラメータ（ＺＭＰ軌道パラメータ）から構成される。

続いてＳ２２に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置・姿勢、目標各足平位置・姿勢、および目標ＺＭＰ位置から構成される。尚、ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意味する。

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

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

続いてＳ２８に進み、前記した傾斜センサ６０などの出力から上体２４の傾斜などロボット１の状態を検出する。

続いてＳ３０に進み、ロボット１の状態などから姿勢を安定化するための（目標全床反力中心点（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを求める。具体的には、上体傾斜が検出されたとき姿勢安定化を図るために前記した式２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを演算する。

続いてＳ３２に進んで実各足平床反力を検出する。これは前記の如く、６軸力センサ４４の出力から検出する。

続いてＳ３４に進み、両脚補償角θｄｂｖおよび各足平補償角θｎｘ（ｙ）を決定する。これは、前記した複合コンプライアンス動作決定部が行う作業である。

その複合コンプライアンス動作決定部の作業について説明する。説明の便宜のため、両脚支持期において図１１に示すように第１足平２２Ｒ（Ｌ）と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用している状況と仮定する。

ここでベクトルＦｎａｃｔは第ｎ足平床反力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎａｃｔの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。

この瞬間の目標全床反力は、図１２に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標ＺＭＰ）における目標全床反力モーメントベクトルＭｓｕｍｒｅｆは垂直である（定義により、目標ＺＭＰは目標全床反力モーメントの水平方向成分が０である点であるから）。

これを式１に従って目標各足平床反力に分配すると、図１３に示すようになる。同図において、ベクトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、Ｍｎａｃｔと同様である。

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

前述の姿勢安定化制御演算部では、ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒｙに基づいて補償全床反力モーメントＭｄｍｄを算出する。この実施例では鉛直軸（Ｚ軸）まわりのスピンを制御しないので、補償全床反力モーメントＭｄｍｄの鉛直軸成分は０である。上体位置の揺れも制御しないので、補償全床反力モーメントの力成分も０である。この状態に対応する補償全床反力モーメントＭｄｍｄを図１４に示す。

姿勢を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと補償全床反力モーメントＭｄｍｄの和の水平成分に追従させれば良い。

一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向成分は０である。従って、前後左右の姿勢傾きを復元させるためには、目標ＺＭＰまわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分に追従させれば良い。

この実施例にあっては複合コンプライアンス動作決定部は、以下の要求をできる限り満足するように足平の位置・姿勢を修正する。
要求１）ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に追従させる。
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。

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

足平の位置・姿勢の修正は、この実施例では次のように行う。
１）目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベクトルＶを求める。Ｖの大きさは１とする。Ｖを図１５に示す。

２）目標第１足平床反力中心点Ｑ１の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移動する。移動した後の点をＱ１’とする。同様に、目標第２足平床反力中心点Ｑ２の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに回転角θｄｂｖだけ回転移動する。移動した後の点をＱ２’とする。

この回転角θｄｂｖを両脚補償角という。始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ１’とする。同様に、始点がＱ２、終点がＱ２’のベクトルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ２’を示す。

３）目標第１足平を、姿勢は変えずにベクトルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。同様に、目標第２足平を、姿勢は変えずにベクトルＱ２Ｑ２’だけ平行移動させる。移動後の目標各足平を図１６に太線で示す。

４）次に、目標第１足平をＱ１’を中心に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同様に、目標第２足平を目標第２足平をＱ２’を中心に前後方向軸（Ｘ軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわりに回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙをそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角という。回転後の目標各足平を図１７に太線で示す。

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

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

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

特性３）両脚補償角θｄｂｖ、各足平Ｘ補償角および各足平Ｙ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。

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

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

概説すると、補償全床反力モーメント分配器において補償全床反力モーメントＭｄｍｄの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部および第ｎ足平Ｘ（Ｙ）補償角決定部において前述の補償角θｄｂｖおよびθｎｘ（ｙ）を決定する。

次に、決定された各種補償角に基づいて修正目標足平位置算出部は、補償された足平位置・姿勢（これを修正目標足平位置・姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置・姿勢算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求め、それらを打ち消すように修正目標足平位置・姿勢をさらに修正する。

以下詳説すると、補償全床反力モーメント分配器は、補償全床反力モーメントＭｄｍｄを、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モーメントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配する。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角（足平上下量）θｄｂｖを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分が作るモーメントの目標値である。

両脚補償モーメントＭｄｍｄｄｂのＶ方向まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクトルＶは複合コンプライアンス動作決定部の説明で定義したベクトルである。Ｖに直交し、鉛直方向にも直交するベクトルをＵとすると、両脚補償モーメントＭｄｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定される。両脚補償角θｄｂｖを操作しても、床反力のＵ方向モーメント成分を発生することはできないからである。

この実施例では補償全床反力モーメントＭｄｍｄの鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成分Ｍｄｍｄｄｂｚも０に設定される。

第１足平補償モーメントＭｄｍｄ１は、第１足平補償角θ１ｘ，θ１ｙを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントである。第１足平補償モーメントＭｄｍｄ１のＸ成分をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄｌｙと記述する。第２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ２ｘ，θ２ｙを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントである。第２足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。

分配は、例えば次のように行う。
Mdmddbv ＝ Wdbx * Mdmdx + Wdby * Mdmdy
・・・式３
Mdmd1x ＝ W1x * (Mdmdx - Wint * Vx * Mdmddbv)
Mdmd1y ＝ W1y * (Mdmdy - Wint * Vy * Mdmddbv)
Mdmd2x ＝ W2x * (Mdmdx - Wint * Vx * Mdmddbv)
Mdmd2y ＝ W2y * (Mdmdy - Wint * Vy * Mdmddbv)
・・・式４

ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数である。VxはベクトルＶのＸ成分の値、ＶｙはベクトルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚補償角を操作することによって発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。

式３と式４の演算処理を行う補償全床反力モーメント分配器のブロック図を図１９に示す。

歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの設定例を図２０に示す。図２０のパターンは、以下の注意点を考慮して決定することが望ましい。

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

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

この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。

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

Mdmddbv*Vx + Mdmd1x + Mdmd2x = Mdmdx
・・・式５
Mdmddbv*Vy + Mdmd1y + Mdmd2y = Mdmdy
・・・式６

これに式３、式４を代入すると、式５は式７に、式６は式８に変換される。
(Wdbx * Mdmdx + Wdby * Mdmdy)*Vx + W1x * (Mdmdx - Wint * Vx * (Wdbx *
Mdmdx + Wdby * Mdmdy)) + W2x * (Mdmdx - Wint * Vx * (Wdbx * Mdmdx +
Wdby * Mdmdy))= Mdmdx
・・・式７
(Wdbx * Mdmdx + Wdby * Mdmdy)*Vy + W1y * (Mdmdy - Wint * Vy * (Wdbx *
Mdmdx + Wdby * Mdmdy)) + W2y * (Mdmdy - Wint * Vy * (Wdbx * Mdmdx +
Wdby * Mdmdy)) = Mdmdy
・・・式８

ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っても、式７と式８が恒等的に成立するためには、式９、式１０、および式１１を同時に満足すれば良い。
Wint = 1 ・・・式９
W1x + W2x =1 ・・・式１０
W1y + W2y =1 ・・・式１１
即ち、以上の状況では式９、式１０および式１１を同時に満足するように、重みを決定すれば良い。

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

同様に、Ｍｄｍｄ２を目標にして第２足平補償角θ２を操作して第２足平の角度を修正しても、実床反力モーメントの増加量がＭｄｍｄ２に較べて不足する場合がある。例えば、図２２のように両脚支持期の後半にロボットが後傾している状況では、θ２によって第２足平のかかとを下げても実床反力は変化しない。

従って、式５、式６を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメントＭｄｍｄに届かない場合がある。このようなことが生じる可能性が高い状況では、式５、式６の左辺の値を１より大きくすべきである。

歩行時の分配用重み変数設定例である図２０では、Ｗｉｎｔを０に設定することによって、図２１の状況のように、両脚補償角θｄｂｖ を操作しても実全床反力モーメントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。

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

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

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

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

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

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

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

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

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

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

例えば、両脚補償角を操作するための分配重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。

Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ成分が０のベクトルをＷｄｂとすると、式３はベクトルＷｄｂとＭｄｍｄの内積になっている。従って、ＭｄｍｄをベクトルＷｄｂ方向成分とその直交成分に分解し、ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄｂの大きさを乗じたものが、式３によって求められるＭｄｍｄｄｂｖであると言える。

この場合のＭｄｍｄｄｂｖを図２３に示す。これは、両脚補償角を操作することによって実全床反力モーメントのＷｄｂ方向成分を制御するフィードバック制御系を構成することを意味する。もし、Ｗｄｂ方向がベクトルＶと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントのＷｄｂ方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。

従って、無駄な動きを減らした場合には、Ｗｄｂ方向をベクトルＶ方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反力モーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、ＷｄｂとＶの内積が１になるように設定する。一部を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積が１より小さくなるように設定する。

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

両脚補償角決定部について更に詳説すると、図２４は両脚補償角決定部の演算処理のブロック図であり、両脚補償角θｄｂｖは図示の如く演算される。

図２４を参照して説明すると、目標第１足平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ２ａｃｔを、次式により求める。

Mf1f2act = PQ1*F1act + PQ2*F2act ・・・式１２
ここで、ＰＱ１は始点がＰ、終点がＱ１のベクトル、ＰＱ２は始点がＰ、終点がＱ２のベクトルである。

また、式１２の代わりに、次式を用いても実際上はほとんど問題がない。
Mf1f2act = PQ1*F1act + PQ2*F2act+ M1act+ M2act ・・・式１２ａ
式１２ａは、目標全床反力中心点まわりに作用する実全床反力モーメントＭｎａｃｔを算出する式になっている。尚、式１２は、目標全床反力中心点まわりに作用する実全床反力モーメントから、目標各足平床反力中心点まわりに作用する実各足平床反力モーメントを減じたものになっている。

次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトルＶは前述の動作説明において図１５に示したＶである。
Mf1f2actv = Mf1f2act・V ・・・式１３

次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィルタに通してＭｆ１ｆ２ａｃｔｖｆｉｌｔを得る。

次に、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを補償用フィルタに通し、それを、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ方向成分Ｍｄｉｆｆｖを得る。

尚、補償用フィルタは、Ｍｄｍｄｄｂｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。

次に、足平ばね機構などの変形による両脚補償モーメントＶ方向成分への影響を打ち消すための両脚機構変形補償角θｆｆｄｂｖを求める。これは、いわゆるフィードフォワード補償である。

具体的には、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプライアンスモデルを用い、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角θｆｆｄｂｖとすれば良い。

両脚機構変形補償角θｆｆｄｂｖは、近似的には次式により求めれば良い。
θffdbv ＝−α＊Mdmddbv ・・・式１４
ここでαは所定の定数である。

最後に次式によって両脚補償角θｄｂｖを得る。ここでＫｄｂは制御ゲインであり、通常、これは正の値に設定する。
θdbv = Kdb * Mdiffv＋θffdbv ・・・式１５

第ｎ足平補償角決定部について説明すると、図２５はその中の第１足平Ｘ補償角決定部の演算処理を示すブロック図であり、第１足平Ｘ補償角θ１ｘは図示の如く演算する。説明は省略するが、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙも同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求めるアルゴリズムだけを説明する。

第１足平床反力モーメントＸ成分Ｍ１ａｃｔｘをローパスフィルタに通してＭ１ａｃｔｆｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを補償用フィルタに通し、それを、Ｍ１ａｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１ｘを得る。両脚補償角決定と同様、補償用フィルタは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周波数応答特性を改善するものである。

次に、両脚補償角決定と同様、足平ばね機構などの変形による第１足平補償モーメントＸ成分への影響を打ち消すための第１足平Ｘ機構変形補償角θｆｆ１ｘを求める。これは、いわゆるフィードフォワード補償である。

具体的には、第１足平補償モーメントＸ方向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプライアンスモデルを用い、第１足平の変形角度を求め、それの極性を反転したものを第１足平Ｘ機構変形補償角θｆｆ１ｘとすれば良い。

第１足平Ｘ機構変形補償角θｆｆ１ｘは、近似的には次式により求めれば良い。
θff1x＝−α1x＊Mdmd1x ・・・式１６
ここでα１ｘは所定の定数である。

最後に次式によって第１足平Ｘ補償角θ１ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、これも正の値に設定する。
θ1x = K1x * Mdiff1x＋θff1x ・・・式１７
尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。

図１８に戻って説明を続けると、修正目標足平位置・姿勢算出部は、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙに基づき、前述の複合コンプライアンス動作の足平位置・姿勢修正手法に従って目標足平位置・姿勢を修正し、修正目標足平位置・姿勢を得る。

機構変形量算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求める。

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

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

機構変形補償は、ばね機構３２や足底弾性体３４の変形によって生じる実足平位置・姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。

上記を前提として図１０フロー・チャートの説明に戻ると、前記の如く、Ｓ３４において上記した補償角を決定する。

図２８はその作業を示すサブルーチン・フロー・チャートである。

同図を参照して説明すると、先ずＳ１００において前記したベクトルＶを求め、Ｓ１０２に進んで分配用重み変数を図２０に示すように設定し、現在時刻ｔでのこれらの値を求める。続いてＳ１０４に進み、式３および式４によって補償全床反力モーメントＭｄｍｄを両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モーメントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進んで各足平補償角θｎｘ（ｙ）を求める。

次いで図１０フロー・チャートのＳ３６に進み、目標各足平床反力に基づいて機構変形補償量を算出し、Ｓ３８に進んで目標足平位置・姿勢を補償角θｄｂｖ，θｎｘ（ｙ）に応じて修正し、更にこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置・姿勢を得る。

次いでＳ４０に進み、上体位置・姿勢と機構変形補償入り修正足平位置・姿勢から関節変位指令（値）を算出し、Ｓ４２に進んで実関節変位を算出された関節変位指令（値）にサーボ制御し、Ｓ４４に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理を繰り返す。

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

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

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

更には、ロボットに作用する床反力、より具体的には目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントと目標各足平中心点まわりの実各足平床反力モーメントを容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に比較して、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。

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

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

また、特開平５−３０５５８６号公報で提案した両脚コンプライアンス制御では実全床反力（各足平床反力の合力）の目標全床反力中心点（目標ＺＭＰ）まわりのモーメント成分を検出し、その値が望ましい値になるように制御していたが、この実施例に係る装置においては、目標各足平床反力中心点に作用する足平床反力の内のモーメント成分を除いた並進力成分の合成が目標全床反力中心点（目標ＺＭＰ）まわりに作用するモーメントを検出し、その値を望ましい値になるように制御するように変更した（尚、この点は先に提案した制御手法であっても良い）。

図２９はこの発明に係る脚式移動ロボットの制御装置の第２の実施例を示す、図１６と同様な説明図である。

第２の実施例に係る装置は補償動作を簡易化した。この実施例においては、各足平の床反力の力成分を操作する足平位置修正動作の手法としては、図１６に示した手法に代え、図２９に示すように、鉛直方向にだけ移動させるようにした。このとき、第１足平鉛直方向移動量Ｚ１と第２足平鉛直方向移動量Ｚ２は、次式によって求める。
Z1 = -線分PQ1 の長さ *θdbv
Z2 = 線分PQ2 の長さ *θdbv
・・・式１８
但し、ここで、θｄｂｖには式１５で求められる値を代入する。

尚、その他の構成は、第１の実施例と異ならない。第２の実施例においては、上記の如く構成したことで、第１の実施例とほぼ同様の作用、効果を得ることができる。

図３０はこの発明に係る脚式移動ロボットの制御装置の第３の実施例を示す、図４と同様な説明図である。また、図３１は、その動作を示す、図１０と同様なフロー・チャートである。

第３の実施例においては、実全床反力モーメントのＺ成分（鉛直軸まわり成分）に対するコンプライアンス制御を追加した。即ち、第３の実施例は、前記したＺ軸まわり固有回転振動に起因する実各足平床反力モーメントのＺ成分を低減するようにした。

第３の実施例にあっては、そのために、第１の実施例に係る装置の構成に、状態検出器、姿勢安定化制御演算部、複合コンプライアンス動作決定部に新たな機能を追加した。

図３０を参照して説明すると、第３の実施例に係る装置においては先ず、ロボット１の上体２４の適宜位置にヨーレートセンサ１００を設けると共に、その出力などを入力し、現在の位置、姿勢、および進行方向を推定する自己位置・姿勢・方向推定器１０２を備えるようにした。ヨーレートセンサ１００は、ロボット１のＺ軸まわりのヨーレート（回転角速度）に応じた信号を出力する。尚、Ｘ，Ｙ軸まわりの回転角速度は、傾斜センサ６０の出力に基づいて算出する。

自己位置・姿勢・方向推定器１０２は、目標足平軌道または実関節角に基づいて一歩前の着地位置に対する今回の着地位置の相対位置および方向を求めると共に、ヨーレートセンサ１００の検出値を積分することにより、ロボットの進行方向を検出する。

さらに、これらの情報に基づき、デッドレコニングによって、目標経路に対するロボット１の位置ずれと方向ずれを推定する。尚、デッドレコニングでは推定値の誤差が発散する傾向があるので、カメラなどを用いて環境を認識して補正しても良い。また、上体２４が鉛直軸に対して傾くと、旋回時のヨーレート検出値は実際値より少ない値となるので、傾斜センサ６０の出力によってヨーレート検出値を補正するのが望ましい。

第３の実施例に係る装置を図３１フロー・チャートを参照して説明すると、Ｓ１０からＳ３０まで進んだ後、Ｓ３０ａにおいて、推定されたロボット１の位置および／または方向のずれに基づき、そのずれが減少するように、補償全床反力モーメントのＺ成分Ｍｄｍｄｚを求める。

以下、図３２に示す経路に沿って歩行する場合を例にとって説明すると、上記した補償全床反力モーメントのＺ成分Ｍｄｍｄｚを求める制御則として次式を用いる。
Mdmdz ＝−Kthzθerrz− Kwz (dθerrz / dt)− Khzｈ
・・・式１９
ここで、θｅｒｒｚは方向のずれ、ｄθｅｒｒｚ／ｄｔはその時間微分値、ｈは経路からの横ずれである。同図にθerrz，ｈを示す。この図の状態では、θｅｒｒｚ，ｈは正である。また、Ｋｔｈｚ，ＫｗｚおよびＫｈｚは経路誘導制御ゲイン（定数）である。

前記した如く、ロボット１が歩行しているとき、足平のばね機構３２や足底弾性体３４のねじれ弾性とロボット１の鉛直軸まわりの慣性力モーメントによって、上体２４が鉛直軸まわりに回転振動する。これをＺ軸まわり固有回転振動と呼ぶ。この振動によって、実各足平床反力モーメントのＺ成分が振動する。ロボット１が歩行しているとき、実足平床反力は、ほぼ、目標足平床反力モーメントと上記振動の和になる。

上記振動の振幅が過大になると、実足平床反力モーメントのピーク値が摩擦の限界を超え、その瞬間に足底が滑り、ロボット１はスピンする。スピンが大きいと、姿勢安定性を失って転倒する場合もある。つまり、Ｚ軸まわり固有回転振動を抑制するだけでも、姿勢安定性を向上させる効果があることがわかる。

尚、単にＺ軸まわり固有回転振動を抑制するだけならば、ヨーレートのずれ（方向ずれの時間微分値）だけから補償全床反力モーメントのＺ成分Ｍｄｍｄｚを決定すれば良い。即ち、式１９において、Ｋｗｚ以外の経路誘導制御ゲインを０に設定すれば良い。

また、第３の実施例においては、複合コンプライアンス動作決定部に新たな機能を追加した。具体的には、実全床反力と実各足平床反力のＺ軸まわりモーメント成分を操作するため、足平２２Ｒ（Ｌ）の位置姿勢の修正動作を、第１の実施例での動作に加えた。複合コンプライアンス動作決定部は、この修正量を決定する。

その決定方法を述べる前に、追加される動作を以下に具体的に説明する。

全床反力と各足平床反力のＺ軸まわりモーメント成分を操作するための足平の位置姿勢の修正は、第１の実施例で述べた複合コンプライアンス動作によって得られた、修正目標各足平位置姿勢（図１６の太線の足平）と修正目標各足平床反力中心点（図１６のＱ１’，Ｑ２’）に対して次のような修正を加えることによって行われる。

１）修正目標第１足平床反力中心点（図１６のＱ１’）の座標を、目標全床反力作用点（目標ＺＭＰ）を回転中心に、Ｚ軸まわりに、ある回転角θｄｂｚだけ回転移動（rotate）する。移動した後の点をＱ１”とする。

同様に、目標第２足平床反力中心点（図１６のＱ２’）の座標を、目標全床反力作用点（目標ＺＭＰ）を回転中心に、Ｚ軸まわりに、ある回転角θｄｂｚだけ回転移動（rotate）する。移動した後の点をＱ２”とする。この回転角θｄｂｚを、Ｚ軸まわり両脚補償角と呼ぶ。

２）始点がＱ１’終点がＱ１”のベクトルをベクトルＱ１’Ｑ１”とする。同様に、始点がＱ２’終点がＱ２”のベクトルをベクトルＱ２’Ｑ２”とする。図３３に上方から見たＱ１”とＱ２”を示す。

３）修正目標第１足平を、姿勢は変えずに、ベクトルＱ１’Ｑ１”だけ平行移動させる。同様に、修正目標第２足平を、姿勢は変えずに、ベクトルＱ２’Ｑ２”だけ平行移動させる。移動後の修正目標各足平を図３３に太線で示す。

４）次に、３）で得られた修正目標第１足平をＱ１”を中心に、鉛直方向軸（Ｚ軸）まわりに回転角θ１ｚだけ回転させる。同様に、３）で得られた修正目標第２足平をＱ２”を中心に、鉛直方向軸（Ｚ軸）まわりに回転角θ２ｚだけ回転させる。回転角θｎｚを第ｎ足平Ｚ補償角と呼ぶ。回転後の修正目標各足平を図３４に太線で示す。

以上の補償動作量が過大でなければ、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。

特性１）Ｚ軸まわり両脚補償角θｄｂｚだけを操作して修正目標各足平位置を移動させると、移動向きと逆向きに実各足平床反力の力成分が発生する。このとき、修正目標各足平実床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。

特性２）第ｎ足平Ｚ補償角だけを操作して目標各足平姿勢を回転させると、目標第ｎ足平床反力中心点に作用する実第ｎ足平床反力のモーメントのＺ軸成分だけが変化し、その他の床反力成分は少ししか変化しない。

特性３）両脚補償角θｄｂｚ、各足平Ｘ補償角、各足平Ｙ補償角、Ｚ軸まわり両脚補償角θｄｂｚおよび各足平Ｚ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる（換言すれば、線形結合が可能になる）。

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

以上の動作におけるＺ軸まわり両脚補償角θｄｂｚおよび各足平Ｚ補償角を、複合コンプライアンス動作決定部において以下のように決定する。

図３５に第３の実施例の複合コンプライアンス部の概要ブロック図を示す。図示の如く、モーメントＺ成分補償動作決定部１０４を追加した。図３６にそのモーメントＺ成分補償動作決定部１０４の詳細を示す。以下、この追加部分を中心に説明する。

図３６に示す如く、このＺ成分補償モーメント決定部１０４においては、補償全床反力モーメント分配器の出力から、両脚補償モーメントＭｄｍｄｄｂのＺ成分Ｍｄｍｄｂｚと各足平補償モーメントＭｄｍｄ１ｚ，Ｍｄｍｄ２ｚを決定すると共に、実各足平床反力と分配された補償モーメントＺ成分（Ｍｄｍｄｄｂｚ，Ｍｄｍｄｎｚ）などから、Ｚ軸まわり両脚補償角θｄｂｚ、第１足平Ｚ補償角θ１ｚおよび第２足平Ｚ補償角θ２ｚを決定する（これは図３８フロー・チャートでＳ３４ａの処理に相当する）。

修正目標足平位置姿勢算出部では、Ｚ軸まわり両脚補償角および各足平Ｚ補償角も含めて、修正足平位置姿勢を幾何学演算によって求める。

以下に追加処理の詳細を説明する。

図３７を参照して補償全床反力モーメントの分配について説明すると、補償全床反力モーメント分配器では、補償全床反力モーメントＭｄｍｄのＺ成分Ｍｄｍｄｚを、両脚補償モーメントＭｄｍｄｄｂのＺ成分Ｍｄｍｄｄｂｚ、第１足平補償モーメントＭｄｍｄ１のＺ成分Ｍｄｍｄ１ｚ、第２足平補償モーメントＭｄｍｄ２のＺ成分Ｍｄｍｄ２ｚ、に分配する処理が追加される。

尚、両脚補償モーメントのＺ成分Ｍｄｍｄｄｂｚは、両脚補償角θｄｂｚを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分Ｆｎａｃｔが作るモーメントのＺ成分の目標値である。

また、第１足平補償モーメントのＺ成分Ｍｄｍｄ１ｚは、第１足平補償角θ１ｚを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントＺ成分（図３４にＭ１で示す）である。同様に、第２足平補償モーメントのＺ成分Ｍｄｍｄ２は、第２足平補償角θ２ｚを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントＺ成分（図３４にＭ２で示す）である。

分配は、例えば、次式を用いて行う。
Mdmddbz ＝Wdbz× Mdmdz
Mdmd1z ＝W1z × Mdmdz
Mdmd2z ＝W2z × Mdmdz
・・・式２０
ここで、Ｗｄｂｚ，Ｗ１ｚ，Ｗ２ｚは歩行時の分配用重み変数である。その分配用重み変数Ｗｄｂｚ，Ｗ１ｚ，Ｗ２ｚの設定例を、図３８に示す。同図の設定パターンは、以下の注意点を考慮して決定される。

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

注意点２）Ｚ軸まわり両脚補償角および各足平Ｚ補償角を操作することによって発生する実床反力モーメントのＺ成分が、なるべく補償全床反力モーメントのＺ成分Ｍｄｍｄｚに近い値になるように、分配用重み変数Ｗｄｂｚ，Ｗ１ｚ，Ｗ２ｚを決定する。

直立時などのように、両脚補償モーメントのＺ成分Ｍｄｍｄｄｂｚ、各足平補償モーメントＺ成分Ｍｄｍｄ１ｚ，Ｍｄｍｄ２ｚを忠実に実各足平床反力に発生させることができる状況では、以下のように設定する。即ち、目標全床反力中心点（目標ＺＭＰ）まわりの実床反力モーメントＭａｃｔのＺ成分を、補償全床反力モーメントＭｄｍｄのＺ成分に一致させる（換言すれば、第１の実施例で述べた複合コンプライアンス動作部に対する要求１を満足する）ために、以下の式２１を可能な限り満足するように重みを設定する。

Wdbz＋ W1z＋ W2z＝ 1
・・・式２１
尚、歩行時では、式２１の左辺が１に近ければ十分である。換言すれば、必ずしも１でなくても良い。

注意点３）遊脚足平が着地する時点でＺ軸まわり両脚補償角θｄｂｚが０でないと、足平着地位置が目標位置からずれ、軌道誘導制御に悪影響を及ぼす場合がある。従って、足平が着地する時点付近で、Ｚ軸まわり両脚補償角用の分配用重み変数Ｗｄｂｚを０にするのが望ましい。

注意点４）遊脚足平が着地する時点でその足平のＺ補償角が０でないと、足平着地向きが目標向きからずれ、軌道誘導制御に悪影響を及ばす場合がある。従って、第１足平が着地する時点付近で、第１足平Ｚ補償角用の分配用重み変数Ｗ１ｚを０にし、第２足平が着地する頃では、第２足平Ｚ補償角用の分配用重み変数Ｗ２ｚを０にするのが望ましい。

また、Ｚ軸まわり両脚補償角θｄｂｚの決定処理が追加される。Ｚ軸まわり両脚補償角θｄｂｚは、両脚補償角θｄｂｖと同様のアルゴリズムによって求められる。違いは、モーメントと角度の向きがＶ方向からＺ方向に変わっただけである。従って、Ｚ軸まわり両脚補償角θｄｂｚを決定する処理のブロック図は、図２４のＶをＺに置き換えることによって得ることができる。

さらに、第１足平補償角θ１ｚ、第２足平補償角θ２ｚの決定処理が追加される。第ｎ足平Ｚ補償角θｎｚは、第１足平Ｘ補償角θ１ｘを求めるアルゴリズムと同様のアルゴリズムによって求めるられる。違いは、ＸがＺに、１がｎに変わっただけである。従って、第ｎ足平Ｚ補償角θｎｚを決定する処理のブロック図は、図２５のＸをＺに、１をｎに置き換えることによって得ることができる。

上記した、図３１のＳ３４ａで行われる処理のサブルーチンを、図３９のＳ２００ないしＳ２０６に示す。

上記に基づいて、修正目標足平位置姿勢算出（図３１フロー・チャートのＳ３８に相当）において、両脚補償角θｄｂｖ、Ｚ軸まわり両脚補償角θｄｂｚ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第１足平Ｚ補償角θ１ｚ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ、第２足平Ｚ補償角θ２ｚに基づき、前述のＺ軸まわり補償動作を追加した複合コンプライアンス動作の足平位置姿勢修正手法に従って目標足平位置姿勢を修正し、修正目標足平位置姿勢を得る。

第３の実施例は上記の如く、実全床反力モーメントのＺ成分（鉛直軸まわりの成分）に対するコンプライアンス制御を追加したので、従前の実施例で述べた作用、効果に加えて、Ｚ軸まわり固有回転振動に起因する実各足平床反力モーメントのＺ成分の振動を抑制することができ、よって脚式移動ロボットの姿勢安定化制御を一層効果的に実現することができる。

さらには、図３２に示した経路誘導制御などを行うときも、目的とする経路に沿って精度良く誘導することができる。

尚、第３の実施例において、ヨーレートセンサ１００、自己位置・姿勢・方向推定器１０２ならびに経路誘導制御演算部などを設けず、単に補償全床反力モーメントのＺ成分Ｍｄｍｄｚを零またはその近傍に固定するだけでも、実全床反力モーメントのＺ成分に対するコンプライアンス制御として、かなり有効である。

第１ないし第３の実施例では上記の如く、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４，Ｓ２６）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第ｎ足平床反力モーメントＭａｃｔｘ，ｙ，ｚ）を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２からＳ３４，Ｓ３４ａ、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８およびＳ２００からＳ２０６）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が回転するように前記目標位置および／または姿勢を修正する足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、および、前記修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４０，Ｓ４２）を備えるように構成した。

また、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボット１の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２，Ｓ３４，Ｓ３４ａ、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８およびＳ２００からＳ２０６）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、および前記修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４２）を備えるように構成した。

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

さらに、前記ロボットに実際に作用する全床反力モーメント（より正確にはモーメント成分PQ1 ＊F1act ＋PQ2 ＊F2act ＋M1act ＋M2act)、または前記ロボットに実際に作用する全床反力のモーメント（PQ1 ＊F1act ＋PQ2 ＊F2act ＋M1act ＋M2act ）から前記足部に作用する床反力モーメント（M1act ＋M2act)を減算して得たモーメント（Ｍｆ１ｆ２ａｃｔ＝PQ1 ＊F1act ＋PQ2 ＊F2act ）のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量（θｄｂｖ，ｚ）を決定する足部移動量決定手段（複合コンプライアンス動作決定部、Ｓ３４，Ｓ３４ａ、両脚補償角決定部、Ｓ１００からＳ１０８，Ｓ２００からＳ２０６）を備え、前記足部位置・姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および／または姿勢を修正するように構成した。

また、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメント（補償全床反力Ｍｄｍｄ）を求め、前記足部回転量決定手段および／または前記足部移動量決定手段は、少なくとも前記検出された実床反力（実各足平床反力）と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部の回転量および／または移動量を決定する（Ｓ３４，Ｓ３４ａ，Ｓ１００からＳ１０８，Ｓ２００からＳ２０６）如く構成した。

また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（θｅｒｒｘ，ｙ）に基づいて求める（Ｓ２８，Ｓ３０ａ）如く構成した。

また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットのヨーレート（θｅｒｒｚ，ｄθｅｒｒｚ／ｄｔ）に基づいて求める（Ｓ２８，Ｓ３０ａ）ように構成した。

また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの目標経路からのずれ、即ち、目標軌道からの横ずれあるいは方向ずれｈに基づいて求める（Ｓ２８，Ｓ３０ａ）如く構成した。

また、前記姿勢安定化補償全床反力モーメント（補償全床反力Ｍｄｍｄ）の中の所定の成分（Ｍｄｍｄｚ）を零またはその近傍に設定するように構成した。

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

また、前記足部回転量決定手段および／または前記足部移動量決定手段は、前記姿勢安定化補償全床反力モーメントが前記複数本の脚部のそれぞれに分配されるように、前記足部の回転量および／または移動量を決定する（Ｓ３４，Ｓ３４ａ，Ｓ１００からＳ１０８，Ｓ２００からＳ２０６）如く構成した。

また、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボット１の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標軌跡パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））からなる前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２，Ｓ２４）、前記ロボットの姿勢安定化のための補償全床反力（補償全床反力Ｍｄｍｄ）を算出する姿勢安定化補償全床反力算出手段（姿勢安定化制御演算部、Ｓ３０，Ｓ３０ａ）、前記足部に作用する実床反力（実各足平床反力）を検出する足部床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、前記目標歩容の全床反力と前記補償全床反力を分配する床反力分配手段（目標床反力分配器、Ｓ３４，Ｓ３４ａ，Ｓ１００からＳ１０４，Ｓ２００からＳ２０２）、前記分配された目標歩容の全床反力と補償床反力と前記検出された足部実床反力に基づいて前記目標歩容の足部の位置および／または姿勢を修正する修正手段（複合コンプライアンス動作決定部、Ｓ３６からＳ３８、補償角決定部、機構変形量算出部、修正目標足平位置・姿勢算出部、機構変形補償入り修正目標足平位置姿勢算出部）、および前記修正された目標足部位置および姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位制御する関節変位制御手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４０，Ｓ４２）を備える如く構成した。

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

尚、上記した第１ないし第３の実施例において、補償動作における足平回転中心点の取り方を更に敷衍すると、補償動作における足平回転中心点は、図１７に示したような目標各足平床反力中心点の代わりに、その瞬間に想定している足底接地領域の中の別の点を回転中心点に設定しても良い。

その設定手法例を以下に列挙する。演算処理は複雑になるが、場合によっては、足平を回転させても、足平実床反力モーメントだけが変化して、足平実床反力の力成分に対しては、より干渉しないようにすることができる。但し、いずれの方式であっても、足平回転中心点の移動軌跡が不連続にならないように注意する。不連続であると、補償動作が急激に変わって足平がばたつくからである。

手法１）各補償モーメントと各足平目標床反力の力成分から、各補償モーメントを発生させたときに実各足平床反力中心点があるべき位置を求め、これを修正目標各足平床反力中心点と呼ぶ。但し、修正目標各足平床反力中心点は、その瞬間に想定している足底接地領域から越えないように設定する。修正目標各足平床反力中心点またはその点の近傍を回転中心点にする。

手法２）想定している足底接地領域の面積中心点を求め、その点またはその点近傍を回転中心点にする。

手法３）実各足平床反力から実各足平床反力中心点を求め、その点またはその点近傍を回転中心点にする。

手法４）手法１から３に挙げた各種回転中心点候補や目標各足平床反力中心点などの内から複数の候補を選び、その加重平均の点を回転中心点とする。

尚、上記において、足底接地領域の圧力分布の一部に負の圧力が発生しない限り、すなわち粘着力が発生しない限り、実足平床反力中心点は必ず足底接地領域内に存在する。

また、上記した第１ないし第３の実施例において、図４にあっては、目標上体位置・姿勢軌道をそのままロボット幾何学モデルに入力する代わりに、目標上体位置・姿勢軌道の水平位置と修正目標足平位置・姿勢軌道から上体高さを、本出願人が先に特願平８−２１４２６０号で提案した上体高さ決定手法を用いて再計算することによって修正し、それをロボット幾何学モデルに入力しても良い。

目標足平位置・姿勢軌道を大幅に修正すると、元の上体高さのままでは脚が伸び切って姿勢がとれなくなるおそれがある。このような場合には、上記再計算を行えば、余程のことがない限り脚が伸び切るおそれはなくなる。

また、上記した第１ないし第３の実施例において、上体が傾くと、床に対する実足平の位置・姿勢がずれ、その結果、実各足平床反力が目標角足平床反力からずれる。このずれを打ち消したい場合には、上体が傾くことによって生じる実足平の位置・姿勢のずれを、両脚補償角θｄｂｖおよび各足平補償角θｎｘ，θｎｙを補正することによって打ち消せば良い。

具体的には、次のように、上体の傾斜偏差のＸ成分θｅｒｒｘ、Ｙ成分θｅｒｒｙと前記ベクトルＶを用いて補正する。即ち、前述の両脚補償角決定方法で得られた両脚補償角θｄｂｖに、次式のΔθｄｂｖを加えたものを改めて両脚補償角θｄｂｖとする。
Δθdbv = -(θerrx * Vx + θerry * Vy )

前述の各足平補償角決定手法で得られた第１足平Ｘ補償角θ１ｘ、第２足平Ｘ補償角θ２ｘからそれぞれθｅｒｒｘを引いたものを改めて第１足平Ｘ補償角θ１ｘ、第２足平Ｘ補償角θ２ｘとする。

同様に各足平補償角決定方法で得られた第１足平Ｙ補償角θ１ｙ、第２足平Ｙ補償角θ２ｙからそれぞれθｅｒｒｙを引いたものを改めて第１足平Ｙ補償角θ１ｙ、第２足平Ｙ補償角θ２ｙとする。

また、上記した第１ないし第３の実施例において、制御精度を高くする必要がない場合には、両脚機構変形補償角θｆｆｄｂｖは零でも良い。即ち、機構変形補償角の演算を省略しても良い。

また、上記した第１ないし第３の実施例において、制御精度を高くする必要がない場合には、第１足平Ｘ機構変形補償角θｆｆ１ｘは零でも良い。即ち、機構変形補償角の演算を省略しても良い。

また、上記した第１ないし第３の実施例において、分配用重み変数は目標歩容のタイミングに合わせて決定されるので、処理が簡単である。但し、実際の床面状況が目標歩容が想定している床面と大きく異なる場合には、着地タイミングがずれるために実床反力の増加量がＭｄｍｄに較べて大きくずれる場合がある。

予期しない床面状況の変化に対するロバスト性を高めるためには、実床反力の力成分から着地と離床の瞬間を検知し、これをトリガにして分配用重み変数を変化させても良い。

また、実各足平床反力から足平の接地状態（例えば各足平実床反力中心点が望みの接地領域から外れていないかなど）を推定し、接地状態が良くないならば重みを下げてモーメントの発生を抑制するなど、実各足平床反力も考慮して適宜分配用重み変数の値を変えても良い。

また、上記した第１ないし第３の実施例においては逆キネマティクスの解の式を直接的に求めておき、式に上体位置・姿勢と足平位置・姿勢を代入するだけで各関節変位を得るようにした。これらの実施例では解があるが、関節の配置によっては直接解が存在しない場合があり、その場合には当然使えない。

そのときは、上体位置・姿勢に対する足平の相対位置・姿勢の摂動に対する関節の摂動の比などをマトリックスの形で表現する逆ヤコビアンまたは疑似逆ヤコビアンを用い、近似的に各関節変位を得ても良い。通常の産業用ロボットなどでも良く用いられる手法である。前記の手法が使えない場合でも、この手法ならば、解を近似的に求めることができる。

また、上記した第１ないし第３の実施例において、ばね機構３２（および足底弾性体３４）自身はこの発明の本質部分ではない。この発明の本質はフィードバック制御部分にあり、機構変形補償は付随的なものである。

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

また、上記した第１ないし第３の実施例において、先にも触れた如く、目標歩容が床反力以外の反力（目標対象物反力）を環境から受けるように想定し、目標ＺＭＰの定義を、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、それが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標ＺＭＰとしても良い。

また、本出願人は、特開平５−３３７８４９号公報において、目標全床反力中心点をそのままに、目標運動パターンだけを修正し、修正された目標運動パターンのＺＭＰと目標全床反力中心点にずれを生じさせることによって、姿勢の傾きを復元させる手法を提案している。その手法を併用する場合には、目標全床反力中心点は、目標ＺＭＰに一致しない。

また、この発明を２足歩行ロボットに関して説明してきたが、２足歩行ロボットに限らず、多脚ロボットにも応用することができる。

この発明に係る脚式移動ロボットの制御装置を全体的に示す説明図である。 図１に示す２足歩行ロボットの足部の構造を示す説明側面図である。 図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。 この発明に係る脚式移動ロボットの制御装置の構成および動作を機能的に示すブロック図である。 図１に示す脚式移動ロボットが平地を歩行するときの運動パターンの一例を示す説明図である。 図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図である。 図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡のタイム・チャートである。 図５の運動パターンに対応する所定の条件を満たすように設定した目標第１足平床反力中心点軌跡のタイム・チャートである。 図５の運動パターンに対応する所定の条件を満たすように設定した目標第２足平床反力中心点軌跡のタイム・チャートである。 図４と同様に、この発明に係る脚式移動ロボットの制御装置の動作を示すフロー・チャートである。 図１０フロー・チャートの内の両脚補償角などの演算処理を行う、図４に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第１足平と第２足平に実各足平床反力が作用している状況を示す説明図である。 図１１に示す状況における目標全床反力の設定を示す説明図である。 図１１に示す状況における目標各足平床反力の分配を示す説明図である。 図１１に示す状況における補償全床反力モーメントを示す説明図である。 図１１に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線ベクトルＶを示す説明図である。 図１１に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標ＺＭＰ）まわりに、所定角度θｄｂｖだけ回転させたときの状態を示す説明図である。 図１１に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙだけ回転させたときの状態を示す説明図である。 図４の複合コンプライアンス動作決定部の演算処理を示すブロック図である。 図１８に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。 図１８に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム・チャートである。 図２０の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。 図２１と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。 両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを示す説明図である。 図１８に示す両脚補償角決定部の演算処理を示すブロック図である。 図１８に示す各足平の補償角決定部の演算処理を示すブロック図である。 図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。 図２６と同様に、図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。 図１０フロー・チャートの内の両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。 この発明の第２の実施の例に示す図１６と同様の説明図で、足平位置の修正動作の別の例を示す説明図である。 第３の実施例に係る装置を示す、図１４と同様な説明図である。 第３の実施例に係る装置を示す、図１０と同様なフロー・チャートである。 第３の実施例に係る装置が予定する経路誘導を示す説明上面図である。 第３の実施例に係る装置の動作を示す、図１６と同様な説明図である。 第３の実施例に係る装置の動作を示す、図１７と同様な説明図である。 第３の実施例に係る装置の複合コンプライアンス動作部の演算処理を示す、図１８と同様なブロック図である。 図３５のＺ成分補償モーメント決定部の演算処理を示すブロック図である。 図３６に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。 図３７に示す補償全床反力モーメント分配器の分配重み変数の設定例を示すタイム・チャートである。 図３１フロー・チャートの中のＺ軸まわり両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。 ２足歩行ロボットが予期しなかった傾斜面を歩行するときの説明図である。 図４０に示す２足歩行ロボットに対して先に提案した両脚コンプライアンス制御を行った場合の説明図である。 図４０に類似する、２足歩行ロボットが予期しなかった突起を踏んだときの説明図である。 図４２に示す状況で、先に提案した足首コンプライアンス制御を行ったときの説明図である。

符号の説明

１ ２足歩行ロボット（脚式移動ロボット）
２ 脚部リンク
１０，１２，１４Ｒ，Ｌ 股関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平（足部）
２４ 上体
２６ 制御ユニット
３２ ばね機構
３４ 足底弾性体
４４ ６軸力センサ
６０ 傾斜センサ

Claims (6)

1. 少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、
   ａ．前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標軌跡パターンからなる前記ロボットの目標歩容を生成する歩容生成手段、
   ｂ．前記ロボットの姿勢安定化のための補償全床反力を決定する姿勢安定化補償全床反力決定手段、
   ｃ．前記足部に作用する足部実床反力を検出する足部実床反力検出手段、
   ｄ．前記目標歩容の全床反力と前記補償全床反力を少なくとも前記足部のそれぞれに分配する床反力分配手段、
   ｅ．前記分配された目標歩容の全床反力と補償全床反力と前記検出された足部実床反力に基づいて前記目標歩容の足部の位置または姿勢を修正する修正手段、
   および
   ｆ．前記修正された目標歩容の足部の位置または姿勢に基づいて前記第１および第２の関節を変位制御する関節変位制御手段、
   を備えたことを特徴とする脚式移動ロボットの制御装置。
2. 前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの傾き偏差に基づいて求めることを特徴とする請求項１項記載の脚式移動ロボットの制御装置。
3. 前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットのヨーレートに基づいて求めることを特徴とする請求項１項または２項記載の脚式移動ロボットの制御装置。
4. 前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力を、少なくとも前記ロボットの目標経路からのずれに基づいて求めることを特徴とする請求項１項から３項のいずれかに記載の脚式移動ロボットの制御装置。
5. 前記姿勢安定化補償全床反力決定手段は、前記姿勢安定化補償全床反力の中の所定の成分を零またはその近傍に設定することを特徴とする請求項１項から４項のいずれかに記載の脚式移動ロボットの制御装置。
6. 前記修正手段は、前記ロボットの姿勢偏差に基づいて前記目標歩容の足部の位置または姿勢をさらに修正することを特徴とする請求項１項から５項のいずれかに記載の脚式移動ロボットの制御装置。
   

Images