WO1999054095A1

WO1999054095A1 - Controleur pour robot mobile muni de jambes

Info

Publication number: WO1999054095A1
Application number: PCT/JP1999/002075
Authority: WO
Inventors: Toru Takenaka; Tadaaki Hasegawa; Takashi Matsumoto
Original assignee: Honda Giken Kogyo Kabushiki Kaisha
Priority date: 1998-04-20
Filing date: 1999-04-19
Publication date: 1999-10-28
Also published as: DE69943148D1; EP1120203A4; US6289265B1; EP1120203A1; EP1120203B1

Description

明細書脚式移動ロボットの制御装置技術分野

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

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

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

尚、その装置においては、歩容生成装置が平らな床面を想定して歩容を生成していたにも関わらず、第 4 1図に示すように、現在、両脚支持期の初期に、前側の足平が予期しない路面を踏んでしまうと、その足平に想定していた以上の過大な床反力が発生し、ロボットが傾斜する。その問題を解決するために、本出願人は、例えば特開平 5— 3 0 5 5 8 6号公報において 2足歩行の脚式移動ロボットのその種の制御装置を提案している。

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

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

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

このモーメントを 0にしようと、第 4 2図に示すごとく、仮想床面 A— A' を想定し、各足平をあたかも仮想床面上に乗せたまま、仮想床面を目標全床反力中心点（目標 Z M P ) まわりに適当な角度だけ回転させた位置に各足平の位置を移動させる。

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

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

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

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

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

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

上記した問題点を解決するため、両脚コンプライアンス制御に加えて、この特開平 5— 3 0 5 5 8 4号公報で提案する技術（以下『足首コンプライアンス制御』という）を併用することもできる。

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

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

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

更に、ロボットが歩行中に予期しない段差、例えば、実際の床面が予定した床面より低位にある場合など、に遭遇して上体（基体）の加速度が過大になって着地衝撃が増加することがある。また、ロボットはその機構および質量から決定される、上下方向に変位する固有振動を持つ。その振動は変位量としては僅かであるが、場合によっては接地性を低下させる。従って、ロボットが受ける実床反力の力成分に対してコンプライアンス制御を行うのが望ましい。

従って、この発明の第 2の目的は、ロボットが受ける実床反力の力成分に対してコンプライアンス制御を行って着地衝撃を吸収すると共に、接地性を向上させるようにした脚式移動ロボットの制御装置を提供することにある。

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

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

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

更に、ロボッ卜の実各足平床反力の力成分を独立に制御すれば、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによって、姿勢安定性を向上させてスリップを効果的に防止することができる。また、ロボットの実各足平床反力の力成分を適正に制御することで、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となっている状態なども解消させることができ、関節ァクチユエ一夕の負担を軽減させることができる。この発明の第 6の目的は、ロボッ卜の実各足平床反力の力成分を独立に制御することで、姿勢安定性を向上させてスリップを効果的に防止すると共に、関節ァクチユエ一夕の負担を軽減させるようにした脚式移動ロボットの制御装置を提供 ^ _とあ

更に、この発明の第 7の目的は、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットの了クチユエ一夕の負荷を低減することができる脚式移動ロボッ卜の制御装置を提供することにある。発明の開示

上記の目的を達成するために、この発明にあっては、少なくとも基体と、前記基体に第 1の関節を介して連結されると共に、その先端に第 2の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボッ卜の少なくとも前記足部の目標位置および姿勢を含む運動パターンと、前記ロボッ卜に作用する全床反力の目標パターンを少なくとも含む前記口ボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反カを検出する実床反力検出手段、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量を決定する足部回転量決定手段、前記決定された足部回転量に基づいて前記足部の位置および Zまたは姿勢が回転するように前記目標位置および/または姿勢を修正する第 1の足部位置 ·姿勢修正手段、少なくとも前記検出された実床反力前記足部の補償変位を算出する補償変位算出手段、前記算出された補償変位に基づいて目標足部位置 ·姿勢を修正する第 2の足部位置 ·姿勢修正手段、および前記第 1および第 2の足部位置 ·姿勢手段によって修正された足部の位置 ·姿勢に基づいて前記ロボットの第 1および第 2の関節を変位させる関節変位手段を備える如く構成した。

更には、前記足部回転量決定手段は、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメントを算出し、少なくとも前記算出されたモ一メン卜に基づいて前記足部を回転させる回転量を決定すると共に、前記補償変位算出手段は、前記検出された実床反力から実全床反力の力成分を求めて前記生成された歩容の全床反力の力成分との偏差を算出し、前記算出した偏差に基づいて前記足部の補償変位を算出する如く構成した。

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

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

更には、前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モ一メントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量を決定する足部移動量決定手段を備え、前記第 1の足部位置，姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および Zまたは姿勢を修正する如く構成した。

更には、前記全床反力の目標パターンに付加する姿勢安定化補償全床反カモ一メントを求め、前記足部回転量決定手段および/または前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反カモ一メントに基づいて前記足部の回転量および zまたは移動量を決定する如く構成した。

更には、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求める如く構成した。

更には、前記補償変位算出手段は、前記ロボットの基体加速度を検出する基体加速度検出手段、および前記検出された基体加速度に基づき、前記ロボットの姿勢を安定化するための補償床反力の力成分を算出する補償全床反力力成分算出手段を備え、前記算出された補償全床反の力成分から前記偏差を減算して得た差に基づいて前記足部の補償変位を算出する如く構成した。

更には、前記補償変位算出手段は、前記差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。

更には、前記補償変位算出手段は、前記足部が離床しているとき、前記重みを零にする如く構成した。

更には、前記補償変位算出手段は、前記重みを前記実全床反力の力成分の周波数に応じて可変にする如く構成した。

更には、前記補償変位算出手段は、前記足部の補償変位を重力方向について算出する如く構成した。

更には、前記補償変位算出手段は、前記足部の補償変位を重心と目標全床反力中心点を結ぶ線分の方向について算出する如く構成した。

更には、前記補償全床反力の力成分を零またはその近傍に設定する如く構成した。

更には、前記補償変位算出手段は、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を算出する如く構成した。

更には、前記足部回転量決定手段は、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量を決定すると共に、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および/または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および/または姿勢を修正する如く構成した。更には、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置およびまたは姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および/または姿勢を修正する如く構成した。

更には、前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモーメン卜のいずれかを算出し、少なくとも前記算出されたモーメン卜に応じて前記足部を移動させる移動量を決定する足部移動量決定手段を備え、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量およぴ前記決定された移動量に基づいて前記足部の位置および/または姿勢を修正する如く構成した。

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

更には、前記補償変位算出手段は、前記偏差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。

更には、前記所定の重みが時変特性および/または周波数特性を持つ如く構成した。

上記の如く構成したことにより、脚式移動ロボッ卜に作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。

また、脚式移動 oボッ卜の姿勢安定化制御を容易に実現できると共に、脚式移動ロボッ卜が受ける着地衝撃を低減することができ、脚式移動ロボッ卜の接地性を高め、歩行時のスリップゃスピンを防止することができる。

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

更には、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができ、姿勢安定化制御を容易に実現できると共に、脚式移動ロボッ卜が受ける着地衝撃を低減することができ、脚式移動ロボットの接地性を高め、歩行時のスリップゃスピンを防止することができる。

更には、前記したと同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。

更には、前記したと同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。

更には、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。

更には、前記したと同様の作用効果を得ることができると共に、着地衝撃を一層効果的に吸収することができ、また接地性を一層向上させることができる。更には、前記したと同様の作用効果を得ることができると共に、制御を必要限度に減少させることでァクチユエ一夕の負荷を一層低減することができる。更には、前記したと同様の作用効果を得ることができると共に、一層安定した姿勢を得ることができる。

更には、前記したと同様の作用効果を得ることができると共に、演算量を低減することができる。

更には、前記したと同様の作用効果を得ることができると共に、かなりの程度まで着地衝撃の吸収と接地性の向上を実現することができ、更に構成を簡易にすることができる。

更には、前記したと同様に、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。特に、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節ァクチユエ一夕の負担を軽減させることができる。

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

更には、前記したと同様の作用効果を得ることができると共に、補償変位の分配を一層適正に行うことができる。図面の簡単な説明

第 1図は、この発明に係る脚式移動ロボットの制御装置を全体的に示す説明図

Cあ。

第 2図は、第 1図に示す 2足歩行ロボットの足部の構造を示す説明側面図である。

第 3図は、第 1図に示す 2足歩行ロボッ卜の制御ュニットの詳細を示すプロック図である。

第 4図は、この発明に係る脚式移動ロボッ卜の制御装置の構成および動作を機能的に示すプロック図である。

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

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

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

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

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

第 1 0図は、第 4図と同様に、この発明に係る脚式移動ロボットの制御装置の動作を示すフロー ·チャートである。

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

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

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

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

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

第 1 6図は、第 1 1図に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標 Z M P ) まわりに、所定角度 Θ d b Vだけ回転させたときの状態を示す説明図である。

第 1 7図は、第 1 6図に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度 0 n X , θ n yだけ回転させたときの状態を示す説明図で第 1 8図は、第 4図の複合コンプライアンス動作決定部の演算処理を示すプロック図である。

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

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

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

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

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

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

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

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

第 2 9図は、第 4図の上体加速度制御演算部および第 1 8図の同位相補償変位算出部の動作で、第 1 0図フロー ·チャートの S 3 し S 3 5の処理を示す、第 1 6図と同様の説明図である。

第 3 0図は、第 1 8図の同位相補償変位算出部の処理を説明するブロック図である。

第 3 1図は、第 1 0図の S 3 5の処理を示すサブルーチン · フロー ·チャートで、同様に第 1 8図の同位相補償変位算出部の処理を説明するフロー ·チャートである。

第 3 2図は、第 3 0図で使用する重みの設定例を示す説明図である。

第 3 3図は、第 3 2図と同様の、重みの別の設定例を示す説明図である。第 3 4図は、この発明の第 2の実施の形態に係る脚式移動ロボッ卜の制御装置の構成および動作を機能的に示す、第 4図と同様なブロック図である。

第 3 5図は、第 3 4図と同様に、この発明の第 2の実施の形態に係る脚式移動ロボッ卜の制御装置の動作を示す、第 1 0図と同様なフロー ·チヤ一トである。第 3 6図は、第 3 4図の複合コンプライアンス動作決定部の演算処理を示すブ口ック図である。

第 3 7図は、第 3 6図の内力補償変位算出部の処理を詳細に示すブロック図であ

第 3 8図は、第 3 7図ブロック図の処理を説明する説明図である。

第 3 9図は、第 3 6図の内力補償変位算出部の処理を示すフロー，チャートである。第 4 0図は、第 37図および第 3 9図で使用する重みの設定例を示す説明図でめる。

第 4 1図は、 2足歩行ロボットが予期しなかった傾斜面を歩行するときの説明図である。

第 4 2図は、第 4 1図に示す 2足歩行ロボッ卜に対して先に提案した両脚コンプライアンス制御を行った場合の説明図である。

第 4 3図は、第 4 1図に類似する、 2足歩行ロボッ卜が予期しなかった突起を踏んだときの説明図である。

第 44図は第 4 3図に示す状況で、先に提案した足首コンプライアンス制御を行ったときの説明図である。発明を実施するための最良の形態

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

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

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

関節 1 8 R (L) ， 2 0 R (L) の下部には足平（足部） 22 R, 22 Lが取着されると共に、最上位には上体（基体） 24が設けられ、その内部にマイクロコンピュータからなる制御ュニット 2 6 (後述）などが格納される。上記において股関節（あるいは腰関節）は関節 1 O R (L) , 1 2R (L) , 1 4 R (L) から、膝関節は 1 6 R (L) から、足関節（足首関節）は関節 1 8 R (L) , 2 0 R (L) から構成される。また股関節と膝関節とは大腿リンク 28 R, 2 8 L 、膝関節と足関節とは下腿リンク 3 0 R, 3 0 Lで連結される。上記の構成により、脚部リンク 2は左右の足についてそれぞれ 6つの自由度を与えられ、歩行中にこれらの 6 * 2 = 1 2個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に 3次元空間を歩行させることができる（この明細書で「*」はスカラに対する演算としては乗算を、べクトルに対する演算としては外積を示す）。

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

第 1図に示す如く、足関節の下方には公知の 6軸力センサ 4 4が取着され、力の 3方向成分 F x , F y , F zとモーメントの 3方向成分 M x , M y , M zとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。

また、上体 2 4には傾斜センサ 6 0が設置され、 Z軸（鉛直方向（重力方向） ) に対する傾きとその角速度を検出する。傾斜センサ 6 0は Gセンサおよび角速度センサとから構成され、 Gセンサは上体 2 4の X , Υ, Z軸方向の加速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコ一ダが設けられる。

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

図中に実線で表示された足平 2 2 R ( L ) は、床反力を受けていないときの状態を示す。床反力を受けるとパネ機構 3 2と足底弾性体 3 4がたわみ、足平は図中に点線で表示された位置，姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は前記した特開平 5— 3 0 5 5 8 4号に記載されているので、詳細な説明は省略する。更に、第 1図では図示を省略するが、 2足歩行ロボット 1の適宜な位置にはジョィスティック 6 2が設けられ、外部から必要に応じて直進歩行しているロボットを旋回させるなど歩容に対する要求を入力できるように構成される。第 3図は制御ュニット 2 6の詳細を示すブロック図であり、マイクロ ' コンビユー夕から構成される。そこにおいて傾斜センサ 6 0などの出力は A/D変換器 7 0でデジタル値に変換され、その出力はバス 7 2を介して RAM 7 4に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウン夕 7 6を介して RAM7 4内に入力される。

制御ュニット内には CPUからなる第 1、第 2の演算装置 8 0, 8 2が設けられており、第 1の演算装置 8 0は後述の如く、 ROM8 4に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、 RAM7 4に送出する。また第 2の演算装置 8 2は RAM7 4からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出して DZA変換器 8 6とサーボアンプを介して各関節を駆動する電動モー夕に出力する。

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

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

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

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

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

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

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

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

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

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

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

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

また、第 4 4図に示す状況では、第 1足平が接触している床は想定していた床よりも登り傾斜なので、第 1足平は、目標歩容よりもつまさきを上げるべきである o

それにもかかわらず、足首コンプライアンス制御によりつまさきが下がってしまうことは、足首コンプライアンス制御が適切に作用していないとも言える。以上のように、足首コンプライアンス制御は足平の着地点の局所的な床の傾きや段差には効果があるが、長い距離でゆったりと変化する傾斜やうねりには、却つて悪影響を与える場合がある。

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

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

尚、上記した課題を解決すべく、本出願人は先に脚式移動ロボッ卜の制御装置を提案しており、この発明の目的はその提案技術を更に改良し、実全床反力の力成分、特に重力方向の力成分 F zに対するコンプライアンス制御を行って着地衝撃を吸収すると共に、接地性を向上させるようにした脚式移動ロボットの制御装置を提供することにある。

第 4図は、この実施の形態に係る脚式移動ロボッ卜の制御装置（主として第 3 図の第 1の演算装置 8 0に相当）の構成および動作を機能的に示すブロック図である。以下、第 4図を参照してこの装置の全体構成を概説する。

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

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

第 5図にロボット 1が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標 Z M P軌道の床面上軌跡を第 6図に、タイム .チャートを第 7図に示す。この歩容の期間に床に接触したままの足平を、第 1足平、もう一方を第 2足平ということとする。尚、歩容生成器の詳細は先に提案した特願平 8 - 2 1 4 2 6 1号に詳細に述べられているので、これ以上の説明は省略する。第 4図の説明に戻ると、この装置は目標床反力分配器を備え、目標床反力分配器は、目標全床反力中心点（目標 Z M P ) と目標足平位置，姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器から歩容のパラメータ（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期 ·時刻（例えば、現在時刻が両脚支持期の初めから O. lsecであるなど ) などの情報も必要に応じて取り込む。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

モード 6) 上下平行移動揺れこの内で、モード 4とモード 5は、足平のばね機構 32および弾性体 34が前後左右方向のせん断力を受けて撓むことによつて発生するものである。ばね機構 32および足底弾性体 34は剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。

先に提案した技術では主としてモード 1とモード 2を扱っていたが、この発明に係る実施の形態では、それに加えてモード 6も扱う。尚、理解の便宜のため、重複するが、先の提案技術を説明する。

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

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

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

Mdmdy = - Kthy ^erry - Kwy (d^ erry I dt)

• · ·式 2 ここで、 Kt hx, Kt hy, Kwxおよび Kwyは、上体傾斜安定化制御ゲインである。

後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。第 4図の説明に戻ると、この装置は実各足平床反力検出器を備え、実各足平床反力検出器は、 6軸力センサ 4 4によって実各足平床反力（その合力が実全床反力）を検出する。更に、関節のエンコーダによって検出される実変位（あるいは変位指令）に基づき、上体に固定された座標系に対する各足平の相対位置 ·姿勢を算出し、それによつて 6軸力センサ 4 4の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、更に、支持脚座標系に変換する。

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

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

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

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

要求 1 ) ロボットの位置 ·姿勢制御のために、実全床反力を姿勢安定化制御部が出力する補償全床反力（モーメント M d m d ) と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメント M d m d に追従させる。要求 2 ) 各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。

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

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

先ず S 1 0において装置を初期化し、 S 1 2を経て S 1 4に進み、タイマ割り込みを待機する。タイマ割り込みは 5 O m sごとになされ、即ち、制御周期は 5 0 m sである

続いて S 1 6に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときは S 2 2に進むと共に、肯定されるときは S 1 8に進んでタイマ tをイニシャライズし、 S 2 0に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメ一夕と床反力パラメータ（Z M P軌道パラメ一タ）から構成される。

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

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

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

続いて S 2 8に進み、前記した傾斜センサ 6 0などの出力から上体 2 4の傾斜などロボット 1の状態を検出する。このとき、上体 2 4に作用する実際の X， Y , Z方向の加速度を検出する。前記した課題を解決するため、この実施の形態では特に Z方向の上体加速度（「Gb o dyz」という）を例にとって説明するが、以下の説明は X, Y方向についても同様に妥当する。

続いて S 30に進み、ロボット 1の状態などから姿勢を安定化するための（目標全床反力中心点（目標 ZMP) まわりの）補償全床反力モーメント Mdmd X , Mdmd yを求める。具体的には、上体傾斜が検出されたとき姿勢安定化を図るために前記した式 2に従って補償全床反力モーメント Mdmdx, Mdmdy を演算する。

続いて S 3 1に進んで上体 Z方向加速度制御則に基づいて補償全床反力の力成分を算出するが、これについては後述する。

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

続いて S 34に進み、両脚補償角 0db Vおよび各足平補償角 θηχ (y) を決定する。これは、前記した複合コンプライアンス動作決定部が行う作業である o

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

ここでべクトル F n a c tは第 n足平床反力の力成分を表す。べクトル Mn a c tは第 n足平床反力のモーメント成分を表す。べクトル Mn a c tの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。この瞬間の目標全床反力は、第 1 2図に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標 ZMP) における目標全床反力モーメントべクトル Ms umr e f は垂直である（定義により、目標 ZMPは目標全床反カモ —メン卜の水平方向成分が 0である点であるから）。

これを式 1に従って目標各足平床反力に分配すると、第 1 3図に示すようになる。同図において、ベクトル Fn r e f は目標第 n足平床反力の力成分を表す。ベクトル Mn r e f は目標第 n足平床反力のモーメント成分を表す。ベクトル M n r e f の向きの表現は、 Mn a c tと同様である。説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。

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

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

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

この実施の形態にあっては複合コンプライアンス動作決定部は、以下の要求をできる限り満足するように足平の位置 ·姿勢を修正する。

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

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

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

足平の位置 ·姿勢の修正は、この実施の形態では次のように行う。

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

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

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

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

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

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

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

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

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

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

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

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

概説すると、補償全床反力モーメント分配器において補償全床反力モーメント Mdmdの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部および第 n足平 X (Y) 補償角決定部において前述の補償角 0 dbvおよび 0nx (y) を決定する。

次に、決定された各種補償角に基づいて修正目標足平位置 ·姿勢算出部は、補償された足平位置 ·姿勢（これを修正目標足平位置，姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置 ·姿勢算出部は、目標各足平床反力によって発生が予想されるばね機構 32や足底弾性体 34の変形量を求め、それらを打ち消すように修正目標足平位置 ·姿勢を更に修正する。以下詳説すると、補償全床反力モーメント分配器は、補償全床反力モーメント Mdmdを、両脚補償モーメント Mdmd d b、各足平補償モーメント M d m d 1 x, y， Mdmd 2 x, yに分配する。両脚補償モーメント M d m d d bは、両脚補償角（足平上下量） Sdbvを操作することによって目標全床反力中心点 (目標 ZMP) まわりに各足平床反力の力成分が作るモーメン卜の目標値である ο

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

この実施の形態では補償全床反力モーメント Mdmdの鉛直方向成分が 0なので、 Mdmd d bの鉛直方向成分 Mdmd d b zも 0に設定される。第 1足平捕償モーメント Mdmd 1は、第 1足平補償角 I χ, Θ I yを操作することによって目標第 1足平床反力中心点まわりに発生させたいモーメン卜である。第 1足平補償モーメント Mdmd 1の X成分を Mdmd 1 x、 Y成分を M dmd 1 yと記述する。第 2足平補償モーメント M d m d 2は、第 2足平補償角 Θ 2 X, Θ 2 yを操作することによって目標第 2足平床反力中心点まわりに発生させたいモ一メントである。第 2足平補償モーメント Mdmd 2の X成分を Md md 2 x、 Y成分を Mdmd 2 yと記述する。

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

dmddbv = Wdbx * Mdmdx + Wdby * dmdy

■ · ·式 3 dmdlx = Wlx * (Mdmdx - Wint * Vx * Mdmddbv)

dmdly = Wly * (Mdmdy - Wint * Vy * Mdmddbv)

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

Mdmd2y = W2y * (Mdmdy - Wint * Vy * Mdmddbv)

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

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

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

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

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

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

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

Mdmddbv*Vx + dmdlx + dmd2x = Mdmdx

• · ·式 5 dmddbv*Vy + Mdmdly + Mdmd2y = Mdmdy

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

• · ·式 7

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

• · ·式 8

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

Wint = 1 ■ · ·式 9

Wlx + W2x =1 · · ·式 1 0

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

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

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

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

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

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

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

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

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

注意点 4 ) 足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきであ注意点 5 ) 両脚補償角を操作することによって制御できる実全床反力モーメントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。

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

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

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

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

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

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

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

両脚補償角決定部について更に詳説すると、第 24図は両脚補償角決定部の演算処理のプロック図であり、両脚補償角 0 d b Vは図示の如く演算される。第 24図を参照して説明すると、目標第 1足平床反力中心点 Q 1に作用する F 1 a c tと目標第 2足平床反力中心点 Q 2に作用する F 2 a c t力、目標全床反力中心点 Pのまわりに発生させるモーメント 1 f 2 a c tを、次式により求める。

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

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

Mflf2act = PQl*Flact + PQ2*F2act+ Mlactf M2act · · ·式 1 2 a 式 1 2 aは、目標全床反力中心点まわりに作用する実全床反力モーメント Ma c tを算出する式になっている。尚、式 1 2は、目標全床反力中心点まわりに作用する実全床反力モーメン卜から、目標各足平床反力中心点まわりに作用する実各足平床反力モーメントを減じたものになっている。請求項 5項の記載は、これに基づく。

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

flf2actv = Mflf2act · V ■ · ·式 1 3 次に、 M f 1 f 2 a c t vをローパスフィル夕に通して M f 1 f 2 a c t v f i 1 tを得る。

次に、両脚補償モ一メント V方向成分 Mdmd d b vを補償用フィルタに通し、それを、 Mf l f 2 a c t v f i l t力、ら減じ、偏差モーメント V方向成分 M d i f f vを得る。

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

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

具体的には、両脚補償モーメント V方向成分 Mdmd d b Vと変形量との関係を表す機構コンプライアンスモデルを用い、目標第 1足平床反力中心点 Q 1と目標第 2足平床反力中心点 Q 2を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角 0 f f db vとすれば良い。両脚機構変形補償角 0 f f d b vは、近似的には次式により求めれば良い。 6>ffdbv = - a *Mdmddbv · ■ '式 1 4 ここでひは所定の定数である。

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

^dbv = Kdb i Mdiffv+ 0 ffdbv · · ，式 1 5 第 n足平補償角決定部について説明すると、第 2 5図はその中の第 1足平 X補償角決定部の演算処理を示すブロック図であり、第 1足平 X補償角 0 1 Xは図示の如く演算する。説明は省略するが、第 1足平 Y補償角 0 1 y、第 2足平 X補償角 02 x、第 2足平 Y補償角 02 yも同様に求める。ここでは第 1足平 X補償角 Θ 1 Xを求めるアルゴリズムだけを説明する。

第 1足平床反力モーメント X成分 M 1 a c t xを口一パスフィルタに通して M 1 a c t f i 1 t xを得る。第 1足平補償モーメント X成分 Mdmd 1 xを補償用フィルタに通し、それを、 M 1 a c t f i 1 t Xから減じ、偏差乇一メント M d i f f 1 xを得る。両脚補償角決定と同様、補償用フィルタは、 Mdmd 1 x から実全床反力までの伝達関数の周波数応答特性を改善するものである。

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

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

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

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

θ ΐχ = Klx * Mdifflx+^fflx · · ·式 1 7 尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い第 1 8図に戻って説明を続けると、修正目標足平位置 ·姿勢算出部は、両脚補償角 0 d b v、第 1足平 X補償角 0 1 X、第 1足平 Y補償角 0 1 y、第 2足平 X 補償角 0 2 X、第 2足平 Y補償角 0 2 y、および同位相補償変位（後述）に基づき、前述の複合コンプライアンス動作を拡張した足平位置■姿勢修正手法に従つて目標足平位置 ·姿勢を修正し、修正目標足平位置 ·姿勢を得る。

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

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

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

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

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

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

同図を参照して説明すると、先ず S 1 0 0において前記したべクトル Vを求め S 1 0 2に進んで分配用重み変数を第 2 0図に示すように設定し、現在時刻 t でのこれらの値を求める。続いて S 1 0 4に進み、式 3および式 4によって補償全床反力モーメント M d m dを両脚補償モーメント M d m d d b vと各足平補償モーメント M d m d n x ( y ) に分配し、 S 1 0 6に進んで既述の如く両脚補償角 0 db vを求め、 S 1 0 8に進んで各足平補償角 0 nx (y) を求める。

第 1 0図フロー 'チャートの説明に戻ると、続いて S 3 5に進んで同位補償変位を決定するが、これについては後述する。

続いて S 3 6に進み、目標各足平床反力に基づいて機構変形補償量を算出し、 S 3 8に進んで目標足平位置 ·姿勢を補償角 0 d b v， θ η χ (y) 、および同位相補償変位（後述）に応じて修正し、更にこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置 ·姿勢を得る。

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

ここで、第 1 0図フロー 'チャートの S 3 1および S 35の処理について説明する。

これらは、上体加速度を検出し、それが過大または過少にならないように、補償全床反力の力成分（目標値） F dmdを後述する制御則によって決定し、全足平を同時に同じ向きに平行移動させて実全床反力の力成分の高周波成分を F d m · dの高周波成分に近づける制御である。

この制御を行うと、例えば、歩行中に予期しない段差によってロボット 1が落下して足平 22 Rあるいは Lが床面に強く着地したとき、両足平 22R (L) を僅かに重力方向に引き上げることで、着地衝撃を吸収することができる。

また、ロボットは、歩行中、足平のばね機構 3 2や足底弾性体 34の上下伸縮の弾性とロボット 1の質量によって決定される固有振動を持ち、上体 24が上下に振動する。この振動は変位量としては僅かであるが、実足平床反力の Z方向力成分 F zは大きく変動する。

この振動の振幅が過大になると、足平 22 R (L) と床面との接地性が低下し、場合によってロボット 1はスピンする。この制御はかかる振動を抑制し、スピンを防止して姿勢安定化を向上させる。その意図から、この実施の形態においては、先に提案した装置に、第 4図の下部に示す如く、上体 Z方向加速度制御演算部を追加した。上体 Z方向加速度制御演算部は、傾斜センサ 60によって検出された実上体加速度 Gbo dy (より具体的には例えば、検出された実上体上下加速度 Gbひ d y z ) 、目標歩容の上体加速度（即ち、目標上体加速度） Gb o dy r e f から大きくずれないように補償全床反力の力成分（目標値） Fdmdを求める如くした。これが、 S 3 1の処理に相当する。

以下説明すると、 Fdmdは、例えば、次のように算出する。即ち、 Gb o d y r e f の X成分を Gb 0 d y r e f X, X成分を G b o d y r e f y， Z成分を Gbody r e f zとすると、次式に従って目標値と検出値の偏差 F dm d t mpを算出する。

Fdmd tmpx = -Kgx* (Gb o dyz— Gb o dy r e f z) Fdmd tmpy = -Kgy* (Gb o dyz - Gb o dy r e f z ) Fdmd tmp z = -Kgz * (Gb o dyz— Gb o dy r e f z )

. . . 式 1 8

ここで、 Kgx, Kgy, Kgzは、制御ゲイン（定数）である。 Fdmd t mp x, Fdmd tmpy, F d m d t m p zはそれぞれ、前言己偏差 F d m d t mpの X, Y, Z成分であり、それらの合算して偏差 F dmd tmpとする。続いて、算出した Fdmd tmpを制御特性改善用に適宜設計したフィルタに通し、その出力を F dmdとする。

ここで、目標全床反力中心点を始点、ロボット重心位置を終点とするベクトルを（Jx, J y, J z) とすると、上記した Kgx, Kgy, Kgzの比を、 J x, J y, Jzに一致させると良い。そうすると、 Fdmdもベクトル（Jx, J y, J z ) の向きになる。

従って、 F dmdを床反力として目標全床反力中心点に追加的に発生させても、 F dmdはロボット重心位置に向かって作用するので、ロボット 1を回転させるモーメントが発生せず、ロボット 1の姿勢傾きに影響を及ぼすことがない。尚、 F dmdによって決定される全足平の平行移動量が僅かである場合には、 Kg Xと Kgyを零にしても良い。そうしても、同様に、ロボット 1の姿勢傾きに影響を及ぼすことがないからである。

続いて S 35の処理について説明する。第 29図を参照して概説すると、実全床反力の力成分を操作するため、図中の同位相補償変位を無視して S 38までの処理で得られた修正目標第 1足平位置姿勢（第 29図に細線で示す）を、姿勢を変えずに、ある移動量べクトル L 1だけ平行移動させる。

同時に、修正目標第 2足平位置姿勢も、姿勢を変えずに、ある移動量ベクトル L 2だけ平行移動させる。移動後の修正目標第 1、第 2足平位置姿勢を第 29図に太線で示す。

これが、実全床反力の力成分に対するコンプライアンス制御であり、この明細書では「同位相平行移動動作」といい、移動ベクトル Lnを第 n足平同位相補償変位という（n= 1, 2) 。

即ち、第 1 8図の下部に示す如く、同位相補償変位算出部を設け、補償全床反力の力成分（目標値）と実全床反力の力成分から各足平同位相補償変位を算出し、修正目標足平位置姿勢算出部では、各足平同位相補償変位も含めて修正目標足平位置姿勢を算出するようにした。

第 30図はその作業を更に詳細に示すブロック図であり、第 3 1図は第 30図の作業を説明するフロー ·チャートである。

以下第 3 1図を参照して説明すると、 S 200において実全床反力の力成分 F t 0 t a 1 a c tを求める。即ち、実第 1足平床反力の力成分 F 1 a c tと実第 2足平床反力の力成分 F 2 a c tから次の式（式 1 9) によって実全床反力の力成分 F t 0 t a 1 a c tを算出する。

F t o t a l a c t=F l a c t +F 2 a c t . . . 式 1 9 続いて S 202に進み、 F t 0 t a 1 a c tから目標全床反力（即ち、目標歩容のカ成分 F r e f ) を減算して偏差 F t o t a 1 e r rを求め、 S 204に進んで適宜なフィルタを通して F t 0 t a 1 e r r f i 1 tを得る。

次いで S 206に進み、補償全床反力の力成分（目標値） F dm dを補償用フィル夕に通し、フィルタ通過値から F t o t a l e r r f i l tを減算して差 F t 0 t a 1 d i f f を得る。この補償用フィルタは、 F dm dから実全床反力の力成分までの周波数応答性を改善するものである。

次いで S 208に進み、次式（式 20) によって同位相補償変位べクトル Lを算出する。

L=K l *F t o t a l d i f f . . . . 式 20

ここで、 K 1は制御ゲイン（定数。スカラ量）である。

次いで、算出されたべクトル Lを以下の式によって各足平に配分する。

L 1 =W1 1 *L

L 2=W1 2 *L . . . . 式 2 1

ここで、 Wl 1， Wl 2はアツテネ一夕（重み）であり、例えば第 32図に示すように設定する。

この Wl 1, W1 2は常に 1でも良いが、足平 22R (L) が離床した後は上下に無駄に動くことを避けるために、空中にあるときは（図示の例では片脚支持期に第 2足平側） 0にすると良い。

かく得られた同位相補償変位 L nも含め、第 1 0図フロー ·チャートの S 38 において機構変形補償入り修正目標足平位置姿勢が算出される。

具体的には、両脚補償角 Θ d b V、第 1足平 X補償角 1 X、第 1足平 Y補償角 01 y、第 2足平 X補償角 02 x、第 2足平 Y補償角 02 y、第 1足平同位相補償変位 L l、および第 2足平同位相補償変位 L 2に基づき、前記した同位相平行移動動作を含む複合コンプライアンス動作に従って修正目標足平位置姿勢が算出される。

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

即ち、この実施の形態に係る装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平 5— 305584号公報で提案した足首コンプライアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この実施の形態に係る装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモ一メントを望ましい値に制御するようにした。この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。

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

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

また、ロボッ卜が受ける実床反力の力成分に対してコンプライアンス制御を行うようにしたので、予期しない段差などに遭遇して過大な着地衝撃を生じても、その着地衝撃を吸収することができると共に、接地性を向上させることができてスピンの発生などを防止することができる。

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

ここで、同位相補償変位 Lの各足平への配分について補足すると、式 2 1においては、例えば重み W l 1 , W 1 2を 1 . 0に固定、あるいは遊脚の場合 0にするなど、換言すれば、実全床反力の力成分の周波数に対して固定した。

そのように設定すると、実全床反力の力成分の低周波成分を任意に制御することはできない。何故ならば、例えば、直立時に両足平を重力方向に同位相平行移動させても（即ち、両脚を縮めても）、その瞬間には実全床反力の力成分が減少するが、その直後に上体 2 4は下に移動した時点で再び元の実全床反力に戻ってしまうからである。

それを解消すべく、式 2 0に積分項を加えると、実全床反力の力の低周波成分まで制御することができる。しかし、高速歩行時には、上体高さが目標歩容から大きく変化すると、動バランスを崩すので、積分項を加えることは好ましくないそこで、同位相補償変位 Lの配分量を両足平で相違させても良く、あるいは実全床反力の力成分の周波数に対して可変にしても良い。即ち、周波数成分を持つた重みにしても良い。

それについて以下説明すると、第 33図に示す如く、重み Wl 1, W1 2を実全床反力の力成分の周波数に対して可変に設定する。具体的には、所定値、例えば 5 Hzよりも十分に低い周波数領域にあっては、重み Wl 1, W1 2の比を 1 付近に設定すると共に、それ以上の周波数領域では比が変わるように設定する。例えば、 5 Hzよりも十分に低い周波数領域にあっては、 L I : L 2= 1 ： 1 となるように決定する。即ち、実全床反力の力の低周波成分に対しては、上体 2 4が足平 22 R (L) の伸縮につられて重力方向に大きく動くので、いずれの足平も浮かないように、両足平の値を同一にする。

他方、 5 Hz以上の高周波数領域にあっては、第 29図において、目標全床反力中心点 Pまわりにモ一メン卜が発生しないように、その目標全床反力中心点 P と修正目標第 n足平床反力中心点 Qn' との距離に反比例するように、設定するより具体的には、第 29図において、 L 1 ： L 2 = PQ 2' ： PQ Γ となるように決定する。

尚、前述の式 1 8は、以下のように一般化しても良い。

Fdmd tmpx =— Kgxx* (Gb o dyx— Gb o dy r e f x)

- K g x y * (Gb odyy-Gb o dy r e f y) - K g x z * (Gb odyz-Gb o dy r e f z) Fdmd tmpy = -Kgyx* (Gb o dyx - Gb o dy r e f x)

— Kgyy* (Gb o dyy-Gb ody r e f y) - K g y z * (Gbodyz-Gb o dy r e f z) Fdmd tmp z = -Kgzx* (Gb o dyx— Gb o dy r e f x)

- K g z y * (Gb o dyy-Gb o dy r e f y) - K g z z * (Gb o dyz-Gbo dy r e f z) . . . . 式 22 ただし、 Kgxx， K g x y , K g x z , K g y x, K g y y , Kgy z, K g z x, Kg z y, K g z zは、制御ゲインである。

尚、上記した実施の形態において、上体加速度の検出および補償床反力の力成分（目標値） F dm dを決定する制御則を省略し、 F dm dを零またはその近傍に固定するだけでも、実全床反力の力成分に対するコンプライアンス制御として、かなり有効である。尚、請求項 1 3項はこの記載に基づく。

第 34図は、この発明の第 2の実施の形態に係る脚式移動ロボッ卜の制御装置 (主として第 3図の第 1の演算装置 8 0に相当）の構成および動作を機能的に示すプロック図である。

第 2の実施の形態に係る脚式移動ロボッ卜の制御装置は、第 1の実施の形態に係る装置と同様に先の提案技術を更に改良し、特に、実各足平床反力の力成分を独立に制御して姿勢安定性を向上させてスピンなどを防止すると共に、関節ァクチユエ一夕の負担も柽減するようにした

即ち、第 1の実施の形態においては前述した 6種のモードの内、モード 1、モード 2およびモード 6を扱ったが、第 2の実施の形態では、それに加えて、ロボット全体の挙動（モード 1から 6) に直接的には影響しない内力の制御を行う。ここでいう「内力」とは、全床反力に影響を与えない各足平床反力の成分の組み合わせを意味する。

第 3 5図は、第 2の実施の形態に係る装置の動作を示す、第 1 0図と同様なフ □— ·チヤ一トである。

以下、第 1の実施の形態と相違する点に焦点をおいて説明すると、第 1の実施の形態と同様に S 1 0から S 3 2までの処理を行った後、 S 34に進み、両脚補償角 0 db Vおよび各足平補償角 0 nx (y) を決定し、次いで S 35に進み、各足平内力補償変位を決定する。

S 34の処理は前記した複合コンプライアンス動作決定部が行う作業であり、第 3 6図は第 2の実施の形態における複合コンプライアンス動作決定部の演算処理を示す、第 1 8図と同様なブロック図である。

第 3 6図から明らかな如く、第 2の実施の形態において、修正目標足平位置 - 姿勢算出部は、第 1の実施の形態で用いた同位相補償変位に代え、内力補償変位なる概念を用い、それと前記した両脚補償角 6> d b vなどに基づいて前述の複合コンプライアンス動作を拡張した足平位置 ·姿勢修正手法に従つて目標足平位置 •姿勢を修正し、修正目標足平位置 ·姿勢を得るようにした。従って、第 2の実施の形態では、 S 3 1の処理が不要となる。

S 3 5の各足平内力補償変位の演算処理について説明すると、上記した S 3 5 を除く処理で行われる制御によってロボット全体の挙動に影響を与える実全床反力のモーメント成分と実各足平モ一メントを独立に制御することができる。しかし、実各足平床反力の力成分を独立に制御することはできない。

第 1の実施の形態では、実各足平床反力の力成分の和を制御することができるので、実各足平の力成分を独立に制御するためには、実各足平床反力の力成分の差を制御するものを加えれば良い。 S 3 5の処理はそれを意図するもので、以下の追加される制御を「内力コンプライアンス制御」という。

この制御は、目標第 1足平と目標第 2足平の間隔を僅かに延ばしたり、縮めたりすることによって、両足平を結ぶ線分方向に、足平床反力の力成分を、それぞれの足平に互いに逆向きに発生させる。この動作を「逆位相平行移動動作」とい Ό ο

この制御を行うと、実全床反力と実各足平モーメントに対して S 3 5の処理を除く処理で述べた制御に干渉することなく、実第 1足平の力成分と実第 2足平の力成分の比率を変えることができるので、この比率を状況に応じて適切に制御することによって、前記したように、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによつて、姿勢安定性を向上させてスリップを効果的に防止することができる。

また、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となつている状態なども解消させることができ、関節ァクチユエ一夕の負担を軽減させることができる。

その意図から、第 2の実施の形態においては、先に提案した装置に、第 3 6図の下部に示す如く、内力補償変位算出部を追加した。具体的には、内力補償変位算出部および内力補償変位を含む修正目標足平位置姿勢算出部は、 S 3 5の処理を除く処理で得られた修正目標各足平位置 ·姿勢に以下の操作を加える。

第 37図は内力補償変位算出部の処理を詳細に示すプロック図であり、第 38 図はその逆位相平行移動動作の説明図である。第 37図に示す如く、内力捕償変位算出部は、実各足平床反力の力成分と目標各足平床反力の力成分から、第 38 図に示す各足平内力補償変位 Dn (n= 1 , 2) を算出する。また、修正目標足平位置姿勢算出部は、その各足平内力補償変位 Dnを含め、幾何学演算によって修正足平位置 ·姿勢を算出する。

第 38図を参照して概説すると、先ず、始点が Q 1 ' 、始点が Q2' のべクトルと、向きが同じで大きさが 1の方向ベクトルを求める。これを以降 V' と記述する。

次に、 S 35の処理を除く処理で得られた修正目標第 1足平を、姿勢を変えずに、 D 1 V' だけ平行移動させる。図示の如く、 D 1は修正目標第 1足平の V' 方向への移動量であり、以降、第 1足平内力補償変位という。 D 1は、スカラ変数である。

同時に、修正目標第 2足平を、姿勢を変えずに、 D2V' だけ平行移動させる。 D 2は修正目標第 2足平の V' 方向への移動量であり、以降、第 2足平内力補償変位という。 D 1も、スカラ変数である。尚、図示の状態では、 D 1は負、 D 2は正である。通常、 D 1と D 2は極性が逆であるため、両足平は、逆位相に動く。以上が、逆位相平行移動動作である。

上記を第 39図フロー 'チヤ一トを参照して更に詳細に説明する。

S 300において第 40図の時変特性を示す、予め設定された分配用重み変数 Wd l, Wd 2の現在時刻 tにおける値を求める。

続いて S 302に進み、実各足平床反力の力成分および目標各足平床反力の力成分から式 1 9 aによって偏差 Fd e r rを算出する。即ち、第 37図に示すように、実第 1足平床反力の力成分 F 1 a c t、実第 2足平床反力の力成分 F 2 a c t、目標第 1足平床反力の力成分 F 1 r e f . および目標第 2足平床反力の力成分 F 2 r e ί力、ら、次の式（式 1 9) によって偏差 Fd e r rを算出する。

Fd e r r=F 2 a c t - F l a c t— (F 2 r e卜 F l r e f )

■ · '式 1 9 a 尚、目標第 1足平床反力の力成分 F 1 r e f および目標第 2足平床反力の力成分 F 2 r e f は、前記した目標歩容の中の値である。

続いて S 304に進み、算出した偏差 Fd e r rを制御系の発振を防止するための口一パスフィルタに通し、更に、ゲイン Kd倍して差 D i f f を算出する。続いて S 306に進み、次式（式 20 a, 式 21 a) によって、第 1足平内力補償変位 D 1と第 2足平内力補償変位 D 2を算出する。

D 1 =-Wd 1 *D d i f f . . . 式 20 a

D2= Wd 2 *D d i f f . . . 式 2 1 a

ここで、 Wd 1および Wd 2は、非負の分配重み変数である。 Wd 1と Wd 2 の和は 1のとき、 D d i ί ίは、修正目標第 2足平と修正目標第 1足平の間の距離の伸び量を意味する。

分配用重み変数 Wd ηの設定例は第 40図に示す通りであるが、図示の例では以下の点を考慮して設定される。即ち、各足平内力補償変位が不連続に変化すると、関節に過大なトルクが発生することから、 Wdnは、連続的に変化するように設定される。

また、足平が着地する頃に Wdnが零でないと、足平着地位置が目標位置からずれ、却ってこの内力制御の支障となる場合がある。従って、足平が着地する付近では Wdnは零にする。また、片足支持期は制御不要であることから、 Wdn を零にすると共に、両脚支持期および直立時には Wd 1と Wd 2の和を 1にするかく得られた同位相補償変位 Dnも含め、第 35図フロー ·チャートの S 38 において機構変形補償入り修正目標足平位置姿勢が算出される。

具体的には、両脚補償角 0 d b v、第 1足平 X補償角 01 x、第 1足平 Y補償角 1 y、第 2足平 X補償角 02x、第 2足平 Y補償角 02y、第 1足平内力捕償変位 D l、および第 2足平内力補償変位 D 2に基づき、前記した同位相平行移動動作を含む複合コンプライアンス動作に従って修正目標足平位置姿勢が算出される。

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

即ち、第 2の実施の形態にあっても、第 1の実施の形態と同様に、先に提案した技術を改良し、移動する目摞足平床反力中心点における実各足平床反力モ一メントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモーメントを望ましい値に制御するようにした。それによつて、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となつた。

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

特に、 αボットの実各足平床反力の力成分を独立に制御するように構成したことで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節ァクチユエ一夕の負担を軽減させることができる。

上記した第 1および第 2の実施の形態においては、少なくとも基体（上体 24 ) と、前記基体に第 1の関節（ 1 0, 1 2, 1 4 R (L) ) を介して連結されると共に、その先端に第 2の関節（ 1 8, 20 R (L) ) を介して連結される足部

(足平 22R (L) ) を備えた複数本（2本）の脚部（脚部リンク 2) からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置，姿勢、目標足平位置，姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（=目標 ΖΜΡ) ) を少なくとも含む前記ロボッ卜の歩容を生成する歩容生成手段（歩容生成器、 S 1 0から S 22) 、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、 S 24, S 2 6) 、前記足部に作用する実床反力

(実各足平床反力）を検出する実床反力検出手段（6軸力センサ 44、実各足平床反力検出器、 S 32) 、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角 0 d b v, z、第 n足平補償角 0 nx, y, z) を決定する足部回転量決定手段（複合コンプライアンス動作決定部、 S 32 から S 34、両脚補償角決定部、第 n足平補償角決定部、 S 1 00から S 1 08 ) 、前記決定された足部回転量に基づいて前記足部の位置およびノまたは姿勢が回転するように前記目標位置および Zまたは姿勢を修正する第 1の足部位置，姿勢修正手段（複合コンプライアンス動作決定部、 S 38, S 40、修正目標足平位置 ·姿勢算出部）、少なくとも前記検出された実床反力に基づいて前記足部の補償変位 Lnを算出する補償変位算出手段（上体 Z方向加速度制御演算部、同位相補償変位算出部、 S 3 し S 35, S 200から S 208 ) 、前記算出された補償変位に基づいて目標足部位置 ·姿勢を修正する第 2の足部位置 ·姿勢修正手段（上体 Z方向加速度制御演算部、同位相補償変位算出部、 S 38) および前記第 1および前記第 1および第 2の足部位置 .姿勢手段によって修正された足部の位置 '姿勢に基づいて前記ロボッ卜の第 1および第 2の関節（ 1 0, 1 2， 1 4 , 1 8， 20 R (L) ) を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、 S 40, S 42) を備えるように構成した。

また、前記足部回転量決定手段は、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第 n足平床反力モーメント Ma c t x, y, z ) を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角 0dbv, z、第 n足平補償角 θ nx， y, z) を決定すると共に、前記補償変位算出手段は、前記検出された実床反力から実全床反力の力成分 F t 0 t a 1 a c tを求めて前記生成された歩容の全床反力の力成分 F r e f との偏差 F t o t a 1 e r rを算出し、前記算出した偏差に基づいて前記足部の補償変位 L nを算出するように構成した。

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

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

更に、前記ロボットに実際に作用する全床反力モーメント（より正確にはモーメント成分 PQ1 *Flact +PQ2 *F2act +Mlact +M2act)、または前記ロボットに実際に作用する全床反力のモーメント（PQ1 *Flact +PQ2 *F2act +Mlact +M2act ) から前記足部に作用する床反力モーメント（Mlact +M2act)を減算して得たモーメント（Mf 1 ί 2 a c t =PQ1 *Flact +PQ2 *F2act ) のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量（0 d b V, z)を決定する足部移動量決定手段（複合コンプライアンス動作決定部、 S 34、両脚補償角決定部、 S 1 0 0から S 1 0 8) を備え、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および Zまたは姿勢を修正するように構成した。

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

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

また、前記補償変位算出手段は、前記ロボッ卜の基体加速度（実上体上下加速度） Gb 0 d yを検出する基体加速度検出手段（S 3 1 ) 、および前記検出された基体加速度に基づき、前記ロボットの姿勢を安定化するための補償床反力の力成分（の目標値 F dmd) を算出する補償全床反力力成分算出手段（S 3 1 ) を備え、前記算出された補償全床反の力成分から前記偏差を減算して得た差（F t 0 t a 1 d i f f あるいは L) に基づいて前記足部の補償変位 L nを算出する（ S 2 0 0から S 2 0 8 ) 如く構成した。また、前記補償変位算出手段は、前記差（F t 0 t a 1 d i ί f あるいは L) に所定の重み（アツテネータ Wl し W1 2) を乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。

また、前記補償変位算出手段は、前記足部が離床しているとき、前記重みを零にする如く構成した。

また、前記補償変位算出手段は、前記重みを前記実全床反力の力成分の周波数に応じて可変にする如く構成した。

また、前記補償変位算出手段は、前記足部の補償変位を重力方向について算出する如く構成した。

また、前記補償変位算出手段は、前記足部の補償変位を重心と目標全床反力中心点を結ぶ線分の方向について算出する如く構成した。

また、前記補償全床反力の力成分を零またはその近傍に設定する如く構成した o

また、少なくとも基体（上体 24) と、前記基体に第 1の関節（1 0， 1 2, 1 4 R (L) ) を介して連結されると共に、その先端に第 2の関節（1 8， 20 R (L) ) を介して連結される足部（足平 22R (L) ) を備えた複数本（2本 ) の脚部（脚部リンク 2) からなる脚式移動ロボッ卜の制御装置において、前記ロボッ卜の少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置 ·姿勢、目標足平位置 ·姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（=目標 ZMP) ) を少なくとも含む前記ロボッ卜の歩容を生成する歩容生成手段（歩容生成器、 S 1 0から S 22) 、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点 ) を決定する目標足部床反力中心点決定手段（目標床反力分配器、 S 24， S 2 6) 、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（6軸力センサ 44、実各足平床反力検出器、 S 32) 、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第 n足平床反力モーメント Ma c t X, y, z ) を算出し、少なくとも前記算出されたモーメン卜に基づいて前記足部を回転させる回転量（両脚補償角 0 d b V , z、第 n足平補償角 0nx, y, z ) を決定する足部回転量決定手段（複合コンプライアンス動作決定部、 S 32から S 34、両脚補償角決定部、第 n足平補償角決定部、 S 1 00から S 1 08) 、前記決定された足部回転量に基づいて前記足部の位置および/または姿勢が回転するように前記目標位置およびまたは姿勢を修正する第 1の足部位置 ·姿勢修正手段（複合コンプライアンス動作決定部、 S 38, S 40、修正目標足平位置 ·姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（F 1 a c t +F 2 a c t) を求めて前記生成された歩容の内力成分（F l r e f +F 2 r e f ) との偏差 F d e r r， D d i f f を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位 Dnを算出する補償変位算出手段（内力補償変位算出部、 S 35, S 300から S 306 ) 、前記算出された補償変位に基づいて目標足部位置 ·姿勢を修正する第 2の足部位置■姿勢修正手段（上体 Z方向加速度制御演算部、同位相補償変位算出部、 S 38) および前記第 1および前記第 1および第 2の足部位置 ·姿勢手段によって修正された足部の位置 ·姿勢に基づいて前記ロボッ卜の第 1および第 2の関節（1 0, 1 2, 1 4, 1 8, 20 R (L) ) を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、 S 4 0， S 42) を備える如く構成した。

より具体的には、前記補償変位手段は、前記検出された実床反力から実全床反力に影響しない内力成分（F l a c t +F 2 a c t) を求めて前記生成された歩容の内力成分（F l r e f +F2 r e f ) との偏差 F d e r r , D d i f f を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位 D nを算出する（内力補償変位算出部、 S 35, S 300から S 306 ) 如く構成した。また、少なくとも基体（上体 24) と、前記基体に第 1の関節（1 0, 1 2， 1 4 R (L) ) を介して連結されると共に、その先端に第 2の関節（1 8, 20 R (L) ) を介して連結される足部（足平 22R (L) ) を備えた複数本（2本 ) の脚部（脚部リンク 2) からなる脚式移動ロボット 1の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上体位置 ·姿勢、目標足平位置 ·姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（=目標 ZMP) ) を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、 S 1 0から S 22) 、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、 S 24) 、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（ 6軸力センサ 44、実各足平床反力検出器、 S 32) 、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角 0 dbv, z、第 n足平補償角 0nx, y, z ) を決定する足部回転量決定手段（複合コンブラィアンス動作決定部、 S 32, S 34、両脚補償角決定部、第 n足平補償角決定部、 S 1 00から S 1 08) 、前記決定された足部回転量に基づいて前記足部の位置および,または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および Zまたは姿勢を修正する第 1 の足部位置 ·姿勢修正手段（複合コンプライアンス動作決定部、 S 38， S 40 、修正目標足平位置 ·姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（F 1 a c t +F 2 a c t) を求めて前記生成された歩容の内力成分（F r e f l +F r e f 2) との偏差 F d e r r, Dd i f f を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位 Dnを算出する補償変位算出手段（上体 Z方向加速度制御演算部、同位相補償変位算出部、 S 35 , S 300から S 306 ) 、前記算出された補償変位に基づいて目標足部位置 - 姿勢を修正する第 2の足部位置 ·姿勢修正手段（上体 Z方向加速度制御演算部、同位相補償変位算出部、 S 38) および前記第 1および前記第 1および第 2の足部位置 ·姿勢手段によって修正された足部の位置 ·姿勢に基づいて前記ロボットの第 1および第 2の関節（ 1 0， 1 2, 1 4， 1 8, 2 OR (L) ) を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、 S 42) を備えるように構成した。

より具体的には、足部回転量決定手段は、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角 dbv, τ 第 η足平補償角 0nx, y, z) を決定すると共に（複合コンプライアンス動作決定部、 S 32， S 34、両脚補償角決定部、第 n足平補償角決定部、 S 1 00から S 1 0 8 ) 、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および Zまたは姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および/または姿勢を修正する（複合コンプライアンス動作決定部、 S 3 8, S 4 0、修正目標足平位置 ·姿勢算出部）ように構成した。

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

更に、前記ロボットに実際に作用する全床反力モーメント（より正確にはモ一メント成分 PQ1 *Flact +PQ2 *F2act +Mlact +M2act)、または前記ロボットに実際に作用する全床反力のモーメント（PQ1 *Flact +PQ2 *F2act +Mlact +M2act ) から前記足部に作用する床反力モーメント（Mlact +M2act)を減算して得たモーメント（M f 1 f 2 a c t =PQ1 *Flact +PQ2 *F2act ) のいずれかを算出し、少なくとも前記算出されたモーメン卜に応じて前記足部を移動させる移動量（0 d b v, z) を決定する足部移動量決定手段（複合コンプライアンス動作決定部、 S 34、両脚補償角決定部、 S 1 0 0から S 1 0 8) を備え、前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および/または姿勢を修正するように構成した。

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

また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（6) e r r X, y) に基づいて求める（S 2 8， S 3 0) 如く構成したまた、前記補償変位算出手段は、前記偏差（F d e r r , D d i f f ) に所定の重み（W d n ) を乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。

また、前記所定の重みが時変特性および Zまたは周波数特性を持つ如く構成した。

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

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

また、上記した第 1および第 2の実施の形態を、先に提案した出願の第 1の実施の形態に関して説明したが、第 2あるいはそれ以降の実施の形態に関して追加しても良い。

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

この発明によれば、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。

また、脚式移動ロボッ卜の姿勢安定化制御を容易に実現できると共に、脚式移動ロボッ卜が受ける実床反力の力成分に対してコンプライアンス制御を行うことで着地衝撃を低減することができ、脚式移動ロボットの接地性を高め、歩行時のスリップやスピンを防止することができる。更に、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節ァクチユエ一夕の負担を軽減させることができる。更には、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。また、脚式移動ロボットのァクチユエ一夕の負荷を低減することができる。

Claims

請求の範囲

1 . 少なくとも基体と、前記基体に第 1の関節を介して連結されると共に、その先端に第 2の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動 oボットの制御装置において、

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

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

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

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

e . 前記決定された足部回転量に基づいて前記足部の位置および/または姿勢が回転するように前記目標位置およびまたは姿勢を修正する第 1の足部位置 •姿勢修正手段、

f . 少なくとも前記検出された実床反力に基づいて前記足部の補償変位を算出する補償変位算出手段、

g . 前記算出された補償変位に基づいて目標足部位置■姿勢を修正する第 2の足部位置 ·姿勢修正手段、

および

h . 前記第 1および第 2の足部位置 ·姿勢手段によって修正された足部の位置 · 姿勢に基づいて前記ロボットの第 1および第 2の関節を変位させる関節変位手段、

を備えたことを特徴とする脚式移動ロボッ卜の制御装置。

2 . 前記足部回転量決定手段は、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量を決定すると共に、前記補償変位算出手段は、前記検出された実床反力から実全床反力の力成分を求めて前記生成された歩容の全床反力の力成分との偏差を算出し、前記算出した偏差に基づいて前記足部の補償変位を算出することを特徴とする請求項 1項記載の脚式移動 Πボッ卜の制御装置。

3 . 前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および Zまたは姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および/または姿勢を修正することを特徵とする請求項 1項または 2項記載の脚式移動ロボッ卜の制御

4 . 前記第 1の足部位置 ·姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および/または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置およびノまたは姿勢を修正することを特徵とする請求項 1項または 2項記載の脚式移動口ボットの制御

5 . 更に、

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

を備え、第 1の前記足部位置 ·姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および/または姿勢を修正することを特徴とする請求項 1項ないし 4項のいずれかに記載の脚式移動ロボットの制御装置。

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

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

8 . 前記補償変位算出手段は、

j . 前記ロボッ卜の基体加速度を検出する基体加速度検出手段、

および

k . 前記検出された基体加速度に基づき、前記ロボッ卜の姿勢を安定化するための補償床反力の力成分を算出する補償全床反力力成分算出手段、

を備え、前記算出された補償全床反の力成分から前記偏差を減算して得た差に基づいて前記足部の補償変位を算出することを特徴とする請求項 1項ないし 7項のいずれかに記載の脚式移動ロボットの制御装置。

9 . 前記補償変位算出手段は、前記差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配することを特徴とする請求項 8項記載の脚式移動ロボッ卜の制御装置。

1 0 . 前記補償変位算出手段は、前記足部が離床しているとき、前記重みを零にすることを特徴とする請求項 9項記載の脚式移動ロボットの制御装置。

1 1 . 前記補償変位算出手段は、前記重みを前記実全床反力の力成分の周波数に応じて可変にすることを特徴とする請求項 9項記載の脚式移動口ボットの制御装

1 2 . 前記補償変位算出手段は、前記足部の補償変位を重力方向について算出することを特徴とする請求項 1項ないし 1 1項のいずれかに記載の脚式移動ロボッ卜の制御装置。

1 3 . 前記補償変位算出手段は、前記足部の補償変位を重心と目標全床反力中心点を結ぶ線分の方向について算出することを特徴とする請求項 1項ないし 1 1項のいずれかに記載の脚式移動ロボッ卜の制御装置。

1 4 . 前記補償全床反力の力成分を零またはその近傍に設定することを特徴とする請求項 8項ないし 1 3項のいずれかに記載の脚式移動ロボッ卜の制御装置。

1 5 . 前記補償変位算出手段は、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を算出することを特徴とする請求項 1項記載の脚式移動ロボットの制御装置。

1 6 . 前記足部回転量決定手段は、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量を決定すると共に、前記第 1の足部位置■姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および/または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および,または姿勢を修正することを特徴とする請求項

3項記載の脚式移動ロボッ卜の制御装置。

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

1 8 . 更に、

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

を備え、前記第 1の前記足部位置，姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置およびノまたは姿勢を修正することを特徴とする請求項 1 5項ないし 1 7項のいずれかに記載の脚式移動ロボッ卜の制御装置。

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

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

2 1 . 前記補償変位算出手段は、前記偏差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配することを特徴とする請求項 1 5項ないし 2 0項記載の脚式移動ロボットの制御装置。

2 2 . 前記所定の重みが時変特性および/または周波数特性を持つことを特徴とする請求項 2 1項記載の脚式移動ロボットの制御装置。